bool srvCallback_ElmoRecorderConfig(cob_srvs::ElmoRecorderConfig::Request &req,
cob_srvs::ElmoRecorderConfig::Response &res ){
if(m_bisInitialized) {
m_CanCtrlPltf->evalCanBuffer();
res.success = m_CanCtrlPltf->ElmoRecordings(0, req.recordinggap, "");
res.message = "Successfully configured all motors for instant record";
}
return true;
}
bool srvCallback_ElmoRecorderReadout(cob_srvs::ElmoRecorderReadout::Request &req,
cob_srvs::ElmoRecorderReadout::Response &res ){
if(m_bisInitialized) {
m_CanCtrlPltf->evalCanBuffer();
res.success = m_CanCtrlPltf->ElmoRecordings(1, req.subindex, req.fileprefix);
if(res.success == 0) res.message = "Successfully requested reading out of Recorded data";
else if(res.success == 1) res.message = "Recorder hasn't been configured well yet";
else if(res.success == 2) res.message = "A previous transmission is still in progress";
}
return true;
}
bool srvCallback_ElmoRecorderConfig(cob_base_drive_chain::ElmoRecorderConfig::Request &req,
cob_base_drive_chain::ElmoRecorderConfig::Response &res ){
if(m_bisInitialized)
{
#ifdef __SIM__
res.success = true;
#else
m_CanCtrlPltf->evalCanBuffer();
res.success = m_CanCtrlPltf->ElmoRecordings(0, req.recordinggap, "");
#endif
res.message = "Successfully configured all motors for instant record";
}
return true;
}
// topic callback functions
// function will be called when a new message arrives on a topic
void topicCallback_JointStateCmd(const sensor_msgs::JointState::ConstPtr& msg)
{
ROS_DEBUG("Topic Callback joint_command");
// only process cmds when system is initialized
if(m_bisInitialized == true)
{
ROS_DEBUG("Topic Callback joint_command - Sending Commands to drives (initialized)");
int iRet;
sensor_msgs::JointState JointStateCmd = *msg;
// check if velocities lie inside allowed boundaries
for(int i = 0; i < m_iNumMotors; i++)
{
// for steering motors
if( i == 1 || i == 3 || i == 5 || i == 7) // ToDo: specify this via the config-files
{
if (JointStateCmd.velocity[i] > m_Param.dMaxSteerRateRadpS)
{
JointStateCmd.velocity[i] = m_Param.dMaxSteerRateRadpS;
}
if (JointStateCmd.velocity[i] < -m_Param.dMaxSteerRateRadpS)
{
JointStateCmd.velocity[i] = -m_Param.dMaxSteerRateRadpS;
}
}
// for driving motors
else
{
if (JointStateCmd.velocity[i] > m_Param.dMaxDriveRateRadpS)
{
JointStateCmd.velocity[i] = m_Param.dMaxDriveRateRadpS;
}
if (JointStateCmd.velocity[i] < -m_Param.dMaxDriveRateRadpS)
{
JointStateCmd.velocity[i] = -m_Param.dMaxDriveRateRadpS;
}
}
// and cmd velocities to Can-Nodes
//m_CanCtrlPltf->setVelGearRadS(iCanIdent, dVelEncRadS);
ROS_DEBUG("Send data to drives");
iRet = m_CanCtrlPltf->setVelGearRadS(i, JointStateCmd.velocity[i]);
ROS_DEBUG("Successfully sent data to drives");
if(m_bPubEffort)
m_CanCtrlPltf->requestMotorTorque();
}
}
}
//##################################
//#### function implementations ####
bool NodeClass::initDrives()
{
ROS_INFO("Initializing Base Drive Chain");
// init member vectors
m_Param.vdWheelNtrlPosRad.assign((m_iNumDrives),0);
// m_Param.vdWheelNtrlPosRad.assign(4,0);
// ToDo: replace the following steps by ROS configuration files
// create Inifile class and set target inifile (from which data shall be read)
IniFile iniFile;
//n.param<std::string>("PltfIniLoc", sIniFileName, "Platform/IniFiles/Platform.ini");
iniFile.SetFileName(sIniDirectory + "Platform.ini", "PltfHardwareCoB3.h");
// get max Joint-Velocities (in rad/s) for Steer- and Drive-Joint
iniFile.GetKeyDouble("DrivePrms", "MaxDriveRate", &m_Param.dMaxDriveRateRadpS, true);
iniFile.GetKeyDouble("DrivePrms", "MaxSteerRate", &m_Param.dMaxSteerRateRadpS, true);
#ifdef __SIM__
// get Offset from Zero-Position of Steering
if(m_iNumDrives >=1)
m_Param.vdWheelNtrlPosRad[0] = 0.0f;
if(m_iNumDrives >=2)
m_Param.vdWheelNtrlPosRad[1] = 0.0f;
if(m_iNumDrives >=3)
m_Param.vdWheelNtrlPosRad[2] = 0.0f;
if(m_iNumDrives >=4)
m_Param.vdWheelNtrlPosRad[3] = 0.0f;
#else
// get Offset from Zero-Position of Steering
if(m_iNumDrives >=1)
iniFile.GetKeyDouble("DrivePrms", "Wheel1NeutralPosition", &m_Param.vdWheelNtrlPosRad[0], true);
if(m_iNumDrives >=2)
iniFile.GetKeyDouble("DrivePrms", "Wheel2NeutralPosition", &m_Param.vdWheelNtrlPosRad[1], true);
if(m_iNumDrives >=3)
iniFile.GetKeyDouble("DrivePrms", "Wheel3NeutralPosition", &m_Param.vdWheelNtrlPosRad[2], true);
if(m_iNumDrives >=4)
iniFile.GetKeyDouble("DrivePrms", "Wheel4NeutralPosition", &m_Param.vdWheelNtrlPosRad[3], true);
//Convert Degree-Value from ini-File into Radian:
for(int i=0; i<m_iNumDrives; i++)
{
m_Param.vdWheelNtrlPosRad[i] = MathSup::convDegToRad(m_Param.vdWheelNtrlPosRad[i]);
}
#endif
// debug log
ROS_INFO("Initializing CanCtrlItf");
bool bTemp1;
#ifdef __SIM__
bTemp1 = true;
#else
bTemp1 = m_CanCtrlPltf->initPltf();
#endif
// debug log
ROS_INFO("Initializing done");
return bTemp1;
}
// reset Can-Configuration
bool srvCallback_Recover(cob_srvs::Trigger::Request &req,
cob_srvs::Trigger::Response &res )
{
if(m_bisInitialized)
{
ROS_DEBUG("Service callback reset");
#ifdef __SIM__
res.success.data = true;
#else
res.success.data = m_CanCtrlPltf->resetPltf();
#endif
if (res.success.data) {
ROS_INFO("base resetted");
} else {
res.error_message.data = "reset of base failed";
ROS_WARN("Resetting base failed");
}
}
else
{
ROS_WARN("...base already recovered...");
res.success.data = true;
res.error_message.data = "base already recovered";
}
return true;
}
// Destructor
~NodeClass()
{
#ifdef __SIM__
#else
m_CanCtrlPltf->shutdownPltf();
#endif
}
bool srvCallback_ElmoRecorderReadout(cob_base_drive_chain::ElmoRecorderReadout::Request &req,
cob_base_drive_chain::ElmoRecorderReadout::Response &res ){
if(m_bisInitialized) {
#ifdef __SIM__
res.success = true;
#else
m_CanCtrlPltf->evalCanBuffer();
res.success = m_CanCtrlPltf->ElmoRecordings(1, req.subindex, req.fileprefix);
#endif
if(res.success == 0) {
res.message = "Successfully requested reading out of Recorded data";
m_bReadoutElmo = true;
ROS_WARN("CPU consuming evalCanBuffer used for ElmoReadout activated");
} else if(res.success == 1) res.message = "Recorder hasn't been configured well yet";
else if(res.success == 2) res.message = "A previous transmission is still in progress";
}
return true;
}
// shutdown Drivers and Can-Node
bool srvCallback_Shutdown(cob_srvs::Trigger::Request &req,
cob_srvs::Trigger::Response &res )
{
ROS_DEBUG("Service callback shutdown");
res.success.data = m_CanCtrlPltf->shutdownPltf();
if (res.success.data)
ROS_INFO("Drives shut down");
else
ROS_INFO("Shutdown of Drives FAILED");
return true;
}
// reset Can-Configuration
bool srvCallback_Reset(cob_srvs::Trigger::Request &req,
cob_srvs::Trigger::Response &res )
{
ROS_DEBUG("Service Callback Reset");
res.success = m_CanCtrlPltf->resetPltf();
if (res.success)
ROS_INFO("Can-Node resetted");
else
res.errorMessage.data = "reset of can-nodes failed";
ROS_INFO("Reset of Can-Node FAILED");
return true;
}
// reset Can-Configuration
bool srvCallback_Recover(cob_srvs::Trigger::Request &req,
cob_srvs::Trigger::Response &res )
{
ROS_DEBUG("Service callback reset");
res.success.data = m_CanCtrlPltf->resetPltf();
if (res.success.data) {
ROS_INFO("Can-Node resetted");
} else {
res.error_message.data = "reset of can-nodes failed";
ROS_WARN("Reset of Can-Node FAILED");
}
return true;
}
//EXPERIMENTAL: publish JointStates cyclical instead of service callback
bool publish_JointStates()
{
ROS_DEBUG("Service Callback GetJointState");
// init local variables
int j, k, ret_sprintf;
bool bIsError;
std::vector<double> vdAngGearRad, vdVelGearRad, vdEffortGearNM;
std::string str_steer, str_drive, str_num, str_cat;
// ToDo: search for a more elegant way to compose JointNames
char c_num [1];
// init strings
str_steer = "Steer";
str_drive = "Drive";
// set default values
vdAngGearRad.resize(m_iNumMotors, 0);
vdVelGearRad.resize(m_iNumMotors, 0);
vdEffortGearNM.resize(m_iNumMotors, 0);
// create temporary (local) JointState/Diagnostics Data-Container
sensor_msgs::JointState jointstate;
diagnostic_msgs::DiagnosticStatus diagnostics;
//Do you have to set frame_id manually??
// get time stamp for header
jointstate.header.stamp = ros::Time::now();
// set frame_id for header
// jointstate.header.frame_id = frame_id; //Where to get this id from?
// assign right size to JointState
jointstate.name.resize(m_iNumMotors);
jointstate.position.resize(m_iNumMotors);
jointstate.velocity.resize(m_iNumMotors);
jointstate.effort.resize(m_iNumMotors);
if(m_bisInitialized == false)
{
// as long as system is not initialized
bIsError = false;
j = 0;
k = 0;
// set data to jointstate
for(int i = 0; i<m_iNumMotors; i++)
{
jointstate.position[i] = 0.0;
jointstate.velocity[i] = 0.0;
jointstate.effort[i] = 0.0;
/*
// set joint names
if( i == 1 || i == 3 || i == 5 || i == 7) // ToDo: specify this via the config-files
{
// create name for identification in JointState msg
j = j+1;
ret_sprintf = sprintf(c_num, "%i", j);
str_num.assign(1, c_num[0]);
str_cat = str_steer + str_num;
}
else
{
// create name for identification in JointState msg
k = k+1;
ret_sprintf = sprintf(c_num, "%i", k);
str_num.assign(1, c_num[0]);
str_cat = str_drive + str_num;
}
// set joint names
jointstate.name[i] = str_cat;
*/
}
}
else
{
// as soon as drive chain is initialized
// read Can-Buffer
ROS_DEBUG("Read CAN-Buffer");
m_CanCtrlPltf->evalCanBuffer();
ROS_DEBUG("Successfully read CAN-Buffer");
j = 0;
k = 0;
for(int i = 0; i<m_iNumMotors; i++)
{
m_CanCtrlPltf->getGearPosVelRadS(i, &vdAngGearRad[i], &vdVelGearRad[i]);
//Get motor torque
if(m_bPubEffort) {
for(int i=0; i<m_iNumMotors; i++) {
m_CanCtrlPltf->getMotorTorque(i, &vdEffortGearNM[i]); //(int iCanIdent, double* pdTorqueNm)
}
}
// if a steering motor was read -> correct for offset
if( i == 1 || i == 3 || i == 5 || i == 7) // ToDo: specify this via the config-files
{
// correct for initial offset of steering angle (arbitrary homing position)
vdAngGearRad[i] += m_Param.vdWheelNtrlPosRad[j];
MathSup::normalizePi(vdAngGearRad[i]);
j = j+1;
// create name for identification in JointState msg
/*
ret_sprintf = sprintf(c_num, "%i", j);
str_num.assign(1, c_num[0]);
str_cat = str_steer + str_num;
}
else
{
// create name for identification in JointState msg
k = k+1;
ret_sprintf = sprintf(c_num, "%i", k);
str_num.assign(1, c_num[0]);
str_cat = str_drive + str_num;
*/
}
// set joint names
// jointstate.name[i] = str_cat;
}
// set data to jointstate
for(int i = 0; i<m_iNumMotors; i++)
{
jointstate.position[i] = vdAngGearRad[i];
jointstate.velocity[i] = vdVelGearRad[i];
jointstate.effort[i] = vdEffortGearNM[i];
}
}
// set answer to srv request
// res.jointstate = jointstate;
// publish jointstate message
topicPub_JointState.publish(jointstate);
ROS_DEBUG("published new drive-chain configuration (JointState message)");
if(m_bisInitialized)
{
// read Can only after initialization
bIsError = m_CanCtrlPltf->isPltfError();
}
// set data to diagnostics
if(bIsError)
{
diagnostics.level = 2;
diagnostics.name = "drive-chain can node";
diagnostics.message = "one or more drives are in Error mode";
}
else
{
if (m_bisInitialized)
{
diagnostics.level = 0;
diagnostics.name = "drive-chain can node";
diagnostics.message = "drives operating normal";
}
else
{
diagnostics.level = 1;
diagnostics.name = "drive-chain can node";
diagnostics.message = "drives are initializing";
}
}
// publish diagnostic message
topicPub_Diagnostic.publish(diagnostics);
ROS_DEBUG("published new drive-chain configuration (JointState message)");
return true;
}
// Destructor
~NodeClass()
{
m_CanCtrlPltf->shutdownPltf();
}
//publish JointStates cyclical instead of service callback
bool publish_JointStates()
{
ROS_DEBUG("Service Callback GetJointState");
// init local variables
int j, k;
bool bIsError;
std::vector<double> vdAngGearRad, vdVelGearRad, vdEffortGearNM;
// set default values
vdAngGearRad.resize(m_iNumMotors, 0);
vdVelGearRad.resize(m_iNumMotors, 0);
vdEffortGearNM.resize(m_iNumMotors, 0);
// create temporary (local) JointState/Diagnostics Data-Container
sensor_msgs::JointState jointstate;
diagnostic_msgs::DiagnosticStatus diagnostics;
pr2_controllers_msgs::JointTrajectoryControllerState controller_state;
// get time stamp for header
jointstate.header.stamp = ros::Time::now();
controller_state.header.stamp = jointstate.header.stamp;
// assign right size to JointState
//jointstate.name.resize(m_iNumMotors);
jointstate.position.assign(m_iNumMotors, 0.0);
jointstate.velocity.assign(m_iNumMotors, 0.0);
jointstate.effort.assign(m_iNumMotors, 0.0);
if(m_bisInitialized == false)
{
// as long as system is not initialized
bIsError = false;
j = 0;
k = 0;
// set data to jointstate
for(int i = 0; i<m_iNumMotors; i++)
{
jointstate.position[i] = 0.0;
jointstate.velocity[i] = 0.0;
jointstate.effort[i] = 0.0;
}
jointstate.name.push_back("fl_caster_r_wheel_joint");
jointstate.name.push_back("fl_caster_rotation_joint");
jointstate.name.push_back("bl_caster_r_wheel_joint");
jointstate.name.push_back("bl_caster_rotation_joint");
jointstate.name.push_back("br_caster_r_wheel_joint");
jointstate.name.push_back("br_caster_rotation_joint");
jointstate.name.push_back("fr_caster_r_wheel_joint");
jointstate.name.push_back("fr_caster_rotation_joint");
}
else
{
// as soon as drive chain is initialized
// read Can-Buffer
#ifdef __SIM__
#else
ROS_DEBUG("Read CAN-Buffer");
m_CanCtrlPltf->evalCanBuffer();
ROS_DEBUG("Successfully read CAN-Buffer");
#endif
j = 0;
k = 0;
for(int i = 0; i<m_iNumMotors; i++)
{
#ifdef __SIM__
vdAngGearRad[i] = m_gazeboPos[i];
vdVelGearRad[i] = m_gazeboVel[i];
#else
m_CanCtrlPltf->getGearPosVelRadS(i, &vdAngGearRad[i], &vdVelGearRad[i]);
#endif
//Get motor torque
if(m_bPubEffort) {
for(int i=0; i<m_iNumMotors; i++)
{
#ifdef __SIM__
//vdEffortGearNM[i] = m_gazeboEff[i];
#else
m_CanCtrlPltf->getMotorTorque(i, &vdEffortGearNM[i]); //(int iCanIdent, double* pdTorqueNm)
#endif
}
}
// if a steering motor was read -> correct for offset
if( i == 1 || i == 3 || i == 5 || i == 7) // ToDo: specify this via the config-files
{
// correct for initial offset of steering angle (arbitrary homing position)
vdAngGearRad[i] += m_Param.vdWheelNtrlPosRad[j];
MathSup::normalizePi(vdAngGearRad[i]);
j = j+1;
}
}
// set data to jointstate
for(int i = 0; i<m_iNumMotors; i++)
{
jointstate.position[i] = vdAngGearRad[i];
jointstate.velocity[i] = vdVelGearRad[i];
jointstate.effort[i] = vdEffortGearNM[i];
}
jointstate.name.push_back("fl_caster_r_wheel_joint");
jointstate.name.push_back("fl_caster_rotation_joint");
jointstate.name.push_back("bl_caster_r_wheel_joint");
jointstate.name.push_back("bl_caster_rotation_joint");
jointstate.name.push_back("br_caster_r_wheel_joint");
jointstate.name.push_back("br_caster_rotation_joint");
jointstate.name.push_back("fr_caster_r_wheel_joint");
jointstate.name.push_back("fr_caster_rotation_joint");
}
controller_state.joint_names = jointstate.name;
controller_state.actual.positions = jointstate.position;
controller_state.actual.velocities = jointstate.velocity;
// publish jointstate message
topicPub_JointState.publish(jointstate);
topicPub_ControllerState.publish(controller_state);
ROS_DEBUG("published new drive-chain configuration (JointState message)");
if(m_bisInitialized)
{
// read Can only after initialization
#ifdef __SIM__
bIsError = false;
#else
bIsError = m_CanCtrlPltf->isPltfError();
#endif
}
// set data to diagnostics
if(bIsError)
{
diagnostics.level = 2;
diagnostics.name = "drive-chain can node";
diagnostics.message = "one or more drives are in Error mode";
}
else
{
if (m_bisInitialized)
{
diagnostics.level = 0;
diagnostics.name = "drive-chain can node";
diagnostics.message = "drives operating normal";
}
else
{
diagnostics.level = 1;
diagnostics.name = "drive-chain can node";
diagnostics.message = "drives are initializing";
}
}
// publish diagnostic message
topicPub_Diagnostic.publish(diagnostics);
ROS_DEBUG("published new drive-chain configuration (JointState message)");
return true;
}
// topic callback functions
// function will be called when a new message arrives on a topic
void topicCallback_JointStateCmd(const pr2_controllers_msgs::JointTrajectoryControllerState::ConstPtr& msg)
{
ROS_DEBUG("Topic Callback joint_command");
// only process cmds when system is initialized
if(m_bisInitialized == true)
{
ROS_DEBUG("Topic Callback joint_command - Sending Commands to drives (initialized)");
sensor_msgs::JointState JointStateCmd;
JointStateCmd.position.resize(m_iNumMotors);
JointStateCmd.velocity.resize(m_iNumMotors);
JointStateCmd.effort.resize(m_iNumMotors);
for(unsigned int i = 0; i < msg->joint_names.size(); i++)
{
// associate inputs to according steer and drive joints
// ToDo: specify this globally (Prms-File or config-File or via msg-def.)
// check if velocities lie inside allowed boundaries
//DRIVES
if(msg->joint_names[i] == "fl_caster_r_wheel_joint")
{
JointStateCmd.position[0] = msg->desired.positions[i];
JointStateCmd.velocity[0] = msg->desired.velocities[i];
//JointStateCmd.effort[0] = msg->effort[i];
}
if(msg->joint_names[i] == "bl_caster_r_wheel_joint")
{
JointStateCmd.position[2] = msg->desired.positions[i];
JointStateCmd.velocity[2] = msg->desired.velocities[i];
//JointStateCmd.effort[2] = msg->effort[i];
}
if(msg->joint_names[i] == "br_caster_r_wheel_joint")
{
JointStateCmd.position[4] = msg->desired.positions[i];
JointStateCmd.velocity[4] = msg->desired.velocities[i];
//JointStateCmd.effort[4] = msg->effort[i];
}
if(msg->joint_names[i] == "fr_caster_r_wheel_joint")
{
JointStateCmd.position[6] = msg->desired.positions[i];
JointStateCmd.velocity[6] = msg->desired.velocities[i];
//JointStateCmd.effort[6] = msg->effort[i];
}
//STEERS
if(msg->joint_names[i] == "fl_caster_rotation_joint")
{
JointStateCmd.position[1] = msg->desired.positions[i];
JointStateCmd.velocity[1] = msg->desired.velocities[i];
//JointStateCmd.effort[1] = msg->effort[i];
}
if(msg->joint_names[i] == "bl_caster_rotation_joint")
{
JointStateCmd.position[3] = msg->desired.positions[i];
JointStateCmd.velocity[3] = msg->desired.velocities[i];
//JointStateCmd.effort[3] = msg->effort[i];
}
if(msg->joint_names[i] == "br_caster_rotation_joint")
{
JointStateCmd.position[5] = msg->desired.positions[i];
JointStateCmd.velocity[5] = msg->desired.velocities[i];
//JointStateCmd.effort[5] = msg->effort[i];
}
if(msg->joint_names[i] == "fr_caster_rotation_joint")
{
JointStateCmd.position[7] = msg->desired.positions[i];
JointStateCmd.velocity[7] = msg->desired.velocities[i];
//JointStateCmd.effort[7] = msg->effort[i];
}
}
// check if velocities lie inside allowed boundaries
for(int i = 0; i < m_iNumMotors; i++)
{
#ifdef __SIM__
#else
// for steering motors
if( i == 1 || i == 3 || i == 5 || i == 7) // ToDo: specify this via the config-files
{
if (JointStateCmd.velocity[i] > m_Param.dMaxSteerRateRadpS)
{
JointStateCmd.velocity[i] = m_Param.dMaxSteerRateRadpS;
}
if (JointStateCmd.velocity[i] < -m_Param.dMaxSteerRateRadpS)
{
JointStateCmd.velocity[i] = -m_Param.dMaxSteerRateRadpS;
}
}
// for driving motors
else
{
if (JointStateCmd.velocity[i] > m_Param.dMaxDriveRateRadpS)
{
JointStateCmd.velocity[i] = m_Param.dMaxDriveRateRadpS;
}
if (JointStateCmd.velocity[i] < -m_Param.dMaxDriveRateRadpS)
{
JointStateCmd.velocity[i] = -m_Param.dMaxDriveRateRadpS;
}
}
#endif
// and cmd velocities to Can-Nodes
//m_CanCtrlPltf->setVelGearRadS(iCanIdent, dVelEncRadS);
#ifdef __SIM__
ROS_DEBUG("Send velocity data to gazebo");
std_msgs::Float64 fl;
fl.data = JointStateCmd.velocity[i];
if(msg->joint_names[i] == "fl_caster_r_wheel_joint")
fl_caster_pub.publish(fl);
if(msg->joint_names[i] == "fr_caster_r_wheel_joint")
fr_caster_pub.publish(fl);
if(msg->joint_names[i] == "bl_caster_r_wheel_joint")
bl_caster_pub.publish(fl);
if(msg->joint_names[i] == "br_caster_r_wheel_joint")
br_caster_pub.publish(fl);
if(msg->joint_names[i] == "fl_caster_rotation_joint")
fl_steer_pub.publish(fl);
if(msg->joint_names[i] == "fr_caster_rotation_joint")
fr_steer_pub.publish(fl);
if(msg->joint_names[i] == "bl_caster_rotation_joint")
bl_steer_pub.publish(fl);
if(msg->joint_names[i] == "br_caster_rotation_joint")
br_steer_pub.publish(fl);
ROS_DEBUG("Successfully sent velicities to gazebo");
#else
ROS_DEBUG("Send velocity data to drives");
m_CanCtrlPltf->setVelGearRadS(i, JointStateCmd.velocity[i]);
ROS_DEBUG("Successfully sent velicities to drives");
#endif
}
#ifdef __SIM__
#else
if(m_bPubEffort) {
m_CanCtrlPltf->requestMotorTorque();
}
#endif
}
}