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