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_relayboardV2_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 }
00248 topicPub_keypad = n.advertise<neo_msgs::Keypad>("/RelayBoardV2/Keypad",1);
00249
00250
00251 if(m_ilog == 1)
00252 {
00253 ROS_INFO("Log enabled");
00254 m_SerRelayBoard->enable_logging();
00255 }
00256 else
00257 {
00258 ROS_INFO("Log disabled");
00259 m_SerRelayBoard->disable_logging();
00260 }
00261
00262 return 0;
00263 }
00264
00265
00266 void neo_relayboardV2_node::HandleCommunication()
00267 {
00268 if(!m_iRelayBoard_available) return;
00269 int send_return = 0;
00270 int receive_return = 101;
00271 static int last_error = 0;
00272 while(receive_return == 101)
00273 {
00274
00275 send_return = m_SerRelayBoard->sendDataToRelayBoard();
00276 receive_return = m_SerRelayBoard->evalRxBuffer();
00277
00278 if(receive_return == last_error)
00279 {
00280
00281 }
00282 else if(receive_return == 0)
00283 {
00284 ROS_INFO("communicating with RelayBoard");
00285 }
00286 else if(receive_return == 101)
00287 {
00288 ROS_ERROR("No answer from RelayBoard ... ");
00289 }
00290 else if(receive_return == 97)
00291 {
00292 ROS_ERROR("Wrong Checksum");
00293 }
00294 else if(receive_return == 99)
00295 {
00296 ROS_ERROR("No valid Message header found");
00297 }
00298 else
00299 {
00300
00301 ROS_ERROR("Unknown error");
00302 }
00303 last_error = receive_return;
00304
00305 }
00306 }
00307
00308 double neo_relayboardV2_node::getRequestRate()
00309 {
00310 return m_dRequestRate;
00311 }
00312
00313 void neo_relayboardV2_node::PublishRelayBoardState()
00314 {
00315 if(!m_iRelayBoard_available) return;
00316 int value = 0;
00317 std_msgs::Int16 state;
00318 m_SerRelayBoard->getRelayBoardState(&value);
00319 state.data = value;
00320 topicPub_RelayBoardState.publish(state);
00321
00322
00323
00324 if((value & 0x400) != 0)
00325 {
00326 ROS_INFO("-----------SHUTDOWN Signal from RelayBoardV2----------");
00327 ros::shutdown();
00328 usleep(2000);
00329 system("dbus-send --system --print-reply --dest=org.freedesktop.login1 /org/freedesktop/login1 \"org.freedesktop.login1.Manager.PowerOff\" boolean:true");
00330 }
00331 }
00332 void neo_relayboardV2_node::PublishTemperature()
00333 {
00334 if(!m_iRelayBoard_available) return;
00335 int temperature = 0;
00336 std_msgs::Int16 temp;
00337 m_SerRelayBoard->getTemperature(&temperature);
00338 temp.data = temperature;
00339 topicPub_temperatur.publish(temp);
00340 }
00341 void neo_relayboardV2_node::PublishBattVoltage()
00342 {
00343 if(!m_iRelayBoard_available) return;
00344 int battvolt = 0;
00345 std_msgs::Int16 bat;
00346 m_SerRelayBoard->getBattVoltage(&battvolt);
00347 bat.data = battvolt/1000;
00348 topicPub_batVoltage.publish(bat);
00349 }
00350 void neo_relayboardV2_node::PublishChargingCurrent()
00351 {
00352 if(!m_iRelayBoard_available) return;
00353 int value = 0;
00354 std_msgs::Int16 current;
00355 m_SerRelayBoard->getChargingCurrent(&value);
00356 current.data = value;
00357 topicPub_chargeCurrent.publish(current);
00358 }
00359 void neo_relayboardV2_node::PublishChargingState()
00360 {
00361 if(!m_iRelayBoard_available) return;
00362 int value = 0;
00363 std_msgs::Int16 state;
00364 m_SerRelayBoard->getChargingState(&value);
00365 state.data = value;
00366 topicPub_chargeState.publish(state);
00367 }
00368 void neo_relayboardV2_node::PublishKeyPad()
00369 {
00370 if(!m_iRelayBoard_available) return;
00371 int value = 0;
00372 neo_msgs::Keypad pad;
00373 m_SerRelayBoard->getKeyPad(&value);
00374 pad.button[0] = !(value & 1);
00375 pad.button[1] = !(value & 2);
00376 pad.button[2] = !(value & 4);
00377 pad.button[3] = !(value & 8);
00378 pad.button[4] = !(value & 16);
00379 topicPub_keypad.publish(pad);
00380 }
00381 void neo_relayboardV2_node::PublishEmergencyStopStates()
00382 {
00383 if(!m_iRelayBoard_available) return;
00384 bool EM_signal;
00385 ros::Duration duration_since_EM_confirmed;
00386 neo_msgs::EmergencyStopState EM_msg;
00387
00388
00389 EM_msg.emergency_button_stop = m_SerRelayBoard->isEMStop();
00390 EM_msg.scanner_stop = m_SerRelayBoard->isScannerStop();
00391
00392
00393 EM_signal = (EM_msg.emergency_button_stop || EM_msg.scanner_stop);
00394
00395 switch (m_iEM_stop_state)
00396 {
00397 case ST_EM_FREE:
00398 {
00399 if (EM_signal == true)
00400 {
00401 ROS_ERROR("Emergency stop was issued");
00402 m_iEM_stop_state = EM_msg.EMSTOP;
00403 }
00404 break;
00405 }
00406 case ST_EM_ACTIVE:
00407 {
00408 if (EM_signal == false)
00409 {
00410 ROS_INFO("Emergency stop was confirmed");
00411 m_iEM_stop_state = EM_msg.EMCONFIRMED;
00412 m_time_of_EM_confirmed = ros::Time::now();
00413 }
00414 break;
00415 }
00416 case ST_EM_CONFIRMED:
00417 {
00418 if (EM_signal == true)
00419 {
00420 ROS_ERROR("Emergency stop was issued");
00421 m_iEM_stop_state = EM_msg.EMSTOP;
00422 }
00423 else
00424 {
00425 duration_since_EM_confirmed = ros::Time::now() - m_time_of_EM_confirmed;
00426 if( duration_since_EM_confirmed.toSec() > m_duration_for_EM_free.toSec() )
00427 {
00428 ROS_INFO("Emergency stop released");
00429 m_iEM_stop_state = EM_msg.EMFREE;
00430 }
00431 }
00432 break;
00433 }
00434 };
00435
00436 EM_msg.emergency_state = m_iEM_stop_state;
00437
00438
00439 topicPub_isEmergencyStop.publish(EM_msg);
00440 }
00441 void neo_relayboardV2_node::PublishRelayBoardDigOut()
00442 {
00443 if(!m_iRelayBoard_available) return;
00444 int data = 0;
00445 std_msgs::Int16 i;
00446 m_SerRelayBoard->getRelayBoardDigOut(&data);
00447 i.data = data;
00448 topicPub_SendRelayStates.publish(i);
00449
00450 }
00451
00452 void neo_relayboardV2_node::getNewLCDMsg(const neo_msgs::LCDOutput& msg)
00453 {
00454 if(!m_iRelayBoard_available) return;
00455 m_SerRelayBoard->writeLCD(msg.msg_line);
00456
00457 }
00458 void neo_relayboardV2_node::getRelayBoardDigOut(const neo_msgs::IOOut& setOut)
00459 {
00460 if(!m_iRelayBoard_available) return;
00461 m_SerRelayBoard->setRelayBoardDigOut(setOut.channel, setOut.active);
00462 }
00463 void neo_relayboardV2_node::startCharging(const std_msgs::Empty& empty)
00464 {
00465 if(!m_iRelayBoard_available) return;
00466 m_SerRelayBoard->startCharging();
00467 }
00468
00469 void neo_relayboardV2_node::stopCharging(const std_msgs::Empty& empty)
00470 {
00471 if(!m_iRelayBoard_available) return;
00472 m_SerRelayBoard->stopCharging();
00473 }
00474
00475
00476 void neo_relayboardV2_node::PublishJointStates()
00477 {
00478 ROS_INFO("PUB_JOINT_STATES");
00479 if(!m_iRelayBoard_available) return;
00480 long lEnc[8] = {0,0,0,0,0,0,0,0};
00481 long lEncS[8] = {0,0,0,0,0,0,0,0};
00482 int iStatus[8] = {0,0,0,0,0,0,0,0};
00483 int iMotor_Nr = 0;
00484 static float sfLastPos[8] = {0,0,0,0,0,0,0,0};
00485
00486 sensor_msgs::JointState state;
00487
00488
00489 state.name.resize(8);
00490 state.position.resize(8);
00491 state.velocity.resize(8);
00492
00493
00494
00495
00496
00497
00498 for(int i = 0; i<8; i++)
00499 {
00500 m_SerRelayBoard->getMotorEnc(i,&lEnc[i]);
00501 m_SerRelayBoard->getMotorEncS(i,&lEncS[i]);
00502 m_SerRelayBoard->getMotorState(i,&iStatus[i]);
00503
00504 state.position[i] = (float)lEnc[i] - sfLastPos[i];
00505 sfLastPos[i] = (float)lEnc[i];
00506
00507 state.velocity[i] = m_Drives[i].iSign * m_Drives[i].convIncrPerPeriodToRadS((float)lEncS[i]);
00508 }
00509 topicPub_drives.publish(state);
00510 ROS_INFO("ENDE PUB_JOINT_STATES");
00511 }
00512
00513 void neo_relayboardV2_node::getNewVelocitiesFomTopic(const trajectory_msgs::JointTrajectory jt)
00514 {
00515 if(!m_iRelayBoard_available) return;
00516 ROS_INFO("SUB NEW_VEL");
00517 double dvelocity = 0.0;
00518 trajectory_msgs::JointTrajectoryPoint point = jt.points[0];
00519
00520
00521
00522
00523
00524
00525
00526
00527
00528 ROS_INFO("1");
00529 for(int i=0; i<m_imotor_count; i++)
00530 {
00531 ROS_INFO("2");
00532
00533
00534 dvelocity = m_Drives[i].iSign * m_Drives[i].convRadSToIncrPerPeriod(point.velocities[i]);
00535
00536
00537
00538
00539
00540
00541 m_SerRelayBoard->setMotorDesiredEncS(i, (long)dvelocity);
00542 }
00543
00544 ROS_INFO("END_SUB_NEW_VEL");
00545 }
00546
00547
00548 void neo_relayboardV2_node::PublishUSBoardData()
00549 {
00550 if(!m_iRelayBoard_available || !m_ihasUSBoard) return;
00551 int usSensors[8];
00552 int usAnalog[4];
00553 neo_msgs::USBoard usBoard;
00554 m_SerRelayBoard->getUSBoardData1To8(usSensors);
00555 for(int i=0; i<8; i++) usBoard.sensor[i] = usSensors[i];
00556 m_SerRelayBoard->getUSBoardData9To16(usSensors);
00557 for(int i=0; i<8; i++) usBoard.sensor[i+8] = usSensors[i];
00558 m_SerRelayBoard->getUSBoardAnalogIn(usAnalog);
00559 for(int i=0; i<4; i++) usBoard.analog[i] = usAnalog[i];
00560 topicPub_usBoard.publish(usBoard);
00561 }
00562 void neo_relayboardV2_node::startUSBoard(const std_msgs::Int16& configuration)
00563 {
00564 if(!m_iRelayBoard_available || !m_ihasUSBoard) return;
00565 m_SerRelayBoard->startUSBoard(configuration.data);
00566 }
00567
00568 void neo_relayboardV2_node::stopUSBoard(const std_msgs::Empty& empty)
00569 {
00570 if(!m_iRelayBoard_available || !m_ihasUSBoard) return;
00571 m_SerRelayBoard->stopUSBoard();
00572 }
00573
00574
00575 void neo_relayboardV2_node::getIOBoardDigOut(const neo_msgs::IOOut& setOut)
00576 {
00577 if(!m_iRelayBoard_available || !m_ihasIOBoard) return;
00578 m_SerRelayBoard->setIOBoardDigOut(setOut.channel, setOut.active);
00579 }
00580
00581 void neo_relayboardV2_node::PublishIOBoardDigIn()
00582 {
00583 if(!m_iRelayBoard_available || !m_ihasIOBoard) return;
00584 int data = 0;
00585 std_msgs::Int16 i;
00586 m_SerRelayBoard->getIOBoardDigIn(&data);
00587 i.data = data;
00588 topicPub_ioDigIn.publish(i);
00589
00590 }
00591
00592 void neo_relayboardV2_node::PublishIOBoardDigOut()
00593 {
00594 if(!m_iRelayBoard_available || !m_ihasIOBoard) return;
00595 int data = 0;
00596 std_msgs::Int16 i;
00597 m_SerRelayBoard->getIOBoardDigOut(&data);
00598 i.data = data;
00599 topicPub_ioDigOut.publish(i);
00600 }
00601
00602 void neo_relayboardV2_node::PublishIOBoardAnalogIn()
00603 {
00604 if(!m_iRelayBoard_available || !m_ihasIOBoard) return;
00605 int *pointer = 0;
00606 int analogin[8];
00607 neo_msgs::IOAnalogIn in;
00608 pointer = analogin;
00609 m_SerRelayBoard->getIOBoardAnalogIn(pointer);
00610 for(int i=0;i <8; i++) in.input[i] = pointer[i];
00611 topicPub_analogIn.publish(in);
00612 }
00613