// Listen for Pltf Cmds void topicCallbackTwistCmd(const geometry_msgs::Twist::ConstPtr& msg) { double vx_cmd_mms, vy_cmd_mms, w_cmd_rads; // controller expects velocities in mm/s, ROS works with SI-Units -> convert // ToDo: rework Controller Class to work with SI-Units vx_cmd_mms = msg->linear.x*1000.0; vy_cmd_mms = msg->linear.y*1000.0; w_cmd_rads = msg->angular.z; // only process if controller is already initialized if (is_initialized_bool_) { ROS_DEBUG("received new velocity command [cmdVelX=%3.5f,cmdVelY=%3.5f,cmdVelTh=%3.5f]", msg->linear.x, msg->linear.y, msg->angular.z); // Set desired value for Plattform Velocity to UndercarriageCtrl (setpoint setting) ucar_ctrl_.SetDesiredPltfVelocity(vx_cmd_mms, vy_cmd_mms, w_cmd_rads, 0.0); // ToDo: last value (0.0) is not used anymore --> remove from interface } else { // Set desired value for Plattform Velocity to zero (setpoint setting) ucar_ctrl_.SetDesiredPltfVelocity( 0.0, 0.0, 0.0, 0.0); // ToDo: last value (0.0) is not used anymore --> remove from interface } }
// Listen for Emergency Stop void topicCallbackEMStop(const cob_msgs::EmergencyStopState::ConstPtr& msg) { int EM_state; EM_state = msg->emergency_state; if (EM_state == msg->EMFREE) { // Reset EM flag in Ctrlr if (is_initialized_bool_) { ucar_ctrl_.setEMStopActive(false); // reset only done, when system initialized // -> allows to stop ctrlr during init, reset and shutdown } } else { ROS_DEBUG("Undercarriage Controller stopped due to EM-Stop"); // Set desired value for Plattform Velocity to zero (setpoint setting) ucar_ctrl_.SetDesiredPltfVelocity( 0.0, 0.0, 0.0, 0.0); // ToDo: last value (0.0) is not used anymore --> remove from interface // Set EM flag and stop Ctrlr ucar_ctrl_.setEMStopActive(true); } }
// Listens for status of underlying hardware (base drive chain) void topicCallbackDiagnostic(const diagnostic_msgs::DiagnosticStatus::ConstPtr& msg) { sensor_msgs::JointState joint_state_cmd; // prepare joint_cmds for heartbeat (compose header) joint_state_cmd.header.stamp = ros::Time::now(); //joint_state_cmd.header.frame_id = frame_id; //Where to get this id from? // ToDo: configure over Config-File (number of motors) and Msg // assign right size to JointState data containers //joint_state_cmd.set_name_size(m_iNumMotors); joint_state_cmd.position.resize(m_iNumJoints); joint_state_cmd.velocity.resize(m_iNumJoints); joint_state_cmd.effort.resize(m_iNumJoints); // compose jointcmds for(int i=0; i<m_iNumJoints; i++) { joint_state_cmd.position[i] = 0.0; joint_state_cmd.velocity[i] = 0.0; joint_state_cmd.effort[i] = 0.0; } // set status of underlying drive chain to member variable drive_chain_diagnostic_ = msg->level; // if controller is already started up ... if (is_initialized_bool_) { // ... but underlying drive chain is not yet operating normal if (drive_chain_diagnostic_ != diagnostic_status_lookup_.OK) { // halt controller ROS_DEBUG("drive chain not availlable: halt Controller"); // Set EM flag to Ctrlr (resets internal states) ucar_ctrl_->setEMStopActive(true); // Set desired value for Plattform Velocity to zero (setpoint setting) ucar_ctrl_->SetDesiredPltfVelocity( 0.0, 0.0, 0.0, 0.0); // ToDo: last value (0.0) is not used anymore --> remove from interface ROS_DEBUG("Forced platform-velocity cmds to zero"); // if is not Initializing if (drive_chain_diagnostic_ != diagnostic_status_lookup_.WARN) { // publish zero-vel. jointcmds to avoid Watchdogs stopping ctrlr topic_pub_joint_state_cmd_.publish(joint_state_cmd); } } } // ... while controller is not initialized send heartbeats to keep motors alive else { // ... as soon as base drive chain is initialized if(drive_chain_diagnostic_ != diagnostic_status_lookup_.WARN) { // publish zero-vel. jointcmds to avoid Watchdogs stopping ctrlr topic_pub_joint_state_cmd_.publish(joint_state_cmd); } } }
// perform one control step, calculate inverse kinematics and publish updated joint cmd's (if no EMStop occurred) void NodeClass::CalcCtrlStep() { double vx_cmd_ms, vy_cmd_ms, w_cmd_rads, dummy; std::vector<double> drive_jointvel_cmds_rads, steer_jointvel_cmds_rads, steer_jointang_cmds_rad; sensor_msgs::JointState joint_state_cmd; int num_joints = 8; int j, k; // if controller is initialized and underlying hardware is operating normal if (is_initialized_bool_ && (drive_chain_diagnostic_ != diagnostic_status_lookup_.OK)) { // perform one control step, // get the resulting cmd's for the wheel velocities and -angles from the controller class // and output the achievable pltf velocity-cmds (if velocity limits where exceeded) ucar_ctrl_.GetNewCtrlStateSteerDriveSetValues(drive_jointvel_cmds_rads, steer_jointvel_cmds_rads, steer_jointang_cmds_rad, vx_cmd_ms, vy_cmd_ms, w_cmd_rads, dummy); // ToDo: adapt interface of controller class --> remove last values (not used anymore) // convert variables to SI-Units vx_cmd_ms = vx_cmd_ms/1000.0; vy_cmd_ms = vy_cmd_ms/1000.0; // compose jointcmds // compose header joint_state_cmd.header.stamp = ros::Time::now(); //joint_state_cmd.header.frame_id = frame_id; //Where to get this id from? // ToDo: configure over Config-File (number of motors) and Msg // assign right size to JointState data containers //joint_state_cmd.set_name_size(m_iNumMotors); joint_state_cmd.set_position_size(num_joints); joint_state_cmd.set_velocity_size(num_joints); joint_state_cmd.set_effort_size(num_joints); // compose data body j = 0; k = 0; for(int i = 0; i<num_joints; i++) { // for steering motors if( i == 1 || i == 3 || i == 5 || i == 7) // ToDo: specify this via the Msg { joint_state_cmd.position[i] = steer_jointang_cmds_rad[j]; joint_state_cmd.velocity[i] = steer_jointvel_cmds_rads[j]; joint_state_cmd.effort[i] = 0.0; j = j + 1; } else { joint_state_cmd.position[i] = 0.0; joint_state_cmd.velocity[i] = drive_jointvel_cmds_rads[k]; joint_state_cmd.effort[i] = 0.0; k = k + 1; } } // publish jointcmds topic_pub_joint_state_cmd_.publish(joint_state_cmd); } }
// acquires the current undercarriage configuration from base_drive_chain void NodeClass::GetJointState() { int num_joints; int iter_k, iter_j; std::vector<double> drive_joint_ang_rad, drive_joint_vel_rads, drive_joint_effort_NM; std::vector<double> steer_joint_ang_rad, steer_joint_vel_rads, steer_joint_effort_NM; cob_srvs::GetJointState srv_get_joint; // request update for pltf config --> call GetJointstate service srv_client_get_joint_state_.call(srv_get_joint); // copy configuration into vector classes num_joints = srv_get_joint.response.jointstate.position.size(); // drive joints drive_joint_ang_rad.assign(num_joints, 0.0); drive_joint_vel_rads.assign(num_joints, 0.0); drive_joint_effort_NM.assign(num_joints, 0.0); // steer joints steer_joint_ang_rad.assign(num_joints, 0.0); steer_joint_vel_rads.assign(num_joints, 0.0); steer_joint_effort_NM.assign(num_joints, 0.0); // init iterators iter_k = 0; iter_j = 0; for(int i = 0; i < num_joints; i++) { // associate inputs to according steer and drive joints // ToDo: specify this globally (Prms-File or config-File or via msg-def.) // ToDo: use joint names instead of magic integers if( i == 1 || i == 3 || i == 5 || i == 7) { steer_joint_ang_rad[iter_k] = srv_get_joint.response.jointstate.position[i]; steer_joint_vel_rads[iter_k] = srv_get_joint.response.jointstate.velocity[i]; steer_joint_effort_NM[iter_k] = srv_get_joint.response.jointstate.effort[i]; iter_k = iter_k + 1; } else { drive_joint_ang_rad[iter_j] = srv_get_joint.response.jointstate.position[i]; drive_joint_vel_rads[iter_j] = srv_get_joint.response.jointstate.velocity[i]; drive_joint_effort_NM[iter_j] = srv_get_joint.response.jointstate.effort[i]; iter_j = iter_j + 1; } } // Set measured Wheel Velocities and Angles to Controler Class (implements inverse kinematic) ucar_ctrl_->SetActualWheelValues(drive_joint_vel_rads, steer_joint_vel_rads, drive_joint_ang_rad, steer_joint_ang_rad); }
// Listens for status of underlying hardware (base drive chain) void topicCallbackDiagnostic(const diagnostic_msgs::DiagnosticStatus::ConstPtr& msg) { // set status of underlying drive chain to member variable drive_chain_diagnostic_ = msg->level; // if controller is already started up ... if (is_initialized_bool_) { // ... but underlying drive chain is not yet operating normal if (drive_chain_diagnostic_ != diagnostic_status_lookup_.OK) { // halt controller ROS_DEBUG("drive chain not availlable: halt Controller"); // Set EM flag to Ctrlr (resets internal states) ucar_ctrl_.setEMStopActive(true); // Set desired value for Plattform Velocity to zero (setpoint setting) ucar_ctrl_.SetDesiredPltfVelocity( 0.0, 0.0, 0.0, 0.0); // ToDo: last value (0.0) is not used anymore --> remove from interface } } }
//################################## //#### function implementations #### bool NodeClass::InitCtrl() { ROS_INFO("Initializing Undercarriage Controller"); // ToDo: // 1st step: pass the path to the Inifile via the Parameterserver // 2nd step: replace the Inifiles by ROS configuration files // create Inifile class and set target inifile (from which data shall be read) //IniFile iniFile; // Parameters are set within the launch file //n.param<std::string>("base_drive_chain_node/IniDirectory", sIniDirectory, "Platform/IniFiles/"); //ROS_INFO("IniDirectory loaded from Parameter-Server is: %s", sIniDirectory.c_str()); //iniFile.SetFileName(sIniDirectory + "Platform.ini", "PltfHardwareCoB3.h"); // Init Controller Class ucar_ctrl_.InitUndercarriageCtrl(); ROS_INFO("Initializing Undercarriage Controller done"); return true; }
// shutdown Controller bool srvCallbackShutdown(cob_srvs::Switch::Request &req, cob_srvs::Switch::Response &res ) { if (is_initialized_bool_) { // stop controller // Set desired value for Plattform Velocity to zero (setpoint setting) ucar_ctrl_.SetDesiredPltfVelocity( 0.0, 0.0, 0.0, 0.0); // flag that controller is not running anymore is_initialized_bool_ = false; res.success = true; } else { // shutdown not possible, because pltf not running ROS_ERROR("...platform not initialized..."); res.success = false; res.errorMessage.data = "platform already or still down"; } return true; }
// calculates odometry from current measurement values // and publishes it via an odometry topic and the tf broadcaster void NodeClass::UpdateOdometry() { double vel_x_rob_ms, vel_y_rob_ms, rot_rob_rads, delta_x_rob_m, delta_y_rob_m, delta_theta_rob_rad; double dummy1, dummy2; double dt; ros::Time current_time; // if drive chain already initialized process joint data if (drive_chain_diagnostic_ != diagnostic_status_lookup_.OK) { // Get resulting Pltf Velocities from Ctrl-Class (result of forward kinematics) // !Careful! Controller internally calculates with mm instead of m // ToDo: change internal calculation to SI-Units // ToDo: last values are not used anymore --> remove from interface ucar_ctrl_.GetActualPltfVelocity(delta_x_rob_m, delta_y_rob_m, delta_theta_rob_rad, dummy1, vel_x_rob_ms, vel_y_rob_ms, rot_rob_rads, dummy2); // convert variables to SI-Units vel_x_rob_ms = vel_x_rob_ms/1000.0; vel_y_rob_ms = vel_y_rob_ms/1000.0; delta_x_rob_m = delta_x_rob_m/1000.0; delta_y_rob_m = delta_y_rob_m/1000.0; } else { // otherwise set data (velocity and pose-delta) to zero vel_x_rob_ms = 0.0; vel_y_rob_ms = 0.0; delta_x_rob_m = 0.0; delta_y_rob_m = 0.0; } // calc odometry (from startup) // get time since last odometry-measurement current_time = ros::Time::now(); dt = current_time.toSec() - last_time_.toSec(); last_time_ = current_time; // calculation from ROS odom publisher tutorial http://www.ros.org/wiki/navigation/Tutorials/RobotSetup/Odom x_rob_m_ = x_rob_m_ + (vel_x_rob_ms * cos(theta_rob_rad_) - vel_y_rob_ms * sin(theta_rob_rad_)) * dt; y_rob_m_ = y_rob_m_ + (vel_x_rob_ms * sin(theta_rob_rad_) + vel_y_rob_ms * cos(theta_rob_rad_)) * dt; theta_rob_rad_ = rot_rob_rads * dt; // format data for compatibility with tf-package and standard odometry msg // generate quaternion for rotation geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromYaw(theta_rob_rad_); // compose and publish transform for tf package geometry_msgs::TransformStamped odom_tf; // compose header odom_tf.header.stamp = current_time; odom_tf.header.frame_id = "/odom"; odom_tf.child_frame_id = "/base_footprint"; // compose data container odom_tf.transform.translation.x = x_rob_m_; odom_tf.transform.translation.y = y_rob_m_; odom_tf.transform.translation.z = 0.0; odom_tf.transform.rotation = odom_quat; // compose and publish odometry message as topic nav_msgs::Odometry odom_top; // compose header odom_top.header.stamp = current_time; odom_top.header.frame_id = "/odom"; odom_top.child_frame_id = "/base_footprint"; // compose pose of robot odom_top.pose.pose.position.x = x_rob_m_; odom_top.pose.pose.position.y = y_rob_m_; odom_top.pose.pose.position.z = 0.0; odom_top.pose.pose.orientation = odom_quat; // compose twist of robot odom_top.twist.twist.linear.x = vel_x_rob_ms; odom_top.twist.twist.linear.y = vel_y_rob_ms; odom_top.twist.twist.linear.z = 0.0; odom_top.twist.twist.angular.x = 0.0; odom_top.twist.twist.angular.y = 0.0; odom_top.twist.twist.angular.z = rot_rob_rads; // publish data // publish the transform tf_broadcast_odometry_.sendTransform(odom_tf); // publish odometry msg topic_pub_odometry_.publish(odom_top); }
// reset Controller Configuration bool srvCallbackReset(cob_srvs::Switch::Request &req, cob_srvs::Switch::Response &res ) { bool ctrlr_reset; if (is_initialized_bool_) { // flag that Controller is not initialized is_initialized_bool_ = false; // first of all stop controller (similar to EMStop) // Set desired value for Plattform Velocity to zero (setpoint setting) ucar_ctrl_.SetDesiredPltfVelocity( 0.0, 0.0, 0.0, 0.0); // ToDo: last value (0.0) is not used anymore --> remove from interface // Set EM flag and stop Ctrlr ucar_ctrl_.setEMStopActive(true); // now re-init controller configuration ctrlr_reset = InitCtrl(); if (ctrlr_reset) { // restart controller // Set desired value for Plattform Velocity to zero (setpoint setting) ucar_ctrl_.SetDesiredPltfVelocity( 0.0, 0.0, 0.0, 0.0); // ToDo: last value (0.0) is not used anymore --> remove from interface // Set EM flag and stop Ctrlr ucar_ctrl_.setEMStopActive(false); // reset Time last_time_ = ros::Time::now(); // and reset odometry x_rob_m_ = 0.0; y_rob_m_ = 0.0; theta_rob_rad_ = 0.0; // flag that controller is initialized is_initialized_bool_ = true; // set answer for service request ROS_INFO("Undercarriage Controller resetted"); res.success = true; } else { // set answer for service request ROS_INFO("Re-Init after Reset of Undercarriage Controller failed"); res.success = false; res.errorMessage.data = "reinit after reset of undercarriage controller failed"; } } else { // Reset not possible, because Controller not yet set up ROS_ERROR("... undercarriage controller not yet initialized, reset not possible ..."); // set answer for service request res.success = false; res.errorMessage.data = "undercarriage controller not yet initialized"; } return true; }
// calculates odometry from current measurement values // and publishes it via an odometry topic and the tf broadcaster void NodeClass::UpdateOdometry() { double vel_x_rob_ms, vel_y_rob_ms, vel_rob_ms, rot_rob_rads, delta_x_rob_m, delta_y_rob_m, delta_theta_rob_rad; double dummy1, dummy2; double dt; ros::Time current_time; // if drive chain already initialized process joint data //if (drive_chain_diagnostic_ != diagnostic_status_lookup_.OK) if (is_initialized_bool_) { // Get resulting Pltf Velocities from Ctrl-Class (result of forward kinematics) // !Careful! Controller internally calculates with mm instead of m // ToDo: change internal calculation to SI-Units // ToDo: last values are not used anymore --> remove from interface ucar_ctrl_->GetActualPltfVelocity(delta_x_rob_m, delta_y_rob_m, delta_theta_rob_rad, dummy1, vel_x_rob_ms, vel_y_rob_ms, rot_rob_rads, dummy2); // convert variables to SI-Units vel_x_rob_ms = vel_x_rob_ms/1000.0; vel_y_rob_ms = vel_y_rob_ms/1000.0; delta_x_rob_m = delta_x_rob_m/1000.0; delta_y_rob_m = delta_y_rob_m/1000.0; ROS_DEBUG("Odmonetry delta is: x=%f, y=%f, th=%f", delta_x_rob_m, delta_y_rob_m, rot_rob_rads); } else { // otherwise set data (velocity and pose-delta) to zero vel_x_rob_ms = 0.0; vel_y_rob_ms = 0.0; delta_x_rob_m = 0.0; delta_y_rob_m = 0.0; } // calc odometry (from startup) // get time since last odometry-measurement current_time = ros::Time::now(); dt = current_time.toSec() - last_time_.toSec(); last_time_ = current_time; vel_rob_ms = sqrt(vel_x_rob_ms*vel_x_rob_ms + vel_y_rob_ms*vel_y_rob_ms); // calculation from ROS odom publisher tutorial http://www.ros.org/wiki/navigation/Tutorials/RobotSetup/Odom, using now midpoint integration x_rob_m_ = x_rob_m_ + ((vel_x_rob_ms+vel_x_rob_last_)/2.0 * cos(theta_rob_rad_) - (vel_y_rob_ms+vel_y_rob_last_)/2.0 * sin(theta_rob_rad_)) * dt; y_rob_m_ = y_rob_m_ + ((vel_x_rob_ms+vel_x_rob_last_)/2.0 * sin(theta_rob_rad_) + (vel_y_rob_ms+vel_y_rob_last_)/2.0 * cos(theta_rob_rad_)) * dt; theta_rob_rad_ = theta_rob_rad_ + rot_rob_rads * dt; //theta_rob_rad_ = theta_rob_rad_ + (rot_rob_rads+vel_theta_rob_last_)/2.0 * dt; vel_x_rob_last_ = vel_x_rob_ms; vel_y_rob_last_ = vel_y_rob_ms; vel_theta_rob_last_ = rot_rob_rads; // format data for compatibility with tf-package and standard odometry msg // generate quaternion for rotation geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromYaw(theta_rob_rad_); // compose and publish transform for tf package geometry_msgs::TransformStamped odom_tf; // compose header odom_tf.header.stamp = joint_state_odom_stamp_; odom_tf.header.frame_id = "/wheelodom"; odom_tf.child_frame_id = "/base_footprint"; // compose data container odom_tf.transform.translation.x = x_rob_m_; odom_tf.transform.translation.y = y_rob_m_; odom_tf.transform.translation.z = 0.0; odom_tf.transform.rotation = odom_quat; // compose and publish odometry message as topic nav_msgs::Odometry odom_top; // compose header odom_top.header.stamp = joint_state_odom_stamp_; odom_top.header.frame_id = "/wheelodom"; odom_top.child_frame_id = "/base_footprint"; // compose pose of robot odom_top.pose.pose.position.x = x_rob_m_; odom_top.pose.pose.position.y = y_rob_m_; odom_top.pose.pose.position.z = 0.0; odom_top.pose.pose.orientation = odom_quat; for(int i = 0; i < 6; i++) odom_top.pose.covariance[i*6+i] = 0.1; // compose twist of robot odom_top.twist.twist.linear.x = vel_x_rob_ms; odom_top.twist.twist.linear.y = vel_y_rob_ms; odom_top.twist.twist.linear.z = 0.0; odom_top.twist.twist.angular.x = 0.0; odom_top.twist.twist.angular.y = 0.0; odom_top.twist.twist.angular.z = rot_rob_rads; for(int i = 0; i < 6; i++) odom_top.twist.covariance[6*i+i] = 0.1; if (fabs(vel_x_rob_ms) > 0.005 or fabs(vel_y_rob_ms) > 0.005 or fabs(rot_rob_rads) > 0.005) { is_moving_ = true; } else { is_moving_ = false; } // publish the transform (for debugging, conflicts with robot-pose-ekf) // tf_broadcast_odometry_.sendTransform(odom_tf); // publish odometry msg topic_pub_odometry_.publish(odom_top); }
// perform one control step, calculate inverse kinematics and publish updated joint cmd's (if no EMStop occurred) void NodeClass::CalcCtrlStep() { double vx_cmd_ms, vy_cmd_ms, w_cmd_rads, dummy; std::vector<double> drive_jointvel_cmds_rads, steer_jointvel_cmds_rads, steer_jointang_cmds_rad; pr2_controllers_msgs::JointTrajectoryControllerState joint_state_cmd; int j, k; iwatchdog_ += 1; // if controller is initialized and underlying hardware is operating normal if (is_initialized_bool_) //&& (drive_chain_diagnostic_ != diagnostic_status_lookup_.OK)) { // as soon as (but only as soon as) platform drive chain is initialized start to send velocity commands // Note: topicCallbackDiagnostic checks whether drives are operating nominal. // -> if warning or errors are issued target velocity is set to zero // perform one control step, // get the resulting cmd's for the wheel velocities and -angles from the controller class // and output the achievable pltf velocity-cmds (if velocity limits where exceeded) ucar_ctrl_->GetNewCtrlStateSteerDriveSetValues(drive_jointvel_cmds_rads, steer_jointvel_cmds_rads, steer_jointang_cmds_rad, vx_cmd_ms, vy_cmd_ms, w_cmd_rads, dummy); // ToDo: adapt interface of controller class --> remove last values (not used anymore) // if drives not operating nominal -> force commands to zero if(drive_chain_diagnostic_ != diagnostic_status_lookup_.OK) { steer_jointang_cmds_rad.assign(m_iNumJoints, 0.0); steer_jointvel_cmds_rads.assign(m_iNumJoints, 0.0); } // convert variables to SI-Units vx_cmd_ms = vx_cmd_ms/1000.0; vy_cmd_ms = vy_cmd_ms/1000.0; // compose jointcmds // compose header joint_state_cmd.header.stamp = ros::Time::now(); //joint_state_cmd.header.frame_id = frame_id; //Where to get this id from? // ToDo: configure over Config-File (number of motors) and Msg // assign right size to JointState data containers //joint_state_cmd.set_name_size(m_iNumMotors); joint_state_cmd.desired.positions.resize(m_iNumJoints); joint_state_cmd.desired.velocities.resize(m_iNumJoints); //joint_state_cmd.effort.resize(m_iNumJoints); joint_state_cmd.joint_names.push_back("fl_caster_r_wheel_joint"); joint_state_cmd.joint_names.push_back("fl_caster_rotation_joint"); joint_state_cmd.joint_names.push_back("bl_caster_r_wheel_joint"); joint_state_cmd.joint_names.push_back("bl_caster_rotation_joint"); joint_state_cmd.joint_names.push_back("br_caster_r_wheel_joint"); joint_state_cmd.joint_names.push_back("br_caster_rotation_joint"); joint_state_cmd.joint_names.push_back("fr_caster_r_wheel_joint"); joint_state_cmd.joint_names.push_back("fr_caster_rotation_joint"); // compose data body j = 0; k = 0; for(int i = 0; i<m_iNumJoints; i++) { if(iwatchdog_ < 50) { // for steering motors if( i == 1 || i == 3 || i == 5 || i == 7) // ToDo: specify this via the Msg { joint_state_cmd.desired.positions[i] = steer_jointang_cmds_rad[j]; joint_state_cmd.desired.velocities[i] = steer_jointvel_cmds_rads[j]; //joint_state_cmd.effort[i] = 0.0; j = j + 1; } else { joint_state_cmd.desired.positions[i] = 0.0; joint_state_cmd.desired.velocities[i] = drive_jointvel_cmds_rads[k]; //joint_state_cmd.effort[i] = 0.0; k = k + 1; } } else { joint_state_cmd.desired.positions[i] = 0.0; joint_state_cmd.desired.velocities[i] = 0.0; //joint_state_cmd.effort[i] = 0.0; } } // publish jointcmds topic_pub_controller_joint_command_.publish(joint_state_cmd); } }
void topicCallbackJointControllerStates(const pr2_controllers_msgs::JointTrajectoryControllerState::ConstPtr& msg) { int num_joints; int iter_k, iter_j; std::vector<double> drive_joint_ang_rad, drive_joint_vel_rads, drive_joint_effort_NM; std::vector<double> steer_joint_ang_rad, steer_joint_vel_rads, steer_joint_effort_NM; cob_base_drive_chain::GetJointState srv_get_joint; joint_state_odom_stamp_ = msg->header.stamp; // copy configuration into vector classes num_joints = msg->joint_names.size(); // drive joints drive_joint_ang_rad.assign(m_iNumJoints, 0.0); drive_joint_vel_rads.assign(m_iNumJoints, 0.0); drive_joint_effort_NM.assign(m_iNumJoints, 0.0); // steer joints steer_joint_ang_rad.assign(m_iNumJoints, 0.0); steer_joint_vel_rads.assign(m_iNumJoints, 0.0); steer_joint_effort_NM.assign(m_iNumJoints, 0.0); // init iterators iter_k = 0; iter_j = 0; for(int i = 0; i < num_joints; i++) { // associate inputs to according steer and drive joints // ToDo: specify this globally (Prms-File or config-File or via msg-def.) if(msg->joint_names[i] == "fl_caster_r_wheel_joint") { drive_joint_ang_rad[0] = msg->actual.positions[i]; drive_joint_vel_rads[0] = msg->actual.velocities[i]; //drive_joint_effort_NM[0] = msg->effort[i]; } if(msg->joint_names[i] == "bl_caster_r_wheel_joint") { drive_joint_ang_rad[1] = msg->actual.positions[i]; drive_joint_vel_rads[1] = msg->actual.velocities[i]; //drive_joint_effort_NM[1] = msg->effort[i]; } if(msg->joint_names[i] == "br_caster_r_wheel_joint") { drive_joint_ang_rad[2] = msg->actual.positions[i]; drive_joint_vel_rads[2] = msg->actual.velocities[i]; //drive_joint_effort_NM[2] = msg->effort[i]; } if(msg->joint_names[i] == "fr_caster_r_wheel_joint") { drive_joint_ang_rad[3] = msg->actual.positions[i]; drive_joint_vel_rads[3] = msg->actual.velocities[i]; //drive_joint_effort_NM[3] = msg->effort[i]; } if(msg->joint_names[i] == "fl_caster_rotation_joint") { steer_joint_ang_rad[0] = msg->actual.positions[i]; steer_joint_vel_rads[0] = msg->actual.velocities[i]; //steer_joint_effort_NM[0] = msg->effort[i]; } if(msg->joint_names[i] == "bl_caster_rotation_joint") { steer_joint_ang_rad[1] = msg->actual.positions[i]; steer_joint_vel_rads[1] = msg->actual.velocities[i]; //steer_joint_effort_NM[1] = msg->effort[i]; } if(msg->joint_names[i] == "br_caster_rotation_joint") { steer_joint_ang_rad[2] = msg->actual.positions[i]; steer_joint_vel_rads[2] = msg->actual.velocities[i]; //steer_joint_effort_NM[2] = msg->effort[i]; } if(msg->joint_names[i] == "fr_caster_rotation_joint") { steer_joint_ang_rad[3] = msg->actual.positions[i]; steer_joint_vel_rads[3] = msg->actual.velocities[i]; //steer_joint_effort_NM[3] = msg->effort[i]; } } // Set measured Wheel Velocities and Angles to Controler Class (implements inverse kinematic) ucar_ctrl_->SetActualWheelValues(drive_joint_vel_rads, steer_joint_vel_rads, drive_joint_ang_rad, steer_joint_ang_rad); // calculate odometry every time UpdateOdometry(); }