Go to the documentation of this file.00001 from BAL.Header.Response.ConnectionResponse import ConnectionResponse
00002 from BAL.Interfaces.Runnable import Runnable
00003 from BAL.Handlers.incomingHandler import SERVO_RES, CON_REQ, MOTOR_RES, CLOSE_DIFF_RES, URF_RES, SWITCH_RES, IMU_RES, GPS_RES, \
00004 PPM_RES, BAT_RES, IMU_CLIB_RES
00005 import rospy
00006
00007 __author__ = 'tom1231'
00008
00009
00010 class IncomingDataHandler(Runnable):
00011 def __init__(self, data, output, devices):
00012 self._output = output
00013 self._data = data
00014 self._dev = devices
00015
00016 def run(self):
00017 if self._data.checkPackage():
00018 if self._data.getId() == CON_REQ:
00019 if self._data.toConnect():
00020 self._output.write(ConnectionResponse(True).dataTosend())
00021 elif self._data.getId() == SERVO_RES:
00022 self._dev['servos'][self._data.getServoNum()].publish(self._data.getServoPos())
00023
00024 elif self._data.getId() == MOTOR_RES:
00025 self._dev['motorsCl'][self._data.getMotorId()].publish(self._data.getMotorMsg())
00026 elif self._data.getId() == URF_RES:
00027 self._dev['urf'][self._data.getURFId()].publish(self._data.getRange())
00028 elif self._data.getId() == CLOSE_DIFF_RES:
00029 self._dev['diff'][0].publish(self._data.getPublishData())
00030 elif self._data.getId() == SWITCH_RES:
00031 self._dev['switch'][self._data.getSwitchNum()].publish(self._data.getStatus())
00032 elif self._data.getId() in [IMU_RES, IMU_CLIB_RES]:
00033 self._dev['imu'][0].publish(self._data)
00034 elif self._data.getId() == GPS_RES:
00035 self._dev['gps'][0].publish(self._data)
00036 elif self._data.getId() == PPM_RES:
00037 self._dev['ppm'][0].publish(self._data)
00038 elif self._data.getId() == BAT_RES:
00039 self._dev['battery'][0].publish(self._data.getStatus())
00040
00041 else:
00042 rospy.logdebug('CheckSum is not valid')
00043
00044
00045