Implement managed nodes

rclpy/rclpy/lifecycle/managed_entity.py

@@ -62,15 +62,15 @@ def on_deactivate(self, state) -> TransitionCallbackReturn:
         return TransitionCallbackReturn.SUCCESS
 
     @property
-    def enabled(self):
+    def is_activated(self):
         return self._enabled
 
     @staticmethod
     def when_enabled(wrapped=None, *, when_not_enabled=None):
         def decorator(wrapped):
             @wraps(wrapped)
             def only_when_enabled_wrapper(self: SimpleManagedEntity, *args, **kwargs):
-                if not self.enabled:
+                if not self._enabled:
                     if when_not_enabled is not None:
                         when_not_enabled()
                     return

rclpy/rclpy/lifecycle/node.py

@@ -144,11 +144,8 @@ def __init__(
             ]
             for s in lifecycle_services:
                 callback_group.add_entity(s)
-            # TODO(ivanpauno): Modify attribute in Node to be "protected" instead of "private".
-            # i.e. Node.__services -> Node._services
-            # Maybe the same with similar attributes (__publishers, etc).
-            # Maybe have some interface to add a service/etc instead (?).
-            self._Node__services.extend(lifecycle_services)
+            # Extend base class list of services, so they are added to the executor when spinning.
+            self._services.extend(lifecycle_services)
 
     def trigger_configure(self):
         return self.__change_state(lifecycle_msgs.msg.Transition.TRANSITION_CONFIGURE)
@@ -166,6 +163,7 @@ def trigger_shutdown(self):
                 lifecycle_msgs.msg.Transition.TRANSITION_INACTIVE_SHUTDOWN)
         if current_state == 'active':
             return self.__change_state(lifecycle_msgs.msg.Transition.TRANSITION_ACTIVE_SHUTDOWN)
+        raise _rclpy.RCLError('Shutdown transtion not possible')
 
     def trigger_activate(self):
         return self.__change_state(lifecycle_msgs.msg.Transition.TRANSITION_ACTIVATE)
@@ -174,52 +172,106 @@ def trigger_deactivate(self):
         return self.__change_state(lifecycle_msgs.msg.Transition.TRANSITION_DEACTIVATE)
 
     def add_managed_entity(self, entity: ManagedEntity):
+        if not isinstance(entity, ManagedEntity):
+            raise TypeError('Expected a rclpy.lifecycle.ManagedEntity instance.')
         self._managed_entities.add(entity)
 
-    def on_configure(self, state) -> TransitionCallbackReturn:
+    def __transition_callback_impl(self, callback_name: str, state: LifecycleState):
         for entity in self._managed_entities:
-            ret = entity.on_configure(state)
-            # TODO(ivanpauno): Should we stop calling the other managed entities callabacks
-            # if one fails or errors?
-            # Should the behavior be the same in all the other cases?
+            cb = getattr(entity, callback_name)
+            ret = cb(state)
+            if not isinstance(ret, TransitionCallbackReturn):
+                raise TypeError(
+                    f'{callback_name}() return value of class {type(entity)} should be'
+                    ' `TransitionCallbackReturn`.\n'
+                    f'Instance of the class that caused the failure: {entity}')
             if ret != TransitionCallbackReturn.SUCCESS:
                 return ret
         return TransitionCallbackReturn.SUCCESS
 
-    def on_cleanup(self, state) -> TransitionCallbackReturn:
-        for entity in self._managed_entities:
-            ret = entity.on_cleanup(state)
-            if ret != TransitionCallbackReturn.SUCCESS:
-                return ret
-        return TransitionCallbackReturn.SUCCESS
+    def on_configure(self, state: LifecycleState) -> TransitionCallbackReturn:
+        """
+        Handle a configuring transition.
 
-    def on_shutdown(self, state) -> TransitionCallbackReturn:
-        for entity in self._managed_entities:
-            ret = entity.on_shutdown(state)
-            if ret != TransitionCallbackReturn.SUCCESS:
-                return ret
-        return TransitionCallbackReturn.SUCCESS
+        This is the default on_configure() callback.
+        It will call all on_configure() callbacks of managed entities, giving up at the first
+        entity that returns TransitionCallbackReturn.FAILURE or TransitionCallbackReturn.ERROR.
 
-    def on_activate(self, state) -> TransitionCallbackReturn:
-        for entity in self._managed_entities:
-            ret = entity.on_activate(state)
-            if ret != TransitionCallbackReturn.SUCCESS:
-                return ret
-        return TransitionCallbackReturn.SUCCESS
+        It's possible to override this callback if the default behavior is not desired.
+        If you only want to extend what this callback does, make sure to call
+        super().on_configure() in derived classes.
+        """
+        return self.__transition_callback_impl('on_configure', state)
 
-    def on_deactivate(self, state) -> TransitionCallbackReturn:
-        for entity in self._managed_entities:
-            ret = entity.on_deactivate(state)
-            if ret != TransitionCallbackReturn.SUCCESS:
-                return ret
-        return TransitionCallbackReturn.SUCCESS
+    def on_cleanup(self, state: LifecycleState) -> TransitionCallbackReturn:
+        """
+        Handle a cleaning up transition.
 
-    def on_error(self, state) -> TransitionCallbackReturn:
-        for entity in self._managed_entities:
-            ret = entity.on_error(state)
-            if ret != TransitionCallbackReturn.SUCCESS:
-                return ret
-        return TransitionCallbackReturn.SUCCESS
+        This is the default on_cleanup() callback.
+        It will call all on_cleanup() callbacks of managed entities, giving up at the first
+        entity that returns TransitionCallbackReturn.FAILURE or TransitionCallbackReturn.ERROR.
+
+        It's possible to override this callback if the default behavior is not desired.
+        If you only want to extend what this callback does, make sure to call
+        super().on_cleanup() in derived classes.
+        """
+        return self.__transition_callback_impl('on_cleanup', state)
+
+    def on_shutdown(self, state: LifecycleState) -> TransitionCallbackReturn:
+        """
+        Handle a shutting down transition.
+
+        This is the default on_shutdown() callback.
+        It will call all on_shutdown() callbacks of managed entities, giving up at the first
+        entity that returns TransitionCallbackReturn.FAILURE or TransitionCallbackReturn.ERROR.
+
+        It's possible to override this callback if the default behavior is not desired.
+        If you only want to extend what this callback does, make sure to call
+        super().on_shutdown() in derived classes.
+        """
+        return self.__transition_callback_impl('on_shutdown', state)
+
+    def on_activate(self, state: LifecycleState) -> TransitionCallbackReturn:
+        """
+        Handle an activating transition.
+
+        This is the default on_activate() callback.
+        It will call all on_activate() callbacks of managed entities, giving up at the first
+        entity that returns TransitionCallbackReturn.FAILURE or TransitionCallbackReturn.ERROR.
+
+        It's possible to override this callback if the default behavior is not desired.
+        If you only want to extend what this callback does, make sure to call
+        super().on_activate() in derived classes.
+        """
+        return self.__transition_callback_impl('on_activate', state)
+
+    def on_deactivate(self, state: LifecycleState) -> TransitionCallbackReturn:
+        """
+        Handle a deactivating transition.
+
+        This is the default on_deactivate() callback.
+        It will call all on_deactivate() callbacks of managed entities, giving up at the first
+        entity that returns TransitionCallbackReturn.FAILURE or TransitionCallbackReturn.ERROR.
+
+        It's possible to override this callback if the default behavior is not desired.
+        If you only want to extend what this callback does, make sure to call
+        super().on_deactivate() in derived classes.
+        """
+        return self.__transition_callback_impl('on_deactivate', state)
+
+    def on_error(self, state: LifecycleState) -> TransitionCallbackReturn:
+        """
+        Handle a transition error.
+
+        This is the default on_error() callback.
+        It will call all on_error() callbacks of managed entities, giving up at the first
+        entity that returns TransitionCallbackReturn.FAILURE or TransitionCallbackReturn.ERROR.
+
+        It's possible to override this callback if the default behavior is not desired.
+        If you only want to extend what this callback does, make sure to call
+        super().on_error() in derived classes.
+        """
+        return self.__transition_callback_impl('on_error', state)
 
     def create_lifecycle_publisher(self, *args, **kwargs):
         # TODO(ivanpauno): Should we override lifecycle publisher?
@@ -252,9 +304,6 @@ def __register_callback(
         a TransitionCallbackReturn value.
         """
         self._callbacks[state_id] = callback
-        # TODO(ivanpauno): Copying rclcpp style.
-        # Maybe having a return value doesn't make sense (?).
-        # Should we error/warn if overridding an existing callback?
         return True
 
     def __execute_callback(
@@ -289,8 +338,6 @@ def __change_state(self, transition_id: int) -> TransitionCallbackReturn:
         return cb_return_code
 
     def __check_is_initialized(self):
-        # TODO(ivanpauno): This sanity check is probably not needed, just doing the same checks as
-        # rclcpp for the moment.
         if not self._state_machine.initialized:
             raise RuntimeError(
                 'Internal error: got service request while lifecycle state machine '
@@ -376,7 +423,7 @@ class LifecycleNode(LifecycleNodeMixin, Node):
     A ROS 2 managed node.
 
     This class extends Node with the methods provided by LifecycleNodeMixin.
-    Methods in LifecycleNodeMixin overridde the ones in Node.
+    Methods in LifecycleNodeMixin override the ones in Node.
     """
 
     def __init__(self, node_name, *, enable_communication_interface: bool = True, **kwargs):