|
@@ -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. ";
|