int NodeClass::requestBoardStatus() {
int ret;
// Request Status of RelayBoard
ret = m_SerRelayBoard->sendRequest();
if(ret != SerRelayBoard::NO_ERROR) {
ROS_ERROR("Error in sending message to Relayboard over SerialIO, lost bytes during writing");
}
ret = m_SerRelayBoard->evalRxBuffer();
if(ret==SerRelayBoard::NOT_INITIALIZED) {
ROS_ERROR("Failed to read relayboard data over Serial, the device is not initialized");
relayboard_online = false;
} else if(ret==SerRelayBoard::NO_MESSAGES) {
ROS_ERROR("For a long time, no messages from RelayBoard have been received, check com port!");
if(time_last_message_received_.toSec() - ros::Time::now().toSec() > relayboard_timeout_) {relayboard_online = false;}
} else if(ret==SerRelayBoard::TOO_LESS_BYTES_IN_QUEUE) {
//ROS_ERROR("Relayboard: Too less bytes in queue");
} else if(ret==SerRelayBoard::CHECKSUM_ERROR) {
ROS_ERROR("A checksum error occurred while reading from relayboard data");
} else if(ret==SerRelayBoard::NO_ERROR) {
relayboard_online = true;
relayboard_available = true;
time_last_message_received_ = ros::Time::now();
}
return 0;
}
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::sendBatteryVoltage()
{
ROS_DEBUG("Current Battery Voltage: %f", (m_SerRelayBoard->getBatteryVoltage()/1000.0) + voltage_offset_);
double percentage = ((m_SerRelayBoard->getBatteryVoltage()/1000.0) + voltage_offset_ - voltage_min_) * 100/(voltage_max_ - voltage_min_);
//Not supported by relayboard
//ROS_INFO("Current Charge current: %d", m_SerRelayBoard->getChargeCurrent());
//---
pr2_msgs::PowerState ps;
ps.header.stamp = ros::Time::now();
ps.power_consumption = 0.0;
ps.time_remaining = ros::Duration(1000); //TODO: Needs to be calculated based on battery type (plumb or lithion-ion), maybe specify charging characteristics in table in .yaml file
ps.relative_capacity = percentage; // TODO: Needs to be calculated based on battery type (plumb or lithion-ion), maybe specify charging characteristics in table in .yaml file
topicPub_PowerState.publish(ps);
}
int NodeClass::init()
{
if (n.hasParam("ComPort"))
{
n.getParam("ComPort", sComPort);
ROS_INFO("Loaded ComPort parameter from parameter server: %s",sComPort.c_str());
}
else
{
sComPort ="/dev/ttyUSB0";
ROS_WARN("ComPort Parameter not available, using default Port: %s",sComPort.c_str());
}
n.param("message_timeout", relayboard_timeout_, 2.0);
n.param("protocol_version", protocol_version_, 1);
m_SerRelayBoard = new SerRelayBoard(sComPort, protocol_version_);
ROS_INFO("Opened Relayboard at ComPort = %s", sComPort.c_str());
m_SerRelayBoard->init();
// Init member variable for EM State
EM_stop_status_ = ST_EM_ACTIVE;
duration_for_EM_free_ = ros::Duration(1);
return 0;
}
int NodeClass::init() {
n.param<std::string>("cob_relayboard_node/ComPort", sComPort, "/dev/ttyUSB2");
m_SerRelayBoard = new SerRelayBoard(sComPort);
m_SerRelayBoard->init();
return 0;
}
int NodeClass::init() {
n.param<std::string>("cob_relayboard_node/ComPort", sComPort, "/dev/ttyUSB2");
m_SerRelayBoard = new SerRelayBoard(sComPort);
m_SerRelayBoard->init();
// Init member variable for EM State
EM_stop_status_ = ST_EM_FREE;
duration_for_EM_free_ = ros::Duration(1);
return 0;
}
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);
}
