添加设备管理服务接口

src/canbus/CMakeLists.txt

@@ -58,6 +58,7 @@ add_message_files(FILES
    PedalActCtrReq.srv
    SftActCtrReq.srv
    StrActCtrReq.srv
+   RBTManager.srv
  )
 
 ## Generate actions in the 'action' folder

src/canbus/canbus.cpp

@@ -107,6 +107,9 @@ int Canbus::Start() {
                                                               &Canbus::StrActCtrRequest, this);
   ros::ServiceServer PedalActCtrService = node_handle_->advertiseService(robot::common::PedalActCtrService,
                                                               &Canbus::PedalActCtrRequest, this);
+
+  ros::ServiceServer RBTManagerService = node_handle_->advertiseService(robot::common::RBTManagerService,
+                                                              &Canbus::RBTManager, this);
   //data receive
   while(ros::ok())
   {
@@ -142,22 +145,17 @@ void Canbus::OnTimer(const ros::TimerEvent &) {
   try
   {
       MessageCoder::CanMsgPackage((void*)&icuMonitor, robot::common::ICUMonitor, &canIcuMonitor);
+      write_data(&canIcuMonitor);
       MessageCoder::CanMsgPackage((void*)&status1, robot::common::VehicleStatus1, &canStatus1);
+       write_data(&canStatus1);
       MessageCoder::CanMsgPackage((void*)&status2, robot::common::VehicleStatus2, &canStatus2);
-      MessageCoder::CanMsgPackage((void*)&status3, robot::common::VehicleStatus3, &canStatus3);
-      write_data(&canStatus1);
       write_data(&canStatus2);
+      MessageCoder::CanMsgPackage((void*)&status3, robot::common::VehicleStatus3, &canStatus3);
       write_data(&canStatus3);
-      write_data(&canIcuMonitor);
-
-      T_STR_ACT_CTR_REQ strAct = {0};
-      strAct.iStrAngCtrReq =  2324;
-      strAct.iStrSpdCtrReq= 10000;
-      strAct.iStrTrqCtrReq = 5000;
-      strAct.uiStrCtrMdReq = 1;
-      TPCANMsg pStr;
-      MessageCoder::CanMsgPackage((void*)&strAct,robot::common::SteerActCtrReq,&pStr);
-      write_data(&pStr);
+      // enable
+      write_data(&canActuaEnableReqMsg);
+      // robot manager
+      write_data(&canRobotManagerMsg);
   }
   catch (ros::Exception ex)
   {
@@ -310,8 +308,7 @@ bool Canbus::ActuatorEnableRequest(canbus::ActuatorEnableRequest::Request &req,
                            canbus::ActuatorEnableRequest::Response &res)
 {
     LOG(INFO)<< "controlservice start ActuatorEnableRequest";
-    // actuator enable request
-    TPCANMsg canActuaEnableReqMsg;
+
     try{
         T_ACTUATOR_ENABLE_REQUEST request;
         request.uiAccClchCtrReq = req.uiAccClchCtrReq;
@@ -327,7 +324,7 @@ bool Canbus::ActuatorEnableRequest(canbus::ActuatorEnableRequest::Request &req,
         request.uiYSftMotEnReq = req.uiYSftMotEnReq;
 
         MessageCoder::CanMsgPackage((void*)&request, robot::common::ActuatorEnableReq, &canActuaEnableReqMsg);
-        write_data(&canActuaEnableReqMsg);
+
         res.uiReturn = 1;
     }
     catch (ros::Exception ex)
@@ -455,7 +452,33 @@ bool Canbus::StrActCtrRequest(canbus::StrActCtrReq::Request &req,
 }
 
 
+bool Canbus::RBTManager(canbus::RBTManager::Request &req,
+                      canbus::RBTManager::Response &res)
+{
+    LOG(INFO)<< "RBTManager start ActuatorEnableRequest";
+
+    try{
+        T_RBT_MANAGER request;
+        request.uiAccMotInt = req.uiAccMotInt;
+        request.uiBrkMotInt = req.uiBrkMotInt;
+        request.uiClchMotInt = req.uiClchMotInt;
+        request.uiFltClr = req.uiFltClr;
+        request.uiStrMotInt= req.uiStrMotInt;
+        request.uiXSftMotInt = req.uiXSftMotInt;
+        request.uiYSftMotInt = req.uiYSftMotInt;
 
+        MessageCoder::CanMsgPackage((void*)&request, robot::common::RobotManager, &canRobotManagerMsg);
+
+        res.uiReturn = 1;
+    }
+    catch (ros::Exception ex)
+    {
+        res.uiReturn = 0;
+        LOG(ERROR)<< "RBTManager ActuatorEnableRequest failed "<< ex.what();
+        return false;
+    }
+    return true;
+}
 
 
 

src/canbus/canbus.h

@@ -30,6 +30,7 @@
 #include <canbus/PedalActCtrReq.h>
 #include <canbus/SftActCtrReq.h>
 #include <canbus/StrActCtrReq.h>
+#include <canbus/RBTManager.h>
 
 
 /**
@@ -144,6 +145,9 @@ class Canbus : public robot::common::RobotApp{
   bool StrActCtrRequest(canbus::StrActCtrReq::Request &req,
                         canbus::StrActCtrReq::Response &res);
 
+  bool RBTManager(canbus::RBTManager::Request &req,
+                        canbus::RBTManager::Response &res);
+
 private:
 
   int64_t last_timestamp_ = 0;
@@ -179,6 +183,12 @@ private:
 
   canbus::obdMsg obd; // obd data
 
+  // actuator enable request
+  TPCANMsg canActuaEnableReqMsg;
+
+  // robot manager
+  TPCANMsg canRobotManagerMsg;
+
 };
 
 }  // namespace canbus

src/canbus/srv/RBTManager.srv

@@ -0,0 +1,9 @@
+uint32 uiAccMotInt		#油门电机初始化
+uint32 uiBrkMotInt		#制动电机初始化
+uint32 uiClchMotInt		#离合电机初始化
+uint32 uiStrMotInt		#转向电机初始化
+uint32 uiXSftMotInt		#档位X电机初始化
+uint32 uiYSftMotInt		#档位Y电机初始化
+uint32 uiFltClr                 #故障清除
+---
+uint32 uiReturn

src/common/message.h

@@ -52,6 +52,8 @@ const std::string SteerActCtrService = "streeractstrservice";
 const std::string PedalActCtrService = "pedalactctrservice";
 
 const std::string CommandModeCmdService = "commandmodecmdservice";
+
+const std::string RBTManagerService = "rbtmanagerservice";
 // -----------------services ----------------------------