src/canbus/CMakeLists.txt

@@ -219,8 +219,6 @@ add_executable(canbus
     candatatype.h
     messagecoder.h
     messagecoder.cpp
-    controlservices.h
-    controlservices.cpp
  )
 add_dependencies(canbus ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
 target_link_libraries(canbus

src/canbus/canbus.cpp

@@ -50,8 +50,7 @@ int Canbus::Init() {
   //initialize the VehHBrkStat as Hold
   status1.uiVehHBrkStat = 1;
 
-  canRCUData_pub = node_handle_->advertise<canbus::canMsg>(robot::common::CanDataAnalysis, 2000);
-  canObdData_pub = node_handle_->advertise<canbus::obdMsg>(robot::common::ObdData, 2000);
+
   InitObd();
 
   LOG(INFO)<<Name()<<" init end.";
@@ -94,7 +93,20 @@ int Canbus::Start() {
   timer_ = CreateTimer(ros::Duration(duration), &Canbus::OnTimer, this);
 
   // this location is necessary
-  CreateServices();
+
+  canRCUData_pub = node_handle_->advertise<canbus::canMsg>(robot::common::CanDataAnalysis, 2000);
+  canObdData_pub = node_handle_->advertise<canbus::obdMsg>(robot::common::ObdData, 2000);
+
+  ros::ServiceServer ActuatorEnableService = node_handle_->advertiseService(robot::common::ActuatorEnableService,
+                                                                &Canbus::ActuatorEnableRequest, this);
+  ros::ServiceServer LearnModeService = node_handle_->advertiseService(robot::common::LearnModeService,
+                                                            &Canbus::ExcuteLearnModeRequest, this);
+  ros::ServiceServer ShiftActCtrService = node_handle_->advertiseService(robot::common::ShiftActCtrService,
+                                                              &Canbus::SftActCtrRequest, this);
+  ros::ServiceServer SteerActCtrService = node_handle_->advertiseService(robot::common::SteerActCtrService,
+                                                              &Canbus::StrActCtrRequest, this);
+  ros::ServiceServer PedalActCtrService = node_handle_->advertiseService(robot::common::PedalActCtrService,
+                                                              &Canbus::PedalActCtrRequest, this);
   //data receive
   while(ros::ok())
   {
@@ -103,7 +115,7 @@ int Canbus::Start() {
 //       rosMsg.data = "test";
 //       canRCUData_pub.publish(rosMsg);
 //       LOG(INFO)<<Name()<<" Publish CanData :" << "ID:" << rosMsg.id << ",Data:"<<rosMsg.data;
-      ExcuteServices();
+
       read_loop();
       canObdData_pub.publish(obd);
       ros::spinOnce();
@@ -113,6 +125,8 @@ int Canbus::Start() {
 }
 
 
+
+
 void Canbus::OnTimer(const ros::TimerEvent &) {
   LOG(INFO)<< Name()<< " timer start ..";
   if(heatbeat > 15)
@@ -293,67 +307,160 @@ int Canbus::write_data(TPCANMsg *pMsgBuff)
 }
 
 
-void Canbus::CreateServices()
+bool Canbus::ActuatorEnableRequest(canbus::ActuatorEnableRequest::Request &req,
+                           canbus::ActuatorEnableRequest::Response &res)
 {
-    ros::ServiceServer ActuatorEnableService = node_handle_->advertiseService(robot::common::ActuatorEnableService,
-                                                              &ControlServices::ActuatorEnableRequest, &services);
+    LOG(INFO)<< "controlservice start ActuatorEnableRequest";
+    // actuator enable request
+    TPCANMsg canActuaEnableReqMsg;
+    try{
+        T_ACTUATOR_ENABLE_REQUEST request;
+        request.uiAccClchCtrReq = req.uiAccClchCtrReq;
+        request.uiAccMotEnReq = req.uiAccMotEnReq;
+        request.uiBrkMotEnReq = req.uiBrkMotEnReq;
+        request.uiClchMotEnReq = req.uiClchMotEnReq;
+        request.uiCtrMdReq= req.uiCtrMdReq;
+        request.uiEstpEnReq = req.uiEstpEnReq;
+        request.uiPauseEnReq = req.uiPauseEnReq;
+        request.uiPmpEnReq = req.uiPmpEnReq;
+        request.uiStrMotEnReq = req.uiStrMotEnReq;
+        request.uiXSftMotEnReq = req.uiXSftMotEnReq;
+        request.uiYSftMotEnReq = req.uiYSftMotEnReq;
+
+        MessageCoder::CanMsgPackage((void*)&request, robot::common::ActuatorEnableReq, &canActuaEnableReqMsg);
+        write_data(&canActuaEnableReqMsg);
+        res.uiReturn = 1;
+    }
+    catch (ros::Exception ex)
+    {
+        res.uiReturn = 0;
+        LOG(ERROR)<< "controlservice ActuatorEnableRequest failed "<< ex.what();
+        return false;
+    }
+    return true;
+}
 
-    ros::ServiceServer LearnModeService = node_handle_->advertiseService(robot::common::LearnModeService,
-                                                              &ControlServices::ExcuteLearnModeRequest, &services);
 
-    ros::ServiceServer ShiftActCtrService = node_handle_->advertiseService(robot::common::ShiftActCtrService,
-                                                              &ControlServices::SftActCtrRequest, &services);
+bool Canbus::ExcuteLearnModeRequest(canbus::LearnModeRequest::Request & req,
+                           canbus::LearnModeRequest::Response &res)
+{
+    LOG(INFO)<< "controlservice start ExcuteLearnModeRequest";
+    // learn mode request
+    TPCANMsg canLearnModeReqMsg;
+    try{
+        T_LEARN_MODE_REQUEST request;
+        request.uiClrAccTbl = req.uiClrAccTbl;
+        request.uiClrBrkTbl = req.uiClrBrkTbl;
+        request.uiClrClchTbl = req.uiClrClchTbl;
+        request.uiClrSftTbl = req.uiClrSftTbl;
+        request.uiClrStrTbl= req.uiClrStrTbl;
+        request.uiPsngLrnReq = req.uiPsngLrnReq;
+        request.uiSftAutLrnFnsConf = req.uiSftAutLrnFnsConf;
+        request.uiSftPsngLrnReq = req.uiSftPsngLrnReq;
+        request.uiStpAutLrn = req.uiStpAutLrn;
+        request.uiSysLrnReq = req.uiSysLrnReq;
+
+        MessageCoder::CanMsgPackage((void*)&request, robot::common::LearnModeReq, &canLearnModeReqMsg);
+        write_data(&canLearnModeReqMsg);
+        res.uiReturn = 1;
+    }
+    catch (ros::Exception ex)
+    {
+        res.uiReturn = 0;
+        LOG(ERROR)<< "controlservice ExcuteLearnModeRequest failed "<< ex.what();
+        return false;
+    }
+    return true;
+}
 
-    ros::ServiceServer SteerActCtrService = node_handle_->advertiseService(robot::common::SteerActCtrService,
-                                                              &ControlServices::StrActCtrRequest, &services);
 
-    ros::ServiceServer PedalActCtrService = node_handle_->advertiseService(robot::common::PedalActCtrService,
-                                                              &ControlServices::PedalActCtrRequest, &services);
+bool Canbus::PedalActCtrRequest(canbus::PedalActCtrReq::Request &req,
+                        canbus::PedalActCtrReq::Response &res)
+{
+    LOG(INFO)<< "controlservice start PedalActCtrRequest";
+    // PedalActCtr request
+    TPCANMsg canPedalActCtrReqMsg;
+    try{
+        T_PEDAL_ACT_CTR_REQ request;
+        request.iAccSpdCtrReq = req.iAccSpdCtrReq;
+        request.iBrkSpdCtrReq = req.iBrkSpdCtrReq;
+        request.uiAccPsngCtrReq = req.uiAccPsngCtrReq;
+        request.uiBrkCtrMdReq = req.uiBrkCtrMdReq;
+        request.uiBrkFocCtrReq = req.uiBrkFocCtrReq;
+        request.uiBrkFocCtrReq = req.uiBrkFocCtrReq;
+        request.uiBrkPsngCtrReq = req.uiBrkPsngCtrReq;
+
+        MessageCoder::CanMsgPackage((void*)&request, robot::common::PedalActCtrReq, &canPedalActCtrReqMsg);
+        write_data(&canPedalActCtrReqMsg);
+        res.uiReturn = 1;
+    }
+    catch (ros::Exception ex)
+    {
+        res.uiReturn = 0;
+        LOG(ERROR)<< "controlservice PedalActCtrRequest failed "<< ex.what();
+        return false;
+    }
+    return true;
 }
 
-
-void Canbus::ExcuteServices()
+bool Canbus::SftActCtrRequest(canbus::SftActCtrReq::Request &req,
+                      canbus::SftActCtrReq::Response &res)
 {
-    try
+    LOG(INFO)<< "controlservice start SftActCtrRequest";
+    // SftActCtr request
+    TPCANMsg canSftActCtrReqMsg;
+    try{
+        T_SFT_ACT_CTR_REQ request;
+        request.iClchSpdCtrReq = req.iClchSpdCtrReq;
+        request.uiClchPsngCtrReq = req.uiClchPsngCtrReq;
+        request.uiSftMdReq = req.uiSftMdReq;
+        request.uiSftPsngCtrReq = req.uiSftPsngCtrReq;
+
+        MessageCoder::CanMsgPackage((void*)&request, robot::common::ShiftActCtrReq, &canSftActCtrReqMsg);
+        write_data(&canSftActCtrReqMsg);
+        res.uiReturn = 1;
+    }
+    catch (ros::Exception ex)
     {
-        if (services.bLearnModeReq)
-        {
-              write_data(&services.canLearnModeReqMsg);
-              services.bLearnModeReq = false;
-              LOG(INFO)<<Name()<< " ExcuteServices  excute LearnModeReq success. ";
-        }
-        if (services.bActuaEnableReq)
-        {
-              write_data(&services.canActuaEnableReqMsg);
-              services.bActuaEnableReq = false;
-              LOG(INFO)<<Name()<< " ExcuteServices  excute ActuaEnableReq success. ";
-        }
-        if (services.bPedalActCtrReq)
-        {
-              write_data(&services.canPedalActCtrReqMsg);
-              services.bPedalActCtrReq = false;
-              LOG(INFO)<<Name()<< " ExcuteServices  excute PedalActCtrReq success. ";
-        }
-        if (services.bSftActCtrReq)
-        {
-              write_data(&services.canSftActCtrReqMsg);
-              services.bSftActCtrReq = false;
-              LOG(INFO)<<Name()<< " ExcuteServices  excute SftActCtrReq success. ";
-        }
-        if (services.bStrActCtrReq)
-        {
-              write_data(&services.canStrActCtrReqMsg);
-              services.bStrActCtrReq = false;
-              LOG(INFO)<<Name()<< " ExcuteServices  excute StrActCtrReq success. ";
-        }
+        res.uiReturn = 0;
+        LOG(ERROR)<< "controlservice SftActCtrRequest failed "<< ex.what();
+        return false;
+    }
+    return true;
+}
+
+bool Canbus::StrActCtrRequest(canbus::StrActCtrReq::Request &req,
+                      canbus::StrActCtrReq::Response &res)
+{
+    LOG(INFO)<< "controlservice start StrActCtrRequest";
+    // StrActCtr request
+    TPCANMsg canStrActCtrReqMsg;
+    try{
+        T_STR_ACT_CTR_REQ request;
+        request.iStrAngCtrReq = req.iStrAngCtrReq;
+        request.iStrSpdCtrReq = req.iStrSpdCtrReq;
+        request.iStrTrqCtrReq = req.iStrTrqCtrReq;
+        request.uiStrCtrMdReq = req.uiStrCtrMdReq;
+
+        MessageCoder::CanMsgPackage((void*)&request, robot::common::SteerActCtrReq, &canStrActCtrReqMsg);
+        write_data(&canStrActCtrReqMsg);
+        res.uiReturn = 1;
     }
     catch (ros::Exception ex)
     {
-        LOG(ERROR)<<Name()<< " ExcuteServices error. "<< ex.what();
+        res.uiReturn = 0;
+        LOG(ERROR)<< "controlservice StrActCtrRequest failed "<< ex.what();
+        return false;
     }
+    return true;
 }
 
 
 
+
+
+
+
+
 }  // namespace canbus
 }  // namespace robot

src/canbus/canbus.h

@@ -24,8 +24,12 @@
 #include "../common/robotapp.h"
 #include "ros/ros.h"
 #include "candatatype.h"
-#include "controlservices.h"
 #include <canbus/obdMsg.h>
+#include <canbus/ActuatorEnableRequest.h>
+#include <canbus/LearnModeRequest.h>
+#include <canbus/PedalActCtrReq.h>
+#include <canbus/SftActCtrReq.h>
+#include <canbus/StrActCtrReq.h>
 
 
 /**
@@ -85,6 +89,8 @@ class Canbus : public robot::common::RobotApp{
 
 
  private:
+
+  void InitObd();
   /**
   * @brief timer callback function
   */
@@ -123,11 +129,20 @@ class Canbus : public robot::common::RobotApp{
   /**
   * @brief create services
   */
-  void CreateServices();
+  bool ActuatorEnableRequest(canbus::ActuatorEnableRequest::Request &req,
+                             canbus::ActuatorEnableRequest::Response &res);
 
-  void ExcuteServices();
+  bool ExcuteLearnModeRequest(canbus::LearnModeRequest::Request &req,
+                                canbus::LearnModeRequest::Response &res);
 
-  void InitObd();
+  bool PedalActCtrRequest(canbus::PedalActCtrReq::Request &req,
+                          canbus::PedalActCtrReq::Response &res);
+
+  bool SftActCtrRequest(canbus::SftActCtrReq::Request &req,
+                        canbus::SftActCtrReq::Response &res);
+
+  bool StrActCtrRequest(canbus::StrActCtrReq::Request &req,
+                        canbus::StrActCtrReq::Response &res);
 
 private:
 
@@ -162,8 +177,6 @@ private:
 
   ros::Publisher canObdData_pub; // 发布OBD数据包
 
-  ControlServices services;
-
   canbus::obdMsg obd; // obd data
 
 };

src/canbus/controlservices.cpp

@@ -1,153 +0,0 @@
-#include "controlservices.h"
-#include "candatatype.h"
-#include "../common/gloghelper.h"
-#include "messagecoder.h"
-
-
-namespace robot {
-namespace canbusSpace {
-ControlServices::ControlServices()
-{
-
-}
-
-bool ControlServices::ActuatorEnableRequest(canbus::ActuatorEnableRequest::Request &req,
-                           canbus::ActuatorEnableRequest::Response &res)
-{
-    LOG(INFO)<< "controlservice start ActuatorEnableRequest";
-    try{
-        T_ACTUATOR_ENABLE_REQUEST request;
-        request.uiAccClchCtrReq = req.uiAccClchCtrReq;
-        request.uiAccMotEnReq = req.uiAccMotEnReq;
-        request.uiBrkMotEnReq = req.uiBrkMotEnReq;
-        request.uiClchMotEnReq = req.uiClchMotEnReq;
-        request.uiCtrMdReq= req.uiCtrMdReq;
-        request.uiEstpEnReq = req.uiEstpEnReq;
-        request.uiPauseEnReq = req.uiPauseEnReq;
-        request.uiPmpEnReq = req.uiPmpEnReq;
-        request.uiStrMotEnReq = req.uiStrMotEnReq;
-        request.uiXSftMotEnReq = req.uiXSftMotEnReq;
-        request.uiYSftMotEnReq = req.uiYSftMotEnReq;
-
-        MessageCoder::CanMsgPackage((void*)&request, robot::common::ActuatorEnableReq, &canActuaEnableReqMsg);
-        res.uiReturn = 1;
-    }
-    catch (ros::Exception ex)
-    {
-        res.uiReturn = 0;
-        LOG(ERROR)<< "controlservice ActuatorEnableRequest failed "<< ex.what();
-        return false;
-    }
-    bActuaEnableReq = true;
-    return true;
-}
-
-
-bool ControlServices::ExcuteLearnModeRequest(canbus::LearnModeRequest::Request & req,
-                           canbus::LearnModeRequest::Response &res)
-{
-    LOG(INFO)<< "controlservice start ExcuteLearnModeRequest";
-    try{
-        T_LEARN_MODE_REQUEST request;
-        request.uiClrAccTbl = req.uiClrAccTbl;
-        request.uiClrBrkTbl = req.uiClrBrkTbl;
-        request.uiClrClchTbl = req.uiClrClchTbl;
-        request.uiClrSftTbl = req.uiClrSftTbl;
-        request.uiClrStrTbl= req.uiClrStrTbl;
-        request.uiPsngLrnReq = req.uiPsngLrnReq;
-        request.uiSftAutLrnFnsConf = req.uiSftAutLrnFnsConf;
-        request.uiSftPsngLrnReq = req.uiSftPsngLrnReq;
-        request.uiStpAutLrn = req.uiStpAutLrn;
-        request.uiSysLrnReq = req.uiSysLrnReq;
-
-        MessageCoder::CanMsgPackage((void*)&request, robot::common::LearnModeReq, &canLearnModeReqMsg);
-        res.uiReturn = 1;
-    }
-    catch (ros::Exception ex)
-    {
-        res.uiReturn = 0;
-        LOG(ERROR)<< "controlservice ExcuteLearnModeRequest failed "<< ex.what();
-        return false;
-    }
-    bLearnModeReq = true;
-    return true;
-}
-
-bool ControlServices::PedalActCtrRequest(canbus::PedalActCtrReq::Request &req,
-                        canbus::PedalActCtrReq::Response &res)
-{
-    LOG(INFO)<< "controlservice start PedalActCtrRequest";
-    try{
-        T_PEDAL_ACT_CTR_REQ request;
-        request.iAccSpdCtrReq = req.iAccSpdCtrReq;
-        request.iBrkSpdCtrReq = req.iBrkSpdCtrReq;
-        request.uiAccPsngCtrReq = req.uiAccPsngCtrReq;
-        request.uiBrkCtrMdReq = req.uiBrkCtrMdReq;
-        request.uiBrkFocCtrReq = req.uiBrkFocCtrReq;
-        request.uiBrkFocCtrReq = req.uiBrkFocCtrReq;
-        request.uiBrkPsngCtrReq = req.uiBrkPsngCtrReq;
-
-        MessageCoder::CanMsgPackage((void*)&request, robot::common::PedalActCtrReq, &canPedalActCtrReqMsg);
-        res.uiReturn = 1;
-    }
-    catch (ros::Exception ex)
-    {
-        res.uiReturn = 0;
-        LOG(ERROR)<< "controlservice PedalActCtrRequest failed "<< ex.what();
-        return false;
-    }
-    bPedalActCtrReq = true;
-    return true;
-}
-
-bool ControlServices::SftActCtrRequest(canbus::SftActCtrReq::Request &req,
-                      canbus::SftActCtrReq::Response &res)
-{
-    LOG(INFO)<< "controlservice start SftActCtrRequest";
-    try{
-        T_SFT_ACT_CTR_REQ request;
-        request.iClchSpdCtrReq = req.iClchSpdCtrReq;
-        request.uiClchPsngCtrReq = req.uiClchPsngCtrReq;
-        request.uiSftMdReq = req.uiSftMdReq;
-        request.uiSftPsngCtrReq = req.uiSftPsngCtrReq;
-
-        MessageCoder::CanMsgPackage((void*)&request, robot::common::ShiftActCtrReq, &canSftActCtrReqMsg);
-        res.uiReturn = 1;
-    }
-    catch (ros::Exception ex)
-    {
-        res.uiReturn = 0;
-        LOG(ERROR)<< "controlservice SftActCtrRequest failed "<< ex.what();
-        return false;
-    }
-    bSftActCtrReq = true;
-    return true;
-}
-
-bool ControlServices::StrActCtrRequest(canbus::StrActCtrReq::Request &req,
-                      canbus::StrActCtrReq::Response &res)
-{
-    LOG(INFO)<< "controlservice start StrActCtrRequest";
-    try{
-        T_STR_ACT_CTR_REQ request;
-        request.iStrAngCtrReq = req.iStrAngCtrReq;
-        request.iStrSpdCtrReq = req.iStrSpdCtrReq;
-        request.iStrTrqCtrReq = req.iStrTrqCtrReq;
-        request.uiStrCtrMdReq = req.uiStrCtrMdReq;
-
-        MessageCoder::CanMsgPackage((void*)&request, robot::common::SteerActCtrReq, &canStrActCtrReqMsg);
-        res.uiReturn = 1;
-    }
-    catch (ros::Exception ex)
-    {
-        res.uiReturn = 0;
-        LOG(ERROR)<< "controlservice StrActCtrRequest failed "<< ex.what();
-        return false;
-    }
-    bStrActCtrReq = true;
-    return true;
-}
-
-
-}
-}

src/canbus/controlservices.h

@@ -1,68 +0,0 @@
-#ifndef CONTROLSERVICES_H
-#define CONTROLSERVICES_H
-
-#include "canbus/LearnModeRequest.h"
-#include "canbus/ActuatorEnableRequest.h"
-#include "canbus/PedalActCtrReq.h"
-#include "canbus/SftActCtrReq.h"
-#include "canbus/StrActCtrReq.h"
-#include "../common/canmsgobdmsgid.h"
-#include <libpcan.h>
-
-/**
- * @namespace robot::canbus
- * @brief robot::canbus
- */
-namespace robot {
-namespace canbusSpace {
-
-
-class ControlServices
-{
-public:
-    ControlServices();
-
-    bool ActuatorEnableRequest(canbus::ActuatorEnableRequest::Request &req,
-                               canbus::ActuatorEnableRequest::Response &res);
-
-    bool ExcuteLearnModeRequest(canbus::LearnModeRequest::Request &req,
-                                  canbus::LearnModeRequest::Response &res);
-
-    bool PedalActCtrRequest(canbus::PedalActCtrReq::Request &req,
-                            canbus::PedalActCtrReq::Response &res);
-
-    bool SftActCtrRequest(canbus::SftActCtrReq::Request &req,
-                          canbus::SftActCtrReq::Response &res);
-
-    bool StrActCtrRequest(canbus::StrActCtrReq::Request &req,
-                          canbus::StrActCtrReq::Response &res);
-
-public:
-
-   // actuator enable request
-   TPCANMsg canActuaEnableReqMsg;
-   // actuator enable request write Flag
-   bool bActuaEnableReq;
-   // learn mode request
-   TPCANMsg canLearnModeReqMsg;
-   // learnmode request write Flag
-   bool bLearnModeReq;
-   // PedalActCtr request
-   TPCANMsg canPedalActCtrReqMsg;
-   // PedalActCtr request write Flag
-   bool bPedalActCtrReq;
-   // SftActCtr request
-   TPCANMsg canSftActCtrReqMsg;
-   // SftActCtr request write Flag
-   bool bSftActCtrReq;
-   // StrActCtr request
-   TPCANMsg canStrActCtrReqMsg;
-   // StrActCtr request write Flag
-   bool bStrActCtrReq;
-
-};
-
-}
-}
-
-#endif // CONTROLSERVICES_H

src/decision/decision.cpp

@@ -11,15 +11,15 @@ namespace decision {
 std::string Decision::Name() const { return "decision"; }
 
 int Decision::Init(){
-    LOG(INFO)<<Name()<<":Decision init...";
-
-    return 0;
+    LOG(INFO)<<Name()<<"Decision init...";
+    return ROBOT_SUCCESS;
 }
 
 int Decision::Start(){
-    LOG(INFO)<<Name()<<":Decision start...";
-    return 0;
+    LOG(INFO)<<"Decision start...";
+    return ROBOT_SUCCESS;
 }
+
 void Decision::Stop(){
     LOG(INFO)<<Name()<<":Decision stop...";
     return ;

src/decision/decision.h

@@ -16,6 +16,9 @@
 namespace robot {
 namespace decision {
 
+#define ROBOT_SUCCESS     0
+#define ROBOT_FAILTURE   -1
+
 /**
 * @class Decision
 *
@@ -134,6 +137,8 @@ public:
     int DeleteLocationRuleInfo(int iLocationID);
 
 };
+
+
 }
 }
 #endif // ROBOT_ROBOT_DECISION_H