Feature: implement state machine shutdown (#412)

Implement shutdown state machine

* Add preemption for launch state

* Implement shutdown state

* Add safety and health state preemption

* Fix sound scheduler wait loop

* Add preemption from idle state

* Refactor and rename

* Add preemption

* Fix test

* Remove leftover

* Implement preemption on ROS shutdown

* Fix imports

* Fix warning message

* Add kill gazebo on shutdown

* Use better super

* Remove wrong preempt transitions

* More better super

march_sound_scheduler/src/sound_scheduler_node.cpp

@@ -10,7 +10,7 @@ int main(int argc, char** argv)
 
   ros::Publisher sound_pub = n.advertise<sound_play::SoundRequest>("robotsound", 0);
 
-  while (ros::ok() && 0 == sound_pub.getNumSubscribers())
+  while (ros::ok() && sound_pub.getNumSubscribers() == 0)
   {
     ROS_DEBUG("Waiting on sound play topic");
     ros::Duration(0.1).sleep();

march_state_machine/package.xml

@@ -20,6 +20,7 @@
 
   <exec_depend>rospy</exec_depend>
   <exec_depend>executive_smach</exec_depend>
+  <exec_depend>python-psutil</exec_depend>
 
   <test_depend>rostest</test_depend>
 </package>

march_state_machine/scripts/march_state_machine

@@ -1,6 +1,6 @@
 #!/usr/bin/env python
 
-from march_state_machine import StateMachine
+from march_state_machine import state_machine
 
 if __name__ == '__main__':
-    StateMachine.main()
+    state_machine.main()

march_state_machine/src/march_state_machine/ControlFlow.py → march_state_machine/src/march_state_machine/control_flow.py

@@ -14,6 +14,27 @@ def __init__(self):
         self._input_device_instruction_response_publisher = rospy.Publisher('/march/input_device/instruction_response',
                                                                             Bool,
                                                                             queue_size=20)
+        self._stopped_cb = None
+        self._gait_selected_cb = None
+
+    def clear_callbacks(self):
+        """Clears all currently registered callbacks."""
+        self._stopped_cb = None
+        self._gait_selected_cb = None
+
+    def set_stopped_callback(self, cb):
+        """Sets the callback for when stop is pressed.
+
+        :param cb: Callback that will be called with no arguments
+        """
+        self._stopped_cb = cb
+
+    def set_gait_selected_callback(self, cb):
+        """Sets the callback for when a new gait is selected.
+
+        :param cb: Callback that will be called with one string argument gait_name
+        """
+        self._gait_selected_cb = cb
 
     def stop_pressed(self):
         return self._stopped
@@ -44,8 +65,12 @@ def gait_rejected(self):
     def callback_input_device_instruction(self, msg):
         if msg.type == GaitInstruction.STOP:
             self._stopped = True
+            if callable(self._stopped_cb):
+                self._stopped_cb()
         if msg.type == GaitInstruction.GAIT:
             self._gait = msg.gait_name
+            if callable(self._gait_selected_cb):
+                self._gait_selected_cb(self._gait)
         if msg.type == GaitInstruction.PAUSE:
             self._paused = True
         if msg.type == GaitInstruction.CONTINUE:

march_state_machine/src/march_state_machine/healthy_sm.py

@@ -18,14 +18,14 @@
 from march_state_machine import rough_terrain_middle_steps_sm
 from march_state_machine import ramp_door_slope_up_sm
 from march_state_machine import stairs_sm
-from march_state_machine.states.IdleState import IdleState
-from march_state_machine.states.GaitState import GaitState
+from march_state_machine.states.idle_state import IdleState
+from march_state_machine.states.gait_state import GaitState
 from std_srvs.srv import Empty, EmptyRequest
 
 
 class HealthyStart(smach.State):
     def __init__(self):
-        smach.State.__init__(self, outcomes=['succeeded'])
+        super(HealthyStart, self).__init__(outcomes=['succeeded'])
 
     def execute(self, userdata):
         if rospy.get_param('~unpause', False):
@@ -37,7 +37,7 @@ def execute(self, userdata):
 
 
 def create():
-    sm_healthy = smach.StateMachine(outcomes=['succeeded', 'failed'])
+    sm_healthy = smach.StateMachine(outcomes=['succeeded', 'failed', 'preempted'])
     # Open the container
     with sm_healthy:
         # Add states to the container
@@ -47,79 +47,74 @@ def create():
                                transitions={
                                    'home_sit': 'HOME SIT',
                                    'home_stand': 'HOME STAND',
-                                   'preempted': 'failed'
                                })
 
         # Movement states
         smach.StateMachine.add('HOME SIT', GaitState('home', 'home_sit'),
-                               transitions={'succeeded': 'SITTING', 'preempted': 'failed', 'aborted': 'UNKNOWN'})
+                               transitions={'succeeded': 'SITTING', 'aborted': 'UNKNOWN'})
 
         smach.StateMachine.add('HOME STAND', GaitState('home', 'home_stand'),
-                               transitions={'succeeded': 'STANDING', 'preempted': 'failed', 'aborted': 'UNKNOWN'})
+                               transitions={'succeeded': 'STANDING', 'aborted': 'UNKNOWN'})
 
         smach.StateMachine.add('GAIT WALK', walk_sm.create(),
-                               transitions={'succeeded': 'STANDING', 'preempted': 'failed', 'failed': 'UNKNOWN'})
+                               transitions={'succeeded': 'STANDING', 'failed': 'UNKNOWN'})
 
         smach.StateMachine.add('GAIT SIT', sit_sm.create(),
-                               transitions={'succeeded': 'SITTING', 'preempted': 'failed', 'failed': 'UNKNOWN'})
+                               transitions={'succeeded': 'SITTING', 'failed': 'UNKNOWN'})
 
         smach.StateMachine.add('GAIT STAND', stand_sm.create(),
-                               transitions={'succeeded': 'STANDING', 'preempted': 'failed', 'failed': 'UNKNOWN'})
+                               transitions={'succeeded': 'STANDING', 'failed': 'UNKNOWN'})
 
         smach.StateMachine.add('GAIT SINGLE STEP SMALL', single_step_small_sm.create(),
-                               transitions={'succeeded': 'STANDING', 'preempted': 'failed', 'failed': 'UNKNOWN'})
+                               transitions={'succeeded': 'STANDING', 'failed': 'UNKNOWN'})
 
         smach.StateMachine.add('GAIT SINGLE STEP NORMAL', single_step_normal_sm.create(),
-                               transitions={'succeeded': 'STANDING', 'preempted': 'failed', 'failed': 'UNKNOWN'})
+                               transitions={'succeeded': 'STANDING', 'failed': 'UNKNOWN'})
 
         smach.StateMachine.add('GAIT SIDE STEP LEFT', side_step_left_sm.create(),
-                               transitions={'succeeded': 'STANDING', 'preempted': 'failed', 'failed': 'UNKNOWN'})
+                               transitions={'succeeded': 'STANDING', 'failed': 'UNKNOWN'})
 
         smach.StateMachine.add('GAIT SIDE STEP RIGHT', side_step_right_sm.create(),
-                               transitions={'succeeded': 'STANDING', 'preempted': 'failed', 'failed': 'UNKNOWN'})
+                               transitions={'succeeded': 'STANDING', 'failed': 'UNKNOWN'})
 
         smach.StateMachine.add('GAIT SIDE STEP LEFT SMALL', side_step_left_small_sm.create(),
-                               transitions={'succeeded': 'STANDING', 'preempted': 'failed', 'failed': 'UNKNOWN'})
+                               transitions={'succeeded': 'STANDING', 'failed': 'UNKNOWN'})
 
         smach.StateMachine.add('GAIT SIDE STEP RIGHT SMALL', side_step_right_small_sm.create(),
-                               transitions={'succeeded': 'STANDING', 'preempted': 'failed', 'failed': 'UNKNOWN'})
+                               transitions={'succeeded': 'STANDING', 'failed': 'UNKNOWN'})
 
         smach.StateMachine.add('GAIT SOFA SIT', sofa_sit_sm.create(),
-                               transitions={'succeeded': 'SOFA SITTING', 'preempted': 'failed', 'failed': 'UNKNOWN'})
+                               transitions={'succeeded': 'SOFA SITTING', 'failed': 'UNKNOWN'})
 
         smach.StateMachine.add('GAIT SOFA STAND', sofa_stand_sm.create(),
-                               transitions={'succeeded': 'STANDING', 'preempted': 'failed', 'failed': 'UNKNOWN'})
+                               transitions={'succeeded': 'STANDING', 'failed': 'UNKNOWN'})
 
         smach.StateMachine.add('GAIT STAIRS UP', stairs_sm.create('stairs_up'),
-                               transitions={'succeeded': 'STANDING', 'preempted': 'failed', 'failed': 'UNKNOWN'})
+                               transitions={'succeeded': 'STANDING', 'failed': 'UNKNOWN'})
 
         smach.StateMachine.add('GAIT STAIRS DOWN', stairs_sm.create('stairs_down'),
-                               transitions={'succeeded': 'STANDING', 'preempted': 'failed', 'failed': 'UNKNOWN'})
+                               transitions={'succeeded': 'STANDING', 'failed': 'UNKNOWN'})
 
         smach.StateMachine.add('GAIT TILTED PATH', tilted_path_sm.create(),
-                               transitions={'succeeded': 'STANDING', 'preempted': 'failed', 'failed': 'UNKNOWN'})
+                               transitions={'succeeded': 'STANDING', 'failed': 'UNKNOWN'})
 
         smach.StateMachine.add('GAIT WALK SMALL', walk_small_sm.create(),
-                               transitions={'succeeded': 'STANDING',
-                                            'preempted': 'failed', 'failed': 'UNKNOWN'})
+                               transitions={'succeeded': 'STANDING', 'failed': 'UNKNOWN'})
 
         smach.StateMachine.add('GAIT RT HIGH STEP', rough_terrain_high_step_sm.create(),  # RT stands for Rough Terrain
-                               transitions={'succeeded': 'STANDING',
-                                            'preempted': 'failed', 'failed': 'UNKNOWN'})
+                               transitions={'succeeded': 'STANDING', 'failed': 'UNKNOWN'})
 
         smach.StateMachine.add('GAIT RT MIDDLE STEPS', rough_terrain_middle_steps_sm.create(),
-                               transitions={'succeeded': 'STANDING',
-                                            'preempted': 'failed', 'failed': 'UNKNOWN'})
+                               transitions={'succeeded': 'STANDING', 'failed': 'UNKNOWN'})
 
         smach.StateMachine.add('GAIT RD SLOPE UP', ramp_door_slope_up_sm.create(),  # RD stands for Ramp and Door
-                               transitions={'succeeded': 'STANDING',
-                                            'preempted': 'failed', 'failed': 'UNKNOWN'})
+                               transitions={'succeeded': 'STANDING', 'failed': 'UNKNOWN'})
 
         # Idle states
         smach.StateMachine.add('SITTING', IdleState(outcomes=['gait_stand', 'preempted']),
-                               transitions={'gait_stand': 'GAIT STAND', 'preempted': 'failed'})
+                               transitions={'gait_stand': 'GAIT STAND'})
         smach.StateMachine.add('SOFA SITTING', IdleState(outcomes=['gait_sofa_stand', 'preempted']),
-                               transitions={'gait_sofa_stand': 'GAIT SOFA STAND', 'preempted': 'failed'})
+                               transitions={'gait_sofa_stand': 'GAIT SOFA STAND'})
         smach.StateMachine.add('STANDING', IdleState(outcomes=['gait_sit', 'gait_walk', 'gait_single_step_small',
                                                                'gait_single_step_normal', 'gait_side_step_left',
                                                                'gait_side_step_right', 'gait_side_step_left_small',
@@ -144,7 +139,6 @@ def create():
                                             'gait_walk_small': 'GAIT WALK SMALL',
                                             'gait_rough_terrain_high_step': 'GAIT RT HIGH STEP',
                                             'gait_rough_terrain_middle_steps': 'GAIT RT MIDDLE STEPS',
-                                            'gait_ramp_door_slope_up': 'GAIT RD SLOPE UP',
-                                            'preempted': 'failed'})
+                                            'gait_ramp_door_slope_up': 'GAIT RD SLOPE UP'})
 
-        return sm_healthy
+    return sm_healthy

march_state_machine/src/march_state_machine/launch_sm.py

@@ -3,18 +3,16 @@
 from march_state_machine.states.wait_for_gait_server_state import WaitForGaitServerState
 
 
-##
-# @brief Create the launch state machine.
-# @details
-# @return The launch state machine object.
 def create():
+    """Creates the launch state machine.
+
+    :return The launch sequence machine object
+    """
     sm_launch = smach.Sequence(
-        outcomes=['succeeded', 'failed'],
+        outcomes=['succeeded', 'preempted', 'failed'],
         connector_outcome='succeeded')
 
-    # Open the container
     with sm_launch:
-        # Add states to the container
         smach.Sequence.add('WAIT FOR GAIT SERVER', WaitForGaitServerState())
 
     return sm_launch

march_state_machine/src/march_state_machine/ramp_door_slope_up_sm.py

@@ -1,8 +1,7 @@
-
 import smach
 
-from march_state_machine.states.GaitState import GaitState
-from march_state_machine.states.StoppableState import StoppableState
+from march_state_machine.states.gait_state import GaitState
+from march_state_machine.states.stoppable_state import StoppableState
 
 
 def create():
@@ -12,24 +11,24 @@ def create():
     with sm_ramp_door_slope_up:
         # Movement states
         smach.StateMachine.add('RIGHT OPEN', GaitState("ramp_door_slope_up", "right_open"),
-                               transitions={'succeeded': 'LEFT OPEN', 'preempted': 'preempted', 'aborted': 'failed'})
+                               transitions={'succeeded': 'LEFT OPEN', 'aborted': 'failed'})
 
         smach.StateMachine.add('LEFT OPEN', GaitState("ramp_door_slope_up", "left_open"),
-                               transitions={'succeeded': 'RIGHT SWING', 'preempted': 'preempted', 'aborted': 'failed'})
+                               transitions={'succeeded': 'RIGHT SWING', 'aborted': 'failed'})
 
         smach.StateMachine.add('RIGHT SWING', StoppableState("ramp_door_slope_up", "right_swing"),
                                transitions={'succeeded': 'LEFT SWING',
                                             'stopped': 'LEFT CLOSE',
-                                            'preempted': 'preempted', 'aborted': 'failed'})
+                                            'aborted': 'failed'})
 
         smach.StateMachine.add('LEFT SWING', StoppableState("ramp_door_slope_up", "left_swing"),
                                transitions={'succeeded': 'RIGHT SWING',
-                                            'stopped': 'RIGHT CLOSE', 'preempted': 'preempted', 'aborted': 'failed'})
+                                            'stopped': 'RIGHT CLOSE', 'aborted': 'failed'})
 
         smach.StateMachine.add('RIGHT CLOSE', GaitState("ramp_door_slope_up", "right_close"),
-                               transitions={'succeeded': 'succeeded', 'preempted': 'preempted', 'aborted': 'failed'})
+                               transitions={'succeeded': 'succeeded', 'aborted': 'failed'})
 
         smach.StateMachine.add('LEFT CLOSE', GaitState("ramp_door_slope_up", "left_close"),
-                               transitions={'succeeded': 'succeeded', 'preempted': 'preempted', 'aborted': 'failed'})
+                               transitions={'succeeded': 'succeeded', 'aborted': 'failed'})
 
     return sm_ramp_door_slope_up

march_state_machine/src/march_state_machine/rough_terrain_high_step_sm.py

@@ -1,14 +1,13 @@
-#!/usr/bin/env python
 import smach
 
-from march_state_machine.states.GaitState import GaitState
+from march_state_machine.states.gait_state import GaitState
 
 
 def create():
     sm_rough_terrain_high_step = smach.StateMachine(outcomes=['succeeded', 'preempted', 'failed'])
     with sm_rough_terrain_high_step:
         smach.StateMachine.add('RIGHT OPEN', GaitState("rough_terrain_high_step", "right_open"),
-                               transitions={'succeeded': 'LEFT CLOSE', 'preempted': 'failed', 'aborted': 'failed'})
+                               transitions={'succeeded': 'LEFT CLOSE', 'aborted': 'failed'})
         smach.StateMachine.add('LEFT CLOSE', GaitState("rough_terrain_high_step", "left_close"),
-                               transitions={'succeeded': 'succeeded', 'preempted': 'preempted', 'aborted': 'failed'})
+                               transitions={'succeeded': 'succeeded', 'aborted': 'failed'})
     return sm_rough_terrain_high_step