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