Merge branch 'master' of http://118.24.47.114/madesheng/self-driving-robot

src/common/message.h

@@ -38,6 +38,20 @@ const std::string SlData = "slData";
 const std::string Si2Data = "si2Data";
 
 const std::string SeData = "seData";
+
+const std::string AcceData = "acceData";
+
+const std::string BrkeData = "brkeData";
+
+const std::string XsfteData = "xsfteData";
+
+const std::string YsfteData = "ysfteData";
+
+const std::string StreData = "streData";
+
+const std::string EiData = "eiData";
+
+const std::string EeData = "eeData";
 // -----------------topics ----------------------------
 
 // -----------------services ----------------------------

src/perception/CMakeLists.txt

@@ -51,6 +51,9 @@ add_message_files(
   slMsg.msg
   seMsg.msg
   si2Msg.msg
+  cecMsg.msg
+  eiMsg.msg
+  eeMsg.msg
  )
 
 ## Generate services in the 'srv' folder

src/perception/msg/aeMsg.msg

@@ -1,35 +0,0 @@
-uint32 AccErrag
-uint32 AccErrah
-uint32 AccErrai
-uint32 AccErry
-uint32 AccErrz
-uint32 AccErraa
-uint32 AccErrab
-uint32 AccErrac
-uint32 AccErrad
-uint32 AccErrae
-uint32 AccErraf
-uint32 AccErrq
-uint32 AccErrr
-uint32 AccErrs
-uint32 AccErrt
-uint32 AccErru
-uint32 AccErrv
-uint32 AccErrw
-uint32 AccErrx
-uint32 AccErri
-uint32 AccErrj
-uint32 AccErrk
-uint32 AccErrl
-uint32 AccErrm
-uint32 AccErrn
-uint32 AccErro
-uint32 AccErrp
-uint32 AccErra
-uint32 AccErrb
-uint32 AccErrc
-uint32 AccErrd
-uint32 AccErre
-uint32 AccErrf
-uint32 AccErrg
-uint32 AccErrh

src/perception/msg/cecMsg.msg

@@ -0,0 +1,35 @@
+uint32  ag
+uint32  ah
+uint32  ai
+uint32  y
+uint32  z
+uint32  aa
+uint32  ab
+uint32  ac
+uint32  ad
+uint32  ae
+uint32  af
+uint32  q
+uint32  r
+uint32  s
+uint32  t
+uint32  u
+uint32  v
+uint32  w
+uint32  x
+uint32  i
+uint32  j
+uint32  k
+uint32  l
+uint32  m
+uint32  n
+uint32  o
+uint32  p
+uint32  a
+uint32  b
+uint32  c
+uint32  d
+uint32  e
+uint32  f
+uint32  g
+uint32  h

src/perception/msg/eeMsg.msg

@@ -0,0 +1,10 @@
+uint32 uiEspErra
+uint32 uiEspErrb
+uint32 uiEspErrc
+uint32 uiEspErrd
+uint32 uiEspErre
+uint32 uiEspErrf
+uint32 uiEspErrg
+uint32 uiEspErrh
+uint32 uiEspErri
+uint32 uiEspErrj

src/perception/msg/eiMsg.msg

@@ -0,0 +1,4 @@
+uint32 EstpAlive
+uint32 EstpEnStat
+uint32 PmpEnStat
+uint32 EstpAbnStat

src/perception/perception.cpp

@@ -6,6 +6,9 @@
 #include <perception/slMsg.h>
 #include <perception/seMsg.h>
 #include <perception/si2Msg.h>
+#include <perception/cecMsg.h>
+#include <perception/eiMsg.h>
+#include <perception/eeMsg.h>
 
 namespace  robot {
 namespace perception {
@@ -29,13 +32,22 @@ int Perception::Start()
     try
     {
         ros::Subscriber sub = node_handle_->subscribe(robot::common::CanDataAnalysis, 1000, &Perception::CanTopicCallback, this);
-        //ros::Subscriber sub1 = node_handle_->subscribe(robot::common::AesData, 1000, &Perception::CanTopicCallbackTest, this);
+
         bus_pub_6E5 = node_handle_->advertise<::perception::aesMsg>(robot::common::AesData, 2000);
         bus_pub_6EE = node_handle_->advertise<::perception::slMsg>(robot::common::SlData, 2000);
         bus_pub_6ED = node_handle_->advertise<::perception::si2Msg>(robot::common::Si2Data, 2000);
 
         bus_pub_6D1 = node_handle_->advertise<::perception::seMsg>(robot::common::SeData, 2000);
 
+        bus_pub_6D2 = node_handle_->advertise<::perception::cecMsg>(robot::common::AcceData, 2000);
+        bus_pub_6D3 = node_handle_->advertise<::perception::cecMsg>(robot::common::BrkeData, 2000);
+        bus_pub_6D4 = node_handle_->advertise<::perception::cecMsg>(robot::common::XsfteData, 2000);
+        bus_pub_6D5 = node_handle_->advertise<::perception::cecMsg>(robot::common::YsfteData, 2000);
+        bus_pub_6D6 = node_handle_->advertise<::perception::cecMsg>(robot::common::StreData, 2000);
+
+        bus_pub_2E0 = node_handle_->advertise<::perception::eiMsg>(robot::common::EiData, 2000);
+        bus_pub_2E1 = node_handle_->advertise<::perception::eeMsg>(robot::common::EeData, 2000);
+
         ros::spin();
     }
     catch(ros::Exception e)
@@ -43,18 +55,10 @@ int Perception::Start()
          LOG(INFO) << Name() << " Start exception is:" << e.what();
          return ROBOT_FAILTURE;
     }  
-
-
-
     LOG(INFO) << Name() << " Start end.";
     return ROBOT_SUCCESS;
 }
 
-void Perception::CanTopicCallbackTest(const ::perception::aesMsg::ConstPtr& msg)
-{
-     LOG(INFO) << " Msg Test :" << "1:" << msg->AccMotEnStat << ",2:" << msg->AccClchCtrStat;
-}
-
 void Perception::Stop()
 {
    LOG(INFO) << Name() << " Stop start...";
@@ -66,8 +70,6 @@ void Perception::CanTopicCallback(const canbus::canMsg::ConstPtr& msg)
 {
     LOG(INFO) << " Msg CanData :" << "ID:" << msg->id << ",Data:" << msg->data;
 
-    bool sendFlag = false;
-
     switch (msg->id) {
 
     case robot::common::ActuatorEnableStatus:
@@ -92,27 +94,37 @@ void Perception::CanTopicCallback(const canbus::canMsg::ConstPtr& msg)
         break;
     case robot::common::AccErr:
     {
-        PublishState_6D2(msg);
+        PublishState_6D2_6(msg, 2, bus_pub_6D2);
     }
         break;
     case robot::common::BrkErr:
     {
-        PublishState_6D3(msg);
+        PublishState_6D2_6(msg, 3, bus_pub_6D3);
     }
         break;
     case robot::common::XsftErr:
     {
-        PublishState_6D4(msg);
+        PublishState_6D2_6(msg, 4, bus_pub_6D4);
     }
         break;
     case robot::common::YsftErr:
     {
-        PublishState_6D5(msg);
+        PublishState_6D2_6(msg, 5, bus_pub_6D5);
     }
         break;
     case robot::common::StrErr:
     {
-        PublishState_6D6(msg);
+        PublishState_6D2_6(msg, 6, bus_pub_6D6);
+    }
+        break;
+    case robot::common::EstopInfo:
+    {
+        PublishState_2E0(msg);
+    }
+        break;
+    case robot::common::EstopErr:
+    {
+        PublishState_2E1(msg);
     }
         break;
     default:
@@ -142,7 +154,8 @@ bool Perception::PublishState_6E5(const canbus::canMsg::ConstPtr &msg)
     {
 
         memcpy(&s_state_6E5, msg->data.c_str(), copySize);
-
+        if(msg->data.size() < sizeof(s_state_6E5))
+            memset((char*)&s_state_6E5 + msg->data.size(), 0, sizeof(s_state_6E5) - copySize);
         if(bus_pub_6E5)
         {
             ::perception::aesMsg msg;
@@ -199,7 +212,8 @@ bool Perception::PublishState_6EE(const canbus::canMsg::ConstPtr &msg)
     {
 
         memcpy(&s_state_6EE, msg->data.c_str(), copySize);
-
+        if(msg->data.size() < sizeof(s_state_6EE))
+            memset((char*)&s_state_6EE + msg->data.size(), 0, sizeof(s_state_6EE) - copySize);
         if(bus_pub_6EE)
         {
             ::perception::slMsg msg;
@@ -264,7 +278,8 @@ bool Perception::PublishState_6ED(const canbus::canMsg::ConstPtr &msg)
     {
 
         memcpy(&s_state_6ED, msg->data.c_str(), copySize);
-
+        if(msg->data.size() < sizeof(s_state_6ED))
+            memset((char*)&s_state_6ED + msg->data.size(), 0, sizeof(s_state_6ED) - copySize);
         if(bus_pub_6ED)
         {
             ::perception::si2Msg msg;
@@ -324,7 +339,8 @@ bool Perception::PublishState_6D1(const canbus::canMsg::ConstPtr &msg)
     {
 
         memcpy(&s_state_6D1, msg->data.c_str(), copySize);
-
+        if(msg->data.size() < sizeof(s_state_6D1))
+            memset((char*)&s_state_6D1 + msg->data.size(), 0, sizeof(s_state_6D1) - copySize);
         if(bus_pub_6D1)
         {
             ::perception::seMsg msg;
@@ -370,26 +386,215 @@ bool Perception::PublishState_6D1(const canbus::canMsg::ConstPtr &msg)
     }
 }
 
-bool Perception::PublishState_6D2(const canbus::canMsg::ConstPtr &msg)
+bool Perception::PublishState_6D2_6(const canbus::canMsg::ConstPtr &msg, int type, const ros::Publisher &pub)
 {
-}
+    static robot::canbusSpace::T_ACC_ERR s_state_6D2;
+    static robot::canbusSpace::T_BRK_ERR s_state_6D3;
+    static robot::canbusSpace::T_XSFT_ERR s_state_6D4;
+    static robot::canbusSpace::T_YSFT_ERR s_state_6D5;
+    static robot::canbusSpace::T_STR_ERR s_state_6D6;
+
+    robot::canbusSpace::T_ACC_ERR *pErr;
+    switch (type) {
+    case 2:
+        pErr = &s_state_6D2;
+        break;
+    case 3:
+        pErr = (robot::canbusSpace::T_ACC_ERR *)&s_state_6D3;
+        break;
+    case 4:
+        pErr = (robot::canbusSpace::T_ACC_ERR *)&s_state_6D4;
+        break;
+    case 5:
+        pErr = (robot::canbusSpace::T_ACC_ERR *)&s_state_6D5;
+        break;
+    case 6:
+        pErr = (robot::canbusSpace::T_ACC_ERR *)&s_state_6D6;
+        break;
+    default:
+        LOG(INFO) << Name() << " PublishState_6D2_6 " << type << " type is unvalid";
+        return false;
+        break;
+    }
 
-bool Perception::PublishState_6D3(const canbus::canMsg::ConstPtr &msg)
-{
-}
+    int copySize;
+    if(msg->data.size() < sizeof(s_state_6D2))
+        copySize = msg->data.size();
+    else
+        copySize = sizeof(s_state_6D2);
+
+    //6D2_2 6D2_3 6D2_4 6D2_5 6D2_6 数据结构相同
+    robot::canbusSpace::T_ACC_ERR state_6D2;
+    memset(&state_6D2, 0 ,sizeof(state_6D2));
+    memcpy(&state_6D2, msg->data.c_str(), copySize);
+
+    if(state_6D2.uiAccErra != pErr->uiAccErra || state_6D2.uiAccErraa != pErr->uiAccErraa || state_6D2.uiAccErrab != pErr->uiAccErrab ||
+            state_6D2.uiAccErrac != pErr->uiAccErrac || state_6D2.uiAccErrad != pErr->uiAccErrad || state_6D2.uiAccErrae != pErr->uiAccErrae ||
+            state_6D2.uiAccErraf != pErr->uiAccErraf || state_6D2.uiAccErrag != pErr->uiAccErrag || state_6D2.uiAccErrah != pErr->uiAccErrah ||
+            state_6D2.uiAccErrai != pErr->uiAccErrai || state_6D2.uiAccErrb != pErr->uiAccErrb || state_6D2.uiAccErrc != pErr->uiAccErrc ||
+            state_6D2.uiAccErrd != pErr->uiAccErrd || state_6D2.uiAccErre != pErr->uiAccErre || state_6D2.uiAccErrf != pErr->uiAccErrf ||
+            state_6D2.uiAccErrg != pErr->uiAccErrg || state_6D2.uiAccErrh != pErr->uiAccErrh || state_6D2.uiAccErrai != pErr->uiAccErrai ||
+            state_6D2.uiAccErrj != pErr->uiAccErrj || state_6D2.uiAccErrk != pErr->uiAccErrk || state_6D2.uiAccErrl != pErr->uiAccErrl ||
+            state_6D2.uiAccErrm != pErr->uiAccErrm || state_6D2.uiAccErrn != pErr->uiAccErrn || state_6D2.uiAccErro != pErr->uiAccErro ||
+            state_6D2.uiAccErrp != pErr->uiAccErrp || state_6D2.uiAccErrq != pErr->uiAccErrq || state_6D2.uiAccErrr != pErr->uiAccErrr ||
+            state_6D2.uiAccErrs != pErr->uiAccErrs || state_6D2.uiAccErrt != pErr->uiAccErrt || state_6D2.uiAccErru != pErr->uiAccErru ||
+            state_6D2.uiAccErrv != pErr->uiAccErrv || state_6D2.uiAccErrw != pErr->uiAccErrw || state_6D2.uiAccErrx != pErr->uiAccErrx ||
+            state_6D2.uiAccErry != pErr->uiAccErry || state_6D2.uiAccErrz != pErr->uiAccErrz)
+    {
 
-bool Perception::PublishState_6D4(const canbus::canMsg::ConstPtr &msg)
-{
+        memcpy(pErr, msg->data.c_str(), copySize);
+        if(msg->data.size() < sizeof(s_state_6D2))
+            memset((char*)&s_state_6D2 + msg->data.size(), 0, sizeof(s_state_6D2) - copySize);
+        if(pub)
+        {
+            ::perception::cecMsg msg;
+            msg.a = state_6D2.uiAccErra;
+            msg.aa = state_6D2.uiAccErraa;
+            msg.ab = state_6D2.uiAccErrab;
+            msg.ac = state_6D2.uiAccErrac;
+            msg.ad = state_6D2.uiAccErrad;
+            msg.ae = state_6D2.uiAccErrae;
+            msg.af = state_6D2.uiAccErraf;
+            msg.ag = state_6D2.uiAccErrag;
+            msg.ah = state_6D2.uiAccErrah;
+            msg.ai = state_6D2.uiAccErrai;
+            msg.b = state_6D2.uiAccErrb;
+            msg.c = state_6D2.uiAccErrc;
+            msg.d = state_6D2.uiAccErrd;
+            msg.e = state_6D2.uiAccErre;
+            msg.f = state_6D2.uiAccErrf;
+            msg.g = state_6D2.uiAccErrg;
+            msg.h = state_6D2.uiAccErrh;
+            msg.i = state_6D2.uiAccErri;
+            msg.j = state_6D2.uiAccErrj;
+            msg.k = state_6D2.uiAccErrk;
+            msg.l = state_6D2.uiAccErrl;
+            msg.m = state_6D2.uiAccErrm;
+            msg.n = state_6D2.uiAccErrn;
+            msg.o = state_6D2.uiAccErro;
+            msg.p = state_6D2.uiAccErrp;
+            msg.q = state_6D2.uiAccErrq;
+            msg.r = state_6D2.uiAccErrr;
+            msg.s = state_6D2.uiAccErrs;
+            msg.t = state_6D2.uiAccErrt;
+            msg.u = state_6D2.uiAccErru;
+            msg.v = state_6D2.uiAccErrv;
+            msg.w = state_6D2.uiAccErrw;
+            msg.x = state_6D2.uiAccErrx;
+            msg.y = state_6D2.uiAccErry;
+            msg.z = state_6D2.uiAccErrz;
+            pub.publish(msg);
+        }
+        else
+        {
+            LOG(INFO) << Name() << " PublishState_6D2_6 publisher " << type <<" is unvalid";
+            return false;
+        }
+        return true;
+    }
+    else
+    {
+        LOG(INFO) << Name() << " PublishState_6D2_6 " << type << " data is repeat";
+        return false;
+    }
 }
 
-bool Perception::PublishState_6D5(const canbus::canMsg::ConstPtr &msg)
+bool Perception::PublishState_2E0(const canbus::canMsg::ConstPtr &msg)
 {
+    static robot::canbusSpace::T_ESTOP_2E0 s_state_2E0;
+
+    int copySize;
+    if(msg->data.size() < sizeof(s_state_2E0))
+    {
+        copySize = msg->data.size();
+    }
+    else
+        copySize = sizeof(s_state_2E0);
+
+    robot::canbusSpace::T_ESTOP_2E0 state_2E0;
+    memset(&state_2E0, 0 ,sizeof(state_2E0));
+    memcpy(&state_2E0, msg->data.c_str(), copySize);
+
+    if(state_2E0.uiEstpAbnStat != s_state_2E0.uiEstpAbnStat || state_2E0.uiEstpAlive != state_2E0.uiEstpAlive ||
+            state_2E0.uiEstpEnStat != s_state_2E0.uiEstpEnStat || state_2E0.uiPmpEnStat != s_state_2E0.uiPmpEnStat )
+    {
+
+        memcpy(&s_state_2E0, msg->data.c_str(), copySize);
+        if(msg->data.size() < sizeof(s_state_2E0))
+            memset((char*)&s_state_2E0 + msg->data.size(), 0, sizeof(s_state_2E0) - copySize);
+        if(bus_pub_2E0)
+        {
+            ::perception::eiMsg msg;
+            msg.EstpAbnStat = state_2E0.uiEstpAbnStat;
+            msg.EstpAlive = state_2E0.uiEstpAlive;
+            msg.EstpEnStat = state_2E0.uiEstpEnStat;
+            msg.PmpEnStat = state_2E0.uiPmpEnStat;
+            bus_pub_2E0.publish(msg);
+        }
+        else
+        {
+            LOG(INFO) << Name() << " PublishState_2E0 publisher is unvalid";
+            return false;
+        }
+        return true;
+    }
+    else
+    {
+        LOG(INFO) << Name() << " PublishState_2E0 data is repeat";
+        return false;
+    }
 }
 
-bool Perception::PublishState_6D6(const canbus::canMsg::ConstPtr &msg)
+bool Perception::PublishState_2E1(const canbus::canMsg::ConstPtr &msg)
 {
-}
+    static robot::canbusSpace::T_ESTOP_2E1 s_state_2E1;
 
+    int copySize;
+    if(msg->data.size() < sizeof(s_state_2E1))
+        copySize = msg->data.size();
+    else
+        copySize = sizeof(s_state_2E1);
 
+    robot::canbusSpace::T_ESTOP_2E1 state_2E1;
+    memset(&state_2E1, 0 ,sizeof(state_2E1));
+    memcpy(&state_2E1, msg->data.c_str(), copySize);
+
+    if(state_2E1.uiEspErra != s_state_2E1.uiEspErra || state_2E1.uiEspErrb != s_state_2E1.uiEspErrb || state_2E1.uiEspErrc != s_state_2E1.uiEspErrc ||
+            state_2E1.uiEspErrd != s_state_2E1.uiEspErrd || state_2E1.uiEspErre != s_state_2E1.uiEspErre || state_2E1.uiEspErrf != s_state_2E1.uiEspErrf ||
+            state_2E1.uiEspErrg != s_state_2E1.uiEspErrg || state_2E1.uiEspErrh != s_state_2E1.uiEspErrh || state_2E1.uiEspErri != s_state_2E1.uiEspErri ||
+            state_2E1.uiEspErrj != s_state_2E1.uiEspErrj)
+    {
+
+        memcpy(&s_state_2E1, msg->data.c_str(), copySize);
+        if(msg->data.size() < sizeof(s_state_2E1))
+            memset((char*)&s_state_2E1 + msg->data.size(), 0, sizeof(s_state_2E1) - copySize);
+        if(bus_pub_2E1)
+        {
+            ::perception::eeMsg msg;
+            msg.uiEspErra = state_2E1.uiEspErra;
+            msg.uiEspErrb = state_2E1.uiEspErrb;
+            msg.uiEspErrc = state_2E1.uiEspErrc;
+            msg.uiEspErrd = state_2E1.uiEspErrd;
+            msg.uiEspErre = state_2E1.uiEspErre;
+            msg.uiEspErrf = state_2E1.uiEspErrf;
+            msg.uiEspErrg = state_2E1.uiEspErrg;
+            msg.uiEspErrh = state_2E1.uiEspErrh;
+            msg.uiEspErri = state_2E1.uiEspErri;
+            msg.uiEspErrj = state_2E1.uiEspErrj;
+            bus_pub_2E1.publish(msg);
+        }
+        else
+        {
+            LOG(INFO) << Name() << " PublishState_2E1 publisher is unvalid";
+            return false;
+        }
+        return true;
+    }
+    else
+    {
+        LOG(INFO) << Name() << " PublishState_2E1 data is repeat";
+        return false;
+    }
+}
     }  // namespace perception
 }  // namespace robot

src/perception/perception.h

@@ -5,7 +5,7 @@
 #include "ros/ros.h"
 #include "std_msgs/String.h"
 #include <canbus/canMsg.h>
-#include <perception/aesMsg.h>
+
 namespace robot {
 namespace perception {
 
@@ -54,7 +54,9 @@ public:
     /**
     * @brief Publish State_6ED topic
     */
+
     bool PublishState_6ED(const canbus::canMsg::ConstPtr& msg);
+
     /**
     * @brief Publish State_6EE topic
     */
@@ -67,18 +69,24 @@ public:
 
     bool PublishState_6D1(const canbus::canMsg::ConstPtr& msg);
 
-    bool PublishState_6D2(const canbus::canMsg::ConstPtr& msg);
+    /**
+    * @brief Publish State_6D2~State_6D6 topic
+    */
 
-    bool PublishState_6D3(const canbus::canMsg::ConstPtr& msg);
+    bool PublishState_6D2_6(const canbus::canMsg::ConstPtr& msg, int type, const ros::Publisher &pub);
 
-    bool PublishState_6D4(const canbus::canMsg::ConstPtr& msg);
+    /**
+    * @brief Publish State_2E0 topic
+    */
 
-    bool PublishState_6D5(const canbus::canMsg::ConstPtr& msg);
+    bool PublishState_2E0(const canbus::canMsg::ConstPtr& msg);
 
-    bool PublishState_6D6(const canbus::canMsg::ConstPtr& msg);
+    /**
+    * @brief Publish State_2E1 topic
+    */
 
+    bool PublishState_2E1(const canbus::canMsg::ConstPtr& msg);
 
-    void CanTopicCallbackTest(const ::perception::aesMsg::ConstPtr& msg);
 private:
 
      ros::Publisher bus_pub_6E5;
@@ -86,11 +94,15 @@ private:
      ros::Publisher bus_pub_6EE;
 
      ros::Publisher bus_pub_6D1;
-     ros::Publisher bus_pub_6D2;
+
+     ros::Publisher bus_pub_6D2;  
      ros::Publisher bus_pub_6D3;
      ros::Publisher bus_pub_6D4;
      ros::Publisher bus_pub_6D5;
      ros::Publisher bus_pub_6D6;
+
+     ros::Publisher bus_pub_2E0;
+     ros::Publisher bus_pub_2E1;
 };