Implement managed nodes (#865)

Signed-off-by: Ivan Santiago Paunovic <[email protected]>

Author: ivanpauno

rclpy/CMakeLists.txt

@@ -16,9 +16,11 @@ endif()
 
 find_package(ament_cmake REQUIRED)
 find_package(ament_cmake_python REQUIRED)
+find_package(lifecycle_msgs REQUIRED)
 find_package(rcl REQUIRED)
-find_package(rcl_logging_interface REQUIRED)
 find_package(rcl_action REQUIRED)
+find_package(rcl_lifecycle REQUIRED)
+find_package(rcl_logging_interface REQUIRED)
 find_package(rcl_yaml_param_parser REQUIRED)
 find_package(rcpputils REQUIRED)
 find_package(rcutils REQUIRED)
@@ -113,6 +115,7 @@ pybind11_add_module(_rclpy_pybind11 SHARED
   src/rclpy/exceptions.cpp
   src/rclpy/graph.cpp
   src/rclpy/guard_condition.cpp
+  src/rclpy/lifecycle.cpp
   src/rclpy/logging.cpp
   src/rclpy/names.cpp
   src/rclpy/node.cpp
@@ -133,8 +136,11 @@ target_include_directories(_rclpy_pybind11 PRIVATE
   src/rclpy/
 )
 target_link_libraries(_rclpy_pybind11 PRIVATE
+  lifecycle_msgs::lifecycle_msgs__rosidl_generator_c
+  lifecycle_msgs::lifecycle_msgs__rosidl_typesupport_c
   rcl::rcl
   rcl_action::rcl_action
+  rcl_lifecycle::rcl_lifecycle
   rcl_logging_interface::rcl_logging_interface
   rcpputils::rcpputils
   rcutils::rcutils

rclpy/package.xml

@@ -25,6 +25,7 @@
 
   <depend>rmw_implementation</depend>
   <depend>rcl</depend>
+  <depend>rcl_lifecycle</depend>
   <depend>rcl_logging_interface</depend>
   <depend>rcl_action</depend>
   <depend>rcl_yaml_param_parser</depend>

rclpy/rclpy/lifecycle/__init__.py

@@ -0,0 +1,44 @@
+# Copyright 2021 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from .managed_entity import ManagedEntity
+from .managed_entity import SimpleManagedEntity
+from .node import LifecycleNode
+from .node import LifecycleNodeMixin
+from .node import LifecycleState
+from .publisher import LifecyclePublisher
+
+from ..impl.implementation_singleton import rclpy_implementation as _rclpy
+
+# reexport LifecycleNode as Node, so it's possible to write:
+# from rclpy.lifecycle import Node
+# Do not include that in __all__ to avoid mixing it up with rclpy.node.Node.
+Node = LifecycleNode
+# same idea here
+NodeMixin = LifecycleNodeMixin
+State = LifecycleState
+Publisher = LifecyclePublisher
+
+# enum defined in pybind11 plugin
+TransitionCallbackReturn = _rclpy.TransitionCallbackReturnType
+
+
+__all__ = [
+    'LifecycleNodeMixin',
+    'LifecycleNode',
+    'LifecycleState',
+    'LifecyclePublisher',
+    'ManagedEntity',
+    'SimpleManagedEntity',
+]

rclpy/rclpy/lifecycle/managed_entity.py

@@ -0,0 +1,81 @@
+# Copyright 2021 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from functools import wraps
+
+from ..impl.implementation_singleton import rclpy_implementation as _rclpy
+
+
+TransitionCallbackReturn = _rclpy.TransitionCallbackReturnType
+
+
+class ManagedEntity:
+
+    def on_configure(self, state) -> TransitionCallbackReturn:
+        """Handle configure transition request."""
+        return TransitionCallbackReturn.SUCCESS
+
+    def on_cleanup(self, state) -> TransitionCallbackReturn:
+        """Handle cleanup transition request."""
+        return TransitionCallbackReturn.SUCCESS
+
+    def on_shutdown(self, state) -> TransitionCallbackReturn:
+        """Handle shutdown transition request."""
+        return TransitionCallbackReturn.SUCCESS
+
+    def on_activate(self, state) -> TransitionCallbackReturn:
+        """Handle activate transition request."""
+        return TransitionCallbackReturn.SUCCESS
+
+    def on_deactivate(self, state) -> TransitionCallbackReturn:
+        """Handle deactivate transition request."""
+        return TransitionCallbackReturn.SUCCESS
+
+    def on_error(self, state) -> TransitionCallbackReturn:
+        """Handle error transition request."""
+        return TransitionCallbackReturn.SUCCESS
+
+
+class SimpleManagedEntity(ManagedEntity):
+    """A simple managed entity that only sets a flag when activated/deactivated."""
+
+    def __init__(self):
+        self._enabled = False
+
+    def on_activate(self, state) -> TransitionCallbackReturn:
+        self._enabled = True
+        return TransitionCallbackReturn.SUCCESS
+
+    def on_deactivate(self, state) -> TransitionCallbackReturn:
+        self._enabled = False
+        return TransitionCallbackReturn.SUCCESS
+
+    @property
+    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 when_not_enabled is not None:
+                        when_not_enabled()
+                    return
+                wrapped(self, *args, **kwargs)
+            return only_when_enabled_wrapper
+        if wrapped is None:
+            return decorator
+        return decorator(wrapped)