添加obd自定义消息发布

src/canbus/canbus.cpp

@@ -50,13 +50,16 @@ int Canbus::Init() {
   //initialize the VehHBrkStat as Hold
   status1.uiVehHBrkStat = 1;
 
-  canRCUData_pub = node_handle_->advertise<canbus::canMsg>(robot::common::CanDataAnalysis, 20000);
-
+  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.";
   return ROBOT_SUCCESS;
 }
 
+
+
 //打开Can总线
 int Canbus::Start() {
   char chVersion[VERSIONSTRING_LEN];            //store information of can version
@@ -102,6 +105,7 @@ int Canbus::Start() {
 //       LOG(INFO)<<Name()<<" Publish CanData :" << "ID:" << rosMsg.id << ",Data:"<<rosMsg.data;
       ExcuteServices();
       read_loop();
+      canObdData_pub.publish(obd);
       ros::spinOnce();
       loop_rate.sleep();
    }
@@ -160,6 +164,17 @@ void Canbus::Stop() {
 }
 
 
+void Canbus::InitObd()
+{
+     obd.AccPsng = 0;
+     obd.BrkPsng = 0;
+     obd.SftPsng = 0;
+     obd.StrAng = 0;
+     obd.VehHBrkStat = 0;
+     obd.VehSpd = 0;
+     obd.VehSwStat = 0;
+}
+
 //read from CAN forever - until manual break
 int Canbus::read_loop()
 {
@@ -172,6 +187,7 @@ int Canbus::read_loop()
         return errno;
     }
     read_data(&(msg.Msg));
+
      // deal receive data
     // check if a CAN status is pending
     if (msg.Msg.MSGTYPE & MSGTYPE_STATUS) {
@@ -198,31 +214,35 @@ int Canbus::read_data(TPCANMsg *pMsgBuff)
     {
        T_OBD_ACC_PSNG tOBDAccPsng = *((T_OBD_ACC_PSNG*)MessageCoder::CanMsgAnalyse(robot::common::ObdAccPsng,pMsgBuff->DATA));
        status1.uiAccPsng = tOBDAccPsng.obdAccPsng;
+       obd.AccPsng = status1.uiAccPsng * 0.1;
     }
     else if(pMsgBuff->ID == robot::common::ObdBrkPsng)
     {
        T_OBD_BRK_PSNG tOBDBrkPsng = *((T_OBD_BRK_PSNG*)MessageCoder::CanMsgAnalyse(robot::common::ObdBrkPsng,pMsgBuff->DATA));
        status1.uiBrkPsng = tOBDBrkPsng.obdBrkPsng;
+       obd.BrkPsng =  status1.uiBrkPsng * 0.1;
     }
     else if(pMsgBuff->ID == robot::common::ObdSftPsng)
     {
        T_OBD_SFT_PSNG tOBDSftPsng = *((T_OBD_SFT_PSNG*)MessageCoder::CanMsgAnalyse(robot::common::ObdSftPsng,pMsgBuff->DATA));
        status1.uiSftPsng = tOBDSftPsng.obdSftPsng;
+       obd.SftPsng = status1.uiSftPsng;
     }
     else if(pMsgBuff->ID == robot::common::ObdVehHBrkStat)
     {
         T_OBD_VEH_BRK_STAT tOBDVehBrkStat = *((T_OBD_VEH_BRK_STAT*)MessageCoder::CanMsgAnalyse(robot::common::ObdVehHBrkStat,pMsgBuff->DATA));
-        status1.uiVehHBrkStat = tOBDVehBrkStat.obdVehHBrkStat;
+       obd.VehHBrkStat = status1.uiVehHBrkStat = tOBDVehBrkStat.obdVehHBrkStat;
     }
     else if(pMsgBuff->ID == robot::common::ObdVehSwStat)
     {
         T_OBD_VEH_SW_STAT tOBDVehSwStat = *((T_OBD_VEH_SW_STAT*)MessageCoder::CanMsgAnalyse(robot::common::ObdVehSwStat,pMsgBuff->DATA));
-        status1.uiVehSwStat = tOBDVehSwStat.obdVehSwStat;
+        obd.VehSwStat = status1.uiVehSwStat = tOBDVehSwStat.obdVehSwStat;
     }
     else if(pMsgBuff->ID == robot::common::ObdStrAng)
     {
        T_OBD_STR_ANG tOBDStrAng = *((T_OBD_STR_ANG*)MessageCoder::CanMsgAnalyse(robot::common::ObdStrAng,pMsgBuff->DATA));
        status2.iStrAng = tOBDStrAng.obdStrAng;
+       obd.StrAng = status2.iStrAng * 0.01;
     }
     else if(pMsgBuff->ID == robot::common::ObdEngSpd)
     {

src/canbus/canbus.h

@@ -25,6 +25,7 @@
 #include "ros/ros.h"
 #include "candatatype.h"
 #include "controlservices.h"
+#include <canbus/obdMsg.h>
 
 
 /**
@@ -84,15 +85,11 @@ class Canbus : public robot::common::RobotApp{
 
 
  private:
-  // void PublishChassis();
-  // void PublishChassisDetail();
   /**
   * @brief timer callback function
   */
   void OnTimer(const ros::TimerEvent &event);
-  // void OnControlCommand(const apollo::control::ControlCommand &control_command);
-  // apollo::common::Status OnError(const std::string &error_msg);
-  // void RegisterCanClients();
+
   //define function pointer,there is a one-to-one mapping between target function and your defined function
   funCAN_Init_TYPE        funCAN_Init;
   funLINUX_CAN_Open_TYPE  funLINUX_CAN_Open;
@@ -130,6 +127,8 @@ class Canbus : public robot::common::RobotApp{
 
   void ExcuteServices();
 
+  void InitObd();
+
 private:
 
   int64_t last_timestamp_ = 0;
@@ -158,11 +157,15 @@ private:
 
   int heatbeat = 0;  //ICUAlive
 
-  // 发布ＲＣＵ数据包
-  ros::Publisher canRCUData_pub;
+
+  ros::Publisher canRCUData_pub; // 发布ＲＣＵ数据包
+
+  ros::Publisher canObdData_pub; // 发布OBD数据包
 
   ControlServices services;
 
+  canbus::obdMsg obd; // obd data
+
 };
 
 }  // namespace canbus

src/common/message.h

@@ -22,6 +22,8 @@ namespace common{
 const std::string Geometry = "geometry"; // utm coords
 
 const std::string CanDataAnalysis = "canDataAnalysis";
+
+const std::string ObdData = "obddata";
 // -----------------topics ----------------------------
 
 // -----------------services ----------------------------