系统运行状态参数反馈２解析提交

src/common/message.h

@@ -29,6 +29,8 @@ const std::string AesData = "aesData";
 
 const std::string SlData = "slData";
 
+const std::string Si2Data = "si2Data";
+
 const std::string SeData = "seData";
 // -----------------topics ----------------------------
 

src/perception/CMakeLists.txt

@@ -50,6 +50,7 @@ add_message_files(
   aesMsg.msg
   slMsg.msg
   seMsg.msg
+  si2Msg.msg
  )
 
 ## Generate services in the 'srv' folder

src/perception/msg/si2Msg.msg

@@ -0,0 +1,14 @@
+uint32 CtrMdStat
+uint32 RCUAlive
+uint32 AccCtrAlive
+uint32 AccMotClbr
+uint32 BrkCtrAlive
+uint32 BrkMotClbr
+uint32 ClchCtrAlive
+uint32 ClchMotClbr
+uint32 StrCtrAlive
+uint32 StrMotClbr
+uint32 XSftArmCtrAlive
+uint32 XSftMotClbr
+uint32 YSftArmCtrAlive
+uint32 YSftMotClbr

src/perception/perception.cpp

@@ -5,6 +5,7 @@
 #include <perception/aesMsg.h>
 #include <perception/slMsg.h>
 #include <perception/seMsg.h>
+#include <perception/si2Msg.h>
 
 namespace  robot {
 namespace perception {
@@ -53,6 +54,11 @@ void Perception::CanTopicCallback(const canbus::canMsg::ConstPtr& msg)
         PublishState_6E5(msg);
     }
         break;
+    case robot::common::RSysInfoBack2:
+    {
+        PublishState_6ED(msg);
+    }
+        break;
     case robot::common::SysLearnBack:
     {
         PublishState_6EE(msg);
@@ -188,6 +194,65 @@ bool Perception::PublishState_6EE(const canbus::canMsg::ConstPtr &msg)
     }
 }
 
+bool Perception::PublishState_6ED(const canbus::canMsg::ConstPtr &msg)
+{
+    static robot::canbusSpace::T_STATE_OUT_6ED s_state_6ED;
+    memset(&s_state_6ED, 0 ,sizeof(s_state_6ED));
+
+    robot::canbusSpace::T_STATE_OUT_6ED state_6ED;
+    memcpy(&state_6ED, msg->data.c_str(), sizeof(state_6ED));
+
+    if(state_6ED.uiAccCtrAlive != s_state_6ED.uiAccCtrAlive || state_6ED.uiAccMotClbr != s_state_6ED.uiAccMotClbr ||
+            state_6ED.uiBrkCtrAlive != s_state_6ED.uiBrkCtrAlive || state_6ED.uiBrkMotClbr != s_state_6ED.uiBrkMotClbr ||
+            state_6ED.uiClchCtrAlive != s_state_6ED.uiClchCtrAlive || state_6ED.uiClchMotClbr != s_state_6ED.uiClchMotClbr ||
+            state_6ED.uiCtrMdStat != s_state_6ED.uiCtrMdStat || state_6ED.uiRCUAlive != s_state_6ED.uiRCUAlive ||
+            state_6ED.uiStrCtrAlive != s_state_6ED.uiStrCtrAlive || state_6ED.uiStrMotClbr != s_state_6ED.uiStrMotClbr ||
+            state_6ED.uiXSftArmCtrAlive != s_state_6ED.uiXSftArmCtrAlive || state_6ED.uiXSftMotClbr != s_state_6ED.uiXSftMotClbr ||
+            state_6ED.uiYSftArmCtrAlive != s_state_6ED.uiYSftArmCtrAlive || state_6ED.uiYSftMotClbr != s_state_6ED.uiYSftMotClbr)
+    {
+        try
+        {
+            ros::Publisher publisher = node_handle_->advertise<::perception::si2Msg>(robot::common::Si2Data, 2000);
+            if(publisher)
+            {
+                ::perception::si2Msg msg;
+
+                msg.AccCtrAlive = state_6ED.uiAccCtrAlive;
+                msg.AccMotClbr = state_6ED.uiAccMotClbr;
+                msg.BrkCtrAlive = state_6ED.uiBrkCtrAlive;
+                msg.BrkMotClbr = state_6ED.uiBrkMotClbr;
+                msg.ClchCtrAlive = state_6ED.uiClchCtrAlive;
+                msg.ClchMotClbr = state_6ED.uiClchMotClbr;
+                msg.CtrMdStat = state_6ED.uiCtrMdStat;
+                msg.RCUAlive = state_6ED.uiRCUAlive;
+                msg.StrCtrAlive = state_6ED.uiStrCtrAlive;
+                msg.StrMotClbr = state_6ED.uiStrMotClbr;
+                msg.XSftArmCtrAlive = state_6ED.uiXSftArmCtrAlive;
+                msg.XSftMotClbr = state_6ED.uiXSftMotClbr;
+                msg.YSftArmCtrAlive = state_6ED.uiYSftArmCtrAlive;
+                msg.YSftMotClbr = state_6ED.uiYSftMotClbr;
+                publisher.publish(msg);
+            }
+            else
+            {
+                LOG(INFO) << Name() << " PublishState_6ED publisher is unvalid";
+                return false;
+            }
+        }
+        catch(ros::Exception e)
+        {
+            LOG(INFO) << Name() << " PublishState_6ED exception is:" << e.what() ;
+            return false;
+        }
+        return true;
+    }
+    else
+    {
+        LOG(INFO) << Name() << " PublishState_6ED data is repeat";
+        return false;
+    }
+}
+
 bool Perception::PublishState_6D1(const canbus::canMsg::ConstPtr &msg)
 {
     static robot::canbusSpace::T_STATE_OUT_6D1 s_state_6D1;

src/perception/perception.h

@@ -48,6 +48,10 @@ public:
 
     bool PublishState_6E5(const canbus::canMsg::ConstPtr& msg);
 
+    /**
+    * @brief Publish State_6ED topic
+    */
+    bool PublishState_6ED(const canbus::canMsg::ConstPtr& msg);
     /**
     * @brief Publish State_6EE topic
     */