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

src/canbus/canbus.cpp

@@ -8,6 +8,7 @@
 #include"candatatype.h"
 #include"messagecoder.h"
 #include"../common/messagetopics.h"
+#include"../common/canmsgobdmsgid.h"
 #include "std_msgs/String.h"
 
 namespace  robot{
@@ -79,9 +80,8 @@ int Canbus::Start() {
       LOG(INFO)<<Name()<<": can't open ."<<szDevNode;
   }
 
-  ros::Rate loop_rate(2000);
+  ros::Rate loop_rate(1/rate);
   // 5. set timer to triger publish info periodly
-
   timer_ = CreateTimer(ros::Duration(duration), &Canbus::OnTimer, this);
    //data receive
    while(ros::ok())
@@ -101,10 +101,10 @@ void Canbus::OnTimer(const ros::TimerEvent &) {
   }
   heatbeat ++;
   icuMonitor.uiICUAlive = heatbeat;
-  MessageCoder::CanMsgPackage((void*)&icuMonitor,0x5FA,&canIcuMonitor);
-  MessageCoder::CanMsgPackage((void*)&status1,0x6F1,&canStatus1);
-  MessageCoder::CanMsgPackage((void*)&status2,0x6F2,&canStatus2);
-  MessageCoder::CanMsgPackage((void*)&status3,0x6F3,&canStatus3);
+  MessageCoder::CanMsgPackage((void*)&icuMonitor,robot::common::ICUMonitor,&canIcuMonitor);
+  MessageCoder::CanMsgPackage((void*)&status1,robot::common::VehicleStatus1,&canStatus1);
+  MessageCoder::CanMsgPackage((void*)&status2,robot::common::VehicleStatus2,&canStatus2);
+  MessageCoder::CanMsgPackage((void*)&status3,robot::common::VehicleStatus3,&canStatus3);
   write_data(&canStatus1);
   write_data(&canStatus2);
   write_data(&canStatus3);
@@ -156,86 +156,48 @@ int Canbus::read_data(TPCANMsg *pMsgBuff)
 //    MessageCoder::ConvertUnCharToStr(strData,pMsgBuff->DATA,8);
 
 //    LOG(INFO)<<Name()<<" Receive CanData : ID: "<<std::string(strId)<<",Data: "<<std::string(strData);
-    if(pMsgBuff->ID == 0x1A1)
+    if(pMsgBuff->ID == robot::common::ObdAccPsng)
     {
-//       char strId1[4] = {0};
-//       char strData1[16]={0};
-//       sprintf(strId1,"%3xh",pMsgBuff->ID);
-//       MessageCoder::ConvertUnCharToStr(strData1,pMsgBuff->DATA,8);
-//       LOG(INFO)<<Name()<<" Receive CanData : ID: "<<std::string(strId1)<<",Data: "<<std::string(strData1);
-       T_OBD_ACC_PSNG tOBDAccPsng = *((T_OBD_ACC_PSNG*)MessageCoder::CanMsgAnalyse(0x1A1,pMsgBuff->DATA));
+       T_OBD_ACC_PSNG tOBDAccPsng = *((T_OBD_ACC_PSNG*)MessageCoder::CanMsgAnalyse(robot::common::ObdAccPsng,pMsgBuff->DATA));
        status1.uiAccPsng = tOBDAccPsng.obdAccPsng;
-//       LOG(INFO)<<Name()<<"ID:0x1A1,Data: "<<tOBDAccPsng.obdAccPsng;
     }
-    else if(pMsgBuff->ID == 0x0F1)
+    else if(pMsgBuff->ID == robot::common::ObdBrkPsng)
     {
-//       char strId2[4] = {0};
-//       char strData2[16]={0};
-//       sprintf(strId2,"%3xh",pMsgBuff->ID);
-//       MessageCoder::ConvertUnCharToStr(strData2,pMsgBuff->DATA,8);
-//       LOG(INFO)<<Name()<<" Receive CanData : ID: "<<std::string(strId2)<<",Data: "<<std::string(strData2);
-       T_OBD_BRK_PSNG tOBDBrkPsng = *((T_OBD_BRK_PSNG*)MessageCoder::CanMsgAnalyse(0x0F1,pMsgBuff->DATA));
+       T_OBD_BRK_PSNG tOBDBrkPsng = *((T_OBD_BRK_PSNG*)MessageCoder::CanMsgAnalyse(robot::common::ObdBrkPsng,pMsgBuff->DATA));
        status1.uiBrkPsng = tOBDBrkPsng.obdBrkPsng;
-//       LOG(INFO)<<Name()<<"ID:0x0F1,Data: "<<tOBDBrkPsng.obdBrkPsng;
     }
-    else if(pMsgBuff->ID == 0x135)
+    else if(pMsgBuff->ID == robot::common::ObdSftPsng)
     {
-//       char strId3[4] = {0};
-//       char strData3[16]={0};
-//       sprintf(strId3,"%3xh",pMsgBuff->ID);
-//       MessageCoder::ConvertUnCharToStr(strData3,pMsgBuff->DATA,8);
-//       LOG(INFO)<<Name()<<" Receive CanData : ID: "<<std::string(strId3)<<",Data: "<<std::string(strData3);
-       T_OBD_SFT_PSNG tOBDSftPsng = *((T_OBD_SFT_PSNG*)MessageCoder::CanMsgAnalyse(0x135,pMsgBuff->DATA));
+       T_OBD_SFT_PSNG tOBDSftPsng = *((T_OBD_SFT_PSNG*)MessageCoder::CanMsgAnalyse(robot::common::ObdSftPsng,pMsgBuff->DATA));
        status1.uiSftPsng = tOBDSftPsng.obdSftPsng;
-//      LOG(INFO)<<Name()<<"ID:0x135,Data: "<<tOBDSftPsng.obdSftPsng;
     }
-    else if(pMsgBuff->ID == 0x1F1)
+    else if(pMsgBuff->ID == robot::common::ObdVehHBrkStat)
     {
-//        char strId4[4] = {0};
-//        char strData4[16]={0};
-//        sprintf(strId4,"%3xh",pMsgBuff->ID);
-//        MessageCoder::ConvertUnCharToStr(strData4,pMsgBuff->DATA,8);
-//        LOG(INFO)<<Name()<<" Receive CanData : ID: "<<std::string(strId4)<<",Data: "<<std::string(strData4);
-        T_OBD_VEH_BRK_STAT tOBDVehBrkStat = *((T_OBD_VEH_BRK_STAT*)MessageCoder::CanMsgAnalyse(0x1F1,pMsgBuff->DATA));
+        T_OBD_VEH_BRK_STAT tOBDVehBrkStat = *((T_OBD_VEH_BRK_STAT*)MessageCoder::CanMsgAnalyse(robot::common::ObdVehHBrkStat,pMsgBuff->DATA));
         status1.uiVehHBrkStat = tOBDVehBrkStat.obdVehHBrkStat;
-//        LOG(INFO)<<Name()<<"ID:0x1F1,Data: "<<tOBDVehBrkStat.obdVehHBrkStat;
     }
-    else if(pMsgBuff->ID == 0x121)
+    else if(pMsgBuff->ID == robot::common::ObdVehSwStat)
     {
-//        char strId5[4] = {0};
-//        char strData5[16]={0};
-//        sprintf(strId5,"%3xh",pMsgBuff->ID);
-//        MessageCoder::ConvertUnCharToStr(strData5,pMsgBuff->DATA,8);
-//        LOG(INFO)<<Name()<<" Receive CanData : ID: "<<std::string(strId5)<<",Data: "<<std::string(strData5);
-        T_OBD_VEH_SW_STAT tOBDVehSwStat = *((T_OBD_VEH_SW_STAT*)MessageCoder::CanMsgAnalyse(0x121,pMsgBuff->DATA));
+        T_OBD_VEH_SW_STAT tOBDVehSwStat = *((T_OBD_VEH_SW_STAT*)MessageCoder::CanMsgAnalyse(robot::common::ObdVehSwStat,pMsgBuff->DATA));
         status1.uiVehSwStat = tOBDVehSwStat.obdVehSwStat;
-//        LOG(INFO)<<Name()<<"ID:0x121,Data: "<<tOBDVehSwStat.obdVehSwStat;
     }
-    else if(pMsgBuff->ID == 0x1E5)
+    else if(pMsgBuff->ID == robot::common::ObdStrAng)
     {
-//       char strId6[4] = {0};
-//       char strData6[16]={0};
-//       sprintf(strId6,"%3xh",pMsgBuff->ID);
-//       MessageCoder::ConvertUnCharToStr(strData6,pMsgBuff->DATA,8);
-//       LOG(INFO)<<Name()<<" Receive CanData : ID: "<<std::string(strId6)<<",Data: "<<std::string(strData6);
-       T_OBD_STR_ANG tOBDStrAng = *((T_OBD_STR_ANG*)MessageCoder::CanMsgAnalyse(0x1E5,pMsgBuff->DATA));
+       T_OBD_STR_ANG tOBDStrAng = *((T_OBD_STR_ANG*)MessageCoder::CanMsgAnalyse(robot::common::ObdStrAng,pMsgBuff->DATA));
        status2.iStrAng = tOBDStrAng.obdStrAng;
-//        LOG(INFO)<<Name()<<"ID:0x1E5,Data: "<<tOBDStrAng.obdStrAng;
     }
-    else if(pMsgBuff->ID == 0x0C9)
+    else if(pMsgBuff->ID == robot::common::ObdEngSpd)
     {
-//        char strId7[4] = {0};
-//        char strData7[16]={0};
-//        sprintf(strId7,"%3xh",pMsgBuff->ID);
-//        MessageCoder::ConvertUnCharToStr(strData7,pMsgBuff->DATA,8);
-//        LOG(INFO)<<Name()<<" Receive CanData : ID: "<<std::string(strId7)<<",Data: "<<std::string(strData7);
-        T_OBD_ENG_SPD tOBDEngSpd = *((T_OBD_ENG_SPD*)MessageCoder::CanMsgAnalyse(0x0C9,pMsgBuff->DATA));
-        status2.uiEngSpd = tOBDEngSpd.obdEngSpd;
-        //LOG(INFO)<<Name()<<"ID:0x0C9,Data: "<<tOBDEngSpd.obdEngSpd;
+        //T_OBD_ENG_SPD tOBDEngSpd = *((T_OBD_ENG_SPD*)MessageCoder::CanMsgAnalyse(robot::common::ObdEngSpd,pMsgBuff->DATA));
+        //status2.uiEngSpd = tOBDEngSpd.obdEngSpd;
     }
-    else if((pMsgBuff->ID > 0x5F0 && pMsgBuff->ID < 0x7FF) || pMsgBuff->ID ==0x2E0 || pMsgBuff->ID ==0x2E1)
+    else if((pMsgBuff->ID >= robot::common::PowerOnReq && pMsgBuff->ID <= robot::common::BrakeForceCtrStp) ||
+            (pMsgBuff->ID >= robot::common::SysMonitorBack && pMsgBuff->ID <= robot::common::SteerPosTimeReq) ||
+            (pMsgBuff->ID >= robot::common::RobotAcuatorPos && pMsgBuff->ID <= robot::common::ComputerStaus) ||
+            (pMsgBuff->ID >= robot::common::SteerTranLineCtr && pMsgBuff->ID <= robot::common::VehicleStart) ||
+            pMsgBuff->ID ==robot::common::EstopInfo || pMsgBuff->ID ==robot::common::EstopErr)
     {
-        //publish_data(pMsgBuff);
+        publish_data(pMsgBuff);
 
     }
     return ROBOT_SUCCESS;
@@ -243,24 +205,21 @@ int Canbus::read_data(TPCANMsg *pMsgBuff)
 
 void Canbus::publish_data(TPCANMsg *pMsgBuff)
 {
-    //dataAnalysis::CanData canData;
-    //canData.set_id(pMsgBuff->ID);
-    //canData.set_data(reinterpret_cast<const char*>(pMsgBuff->DATA));
     std_msgs::String ros_msg;
     std::stringstream ss;
     ss << "Id:" << pMsgBuff->ID << ",Data:" << pMsgBuff->DATA;
     ros_msg.data = ss.str();
     ros::Publisher canData_pub = node_handle_->advertise<std_msgs::String>(robot::common::CanDataAnalysis, 200);
     canData_pub.publish(ros_msg);
-    LOG(INFO)<<Name()<<" Publish CanData :"<<ss.str();
+    LOG(INFO)<<Name()<<" Publish CanData :"<<ros_msg.data;
 }
 
 int Canbus::write_data(TPCANMsg *pMsgBuff)
 {
-    char strId[4] = {0};
-    sprintf(strId,"%3xh",pMsgBuff->ID);
-    char strData[16]={0};
-    MessageCoder::ConvertUnCharToStr(strData,pMsgBuff->DATA,8);
+  char strId[4] = {0};
+  sprintf(strId,"%3xh",pMsgBuff->ID);
+  char strData[16]={0};
+  MessageCoder::ConvertUnCharToStr(strData,pMsgBuff->DATA,8);
   LOG(INFO)<<Name()<<" send CanData : ID: "<<std::string(strId)<<",Data:"<<std::string(strData);
   if (funCAN_Write(pcan_handle, pMsgBuff)){
      LOG(ERROR)<<Name()<< " write: LINUX_CAN_write_timeout error. ";

src/canbus/canbus.h

@@ -124,6 +124,8 @@ class Canbus : public robot::common::RobotApp{
   ros::Timer timer_;
 
   const double duration = 0.01;
+
+  const double rate = 0.005;  //s
   //GLOBALS
   void *libm_handle = NULL;//define pointer used for file acess of libpcan.so
   HANDLE pcan_handle =NULL;//void *pcan_handle

src/canbus/messagecoder.h

@@ -6,6 +6,7 @@
 
 namespace robot {
 namespace canbus {
+
 class MessageCoder
 {
 private:

src/common/canmsgobdmsgid.h

@@ -0,0 +1,125 @@
+#ifndef CANMSGOBDMSGID_H
+#define CANMSGOBDMSGID_H
+
+/**
+ * @namespace robot::common
+ * @brief robot::common
+ */
+namespace robot{
+namespace common{
+
+//控制器上电命令
+const int PowerOnReq = 0x5F1;
+//执行机构使能请求
+const int ActuatorEnableReq = 0x5F2;
+//学习模式请求确认
+const int LearnModeReq = 0x5F3;
+//踏板执行机构控制命令
+const int PedalActCtrReq = 0x5F4;
+//转向执行机构控制命令
+const int SteerActCtrReq = 0x5F5;
+//档位执行机构控制命令
+const int ShiftActCtrReq = 0x5F6;
+//机器人自检控制命令
+const int RobotSelfCheckCtrReq = 0x5F7;
+//机器人手动控制命令
+const int RobotDriverCtr = 0x5F8;
+//设备管理
+const int RobotManager = 0x5F9;
+//上位机监控
+const int ICUMonitor = 0x5FA;
+//执行机构加速度控制请求
+const int ActuatorAccelerateCtrReq = 0x5FB;
+//执行机构限值设置
+const int ActuatorCtrLimit = 0x5FC;
+//力矩保护设置
+const int TorqueProtectStp = 0x5FD;
+//制动机构力时间控制设置
+const int BrakeForceCtrStp = 0x5FE;
+//转向三角波
+const int SteerTranLineCtr = 0x7F3;
+//转向正弦控制
+const int SteerSinCtr = 0x7F4;
+//油门机构位置时间控制
+const int AccPositionCtrStp = 0x7F5;
+//方向盘位置时间序列控制
+const int SteerPositionTimeCtr = 0x7F6;
+//车辆一键启动
+const int VehicleStart = 0x7F7;
+//车辆状态1
+const int VehicleStatus1 = 0x6F1;
+//车辆状态2
+const int VehicleStatus2 = 0x6F2;
+//车辆状态3
+const int VehicleStatus3 = 0x6F3;
+//工控机状态
+const int ComputerStaus = 0x6F4;
+//机器人执行机构实际位置
+const int RobotAcuatorPos = 0x6E0;
+//(油门和制动)踏板学习位置反馈
+const int PedalLearnPosBack = 0x6E1;
+//转向学习位置反馈
+const int SteerLearnPosBack = 0x6E2;
+//档位学习位置反馈1
+const int ShiftLearnPosBack1 = 0x6E3;
+//控制器上电状态
+const int CtrPowerStatus = 0x6E4;
+//执行机构使能状态
+const int ActuatorEnableStatus = 0x6E5;
+//油门控制状态反馈
+const int AccPedalStatusBack = 0x6E6;
+//制动踏板控制状态反馈
+const int BrkPedalStatusBack = 0x6E7;
+//离合踏板控制状态反馈
+const int ClutchPedalStatusBack = 0x6E8;
+//转向控制状态反馈
+const int SteerStatusBack = 0x6E9;
+//档位控制状态反馈
+const int ShiftStatusBack = 0x6EA;
+//系统运行状态参数反馈1
+const int RSysInfoBack1 = 0x6EB;
+//系统运行状态参数反馈2
+const int RSysInfoBack2 = 0x6ED;
+//系统学习状态反馈
+const int SysLearnBack = 0x6EE;
+//档位学习位置反馈2
+const int ShiftLearnPosBack2 = 0x6EF;
+//系统监控状态反馈
+const int SysMonitorBack = 0x6D0;
+//系统报警
+const int SysWarnErr = 0x6D1;
+//油门报警
+const int AccErr= 0x6D2;
+//制动报警
+const int BrkErr = 0x6D3;
+//档位X电机报警
+const int XsftErr = 0x6D4;
+//档位Y电机报警
+const int YsftErr = 0x6D5;
+//转向电机报警
+const int StrErr = 0x6D6;
+//方向盘位置时间控制请求
+const int SteerPosTimeReq = 0x6D7;
+//Estop系统信息
+const int EstopInfo = 0x2E0;
+//Estop故障码
+const int EstopErr = 0x2E1;
+//OBD实际油门踏板行程
+const int ObdAccPsng = 0x1A1;
+//OBD实际制动踏板行程
+const int ObdBrkPsng = 0x0F1;
+//OBD实际挡位
+const int ObdSftPsng = 0x135;
+//OBD车辆手刹状态
+const int ObdVehHBrkStat = 0x1F1;
+//OBD实际车辆启动状态
+const int ObdVehSwStat = 0x121;
+//OBD实际方向盘转角
+const int ObdStrAng = 0x1E5;
+//OBD实际发动机转速
+const int ObdEngSpd = 0x0C9;
+
+} // common
+} // robot
+
+#endif // CANMSGOBDMSGID_H