padkontrol.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 #
00003 # joystick input driver for Korg padKontrol input device
00004 #
00005 # Author: Ryohei Ueda
00006 #
00007 
00008 import roslib;
00009 import rospy
00010 
00011 import pygame
00012 import pygame.midi
00013 
00014 import sys
00015 
00016 from sensor_msgs.msg import *
00017 
00018 BUTTON_INDICES = [61, 69, 65, 63, 60, 59, 57, 55, 49, 51, 68, 56, 48, 52, 54, 58]
00019 AXIS_INDICES = [20, 21]
00020 
00021 
00022 def main():
00023    pygame.midi.init()
00024    devices = pygame.midi.get_count()
00025    if devices < 1:
00026       print "No MIDI devices detected"
00027       exit(-1)
00028    print "Found %d MIDI devices" % devices
00029 
00030    if len(sys.argv) > 1:
00031       input_dev = int(sys.argv[1])
00032    else:
00033       print "no input device supplied. will try to use default device."
00034       input_dev = pygame.midi.get_default_input_id()
00035       if input_dev == -1:
00036          print "No default MIDI input device"
00037          exit(-1)
00038    print "Using input device %d" % input_dev
00039 
00040    controller = pygame.midi.Input(input_dev)
00041    print "Opened it"
00042 
00043    rospy.init_node('kontrol')
00044    pub = rospy.Publisher('/joy_pad', Joy, latch=True)
00045 
00046    m = Joy()
00047    m.axes = [ 0 ] * 2
00048    m.buttons = [ 0 ] * 16
00049    mode = None
00050 
00051    p = False
00052 
00053    while not rospy.is_shutdown():
00054       m.header.stamp = rospy.Time.now()
00055       
00056       # count the number of events that are coalesced together
00057       c = 0
00058       while controller.poll():
00059          c += 1
00060          data = controller.read(1)
00061          print data
00062          # loop through events received
00063          for event in data:
00064             control = event[0]
00065             timestamp = event[1]
00066             # look for continuous controller commands
00067 
00068             # 153 -> pushed
00069             # 137 -> pulled
00070             if (control[0] == 153 or control[0] == 137):
00071                 button_index = control[1]
00072                 for bi, i in zip(BUTTON_INDICES, range(len(BUTTON_INDICES))):
00073                     if button_index == bi:
00074                         if control[0] == 153:
00075                             m.buttons[i] = 1
00076                         else:
00077                             m.buttons[i] = 0
00078                         p = True
00079             elif control[0] == 185 or control[0] == 233:
00080                axis_index = control[1]
00081                for bi, i in zip(AXIS_INDICES, range(len(AXIS_INDICES))):
00082                   if axis_index == bi:
00083                      m.axes[i] = control[2] / 127.0
00084                      p = True
00085             else:
00086                continue
00087       if p:
00088          pub.publish(m)
00089          p = False
00090 
00091       rospy.sleep(0.01) # 100hz max
00092                   
00093 
00094 
00095 if __name__ == '__main__':
00096    try:
00097       main()
00098    except rospy.ROSInterruptException: pass


hrpsys_gazebo_atlas
Author(s): Yohei Kakiuchi
autogenerated on Thu Jun 6 2019 20:57:50