//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;
}
//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;
}
