Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038
00039
00040
00041
00042
00043
00044
00045
00046
00047
00048
00049
00050
00051
00052
00053
00054
00055
00056
00057
00058
00059
00060
00061 #include <ros/ros.h>
00062 #include <cob_relayboard/SerRelayBoard.h>
00063
00064
00065 #include <std_msgs/Bool.h>
00066 #include <std_msgs/Float64.h>
00067 #include <cob_relayboard/EmergencyStopState.h>
00068 #include <pr2_msgs/PowerBoardState.h>
00069
00070
00071
00072
00073
00074
00075
00076
00077
00078
00079 class NodeClass
00080 {
00081
00082 public:
00083
00084 ros::NodeHandle n;
00085
00086
00087 ros::Publisher topicPub_isEmergencyStop;
00088 ros::Publisher topicPub_PowerBoardState;
00089 ros::Publisher topicPub_Voltage;
00090
00091
00092
00093
00094 NodeClass()
00095 {
00096 n = ros::NodeHandle("~");
00097
00098 topicPub_isEmergencyStop = n.advertise<cob_relayboard::EmergencyStopState>("/emergency_stop_state", 1);
00099 topicPub_PowerBoardState = n.advertise<pr2_msgs::PowerBoardState>("/power_board/state", 1);
00100 topicPub_Voltage = n.advertise<std_msgs::Float64>("/power_board/voltage", 10);
00101
00102
00103 EM_stop_status_ = ST_EM_FREE;
00104 relayboard_available = false;
00105 relayboard_online = false;
00106 relayboard_timeout_ = 2.0;
00107 protocol_version_ = 1;
00108 duration_for_EM_free_ = ros::Duration(1);
00109 }
00110
00111
00112 ~NodeClass()
00113 {
00114 delete m_SerRelayBoard;
00115 }
00116
00117 void sendEmergencyStopStates();
00118 void sendBatteryVoltage();
00119 int init();
00120
00121 private:
00122 std::string sComPort;
00123 SerRelayBoard * m_SerRelayBoard;
00124
00125 int EM_stop_status_;
00126 ros::Duration duration_for_EM_free_;
00127 ros::Time time_of_EM_confirmed_;
00128 double relayboard_timeout_;
00129 int protocol_version_;
00130
00131 ros::Time time_last_message_received_;
00132 bool relayboard_online;
00133 bool relayboard_available;
00134
00135
00136 enum
00137 {
00138 ST_EM_FREE = 0,
00139 ST_EM_ACTIVE = 1,
00140 ST_EM_CONFIRMED = 2
00141 };
00142
00143 int requestBoardStatus();
00144 };
00145
00146
00147
00148 int main(int argc, char** argv)
00149 {
00150
00151 ros::init(argc, argv, "cob_relayboard_node");
00152
00153 NodeClass node;
00154 if(node.init() != 0) return 1;
00155
00156 ros::Rate r(20);
00157 while(node.n.ok())
00158 {
00159 node.sendEmergencyStopStates();
00160
00161 ros::spinOnce();
00162 r.sleep();
00163 }
00164
00165 return 0;
00166 }
00167
00168
00169
00170
00171 int NodeClass::init()
00172 {
00173 if (n.hasParam("ComPort"))
00174 {
00175 n.getParam("ComPort", sComPort);
00176 ROS_INFO("Loaded ComPort parameter from parameter server: %s",sComPort.c_str());
00177 }
00178 else
00179 {
00180 sComPort ="/dev/ttyUSB0";
00181 ROS_WARN("ComPort Parameter not available, using default Port: %s",sComPort.c_str());
00182 }
00183
00184 n.param("relayboard_timeout", relayboard_timeout_, 2.0);
00185 n.param("protocol_version", protocol_version_, 1);
00186
00187 m_SerRelayBoard = new SerRelayBoard(sComPort, protocol_version_);
00188 ROS_INFO("Opened Relayboard at ComPort = %s", sComPort.c_str());
00189
00190 m_SerRelayBoard->init();
00191
00192
00193 EM_stop_status_ = ST_EM_ACTIVE;
00194 duration_for_EM_free_ = ros::Duration(1);
00195
00196 return 0;
00197 }
00198
00199 int NodeClass::requestBoardStatus() {
00200 int ret;
00201
00202
00203 ret = m_SerRelayBoard->sendRequest();
00204 if(ret != SerRelayBoard::NO_ERROR) {
00205 ROS_ERROR("Error in sending message to Relayboard over SerialIO, lost bytes during writing");
00206 }
00207
00208 ret = m_SerRelayBoard->evalRxBuffer();
00209 if(ret==SerRelayBoard::NOT_INITIALIZED) {
00210 ROS_ERROR("Failed to read relayboard data over Serial, the device is not initialized");
00211 relayboard_online = false;
00212 } else if(ret==SerRelayBoard::NO_MESSAGES) {
00213 ROS_ERROR("For a long time, no messages from RelayBoard have been received, check com port!");
00214 if(time_last_message_received_.toSec() - ros::Time::now().toSec() > relayboard_timeout_) {relayboard_online = false;}
00215 } else if(ret==SerRelayBoard::TOO_LESS_BYTES_IN_QUEUE) {
00216
00217 } else if(ret==SerRelayBoard::CHECKSUM_ERROR) {
00218 ROS_ERROR("A checksum error occurred while reading from relayboard data");
00219 } else if(ret==SerRelayBoard::NO_ERROR) {
00220 relayboard_online = true;
00221 relayboard_available = true;
00222 time_last_message_received_ = ros::Time::now();
00223 }
00224
00225 return 0;
00226 }
00227
00228 void NodeClass::sendBatteryVoltage()
00229 {
00230 std_msgs::Float64 voltage;
00231 voltage.data = m_SerRelayBoard->getBatteryVoltage()/1000.0;
00232 topicPub_Voltage.publish(voltage);
00233 }
00234
00235 void NodeClass::sendEmergencyStopStates()
00236 {
00237 requestBoardStatus();
00238
00239 if(!relayboard_available) return;
00240
00241 sendBatteryVoltage();
00242
00243
00244 bool EM_signal;
00245 ros::Duration duration_since_EM_confirmed;
00246 cob_relayboard::EmergencyStopState EM_msg;
00247 pr2_msgs::PowerBoardState pbs;
00248 pbs.header.stamp = ros::Time::now();
00249
00250
00251 EM_msg.emergency_button_stop = m_SerRelayBoard->isEMStop();
00252 EM_msg.scanner_stop = m_SerRelayBoard->isScannerStop();
00253
00254
00255 EM_signal = (EM_msg.emergency_button_stop || EM_msg.scanner_stop);
00256
00257 switch (EM_stop_status_)
00258 {
00259 case ST_EM_FREE:
00260 {
00261 if (EM_signal == true)
00262 {
00263 ROS_INFO("Emergency stop was issued");
00264 EM_stop_status_ = EM_msg.EMSTOP;
00265 }
00266 break;
00267 }
00268 case ST_EM_ACTIVE:
00269 {
00270 if (EM_signal == false)
00271 {
00272 ROS_INFO("Emergency stop was confirmed");
00273 EM_stop_status_ = EM_msg.EMCONFIRMED;
00274 time_of_EM_confirmed_ = ros::Time::now();
00275 }
00276 break;
00277 }
00278 case ST_EM_CONFIRMED:
00279 {
00280 if (EM_signal == true)
00281 {
00282 ROS_INFO("Emergency stop was issued");
00283 EM_stop_status_ = EM_msg.EMSTOP;
00284 }
00285 else
00286 {
00287 duration_since_EM_confirmed = ros::Time::now() - time_of_EM_confirmed_;
00288 if( duration_since_EM_confirmed.toSec() > duration_for_EM_free_.toSec() )
00289 {
00290 ROS_INFO("Emergency stop released");
00291 EM_stop_status_ = EM_msg.EMFREE;
00292 }
00293 }
00294 break;
00295 }
00296 };
00297
00298
00299 EM_msg.emergency_state = EM_stop_status_;
00300
00301
00302 if(EM_msg.emergency_button_stop)
00303 pbs.run_stop = false;
00304 else
00305 pbs.run_stop = true;
00306
00307
00308 if(EM_msg.scanner_stop)
00309 pbs.wireless_stop = false;
00310 else
00311 pbs.wireless_stop = true;
00312
00313
00314
00315 if(relayboard_online == false) {
00316 EM_msg.emergency_state = EM_msg.EMSTOP;
00317 }
00318 topicPub_isEmergencyStop.publish(EM_msg);
00319 topicPub_PowerBoardState.publish(pbs);
00320 }