Go to the documentation of this file.00001 import serial
00002 import time
00003 import roslaunch
00004 import rosnode
00005 from BAL.Devices.DevicesBuilder.deviceBuilder import DeviceBuilder
00006 from BAL.Exceptions.VersionError import VersionError, NEED_TO_UPDATE
00007 from BAL.Handlers.incomingDataHandler import IncomingDataHandler
00008 from BAL.Handlers.incomingHandler import IncomingHandler, MOTOR_RES, CLOSE_DIFF_RES, URF_RES, SWITCH_RES, IMU_RES, \
00009 GPS_RES, \
00010 PPM_RES, BAT_RES, IMU_CLIB_RES
00011 from BAL.Handlers.incomingMsgHandler import IncomingMsgHandler
00012 from BAL.Handlers.serialWriteHandler import SerialWriteHandler, HEADER_START, HEADER_DEBUG, KEEP_ALIVE_HEADER
00013 from BAL.Header.Response.IMUPublishResponse import IMUPublishResponse
00014 from BAL.Header.Response.ServoPublishResponse import ServoPublishResponse
00015 from BAL.Header.Response.URFPublishResponse import URFPublishResponse
00016 from BAL.Header.Response.VersionResponds import VersionResponds
00017 from BAL.Header.Response.batteryPublishResponse import BatteryPublishResponse
00018 from BAL.Header.Response.closeDiffPublishResponse import CloseDiffPublishRepose
00019 from BAL.Header.Response.closeLoopPublishResponse import CloseLoopPublishResponse
00020 from BAL.Header.Response.gpsPublishResponse import GPSPublishResponse
00021 from BAL.Header.Response.imuCalibResponse import ImuCalibResponse
00022 from BAL.Header.Response.ppmPublishResponse import PPMPublishResponse
00023 from BAL.Header.Response.switchResponse import SwitchResponse
00024
00025 from BAL.ServerPkg.server import Server
00026
00027 __author__ = 'tom1231'
00028 import roslib;
00029
00030 roslib.load_manifest('ric_board')
00031 import rospy
00032 import subprocess
00033 import shlex
00034 from BAL.Header.Response.ConnectionResponse import ConnectionResponse, RES_ID
00035 from BAL.Header.Requests.ConnectionRequest import ConnectionRequest
00036 from BAL.RiCParam.RiCParam import RiCParam
00037 from serial import Serial, SerialException
00038 from threading import Thread
00039
00040 CON_REQ = 1
00041
00042 SERVO_RES = 102
00043 STATUS_RES = 100
00044
00045 INFO = 0
00046 ERROR = 1
00047 WARRNING = 2
00048
00049 VERSION = 7.0
00050
00051 WD_TIMEOUT = 5000
00052
00053
00054 class Program:
00055 def __init__(self):
00056 try:
00057
00058 self._toQuit = False
00059
00060 rospy.init_node('RiCTraffic')
00061 params = RiCParam()
00062 ser = Serial('/dev/%s' % params.getConPort())
00063 ser.flushInput()
00064 ser.flushOutput()
00065 incomingHandler = IncomingHandler()
00066 input = ser
00067 output = SerialWriteHandler(ser, incomingHandler, input)
00068 data = []
00069 toPrint = ''
00070 input.baudrate = 9600
00071 incomingLength = 0
00072 headerId = 0
00073 devBuilder = DeviceBuilder(params, output, input, incomingHandler)
00074 gotHeaderStart = False
00075 gotHeaderDebug = False
00076 msgHandler = None
00077 server = None
00078
00079 rospy.loginfo("Current version: %.2f" % VERSION)
00080 is_wd_active = False
00081 try:
00082 self.waitForConnection(output)
00083 if self.checkVer(input):
00084 input.timeout = None
00085 rospy.loginfo("Configuring devices...")
00086 devBuilder.createServos()
00087 devBuilder.createCLMotors()
00088 devBuilder.createDiff()
00089 devBuilder.createURF()
00090 devBuilder.createSwitchs()
00091 devBuilder.createPPM()
00092 devBuilder.createIMU()
00093 devBuilder.createRelays()
00094 devBuilder.createGPS()
00095 devBuilder.createOpenLoopMotors()
00096 devBuilder.createBattery()
00097 devBuilder.createOpenDiff()
00098 devBuilder.createDiffFour()
00099 devBuilder.createEmergencySwitch()
00100 devs = devBuilder.getDevs()
00101 devBuilder.sendFinishBuilding()
00102 input.timeout = None
00103 rospy.loginfo("Done, RiC Board is ready.")
00104 msgHandler = IncomingMsgHandler(devs, output)
00105 server = Server(devs, params)
00106 Thread(target=self.checkForSubscribers, args=(devs,)).start()
00107 Thread(target=msgHandler.run, args=()).start()
00108 wd_keep_alive = int(round(time.time() * 1000))
00109 while not rospy.is_shutdown() and not is_wd_active:
00110 if gotHeaderStart:
00111 if len(data) < 1:
00112 data.append(input.read())
00113 incomingLength, headerId = incomingHandler.getIncomingHeaderSizeAndId(data)
00114 elif incomingLength >= 1:
00115 for i in range(1, incomingLength):
00116 data.append(input.read())
00117 msg = self.genData(data, headerId)
00118 if msg is not None and msg.getId() != CON_REQ:
00119 msgHandler.addMsg(msg)
00120 elif msg.getId() == CON_REQ and not msg.toConnect():
00121 subprocess.Popen(shlex.split("pkill -f RiCTraffic"))
00122 rospy.logerr("Emergency button is activated.")
00123 break
00124 data = []
00125 gotHeaderStart = False
00126 else:
00127 data = []
00128 gotHeaderStart = False
00129 elif gotHeaderDebug:
00130 size = ord(input.read())
00131
00132 for i in xrange(size):
00133 toPrint += input.read()
00134
00135 code = ord(input.read())
00136
00137 if code == INFO:
00138 rospy.loginfo(toPrint)
00139 elif code == ERROR:
00140 rospy.logerr(toPrint)
00141 elif code == WARRNING:
00142 rospy.logwarn(toPrint)
00143
00144 toPrint = ''
00145 gotHeaderDebug = False
00146 elif input.inWaiting() > 0:
00147 checkHead = ord(input.read())
00148 if checkHead == HEADER_START:
00149 gotHeaderStart = True
00150 elif checkHead == HEADER_DEBUG:
00151 gotHeaderDebug = True
00152 elif checkHead == KEEP_ALIVE_HEADER:
00153 wd_keep_alive = int(round(time.time() * 1000))
00154 is_wd_active = (int(round(time.time() * 1000)) - wd_keep_alive) > WD_TIMEOUT
00155 else:
00156 raise VersionError(NEED_TO_UPDATE)
00157 if is_wd_active:
00158 rospy.logerr(
00159 "RiCBoard isn't responding.\nThe Following things can make this happen:"
00160 "\n1) If accidentally the manual driving is turn on, If so turn it off the relaunch the RiCBoard"
00161 "\n2) If accidentally the RiCTakeovver gui is turn on,If so turn it off the relaunch the RiCBoard"
00162 "\n3) The RiCBoard is stuck, If so please power off the robot and start it again. And contact RobotICan support by this email: tom@robotican.net")
00163 except KeyboardInterrupt:
00164 pass
00165
00166 except VersionError:
00167 rospy.logerr("Can't load RiCBoard because the version don't mach please update the firmware.")
00168
00169
00170 finally:
00171 if not is_wd_active:
00172 con = ConnectionResponse(False)
00173 output.writeAndWaitForAck(con.dataTosend(), RES_ID)
00174 ser.close()
00175 if msgHandler != None: msgHandler.close()
00176 self._toQuit = True
00177
00178 except SerialException:
00179 rospy.logerr("Can't find RiCBoard, please check if its connected to the computer.")
00180
00181 def checkForSubscribers(self, devs):
00182 while not self._toQuit:
00183 if len(devs['servos']) > 0:
00184 for servo in devs['servos']:
00185 servo.checkForSubscribers()
00186 if len(devs['motorsCl']) > 0:
00187 for motor in devs['motorsCl']:
00188 motor.checkForSubscribers()
00189 if len(devs['urf']) > 0:
00190 for urf in devs['urf']:
00191 urf.checkForSubscribers()
00192 if len(devs['switch']) > 0:
00193 for switch in devs['switch']:
00194 switch.checkForSubscribers()
00195 if len(devs['diff']) > 0:
00196 devs['diff'][0].checkForSubscribers()
00197 if len(devs['imu']) > 0:
00198 devs['imu'][0].checkForSubscribers()
00199 if len(devs['gps']) > 0:
00200 devs['gps'][0].checkForSubscribers()
00201 if len(devs['ppm']) > 0:
00202 devs['ppm'][0].checkForSubscribers()
00203 if len(devs['battery']) > 0:
00204 devs['battery'][0].checkForSubscribers()
00205
00206 def genData(self, data, headerId):
00207 result = None
00208 if headerId == CON_REQ: result = ConnectionRequest()
00209 if headerId == SERVO_RES: result = ServoPublishResponse()
00210 if headerId == MOTOR_RES: result = CloseLoopPublishResponse()
00211 if headerId == CLOSE_DIFF_RES: result = CloseDiffPublishRepose()
00212 if headerId == URF_RES: result = URFPublishResponse()
00213 if headerId == SWITCH_RES: result = SwitchResponse()
00214 if headerId == IMU_RES: result = IMUPublishResponse()
00215 if headerId == GPS_RES: result = GPSPublishResponse()
00216 if headerId == PPM_RES: result = PPMPublishResponse()
00217 if headerId == BAT_RES: result = BatteryPublishResponse()
00218 if headerId == IMU_CLIB_RES: result = ImuCalibResponse()
00219
00220 if result is not None: result.buildRequest(data)
00221 return result
00222
00223 def waitForConnection(self, output):
00224 output.writeAndWaitForAck(ConnectionResponse(True).dataTosend(), RES_ID)
00225
00226 def checkVer(self, input):
00227 data = []
00228 gotHeaderStart = False
00229 verInfo = VersionResponds()
00230 input.setTimeout(1)
00231 countUntilTimeout = 0
00232 while not rospy.is_shutdown() and countUntilTimeout < 3:
00233 try:
00234 if gotHeaderStart:
00235 for i in range(1, 11):
00236 data.append(input.read())
00237 verInfo.buildRequest(data)
00238
00239 if verInfo.checkPackage() and abs(verInfo.getVersion() - VERSION) < 1:
00240 if verInfo.getVersion() < VERSION:
00241 rospy.logwarn(
00242 "RiCBord has a firmware %.2f please update the firmware for better performers" % (
00243 verInfo.getVersion()))
00244 elif verInfo.getVersion() > VERSION:
00245 rospy.logwarn(
00246 "RiCBord has a firmware %.2f please update your package for better performers" % (
00247 verInfo.getVersion()))
00248 return True
00249 else:
00250 return False
00251
00252 elif ord(input.read()) == HEADER_START:
00253 gotHeaderStart = True
00254
00255 except TypeError:
00256 pass
00257 finally:
00258 countUntilTimeout += 1
00259 return False