program_runner.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 from StringIO import StringIO
00004 from threading import Event
00005 
00006 import roslib;
00007 roslib.load_manifest('slider_gui')
00008 import rospy
00009 
00010 from actions.ActionSequence import ActionSequence
00011 from actions.DefaultAction import DefaultAction
00012 from Ps3Subscriber import Ps3Subscriber
00013 from SimpleFormat import SimpleFormat
00014 from program_queue.srv import *
00015 from sound_play.msg import SoundRequest
00016 from sound_play.libsoundplay import SoundClient
00017 
00018 class Runner:
00019     def __init__(self):
00020         self._event = None
00021         self._sequences = []
00022         self._running_sequence = None
00023 
00024         self._default_pose = DefaultAction()
00025         self._default_pose.set_duration(8.0)
00026 
00027         self._ps3_subscriber = Ps3Subscriber()
00028 
00029         self._soundhandle = SoundClient()
00030 
00031         rospy.Service('run_slider_program', CallProgram, self._handle_run_program)
00032         rospy.loginfo('Runner ready')
00033 
00034     def _handle_run_program(self, req):
00035         print 'Running program...'
00036 
00037         input = StringIO(req.code)
00038         storage = SimpleFormat(input)
00039         count = storage.deserialize_data()
00040         assert(count > 0)
00041         self._sequences = []
00042         for index in range(count):
00043             sequence = ActionSequence()
00044             sequence.deserialize(storage)
00045             print 'Loaded sequence %d with %d poses' % (index + 1, len(sequence.actions()))
00046             sequence.executing_action_signal.connect(self._progress)
00047             sequence.execute_sequence_finished_signal.connect(self._finished)
00048             self._sequences.append(sequence)
00049 
00050         self._event = Event()
00051         # wait until default pose has finished
00052         print 'Execute default pose'
00053         self._default_pose.execute_finished_signal.connect(self._event.set)
00054         self._default_pose.execute()
00055         self._event.wait()
00056         self._default_pose.execute_finished_signal.disconnect(self._event.set)
00057 
00058         # enable interactive mode
00059         print 'Enable interactive mode'
00060         self._event.clear()
00061         self._default_pose.execute_finished_signal.connect(self._finished)
00062         self._ps3_subscriber.buttons_changed.connect(self._check_ps3_buttons)
00063         self._event.wait()
00064         self._default_pose.execute_finished_signal.disconnect(self._finished)
00065         self._ps3_subscriber.buttons_changed.disconnect(self._check_ps3_buttons)
00066 
00067         self._stop_current_sequence()
00068 
00069         for sequence in self._sequences:
00070             sequence.executing_action_signal.disconnect(self._progress)
00071             sequence.execute_sequence_finished_signal.disconnect(self._finished)
00072         self._sequences = []
00073 
00074         print 'Running program finished'
00075         return CallProgramResponse(0, "Slider Program Successful")
00076 
00077     def _check_ps3_buttons(self):
00078         triggered_buttons = self._ps3_subscriber.get_triggered_buttons()
00079 
00080         if Ps3Subscriber.select_button in triggered_buttons:
00081             self._event.set()
00082         elif Ps3Subscriber.start_button in triggered_buttons:
00083             self._execute_sequence(-1)
00084             pass
00085 
00086         elif Ps3Subscriber.square_button in triggered_buttons:
00087             self._execute_sequence(0)
00088         elif Ps3Subscriber.triangle_button in triggered_buttons:
00089             self._execute_sequence(1)
00090         elif Ps3Subscriber.circle_button in triggered_buttons:
00091             self._execute_sequence(2)
00092         elif Ps3Subscriber.cross_button in triggered_buttons:
00093             self._execute_sequence(3)
00094 
00095         elif Ps3Subscriber.top_button in triggered_buttons:
00096             self._soundhandle.playWave('/u/applications/ros/pr2_hack_the_future/slider_gui/sounds/sound1.wav')
00097         elif Ps3Subscriber.right_button in triggered_buttons:
00098             self._soundhandle.playWave('/u/applications/ros/pr2_hack_the_future/slider_gui/sounds/four_beeps.wav')
00099         elif Ps3Subscriber.bottom_button in triggered_buttons:
00100             self._soundhandle.playWave('/u/applications/ros/pr2_hack_the_future/slider_gui/sounds/sound3.wav')
00101         elif Ps3Subscriber.left_button in triggered_buttons:
00102             self._soundhandle.playWave('/u/applications/ros/pr2_hack_the_future/slider_gui/sounds/thankyou.wav')
00103 
00104     def _execute_sequence(self, index):
00105         self._stop_current_sequence()
00106         if index == -1:
00107             print 'Execute default pose'
00108             self._running_sequence = index
00109             self._default_pose.execute()
00110         elif index in range(len(self._sequences)):
00111             print 'Execute sequence %d' % (index + 1)
00112             self._running_sequence = index
00113             sequence = self._sequences[index]
00114             sequence.execute_all()
00115 
00116     def _progress(self, index):
00117         print 'Step %d done' % (index + 1)
00118 
00119     def _finished(self):
00120         self._running_sequence = None
00121 
00122     def _stop_current_sequence(self):
00123         if self._running_sequence is not None:
00124             if self._running_sequence == -1:
00125                 print 'Stop default pose'
00126                 self._default_pose.stop()
00127             else:
00128                 print 'Stop sequence %d' % (self._running_sequence + 1)
00129                 sequence = self._sequences[self._running_sequence]
00130                 sequence.stop()
00131             self._running_sequence = None
00132 
00133 
00134 if __name__ == '__main__':
00135     rospy.init_node('program_runner')
00136     runner = Runner()
00137     rospy.spin()


slider_gui
Author(s): Dirk Thomas
autogenerated on Wed Aug 26 2015 15:37:50