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

src/perception/perception.cpp

@@ -2,13 +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>
-#include <perception/si2Msg.h>
-#include <perception/cecMsg.h>
-#include <perception/eiMsg.h>
-#include <perception/eeMsg.h>
+#include "perception/aesMsg.h"
+#include "perception/slMsg.h"
+#include "perception/seMsg.h"
+#include "perception/si2Msg.h"
+#include "perception/cecMsg.h"
+#include "perception/eiMsg.h"
+#include "perception/eeMsg.h"
+#include "canbus/ActuatorEnableRequest.h"
 
 namespace  robot {
 namespace perception {
@@ -48,11 +49,13 @@ int Perception::Start()
         bus_pub_2E0 = node_handle_->advertise<::perception::eiMsg>(robot::common::EiData, 2000);
         bus_pub_2E1 = node_handle_->advertise<::perception::eeMsg>(robot::common::EeData, 2000);
 
+        clientActuatorEnableRequest = node_handle_->serviceClient<canbus::ActuatorEnableRequest>(robot::common::ActuatorEnableService);
+
         ros::spin();
     }
     catch(ros::Exception e)
     {
-         LOG(INFO) << Name() << " Start exception is:" << e.what();
+         LOG(ERROR) << Name() << " Start exception is:" << e.what();
          return ROBOT_FAILTURE;
     }  
     LOG(INFO) << Name() << " Start end.";
@@ -89,31 +92,37 @@ void Perception::CanTopicCallback(const canbus::canMsg::ConstPtr& msg)
         break;
     case robot::common::SysWarnErr:
     {
+        SoftStopControl();
         PublishState_6D1(msg);
     }
         break;
     case robot::common::AccErr:
     {
+        SoftStopControl();
         PublishState_6D2_6(msg, 2, bus_pub_6D2);
     }
         break;
     case robot::common::BrkErr:
     {
+        SoftStopControl();
         PublishState_6D2_6(msg, 3, bus_pub_6D3);
     }
         break;
     case robot::common::XsftErr:
     {
+        SoftStopControl();
         PublishState_6D2_6(msg, 4, bus_pub_6D4);
     }
         break;
     case robot::common::YsftErr:
     {
+        SoftStopControl();
         PublishState_6D2_6(msg, 5, bus_pub_6D5);
     }
         break;
     case robot::common::StrErr:
     {
+        SoftStopControl();
         PublishState_6D2_6(msg, 6, bus_pub_6D6);
     }
         break;
@@ -172,7 +181,7 @@ bool Perception::PublishState_6E5(const canbus::canMsg::ConstPtr &msg)
         }
         else
         {
-            LOG(INFO) << Name() << " PublishState_6E5 publisher is unvalid";
+            LOG(ERROR) << Name() << " PublishState_6E5 publisher is unvalid";
             return false;
         }
         return true;
@@ -242,7 +251,7 @@ bool Perception::PublishState_6EE(const canbus::canMsg::ConstPtr &msg)
         }
         else
         {
-            LOG(INFO) << Name() << " PublishState_6EE publisher is unvalid";
+            LOG(ERROR) << Name() << " PublishState_6EE publisher is unvalid";
             return false;
         }
         return true;
@@ -301,7 +310,7 @@ bool Perception::PublishState_6ED(const canbus::canMsg::ConstPtr &msg)
         }
         else
         {
-            LOG(INFO) << Name() << " PublishState_6ED publisher is unvalid";
+            LOG(ERROR) << Name() << " PublishState_6ED publisher is unvalid";
             return false;
         }
         return true;
@@ -374,7 +383,7 @@ bool Perception::PublishState_6D1(const canbus::canMsg::ConstPtr &msg)
         }
         else
         {
-            LOG(INFO) << Name() << " PublishState_6D1 publisher is unvalid";
+            LOG(ERROR) << Name() << " PublishState_6D1 publisher is unvalid";
             return false;
         }
         return true;
@@ -487,7 +496,7 @@ bool Perception::PublishState_6D2_6(const canbus::canMsg::ConstPtr &msg, int typ
         }
         else
         {
-            LOG(INFO) << Name() << " PublishState_6D2_6 publisher " << type <<" is unvalid";
+            LOG(ERROR) << Name() << " PublishState_6D2_6 publisher " << type <<" is unvalid";
             return false;
         }
         return true;
@@ -533,7 +542,7 @@ bool Perception::PublishState_2E0(const canbus::canMsg::ConstPtr &msg)
         }
         else
         {
-            LOG(INFO) << Name() << " PublishState_2E0 publisher is unvalid";
+            LOG(ERROR) << Name() << " PublishState_2E0 publisher is unvalid";
             return false;
         }
         return true;
@@ -585,7 +594,7 @@ bool Perception::PublishState_2E1(const canbus::canMsg::ConstPtr &msg)
         }
         else
         {
-            LOG(INFO) << Name() << " PublishState_2E1 publisher is unvalid";
+            LOG(ERROR) << Name() << " PublishState_2E1 publisher is unvalid";
             return false;
         }
         return true;
@@ -596,5 +605,25 @@ bool Perception::PublishState_2E1(const canbus::canMsg::ConstPtr &msg)
         return false;
     }
 }
+
+bool Perception::SoftStopControl()
+{
+    canbus::ActuatorEnableRequest srv;
+    // 系统软急停使能: enable
+    srv.request.uiPauseEnReq = 1;
+    // 系统控制模式请求: 人工驾驶
+    srv.request.uiCtrMdReq = 3;
+
+    if (clientActuatorEnableRequest.call(srv))
+    {
+        LOG(INFO) << Name() << " SoftStopControl call service return success, value = " << srv.response.uiReturn;
+        return true;
+    }
+    else
+    {
+        LOG(ERROR) << Name() << " SoftStopControl call service return fail";
+        return false;
+    }
+}
     }  // namespace perception
 }  // namespace robot

src/perception/perception.h

@@ -87,6 +87,8 @@ public:
 
     bool PublishState_2E1(const canbus::canMsg::ConstPtr& msg);
 
+    bool SoftStopControl();
+
 private:
 
      ros::Publisher bus_pub_6E5;
@@ -103,6 +105,8 @@ private:
 
      ros::Publisher bus_pub_2E0;
      ros::Publisher bus_pub_2E1;
+
+     ros::ServiceClient clientActuatorEnableRequest;
 };