|
@@ -107,6 +107,9 @@ int Canbus::Start() {
|
|
&Canbus::StrActCtrRequest, this);
|
|
&Canbus::StrActCtrRequest, this);
|
|
ros::ServiceServer PedalActCtrService = node_handle_->advertiseService(robot::common::PedalActCtrService,
|
|
ros::ServiceServer PedalActCtrService = node_handle_->advertiseService(robot::common::PedalActCtrService,
|
|
&Canbus::PedalActCtrRequest, this);
|
|
&Canbus::PedalActCtrRequest, this);
|
|
|
|
+
|
|
|
|
+ ros::ServiceServer RBTManagerService = node_handle_->advertiseService(robot::common::RBTManagerService,
|
|
|
|
+ &Canbus::RBTManager, this);
|
|
//data receive
|
|
//data receive
|
|
while(ros::ok())
|
|
while(ros::ok())
|
|
{
|
|
{
|
|
@@ -142,22 +145,17 @@ void Canbus::OnTimer(const ros::TimerEvent &) {
|
|
try
|
|
try
|
|
{
|
|
{
|
|
MessageCoder::CanMsgPackage((void*)&icuMonitor, robot::common::ICUMonitor, &canIcuMonitor);
|
|
MessageCoder::CanMsgPackage((void*)&icuMonitor, robot::common::ICUMonitor, &canIcuMonitor);
|
|
|
|
+ write_data(&canIcuMonitor);
|
|
MessageCoder::CanMsgPackage((void*)&status1, robot::common::VehicleStatus1, &canStatus1);
|
|
MessageCoder::CanMsgPackage((void*)&status1, robot::common::VehicleStatus1, &canStatus1);
|
|
|
|
+ write_data(&canStatus1);
|
|
MessageCoder::CanMsgPackage((void*)&status2, robot::common::VehicleStatus2, &canStatus2);
|
|
MessageCoder::CanMsgPackage((void*)&status2, robot::common::VehicleStatus2, &canStatus2);
|
|
- MessageCoder::CanMsgPackage((void*)&status3, robot::common::VehicleStatus3, &canStatus3);
|
|
|
|
- write_data(&canStatus1);
|
|
|
|
write_data(&canStatus2);
|
|
write_data(&canStatus2);
|
|
|
|
+ MessageCoder::CanMsgPackage((void*)&status3, robot::common::VehicleStatus3, &canStatus3);
|
|
write_data(&canStatus3);
|
|
write_data(&canStatus3);
|
|
- write_data(&canIcuMonitor);
|
|
|
|
-
|
|
|
|
- T_STR_ACT_CTR_REQ strAct = {0};
|
|
|
|
- strAct.iStrAngCtrReq = 2324;
|
|
|
|
- strAct.iStrSpdCtrReq= 10000;
|
|
|
|
- strAct.iStrTrqCtrReq = 5000;
|
|
|
|
- strAct.uiStrCtrMdReq = 1;
|
|
|
|
- TPCANMsg pStr;
|
|
|
|
- MessageCoder::CanMsgPackage((void*)&strAct,robot::common::SteerActCtrReq,&pStr);
|
|
|
|
- write_data(&pStr);
|
|
|
|
|
|
+ // enable
|
|
|
|
+ write_data(&canActuaEnableReqMsg);
|
|
|
|
+ // robot manager
|
|
|
|
+ write_data(&canRobotManagerMsg);
|
|
}
|
|
}
|
|
catch (ros::Exception ex)
|
|
catch (ros::Exception ex)
|
|
{
|
|
{
|
|
@@ -310,8 +308,7 @@ bool Canbus::ActuatorEnableRequest(canbus::ActuatorEnableRequest::Request &req,
|
|
canbus::ActuatorEnableRequest::Response &res)
|
|
canbus::ActuatorEnableRequest::Response &res)
|
|
{
|
|
{
|
|
LOG(INFO)<< "controlservice start ActuatorEnableRequest";
|
|
LOG(INFO)<< "controlservice start ActuatorEnableRequest";
|
|
- // actuator enable request
|
|
|
|
- TPCANMsg canActuaEnableReqMsg;
|
|
|
|
|
|
+
|
|
try{
|
|
try{
|
|
T_ACTUATOR_ENABLE_REQUEST request;
|
|
T_ACTUATOR_ENABLE_REQUEST request;
|
|
request.uiAccClchCtrReq = req.uiAccClchCtrReq;
|
|
request.uiAccClchCtrReq = req.uiAccClchCtrReq;
|
|
@@ -327,7 +324,7 @@ bool Canbus::ActuatorEnableRequest(canbus::ActuatorEnableRequest::Request &req,
|
|
request.uiYSftMotEnReq = req.uiYSftMotEnReq;
|
|
request.uiYSftMotEnReq = req.uiYSftMotEnReq;
|
|
|
|
|
|
MessageCoder::CanMsgPackage((void*)&request, robot::common::ActuatorEnableReq, &canActuaEnableReqMsg);
|
|
MessageCoder::CanMsgPackage((void*)&request, robot::common::ActuatorEnableReq, &canActuaEnableReqMsg);
|
|
- write_data(&canActuaEnableReqMsg);
|
|
|
|
|
|
+
|
|
res.uiReturn = 1;
|
|
res.uiReturn = 1;
|
|
}
|
|
}
|
|
catch (ros::Exception ex)
|
|
catch (ros::Exception ex)
|
|
@@ -455,7 +452,33 @@ bool Canbus::StrActCtrRequest(canbus::StrActCtrReq::Request &req,
|
|
}
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
+bool Canbus::RBTManager(canbus::RBTManager::Request &req,
|
|
|
|
+ canbus::RBTManager::Response &res)
|
|
|
|
+{
|
|
|
|
+ LOG(INFO)<< "RBTManager start ActuatorEnableRequest";
|
|
|
|
+
|
|
|
|
+ try{
|
|
|
|
+ T_RBT_MANAGER request;
|
|
|
|
+ request.uiAccMotInt = req.uiAccMotInt;
|
|
|
|
+ request.uiBrkMotInt = req.uiBrkMotInt;
|
|
|
|
+ request.uiClchMotInt = req.uiClchMotInt;
|
|
|
|
+ request.uiFltClr = req.uiFltClr;
|
|
|
|
+ request.uiStrMotInt= req.uiStrMotInt;
|
|
|
|
+ request.uiXSftMotInt = req.uiXSftMotInt;
|
|
|
|
+ request.uiYSftMotInt = req.uiYSftMotInt;
|
|
|
|
|
|
|
|
+ MessageCoder::CanMsgPackage((void*)&request, robot::common::RobotManager, &canRobotManagerMsg);
|
|
|
|
+
|
|
|
|
+ res.uiReturn = 1;
|
|
|
|
+ }
|
|
|
|
+ catch (ros::Exception ex)
|
|
|
|
+ {
|
|
|
|
+ res.uiReturn = 0;
|
|
|
|
+ LOG(ERROR)<< "RBTManager ActuatorEnableRequest failed "<< ex.what();
|
|
|
|
+ return false;
|
|
|
|
+ }
|
|
|
|
+ return true;
|
|
|
|
+}
|
|
|
|
|
|
|
|
|
|
|
|
|