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 #include <ros/ros.h>
00036 #include <../include/neo_relayboard_v2_node.h>
00037 #include <math.h>
00038
00039
00040
00041 int neo_relayboardV2_node::init()
00042 {
00043
00044
00045
00046 if (n.hasParam("ComPort"))
00047 {
00048 n.getParam("ComPort", m_sComPort);
00049 ROS_INFO("Loaded ComPort parameter from parameter server: %s",m_sComPort.c_str());
00050 }
00051
00052 n.getParam("hasIOBoard", m_ihasIOBoard);
00053 n.getParam("hasUSBoard", m_ihasUSBoard);
00054
00055
00056 n.param("message_timeout", m_dRelayBoard_timeout, 2.0);
00057 n.param("requestRate", m_dRequestRate, 25.0);
00058
00059
00060 n.getParam("log", m_ilog);
00061
00062
00063
00064 n.param("drive2/motor_active", m_Drives[0].imotor_active, 0);
00065 n.param("drive2/homing_active", m_Drives[0].ihoming_active, 0);
00066 n.param("drive2/EncIncrPerRevMot", m_Drives[0].iEncIncrPerRevMot, 0);
00067 n.param("drive2/VelMeasFrqHz", m_Drives[0].dVelMeasFrqHz, 0.0);
00068 n.param("drive2/GearRatio", m_Drives[0].dGearRatio, 0.0);
00069 n.param("drive2/BeltRatio", m_Drives[0].dBeltRatio, 0.0);
00070 n.param("drive2/Sign", m_Drives[0].iSign, 0);
00071 n.param("drive2/VelMaxEncIncrS", m_Drives[0].dVelMaxEncIncrS, 0.0);
00072 n.param("drive2/VelPModeEncIncrS", m_Drives[0].dVelPModeEncIncrS, 0.0);
00073 n.param("drive2/AccIncrS2", m_Drives[0].dAccIncrS2, 0.0);
00074 n.param("drive2/DecIncrS2", m_Drives[0].dDecIncrS2, 0.0);
00075 n.param("drive2/Modulo", m_Drives[0].dModulo, 0.0);
00076 m_Drives[0].calcRadToIncr();
00077
00078 n.param("drive3/motor_active", m_Drives[1].imotor_active, 0);
00079 n.param("drive3/homing_active", m_Drives[1].ihoming_active, 0);
00080 n.param("drive3/EncIncrPerRevMot", m_Drives[1].iEncIncrPerRevMot, 0);
00081 n.param("drive3/VelMeasFrqHz", m_Drives[1].dVelMeasFrqHz, 0.0);
00082 n.param("drive3/GearRatio", m_Drives[1].dGearRatio, 0.0);
00083 n.param("drive3/BeltRatio", m_Drives[1].dBeltRatio, 0.0);
00084 n.param("drive3/Sign", m_Drives[1].iSign, 0);
00085 n.param("drive3/VelMaxEncIncrS", m_Drives[1].dVelMaxEncIncrS, 0.0);
00086 n.param("drive3/VelPModeEncIncrS", m_Drives[1].dVelPModeEncIncrS, 0.0);
00087 n.param("drive3/AccIncrS2", m_Drives[1].dAccIncrS2, 0.0);
00088 n.param("drive3/DecIncrS2", m_Drives[1].dDecIncrS2, 0.0);
00089 n.param("drive3/Modulo", m_Drives[1].dModulo, 0.0);
00090 m_Drives[1].calcRadToIncr();
00091
00092 n.param("drive4/motor_active", m_Drives[2].imotor_active, 0);
00093 n.param("drive4/homing_active", m_Drives[2].ihoming_active, 0);
00094 n.param("drive4/EncIncrPerRevMot", m_Drives[2].iEncIncrPerRevMot, 0);
00095 n.param("drive4/VelMeasFrqHz", m_Drives[2].dVelMeasFrqHz, 0.0);
00096 n.param("drive4/GearRatio", m_Drives[2].dGearRatio, 0.0);
00097 n.param("drive4/BeltRatio", m_Drives[2].dBeltRatio, 0.0);
00098 n.param("drive4/Sign", m_Drives[2].iSign, 0);
00099 n.param("drive4/VelMaxEncIncrS", m_Drives[2].dVelMaxEncIncrS, 0.0);
00100 n.param("drive4/VelPModeEncIncrS", m_Drives[2].dVelPModeEncIncrS, 0.0);
00101 n.param("drive4/AccIncrS2", m_Drives[2].dAccIncrS2, 0.0);
00102 n.param("drive4/DecIncrS2", m_Drives[2].dDecIncrS2, 0.0);
00103 n.param("drive4/Modulo", m_Drives[2].dModulo, 0.0);
00104 m_Drives[2].calcRadToIncr();
00105
00106 n.param("drive5/motor_active", m_Drives[3].imotor_active, 0);
00107 n.param("drive5/homing_active", m_Drives[3].ihoming_active, 0);
00108 n.param("drive5/EncIncrPerRevMot", m_Drives[3].iEncIncrPerRevMot, 0);
00109 n.param("drive5/VelMeasFrqHz", m_Drives[3].dVelMeasFrqHz, 0.0);
00110 n.param("drive5/GearRatio", m_Drives[3].dGearRatio, 0.0);
00111 n.param("drive5/BeltRatio", m_Drives[3].dBeltRatio, 0.0);
00112 n.param("drive5/Sign", m_Drives[3].iSign, 0);
00113 n.param("drive5/VelMaxEncIncrS", m_Drives[3].dVelMaxEncIncrS, 0.0);
00114 n.param("drive5/VelPModeEncIncrS", m_Drives[3].dVelPModeEncIncrS, 0.0);
00115 n.param("drive5/AccIncrS2", m_Drives[3].dAccIncrS2, 0.0);
00116 n.param("drive5/DecIncrS2", m_Drives[3].dDecIncrS2, 0.0);
00117 n.param("drive5/Modulo", m_Drives[3].dModulo, 0.0);
00118 m_Drives[3].calcRadToIncr();
00119
00120 n.param("drive6/motor_active", m_Drives[4].imotor_active, 0);
00121 n.param("drive6/homing_active", m_Drives[4].ihoming_active, 0);
00122 n.param("drive6/EncIncrPerRevMot", m_Drives[4].iEncIncrPerRevMot, 0);
00123 n.param("drive6/VelMeasFrqHz", m_Drives[4].dVelMeasFrqHz, 0.0);
00124 n.param("drive6/GearRatio", m_Drives[4].dGearRatio, 0.0);
00125 n.param("drive6/BeltRatio", m_Drives[4].dBeltRatio, 0.0);
00126 n.param("drive6/Sign", m_Drives[4].iSign, 0);
00127 n.param("drive6/VelMaxEncIncrS", m_Drives[4].dVelMaxEncIncrS, 0.0);
00128 n.param("drive6/VelPModeEncIncrS", m_Drives[4].dVelPModeEncIncrS, 0.0);
00129 n.param("drive6/AccIncrS2", m_Drives[4].dAccIncrS2, 0.0);
00130 n.param("drive6/DecIncrS2", m_Drives[4].dDecIncrS2, 0.0);
00131 n.param("drive6/Modulo", m_Drives[4].dModulo, 0.0);
00132 m_Drives[4].calcRadToIncr();
00133
00134 n.param("drive7/motor_active", m_Drives[5].imotor_active, 0);
00135 n.param("drive7/homing_active", m_Drives[5].ihoming_active, 0);
00136 n.param("drive7/EncIncrPerRevMot", m_Drives[5].iEncIncrPerRevMot, 0);
00137 n.param("drive7/VelMeasFrqHz", m_Drives[5].dVelMeasFrqHz, 0.0);
00138 n.param("drive7/GearRatio", m_Drives[5].dGearRatio, 0.0);
00139 n.param("drive7/BeltRatio", m_Drives[5].dBeltRatio, 0.0);
00140 n.param("drive7/Sign", m_Drives[5].iSign, 0);
00141 n.param("drive7/VelMaxEncIncrS", m_Drives[5].dVelMaxEncIncrS, 0.0);
00142 n.param("drive7/VelPModeEncIncrS", m_Drives[5].dVelPModeEncIncrS, 0.0);
00143 n.param("drive7/AccIncrS2", m_Drives[5].dAccIncrS2, 0.0);
00144 n.param("drive7/DecIncrS2", m_Drives[5].dDecIncrS2, 0.0);
00145 n.param("drive7/Modulo", m_Drives[5].dModulo, 0.0);
00146 m_Drives[5].calcRadToIncr();
00147
00148 n.param("drive8/motor_active", m_Drives[6].imotor_active, 0);
00149 n.param("drive8/homing_active", m_Drives[6].ihoming_active, 0);
00150 n.param("drive8/EncIncrPerRevMot", m_Drives[6].iEncIncrPerRevMot, 0);
00151 n.param("drive8/VelMeasFrqHz", m_Drives[6].dVelMeasFrqHz, 0.0);
00152 n.param("drive8/GearRatio", m_Drives[6].dGearRatio, 0.0);
00153 n.param("drive8/BeltRatio", m_Drives[6].dBeltRatio, 0.0);
00154 n.param("drive8/Sign", m_Drives[6].iSign, 0);
00155 n.param("drive8/VelMaxEncIncrS", m_Drives[6].dVelMaxEncIncrS, 0.0);
00156 n.param("drive8/VelPModeEncIncrS", m_Drives[6].dVelPModeEncIncrS, 0.0);
00157 n.param("drive8/AccIncrS2", m_Drives[6].dAccIncrS2, 0.0);
00158 n.param("drive8/DecIncrS2", m_Drives[6].dDecIncrS2, 0.0);
00159 n.param("drive8/Modulo", m_Drives[6].dModulo, 0.0);
00160 m_Drives[6].calcRadToIncr();
00161
00162 n.param("drive9/motor_active", m_Drives[7].imotor_active, 0);
00163 n.param("drive9/homing_active", m_Drives[7].ihoming_active, 0);
00164 n.param("drive9/EncIncrPerRevMot", m_Drives[7].iEncIncrPerRevMot, 0);
00165 n.param("drive9/VelMeasFrqHz", m_Drives[7].dVelMeasFrqHz, 0.0);
00166 n.param("drive9/GearRatio", m_Drives[7].dGearRatio, 0.0);
00167 n.param("drive9/BeltRatio", m_Drives[7].dBeltRatio, 0.0);
00168 n.param("drive9/Sign", m_Drives[7].iSign, 0);
00169 n.param("drive9/VelMaxEncIncrS", m_Drives[7].dVelMaxEncIncrS, 0.0);
00170 n.param("drive9/VelPModeEncIncrS", m_Drives[7].dVelPModeEncIncrS, 0.0);
00171 n.param("drive9/AccIncrS2", m_Drives[7].dAccIncrS2, 0.0);
00172 n.param("drive9/DecIncrS2", m_Drives[7].dDecIncrS2, 0.0);
00173 n.param("drive9/Modulo", m_Drives[7].dModulo, 0.0);
00174 m_Drives[7].calcRadToIncr();
00175
00176
00177
00178 if(m_Drives[0].imotor_active == 1) {m_iactive_motors += 1;m_imotor_count++;}
00179 if(m_Drives[1].imotor_active == 1) {m_iactive_motors += 2;m_imotor_count++;}
00180 if(m_Drives[2].imotor_active == 1) {m_iactive_motors += 4;m_imotor_count++;}
00181 if(m_Drives[3].imotor_active == 1) {m_iactive_motors += 8;m_imotor_count++;}
00182 if(m_Drives[4].imotor_active == 1) {m_iactive_motors += 16;m_imotor_count++;}
00183 if(m_Drives[5].imotor_active == 1) {m_iactive_motors += 32;m_imotor_count++;}
00184 if(m_Drives[6].imotor_active == 1) {m_iactive_motors += 64;m_imotor_count++;}
00185 if(m_Drives[7].imotor_active == 1) {m_iactive_motors += 128;m_imotor_count++;}
00186
00187 if(m_Drives[0].ihoming_active == 1) m_ihoming_motors += 1;
00188 if(m_Drives[1].ihoming_active == 1) m_ihoming_motors += 2;
00189 if(m_Drives[2].ihoming_active == 1) m_ihoming_motors += 4;
00190 if(m_Drives[3].ihoming_active == 1) m_ihoming_motors += 8;
00191 if(m_Drives[4].ihoming_active == 1) m_ihoming_motors += 16;
00192 if(m_Drives[5].ihoming_active == 1) m_ihoming_motors += 32;
00193 if(m_Drives[6].ihoming_active == 1) m_ihoming_motors += 64;
00194 if(m_Drives[7].ihoming_active == 1) m_ihoming_motors += 128;
00195
00196 if(m_ihasIOBoard == 1) m_iext_hardware += 1;
00197 if(m_ihasUSBoard == 1) m_iext_hardware += 2;
00198 ROS_INFO("Parameters loaded");
00199 m_SerRelayBoard = new RelayBoardV2();
00200 int ret = m_SerRelayBoard->init(m_sComPort.c_str(),m_iactive_motors,m_ihoming_motors,m_iext_hardware,(long)m_Drives[0].dModulo,(long)m_Drives[1].dModulo,(long)m_Drives[2].dModulo,(long)m_Drives[3].dModulo,(long)m_Drives[4].dModulo,(long)m_Drives[5].dModulo,(long)m_Drives[6].dModulo,(long)m_Drives[7].dModulo);
00201 if(ret == 1)
00202 {
00203 m_iRelayBoard_available = true;
00204 ROS_INFO("Opened RelayboardV2 at ComPort = %s", m_sComPort.c_str());
00205 }
00206 else
00207 {
00208 ROS_ERROR("FAILED to open RelayboardV2 at ComPort = %s", m_sComPort.c_str());
00209
00210 return(1);
00211 }
00212
00213
00214
00215 topicPub_isEmergencyStop = n.advertise<neo_msgs::EmergencyStopState>("/Emergency_Stop_State", 1);
00216 topicPub_batVoltage = n.advertise<std_msgs::Int16>("/RelayBoardV2/Battery_Voltage", 1);
00217 topicPub_chargeCurrent = n.advertise<std_msgs::Int16>("/RelayBoardV2/Charging/Charging_Current", 1);
00218 topicPub_chargeState = n.advertise<std_msgs::Int16>("/RelayBoardV2/Charging/Charging_State", 1);
00219 topicPub_temperatur = n.advertise<std_msgs::Int16>("/RelayBoardV2/Temperature", 1);
00220 topicSub_SetRelayStates = n.subscribe("/RelayBoardV2/Set_Relay_States",1,&neo_relayboardV2_node::getRelayBoardDigOut, this);
00221 topicPub_SendRelayStates = n.advertise<std_msgs::Int16>("/RelayBoardV2/Relay_States",1);
00222 topicPub_RelayBoardState = n.advertise<std_msgs::Int16>("/RelayBoardV2/State",1);
00223 topicSub_startCharging = n.subscribe("/RelayBoardV2/Charging/Start",1,&neo_relayboardV2_node::startCharging, this);
00224 topicSub_stopCharging = n.subscribe("/RelayBoardV2/Charging/Stop",1,&neo_relayboardV2_node::stopCharging, this);
00225
00226 if(m_iactive_motors != 0)
00227 {
00228 topicPub_drives = n.advertise<sensor_msgs::JointState>("/Drives/JointStates",1);
00229 topicSub_drives = n.subscribe("/Drives/Set_Velocities",1,&neo_relayboardV2_node::getNewVelocitiesFomTopic, this);
00230 }
00231
00232 topicSub_lcdDisplay = n.subscribe("/RelayBoardV2/Set_LCD_Message",1,&neo_relayboardV2_node::getNewLCDMsg, this);
00233 if(m_ihasIOBoard == 1)
00234 {
00235 topicSub_setDigOut = n.subscribe("/IOBoard/Set_Dig_Out",1,&neo_relayboardV2_node::getIOBoardDigOut, this);
00236 topicPub_ioDigIn = n.advertise<std_msgs::Int16>("/IOBoard/Dig_In",1);
00237 topicPub_ioDigOut = n.advertise<std_msgs::Int16>("/IOBoard/Dig_Out",1);
00238 topicPub_analogIn = n.advertise<neo_msgs::IOAnalogIn>("/IOBoard/Analog_in",1);
00239
00240 }
00241 if(m_ihasUSBoard == 1)
00242 {
00243 topicPub_usBoard = n.advertise<neo_msgs::USBoard>("/USBoard/Measurements",1);
00244 topicSub_startUSBoard = n.subscribe("/USBoard/Start",1,&neo_relayboardV2_node::startUSBoard, this);
00245 topicSub_stopUSBoard = n.subscribe("/USBoard/Stop",1,&neo_relayboardV2_node::stopUSBoard, this);
00246
00247 topicPub_USRangeSensor1 = n.advertise<sensor_msgs::Range>("/USBoard/Sensor1",1);
00248 topicPub_USRangeSensor2 = n.advertise<sensor_msgs::Range>("/USBoard/Sensor2",1);
00249 topicPub_USRangeSensor3 = n.advertise<sensor_msgs::Range>("/USBoard/Sensor3",1);
00250 topicPub_USRangeSensor4 = n.advertise<sensor_msgs::Range>("/USBoard/Sensor4",1);
00251 topicPub_USRangeSensor5 = n.advertise<sensor_msgs::Range>("/USBoard/Sensor5",1);
00252 topicPub_USRangeSensor6 = n.advertise<sensor_msgs::Range>("/USBoard/Sensor6",1);
00253 topicPub_USRangeSensor7 = n.advertise<sensor_msgs::Range>("/USBoard/Sensor7",1);
00254 topicPub_USRangeSensor8 = n.advertise<sensor_msgs::Range>("/USBoard/Sensor8",1);
00255 topicPub_USRangeSensor9 = n.advertise<sensor_msgs::Range>("/USBoard/Sensor9",1);
00256 topicPub_USRangeSensor10 = n.advertise<sensor_msgs::Range>("/USBoard/Sensor10",1);
00257 topicPub_USRangeSensor11 = n.advertise<sensor_msgs::Range>("/USBoard/Sensor11",1);
00258 topicPub_USRangeSensor12 = n.advertise<sensor_msgs::Range>("/USBoard/Sensor12",1);
00259 topicPub_USRangeSensor13 = n.advertise<sensor_msgs::Range>("/USBoard/Sensor13",1);
00260 topicPub_USRangeSensor14 = n.advertise<sensor_msgs::Range>("/USBoard/Sensor14",1);
00261 topicPub_USRangeSensor15 = n.advertise<sensor_msgs::Range>("/USBoard/Sensor15",1);
00262 topicPub_USRangeSensor16 = n.advertise<sensor_msgs::Range>("/USBoard/Sensor16",1);
00263
00264 }
00265 topicPub_keypad = n.advertise<neo_msgs::Keypad>("/RelayBoardV2/Keypad",1);
00266
00267
00268 if(m_ilog == 1)
00269 {
00270 ROS_INFO("Log enabled");
00271 m_SerRelayBoard->enable_logging();
00272 }
00273 else
00274 {
00275 ROS_INFO("Log disabled");
00276 m_SerRelayBoard->disable_logging();
00277 }
00278
00279 return 0;
00280 }
00281
00282
00283 void neo_relayboardV2_node::HandleCommunication()
00284 {
00285 if(!m_iRelayBoard_available) return;
00286 int send_return = 0;
00287 int receive_return = 101;
00288 static int last_error = 0;
00289 while(receive_return == 101)
00290 {
00291
00292 send_return = m_SerRelayBoard->sendDataToRelayBoard();
00293 receive_return = m_SerRelayBoard->evalRxBuffer();
00294
00295 if(receive_return == last_error)
00296 {
00297
00298 }
00299 else if(receive_return == 0)
00300 {
00301 ROS_INFO("communicating with RelayBoard");
00302 }
00303 else if(receive_return == 101)
00304 {
00305 ROS_ERROR("No answer from RelayBoard ... ");
00306 }
00307 else if(receive_return == 97)
00308 {
00309 ROS_ERROR("Wrong Checksum");
00310 }
00311 else if(receive_return == 99)
00312 {
00313 ROS_ERROR("No valid Message header found");
00314 }
00315 else
00316 {
00317
00318 ROS_ERROR("Unknown error");
00319 }
00320 last_error = receive_return;
00321
00322 }
00323 }
00324
00325 double neo_relayboardV2_node::getRequestRate()
00326 {
00327 return m_dRequestRate;
00328 }
00329
00330 void neo_relayboardV2_node::PublishRelayBoardState()
00331 {
00332 if(!m_iRelayBoard_available) return;
00333 int value = 0;
00334 std_msgs::Int16 state;
00335 m_SerRelayBoard->getRelayBoardState(&value);
00336 state.data = value;
00337 topicPub_RelayBoardState.publish(state);
00338
00339
00340
00341 if((value & 0x400) != 0)
00342 {
00343 ROS_INFO("-----------SHUTDOWN Signal from RelayBoard v2----------");
00344 ros::shutdown();
00345 usleep(2000);
00346 system("dbus-send --system --print-reply --dest=org.freedesktop.login1 /org/freedesktop/login1 \"org.freedesktop.login1.Manager.PowerOff\" boolean:true");
00347 }
00348 }
00349 void neo_relayboardV2_node::PublishTemperature()
00350 {
00351 if(!m_iRelayBoard_available) return;
00352 int temperature = 0;
00353 std_msgs::Int16 temp;
00354 m_SerRelayBoard->getTemperature(&temperature);
00355 temp.data = temperature;
00356 topicPub_temperatur.publish(temp);
00357 }
00358 void neo_relayboardV2_node::PublishBattVoltage()
00359 {
00360 if(!m_iRelayBoard_available) return;
00361 int battvolt = 0;
00362 std_msgs::Int16 bat;
00363 m_SerRelayBoard->getBattVoltage(&battvolt);
00364 bat.data = battvolt/1000;
00365 topicPub_batVoltage.publish(bat);
00366 }
00367 void neo_relayboardV2_node::PublishChargingCurrent()
00368 {
00369 if(!m_iRelayBoard_available) return;
00370 int value = 0;
00371 std_msgs::Int16 current;
00372 m_SerRelayBoard->getChargingCurrent(&value);
00373 current.data = value;
00374 topicPub_chargeCurrent.publish(current);
00375 }
00376 void neo_relayboardV2_node::PublishChargingState()
00377 {
00378 if(!m_iRelayBoard_available) return;
00379 int value = 0;
00380 std_msgs::Int16 state;
00381 m_SerRelayBoard->getChargingState(&value);
00382 state.data = value;
00383 topicPub_chargeState.publish(state);
00384 }
00385 void neo_relayboardV2_node::PublishKeyPad()
00386 {
00387 if(!m_iRelayBoard_available) return;
00388 int value = 0;
00389 neo_msgs::Keypad pad;
00390 m_SerRelayBoard->getKeyPad(&value);
00391 pad.button[0] = !(value & 1);
00392 pad.button[1] = !(value & 2);
00393 pad.button[2] = !(value & 4);
00394 pad.button[3] = !(value & 8);
00395 pad.button[4] = !(value & 16);
00396 topicPub_keypad.publish(pad);
00397 }
00398 void neo_relayboardV2_node::PublishEmergencyStopStates()
00399 {
00400 if(!m_iRelayBoard_available) return;
00401 bool EM_signal;
00402 ros::Duration duration_since_EM_confirmed;
00403 neo_msgs::EmergencyStopState EM_msg;
00404
00405
00406 EM_msg.emergency_button_stop = m_SerRelayBoard->isEMStop();
00407 EM_msg.scanner_stop = m_SerRelayBoard->isScannerStop();
00408
00409
00410 EM_signal = (EM_msg.emergency_button_stop || EM_msg.scanner_stop);
00411
00412 switch (m_iEM_stop_state)
00413 {
00414 case ST_EM_FREE:
00415 {
00416 if (EM_signal == true)
00417 {
00418 ROS_ERROR("Emergency stop was issued");
00419 m_iEM_stop_state = EM_msg.EMSTOP;
00420 }
00421 break;
00422 }
00423 case ST_EM_ACTIVE:
00424 {
00425 if (EM_signal == false)
00426 {
00427 ROS_INFO("Emergency stop was confirmed");
00428 m_iEM_stop_state = EM_msg.EMCONFIRMED;
00429 m_time_of_EM_confirmed = ros::Time::now();
00430 }
00431 break;
00432 }
00433 case ST_EM_CONFIRMED:
00434 {
00435 if (EM_signal == true)
00436 {
00437 ROS_ERROR("Emergency stop was issued");
00438 m_iEM_stop_state = EM_msg.EMSTOP;
00439 }
00440 else
00441 {
00442 duration_since_EM_confirmed = ros::Time::now() - m_time_of_EM_confirmed;
00443 if( duration_since_EM_confirmed.toSec() > m_duration_for_EM_free.toSec() )
00444 {
00445 ROS_INFO("Emergency stop released");
00446 m_iEM_stop_state = EM_msg.EMFREE;
00447 }
00448 }
00449 break;
00450 }
00451 };
00452
00453 EM_msg.emergency_state = m_iEM_stop_state;
00454
00455
00456 topicPub_isEmergencyStop.publish(EM_msg);
00457 }
00458 void neo_relayboardV2_node::PublishRelayBoardDigOut()
00459 {
00460 if(!m_iRelayBoard_available) return;
00461 int data = 0;
00462 std_msgs::Int16 i;
00463 m_SerRelayBoard->getRelayBoardDigOut(&data);
00464 i.data = data;
00465 topicPub_SendRelayStates.publish(i);
00466
00467 }
00468
00469 void neo_relayboardV2_node::getNewLCDMsg(const neo_msgs::LCDOutput& msg)
00470 {
00471 if(!m_iRelayBoard_available) return;
00472 m_SerRelayBoard->writeLCD(msg.msg_line);
00473
00474 }
00475 void neo_relayboardV2_node::getRelayBoardDigOut(const neo_msgs::IOOut& setOut)
00476 {
00477 if(!m_iRelayBoard_available) return;
00478 m_SerRelayBoard->setRelayBoardDigOut(setOut.channel, setOut.active);
00479 }
00480 void neo_relayboardV2_node::startCharging(const std_msgs::Empty& empty)
00481 {
00482 if(!m_iRelayBoard_available) return;
00483 m_SerRelayBoard->startCharging();
00484 }
00485
00486 void neo_relayboardV2_node::stopCharging(const std_msgs::Empty& empty)
00487 {
00488 if(!m_iRelayBoard_available) return;
00489 m_SerRelayBoard->stopCharging();
00490 }
00491
00492
00493 void neo_relayboardV2_node::PublishJointStates()
00494 {
00495 if(!m_iRelayBoard_available) return;
00496 if(m_iactive_motors == 0) return;
00497
00498 long lEnc[8] = {0,0,0,0,0,0,0,0};
00499 long lEncS[8] = {0,0,0,0,0,0,0,0};
00500 int iStatus[8] = {0,0,0,0,0,0,0,0};
00501 int iMotor_Nr = 0;
00502 static float sfLastPos[8] = {0,0,0,0,0,0,0,0};
00503
00504 sensor_msgs::JointState state;
00505
00506
00507 state.name.resize(8);
00508 state.position.resize(8);
00509 state.velocity.resize(8);
00510
00511
00512
00513
00514
00515
00516 for(int i = 0; i<8; i++)
00517 {
00518 m_SerRelayBoard->getMotorEnc(i,&lEnc[i]);
00519 m_SerRelayBoard->getMotorEncS(i,&lEncS[i]);
00520 m_SerRelayBoard->getMotorState(i,&iStatus[i]);
00521
00522 state.position[i] = (float)lEnc[i] - sfLastPos[i];
00523 sfLastPos[i] = (float)lEnc[i];
00524
00525 state.velocity[i] = m_Drives[i].iSign * m_Drives[i].convIncrPerPeriodToRadS((float)lEncS[i]);
00526 }
00527 topicPub_drives.publish(state);
00528 }
00529
00530 void neo_relayboardV2_node::getNewVelocitiesFomTopic(const trajectory_msgs::JointTrajectory jt)
00531 {
00532 if(!m_iRelayBoard_available) return;
00533 if(m_iactive_motors == 0) return;
00534 double dvelocity = 0.0;
00535 trajectory_msgs::JointTrajectoryPoint point = jt.points[0];
00536
00537
00538
00539
00540
00541
00542
00543
00544
00545
00546 for(int i=0; i<m_imotor_count; i++)
00547 {
00548
00549
00550
00551 dvelocity = m_Drives[i].iSign * m_Drives[i].convRadSToIncrPerPeriod(point.velocities[i]);
00552
00553
00554
00555
00556
00557
00558 m_SerRelayBoard->setMotorDesiredEncS(i, (long)dvelocity);
00559 }
00560
00561 }
00562
00563
00564 void neo_relayboardV2_node::PublishUSBoardData()
00565 {
00566 if(!m_iRelayBoard_available || !m_ihasUSBoard) return;
00567 int usSensors1[8];
00568 int usSensors2[8];
00569 int usAnalog[4];
00570 neo_msgs::USBoard usBoard;
00571 m_SerRelayBoard->getUSBoardData1To8(usSensors1);
00572 for(int i=0; i<8; i++) usBoard.sensor[i] = usSensors1[i];
00573 m_SerRelayBoard->getUSBoardData9To16(usSensors2);
00574 for(int i=0; i<8; i++) usBoard.sensor[i+8] = usSensors2[i];
00575 m_SerRelayBoard->getUSBoardAnalogIn(usAnalog);
00576 for(int i=0; i<4; i++) usBoard.analog[i] = usAnalog[i];
00577 topicPub_usBoard.publish(usBoard);
00578
00579
00580 std_msgs::Header USRange1Header;
00581 std_msgs::Header USRange2Header;
00582 std_msgs::Header USRange3Header;
00583 std_msgs::Header USRange4Header;
00584 std_msgs::Header USRange5Header;
00585 std_msgs::Header USRange6Header;
00586 std_msgs::Header USRange7Header;
00587 std_msgs::Header USRange8Header;
00588 std_msgs::Header USRange9Header;
00589 std_msgs::Header USRange10Header;
00590 std_msgs::Header USRange11Header;
00591 std_msgs::Header USRange12Header;
00592 std_msgs::Header USRange13Header;
00593 std_msgs::Header USRange14Header;
00594 std_msgs::Header USRange15Header;
00595 std_msgs::Header USRange16Header;
00596 sensor_msgs::Range USRange1Msg;
00597 sensor_msgs::Range USRange2Msg;
00598 sensor_msgs::Range USRange3Msg;
00599 sensor_msgs::Range USRange4Msg;
00600 sensor_msgs::Range USRange5Msg;
00601 sensor_msgs::Range USRange6Msg;
00602 sensor_msgs::Range USRange7Msg;
00603 sensor_msgs::Range USRange8Msg;
00604 sensor_msgs::Range USRange9Msg;
00605 sensor_msgs::Range USRange10Msg;
00606 sensor_msgs::Range USRange11Msg;
00607 sensor_msgs::Range USRange12Msg;
00608 sensor_msgs::Range USRange13Msg;
00609 sensor_msgs::Range USRange14Msg;
00610 sensor_msgs::Range USRange15Msg;
00611 sensor_msgs::Range USRange16Msg;
00612
00613
00614
00615
00616 USRange1Header.seq = 1;
00617 USRange1Header.stamp = ros::Time::now();
00618 USRange1Header.frame_id = "usrangesensor1";
00619
00620 USRange1Msg.header = USRange1Header;
00621 USRange1Msg.radiation_type = 0;
00622 USRange1Msg.field_of_view = 1.05;
00623 USRange1Msg.min_range = 0.1;
00624 USRange1Msg.max_range = 1.2;
00625 USRange1Msg.range = ((float)usBoard.sensor[0]/100);
00626
00627
00628 topicPub_USRangeSensor1.publish(USRange1Msg);
00629
00630
00631
00632
00633
00634 USRange2Header.seq = 1;
00635 USRange2Header.stamp = ros::Time::now();
00636 USRange2Header.frame_id = "usrangesensor2";
00637
00638 USRange2Msg.header = USRange2Header;
00639 USRange2Msg.radiation_type = 0;
00640 USRange2Msg.field_of_view = 1.05;
00641 USRange2Msg.min_range = 0.1;
00642 USRange2Msg.max_range = 1.2;
00643 USRange2Msg.range = ((float)usBoard.sensor[1]/100);
00644
00645
00646 topicPub_USRangeSensor2.publish(USRange2Msg);
00647
00648
00649
00650
00651
00652 USRange3Header.seq = 1;
00653 USRange3Header.stamp = ros::Time::now();
00654 USRange3Header.frame_id = "usrangesensor3";
00655
00656 USRange3Msg.header = USRange3Header;
00657 USRange3Msg.radiation_type = 0;
00658 USRange3Msg.field_of_view = 1.05;
00659 USRange3Msg.min_range = 0.1;
00660 USRange3Msg.max_range = 1.2;
00661 USRange3Msg.range = ((float)usBoard.sensor[2]/100);
00662
00663
00664 topicPub_USRangeSensor3.publish(USRange3Msg);
00665
00666
00667
00668
00669
00670 USRange4Header.seq = 1;
00671 USRange4Header.stamp = ros::Time::now();
00672 USRange4Header.frame_id = "usrangesensor4";
00673
00674 USRange4Msg.header = USRange4Header;
00675 USRange4Msg.radiation_type = 0;
00676 USRange4Msg.field_of_view = 1.05;
00677 USRange4Msg.min_range = 0.1;
00678 USRange4Msg.max_range = 1.2;
00679 USRange4Msg.range = ((float)usBoard.sensor[3]/100);
00680
00681
00682 topicPub_USRangeSensor4.publish(USRange4Msg);
00683
00684
00685
00686
00687
00688 USRange5Header.seq = 1;
00689 USRange5Header.stamp = ros::Time::now();
00690 USRange5Header.frame_id = "usrangesensor5";
00691
00692 USRange5Msg.header = USRange5Header;
00693 USRange5Msg.radiation_type = 0;
00694 USRange5Msg.field_of_view = 1.05;
00695 USRange5Msg.min_range = 0.1;
00696 USRange5Msg.max_range = 1.2;
00697 USRange5Msg.range = ((float)usBoard.sensor[4]/100);
00698
00699
00700 topicPub_USRangeSensor5.publish(USRange5Msg);
00701
00702
00703
00704
00705
00706 USRange6Header.seq = 1;
00707 USRange6Header.stamp = ros::Time::now();
00708 USRange6Header.frame_id = "usrangesensor6";
00709
00710 USRange6Msg.header = USRange6Header;
00711 USRange6Msg.radiation_type = 0;
00712 USRange6Msg.field_of_view = 1.05;
00713 USRange6Msg.min_range = 0.1;
00714 USRange6Msg.max_range = 1.2;
00715 USRange6Msg.range = ((float)usBoard.sensor[5]/100);
00716
00717
00718 topicPub_USRangeSensor6.publish(USRange6Msg);
00719
00720
00721
00722
00723
00724 USRange7Header.seq = 1;
00725 USRange7Header.stamp = ros::Time::now();
00726 USRange7Header.frame_id = "usrangesensor7";
00727
00728 USRange7Msg.header = USRange7Header;
00729 USRange7Msg.radiation_type = 0;
00730 USRange7Msg.field_of_view = 1.05;
00731 USRange7Msg.min_range = 0.1;
00732 USRange7Msg.max_range = 1.2;
00733 USRange7Msg.range = ((float)usBoard.sensor[6]/100);
00734
00735
00736 topicPub_USRangeSensor7.publish(USRange7Msg);
00737
00738
00739
00740
00741
00742 USRange8Header.seq = 1;
00743 USRange8Header.stamp = ros::Time::now();
00744 USRange8Header.frame_id = "usrangesensor8";
00745
00746 USRange8Msg.header = USRange8Header;
00747 USRange8Msg.radiation_type = 0;
00748 USRange8Msg.field_of_view = 1.05;
00749 USRange8Msg.min_range = 0.1;
00750 USRange8Msg.max_range = 1.2;
00751 USRange8Msg.range = ((float)usBoard.sensor[7]/100);
00752
00753
00754 topicPub_USRangeSensor8.publish(USRange8Msg);
00755
00756
00757
00758
00759
00760 USRange9Header.seq = 1;
00761 USRange9Header.stamp = ros::Time::now();
00762 USRange9Header.frame_id = "usrangesensor9";
00763
00764 USRange9Msg.header = USRange9Header;
00765 USRange9Msg.radiation_type = 0;
00766 USRange9Msg.field_of_view = 1.05;
00767 USRange9Msg.min_range = 0.1;
00768 USRange9Msg.max_range = 1.2;
00769 USRange9Msg.range = ((float)usBoard.sensor[8]/100);
00770
00771
00772 topicPub_USRangeSensor9.publish(USRange9Msg);
00773
00774
00775
00776
00777
00778 USRange10Header.seq = 1;
00779 USRange10Header.stamp = ros::Time::now();
00780 USRange10Header.frame_id = "usrangesensor10";
00781
00782 USRange10Msg.header = USRange10Header;
00783 USRange10Msg.radiation_type = 0;
00784 USRange10Msg.field_of_view = 1.05;
00785 USRange10Msg.min_range = 0.1;
00786 USRange10Msg.max_range = 1.2;
00787 USRange10Msg.range = ((float)usBoard.sensor[9]/100);
00788
00789
00790 topicPub_USRangeSensor10.publish(USRange10Msg);
00791
00792
00793
00794
00795
00796 USRange11Header.seq = 1;
00797 USRange11Header.stamp = ros::Time::now();
00798 USRange11Header.frame_id = "usrangesensor11";
00799
00800 USRange11Msg.header = USRange11Header;
00801 USRange11Msg.radiation_type = 0;
00802 USRange11Msg.field_of_view = 1.05;
00803 USRange11Msg.min_range = 0.1;
00804 USRange11Msg.max_range = 1.2;
00805 USRange11Msg.range = ((float)usBoard.sensor[10]/100);
00806
00807
00808 topicPub_USRangeSensor11.publish(USRange11Msg);
00809
00810
00811
00812
00813
00814 USRange12Header.seq = 1;
00815 USRange12Header.stamp = ros::Time::now();
00816 USRange12Header.frame_id = "usrangesensor12";
00817
00818 USRange12Msg.header = USRange12Header;
00819 USRange12Msg.radiation_type = 0;
00820 USRange12Msg.field_of_view = 1.05;
00821 USRange12Msg.min_range = 0.1;
00822 USRange12Msg.max_range = 1.2;
00823 USRange12Msg.range = ((float)usBoard.sensor[11]/100);
00824
00825
00826 topicPub_USRangeSensor12.publish(USRange12Msg);
00827
00828
00829
00830
00831
00832 USRange13Header.seq = 1;
00833 USRange13Header.stamp = ros::Time::now();
00834 USRange13Header.frame_id = "usrangesensor13";
00835
00836 USRange13Msg.header = USRange13Header;
00837 USRange13Msg.radiation_type = 0;
00838 USRange13Msg.field_of_view = 1.05;
00839 USRange13Msg.min_range = 0.1;
00840 USRange13Msg.max_range = 1.2;
00841 USRange13Msg.range = ((float)usBoard.sensor[12]/100);
00842
00843
00844 topicPub_USRangeSensor13.publish(USRange13Msg);
00845
00846
00847
00848
00849
00850 USRange14Header.seq = 1;
00851 USRange14Header.stamp = ros::Time::now();
00852 USRange14Header.frame_id = "usrangesensor14";
00853
00854 USRange14Msg.header = USRange14Header;
00855 USRange14Msg.radiation_type = 0;
00856 USRange14Msg.field_of_view = 1.05;
00857 USRange14Msg.min_range = 0.1;
00858 USRange14Msg.max_range = 1.2;
00859 USRange14Msg.range = ((float)usBoard.sensor[13]/100);
00860
00861
00862 topicPub_USRangeSensor14.publish(USRange14Msg);
00863
00864
00865
00866
00867
00868 USRange15Header.seq = 1;
00869 USRange15Header.stamp = ros::Time::now();
00870 USRange15Header.frame_id = "usrangesensor15";
00871
00872 USRange15Msg.header = USRange15Header;
00873 USRange15Msg.radiation_type = 0;
00874 USRange15Msg.field_of_view = 1.05;
00875 USRange15Msg.min_range = 0.1;
00876 USRange15Msg.max_range = 1.2;
00877 USRange15Msg.range = ((float)usBoard.sensor[14]/100);
00878
00879
00880 topicPub_USRangeSensor15.publish(USRange15Msg);
00881
00882
00883
00884
00885
00886 USRange16Header.seq = 1;
00887 USRange16Header.stamp = ros::Time::now();
00888 USRange16Header.frame_id = "usrangesensor16";
00889
00890 USRange16Msg.header = USRange16Header;
00891 USRange16Msg.radiation_type = 0;
00892 USRange16Msg.field_of_view = 1.05;
00893 USRange16Msg.min_range = 0.1;
00894 USRange16Msg.max_range = 1.2;
00895 USRange16Msg.range = ((float)usBoard.sensor[15]/100);
00896
00897
00898 topicPub_USRangeSensor16.publish(USRange16Msg);
00899
00900 }
00901 void neo_relayboardV2_node::startUSBoard(const std_msgs::Int16& configuration)
00902 {
00903 if(!m_iRelayBoard_available || !m_ihasUSBoard) return;
00904 m_SerRelayBoard->startUSBoard(configuration.data);
00905 }
00906
00907 void neo_relayboardV2_node::stopUSBoard(const std_msgs::Empty& empty)
00908 {
00909 if(!m_iRelayBoard_available || !m_ihasUSBoard) return;
00910 m_SerRelayBoard->stopUSBoard();
00911 }
00912
00913
00914 void neo_relayboardV2_node::getIOBoardDigOut(const neo_msgs::IOOut& setOut)
00915 {
00916 if(!m_iRelayBoard_available || !m_ihasIOBoard) return;
00917 m_SerRelayBoard->setIOBoardDigOut(setOut.channel, setOut.active);
00918 }
00919
00920 void neo_relayboardV2_node::PublishIOBoardDigIn()
00921 {
00922 if(!m_iRelayBoard_available || !m_ihasIOBoard) return;
00923 int data = 0;
00924 std_msgs::Int16 i;
00925 m_SerRelayBoard->getIOBoardDigIn(&data);
00926 i.data = data;
00927 topicPub_ioDigIn.publish(i);
00928
00929 }
00930
00931 void neo_relayboardV2_node::PublishIOBoardDigOut()
00932 {
00933 if(!m_iRelayBoard_available || !m_ihasIOBoard) return;
00934 int data = 0;
00935 std_msgs::Int16 i;
00936 m_SerRelayBoard->getIOBoardDigOut(&data);
00937 i.data = data;
00938 topicPub_ioDigOut.publish(i);
00939 }
00940
00941 void neo_relayboardV2_node::PublishIOBoardAnalogIn()
00942 {
00943 if(!m_iRelayBoard_available || !m_ihasIOBoard) return;
00944 int *pointer = 0;
00945 int analogin[8];
00946 neo_msgs::IOAnalogIn in;
00947 pointer = analogin;
00948 m_SerRelayBoard->getIOBoardAnalogIn(pointer);
00949 for(int i=0;i <8; i++) in.input[i] = pointer[i];
00950 topicPub_analogIn.publish(in);
00951 }
00952