Пример #1
0
def psoc():
    global ser
    rospy.init_node('PSoC_Listener')
    pub = rospy.Publisher('psoc_data', PSoC)
    sub = rospy.Subscriber('psoc_cmd', String, callback)
    while not rospy.is_shutdown():
        try:
            ser = serial.Serial(port=port,
                                baudrate=921600,
                                timeout=1,
                                writeTimeout=1)
            ser.flush()
            ser.write('>ETFM\n')
            ser.flushOutput()
            rospy.loginfo("PSoC Listener is running on " + ser.portstr)
            while not rospy.is_shutdown():
                ser.flushInput()
                line = ser.readline()
                if len(line) is 0:
                    raise serial.serialutil.SerialException
                tokens = string.split(line)
                if (tokens[0] == '(:' and tokens[len(tokens) - 1] == ':)'):
                    try:
                        p = PSoC()
                        p.left_enc = int(tokens[1])
                        p.right_enc = int(tokens[2])
                        p.vel_v = int(tokens[3])
                        p.vel_w = int(tokens[4])
                        p.time = long(tokens[5])
                        p.rate = int(tokens[6])
                        pub.publish(p)
                        #rospy.loginfo('Telemetry message: '+line)
                    except rospy.exceptions.ROSSerializationException as e:
                        rospy.logwarn('PSoC Listener f****d up.')
                        rospy.logwarn(e.message)
                else:
                    rospy.loginfo('Info message from PSoC: ' + line)
        except serial.serialutil.SerialException, serial.serialutil.SerialTimeoutException:
            rospy.loginfo("PSoC Disconnected... Reconnecting...")
        except OSError or AttributeError as e:
            rospy.logwarn("PSoC's really f****d...")
            rospy.logwarn(e.message)
            rospy.logwarn(e.strerror)
Пример #2
0
def psoc():
    global ser
    rospy.init_node('PSoC_Listener')
    pub = rospy.Publisher('psoc_data', PSoC)
    sub = rospy.Subscriber('psoc_cmd', String, callback)
    while not rospy.is_shutdown():
        try:
            ser = serial.Serial(port = port, baudrate = 921600, timeout = 1, writeTimeout = 1)
            ser.flush()
            ser.write('>ETFM\n')
            ser.flushOutput()
            rospy.loginfo( "PSoC Listener is running on " + ser.portstr )
            while not rospy.is_shutdown():
                ser.flushInput()
                line = ser.readline()
                if len(line) is 0:
                    raise serial.serialutil.SerialException
                tokens = string.split(line)
                if(tokens[0] == '(:' and tokens[len(tokens)-1] == ':)'):
                    try:
                        p = PSoC()
                        p.left_enc = int(tokens[1])
                        p.right_enc = int(tokens[2])
                        p.vel_v = int(tokens[3])
                        p.vel_w = int(tokens[4])
                        p.time = long(tokens[5])
                        p.rate = int(tokens[6])
                        pub.publish(p)
                        #rospy.loginfo('Telemetry message: '+line)
                    except rospy.exceptions.ROSSerializationException as e:
                        rospy.logwarn('PSoC Listener f****d up.')
                        rospy.logwarn(e.message)
                else:
                    rospy.loginfo('Info message from PSoC: '+ line)
        except serial.serialutil.SerialException, serial.serialutil.SerialTimeoutException:
            rospy.loginfo( "PSoC Disconnected... Reconnecting..." )
        except OSError or AttributeError as e:
            rospy.logwarn( "PSoC's really f****d..." )
            rospy.logwarn( e.message )
            rospy.logwarn( e.strerror )
Пример #3
0
def psoc():
    rospy.init_node('PSoC_Listener')
    rospy.loginfo( "PSoC Listener is running on " + ser.portstr )
    pub = rospy.Publisher('psoc_data', PSoC)
    sub = rospy.Subscriber('psoc_cmd', String, callback)
    ser.write('>ETFM\n')
    ser.flushOutput()
    while not rospy.is_shutdown():
        ser.flushInput()
        line = ser.readline()
        tokens = string.split(line)
        if(tokens[0] == '(:' and tokens[len(tokens)-1] == ':)'):
            p = PSoC()
            p.left_enc = int(tokens[1])
            p.right_enc = int(tokens[2])
            p.vel_v = int(tokens[3])
            p.vel_w = int(tokens[4])
            p.time = long(tokens[5])
            p.rate = int(tokens[6])
            pub.publish(p)
            rospy.logdebug('Telemetry message: '+line)
        else:
            rospy.loginfo('Info message from PSoC: '+ line)