添加 油门(acc)、制动(brk)、挡位(sft)X、挡位(sft)Y、转向(str)电机报警数据的去重和发布

src/common/message.h

@@ -38,6 +38,16 @@ 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";
 // -----------------topics ----------------------------
 
 // -----------------services ----------------------------

src/perception/CMakeLists.txt

@@ -51,6 +51,7 @@ add_message_files(
   slMsg.msg
   seMsg.msg
   si2Msg.msg
+  cecMsg.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/perception.cpp

@@ -6,6 +6,7 @@
 #include <perception/slMsg.h>
 #include <perception/seMsg.h>
 #include <perception/si2Msg.h>
+#include <perception/cecMsg.h>
 
 namespace  robot {
 namespace perception {
@@ -36,6 +37,12 @@ int Perception::Start()
 
         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);
+
         ros::spin();
     }
     catch(ros::Exception e)
@@ -43,9 +50,6 @@ int Perception::Start()
          LOG(INFO) << Name() << " Start exception is:" << e.what();
          return ROBOT_FAILTURE;
     }  
-
-
-
     LOG(INFO) << Name() << " Start end.";
     return ROBOT_SUCCESS;
 }
@@ -92,27 +96,27 @@ 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;
     default:
@@ -370,24 +374,116 @@ bool Perception::PublishState_6D1(const canbus::canMsg::ConstPtr &msg)
     }
 }
 
-bool Perception::PublishState_6D2(const canbus::canMsg::ConstPtr &msg)
-{
-}
-
-bool Perception::PublishState_6D3(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_6D4(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_6D5(const canbus::canMsg::ConstPtr &msg)
-{
-}
+        memcpy(pErr, msg->data.c_str(), copySize);
 
-bool Perception::PublishState_6D6(const canbus::canMsg::ConstPtr &msg)
-{
+        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;
+    }
 }
 
 

src/perception/perception.h

@@ -67,15 +67,15 @@ public:
 
     bool PublishState_6D1(const canbus::canMsg::ConstPtr& msg);
 
-    bool PublishState_6D2(const canbus::canMsg::ConstPtr& msg);
+    bool PublishState_6D2_6(const canbus::canMsg::ConstPtr& msg, int type, const ros::Publisher &pub);
 
-    bool PublishState_6D3(const canbus::canMsg::ConstPtr& msg);
+//    bool PublishState_6D3(const canbus::canMsg::ConstPtr& msg);
 
-    bool PublishState_6D4(const canbus::canMsg::ConstPtr& msg);
+//    bool PublishState_6D4(const canbus::canMsg::ConstPtr& msg);
 
-    bool PublishState_6D5(const canbus::canMsg::ConstPtr& msg);
+//    bool PublishState_6D5(const canbus::canMsg::ConstPtr& msg);
 
-    bool PublishState_6D6(const canbus::canMsg::ConstPtr& msg);
+//    bool PublishState_6D6(const canbus::canMsg::ConstPtr& msg);
 
 
     void CanTopicCallbackTest(const ::perception::aesMsg::ConstPtr& msg);
@@ -86,11 +86,13 @@ 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;
+
 };