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