１．添加自定义消息

src/common/message.h

@@ -25,7 +25,7 @@ const std::string CanDataAnalysis = "canDataAnalysis";
 
 const std::string ObdData = "obddata";
 
-const std::string PerceptionData = "perceptionData";
+const std::string AesData = "aesData";
 // -----------------topics ----------------------------
 
 // -----------------services ----------------------------

src/perception/CMakeLists.txt

@@ -8,7 +8,7 @@ add_compile_options(-std=c++11)
 ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
 ## is used, also find other catkin packages
 find_package(catkin REQUIRED COMPONENTS
-  roscpp std_msgs canbus
+  roscpp canbus message_generation
 )
 
 ## System dependencies are found with CMake's conventions
@@ -45,11 +45,10 @@ find_package(catkin REQUIRED COMPONENTS
 ##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
 
 ## Generate messages in the 'msg' folder
-# add_message_files(
-#   FILES
-#   Message1.msg
-#   Message2.msg
-# )
+add_message_files(
+   FILES
+  aesMsg.msg
+ )
 
 ## Generate services in the 'srv' folder
 # add_service_files(
@@ -66,10 +65,10 @@ find_package(catkin REQUIRED COMPONENTS
 # )
 
 ## Generate added messages and services with any dependencies listed here
-#generate_messages(
-#  DEPENDENCIES
-#  std_msgs  # Or other packages containing msgs
-#)
+generate_messages(
+  DEPENDENCIES
+  std_msgs  # Or other packages containing msgs
+)
 
 ################################################
 ## Declare ROS dynamic reconfigure parameters ##
@@ -101,10 +100,10 @@ find_package(catkin REQUIRED COMPONENTS
 ## CATKIN_DEPENDS: catkin_packages dependent projects also need
 ## DEPENDS: system dependencies of this project that dependent projects also need
 catkin_package(
-#  INCLUDE_DIRS include
+# INCLUDE_DIRS include
 #  LIBRARIES perception
-#  CATKIN_DEPENDS
-#  message_runtime
+ CATKIN_DEPENDS
+ message_runtime
 #  CATKIN_DEPENDS roscpp message_generation std_msgs rospy
 #  DEPENDS system_lib
 )

src/perception/msg/aesMsg.msg

@@ -0,0 +1,9 @@
+uint32 AccMotEnStat
+uint32 BrkMotEnStat
+uint32 StrMotEnStat
+uint32 XSftMotEnStat
+uint32 YSftMotEnStat
+uint32 ClchMotEnStat
+uint32 AccClchCtrStat
+uint32 PauseEnStat
+uint32 BrkHVFFStat

src/perception/package.xml

@@ -55,7 +55,6 @@
   <exec_depend>roscpp message_generation std_msgs rospy</exec_depend>
   <exec_depend>message_runtime</exec_depend>
 
-
   <!-- The export tag contains other, unspecified, tags -->
   <export>
     <!-- Other tools can request additional information be placed here -->

src/perception/perception.cpp

@@ -2,7 +2,7 @@
 #include "../common/message.h"
 #include "../common/canmsgobdmsgid.h"
 #include "../canbus/candatatype.h"
-
+#include <perception/aesMsg.h>
 
 
 
@@ -10,8 +10,6 @@ namespace  robot {
 namespace perception {
 
 
-ros::Publisher Perception::s_perData_pub;
-
 std::string Perception::Name() const { return "perception"; }
 
 int Perception::Init()
@@ -20,14 +18,19 @@ int Perception::Init()
 
     node_handle_.reset(new ros::NodeHandle());
 
-    s_perData_pub = node_handle_->advertise<canbus::canMsg>(robot::common::PerceptionData, 2000);
-
     LOG(INFO) << Name() << " Init end.";
 }
 
 int Perception::Start()
 {
-    ros::Subscriber sub = node_handle_->subscribe(robot::common::CanDataAnalysis, 1000, CanTopicCallback);
+    try
+    {
+        ros::Subscriber sub = node_handle_->subscribe(robot::common::CanDataAnalysis, 1000, &Perception::CanTopicCallback, this);
+    }
+    catch(ros::Exception e)
+    {
+         LOG(INFO) << Name() << " Start exception is:" << e.what() ;
+    }
     ros::spin();
 }
 
@@ -44,50 +47,65 @@ void Perception::CanTopicCallback(const canbus::canMsg::ConstPtr& msg)
     bool sendFlag = false;
 
     switch (msg->id) {
-    case robot::common::PowerOnReq:
-    {
-        static robot::canbusSpace::T_CONTROL_CMD_5F1 cmd_5F1_PRO;
-        memset(&cmd_5F1_PRO, 0 ,sizeof(cmd_5F1_PRO));
-
-        robot::canbusSpace::T_CONTROL_CMD_5F1 cmd_5F1_REV;
-        memcpy(&cmd_5F1_REV, msg->data.c_str(), sizeof(cmd_5F1_REV));
 
-        if(cmd_5F1_REV.uiAccCtrPowReq != cmd_5F1_PRO.uiAccCtrPowReq && cmd_5F1_REV.uiBrkCtrPowReq != cmd_5F1_PRO.uiBrkCtrPowReq &&
-                cmd_5F1_REV.uiClchCtrPowReq != cmd_5F1_PRO.uiClchCtrPowReq && cmd_5F1_REV.uiRCUPowReq != cmd_5F1_PRO.uiRCUPowReq &&
-                cmd_5F1_REV.uiStrCtrPowReq != cmd_5F1_PRO.uiStrCtrPowReq && cmd_5F1_REV.uiXSftArmCtrPowReq != cmd_5F1_PRO.uiXSftArmCtrPowReq &&
-                cmd_5F1_REV.uiYSftArmCtrPowReq != cmd_5F1_PRO.uiYSftArmCtrPowReq)
-        {
-            sendFlag = true;
-        }
-
-        memcpy(&cmd_5F1_PRO, msg->data.c_str(), sizeof(cmd_5F1_PRO));
-    }
-        break;
     case robot::common::ActuatorEnableStatus:
     {
-        static robot::canbusSpace::T_STATE_OUT_6E5 s_state_6E5;
-        memset(&s_state_6E5, 0 ,sizeof(s_state_6E5));
-
-        robot::canbusSpace::T_STATE_OUT_6E5 state_6E5;
-        memcpy(&state_6E5, msg->data.c_str(), sizeof(state_6E5));
-
-        if(state_6E5.uiAccClchCtrStat != s_state_6E5.uiAccClchCtrStat && state_6E5.uiAccMotEnStat != s_state_6E5.uiAccMotEnStat &&
-                state_6E5.uiBrkHVFFStat != s_state_6E5.uiBrkHVFFStat && state_6E5.uiBrkMotEnStat != s_state_6E5.uiBrkMotEnStat &&
-                state_6E5.uiClchMotEnStat != s_state_6E5.uiClchMotEnStat && state_6E5.uiPauseEnStat != s_state_6E5.uiPauseEnStat &&
-                state_6E5.uiStrMotEnStat != s_state_6E5.uiStrMotEnStat && state_6E5.uiXSftMotEnStat != s_state_6E5.uiXSftMotEnStat &&
-                state_6E5.uiYSftMotEnStat != s_state_6E5.uiYSftMotEnStat);
-        {
-            sendFlag = true;
-        }
+        PublishState_6E5(msg);
     }
         break;
     default:
         break;
     }
+}
+
+bool Perception::PublishState_6E5(const canbus::canMsg::ConstPtr &msg)
+{
+    static robot::canbusSpace::T_STATE_OUT_6E5 s_state_6E5;
+    memset(&s_state_6E5, 0 ,sizeof(s_state_6E5));
 
-    if (sendFlag)
+    robot::canbusSpace::T_STATE_OUT_6E5 state_6E5;
+    memcpy(&state_6E5, msg->data.c_str(), sizeof(state_6E5));
+
+    if(state_6E5.uiAccClchCtrStat != s_state_6E5.uiAccClchCtrStat && state_6E5.uiAccMotEnStat != s_state_6E5.uiAccMotEnStat &&
+            state_6E5.uiBrkHVFFStat != s_state_6E5.uiBrkHVFFStat && state_6E5.uiBrkMotEnStat != s_state_6E5.uiBrkMotEnStat &&
+            state_6E5.uiClchMotEnStat != s_state_6E5.uiClchMotEnStat && state_6E5.uiPauseEnStat != s_state_6E5.uiPauseEnStat &&
+            state_6E5.uiStrMotEnStat != s_state_6E5.uiStrMotEnStat && state_6E5.uiXSftMotEnStat != s_state_6E5.uiXSftMotEnStat &&
+            state_6E5.uiYSftMotEnStat != s_state_6E5.uiYSftMotEnStat)
+    {
+        try
+        {
+            ros::Publisher publisher = node_handle_->advertise<::perception::aesMsg>(robot::common::AesData, 2000);
+            if(publisher)
+            {
+                ::perception::aesMsg msg;
+                msg.AccClchCtrStat = state_6E5.uiAccClchCtrStat;
+                msg.AccMotEnStat =  state_6E5.uiAccMotEnStat;
+                msg.BrkHVFFStat = state_6E5.uiBrkHVFFStat;
+                msg.BrkMotEnStat = state_6E5.uiBrkMotEnStat;
+                msg.ClchMotEnStat = state_6E5.uiClchMotEnStat;
+                msg.PauseEnStat = state_6E5.uiPauseEnStat;
+                msg.StrMotEnStat = state_6E5.uiStrMotEnStat;
+                msg.XSftMotEnStat = state_6E5.uiXSftMotEnStat;
+                msg.YSftMotEnStat = state_6E5.uiYSftMotEnStat;
+                publisher.publish(msg);
+            }
+            else
+            {
+                LOG(INFO) << Name() << " PublishState_6E5 publisher is unvalid";
+                return false;
+            }
+        }
+        catch(ros::Exception e)
+        {
+            LOG(INFO) << Name() << " PublishState_6E5 exception is:" << e.what() ;
+            return false;
+        }
+        return true;
+    }
+    else
     {
-        s_perData_pub.publish(msg);
+        LOG(INFO) << Name() << " PublishState_6E5 data is repeat";
+        return false;
     }
 }
 

src/perception/perception.h

@@ -39,11 +39,18 @@ public:
     /**
     * @brief Can topic data callback function
     */
-    static void CanTopicCallback(const canbus::canMsg::ConstPtr& msg);
+
+    void CanTopicCallback(const canbus::canMsg::ConstPtr& msg);
+
+    /**
+    * @brief Publish State_6E5 topic
+    */
+
+    bool PublishState_6E5(const canbus::canMsg::ConstPtr& msg);
 
 private:
 
-     static ros::Publisher s_perData_pub;
+     //ros::Publisher busData_pub;
 };