|
@@ -119,6 +119,10 @@ int Canbus::Start() {
|
|
|
&Canbus::SlopeMode,this);
|
|
|
ros::ServiceServer StrLoopService = node_handle_->advertiseService(robot::common::StrLoopService,
|
|
|
&Canbus::StrLoopReq,this);
|
|
|
+ ros::ServiceServer AccLoopService = node_handle_->advertiseService(robot::common::AccLoopService,
|
|
|
+ &Canbus::AccLoopReq,this);
|
|
|
+ ros::ServiceServer BrkLoopService = node_handle_->advertiseService(robot::common::BrkLoopService,
|
|
|
+ &Canbus::BrkLoopReq,this);
|
|
|
//data receive
|
|
|
while(ros::ok())
|
|
|
{
|
|
@@ -133,7 +137,7 @@ int Canbus::Start() {
|
|
|
|
|
|
|
|
|
void Canbus::OnTimer(const ros::TimerEvent &) {
|
|
|
- LOG(INFO)<< Name()<< " timer start ..";
|
|
|
+ //LOG(INFO)<< Name()<< " timer start ..";
|
|
|
if(heatbeat > 15)
|
|
|
{
|
|
|
heatbeat = 0;
|
|
@@ -145,20 +149,20 @@ void Canbus::OnTimer(const ros::TimerEvent &) {
|
|
|
canObdData_pub.publish(obd);
|
|
|
TPCANMsg canIcuMonitor; //ICUMonitor Can Message
|
|
|
MessageCoder::CanMsgPackage((void*)&icuMonitor, robot::common::ICUMonitor, &canIcuMonitor);
|
|
|
- write_data(&canIcuMonitor);
|
|
|
+ write_data(&canIcuMonitor, false);
|
|
|
TPCANMsg canStatus1; //VehicleStaus1 Can Message
|
|
|
MessageCoder::CanMsgPackage((void*)&status1, robot::common::VehicleStatus1, &canStatus1);
|
|
|
- write_data(&canStatus1);
|
|
|
+ write_data(&canStatus1, false);
|
|
|
TPCANMsg canStatus2; //VehicleStatus2 Can Message
|
|
|
MessageCoder::CanMsgPackage((void*)&status2, robot::common::VehicleStatus2, &canStatus2);
|
|
|
- write_data(&canStatus2);
|
|
|
+ write_data(&canStatus2, false);
|
|
|
TPCANMsg canStatus3; //VehicleStatus3 Can Message
|
|
|
MessageCoder::CanMsgPackage((void*)&status3, robot::common::VehicleStatus3, &canStatus3);
|
|
|
- write_data(&canStatus3);
|
|
|
+ write_data(&canStatus3, false);
|
|
|
// enable
|
|
|
- write_data(&canActuaEnableReqMsg);
|
|
|
+ write_data(&canActuaEnableReqMsg, false);
|
|
|
// robot manager
|
|
|
- write_data(&canRobotManagerMsg);
|
|
|
+ write_data(&canRobotManagerMsg, false);
|
|
|
}
|
|
|
catch (ros::Exception ex)
|
|
|
{
|
|
@@ -216,7 +220,6 @@ void Canbus::InitServiceAndMsg()
|
|
|
//read from CAN forever - until manual break
|
|
|
int Canbus::read_loop()
|
|
|
{
|
|
|
-
|
|
|
TPCANRdMsg msg;
|
|
|
__u32 status;
|
|
|
|
|
@@ -312,8 +315,6 @@ void Canbus::publish_data(TPCANMsg *pMsgBuff)
|
|
|
//std::stringstream ss;
|
|
|
//ss << pMsgBuff->DATA;
|
|
|
//rosMsg.data = ss.str();
|
|
|
-
|
|
|
-
|
|
|
char strId[4] = {0};
|
|
|
sprintf(strId,"%3xh",pMsgBuff->ID);
|
|
|
char chData[16]={0};
|
|
@@ -321,16 +322,17 @@ void Canbus::publish_data(TPCANMsg *pMsgBuff)
|
|
|
//std::string strData = chData;
|
|
|
rosMsg.data = chData;
|
|
|
canRCUData_pub.publish(rosMsg);
|
|
|
- LOG(INFO)<<Name()<<" Publish CanData :" << "ID:" << strId << ",Data:"<<std::string(chData);
|
|
|
+ //LOG(INFO)<<Name()<<" Publish CanData :" << "ID:" << strId << ",Data:"<<std::string(chData);
|
|
|
}
|
|
|
|
|
|
-int Canbus::write_data(TPCANMsg *pMsgBuff)
|
|
|
+int Canbus::write_data(TPCANMsg *pMsgBuff, bool print)
|
|
|
{
|
|
|
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 (print)
|
|
|
+ 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. ";
|
|
|
return errno;
|
|
@@ -338,7 +340,6 @@ int Canbus::write_data(TPCANMsg *pMsgBuff)
|
|
|
return ROBOT_SUCCESS;
|
|
|
}
|
|
|
|
|
|
-
|
|
|
bool Canbus::ActuatorEnableRequest(canbus::ActuatorEnableRequest::Request &req,
|
|
|
canbus::ActuatorEnableRequest::Response &res)
|
|
|
{
|
|
@@ -659,9 +660,8 @@ bool Canbus::StrLoopReq(canbus::StrLoopReq::Request &req,
|
|
|
{
|
|
|
LOG(INFO)<< "controlservice start StrLoopReq";
|
|
|
// StrLoopReq request
|
|
|
- TPCANMsg canStrLpReqMsg;
|
|
|
try{
|
|
|
- if(!req.bStart)
|
|
|
+ if(req.uiStart == 0)
|
|
|
{
|
|
|
if(strTimerState)
|
|
|
{
|
|
@@ -683,10 +683,15 @@ bool Canbus::StrLoopReq(canbus::StrLoopReq::Request &req,
|
|
|
strLpReq = req;
|
|
|
|
|
|
double str_duration = (double)req.uiStrTimeInterval/1000;
|
|
|
+ strLoopNum = 0;
|
|
|
+ strIncrease = true;
|
|
|
+ strAngle = strLpReq.iStrAngStart;
|
|
|
+ tStrActCtrReq.iStrSpdCtrReq = strLpReq.iStrSpdCtrReq;
|
|
|
+ tStrActCtrReq.iStrTrqCtrReq = strLpReq.iStrTrqCtrReq;
|
|
|
+ tStrActCtrReq.uiStrCtrMdReq = strLpReq.uiStrCtrMdReq;
|
|
|
str_timer_ = CreateTimer(ros::Duration(str_duration), &Canbus::StrLoopOnTimer, this,false,false);
|
|
|
str_timer_.start();
|
|
|
strTimerState = true;
|
|
|
-
|
|
|
res.uiReturn = 1;
|
|
|
}
|
|
|
catch (ros::Exception ex)
|
|
@@ -700,46 +705,222 @@ bool Canbus::StrLoopReq(canbus::StrLoopReq::Request &req,
|
|
|
|
|
|
void Canbus::StrLoopOnTimer(const ros::TimerEvent &event)
|
|
|
{
|
|
|
- LOG(INFO)<< Name()<< " StrLoopTimer start ..";
|
|
|
-
|
|
|
+ //LOG(INFO)<< Name()<< " StrLoopTimer start ..";
|
|
|
TPCANMsg canStrActCtrReqMsg;
|
|
|
try
|
|
|
{
|
|
|
+ // 发送当前角度
|
|
|
+ tStrActCtrReq.iStrAngCtrReq = strAngle;
|
|
|
+ MessageCoder::CanMsgPackage((void*)&tStrActCtrReq, robot::common::SteerActCtrReq, &canStrActCtrReqMsg);
|
|
|
+ write_data(&canStrActCtrReqMsg);
|
|
|
+ //LOG(INFO)<<Name()<< " Angle "<< std::to_string(tStrActCtrReq.iStrAngCtrReq);
|
|
|
+ //LOG(INFO)<<Name()<< " Num "<< std::to_string(strLoopNum);
|
|
|
+ if (strAngle <= strLpReq.iStrAngStart)
|
|
|
+ strLoopNum++;
|
|
|
if(strLoopNum > strLpReq.uiStrFrequency) //如果执行次数等于往复次数,则结束定时器
|
|
|
{
|
|
|
str_timer_.stop();
|
|
|
strTimerState = false;
|
|
|
- strLoopNum = 0;
|
|
|
+ strLoopNum = 1;
|
|
|
tStrActCtrReq = {0};
|
|
|
+ return;
|
|
|
}
|
|
|
- tStrActCtrReq.iStrSpdCtrReq = strLpReq.iStrSpdCtrReq;
|
|
|
- tStrActCtrReq.iStrTrqCtrReq = strLpReq.iStrTrqCtrReq;
|
|
|
- tStrActCtrReq.uiStrCtrMdReq = strLpReq.uiStrCtrMdReq;
|
|
|
-
|
|
|
- if(tStrActCtrReq.iStrAngCtrReq > strLpReq.iStrAngEnd) //如果转向位置已经到达终止位置,则表示执行完一套动作,执行次数加一
|
|
|
+ if (strIncrease)
|
|
|
{
|
|
|
- strLoopNum++;
|
|
|
- tStrActCtrReq = {0};
|
|
|
+ strAngle += strLpReq.uiStrSpan;
|
|
|
+ if (strAngle >= strLpReq.iStrAngEnd)
|
|
|
+ strIncrease = false;
|
|
|
}
|
|
|
+ else
|
|
|
+ {
|
|
|
+ strAngle -= strLpReq.uiStrSpan;
|
|
|
+ if (strAngle <= strLpReq.iStrAngStart)
|
|
|
+ strIncrease = true;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ catch (ros::Exception ex)
|
|
|
+ {
|
|
|
+ LOG(ERROR)<<Name()<< " StrLoopTimer error. "<< ex.what();
|
|
|
+ }
|
|
|
+}
|
|
|
|
|
|
- if(tStrActCtrReq.iStrAngCtrReq == 0)
|
|
|
+
|
|
|
+bool Canbus::AccLoopReq(canbus::AccLoopReq::Request &req,
|
|
|
+ canbus::AccLoopReq::Response &res)
|
|
|
+{
|
|
|
+ LOG(INFO)<< "controlservice start AccLoopReq";
|
|
|
+ // AccLoopReq request
|
|
|
+ try{
|
|
|
+ if(req.uiStart == 0)
|
|
|
{
|
|
|
- tStrActCtrReq.iStrAngCtrReq = strLpReq.iStrAngStart;
|
|
|
+ if(accTimerState)
|
|
|
+ {
|
|
|
+ acc_timer_.stop();
|
|
|
+ accTimerState = false;
|
|
|
+ accLoopNum = 0;
|
|
|
+ tAccCtrReq = {0};
|
|
|
+ }
|
|
|
+ res.uiReturn = 1;
|
|
|
+ return true;
|
|
|
}
|
|
|
- tStrActCtrReq.iStrAngCtrReq = strLpReq.iStrAngStart + strLpReq.uiStrSpan;
|
|
|
+ if(accTimerState)
|
|
|
+ {
|
|
|
+ acc_timer_.stop();
|
|
|
+ accTimerState = false;
|
|
|
+ accLoopNum = 0;
|
|
|
+ tAccCtrReq = {0};
|
|
|
+ }
|
|
|
+ accLpReq = req;
|
|
|
+
|
|
|
+ double acc_duration = (double)req.uiAccTimeInterval/1000;
|
|
|
+ accLoopNum = 0;
|
|
|
+ accIncrease = true;
|
|
|
+ accPsng = accLpReq.uiAccPsngStart;
|
|
|
+ tAccCtrReq.iAccSpdCtrReq = accLpReq.iAccSpdCtrReq;
|
|
|
+ acc_timer_ = CreateTimer(ros::Duration(acc_duration), &Canbus::AccLoopOnTimer, this,false,false);
|
|
|
+ acc_timer_.start();
|
|
|
+ accTimerState = true;
|
|
|
+ res.uiReturn = 1;
|
|
|
+ }
|
|
|
+ catch (ros::Exception ex)
|
|
|
+ {
|
|
|
+ res.uiReturn = 0;
|
|
|
+ LOG(ERROR)<< "controlservice AccLoopReq failed "<< ex.what();
|
|
|
+ return true;
|
|
|
+ }
|
|
|
+ return true;
|
|
|
+}
|
|
|
|
|
|
- MessageCoder::CanMsgPackage((void*)&tStrActCtrReq, robot::common::SteerActCtrReq, &canStrActCtrReqMsg);
|
|
|
- write_data(&canStrActCtrReqMsg);
|
|
|
+void Canbus::AccLoopOnTimer(const ros::TimerEvent &event)
|
|
|
+{
|
|
|
+ LOG(INFO)<< Name()<< " AccLoopTimer start ..";
|
|
|
|
|
|
+ TPCANMsg canAccCtrReqMsg;
|
|
|
+ try
|
|
|
+ {
|
|
|
+ tAccCtrReq.uiAccPsngCtrReq = accPsng;
|
|
|
+ MessageCoder::CanMsgPackage((void*)&tAccCtrReq, robot::common::PedalActCtrReq, &canAccCtrReqMsg);
|
|
|
+ write_data(&canAccCtrReqMsg);
|
|
|
+ if (accPsng <= accLpReq.uiAccPsngStart)
|
|
|
+ accLoopNum++;
|
|
|
+ if(accLoopNum > accLpReq.uiAccFrequency) //如果执行次数等于往复次数,则结束定时器
|
|
|
+ {
|
|
|
+ acc_timer_.stop();
|
|
|
+ accTimerState = false;
|
|
|
+ accLoopNum = 1;
|
|
|
+ tAccCtrReq = {0};
|
|
|
+ return;
|
|
|
+ }
|
|
|
+ if (accIncrease)
|
|
|
+ {
|
|
|
+ accPsng += accLpReq.uiAccSpan;
|
|
|
+ if (accPsng >= accLpReq.uiAccPsngEnd)
|
|
|
+ accIncrease = false;
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ accPsng -= accLpReq.uiAccSpan;
|
|
|
+ if (accPsng <= accLpReq.uiAccPsngStart)
|
|
|
+ accIncrease = true;
|
|
|
+ }
|
|
|
}
|
|
|
catch (ros::Exception ex)
|
|
|
{
|
|
|
- LOG(ERROR)<<Name()<< " StrLoopTimer error. "<< ex.what();
|
|
|
+ LOG(ERROR)<<Name()<< " AccLoopTimer error. "<< ex.what();
|
|
|
}
|
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
+bool Canbus::BrkLoopReq(canbus::BrkLoopReq::Request &req,
|
|
|
+ canbus::BrkLoopReq::Response &res)
|
|
|
+{
|
|
|
+ LOG(INFO)<< "controlservice start BrkLoopReq";
|
|
|
+ // BrkLoopReq request
|
|
|
+ try{
|
|
|
+ if(req.uiStart == 0)
|
|
|
+ {
|
|
|
+ if(brkTimerState)
|
|
|
+ {
|
|
|
+ brk_timer_.stop();
|
|
|
+ brkTimerState = false;
|
|
|
+ brkLoopNum = 0;
|
|
|
+ tBrkCtrReq = {0};
|
|
|
+ }
|
|
|
+ res.uiReturn = 1;
|
|
|
+ return true;
|
|
|
+ }
|
|
|
+ if(brkTimerState)
|
|
|
+ {
|
|
|
+ brk_timer_.stop();
|
|
|
+ brkTimerState = false;
|
|
|
+ brkLoopNum = 0;
|
|
|
+ tBrkCtrReq = {0};
|
|
|
+ }
|
|
|
+ brkLpReq = req;
|
|
|
+
|
|
|
+ double brk_duration = (double)req.uiBrkTimeInterval/1000;
|
|
|
+ brkLoopNum = 0;
|
|
|
+ brkIncrease = true;
|
|
|
+ brkPsng = brkLpReq.uiBrkPsngStart;
|
|
|
+ tBrkCtrReq.iBrkSpdCtrReq = brkLpReq.iBrkSpdCtrReq;
|
|
|
+ tBrkCtrReq.uiBrkCtrMdReq = brkLpReq.uiBrkCtrMdReq;
|
|
|
+ tBrkCtrReq.uiBrkFocCtrReq = brkLpReq.uiBrkFocCtrReq;
|
|
|
+ brk_timer_ = CreateTimer(ros::Duration(brk_duration), &Canbus::BrkLoopOnTimer, this,false,false);
|
|
|
+ brk_timer_.start();
|
|
|
+ brkTimerState = true;
|
|
|
+ res.uiReturn = 1;
|
|
|
+ }
|
|
|
+ catch (ros::Exception ex)
|
|
|
+ {
|
|
|
+ res.uiReturn = 0;
|
|
|
+ LOG(ERROR)<< "controlservice BrkLoopReq failed "<< ex.what();
|
|
|
+ return true;
|
|
|
+ }
|
|
|
+ return true;
|
|
|
+}
|
|
|
+
|
|
|
+void Canbus::BrkLoopOnTimer(const ros::TimerEvent &event)
|
|
|
+{
|
|
|
+ LOG(INFO)<< Name()<< " BrkLoopTimer start ..";
|
|
|
+
|
|
|
+ TPCANMsg canBrkCtrReqMsg;
|
|
|
+ try
|
|
|
+ {
|
|
|
+ tBrkCtrReq.uiBrkPsngCtrReq = brkPsng;
|
|
|
+ MessageCoder::CanMsgPackage((void*)&tBrkCtrReq, robot::common::PedalActCtrReq, &canBrkCtrReqMsg);
|
|
|
+ write_data(&canBrkCtrReqMsg);
|
|
|
+ if (brkPsng <= brkLpReq.uiBrkPsngStart)
|
|
|
+ brkLoopNum++;
|
|
|
+ if(brkLoopNum > brkLpReq.uiBrkFrequency) //如果执行次数等于往复次数,则结束定时器
|
|
|
+ {
|
|
|
+ brk_timer_.stop();
|
|
|
+ brkTimerState = false;
|
|
|
+ brkLoopNum = 1;
|
|
|
+ tBrkCtrReq = {0};
|
|
|
+ return;
|
|
|
+ }
|
|
|
+ if (brkIncrease)
|
|
|
+ {
|
|
|
+ brkPsng += brkLpReq.uiBrkSpan;
|
|
|
+ if (brkPsng >= brkLpReq.uiBrkPsngEnd)
|
|
|
+ brkIncrease = false;
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ brkPsng -= brkLpReq.uiBrkSpan;
|
|
|
+ if (brkPsng <= brkLpReq.uiBrkPsngStart)
|
|
|
+ brkIncrease = true;
|
|
|
+ }
|
|
|
+
|
|
|
+ }
|
|
|
+ catch (ros::Exception ex)
|
|
|
+ {
|
|
|
+ LOG(ERROR)<<Name()<< " BrkLoopTimer error. "<< ex.what();
|
|
|
+ }
|
|
|
+
|
|
|
+}
|
|
|
+
|
|
|
|
|
|
|
|
|
} // namespace canbus
|