00001 /********************************************************************* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Copyright (c) 2016, Neobotix GmbH 00005 * All rights reserved. 00006 * 00007 * Redistribution and use in source and binary forms, with or without 00008 * modification, are permitted provided that the following conditions 00009 * are met: 00010 * 00011 * * Redistributions of source code must retain the above copyright 00012 * notice, this list of conditions and the following disclaimer. 00013 * * Redistributions in binary form must reproduce the above 00014 * copyright notice, this list of conditions and the following 00015 * disclaimer in the documentation and/or other materials provided 00016 * with the distribution. 00017 * * Neither the name of the Neobotix nor the names of its 00018 * contributors may be used to endorse or promote products derived 00019 * from this software without specific prior written permission. 00020 * 00021 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00022 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00023 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00024 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00025 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00026 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00027 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00028 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00029 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00030 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00031 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00032 * POSSIBILITY OF SUCH DAMAGE. 00033 *********************************************************************/ 00034 00035 // ROS includes 00036 #include <ros/ros.h> 00037 #include <iostream> 00038 #include <RelayBoard_v2.h> 00039 00040 // ROS message includes 00041 00042 #include <std_msgs/Bool.h> 00043 #include <std_msgs/Int16.h> 00044 #include <std_msgs/Empty.h> 00045 #include <std_msgs/Header.h> 00046 #include <sensor_msgs/JointState.h> 00047 #include <sensor_msgs/Range.h> 00048 #include <trajectory_msgs/JointTrajectory.h> 00049 #include <neo_msgs/EmergencyStopState.h> 00050 #include <neo_msgs/Keypad.h> 00051 #include <neo_msgs/LCDOutput.h> 00052 #include <neo_msgs/USBoard.h> 00053 #include <neo_msgs/IOOut.h> 00054 #include <neo_msgs/IOAnalogIn.h> 00055 #include <neo_msgs/PowerState.h> 00056 00057 // ROS service includes 00058 //-- 00059 00060 // external includes 00061 //-- 00062 00063 //#################### 00064 //#### node class #### 00065 class neo_relayboardV2_node 00066 { 00067 public: 00068 // create a handle for this node, initialize node 00069 ros::NodeHandle n; 00070 00071 // topics: 00072 //basic topics: 00073 ros::Publisher topicPub_RelayBoardState; 00074 ros::Publisher topicPub_isEmergencyStop; 00075 ros::Publisher topicPub_batVoltage; 00076 ros::Publisher topicPub_chargeCurrent; 00077 ros::Publisher topicPub_chargeState; 00078 ros::Publisher topicPub_temperatur; 00079 ros::Publisher topicPub_keypad; 00080 ros::Subscriber topicSub_startCharging; 00081 ros::Subscriber topicSub_stopCharging; 00082 00083 ros::Publisher topicPub_SendRelayStates; 00084 ros::Subscriber topicSub_SetRelayStates; 00085 //drives: 00086 ros::Publisher topicPub_drives; // 00087 ros::Subscriber topicSub_drives; // 00088 //ultrasonic board: 00089 ros::Publisher topicPub_usBoard; 00090 ros::Publisher topicPub_USRangeSensor1; 00091 ros::Publisher topicPub_USRangeSensor2; 00092 ros::Publisher topicPub_USRangeSensor3; 00093 ros::Publisher topicPub_USRangeSensor4; 00094 ros::Publisher topicPub_USRangeSensor5; 00095 ros::Publisher topicPub_USRangeSensor6; 00096 ros::Publisher topicPub_USRangeSensor7; 00097 ros::Publisher topicPub_USRangeSensor8; 00098 ros::Publisher topicPub_USRangeSensor9; 00099 ros::Publisher topicPub_USRangeSensor10; 00100 ros::Publisher topicPub_USRangeSensor11; 00101 ros::Publisher topicPub_USRangeSensor12; 00102 ros::Publisher topicPub_USRangeSensor13; 00103 ros::Publisher topicPub_USRangeSensor14; 00104 ros::Publisher topicPub_USRangeSensor15; 00105 ros::Publisher topicPub_USRangeSensor16; 00106 00107 ros::Subscriber topicSub_startUSBoard; 00108 ros::Subscriber topicSub_stopUSBoard; 00109 00110 //io board: 00111 ros::Publisher topicPub_ioDigIn; 00112 ros::Publisher topicPub_ioDigOut; 00113 ros::Publisher topicPub_analogIn; 00114 ros::Subscriber topicSub_lcdDisplay; 00115 ros::Subscriber topicSub_setDigOut; 00116 00117 00118 // Constructor 00119 neo_relayboardV2_node() 00120 { 00121 // Make sure member variables have a defined state at the beginning 00122 m_iEM_stop_state = ST_EM_FREE; 00123 m_iRelayBoard_available = 0; 00124 m_dRelayBoard_timeout = 2.0; 00125 m_duration_for_EM_free = ros::Duration(1); 00126 m_iactive_motors = 0; 00127 m_imotor_count = 0; 00128 m_ihoming_motors = 0; 00129 m_iext_hardware = 0; 00130 m_ihasIOBoard = 0; 00131 m_ihasUSBoard = 0; 00132 } 00133 00134 // Destructor 00135 ~neo_relayboardV2_node() 00136 { 00137 delete m_SerRelayBoard; 00138 } 00139 00140 //Comm Handler 00141 int init(); 00142 void HandleCommunication(); 00143 00144 // Topic Callbacks 00145 void getNewVelocitiesFomTopic(const trajectory_msgs::JointTrajectory jt); 00146 void startUSBoard(const std_msgs::Int16& configuration); 00147 void stopUSBoard(const std_msgs::Empty& empty); 00148 void getRelayBoardDigOut(const neo_msgs::IOOut& setOut); 00149 void getNewLCDMsg(const neo_msgs::LCDOutput& msg); 00150 void getIOBoardDigOut(const neo_msgs::IOOut& setOut); 00151 00152 void startCharging(const std_msgs::Empty& empty); 00153 void stopCharging(const std_msgs::Empty& empty); 00154 00155 // Pubisher functions 00156 //RelayBoard 00157 void PublishRelayBoardState(); 00158 void PublishTemperature(); 00159 void PublishBattVoltage(); 00160 void PublishChargingCurrent(); 00161 void PublishChargingState(); 00162 void PublishEmergencyStopStates(); 00163 void PublishRelayBoardDigOut(); 00164 void PublishKeyPad(); 00165 //Motors 00166 void PublishJointStates(); 00167 //USBoard 00168 void PublishUSBoardData(); 00169 //IOBoard 00170 void PublishIOBoardAnalogIn(); 00171 void PublishIOBoardDigIn(); 00172 void PublishIOBoardDigOut(); 00173 00174 double getRequestRate(); 00175 private: 00176 00177 //----Configuration-------- 00178 int m_iactive_motors; 00179 int m_imotor_count; 00180 int m_ihoming_motors; 00181 int m_iext_hardware; 00182 int m_ihasIOBoard; 00183 int m_ihasUSBoard; 00184 int m_iRelayBoard_available; //the RelayBoard has confirmed the configuration and is ready 00185 std::string m_sComPort; 00186 RelayBoardV2 * m_SerRelayBoard; 00187 00188 DriveParam m_Drives[8]; 00189 00190 //----EM Stop Handling------ 00191 int m_iEM_stop_state; 00192 ros::Duration m_duration_for_EM_free; 00193 ros::Time m_time_of_EM_confirmed; 00194 // possible states of emergency stop 00195 enum 00196 { 00197 ST_EM_FREE = 0, 00198 ST_EM_ACTIVE = 1, 00199 ST_EM_CONFIRMED = 2 00200 }; 00201 //----Msg Handling------------ 00202 double m_dRelayBoard_timeout; 00203 double m_dRequestRate; 00204 ros::Time m_time_last_message_received; 00205 00206 00207 //log 00208 int m_ilog; //enables or disables the log for neo_relayboard 00209 }; 00210