Handle exceptions in lifecycle transition callbacks

rclpy/rclpy/lifecycle/node.py

@@ -12,6 +12,7 @@
 # See the License for the specific language governing permissions and
 # limitations under the License.
 
+import traceback
 from typing import Any
 from typing import Callable
 from typing import Dict
@@ -238,7 +239,13 @@ def __transition_callback_impl(self, callback_name: CallbackNames,
             else:
                 raise ValueError(f'Not valid callback name "{callback_name}" given.')
 
-            ret = cb(state)
+            try:
+                ret = cb(state)
+            except Exception:
+                self._logger.error(
+                    f'Caught exception in {callback_name}() callback of managed entity '
+                    f'{entity}:\n{traceback.format_exc()}')
+                return TransitionCallbackReturn.ERROR
             if not isinstance(ret, _rclpy.TransitionCallbackReturnType):
                 raise TypeError(
                     f'{callback_name}() return value of class {type(entity)} should be'
@@ -397,7 +404,9 @@ def __execute_callback(
             ret = cb(previous_state)
             return ret
         except Exception:
-            # TODO(ivanpauno): log sth here
+            self._logger.error(
+                f'Caught exception in transition callback for state {current_state_id}:\n'
+                f'{traceback.format_exc()}')
             return TransitionCallbackReturn.ERROR
 
     def __change_state(self, transition_id: int) -> TransitionCallbackReturn:

rclpy/test/test_lifecycle.py

@@ -31,6 +31,7 @@
 from rclpy.executors import SingleThreadedExecutor
 from rclpy.lifecycle import LifecycleNode
 from rclpy.lifecycle import LifecycleState
+from rclpy.lifecycle import ManagedEntity
 from rclpy.lifecycle import TransitionCallbackReturn
 from rclpy.node import Node
 from rclpy.publisher import BasePublisher
@@ -121,6 +122,36 @@ def __init__(self, *args: Any, **kwargs: Any):
     assert node._state_machine.current_state[1] == 'finalized'
 
 
+def test_lifecycle_exception_in_managed_entity_callback() -> None:
+    class RaisingManagedEntity(ManagedEntity):
+
+        def on_configure(self, state: LifecycleState) -> TransitionCallbackReturn:
+            raise RuntimeError('exception in on_configure')
+
+    node = LifecycleNode(
+        'test_lifecycle_exception_in_managed_entity_callback',
+        enable_communication_interface=False)
+    node.add_managed_entity(RaisingManagedEntity())
+    assert node.trigger_configure() == TransitionCallbackReturn.ERROR
+    assert node._state_machine.current_state[1] == 'unconfigured'
+    node.destroy_node()
+
+
+def test_lifecycle_exception_in_overridden_callback() -> None:
+    class RaiseOnActivateNode(LifecycleNode):
+
+        def on_activate(self, state: LifecycleState) -> TransitionCallbackReturn:
+            raise RuntimeError('exception in on_activate')
+
+    node = RaiseOnActivateNode(
+        'test_lifecycle_exception_in_overridden_callback',
+        enable_communication_interface=False)
+    assert node.trigger_configure() == TransitionCallbackReturn.SUCCESS
+    assert node.trigger_activate() == TransitionCallbackReturn.ERROR
+    assert node._state_machine.current_state[1] == 'unconfigured'
+    node.destroy_node()
+
+
 def test_lifecycle_services(request: FixtureRequest) -> None:
     lc_node_name = 'test_lifecycle_services_lifecycle'
     lc_node = LifecycleNode(lc_node_name)