cob_relayboard_node.cpp
Go to the documentation of this file.
00001 /****************************************************************
00002  *
00003  * Copyright (c) 2010
00004  *
00005  * Fraunhofer Institute for Manufacturing Engineering   
00006  * and Automation (IPA)
00007  *
00008  * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
00009  *
00010  * Project name: care-o-bot
00011  * ROS stack name: cob_driver
00012  * ROS package name: cob_relayboard
00013  * Description: Reads and sends data over Serial Interface to the Serial Relayboard. Main Tasks: Reading of EmergencyStop and LaserScannerStop
00014  *                                                              
00015  * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
00016  *                      
00017  * Author: Philipp Koehler
00018  * Supervised by: Christian Connette, email:christian.connette@ipa.fhg.de
00019  *
00020  * Date of creation: March 2010
00021  * ToDo:
00022  *
00023  * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
00024  *
00025  * Redistribution and use in source and binary forms, with or without
00026  * modification, are permitted provided that the following conditions are met:
00027  *
00028  *     * Redistributions of source code must retain the above copyright
00029  *       notice, this list of conditions and the following disclaimer.
00030  *     * Redistributions in binary form must reproduce the above copyright
00031  *       notice, this list of conditions and the following disclaimer in the
00032  *       documentation and/or other materials provided with the distribution.
00033  *     * Neither the name of the Fraunhofer Institute for Manufacturing 
00034  *       Engineering and Automation (IPA) nor the names of its
00035  *       contributors may be used to endorse or promote products derived from
00036  *       this software without specific prior written permission.
00037  *
00038  * This program is free software: you can redistribute it and/or modify
00039  * it under the terms of the GNU Lesser General Public License LGPL as 
00040  * published by the Free Software Foundation, either version 3 of the 
00041  * License, or (at your option) any later version.
00042  * 
00043  * This program is distributed in the hope that it will be useful,
00044  * but WITHOUT ANY WARRANTY; without even the implied warranty of
00045  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00046  * GNU Lesser General Public License LGPL for more details.
00047  * 
00048  * You should have received a copy of the GNU Lesser General Public 
00049  * License LGPL along with this program. 
00050  * If not, see <http://www.gnu.org/licenses/>.
00051  *
00052  ****************************************************************/
00053 
00054 //##################
00055 //#### includes ####
00056 
00057 // standard includes
00058 //--
00059 
00060 // ROS includes
00061 #include <ros/ros.h>
00062 #include <cob_relayboard/SerRelayBoard.h>
00063 
00064 // ROS message includes
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 // ROS service includes
00072 //--
00073 
00074 // external includes
00075 //--
00076 
00077 //####################
00078 //#### node class ####
00079 class NodeClass
00080 {
00081   //
00082 public:
00083   // create a handle for this node, initialize node
00084   ros::NodeHandle n;
00085                 
00086   // topics to publish
00087   ros::Publisher topicPub_isEmergencyStop;
00088   ros::Publisher topicPub_PowerBoardState;
00089   ros::Publisher topicPub_Voltage;
00090   // topics to subscribe, callback is called for new messages arriving
00091   // --
00092 
00093   // Constructor
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     // Make sure member variables have a defined state at the beginning
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   // Destructor
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; //the relayboard is sending messages at regular time
00133   bool relayboard_available; //the relayboard has sent at least one message -> publish topic
00134 
00135   // possible states of emergency stop
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 //#### main programm ####
00148 int main(int argc, char** argv)
00149 {
00150   // initialize ROS, spezify name of node
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); //Cycle-Rate: Frequency of publishing EMStopStates
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 //#### function implementations ####
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   // Init member variable for EM State
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   // Request Status of RelayBoard 
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     //ROS_ERROR("Relayboard: Too less bytes in queue");
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; //normalize from mV to V
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   // 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)
00251   EM_msg.emergency_button_stop = m_SerRelayBoard->isEMStop();
00252   EM_msg.scanner_stop = m_SerRelayBoard->isScannerStop();
00253 
00254   // determine current EMStopState
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   // pr2 power_board_state
00302   if(EM_msg.emergency_button_stop)
00303     pbs.run_stop = false;
00304   else
00305     pbs.run_stop = true;
00306         
00307   //for cob the wireless stop field is misused as laser stop field
00308   if(EM_msg.scanner_stop)
00309     pbs.wireless_stop = false; 
00310   else
00311     pbs.wireless_stop = true;
00312   
00313 
00314   //publish EM-Stop-Active-messages, when connection to relayboard got cut
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 }


cob_relayboard
Author(s): Christian Connette
autogenerated on Thu Aug 27 2015 12:45:34