예제 #1
0
    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')
예제 #2
0
    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.")
예제 #3
0
    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."
            )
예제 #4
0
 def waitForConnection(self, output):
     output.writeAndWaitForAck(
         ConnectionResponse(True).dataTosend(), RES_ID)
예제 #5
0
    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.")
예제 #6
0
    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.")