nanokontrol_joy.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 # joystick input driver for nanoKontrol 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-NANO-KONTROL2/dp/B004M8UZS8
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 = [58, 59,
00018                   46, 60, 61, 62,
00019                   43, 44, 42, 41, 45,
00020                   32, 33, 34, 35, 36, 37, 38, 39,
00021                   48, 49, 50, 51, 52, 53, 54, 55,
00022                   64, 65, 66, 67, 68, 69, 70, 71]
00023 AXIS_INDICES = [16, 17, 18, 19, 20, 21, 22, 23,
00024                 0, 1, 2, 3, 4, 5, 6, 7]
00025 
00026 
00027 def main():
00028    rospy.init_node('nanokontrol_joy')
00029    pub = rospy.Publisher('/nanokontrol/joy', Joy, latch=True)
00030 
00031    pygame.midi.init()
00032    devices = pygame.midi.get_count()
00033    if devices < 1:
00034       rospy.logerr("No MIDI devices detected")
00035       exit(-1)
00036    rospy.loginfo("Found %d MIDI devices" % devices)
00037 
00038    if len(sys.argv) > 1:
00039       input_dev = int(sys.argv[1])
00040    else:
00041       rospy.loginfo("no input device supplied. will try to use default device.")
00042       input_dev = pygame.midi.get_default_input_id()
00043       if input_dev == -1:
00044          rospy.logerr("No default MIDI input device")
00045          exit(-1)
00046    rospy.loginfo("Using input device %d" % input_dev)
00047 
00048    controller = pygame.midi.Input(input_dev)
00049    rospy.loginfo("Opened it")
00050 
00051    m = Joy()
00052    m.axes = [ 0 ] * len(AXIS_INDICES)
00053    m.buttons = [ 0 ] * len(BUTTON_INDICES)
00054 
00055    p = False
00056 
00057    while not rospy.is_shutdown():
00058       m.header.stamp = rospy.Time.now()
00059       
00060       # count the number of events that are coalesced together
00061       c = 0
00062       while controller.poll():
00063          c += 1
00064          data = controller.read(1)
00065          rospy.loginfo("%s" % data)
00066          # loop through events received
00067          for event in data:
00068             control = event[0]
00069             timestamp = event[1]
00070             button_index = control[1]
00071 
00072             for bi, i in zip(BUTTON_INDICES, range(len(BUTTON_INDICES))):
00073                if button_index == bi:
00074                   if control[2] == 127:
00075                      m.buttons[i] = 1
00076                   else:
00077                      m.buttons[i] = 0
00078                p = True
00079             for bi, i in zip(AXIS_INDICES, range(len(AXIS_INDICES))):
00080                if button_index == bi:
00081                   m.axes[i] = control[2] / 127.0
00082                   p = True
00083       if p:
00084          pub.publish(m)
00085          p = False
00086 
00087       rospy.sleep(0.01) # 100hz max
00088                   
00089 
00090 
00091 if __name__ == '__main__':
00092    try:
00093       main()
00094    except rospy.ROSInterruptException:
00095       pass


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