test_joy.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 import roslib
00004 import rospy
00005 
00006 import pygame
00007 import pygame.joystick
00008 
00009 import sys
00010 
00011 from sensor_msgs.msg import *
00012 
00013 def main():
00014    node_name = "simple_joy"
00015    if len(sys.argv) > 1:
00016       node_name = sys.argv[1]
00017 
00018    rospy.init_node(node_name)
00019    pub = rospy.Publisher("/" + node_name + "/joy", Joy, latch=True)
00020 
00021    pygame.init()
00022    pygame.joystick.init()
00023    devices = pygame.joystick.get_count()
00024    if devices < 1:
00025       rospy.logerr("No Joystick devices detected")
00026       exit(-1)
00027    rospy.loginfo("Found %d Joystick devices" % devices)
00028 
00029    joy_name = None
00030    input_dev = 0
00031    if len(sys.argv) > 2:
00032       try:
00033          input_dev = int(sys.argv[2])
00034       except ValueError:
00035          joy_name = sys.argv[2]
00036    else:
00037       rospy.loginfo("no input device supplied. will try to use default device.")
00038       input_dev = 0
00039    rospy.loginfo("Using input device %d" % input_dev)
00040 
00041    controller = None
00042    if joy_name == None:
00043       controller = pygame.joystick.Joystick(input_dev)
00044    else:
00045       for i in range(pygame.joystick.get_count()):
00046          if joy_name in pygame.joystick.Joystick(i).get_name():
00047             controller = pygame.joystick.Joystick(i)
00048 
00049    if controller == None:
00050       rospy.logerr("No Joystick controller generated")
00051       exit(-1)
00052 
00053    controller.init()
00054    axes = controller.get_numaxes()
00055    buttons = controller.get_numbuttons()
00056    hats = controller.get_numhats()
00057 
00058    rospy.loginfo("Opened it")
00059 
00060    m = Joy()
00061    m.axes = [ 0 ] * axes
00062    m.buttons = [ 0 ] * buttons
00063 
00064    p = False
00065    done = False
00066 
00067    while not rospy.is_shutdown() and done==False:
00068       m.header.stamp = rospy.Time.now()
00069 
00070       for event in pygame.event.get():
00071          if event.type == pygame.QUIT:
00072             done=True
00073          if event.type == pygame.JOYBUTTONDOWN:
00074             done=False
00075          if event.type == pygame.JOYBUTTONUP:
00076             done=False
00077 
00078       for i in range( axes ):
00079          axis = controller.get_axis( i )
00080          m.axes[i] = axis
00081 
00082       for i in range( buttons ):
00083          button = controller.get_button( i )
00084          m.buttons[i] = button
00085 
00086       for i in range( hats ):
00087          hat = controller.get_hat( i )
00088 
00089       pub.publish(m)
00090       rospy.sleep(0.01) # 100hz max
00091 
00092 if __name__ == '__main__':
00093    try:
00094       main()
00095    except rospy.ROSInterruptException:
00096       pass


jsk_teleop_joy
Author(s): Ryohei Ueda
autogenerated on Wed Jul 19 2017 02:54:50