def run(self):
if self._data.checkPackage():
if self._data.getId() == CON_REQ:
if self._data.toConnect():
self._output.write(ConnectionResponse(True).dataTosend())
elif self._data.getId() == SERVO_RES:
self._dev['servos'][self._data.getServoNum()].publish(
self._data.getServoPos())
elif self._data.getId() == MOTOR_RES:
self._dev['motorsCl'][self._data.getMotorId()].publish(
self._data.getMotorMsg())
elif self._data.getId() == URF_RES:
self._dev['urf'][self._data.getURFId()].publish(
self._data.getRange())
elif self._data.getId() == CLOSE_DIFF_RES:
self._dev['diff'][0].publish(self._data.getPublishData())
elif self._data.getId() == SWITCH_RES:
self._dev['switch'][self._data.getSwitchNum()].publish(
self._data.getStatus())
elif self._data.getId() in [IMU_RES, IMU_CLIB_RES]:
self._dev['imu'][0].publish(self._data)
elif self._data.getId() == GPS_RES:
self._dev['gps'][0].publish(self._data)
elif self._data.getId() == PPM_RES:
self._dev['ppm'][0].publish(self._data)
elif self._data.getId() == BAT_RES:
self._dev['battery'][0].publish(self._data.getStatus())
else:
rospy.logdebug('CheckSum is not valid')
def __init__(self):
try:
rospy.init_node('RiCTraffic')
params = RiCParam()
ser = Serial('/dev/%s' % params.getConPort())
ser.flushInput()
ser.flushOutput()
incomingHandler = IncomingHandler()
input = ser
output = SerialWriteHandler(ser, incomingHandler, input)
data = []
toPrint = ''
input.baudrate = 9600
incomingLength = 0
headerId = 0
devBuilder = DeviceBuilder(params, output, input, incomingHandler)
gotHeaderStart = False
gotHeaderDebug = False
msgHandler = None
server = None
rospy.loginfo("Current version: %.2f" % VERSION)
try:
self.waitForConnection(output)
if self.checkVer(input):
input.timeout = None
rospy.loginfo("Configuring devices...")
devBuilder.createServos()
devBuilder.createCLMotors()
devBuilder.createDiff()
devBuilder.createURF()
devBuilder.createSwitchs()
devBuilder.createIMU()
devBuilder.createRelays()
devBuilder.createGPS()
devBuilder.createPPM()
devBuilder.createOpenLoopMotors()
devBuilder.createBattery()
devBuilder.createOpenDiff()
devBuilder.createDiffFour()
devs = devBuilder.getDevs()
devBuilder.sendFinishBuilding()
input.timeout = None
rospy.loginfo("Done, RiC Board is ready.")
msgHandler = IncomingMsgHandler(devs, output)
server = Server(devs, params)
Thread(target=self.checkForSubscribers, args=(devs,)).start()
Thread(target=msgHandler.run, args=()).start()
while not rospy.is_shutdown():
if gotHeaderStart:
if len(data) < 1:
data.append(input.read())
incomingLength, headerId = incomingHandler.getIncomingHeaderSizeAndId(data)
elif incomingLength >= 1:
for i in range(1, incomingLength):
data.append(input.read())
# print data
msg = self.genData(data, headerId)
if msg is not None:
msgHandler.addMsg(msg)
data = []
gotHeaderStart = False
else:
data = []
gotHeaderStart = False
elif gotHeaderDebug:
size = ord(input.read())
for i in xrange(size):
toPrint += input.read()
code = ord(input.read())
if code == INFO:
rospy.loginfo(toPrint)
elif code == ERROR:
rospy.logerr(toPrint)
elif code == WARRNING:
rospy.logwarn(toPrint)
toPrint = ''
gotHeaderDebug = False
else:
checkHead = ord(input.read())
if checkHead == HEADER_START:
gotHeaderStart = True
elif checkHead == HEADER_DEBUG:
gotHeaderDebug = True
else:
raise VersionError(NEED_TO_UPDATE)
except KeyboardInterrupt:
pass
except VersionError:
rospy.logerr("Can't load RiCBoard because the version don't mach please update the firmware.")
except: pass
finally:
con = ConnectionResponse(False)
output.writeAndWaitForAck(con.dataTosend(), RES_ID)
ser.close()
if msgHandler != None: msgHandler.close()
except SerialException:
rospy.logerr("Can't find RiCBoard, please check if its connected to the computer.")
def __init__(self):
try:
self._toQuit = False
rospy.init_node('RiCTraffic')
params = RiCParam()
ser = Serial('/dev/%s' % params.getConPort())
ser.flushInput()
ser.flushOutput()
incomingHandler = IncomingHandler()
input = ser
output = SerialWriteHandler(ser, incomingHandler, input)
data = []
toPrint = ''
input.baudrate = 9600
incomingLength = 0
headerId = 0
devBuilder = DeviceBuilder(params, output, input, incomingHandler)
gotHeaderStart = False
gotHeaderDebug = False
msgHandler = None
server = None
rospy.loginfo("Current version: %.2f" % VERSION)
is_wd_active = False
try:
self.waitForConnection(output)
if self.checkVer(input):
input.timeout = None
rospy.loginfo("Configuring devices...")
devBuilder.createServos()
devBuilder.createCLMotors()
devBuilder.createDiff()
devBuilder.createURF()
devBuilder.createSwitchs()
devBuilder.createPPM()
devBuilder.createIMU()
devBuilder.createRelays()
devBuilder.createGPS()
devBuilder.createOpenLoopMotors()
devBuilder.createBattery()
devBuilder.createOpenDiff()
devBuilder.createDiffFour()
devBuilder.createEmergencySwitch()
devs = devBuilder.getDevs()
devBuilder.sendFinishBuilding()
input.timeout = None
rospy.loginfo("Done, RiC Board is ready.")
msgHandler = IncomingMsgHandler(devs, output)
server = Server(devs, params)
Thread(target=self.checkForSubscribers,
args=(devs, )).start()
Thread(target=msgHandler.run, args=()).start()
wd_keep_alive = int(round(time.time() * 1000))
while not rospy.is_shutdown() and not is_wd_active:
if gotHeaderStart:
if len(data) < 1:
data.append(input.read())
incomingLength, headerId = incomingHandler.getIncomingHeaderSizeAndId(
data)
elif incomingLength >= 1:
for i in range(1, incomingLength):
data.append(input.read())
msg = self.genData(data, headerId)
if msg is not None and msg.getId() != CON_REQ:
msgHandler.addMsg(msg)
elif msg.getId(
) == CON_REQ and not msg.toConnect():
subprocess.Popen(
shlex.split("pkill -f RiCTraffic"))
rospy.logerr(
"Emergency button is activated.")
break
data = []
gotHeaderStart = False
else:
data = []
gotHeaderStart = False
elif gotHeaderDebug:
size = ord(input.read())
for i in xrange(size):
toPrint += input.read()
code = ord(input.read())
if code == INFO:
rospy.loginfo(toPrint)
elif code == ERROR:
rospy.logerr(toPrint)
elif code == WARRNING:
rospy.logwarn(toPrint)
toPrint = ''
gotHeaderDebug = False
elif input.inWaiting() > 0:
checkHead = ord(input.read())
if checkHead == HEADER_START:
gotHeaderStart = True
elif checkHead == HEADER_DEBUG:
gotHeaderDebug = True
elif checkHead == KEEP_ALIVE_HEADER:
wd_keep_alive = int(round(time.time() * 1000))
is_wd_active = (int(round(time.time() * 1000)) -
wd_keep_alive) > WD_TIMEOUT
else:
raise VersionError(NEED_TO_UPDATE)
if is_wd_active:
rospy.logerr(
"RiCBoard isn't responding.\nThe Following things can make this happen:"
"\n1) If accidentally the manual driving is turn on, If so turn it off the relaunch the RiCBoard"
"\n2) If accidentally the RiCTakeovver gui is turn on,If so turn it off the relaunch the RiCBoard"
"\n3) The RiCBoard is stuck, If so please power off the robot and start it again. And contact RobotICan support by this email: [email protected]"
)
except KeyboardInterrupt:
pass
except VersionError:
rospy.logerr(
"Can't load RiCBoard because the version don't mach please update the firmware."
)
finally:
if not is_wd_active:
con = ConnectionResponse(False)
output.writeAndWaitForAck(con.dataTosend(), RES_ID)
ser.close()
if msgHandler != None: msgHandler.close()
self._toQuit = True
except SerialException:
rospy.logerr(
"Can't find RiCBoard, please check if its connected to the computer."
)
def waitForConnection(self, output):
output.writeAndWaitForAck(
ConnectionResponse(True).dataTosend(), RES_ID)
def __init__(self):
try:
self._toQuit = False
rospy.init_node('RiCTraffic')
params = RiCParam()
ser = Serial('/dev/%s' % params.getConPort())
ser.flushInput()
ser.flushOutput()
incomingHandler = IncomingHandler()
input = ser
output = SerialWriteHandler(ser, incomingHandler, input)
data = []
toPrint = ''
input.baudrate = 9600
incomingLength = 0
headerId = 0
devBuilder = DeviceBuilder(params, output, input, incomingHandler)
gotHeaderStart = False
gotHeaderDebug = False
msgHandler = None
server = None
rospy.loginfo("Current version: %.2f" % VERSION)
is_wd_active = False
try:
self.waitForConnection(output)
if self.checkVer(input):
input.timeout = None
rospy.loginfo("Configuring devices...")
devBuilder.createServos()
devBuilder.createCLMotors()
devBuilder.createDiff()
devBuilder.createURF()
devBuilder.createSwitchs()
devBuilder.createPPM()
devBuilder.createIMU()
devBuilder.createRelays()
devBuilder.createGPS()
devBuilder.createOpenLoopMotors()
devBuilder.createBattery()
devBuilder.createOpenDiff()
devBuilder.createDiffFour()
devBuilder.createEmergencySwitch()
devs = devBuilder.getDevs()
devBuilder.sendFinishBuilding()
input.timeout = None
rospy.loginfo("Done, RiC Board is ready.")
msgHandler = IncomingMsgHandler(devs, output)
server = Server(devs, params)
Thread(target=self.checkForSubscribers, args=(devs,)).start()
Thread(target=msgHandler.run, args=()).start()
wd_keep_alive = int(round(time.time() * 1000))
while not rospy.is_shutdown() and not is_wd_active:
if gotHeaderStart:
if len(data) < 1:
data.append(input.read())
incomingLength, headerId = incomingHandler.getIncomingHeaderSizeAndId(data)
elif incomingLength >= 1:
for i in range(1, incomingLength):
data.append(input.read())
msg = self.genData(data, headerId)
if msg is not None and msg.getId() != CON_REQ:
msgHandler.addMsg(msg)
elif msg.getId() == CON_REQ and not msg.toConnect():
subprocess.Popen(shlex.split("pkill -f RiCTraffic"))
rospy.logerr("Emergency button is activated.")
break
data = []
gotHeaderStart = False
else:
data = []
gotHeaderStart = False
elif gotHeaderDebug:
size = ord(input.read())
for i in xrange(size):
toPrint += input.read()
code = ord(input.read())
if code == INFO:
rospy.loginfo(toPrint)
elif code == ERROR:
rospy.logerr(toPrint)
elif code == WARRNING:
rospy.logwarn(toPrint)
toPrint = ''
gotHeaderDebug = False
elif input.inWaiting() > 0:
checkHead = ord(input.read())
if checkHead == HEADER_START:
gotHeaderStart = True
elif checkHead == HEADER_DEBUG:
gotHeaderDebug = True
elif checkHead == KEEP_ALIVE_HEADER:
wd_keep_alive = int(round(time.time() * 1000))
is_wd_active = (int(round(time.time() * 1000)) - wd_keep_alive) > WD_TIMEOUT
else:
raise VersionError(NEED_TO_UPDATE)
if is_wd_active:
rospy.logerr(
"RiCBoard isn't responding.\nThe Following things can make this happen:"
"\n1) If accidentally the manual driving is turn on, If so turn it off the relaunch the RiCBoard"
"\n2) If accidentally the RiCTakeovver gui is turn on,If so turn it off the relaunch the RiCBoard"
"\n3) The RiCBoard is stuck, If so please power off the robot and start it again. And contact RobotICan support by this email: [email protected]")
except KeyboardInterrupt:
pass
except VersionError:
rospy.logerr("Can't load RiCBoard because the version don't mach please update the firmware.")
finally:
if not is_wd_active:
con = ConnectionResponse(False)
output.writeAndWaitForAck(con.dataTosend(), RES_ID)
ser.close()
if msgHandler != None: msgHandler.close()
self._toQuit = True
except SerialException:
rospy.logerr("Can't find RiCBoard, please check if its connected to the computer.")
def __init__(self):
try:
rospy.init_node('RiCTraffic')
ser = Serial('/dev/RiCBoard')
ser.flushInput()
ser.flushOutput()
# ser = Serial('/dev/ttyUSB0')
incomingHandler = IncomingHandler()
params = RiCParam()
input = ser
output = SerialWriteHandler(ser, incomingHandler, input)
data = []
input.baudrate = 9600
incomingLength = 0
headerId = 0
devBuilder = DeviceBuilder(params, output, input, incomingHandler)
gotHeaderStart = False
try:
self.waitForConnection(input, output)
# rospy.loginfo("Connect to 0x%x.....", self.waitForConnection(input, output))
rospy.loginfo("Configuring devices...")
devBuilder.createServos()
devBuilder.createCLMotors()
devBuilder.createDiff()
devBuilder.createURF()
devBuilder.createSwitchs()
devBuilder.createIMU()
devBuilder.createRelays()
devBuilder.createGPS()
devBuilder.createPPM()
devBuilder.createOpenLoopMotors()
devBuilder.createBattery()
devBuilder.createOpenDiff()
devs = devBuilder.getDevs()
devBuilder.sendFinishBuilding()
input.timeout = None
rospy.loginfo("Done, RiC Board is ready.")
while not rospy.is_shutdown():
if gotHeaderStart:
if len(data) < 2:
data.append(input.read())
if len(data) == 2:
incomingLength ,headerId = incomingHandler.getIncomingHeaderSizeAndId(data)
elif incomingLength >= 2:
for i in range(2, incomingLength):
data.append(input.read())
# print data
msg = self.genData(data, headerId)
if msg is not None:
Thread(target=IncomingDataHandler(msg, output, devs).run, args=()).start()
data = []
gotHeaderStart = False
else:
data = []
gotHeaderStart = False
elif ord(input.read()) == HEADER_START: gotHeaderStart = True
except KeyboardInterrupt: pass
finally:
con = ConnectionResponse(False)
output.writeAndWaitForAck(con.dataTosend(), RES_ID)
ser.close()
except SerialException:
rospy.logerr("Can't find RiCBoard, please check if its connected to the computer.")