Fix is_running behaviour in case of exception in the state

smach/src/smach/state_machine.py

@@ -353,16 +353,18 @@ def execute(self, parent_ud = smach.UserData()):
             # Initialize container outcome
             container_outcome = None
 
-            # Step through state machine
-            while container_outcome is None and self._is_running and not smach.is_shutdown():
-                # Update the state machine
-                container_outcome = self._update_once()
+            try:
+                # Step through state machine
+                while container_outcome is None and self._is_running and not smach.is_shutdown():
+                    # Update the state machine
+                    container_outcome = self._update_once()
 
-            # Copy output keys
-            self._copy_output_keys(self.userdata, parent_ud)
+                # Copy output keys
+                self._copy_output_keys(self.userdata, parent_ud)
 
-            # We're no longer running
-            self._is_running = False
+            finally:
+                # We're no longer running
+                self._is_running = False
 
         return container_outcome
 

smach_ros/test/sequence.py

@@ -53,6 +53,31 @@ def test_sequence(self):
 
         assert outcome == 'done'
 
+    def test_exception(self):
+        class DoneState(State):
+            def __init__(self):
+                State.__init__(self,outcomes=['done'])
+            def execute(self,ud=None):
+                return 'done'
+
+        class ErrorState(State):
+            """State falls with exception"""
+            def __init__(self):
+                State.__init__(self, ['done'])
+            def execute(self, ud):
+                raise Exception('Test exception')
+
+        sq = Sequence(['done'], connector_outcome='done')
+        with sq:
+            Sequence.add('OK', DoneState())
+            Sequence.add('ERROR', ErrorState())
+            Sequence.add('IGNORED', Setter())
+
+        with self.assertRaises(InvalidUserCodeError):
+            sq.execute()
+
+        assert sq.is_running() == False
+        assert 'a' not in sq.userdata  # test IGNORED state wasn't called
 
 def main():
     rospy.init_node('sequence_test',log_level=rospy.DEBUG)

smach_ros/test/state_machine.py

@@ -161,6 +161,23 @@ def test_alt_api(self):
 
         assert outcome == 'succeeded'
 
+    def test_exception(self):
+        class ErrorState(State):
+            """State falls with exception"""
+            def __init__(self):
+                State.__init__(self, ['done'])
+            def execute(self, ud):
+                raise Exception('Test exception')
+
+        sm = StateMachine(['done'])
+        with sm:
+            StateMachine.add('ERROR', ErrorState())
+
+        with self.assertRaises(InvalidUserCodeError):
+            sm.execute()
+
+        assert sm.is_running() == False  # test running flag lowered
+
 def main():
     rospy.init_node('state_machine_test',log_level=rospy.DEBUG)
     rostest.rosrun('smach', 'state_machine_test', TestStateMachine)