lanchpad_mini_joy.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 # joystick input driver for Launchpad Mini input device
00004 # http://www.amazon.com/Novation-Launchpad-Controller-Performing-Producing/dp/B00F9SWK9M
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 = [0, 1, 2, 3, 4, 5, 6, 7, 
00018                   16, 17, 18, 19, 20, 21, 22, 23,
00019                   32, 33, 34, 35, 36, 37, 38, 39,
00020                   48, 49, 50, 51, 52, 53, 54, 55,
00021                   64, 65, 66, 67, 68, 69, 70, 71,
00022                   80, 81, 82, 83, 84, 85, 86, 87,
00023                   96, 97, 98, 99, 100, 101, 102, 103,
00024                   112, 113, 114, 115, 116, 117, 118, 119,
00025                   104, 105, 106, 107, 108, 109, 110, 111,
00026                   8, 24, 40, 56, 72, 88, 104, 120
00027                   ]
00028 
00029 AXIS_INDICES = []
00030 COLOR_R = 3
00031 COLOR_G = 3
00032 
00033 def main():
00034    rospy.init_node('launchpad_mini_joy')
00035    pub = rospy.Publisher('/launchpad_mini/joy', Joy, latch=True)
00036 
00037    pygame.midi.init()
00038    devices = pygame.midi.get_count()
00039    if devices < 1:
00040       rospy.logerr("No MIDI devices detected")
00041       exit(-1)
00042    rospy.loginfo("Found %d MIDI devices" % devices)
00043 
00044    if len(sys.argv) > 1:
00045       input_dev = int(sys.argv[1])
00046    else:
00047       rospy.loginfo("no input device supplied. will try to use default device.")
00048       input_dev = pygame.midi.get_default_input_id()
00049       if input_dev == -1:
00050          rospy.logerr("No default MIDI input device")
00051          exit(-1)
00052 
00053    if len(sys.argv) > 2:
00054       output_dev = int(sys.argv[2])
00055    else:
00056       rospy.loginfo("no output device supplied. will try to use default device.")
00057       output_dev = pygame.midi.get_default_output_id()
00058 
00059       if output_dev == -1:
00060          rospy.logerr("No default MIDI output device")
00061          exit(-1)
00062 
00063    rospy.loginfo("Using input device %d" % input_dev)
00064    rospy.loginfo("Using output device %d" % output_dev)
00065 
00066    controller = pygame.midi.Input(input_dev)
00067    midi_output = pygame.midi.Output(output_dev)
00068    rospy.loginfo("Opened it")
00069 
00070    m = Joy()
00071    m.axes = [ 0 ] * len(AXIS_INDICES)
00072    m.buttons = [ 0 ] * len(BUTTON_INDICES)
00073 
00074    p = False
00075 
00076    while not rospy.is_shutdown():
00077       m.header.stamp = rospy.Time.now()
00078       
00079       # count the number of events that are coalesced together
00080       c = 0
00081       while controller.poll():
00082          c += 1
00083          data = controller.read(1)
00084          rospy.loginfo("%s" % data)
00085          # loop through events received
00086          for event in data:
00087             control = event[0]
00088             timestamp = event[1]
00089             button_type = control[0]
00090             button_index = control[1]
00091 
00092             for bi, i in zip(BUTTON_INDICES, range(len(BUTTON_INDICES))):
00093                if button_index == bi:
00094                   #duplicated number
00095                   if i == 64 and button_type != 176:
00096                      continue
00097                   if i == 78 and button_type != 144:
00098                      continue
00099 
00100                   if control[2] == 127:
00101                      m.buttons[i] = 1
00102                      midi_output.write([[[button_type ,button_index , COLOR_G * 4 + COLOR_R ], timestamp]])
00103                   else:
00104                      m.buttons[i] = 0
00105                      midi_output.write([[[button_type ,button_index , 0], timestamp]])
00106                p = True
00107             for bi, i in zip(AXIS_INDICES, range(len(AXIS_INDICES))):
00108                if button_index == bi:
00109                   m.axes[i] = control[2] / 127.0
00110                   p = True
00111       if p:
00112          pub.publish(m)
00113          p = False
00114 
00115       rospy.sleep(0.01) # 100hz max
00116                   
00117 
00118 
00119 if __name__ == '__main__':
00120    try:
00121       main()
00122    except rospy.ROSInterruptException:
00123       pass


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