Make keyboard_ctrl.py exit on Ctrl+C (issue #1). Also exit on command

'e', shutting down the whole object manipulation demo (partially issue
#3).

thorp_bringup/launch/object_manip_thorp.launch

@@ -60,17 +60,18 @@
 
   <!--  ****** Executive smach and visualization ******  -->
 
-  <node name="object_manipulation_smach" pkg="thorp_smach" type="object_manip_thorp.py" output="screen">   <!-- required="true" -->
+  <node name="object_manipulation_smach" pkg="thorp_smach" type="object_manip_thorp.py" output="screen">
     <param name="rec_objects_frame" value="map"/>
   </node>
 
   <node name="object_manip_smach_viewer" pkg="smach_viewer" type="smach_viewer.py" output="screen"/>
 
   <!--  ******* Keyboard interaction with smach *******  -->
-  <node name="object_manipulation_key_ctrl" pkg="thorp_smach" type="keyboard_ctrl.py" output="screen">
+  <!-- We do it required so it will shutdown the whole demo on exit -->
+  <node name="object_manipulation_key_ctrl" pkg="thorp_smach" type="keyboard_ctrl.py" output="screen" required="true">
     <param name="window_geometry" value="360x240"/>
     <param name="window_caption" value="Object manipulation user commands"/>
-    <param name="shown_text" value="Available commands: &#10;  s:  start &#10;  r:  reset &#10;  f:  fold &#10;  q:  quit"/>
+    <param name="shown_text" value="Available commands: &#10;  s:  start &#10;  x:  stop &#10;  r:  reset &#10;  f:  fold &#10;  e:  exit"/>
     <param name="text_font" value="Comic Sans MS"/>
     <param name="font_size" value="12"/>
   </node>

thorp_smach/scripts/keyboard_ctrl.py

@@ -10,9 +10,10 @@
 
 USER_COMMANDS = {
     's': "start",
+    'x': "stop",
     'r': "reset",
     'f': "fold",
-    'q': "quit"
+    'e': "exit"
 }
 
 def on_key_press(event):
@@ -26,6 +27,10 @@ def on_key_press(event):
         # Show the valid command about to execute
         text.insert('end', "\n\n'%s' command selected" % USER_COMMANDS[event.char])
 
+        if USER_COMMANDS[event.char] == 'exit':
+            quit_tk()
+            return
+
         # Creates a goal to send to the action server.
         goal = thorp_msgs.UserCommandGoal(command=USER_COMMANDS[event.char])
 
@@ -42,11 +47,15 @@ def on_key_press(event):
         # Invalid command; nothing to do
         text.insert('end', "\n\n'%s' is not a valid command" % event.char)
 
-
+def quit_tk():
+    window.quit()
 
 if __name__ == '__main__':
     try:
         rospy.init_node('object_manipulation_key_ctrl')
+
+        # window.mainloop() will block, ignoring Ctrl+C signal, so we stop it manually on ROS shutdown
+        rospy.on_shutdown(quit_tk)
 
         # Creates the SimpleActionClient, passing the type of the action
         # (UserCommandAction) to the constructor.

thorp_smach/src/thorp_smach/object_manip_thorp.py

@@ -39,7 +39,7 @@ class ExecuteUserCommand(smach.State):
     '''Different starts of the SM depending on the command provided when calling
        the actionlib wrapper. TODO: I think this can be done w/o creating a class...'''
     def __init__(self):
-        smach.State.__init__(self, outcomes=['start', 'reset', 'fold', 'quit'],
+        smach.State.__init__(self, outcomes=['start', 'stop', 'reset', 'fold'],
                                    input_keys=['user_command'])
 
     def execute(self, ud):
@@ -50,7 +50,7 @@ def execute(self, ud):
 rospy.init_node('object_manipulation_smach')
 
 # Object manipulation top-level sm
-sm = smach.StateMachine(outcomes=['quit', 'error', 'aborted', 'preempted'],
+sm = smach.StateMachine(outcomes=['stop', 'error', 'aborted', 'preempted'],
                         input_keys = ['user_command'], output_keys = ['ucmd_outcome'])
 with sm:
     ''' User data '''
@@ -71,7 +71,7 @@ def execute(self, ud):
                            transitions={'start':'ObjectDetection', 
                                         'reset':'ObjectDetection', 
                                         'fold':'FoldArm', 
-                                        'quit':'FoldArmAndRelax'})
+                                        'stop':'FoldArmAndRelax'})
 
     # Object detection sub state machine; iterates over object_detection action state and recovery
     # mechanism until an object is detected, it's preempted or there's an error (aborted outcome)
@@ -178,23 +178,23 @@ def execute(self, ud):
                                                        goal_slots=['target_type', 'named_target']),
                            remapping={'target_type':'named_pose_target_type',
                                       'named_target':'arm_folded_named_pose'},
-                           transitions={'succeeded':'RelaxArmAndQuit',
+                           transitions={'succeeded':'RelaxArmAndStop',
                                         'preempted':'preempted',
                                         'aborted':'error'})
 
-    smach.StateMachine.add('RelaxArmAndQuit',
+    smach.StateMachine.add('RelaxArmAndStop',
                            smach_ros.ServiceState('servos/relax_all',
                                                   arbotix_srv.Relax),
-                           transitions={'succeeded':'quit',
-                                        'preempted':'quit',
+                           transitions={'succeeded':'stop',
+                                        'preempted':'stop',
                                         'aborted':'error'})
 
 
     # Construct action server wrapper for top-level sm to control it with keyboard commands
     asw = smach_ros.ActionServerWrapper(
         'user_commands_action_server', thorp_msg.UserCommandAction,
         wrapped_container = sm,
-        succeeded_outcomes = ['quit'],
+        succeeded_outcomes = ['stop'],
         aborted_outcomes = ['aborted'],
         preempted_outcomes = ['error'],
         goal_key = 'user_command',