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