nanopad2_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                   53, 55, 57, 59, 61, 63, 65, 67,
00020                   52, 54, 56, 58, 60, 62, 64, 66,
00021                   69, 71, 73, 75, 77, 79, 81, 83,
00022                   68, 70, 72, 74, 76, 78, 80, 82,
00023                   85, 87, 89, 91, 93, 95, 97, 99,
00024                   84, 86, 88, 90, 92, 94, 96, 98]
00025 AXIS_INDICES = [1, 2]
00026 
00027 
00028 def main():
00029    rospy.init_node('nanopad2_joy')
00030    pub = rospy.Publisher('/nanopad2/joy', Joy, latch=True)
00031 
00032    pygame.midi.init()
00033    devices = pygame.midi.get_count()
00034    if devices < 1:
00035       rospy.logerr("No MIDI devices detected")
00036       exit(-1)
00037    rospy.loginfo("Found %d MIDI devices" % devices)
00038 
00039    if len(sys.argv) > 1:
00040       input_dev = int(sys.argv[1])
00041    else:
00042       rospy.loginfo("no input device supplied. will try to use default device.")
00043       input_dev = pygame.midi.get_default_input_id()
00044       if input_dev == -1:
00045          rospy.logerr("No default MIDI input device")
00046          exit(-1)
00047    rospy.loginfo("Using input device %d" % input_dev)
00048 
00049    controller = pygame.midi.Input(input_dev)
00050    rospy.loginfo("Opened it")
00051 
00052    m = Joy()
00053    m.axes = [ 0 ] * len(AXIS_INDICES)
00054    m.buttons = [ 0 ] * len(BUTTON_INDICES)
00055 
00056    p = False
00057 
00058    while not rospy.is_shutdown():
00059       m.header.stamp = rospy.Time.now()
00060       
00061       # count the number of events that are coalesced together
00062       c = 0
00063       while controller.poll():
00064          c += 1
00065          data = controller.read(1)
00066          rospy.loginfo("%s" % data)
00067          # loop through events received
00068          for event in data:
00069             control = event[0]
00070             timestamp = event[1]
00071             button_index = control[1]
00072 
00073             for bi, i in zip(BUTTON_INDICES, range(len(BUTTON_INDICES))):
00074                if button_index == bi:
00075                   if control[0] == 144:
00076                      m.buttons[i] = 1
00077                   elif control[0] == 128:
00078                      m.buttons[i] = 0
00079                p = True
00080             for bi, i in zip(AXIS_INDICES, range(len(AXIS_INDICES))):
00081                if button_index == bi:
00082                   m.axes[i] = control[2] / 127.0
00083                   p = True
00084       if p:
00085          pub.publish(m)
00086          p = False
00087 
00088       rospy.sleep(0.01) # 100hz max
00089                   
00090 
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 Sep 16 2015 04:37:30