数据库操作封装成静态类

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/common/message.h

@@ -24,6 +24,8 @@ const std::string Geometry = "geometry"; // utm coords
 const std::string CanDataAnalysis = "canDataAnalysis";
 
 const std::string ObdData = "obddata";
+
+const std::string PerceptionData = "perceptionData";
 // -----------------topics ----------------------------
 
 // -----------------services ----------------------------

src/decision/decision.cpp

@@ -11,6 +11,7 @@ namespace decision {
 std::string Decision::Name() const { return "decision"; }
 
 int Decision::Init(){
+<<<<<<< HEAD
     LOG(INFO)<<Name()<<":Decision init...";
     Database::InitDB();
     Database::InsertActionDirective("1",1,"1",1);
@@ -24,12 +25,17 @@ int Decision::Init(){
 //    Database::DeleteLocationRuleInfo("1");
     Database::DeleteLocationRuleInfo(1);
     return 0;
+=======
+    LOG(INFO)<<Name()<<"Decision init...";
+    return ROBOT_SUCCESS;
+>>>>>>> 759ecb392596608d195b9c0ff234e1e42401a6f8
 }
 
 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
 *
@@ -57,6 +60,8 @@ public:
     void Stop() override;
 
 };
+
+
 }
 }
 #endif // ROBOT_ROBOT_DECISION_H

src/perception/CMakeLists.txt

@@ -0,0 +1,224 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(perception)
+
+## Compile as C++11, supported in ROS Kinetic and newer
+add_compile_options(-std=c++11)
+
+## Find catkin macros and libraries
+## 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
+)
+
+## System dependencies are found with CMake's conventions
+# find_package(Boost REQUIRED COMPONENTS system)
+
+
+## Uncomment this if the package has a setup.py. This macro ensures
+## modules and global scripts declared therein get installed
+## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
+# catkin_python_setup()
+
+################################################
+## Declare ROS messages, services and actions ##
+################################################
+
+## To declare and build messages, services or actions from within this
+## package, follow these steps:
+## * Let MSG_DEP_SET be the set of packages whose message types you use in
+##   your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
+## * In the file package.xml:
+##   * add a build_depend tag for "message_generation"
+##   * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET
+##   * If MSG_DEP_SET isn't empty the following dependency has been pulled in
+##     but can be declared for certainty nonetheless:
+##     * add a exec_depend tag for "message_runtime"
+## * In this file (CMakeLists.txt):
+##   * add "message_generation" and every package in MSG_DEP_SET to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * add "message_runtime" and every package in MSG_DEP_SET to
+##     catkin_package(CATKIN_DEPENDS ...)
+##   * uncomment the add_*_files sections below as needed
+##     and list every .msg/.srv/.action file to be processed
+##   * uncomment the generate_messages entry below
+##   * 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
+# )
+
+## Generate services in the 'srv' folder
+# add_service_files(
+#   FILES
+#   Service1.srv
+#   Service2.srv
+# )
+
+## Generate actions in the 'action' folder
+# add_action_files(
+#   FILES
+#   Action1.action
+#   Action2.action
+# )
+
+## Generate added messages and services with any dependencies listed here
+#generate_messages(
+#  DEPENDENCIES
+#  std_msgs  # Or other packages containing msgs
+#)
+
+################################################
+## Declare ROS dynamic reconfigure parameters ##
+################################################
+
+## To declare and build dynamic reconfigure parameters within this
+## package, follow these steps:
+## * In the file package.xml:
+##   * add a build_depend and a exec_depend tag for "dynamic_reconfigure"
+## * In this file (CMakeLists.txt):
+##   * add "dynamic_reconfigure" to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * uncomment the "generate_dynamic_reconfigure_options" section below
+##     and list every .cfg file to be processed
+
+## Generate dynamic reconfigure parameters in the 'cfg' folder
+# generate_dynamic_reconfigure_options(
+#   cfg/DynReconf1.cfg
+#   cfg/DynReconf2.cfg
+# )
+
+###################################
+## catkin specific configuration ##
+###################################
+## The catkin_package macro generates cmake config files for your package
+## Declare things to be passed to dependent projects
+## INCLUDE_DIRS: uncomment this if your package contains header files
+## LIBRARIES: libraries you create in this project that dependent projects also need
+## CATKIN_DEPENDS: catkin_packages dependent projects also need
+## DEPENDS: system dependencies of this project that dependent projects also need
+catkin_package(
+#  INCLUDE_DIRS include
+#  LIBRARIES perception
+#  CATKIN_DEPENDS
+#  message_runtime
+#  CATKIN_DEPENDS roscpp message_generation std_msgs rospy
+#  DEPENDS system_lib
+)
+
+###########
+## Build ##
+###########
+
+## Specify additional locations of header files
+## Your package locations should be listed before other locations
+include_directories(
+# include
+  ${catkin_INCLUDE_DIRS}
+  /usr/local/include
+)
+
+## Declare a C++ library
+# add_library(${PROJECT_NAME}
+#   src/${PROJECT_NAME}/perception.cpp
+# )
+
+## Add cmake target dependencies of the library
+## as an example, code may need to be generated before libraries
+## either from message generation or dynamic reconfigure
+# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Declare a C++ executable
+## With catkin_make all packages are built within a single CMake context
+## The recommended prefix ensures that target names across packages don't collide
+# add_executable(${PROJECT_NAME}_node src/perception_node.cpp)
+
+## Rename C++ executable without prefix
+## The above recommended prefix causes long target names, the following renames the
+## target back to the shorter version for ease of user use
+## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
+# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
+
+## Add cmake target dependencies of the executable
+## same as for the library above
+# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Specify libraries to link a library or executable target against
+# target_link_libraries(${PROJECT_NAME}_node
+#   ${catkin_LIBRARIES}
+# )
+
+#############
+## Install ##
+#############
+
+# all install targets should use catkin DESTINATION variables
+# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
+
+## Mark executable scripts (Python etc.) for installation
+## in contrast to setup.py, you can choose the destination
+# install(PROGRAMS
+#   scripts/my_python_script
+#   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark executables for installation
+## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html
+# install(TARGETS ${PROJECT_NAME}_node
+#   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark libraries for installation
+## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html
+# install(TARGETS ${PROJECT_NAME}
+#   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
+# )
+
+## Mark cpp header files for installation
+# install(DIRECTORY include/${PROJECT_NAME}/
+#   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+#   FILES_MATCHING PATTERN "*.h"
+#   PATTERN ".svn" EXCLUDE
+# )
+
+## Mark other files for installation (e.g. launch and bag files, etc.)
+# install(FILES
+#   # myfile1
+#   # myfile2
+#   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+# )
+
+#############
+## Testing ##
+#############
+
+## Add gtest based cpp test target and link libraries
+# catkin_add_gtest(${PROJECT_NAME}-test test/test_perception.cpp)
+# if(TARGET ${PROJECT_NAME}-test)
+#   target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
+# endif()
+
+## Add folders to be run by python nosetests
+# catkin_add_nosetests(test)
+
+add_executable(perception
+    ../common/time.h
+    ../common/gloghelper.h
+    ../common/gloghelper.cpp
+    ../common/robotapp.h
+    ../common/robotapp.cpp
+    ../canbus/candatatype.h
+    perception_app.cpp
+    perception.h
+    perception.cpp
+ )
+
+target_link_libraries(perception
+    ${catkin_LIBRARIES}
+    glog
+    gflags)

src/perception/package.xml

@@ -0,0 +1,64 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>perception</name>
+  <version>0.1.0</version>
+  <description>The perception package</description>
+
+  <!-- One maintainer tag required, multiple allowed, one person per tag -->
+  <!-- Example:  -->
+  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+  <maintainer email="zhangyu@todo.todo">zhangyu</maintainer>
+
+
+  <!-- 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>
+
+
+  <!-- Url tags are optional, but multiple are allowed, one per tag -->
+  <!-- Optional attribute type can be: website, bugtracker, or repository -->
+  <!-- Example: -->
+  <!-- <url type="website">http://wiki.ros.org/perception</url> -->
+
+
+  <!-- Author tags are optional, multiple are allowed, one per tag -->
+  <!-- Authors do not have to be maintainers, but could be -->
+  <!-- Example: -->
+  <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
+  <author >zhangyu</author>
+
+
+  <!-- The *depend tags are used to specify dependencies -->
+  <!-- Dependencies can be catkin packages or system dependencies -->
+  <!-- Examples: -->
+  <!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
+  <!--   <depend>roscpp</depend> -->
+  <!--   Note that this is equivalent to the following: -->
+  <!--   <build_depend>roscpp</build_depend> -->
+  <!--   <exec_depend>roscpp</exec_depend> -->
+  <!-- Use build_depend for packages you need at compile time: -->
+  <!--   <build_depend>message_generation</build_depend> -->
+  <!-- Use build_export_depend for packages you need in order to build against this package: -->
+  <!--   <build_export_depend>message_generation</build_export_depend> -->
+  <!-- Use buildtool_depend for build tool packages: -->
+  <!--   <buildtool_depend>catkin</buildtool_depend> -->
+  <!-- Use exec_depend for packages you need at runtime: -->
+  <!--   <exec_depend>message_runtime</exec_depend> -->
+  <!-- Use test_depend for packages you need only for testing: -->
+  <!--   <test_depend>gtest</test_depend> -->
+  <!-- Use doc_depend for packages you need only for building documentation: -->
+  <!--   <doc_depend>doxygen</doc_depend> -->
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>roscpp message_generation std_msgs rospy</build_depend>
+  <build_export_depend>roscpp message_generation std_msgs rospy</build_export_depend>
+  <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 -->
+
+  </export>
+</package>

src/perception/perception.cpp

@@ -0,0 +1,95 @@
+#include "perception.h"
+#include "../common/message.h"
+#include "../common/canmsgobdmsgid.h"
+#include "../canbus/candatatype.h"
+
+
+
+
+namespace  robot {
+namespace perception {
+
+
+ros::Publisher Perception::s_perData_pub;
+
+std::string Perception::Name() const { return "perception"; }
+
+int Perception::Init()
+{
+    LOG(INFO) << Name() << " Init start...";
+
+    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);
+    ros::spin();
+}
+
+void Perception::Stop()
+{
+
+}
+
+
+void Perception::CanTopicCallback(const canbus::canMsg::ConstPtr& msg)
+{
+    LOG(INFO) << " Msg CanData :" << "ID:" << msg->id << ",Data:" << msg->data;
+
+    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;
+        }
+    }
+        break;
+    default:
+        break;
+    }
+
+    if (sendFlag)
+    {
+        s_perData_pub.publish(msg);
+    }
+}
+
+}  // namespace perception
+}  // namespace robot

src/perception/perception.h

@@ -0,0 +1,53 @@
+#ifndef ROBOT_ROBOT_PERCEPTION_H
+#define ROBOT_ROBOT_PERCEPTION_H
+
+#include "../common/robotapp.h"
+#include "ros/ros.h"
+#include "std_msgs/String.h"
+#include <canbus/canMsg.h>
+
+namespace robot {
+namespace perception {
+
+class Perception : public robot::common::RobotApp
+{
+public:
+    Perception() {}
+
+    /**
+    * perception 节点名称
+    */
+    std::string Name() const override;
+
+    /**
+    * @brief module initialization function
+    * @return initialization status
+    */
+    int Init() override;
+
+    /**
+    * @brief module start function
+    * @return start status
+    */
+    int Start() override;
+
+    /**
+    * @brief module stop function
+    */
+    void Stop() override;
+
+    /**
+    * @brief Can topic data callback function
+    */
+    static void CanTopicCallback(const canbus::canMsg::ConstPtr& msg);
+
+private:
+
+     static ros::Publisher s_perData_pub;
+};
+
+
+}  // namespace perception
+}  // namespace robot
+
+#endif // ROBOT_ROBOT_PERCEPTION_H

src/perception/perception_app.cpp

@@ -0,0 +1,4 @@
+#include "../common/robotapp.h"
+#include "perception.h"
+
+ROBOT_MAIN(robot::perception::Perception);