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