Beispiel #1
0
def node():
    

    j = initialize_pygame_joystick()
    if j == None:
        exit(1)


    print 'Starting JoyStatus node:'
    print 'Subscribing:'
    print '   <none>'
    print 'Publishing:'
    print '   joy_status'


    pub_joy_status = rospy.Publisher('joy_status', JoyStatus)

    rospy.init_node('hex_joy')

    while not rospy.is_shutdown():
        """
        #rospy.loginfo(grin)
        print 'Sending ms on topic: "grin_status"...'
        pub.publish(grin)
        """

        for event in pygame.event.get():
            pass

        js = JoyStatus()

        js.left_x = j.get_axis(0)
        js.left_y = -j.get_axis(1)
        js.right_x = j.get_axis(3)
        js.right_y = -j.get_axis(2)
        js.throttle = j.get_axis(4)

        js.button_0 = j.get_button(0)
        js.button_1 = j.get_button(1)
        js.button_2 = j.get_button(2)
        js.button_3 = j.get_button(3)
        js.button_4 = j.get_button(4)
        js.button_5 = j.get_button(5)
        js.button_6 = j.get_button(6)
        js.button_7 = j.get_button(7)
        js.button_8 = j.get_button(8)
        js.button_9 = j.get_button(9)

        pub_joy_status.publish(js)

        rospy.sleep(0.01)
Beispiel #2
0
def node():

    if len(sys.argv) < 2:
        usage()
        print 'ERROR: Not enough parameters'
        exit(1)

    port_name = sys.argv[1]

    print 'Starting XBee node:'
    print 'Subscribing:'
    print '   <none>'
    print 'Publishing:'
    print '   joy_status'

    pub_joy_status = rospy.Publisher('joy_status', JoyStatus)

    rospy.init_node('hex_xbee')

    print 'Initializing serial connection on', port_name
    ser = CommsSerial(port_name,115200)

    while not rospy.is_shutdown():

        message = ser.recv()

        msg_fields = struct.unpack('fffff??????????',message[2:])

        js = JoyStatus()



        js.left_x = msg_fields[0]
        js.left_y = msg_fields[1]
        js.right_x = msg_fields[2]
        js.right_y = msg_fields[3]
        js.throttle = msg_fields[4]
        

        js.button_0 = msg_fields[5]
        js.button_1 = msg_fields[6]
        js.button_2 = msg_fields[7]
        js.button_3 = msg_fields[8]
        js.button_4 = msg_fields[9]
        js.button_5 = msg_fields[10]
        js.button_6 = msg_fields[11]
        js.button_7 = msg_fields[12]
        js.button_8 = msg_fields[13]
        js.button_9 = msg_fields[14]


        pub_joy_status.publish(js)

        #rospy.sleep(0.25)



        print 'Message (HEX):  ',
        for d in message:
            print hex(ord(d)),
        print ''
def node():
    
    pygame.init()

    print 'Starting JoyStatus node:'
    print 'Subscribing:'
    print '   <none>'
    print 'Publishing:'
    print '   joy_status'


    pub_joy_status = rospy.Publisher('joy_status', JoyStatus)

    rospy.init_node('hex_joy')

    screen = pygame.display.set_mode((640,480))

    is_up = False
    is_down = False

    while not rospy.is_shutdown():
        """
        #rospy.loginfo(grin)
        print 'Sending ms on topic: "grin_status"...'
        pub.publish(grin)
        """

        for event in pygame.event.get():
            if event.type == pygame.QUIT:
                running = False
            if event.type == pygame.KEYDOWN:
                if event.key == pygame.K_UP:
                    is_up = True
                if event.key == pygame.K_DOWN:
                    is_down = True
            if event.type == pygame.KEYUP:
                if event.key == pygame.K_UP:
                    is_up = False
                if event.key == pygame.K_DOWN:
                    is_down = False

        print 'is_up: ', is_up
        print 'is_down: ', is_down


        js = JoyStatus()

        js.left_x = 0
        js.left_y = 0
        js.right_x = 0
        js.right_y = 0
        js.throttle = 0

        if is_up: js.left_y += 0.5
        if is_down: js.left_y -= 0.5

        js.button_0 = False
        js.button_1 = False
        js.button_2 = False
        js.button_3 = False
        js.button_4 = False
        js.button_5 = False
        js.button_6 = False
        js.button_7 = False
        js.button_8 = False
        js.button_9 = False

        pub_joy_status.publish(js)

        screen.fill( (0, 0, 0) )
        pygame.draw.circle( screen, (255,0,0), (50,50), 10 )
        pygame.display.flip()

        rospy.sleep(0.01)