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

src/common/message.h

@@ -25,7 +25,11 @@ const std::string CanDataAnalysis = "canDataAnalysis";
 
 const std::string ObdData = "obddata";
 
-const std::string PerceptionData = "perceptionData";
+const std::string AesData = "aesData";
+
+const std::string SlData = "slData";
+
+const std::string SeData = "seData";
 // -----------------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,12 @@ 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
+  slMsg.msg
+  seMsg.msg
+ )
 
 ## Generate services in the 'srv' folder
 # add_service_files(
@@ -66,10 +67,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 +102,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/msg/seMsg.msg

@@ -0,0 +1,26 @@
+uint32 SysErra
+uint32 SysErrb
+uint32 SysErrc
+uint32 SysErrd
+uint32 SysErre
+uint32 SysErrf
+uint32 SysErrg
+uint32 SysErrh
+uint32 SysErri
+uint32 SysErrj
+uint32 SysErrk
+uint32 SysErrl
+uint32 SysErrm
+uint32 SysErrn
+uint32 SysErro
+uint32 SysErrp
+uint32 SysErrq
+uint32 SysErrr
+uint32 SysErrs
+uint32 SysErrt
+uint32 SysErru
+uint32 SysErrv
+uint32 SysErrw
+uint32 SysErrx
+uint32 SysErry
+uint32 SysErrz

src/perception/msg/slMsg.msg

@@ -0,0 +1,21 @@
+uint32 AccLrnStat
+uint32 BrkLrnStat
+uint32 ClchLrnStat
+uint32 StrLrnStat
+uint32 SftLrnStat
+uint32 PLrnStat
+uint32 NLrnStat
+uint32 RLrnStat
+uint32 DLrnStat
+uint32 MLrnStat
+uint32 UpLrnStat
+uint32 DpLrnStat
+uint32 Acc0LrnStat
+uint32 AccMaxLrnStat
+uint32 Brk0LrnStat
+uint32 BrkMaxLrnStat
+uint32 Clch0LrnStat
+uint32 ClchMaxLrnStat
+uint32 Str0LrnStat
+uint32 StrLLrnStat
+uint32 StrRLrnStat

src/perception/package.xml

@@ -13,7 +13,7 @@
   <!-- One license tag required, multiple allowed, one license per tag -->
   <!-- Commonly used license strings: -->
   <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
-  <license>Apache 2.0</license>
+  <license>BSD</license>
 
 
   <!-- Url tags are optional, but multiple are allowed, one per tag -->
@@ -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,16 +2,14 @@
 #include "../common/message.h"
 #include "../common/canmsgobdmsgid.h"
 #include "../canbus/candatatype.h"
-
-
-
+#include <perception/aesMsg.h>
+#include <perception/slMsg.h>
+#include <perception/seMsg.h>
 
 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,52 +47,223 @@ void Perception::CanTopicCallback(const canbus::canMsg::ConstPtr& msg)
     bool sendFlag = false;
 
     switch (msg->id) {
-    case robot::common::PowerOnReq:
+
+    case robot::common::ActuatorEnableStatus:
     {
-        static robot::canbusSpace::T_CONTROL_CMD_5F1 cmd_5F1_PRO;
-        memset(&cmd_5F1_PRO, 0 ,sizeof(cmd_5F1_PRO));
+        PublishState_6E5(msg);
+    }
+        break;
+    case robot::common::SysLearnBack:
+    {
+        PublishState_6EE(msg);
+    }
+        break;
+    case robot::common::SysWarnErr:
+    {
+        PublishState_6D1(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));
 
-        robot::canbusSpace::T_CONTROL_CMD_5F1 cmd_5F1_REV;
-        memcpy(&cmd_5F1_REV, msg->data.c_str(), sizeof(cmd_5F1_REV));
+    robot::canbusSpace::T_STATE_OUT_6E5 state_6E5;
+    memcpy(&state_6E5, msg->data.c_str(), sizeof(state_6E5));
 
-        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)
+    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
         {
-            sendFlag = true;
+            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;
+            }
         }
-
-        memcpy(&cmd_5F1_PRO, msg->data.c_str(), sizeof(cmd_5F1_PRO));
+        catch(ros::Exception e)
+        {
+            LOG(INFO) << Name() << " PublishState_6E5 exception is:" << e.what() ;
+            return false;
+        }
+        return true;
     }
-        break;
-    case robot::common::ActuatorEnableStatus:
+    else
     {
-        static robot::canbusSpace::T_STATE_OUT_6E5 s_state_6E5;
-        memset(&s_state_6E5, 0 ,sizeof(s_state_6E5));
+        LOG(INFO) << Name() << " PublishState_6E5 data is repeat";
+        return false;
+    }
+}
 
-        robot::canbusSpace::T_STATE_OUT_6E5 state_6E5;
-        memcpy(&state_6E5, msg->data.c_str(), sizeof(state_6E5));
+bool Perception::PublishState_6EE(const canbus::canMsg::ConstPtr &msg)
+{
+    static robot::canbusSpace::T_STATE_OUT_6EE s_state_6EE;
+    memset(&s_state_6EE, 0 ,sizeof(s_state_6EE));
+
+    robot::canbusSpace::T_STATE_OUT_6EE state_6EE;
+    memcpy(&state_6EE, msg->data.c_str(), sizeof(state_6EE));
 
-        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);
+    if(state_6EE.uiAcc0LrnStat != s_state_6EE.uiAcc0LrnStat || state_6EE.uiAccLrnStat != s_state_6EE.uiAccLrnStat ||
+            state_6EE.uiAccMaxLrnStat != s_state_6EE.uiAccMaxLrnStat || state_6EE.uiBrk0LrnStat != s_state_6EE.uiBrk0LrnStat ||
+            state_6EE.uiBrkLrnStat != s_state_6EE.uiBrkLrnStat || state_6EE.uiBrkMaxLrnStat != s_state_6EE.uiBrkMaxLrnStat ||
+            state_6EE.uiClch0LrnStat != s_state_6EE.uiClch0LrnStat || state_6EE.uiClchLrnStat != s_state_6EE.uiClchLrnStat ||
+            state_6EE.uiClchMaxLrnStat != s_state_6EE.uiClchMaxLrnStat || state_6EE.uiDLrnStat != s_state_6EE.uiDLrnStat ||
+            state_6EE.uiDpLrnStat != s_state_6EE.uiDpLrnStat || state_6EE.uiMLrnStat != s_state_6EE.uiMLrnStat ||
+            state_6EE.uiNLrnStat != s_state_6EE.uiNLrnStat || state_6EE.uiPLrnStat != s_state_6EE.uiPLrnStat ||
+            state_6EE.uiRLrnStat != s_state_6EE.uiRLrnStat || state_6EE.uiSftLrnStat != s_state_6EE.uiSftLrnStat ||
+            state_6EE.uiStr0LrnStat != s_state_6EE.uiStr0LrnStat || state_6EE.uiStrLLrnStat != s_state_6EE.uiStrLLrnStat ||
+            state_6EE.uiStrLrnStat != s_state_6EE.uiStrLrnStat || state_6EE.uiStrRLrnStat != s_state_6EE.uiStrRLrnStat ||
+            state_6EE.uiUpLrnStat != s_state_6EE.uiUpLrnStat)
+    {
+        try
         {
-            sendFlag = true;
+            ros::Publisher publisher = node_handle_->advertise<::perception::aesMsg>(robot::common::SlData, 2000);
+            if(publisher)
+            {
+                ::perception::slMsg msg;
+                msg.Acc0LrnStat = state_6EE.uiAcc0LrnStat;
+                msg.AccLrnStat = state_6EE.uiAccLrnStat;
+                msg.AccMaxLrnStat = state_6EE.uiAccMaxLrnStat;
+                msg.Brk0LrnStat = state_6EE.uiBrk0LrnStat;
+                msg.BrkLrnStat = state_6EE.uiBrkLrnStat;
+                msg.BrkMaxLrnStat = state_6EE.uiBrkMaxLrnStat;
+                msg.Clch0LrnStat = state_6EE.uiClch0LrnStat;
+                msg.ClchLrnStat = state_6EE.uiClchLrnStat;
+                msg.ClchMaxLrnStat = state_6EE.uiClchMaxLrnStat;
+                msg.DLrnStat = state_6EE.uiDLrnStat;
+                msg.DpLrnStat = state_6EE.uiDpLrnStat;
+                msg.MLrnStat = state_6EE.uiMLrnStat;
+                msg.NLrnStat = state_6EE.uiNLrnStat;
+                msg.PLrnStat = state_6EE.uiPLrnStat;
+                msg.RLrnStat = state_6EE.uiRLrnStat;
+                msg.SftLrnStat = state_6EE.uiSftLrnStat;
+                msg.Str0LrnStat = state_6EE.uiStr0LrnStat;
+                msg.StrLLrnStat = state_6EE.uiStrLLrnStat;
+                msg.StrLrnStat = state_6EE.uiStrLrnStat;
+                msg.StrRLrnStat = state_6EE.uiStrRLrnStat;
+                msg.UpLrnStat = state_6EE.uiPLrnStat;
+                publisher.publish(msg);
+            }
+            else
+            {
+                LOG(INFO) << Name() << " PublishState_6EE publisher is unvalid";
+                return false;
+            }
         }
+        catch(ros::Exception e)
+        {
+            LOG(INFO) << Name() << " PublishState_6EE exception is:" << e.what() ;
+            return false;
+        }
+        return true;
     }
-        break;
-    default:
-        break;
+    else
+    {
+        LOG(INFO) << Name() << " PublishState_6EE data is repeat";
+        return false;
     }
+}
 
-    if (sendFlag)
+bool Perception::PublishState_6D1(const canbus::canMsg::ConstPtr &msg)
+{
+    static robot::canbusSpace::T_STATE_OUT_6D1 s_state_6D1;
+    memset(&s_state_6D1, 0 ,sizeof(s_state_6D1));
+
+    robot::canbusSpace::T_STATE_OUT_6D1 state_6D1;
+    memcpy(&state_6D1, msg->data.c_str(), sizeof(state_6D1));
+
+    if(state_6D1.uiSysErra != s_state_6D1.uiSysErra || state_6D1.uiSysErrb != s_state_6D1.uiSysErrb || state_6D1.uiSysErrc != s_state_6D1.uiSysErrc ||
+            state_6D1.uiSysErrd != s_state_6D1.uiSysErrd || state_6D1.uiSysErre != s_state_6D1.uiSysErre || state_6D1.uiSysErrf != s_state_6D1.uiSysErrf ||
+            state_6D1.uiSysErrg != s_state_6D1.uiSysErrg || state_6D1.uiSysErrh != s_state_6D1.uiSysErrh || state_6D1.uiSysErri != s_state_6D1.uiSysErri ||
+            state_6D1.uiSysErrj != s_state_6D1.uiSysErrj || state_6D1.uiSysErrk != s_state_6D1.uiSysErrk || state_6D1.uiSysErrl != s_state_6D1.uiSysErrl ||
+            state_6D1.uiSysErrm != s_state_6D1.uiSysErrm || state_6D1.uiSysErrn != s_state_6D1.uiSysErrn || state_6D1.uiSysErro != s_state_6D1.uiSysErro ||
+            state_6D1.uiSysErrp != s_state_6D1.uiSysErrp || state_6D1.uiSysErrq != s_state_6D1.uiSysErrq || state_6D1.uiSysErrr != s_state_6D1.uiSysErrr ||
+            state_6D1.uiSysErrs != s_state_6D1.uiSysErrs || state_6D1.uiSysErrt != s_state_6D1.uiSysErrt || state_6D1.uiSysErru != s_state_6D1.uiSysErru ||
+            state_6D1.uiSysErrv != s_state_6D1.uiSysErrv || state_6D1.uiSysErrw != s_state_6D1.uiSysErrw || state_6D1.uiSysErrx != s_state_6D1.uiSysErrx ||
+            state_6D1.uiSysErry != s_state_6D1.uiSysErry || state_6D1.uiSysErrz != s_state_6D1.uiSysErrz)
+    {
+        try
+        {
+            ros::Publisher publisher = node_handle_->advertise<::perception::seMsg>(robot::common::SeData, 2000);
+            if(publisher)
+            {
+                ::perception::seMsg msg;
+                msg.SysErra = state_6D1.uiSysErra;
+                msg.SysErrb = state_6D1.uiSysErrb;
+                msg.SysErrc = state_6D1.uiSysErrc;
+                msg.SysErrd = state_6D1.uiSysErrd;
+                msg.SysErre = state_6D1.uiSysErre;
+                msg.SysErrf = state_6D1.uiSysErrf;
+                msg.SysErrg = state_6D1.uiSysErrg;
+                msg.SysErrh = state_6D1.uiSysErrh;
+                msg.SysErri = state_6D1.uiSysErri;
+                msg.SysErrj = state_6D1.uiSysErrj;
+                msg.SysErrk = state_6D1.uiSysErrk;
+                msg.SysErrl = state_6D1.uiSysErrl;
+                msg.SysErrm = state_6D1.uiSysErrm;
+                msg.SysErrn = state_6D1.uiSysErrn;
+                msg.SysErro = state_6D1.uiSysErro;
+                msg.SysErrp = state_6D1.uiSysErrp;
+                msg.SysErrq = state_6D1.uiSysErrq;
+                msg.SysErrr = state_6D1.uiSysErrr;
+                msg.SysErrs = state_6D1.uiSysErrs;
+                msg.SysErrt = state_6D1.uiSysErrt;
+                msg.SysErru = state_6D1.uiSysErru;
+                msg.SysErrv = state_6D1.uiSysErrv;
+                msg.SysErrw = state_6D1.uiSysErrw;
+                msg.SysErrx = state_6D1.uiSysErrx;
+                msg.SysErry = state_6D1.uiSysErry;
+                msg.SysErrz = state_6D1.uiSysErrz;
+                publisher.publish(msg);
+            }
+            else
+            {
+                LOG(INFO) << Name() << " PublishState_6D1 publisher is unvalid";
+                return false;
+            }
+        }
+        catch(ros::Exception e)
+        {
+            LOG(INFO) << Name() << " PublishState_6D1 exception is:" << e.what() ;
+            return false;
+        }
+        return true;
+    }
+    else
     {
-        s_perData_pub.publish(msg);
+        LOG(INFO) << Name() << " PublishState_6D1 data is repeat";
+        return false;
     }
 }
 
-}  // namespace perception
+
+
+
+
+
+    }  // namespace perception
 }  // namespace robot

src/perception/perception.h

@@ -39,11 +39,30 @@ 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);
+
+    /**
+    * @brief Publish State_6EE topic
+    */
+
+    bool PublishState_6EE(const canbus::canMsg::ConstPtr& msg);
+
+    /**
+    * @brief Publish State_6D1 topic
+    */
+
+    bool PublishState_6D1(const canbus::canMsg::ConstPtr& msg);
 
 private:
 
-     static ros::Publisher s_perData_pub;
+     //ros::Publisher busData_pub;
 };