添加 Estop系统信息、Estop故障码 数据的去重和发布

src/common/message.h

@@ -48,6 +48,10 @@ 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

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

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

@@ -7,6 +7,8 @@
 #include <perception/seMsg.h>
 #include <perception/si2Msg.h>
 #include <perception/cecMsg.h>
+#include <perception/eiMsg.h>
+#include <perception/eeMsg.h>
 
 namespace  robot {
 namespace perception {
@@ -30,7 +32,7 @@ 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);
@@ -43,6 +45,9 @@ int Perception::Start()
         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)
@@ -54,11 +59,6 @@ int Perception::Start()
     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...";
@@ -70,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:
@@ -119,6 +117,16 @@ void Perception::CanTopicCallback(const canbus::canMsg::ConstPtr& 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:
         break;
     }
@@ -146,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;
@@ -203,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;
@@ -268,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;
@@ -328,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;
@@ -431,7 +443,8 @@ bool Perception::PublishState_6D2_6(const canbus::canMsg::ConstPtr &msg, int typ
     {
 
         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;
@@ -486,6 +499,102 @@ bool Perception::PublishState_6D2_6(const canbus::canMsg::ConstPtr &msg, int typ
     }
 }
 
+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_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_6(const canbus::canMsg::ConstPtr& msg, int type, const ros::Publisher &pub);
+    /**
+    * @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;
@@ -93,6 +101,8 @@ private:
      ros::Publisher bus_pub_6D5;
      ros::Publisher bus_pub_6D6;
 
+     ros::Publisher bus_pub_2E0;
+     ros::Publisher bus_pub_2E1;
 };