nanopad_joy.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 # joystick input driver for nanoPad2 input device
00004 # http://www.amazon.co.jp/KORG-USB-MIDI-%E3%82%B3%E3%83%B3%E3%83%88%E3%83%AD%E3%83%BC%E3%83%A9%E3%83%BC-NANOPAD2-%E3%83%96%E3%83%A9%E3%83%83%E3%82%AF/dp/B004M8YPKM
00005 
00006 import roslib
00007 import rospy
00008 
00009 import pygame
00010 import pygame.midi
00011 
00012 import sys
00013 
00014 from sensor_msgs.msg import *
00015 
00016 
00017 BUTTON_INDICES = [37, 39, 41, 43, 45, 47, 49, 51,
00018                   36, 38, 40, 42, 44, 46, 48, 50]
00019 AXIS_INDICES = []
00020 
00021 
00022 def main():
00023    rospy.init_node('nanopad_joy')
00024    pub = rospy.Publisher('/nanopad/joy', Joy, latch=True)
00025 
00026    pygame.midi.init()
00027    devices = pygame.midi.get_count()
00028    if devices < 1:
00029       rospy.logerr("No MIDI devices detected")
00030       exit(-1)
00031    rospy.loginfo("Found %d MIDI devices" % devices)
00032 
00033    if len(sys.argv) > 1:
00034       input_dev = int(sys.argv[1])
00035    else:
00036       rospy.loginfo("no input device supplied. will try to use default device.")
00037       input_dev = pygame.midi.get_default_input_id()
00038       if input_dev == -1:
00039          rospy.logerr("No default MIDI input device")
00040          exit(-1)
00041    rospy.loginfo("Using input device %d" % input_dev)
00042 
00043    controller = pygame.midi.Input(input_dev)
00044    rospy.loginfo("Opened it")
00045 
00046    m = Joy()
00047    m.axes = [ 0 ] * len(AXIS_INDICES)
00048    m.buttons = [ 0 ] * len(BUTTON_INDICES)
00049 
00050    p = False
00051 
00052    while not rospy.is_shutdown():
00053       m.header.stamp = rospy.Time.now()
00054       
00055       # count the number of events that are coalesced together
00056       c = 0
00057       while controller.poll():
00058          c += 1
00059          data = controller.read(1)
00060          rospy.loginfo("%s" % data)
00061          # loop through events received
00062          for event in data:
00063             control = event[0]
00064             timestamp = event[1]
00065             button_index = control[1]
00066 
00067             for bi, i in zip(BUTTON_INDICES, range(len(BUTTON_INDICES))):
00068                if button_index == bi:
00069                   if control[0] == 144:
00070                      m.buttons[i] = 1
00071                   elif control[0] == 128:
00072                      m.buttons[i] = 0
00073                p = True
00074             for bi, i in zip(AXIS_INDICES, range(len(AXIS_INDICES))):
00075                if button_index == bi:
00076                   m.axes[i] = control[2] / 127.0
00077                   p = True
00078       if p:
00079          pub.publish(m)
00080          p = False
00081 
00082       rospy.sleep(0.01) # 100hz max
00083                   
00084 
00085 
00086 if __name__ == '__main__':
00087    try:
00088       main()
00089    except rospy.ROSInterruptException:
00090       pass


jsk_teleop_joy
Author(s): Ryohei Ueda
autogenerated on Fri Apr 19 2019 03:45:43