midi_config_player.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 import pygame
00003 import pygame.midi
00004 import select
00005 import sys
00006 import yaml
00007 import rospy
00008 import roslib
00009 
00010 try:
00011   from sensor_msgs.msg import Joy, JoyFeedbackArray
00012   from jsk_teleop_joy.midi_util import MIDIParse, MIDICommand, MIDIException, openMIDIInputByName, openMIDIOutputByName
00013 except:
00014   roslib.load_manifest('jsk_teleop_joy')
00015   from sensor_msgs.msg import Joy, JoyFeedbackArray
00016   from jsk_teleop_joy.midi_util import MIDIParse, MIDICommand, MIDIException, openMIDIInputByName, openMIDIOutputByName
00017 
00018 def feedback_array_cb(out_controller, config, msg_arr):
00019   global joy
00020   output_config = config["output"]
00021   for msg in msg_arr.array:
00022     if len(output_config) <= msg.id:
00023       rospy.logerr("%d is out of output configuration (%d configurations)" % (msg.id, len(output_config)))
00024       return
00025     the_config = output_config[msg.id]
00026     command = the_config[0]
00027     channel = the_config[1]
00028     val = int(msg.intensity * 127)
00029     if val < 0:
00030       val = 0
00031     elif val > 127:
00032       val = 127
00033     if the_config[2]:
00034       out_controller.write_short(command | channel, val, 0)
00035     else:
00036       param1 = the_config[3]
00037       out_controller.write_short(command | channel, param1, val)
00038       try:
00039         index = config["analogs"].index((MIDICommand.CONTINUOUS_CONTROLLER,param1))
00040         joy.axes[index] = msg.intensity
00041       except:
00042         pass
00043 
00044 def main():
00045   global joy
00046   pygame.midi.init()
00047   rospy.init_node('midi_joy')
00048   # parse the arg
00049   argv = rospy.myargv()
00050   if len(argv) == 0:
00051     rospy.logfatal("You need to specify config yaml file")
00052     sys.exit(1)
00053   config_file = argv[1]
00054   joy_pub = rospy.Publisher("/joy", Joy, queue_size=10)
00055   autorepeat_rate = rospy.get_param("~autorepeat_rate", 0)
00056   if autorepeat_rate == 0:
00057     r = rospy.Rate(1000)
00058   else:
00059     r = rospy.Rate(autorepeat_rate)
00060   with open(config_file, "r") as f:
00061     config = yaml.load(f)
00062     # open the device
00063     controller = openMIDIInputByName(config["device_name"])
00064     
00065     joy = Joy()
00066     joy.axes = [0.0] * len(config["analogs"])
00067     # automatically mapping to buttons from axes if it has NOTE_OFF or NOTE_ON MIDI commands
00068     button_configs = [c for c in config["analogs"]
00069                       if c[0] == MIDICommand.NOTE_ON or c[0] == MIDICommand.NOTE_OFF]
00070     if config.has_key("output"):
00071       out_controller = openMIDIOutputByName(config["device_name"])
00072       s = rospy.Subscriber("~set_feedback", JoyFeedbackArray, lambda msg: feedback_array_cb(out_controller, config, msg))
00073     joy.buttons = [0] * len(button_configs)
00074     while not rospy.is_shutdown():
00075       joy.header.stamp = rospy.Time.now()
00076       p = False
00077       while controller.poll():
00078         data = controller.read(1)
00079         for elem_set in data:
00080           p = True
00081           (command, ind, val) = MIDIParse(elem_set)
00082           try:
00083             index = config["analogs"].index((command, ind))
00084             joy.axes[index] = val
00085             if command == MIDICommand.NOTE_ON or command == MIDICommand.NOTE_OFF:
00086               button_index = button_configs.index((command, ind))
00087               if val == 0.0:
00088                 joy.buttons[button_index] = 0
00089               else:
00090                 joy.buttons[button_index] = 1
00091           except:
00092             rospy.logwarn("unknown MIDI message: (%d, %d, %f)" % (command, ind, val))
00093       if (autorepeat_rate != 0) or p:
00094         joy_pub.publish(joy)
00095       r.sleep()
00096 if __name__ == '__main__':
00097   main()
00098   


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