neo_relayboard_v2_lib.cpp
Go to the documentation of this file.
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 #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 //----------------------------------------GET PARAMS-----------------------------------------------------------
00045         //Relayboard Config Parameter
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         //Node Parameter
00056         n.param("message_timeout", m_dRelayBoard_timeout, 2.0);
00057         n.param("requestRate", m_dRequestRate, 25.0);
00058         
00059         //Logging
00060         n.getParam("log", m_ilog);
00061 
00062         //Motor Parameter
00063         //Drive2
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         //Drive3
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         //Drive4
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         //Drive5
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         //Drive6
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         //Drive7
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         //Drive8
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         //Drive9
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 //----------------------------------------END GET PARAMS-------------------------------------------------------
00176 //----------------------------------------OPEN COMPORT---------------------------------------------------------
00177         //Check which motors are active
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         //Check if homing is active
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         //Check external hardware
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                 //eval Error
00210                 return(1);
00211         }
00212 //----------------------------------------END OPEN COMPORT-----------------------------------------------------
00213 //----------------------------------------Init Publisher/Subscriber--------------------------------------------
00214         //topics and subscriber which will allways get published
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         //logging
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 //----------------------------------------END Init Publisher/Subscriber----------------------------------------
00279         return 0;
00280 }
00281 
00282 //--------------------------RelayBoardV2-----------------------------------------------------------------------
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                 //ROS_INFO("Sending");
00292                 send_return = m_SerRelayBoard->sendDataToRelayBoard();
00293                 receive_return = m_SerRelayBoard->evalRxBuffer();
00294 //ROS_INFO("Ret: %i", ret);
00295                 if(receive_return == last_error)
00296                 {
00297                         //Do not show message again
00298                 }
00299                 else if(receive_return == 0) //ok
00300                 {
00301                         ROS_INFO("communicating with RelayBoard");
00302                 }               
00303                 else if(receive_return == 101) //No Answer => resend
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                         //Unknown error
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 //-------Publisher------
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         //handle RelayBoardV2 shutdown
00340         //RelayBoardV2 will power off in < 30 sec
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); //Info Taste      
00392         pad.button[1] = !(value & 2); //Home Taste      
00393         pad.button[2] = !(value & 4); //Start Taste     
00394         pad.button[3] = !(value & 8); //Stop Taste
00395         pad.button[4] = !(value & 16); //Bremsen lösen Taste   
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         // assign input (laser, button) specific EM state
00406         EM_msg.emergency_button_stop = m_SerRelayBoard->isEMStop();
00407         EM_msg.scanner_stop = m_SerRelayBoard->isScannerStop();
00408 
00409         // determine current EMStopState
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 //-------Subscriber------
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 //----------------------END RelayBoardV2-----------------------------------------------------------------------
00492 //--------------------------Motor Ctrl-------------------------------------------------------------------------
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         //Publish Data for all possible Motors
00507         state.name.resize(8);
00508         state.position.resize(8);
00509         state.velocity.resize(8);
00510 
00511         //TODO Joint Names einfügen
00512         //for(int i = 0; i<anz_drives; i++)  state.name[i] = joint_names[i];
00513 
00514         //Motor Data from MSG Handler for each Motor
00515         //Enc (4 Byte), EncS (4 Byte) and Status (2 Byte) for each Motor 
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                 //m_SerRelayBoard->setMotorDesiredEncS(i, 20000);
00522                 state.position[i] = (float)lEnc[i] - sfLastPos[i];
00523                 sfLastPos[i] = (float)lEnc[i];
00524                 //ROS_INFO("Motor %d: Enc: %f",i,(float)lEnc[i]);
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         //Check if Data for all Motors are avaliable
00537         //ROS_INFO("Data in Topic: %d ; Motor Nr: %d",sizeof(point.velocities),m_imotor_count);
00538         //if(sizeof(point.velocities) != m_imotor_count)
00539         //{
00540         //      ROS_ERROR("TOO LESS DATA FOR ALL MOTORS IN TOPIC!");
00541         //}
00542         //else
00543         //{
00544                 //set new velocities
00545                 //ROS_INFO("1");
00546                 for(int i=0; i<m_imotor_count; i++)
00547                 {
00548                         //ROS_INFO("2");
00549                         //convert velocities [rad/s] -> [incr/period]
00550                         //ROS_INFO("Motor: %d ; Vel: %d [rad]; Vel: %d [incr/period]",i,point.velocities[i],dvelocity);
00551                         dvelocity = m_Drives[i].iSign * m_Drives[i].convRadSToIncrPerPeriod(point.velocities[i]);
00552                         //check if velocity is too high -> limit velocity
00553                         /*if(MathSup::limit((int)&dvelocity, (int)Drives[i].getVelMax()) != 0)
00554                         {
00555                                 ROS_ERROR("Velocity for motor %d limited",i+2);
00556                         }*/
00557                         //send Data to MSG Handler
00558                         m_SerRelayBoard->setMotorDesiredEncS(i, (long)dvelocity);
00559                 }
00560         //}
00561 }
00562 //----------------------END Motor Ctrl-------------------------------------------------------------------------
00563 //-----------------------------USBoard-------------------------------------------------------------------------
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         //create a sensor_msgs::Range for each range sensor
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         //-------------------------------------------SENSOR1--------------------------------------------------------
00614         //create USRanger1Msg
00615         //fill in header
00616         USRange1Header.seq = 1;                                 //uint32
00617         USRange1Header.stamp = ros::Time::now();                //time
00618         USRange1Header.frame_id = "usrangesensor1";             //string
00619 
00620         USRange1Msg.header = USRange1Header;
00621         USRange1Msg.radiation_type = 0;                         //uint8   => Enum ULTRASOUND=0; INFRARED=1
00622         USRange1Msg.field_of_view = 1.05;                       //float32 [rad]
00623         USRange1Msg.min_range = 0.1;                            //float32 [m]
00624         USRange1Msg.max_range = 1.2;                            //float32 [m]
00625         USRange1Msg.range = ((float)usBoard.sensor[0]/100);     //float32 [cm] => [m]
00626 
00627         //publish data for first USrange sensor
00628         topicPub_USRangeSensor1.publish(USRange1Msg);
00629         //------------------------------------------------------------------------------------------------------------
00630         
00631         //-------------------------------------------SENSOR2--------------------------------------------------------
00632         //create USRanger2Msg
00633         //fill in header
00634         USRange2Header.seq = 1;                                 //uint32
00635         USRange2Header.stamp = ros::Time::now();                //time
00636         USRange2Header.frame_id = "usrangesensor2";             //string
00637 
00638         USRange2Msg.header = USRange2Header;
00639         USRange2Msg.radiation_type = 0;                         //uint8   => Enum ULTRASOUND=0; INFRARED=1
00640         USRange2Msg.field_of_view = 1.05;                       //float32 [rad]
00641         USRange2Msg.min_range = 0.1;                            //float32 [m]
00642         USRange2Msg.max_range = 1.2;                            //float32 [m]
00643         USRange2Msg.range = ((float)usBoard.sensor[1]/100);     //float32 [cm] => [m]
00644 
00645         //publish data for first USrange sensor
00646         topicPub_USRangeSensor2.publish(USRange2Msg);
00647         //------------------------------------------------------------------------------------------------------------
00648 
00649         //-------------------------------------------SENSOR3--------------------------------------------------------
00650         //create USRanger3Msg
00651         //fill in header
00652         USRange3Header.seq = 1;                                 //uint32
00653         USRange3Header.stamp = ros::Time::now();                //time
00654         USRange3Header.frame_id = "usrangesensor3";             //string
00655 
00656         USRange3Msg.header = USRange3Header;
00657         USRange3Msg.radiation_type = 0;                         //uint8   => Enum ULTRASOUND=0; INFRARED=1
00658         USRange3Msg.field_of_view = 1.05;                       //float32 [rad]
00659         USRange3Msg.min_range = 0.1;                            //float32 [m]
00660         USRange3Msg.max_range = 1.2;                            //float32 [m]
00661         USRange3Msg.range = ((float)usBoard.sensor[2]/100);     //float32 [cm] => [m]
00662 
00663         //publish data for first USrange sensor
00664         topicPub_USRangeSensor3.publish(USRange3Msg);
00665         //------------------------------------------------------------------------------------------------------------
00666 
00667         //-------------------------------------------SENSOR4--------------------------------------------------------
00668         //create USRanger4Msg
00669         //fill in header
00670         USRange4Header.seq = 1;                                 //uint32
00671         USRange4Header.stamp = ros::Time::now();                //time
00672         USRange4Header.frame_id = "usrangesensor4";             //string
00673 
00674         USRange4Msg.header = USRange4Header;
00675         USRange4Msg.radiation_type = 0;                         //uint8   => Enum ULTRASOUND=0; INFRARED=1
00676         USRange4Msg.field_of_view = 1.05;                       //float32 [rad]
00677         USRange4Msg.min_range = 0.1;                            //float32 [m]
00678         USRange4Msg.max_range = 1.2;                            //float32 [m]
00679         USRange4Msg.range = ((float)usBoard.sensor[3]/100);     //float32 [cm] => [m]
00680 
00681         //publish data for first USrange sensor
00682         topicPub_USRangeSensor4.publish(USRange4Msg);
00683         //------------------------------------------------------------------------------------------------------------
00684 
00685         //-------------------------------------------SENSOR5--------------------------------------------------------
00686         //create USRanger5Msg
00687         //fill in header
00688         USRange5Header.seq = 1;                                 //uint32
00689         USRange5Header.stamp = ros::Time::now();                //time
00690         USRange5Header.frame_id = "usrangesensor5";             //string
00691 
00692         USRange5Msg.header = USRange5Header;
00693         USRange5Msg.radiation_type = 0;                         //uint8   => Enum ULTRASOUND=0; INFRARED=1
00694         USRange5Msg.field_of_view = 1.05;                       //float32 [rad]
00695         USRange5Msg.min_range = 0.1;                            //float32 [m]
00696         USRange5Msg.max_range = 1.2;                            //float32 [m]
00697         USRange5Msg.range = ((float)usBoard.sensor[4]/100);     //float32 [cm] => [m]
00698 
00699         //publish data for first USrange sensor
00700         topicPub_USRangeSensor5.publish(USRange5Msg);
00701         //------------------------------------------------------------------------------------------------------------
00702 
00703         //-------------------------------------------SENSOR6--------------------------------------------------------
00704         //create USRanger6Msg
00705         //fill in header
00706         USRange6Header.seq = 1;                                 //uint32
00707         USRange6Header.stamp = ros::Time::now();                //time
00708         USRange6Header.frame_id = "usrangesensor6";             //string
00709 
00710         USRange6Msg.header = USRange6Header;
00711         USRange6Msg.radiation_type = 0;                         //uint8   => Enum ULTRASOUND=0; INFRARED=1
00712         USRange6Msg.field_of_view = 1.05;                       //float32 [rad]
00713         USRange6Msg.min_range = 0.1;                            //float32 [m]
00714         USRange6Msg.max_range = 1.2;                            //float32 [m]
00715         USRange6Msg.range = ((float)usBoard.sensor[5]/100);     //float32 [cm] => [m]
00716 
00717         //publish data for first USrange sensor
00718         topicPub_USRangeSensor6.publish(USRange6Msg);
00719         //------------------------------------------------------------------------------------------------------------
00720         
00721         //-------------------------------------------SENSOR7--------------------------------------------------------
00722         //create USRanger4Msg
00723         //fill in header
00724         USRange7Header.seq = 1;                                 //uint32
00725         USRange7Header.stamp = ros::Time::now();                //time
00726         USRange7Header.frame_id = "usrangesensor7";             //string
00727 
00728         USRange7Msg.header = USRange7Header;
00729         USRange7Msg.radiation_type = 0;                         //uint8   => Enum ULTRASOUND=0; INFRARED=1
00730         USRange7Msg.field_of_view = 1.05;                       //float32 [rad]
00731         USRange7Msg.min_range = 0.1;                            //float32 [m]
00732         USRange7Msg.max_range = 1.2;                            //float32 [m]
00733         USRange7Msg.range = ((float)usBoard.sensor[6]/100);     //float32 [cm] => [m]
00734 
00735         //publish data for first USrange sensor
00736         topicPub_USRangeSensor7.publish(USRange7Msg);
00737         //------------------------------------------------------------------------------------------------------------
00738 
00739         //-------------------------------------------SENSOR8--------------------------------------------------------
00740         //create USRanger8Msg
00741         //fill in header
00742         USRange8Header.seq = 1;                                 //uint32
00743         USRange8Header.stamp = ros::Time::now();                //time
00744         USRange8Header.frame_id = "usrangesensor8";             //string
00745 
00746         USRange8Msg.header = USRange8Header;
00747         USRange8Msg.radiation_type = 0;                         //uint8   => Enum ULTRASOUND=0; INFRARED=1
00748         USRange8Msg.field_of_view = 1.05;                       //float32 [rad]
00749         USRange8Msg.min_range = 0.1;                            //float32 [m]
00750         USRange8Msg.max_range = 1.2;                            //float32 [m]
00751         USRange8Msg.range = ((float)usBoard.sensor[7]/100);     //float32 [cm] => [m]
00752 
00753         //publish data for first USrange sensor
00754         topicPub_USRangeSensor8.publish(USRange8Msg);
00755         //------------------------------------------------------------------------------------------------------------
00756 
00757         //-------------------------------------------SENSOR9--------------------------------------------------------
00758         //create USRanger4Msg
00759         //fill in header
00760         USRange9Header.seq = 1;                                 //uint32
00761         USRange9Header.stamp = ros::Time::now();                //time
00762         USRange9Header.frame_id = "usrangesensor9";             //string
00763 
00764         USRange9Msg.header = USRange9Header;
00765         USRange9Msg.radiation_type = 0;                         //uint8   => Enum ULTRASOUND=0; INFRARED=1
00766         USRange9Msg.field_of_view = 1.05;                       //float32 [rad]
00767         USRange9Msg.min_range = 0.1;                            //float32 [m]
00768         USRange9Msg.max_range = 1.2;                            //float32 [m]
00769         USRange9Msg.range = ((float)usBoard.sensor[8]/100);     //float32 [cm] => [m]
00770 
00771         //publish data for first USrange sensor
00772         topicPub_USRangeSensor9.publish(USRange9Msg);
00773         //------------------------------------------------------------------------------------------------------------
00774 
00775         //-------------------------------------------SENSOR10-------------------------------------------------------
00776         //create USRanger10Msg
00777         //fill in header
00778         USRange10Header.seq = 1;                                //uint32
00779         USRange10Header.stamp = ros::Time::now();               //time
00780         USRange10Header.frame_id = "usrangesensor10";           //string
00781 
00782         USRange10Msg.header = USRange10Header;
00783         USRange10Msg.radiation_type = 0;                        //uint8   => Enum ULTRASOUND=0; INFRARED=1
00784         USRange10Msg.field_of_view = 1.05;                      //float32 [rad]
00785         USRange10Msg.min_range = 0.1;                           //float32 [m]
00786         USRange10Msg.max_range = 1.2;                           //float32 [m]
00787         USRange10Msg.range = ((float)usBoard.sensor[9]/100);    //float32 [cm] => [m]
00788 
00789         //publish data for first USrange sensor
00790         topicPub_USRangeSensor10.publish(USRange10Msg);
00791         //------------------------------------------------------------------------------------------------------------
00792 
00793         //-------------------------------------------SENSOR11-------------------------------------------------------
00794         //create USRanger11Msg
00795         //fill in header
00796         USRange11Header.seq = 1;                                //uint32
00797         USRange11Header.stamp = ros::Time::now();               //time
00798         USRange11Header.frame_id = "usrangesensor11";           //string
00799 
00800         USRange11Msg.header = USRange11Header;
00801         USRange11Msg.radiation_type = 0;                        //uint8   => Enum ULTRASOUND=0; INFRARED=1
00802         USRange11Msg.field_of_view = 1.05;                      //float32 [rad]
00803         USRange11Msg.min_range = 0.1;                           //float32 [m]
00804         USRange11Msg.max_range = 1.2;                           //float32 [m]
00805         USRange11Msg.range = ((float)usBoard.sensor[10]/100);   //float32 [cm] => [m]
00806 
00807         //publish data for first USrange sensor
00808         topicPub_USRangeSensor11.publish(USRange11Msg);
00809         //------------------------------------------------------------------------------------------------------------
00810 
00811         //-------------------------------------------SENSOR12-------------------------------------------------------
00812         //create USRanger12Msg
00813         //fill in header
00814         USRange12Header.seq = 1;                                //uint32
00815         USRange12Header.stamp = ros::Time::now();               //time
00816         USRange12Header.frame_id = "usrangesensor12";           //string
00817 
00818         USRange12Msg.header = USRange12Header;
00819         USRange12Msg.radiation_type = 0;                        //uint8   => Enum ULTRASOUND=0; INFRARED=1
00820         USRange12Msg.field_of_view = 1.05;                      //float32 [rad]
00821         USRange12Msg.min_range = 0.1;                           //float32 [m]
00822         USRange12Msg.max_range = 1.2;                           //float32 [m]
00823         USRange12Msg.range = ((float)usBoard.sensor[11]/100);   //float32 [cm] => [m]
00824 
00825         //publish data for first USrange sensor
00826         topicPub_USRangeSensor12.publish(USRange12Msg);
00827         //------------------------------------------------------------------------------------------------------------
00828 
00829         //-------------------------------------------SENSOR13-------------------------------------------------------
00830         //create USRanger11Msg
00831         //fill in header
00832         USRange13Header.seq = 1;                                //uint32
00833         USRange13Header.stamp = ros::Time::now();               //time
00834         USRange13Header.frame_id = "usrangesensor13";           //string
00835 
00836         USRange13Msg.header = USRange13Header;
00837         USRange13Msg.radiation_type = 0;                        //uint8   => Enum ULTRASOUND=0; INFRARED=1
00838         USRange13Msg.field_of_view = 1.05;                      //float32 [rad]
00839         USRange13Msg.min_range = 0.1;                           //float32 [m]
00840         USRange13Msg.max_range = 1.2;                           //float32 [m]
00841         USRange13Msg.range = ((float)usBoard.sensor[12]/100);   //float32 [cm] => [m]
00842 
00843         //publish data for first USrange sensor
00844         topicPub_USRangeSensor13.publish(USRange13Msg);
00845         //------------------------------------------------------------------------------------------------------------
00846 
00847         //-------------------------------------------SENSOR14-------------------------------------------------------
00848         //create USRanger14Msg
00849         //fill in header
00850         USRange14Header.seq = 1;                                //uint32
00851         USRange14Header.stamp = ros::Time::now();               //time
00852         USRange14Header.frame_id = "usrangesensor14";           //string
00853 
00854         USRange14Msg.header = USRange14Header;
00855         USRange14Msg.radiation_type = 0;                        //uint8   => Enum ULTRASOUND=0; INFRARED=1
00856         USRange14Msg.field_of_view = 1.05;                      //float32 [rad]
00857         USRange14Msg.min_range = 0.1;                           //float32 [m]
00858         USRange14Msg.max_range = 1.2;                           //float32 [m]
00859         USRange14Msg.range = ((float)usBoard.sensor[13]/100);   //float32 [cm] => [m]
00860 
00861         //publish data for first USrange sensor
00862         topicPub_USRangeSensor14.publish(USRange14Msg);
00863         //------------------------------------------------------------------------------------------------------------
00864 
00865         //-------------------------------------------SENSOR15-------------------------------------------------------
00866         //create USRanger15Msg
00867         //fill in header
00868         USRange15Header.seq = 1;                                //uint32
00869         USRange15Header.stamp = ros::Time::now();               //time
00870         USRange15Header.frame_id = "usrangesensor15";           //string
00871 
00872         USRange15Msg.header = USRange15Header;
00873         USRange15Msg.radiation_type = 0;                        //uint8   => Enum ULTRASOUND=0; INFRARED=1
00874         USRange15Msg.field_of_view = 1.05;                      //float32 [rad]
00875         USRange15Msg.min_range = 0.1;                           //float32 [m]
00876         USRange15Msg.max_range = 1.2;                           //float32 [m]
00877         USRange15Msg.range = ((float)usBoard.sensor[14]/100);   //float32 [cm] => [m]
00878 
00879         //publish data for first USrange sensor
00880         topicPub_USRangeSensor15.publish(USRange15Msg);
00881         //------------------------------------------------------------------------------------------------------------
00882 
00883         //-------------------------------------------SENSOR16-------------------------------------------------------
00884         //create USRanger16Msg
00885         //fill in header
00886         USRange16Header.seq = 1;                                //uint32
00887         USRange16Header.stamp = ros::Time::now();               //time
00888         USRange16Header.frame_id = "usrangesensor16";           //string
00889 
00890         USRange16Msg.header = USRange16Header;
00891         USRange16Msg.radiation_type = 0;                        //uint8   => Enum ULTRASOUND=0; INFRARED=1
00892         USRange16Msg.field_of_view = 1.05;                      //float32 [rad]
00893         USRange16Msg.min_range = 0.1;                           //float32 [m]
00894         USRange16Msg.max_range = 1.2;                           //float32 [m]
00895         USRange16Msg.range = ((float)usBoard.sensor[15]/100);   //float32 [cm] => [m]
00896 
00897         //publish data for first USrange sensor
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 //-----------------------------END USBoard---------------------------------------------------------------------
00913 //---------------------------------IOBoard---------------------------------------------------------------------
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 //-----------------------------END IOBoard---------------------------------------------------------------------

neo_relayboard_v2
Author(s): Jan-Niklas Nieland
autogenerated on Thu Jun 6 2019 21:37:15