|
@@ -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)
|
|
|
{
|