void NodeClass::sendEmergencyStopStates()
{
requestBoardStatus();
bool EM_signal;
ros::Duration duration_since_EM_confirmed;
cob_msgs::EmergencyStopState EM_msg;
// assign input (laser, button) specific EM state
EM_msg.emergency_button_stop = m_SerRelayBoard->isEMStop();
EM_msg.scanner_stop = m_SerRelayBoard->isScannerStop();
// determine current EMStopState
EM_signal = (EM_msg.emergency_button_stop || EM_msg.scanner_stop);
switch (EM_stop_status_)
{
case ST_EM_FREE:
{
if (EM_signal == true)
{
ROS_INFO("Emergency stop was issued");
EM_stop_status_ = EM_msg.EMSTOP;
}
break;
}
case ST_EM_ACTIVE:
{
if (EM_signal == false)
{
ROS_INFO("Emergency stop was confirmed");
EM_stop_status_ = EM_msg.EMCONFIRMED;
time_of_EM_confirmed_ = ros::Time::now();
}
break;
}
case ST_EM_CONFIRMED:
{
if (EM_signal == true)
{
ROS_INFO("Emergency stop was issued");
EM_stop_status_ = EM_msg.EMSTOP;
}
else
{
duration_since_EM_confirmed = ros::Time::now() - time_of_EM_confirmed_;
if( duration_since_EM_confirmed.toSec() > duration_for_EM_free_.toSec() )
{
ROS_INFO("Emergency stop released");
EM_stop_status_ = EM_msg.EMFREE;
}
}
break;
}
};
EM_msg.emergency_state = EM_stop_status_;
topicPub_isEmergencyStop.publish(EM_msg);
}
void NodeClass::sendEmergencyStopStates()
{
requestBoardStatus();
std_msgs::Bool msg;
msg.data = m_SerRelayBoard->isEMStop();
topicPub_isEmergencyStop.publish(msg);
msg.data = m_SerRelayBoard->isScannerStop();
topicPub_isScannerStop.publish(msg);
}
void NodeClass::sendEmergencyStopStates()
{
requestBoardStatus();
if(!relayboard_available) return;
sendBatteryVoltage();
bool EM_signal;
ros::Duration duration_since_EM_confirmed;
cob_relayboard::EmergencyStopState EM_msg;
pr2_msgs::PowerBoardState pbs;
pbs.header.stamp = ros::Time::now();
// assign input (laser, button) specific EM state TODO: Laser and Scanner stop can't be read independently (e.g. if button is stop --> no informtion about scanner, if scanner ist stop --> no informtion about button stop)
EM_msg.emergency_button_stop = m_SerRelayBoard->isEMStop();
EM_msg.scanner_stop = m_SerRelayBoard->isScannerStop();
// determine current EMStopState
EM_signal = (EM_msg.emergency_button_stop || EM_msg.scanner_stop);
switch (EM_stop_status_)
{
case ST_EM_FREE:
{
if (EM_signal == true)
{
ROS_INFO("Emergency stop was issued");
EM_stop_status_ = EM_msg.EMSTOP;
}
break;
}
case ST_EM_ACTIVE:
{
if (EM_signal == false)
{
ROS_INFO("Emergency stop was confirmed");
EM_stop_status_ = EM_msg.EMCONFIRMED;
time_of_EM_confirmed_ = ros::Time::now();
}
break;
}
case ST_EM_CONFIRMED:
{
if (EM_signal == true)
{
ROS_INFO("Emergency stop was issued");
EM_stop_status_ = EM_msg.EMSTOP;
}
else
{
duration_since_EM_confirmed = ros::Time::now() - time_of_EM_confirmed_;
if( duration_since_EM_confirmed.toSec() > duration_for_EM_free_.toSec() )
{
ROS_INFO("Emergency stop released");
EM_stop_status_ = EM_msg.EMFREE;
}
}
break;
}
};
EM_msg.emergency_state = EM_stop_status_;
// pr2 power_board_state
if(EM_msg.emergency_button_stop)
pbs.run_stop = false;
else
pbs.run_stop = true;
//for cob the wireless stop field is misused as laser stop field
if(EM_msg.scanner_stop)
pbs.wireless_stop = false;
else
pbs.wireless_stop = true;
//publish EM-Stop-Active-messages, when connection to relayboard got cut
if(relayboard_online == false) {
EM_msg.emergency_state = EM_msg.EMSTOP;
}
topicPub_isEmergencyStop.publish(EM_msg);
topicPub_PowerBoardState.publish(pbs);
}