Implement managed nodes

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)

rclpy/rclpy/lifecycle/publisher.py

@@ -0,0 +1,37 @@
+# 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 typing import Union
+
+from rclpy.publisher import MsgType
+from rclpy.publisher import Publisher
+
+from .managed_entity import SimpleManagedEntity
+
+
+class LifecyclePublisher(SimpleManagedEntity, Publisher):
+    """Managed publisher entity."""
+
+    def __init__(self, *args, **kwargs):
+        SimpleManagedEntity.__init__(self)
+        Publisher.__init__(self, *args, **kwargs)
+
+    @SimpleManagedEntity.when_enabled
+    def publish(self, msg: Union[MsgType, bytes]) -> None:
+        """
+        Publish a message if the lifecycle publisher is enabled.
+
+        See rclpy.publisher.Publisher.publish() for more details.
+        """
+        Publisher.publish(self, msg)

rclpy/rclpy/node.py

@@ -22,6 +22,7 @@
 from typing import Optional
 from typing import Sequence
 from typing import Tuple
+from typing import Type
 from typing import TypeVar
 from typing import Union
 
@@ -152,12 +153,12 @@ def __init__(
         self.__handle = None
         self._context = get_default_context() if context is None else context
         self._parameters: dict = {}
-        self.__publishers: List[Publisher] = []
-        self.__subscriptions: List[Subscription] = []
-        self.__clients: List[Client] = []
-        self.__services: List[Service] = []
-        self.__timers: List[Timer] = []
-        self.__guards: List[GuardCondition] = []
+        self._publishers: List[Publisher] = []
+        self._subscriptions: List[Subscription] = []
+        self._clients: List[Client] = []
+        self._services: List[Service] = []
+        self._timers: List[Timer] = []
+        self._guards: List[GuardCondition] = []
         self.__waitables: List[Waitable] = []
         self._default_callback_group = MutuallyExclusiveCallbackGroup()
         self._parameters_callbacks: List[Callable[[List[Parameter]], SetParametersResult]] = []
@@ -227,32 +228,32 @@ def __init__(
     @property
     def publishers(self) -> Iterator[Publisher]:
         """Get publishers that have been created on this node."""
-        yield from self.__publishers
+        yield from self._publishers
 
     @property
     def subscriptions(self) -> Iterator[Subscription]:
         """Get subscriptions that have been created on this node."""
-        yield from self.__subscriptions
+        yield from self._subscriptions
 
     @property
     def clients(self) -> Iterator[Client]:
         """Get clients that have been created on this node."""
-        yield from self.__clients
+        yield from self._clients
 
     @property
     def services(self) -> Iterator[Service]:
         """Get services that have been created on this node."""
-        yield from self.__services
+        yield from self._services
 
     @property
     def timers(self) -> Iterator[Timer]:
         """Get timers that have been created on this node."""
-        yield from self.__timers
+        yield from self._timers
 
     @property
     def guards(self) -> Iterator[GuardCondition]:
         """Get guards that have been created on this node."""
-        yield from self.__guards
+        yield from self._guards
 
     @property
     def waitables(self) -> Iterator[Waitable]:
@@ -1245,6 +1246,7 @@ def create_publisher(
         callback_group: Optional[CallbackGroup] = None,
         event_callbacks: Optional[PublisherEventCallbacks] = None,
         qos_overriding_options: Optional[QoSOverridingOptions] = None,
+        publisher_class: Type[Publisher] = Publisher,
     ) -> Publisher:
         """
         Create a new publisher.
@@ -1294,14 +1296,14 @@ def create_publisher(
             self._validate_topic_or_service_name(topic)
 
         try:
-            publisher = Publisher(
+            publisher = publisher_class(
                 publisher_object, msg_type, topic, qos_profile,
                 event_callbacks=event_callbacks or PublisherEventCallbacks(),
                 callback_group=callback_group)
         except Exception:
             publisher_object.destroy_when_not_in_use()
             raise
-        self.__publishers.append(publisher)
+        self._publishers.append(publisher)
         self._wake_executor()
 
         for event_callback in publisher.event_handlers:
@@ -1379,7 +1381,7 @@ def create_subscription(
             subscription_object.destroy_when_not_in_use()
             raise
         callback_group.add_entity(subscription)
-        self.__subscriptions.append(subscription)
+        self._subscriptions.append(subscription)
         self._wake_executor()
 
         for event_handler in subscription.event_handlers:
@@ -1425,7 +1427,7 @@ def create_client(
             client_impl, srv_type, srv_name, qos_profile,
             callback_group)
         callback_group.add_entity(client)
-        self.__clients.append(client)
+        self._clients.append(client)
         self._wake_executor()
         return client
 
@@ -1469,7 +1471,7 @@ def create_service(
             service_impl,
             srv_type, srv_name, callback, callback_group, qos_profile)
         callback_group.add_entity(service)
-        self.__services.append(service)
+        self._services.append(service)
         self._wake_executor()
         return service
 
@@ -1500,7 +1502,7 @@ def create_timer(
         timer = Timer(callback, callback_group, timer_period_nsec, clock, context=self.context)
 
         callback_group.add_entity(timer)
-        self.__timers.append(timer)
+        self._timers.append(timer)
         self._wake_executor()
         return timer
 
@@ -1515,7 +1517,7 @@ def create_guard_condition(
         guard = GuardCondition(callback, callback_group, context=self.context)
 
         callback_group.add_entity(guard)
-        self.__guards.append(guard)
+        self._guards.append(guard)
         self._wake_executor()
         return guard
 
@@ -1547,8 +1549,8 @@ def destroy_publisher(self, publisher: Publisher) -> bool:
 
         :return: ``True`` if successful, ``False`` otherwise.
         """
-        if publisher in self.__publishers:
-            self.__publishers.remove(publisher)
+        if publisher in self._publishers:
+            self._publishers.remove(publisher)
             for event_handler in publisher.event_handlers:
                 self.__waitables.remove(event_handler)
             try:
@@ -1565,8 +1567,8 @@ def destroy_subscription(self, subscription: Subscription) -> bool:
 
         :return: ``True`` if succesful, ``False`` otherwise.
         """
-        if subscription in self.__subscriptions:
-            self.__subscriptions.remove(subscription)
+        if subscription in self._subscriptions:
+            self._subscriptions.remove(subscription)
             for event_handler in subscription.event_handlers:
                 self.__waitables.remove(event_handler)
             try:
@@ -1583,8 +1585,8 @@ def destroy_client(self, client: Client) -> bool:
 
         :return: ``True`` if successful, ``False`` otherwise.
         """
-        if client in self.__clients:
-            self.__clients.remove(client)
+        if client in self._clients:
+            self._clients.remove(client)
             try:
                 client.destroy()
             except InvalidHandle:
@@ -1599,8 +1601,8 @@ def destroy_service(self, service: Service) -> bool:
 
         :return: ``True`` if successful, ``False`` otherwise.
         """
-        if service in self.__services:
-            self.__services.remove(service)
+        if service in self._services:
+            self._services.remove(service)
             try:
                 service.destroy()
             except InvalidHandle:
@@ -1615,8 +1617,8 @@ def destroy_timer(self, timer: Timer) -> bool:
 
         :return: ``True`` if successful, ``False`` otherwise.
         """
-        if timer in self.__timers:
-            self.__timers.remove(timer)
+        if timer in self._timers:
+            self._timers.remove(timer)
             try:
                 timer.destroy()
             except InvalidHandle:
@@ -1631,8 +1633,8 @@ def destroy_guard_condition(self, guard: GuardCondition) -> bool:
 
         :return: ``True`` if successful, ``False`` otherwise.
         """
-        if guard in self.__guards:
-            self.__guards.remove(guard)
+        if guard in self._guards:
+            self._guards.remove(guard)
             try:
                 guard.destroy()
             except InvalidHandle:
@@ -1650,7 +1652,7 @@ def destroy_rate(self, rate: Rate):
         self.destroy_timer(rate._timer)
         rate.destroy()
 
-    def destroy_node(self) -> bool:
+    def destroy_node(self):
         """
         Destroy the node.
 
@@ -1670,18 +1672,18 @@ def destroy_node(self) -> bool:
 
         # Destroy dependent items eagerly to work around a possible hang
         # https://github.com/ros2/build_cop/issues/248
-        while self.__publishers:
-            self.destroy_publisher(self.__publishers[0])
-        while self.__subscriptions:
-            self.destroy_subscription(self.__subscriptions[0])
-        while self.__clients:
-            self.destroy_client(self.__clients[0])
-        while self.__services:
-            self.destroy_service(self.__services[0])
-        while self.__timers:
-            self.destroy_timer(self.__timers[0])
-        while self.__guards:
-            self.destroy_guard_condition(self.__guards[0])
+        while self._publishers:
+            self.destroy_publisher(self._publishers[0])
+        while self._subscriptions:
+            self.destroy_subscription(self._subscriptions[0])
+        while self._clients:
+            self.destroy_client(self._clients[0])
+        while self._services:
+            self.destroy_service(self._services[0])
+        while self._timers:
+            self.destroy_timer(self._timers[0])
+        while self._guards:
+            self.destroy_guard_condition(self._guards[0])
         self.__node.destroy_when_not_in_use()
         self._wake_executor()
 

rclpy/src/rclpy/_rclpy_pybind11.cpp

@@ -29,6 +29,7 @@
 #include "exceptions.hpp"
 #include "graph.hpp"
 #include "guard_condition.hpp"
+#include "lifecycle.hpp"
 #include "logging.hpp"
 #include "logging_api.hpp"
 #include "names.hpp"
@@ -230,4 +231,5 @@ PYBIND11_MODULE(_rclpy_pybind11, m) {
   rclpy::define_logging_api(m);
   rclpy::define_signal_handler_api(m);
   rclpy::define_clock_event(m);
+  rclpy::define_lifecycle_api(m);
 }

rclpy/src/rclpy/lifecycle.cpp

@@ -0,0 +1,361 @@
+// 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.
+
+#include <pybind11/pybind11.h>
+#include <pybind11/stl.h>
+
+#include <lifecycle_msgs/msg/transition_event.h>
+#include <lifecycle_msgs/srv/change_state.h>
+#include <lifecycle_msgs/srv/get_state.h>
+#include <lifecycle_msgs/srv/get_available_states.h>
+#include <lifecycle_msgs/srv/get_available_transitions.h>
+
+#include <rcl_lifecycle/rcl_lifecycle.h>
+
+#include <rosidl_runtime_c/message_type_support_struct.h>
+#include <rosidl_runtime_c/service_type_support_struct.h>
+
+#include <memory>
+#include <string>
+#include <tuple>
+#include <vector>
+
+#include "destroyable.hpp"
+#include "exceptions.hpp"
+#include "lifecycle.hpp"
+#include "node.hpp"
+#include "service.hpp"
+
+namespace py = pybind11;
+
+namespace
+{
+class LifecycleStateMachine : public rclpy::Destroyable,
+  public std::enable_shared_from_this<LifecycleStateMachine>
+{
+public:
+  LifecycleStateMachine(
+    rclpy::Node & node, bool enable_com_interface)
+  : node_(node)
+  {
+    state_machine_ = std::shared_ptr<rcl_lifecycle_state_machine_t>(
+      new rcl_lifecycle_state_machine_t(rcl_lifecycle_get_zero_initialized_state_machine()),
+      [node](rcl_lifecycle_state_machine_t * state_machine) {
+        rcl_ret_t ret = rcl_lifecycle_state_machine_fini(state_machine, node.rcl_ptr());
+        if (RCL_RET_OK != ret) {
+          // Warning should use line number of the current stack frame
+          int stack_level = 1;
+          PyErr_WarnFormat(
+            PyExc_RuntimeWarning, stack_level, "Failed to fini lifecycle state machine: %s",
+            rcl_get_error_string().str);
+          rcl_reset_error();
+        }
+      });
+    auto state_machine_options = rcl_lifecycle_get_default_state_machine_options();
+    state_machine_options.enable_com_interface = enable_com_interface;
+    rcl_ret_t ret = rcl_lifecycle_state_machine_init(
+      state_machine_.get(),
+      node_.rcl_ptr(),
+      ROSIDL_GET_MSG_TYPE_SUPPORT(lifecycle_msgs, msg, TransitionEvent),
+      ROSIDL_GET_SRV_TYPE_SUPPORT(lifecycle_msgs, srv, ChangeState),
+      ROSIDL_GET_SRV_TYPE_SUPPORT(lifecycle_msgs, srv, GetState),
+      ROSIDL_GET_SRV_TYPE_SUPPORT(lifecycle_msgs, srv, GetAvailableStates),
+      ROSIDL_GET_SRV_TYPE_SUPPORT(lifecycle_msgs, srv, GetAvailableTransitions),
+      ROSIDL_GET_SRV_TYPE_SUPPORT(lifecycle_msgs, srv, GetAvailableTransitions),
+      &state_machine_options);
+    if (RCL_RET_OK != ret) {
+      throw rclpy::RCLError("Failed to create lifecycle state machine");
+    }
+    if (state_machine_->options.enable_com_interface) {
+      srv_change_state_ = std::make_shared<rclpy::Service>(
+        node_,
+        std::shared_ptr<rcl_service_t>(
+          state_machine_, &state_machine_->com_interface.srv_change_state));
+      srv_get_state_ = std::make_shared<rclpy::Service>(
+        node_,
+        std::shared_ptr<rcl_service_t>(
+          state_machine_, &state_machine_->com_interface.srv_get_state));
+      srv_get_available_states_ = std::make_shared<rclpy::Service>(
+        node_,
+        std::shared_ptr<rcl_service_t>(
+          state_machine_, &state_machine_->com_interface.srv_get_available_states));
+      srv_get_available_transitions_ = std::make_shared<rclpy::Service>(
+        node_,
+        std::shared_ptr<rcl_service_t>(
+          state_machine_, &state_machine_->com_interface.srv_get_available_transitions));
+      srv_get_transition_graph_ = std::make_shared<rclpy::Service>(
+        node_,
+        std::shared_ptr<rcl_service_t>(
+          state_machine_, &state_machine_->com_interface.srv_get_transition_graph));
+    }
+  }
+
+  ~LifecycleStateMachine()
+  {
+    this->destroy();
+  }
+
+  void
+  destroy() override
+  {
+    state_machine_.reset();
+    srv_change_state_.reset();
+    srv_get_state_.reset();
+    srv_get_available_states_.reset();
+    srv_get_available_transitions_.reset();
+    srv_get_transition_graph_.reset();
+    node_.destroy();
+  }
+
+  bool
+  is_initialized()
+  {
+    return RCL_RET_OK == rcl_lifecycle_state_machine_is_initialized(state_machine_.get());
+  }
+
+  void
+  trigger_transition_by_id(uint8_t transition_id, bool publish_update)
+  {
+    if (
+      rcl_lifecycle_trigger_transition_by_id(
+        state_machine_.get(), transition_id, publish_update) != RCL_RET_OK)
+    {
+      throw rclpy::RCLError("Failed to trigger lifecycle state machine transition");
+    }
+  }
+
+  void
+  trigger_transition_by_label(std::string label, bool publish_update)
+  {
+    if (
+      rcl_lifecycle_trigger_transition_by_label(
+        state_machine_.get(), label.c_str(), publish_update) != RCL_RET_OK)
+    {
+      throw rclpy::RCLError("Failed to trigger lifecycle state machine transition");
+    }
+  }
+
+  uint8_t
+  get_transition_by_label(std::string label)
+  {
+    const auto * transition = rcl_lifecycle_get_transition_by_label(
+      state_machine_->current_state, label.c_str());
+    if (nullptr == transition) {
+      throw rclpy::RCLError("Failed to get transition from label");
+    }
+    return transition->id;
+  }
+
+  std::string
+  std_string_from_maybe_nullptr(const char * input)
+  {
+    if (input) {
+      return input;
+    }
+    return std::string{};
+  }
+
+  py::tuple
+  get_current_state()
+  {
+    return py::make_tuple(
+      state_machine_->current_state->id,
+      state_machine_->current_state->label);
+  }
+
+  std::vector<std::tuple<uint8_t, std::string>>
+  get_available_states()
+  {
+    std::vector<std::tuple<uint8_t, std::string>> ret;
+    ret.reserve(state_machine_->transition_map.states_size);
+    for (size_t i = 0; i <= state_machine_->transition_map.states_size; ++i) {
+      ret.emplace_back(
+        std::make_tuple(
+          state_machine_->transition_map.states[i].id,
+          std_string_from_maybe_nullptr(state_machine_->transition_map.states[i].label)));
+    }
+    return ret;
+  }
+
+  std::vector<std::tuple<uint8_t, std::string, uint8_t, std::string, uint8_t, std::string>>
+  get_available_transitions()
+  {
+    std::vector<std::tuple<uint8_t, std::string, uint8_t, std::string, uint8_t, std::string>> ret;
+    ret.reserve(state_machine_->current_state->valid_transition_size);
+    for (size_t i = 0; i < state_machine_->current_state->valid_transition_size; ++i) {
+      ret.emplace_back(
+        std::make_tuple(
+          state_machine_->current_state->valid_transitions[i].id,
+          std_string_from_maybe_nullptr(
+            state_machine_->current_state->valid_transitions[i].label),
+          state_machine_->current_state->valid_transitions[i].start->id,
+          std_string_from_maybe_nullptr(
+            state_machine_->current_state->valid_transitions[i].start->label),
+          state_machine_->current_state->valid_transitions[i].goal->id,
+          std_string_from_maybe_nullptr(
+            state_machine_->current_state->valid_transitions[i].goal->label)));
+    }
+    return ret;
+  }
+
+  std::vector<std::tuple<uint8_t, std::string, uint8_t, std::string, uint8_t, std::string>>
+  get_transition_graph()
+  {
+    std::vector<std::tuple<uint8_t, std::string, uint8_t, std::string, uint8_t, std::string>> ret;
+    ret.reserve(state_machine_->transition_map.transitions_size);
+    for (size_t i = 0; i < state_machine_->transition_map.transitions_size; ++i) {
+      ret.emplace_back(
+        std::make_tuple(
+          state_machine_->transition_map.transitions[i].id,
+          state_machine_->transition_map.transitions[i].label,
+          state_machine_->transition_map.transitions[i].start->id,
+          state_machine_->transition_map.transitions[i].start->label,
+          state_machine_->transition_map.transitions[i].goal->id,
+          state_machine_->transition_map.transitions[i].goal->label));
+    }
+    return ret;
+  }
+
+  void
+  print()
+  {
+    rcl_print_state_machine(state_machine_.get());
+  }
+
+  std::shared_ptr<rclpy::Service>
+  get_srv_change_state()
+  {
+    return srv_change_state_;
+  }
+
+  std::shared_ptr<rclpy::Service>
+  get_srv_get_state()
+  {
+    return srv_get_state_;
+  }
+
+  std::shared_ptr<rclpy::Service>
+  get_srv_get_available_states()
+  {
+    return srv_get_available_states_;
+  }
+
+  std::shared_ptr<rclpy::Service>
+  get_srv_get_available_transitions()
+  {
+    return srv_get_available_transitions_;
+  }
+
+  std::shared_ptr<rclpy::Service>
+  get_srv_get_transition_graph()
+  {
+    return srv_get_transition_graph_;
+  }
+
+private:
+  rclpy::Node node_;
+  std::shared_ptr<rclpy::Service> srv_change_state_;
+  std::shared_ptr<rclpy::Service> srv_get_state_;
+  std::shared_ptr<rclpy::Service> srv_get_available_states_;
+  std::shared_ptr<rclpy::Service> srv_get_available_transitions_;
+  std::shared_ptr<rclpy::Service> srv_get_transition_graph_;
+  std::shared_ptr<rcl_lifecycle_state_machine_t> state_machine_;
+};
+
+enum class TransitionCallbackReturnType
+{
+  Success = lifecycle_msgs__msg__Transition__TRANSITION_CALLBACK_SUCCESS,
+  Failure = lifecycle_msgs__msg__Transition__TRANSITION_CALLBACK_FAILURE,
+  Error = lifecycle_msgs__msg__Transition__TRANSITION_CALLBACK_ERROR,
+};
+
+std::string
+convert_callback_ret_code_to_label(TransitionCallbackReturnType cb_ret)
+{
+  if (cb_ret == TransitionCallbackReturnType::Success) {
+    return rcl_lifecycle_transition_success_label;
+  }
+  if (cb_ret == TransitionCallbackReturnType::Failure) {
+    return rcl_lifecycle_transition_failure_label;
+  }
+  if (cb_ret == TransitionCallbackReturnType::Error) {
+    return rcl_lifecycle_transition_error_label;
+  }
+  throw std::runtime_error{"Invalid transition callback return type"};
+}
+
+}  // namespace
+
+namespace rclpy
+{
+void
+define_lifecycle_api(py::module m)
+{
+  py::class_<LifecycleStateMachine, Destroyable, std::shared_ptr<LifecycleStateMachine>>(
+    m, "LifecycleStateMachine")
+  .def(py::init<Node &, bool>())
+  .def_property_readonly(
+    "initialized", &LifecycleStateMachine::is_initialized,
+    "Check if state machine is initialized.")
+  .def_property_readonly(
+    "current_state", &LifecycleStateMachine::get_current_state,
+    "Get the current state machine state.")
+  .def_property_readonly(
+    "available_states", &LifecycleStateMachine::get_available_states,
+    "Get the available states.")
+  .def_property_readonly(
+    "available_transitions", &LifecycleStateMachine::get_available_transitions,
+    "Get the available transitions.")
+  .def_property_readonly(
+    "transition_graph", &LifecycleStateMachine::get_transition_graph,
+    "Get the transition graph.")
+  .def(
+    "get_transition_by_label", &LifecycleStateMachine::get_transition_by_label,
+    "Get the transition id from a transition label.")
+  .def(
+    "trigger_transition_by_id", &LifecycleStateMachine::trigger_transition_by_id,
+    "Trigger a transition by transition id.")
+  .def(
+    "trigger_transition_by_label", &LifecycleStateMachine::trigger_transition_by_label,
+    "Trigger a transition by label.")
+  .def_property_readonly(
+    "service_change_state", &LifecycleStateMachine::get_srv_change_state,
+    "Get the change state service.")
+  .def_property_readonly(
+    "service_get_state", &LifecycleStateMachine::get_srv_get_state,
+    "Get the get state service.")
+  .def_property_readonly(
+    "service_get_available_states", &LifecycleStateMachine::get_srv_get_available_states,
+    "Get the get available states service.")
+  .def_property_readonly(
+    "service_get_available_transitions", &LifecycleStateMachine::get_srv_get_available_transitions,
+    "Get the get available transitions service.")
+  .def_property_readonly(
+    "service_get_transition_graph", &LifecycleStateMachine::get_srv_get_transition_graph,
+    "Get the get transition graph service.");
+  py::enum_<TransitionCallbackReturnType>(m, "TransitionCallbackReturnType")
+  .value(
+    "SUCCESS", TransitionCallbackReturnType::Success,
+    "Callback succeeded.")
+  .value(
+    "FAILURE", TransitionCallbackReturnType::Failure,
+    "Callback failed.")
+  .value(
+    "ERROR", TransitionCallbackReturnType::Error,
+    "Callback had an error.")
+  .def(
+    "to_label", &convert_callback_ret_code_to_label,
+    "Convert the transition callback return code to a transition label");
+}
+}  // namespace rclpy

rclpy/src/rclpy/lifecycle.hpp

@@ -0,0 +1,31 @@
+// 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.
+
+#ifndef RCLPY__LIFECYCLE_HPP_
+#define RCLPY__LIFECYCLE_HPP_
+
+#include <pybind11/pybind11.h>
+
+namespace py = pybind11;
+
+namespace rclpy
+{
+/// Define methods on a module for the lifecycle API
+/**
+ * \param[in] module a pybind11 module to add the definition to
+ */
+void
+define_lifecycle_api(py::module module);
+}  // namespace rclpy
+#endif  // RCLPY__LIFECYCLE_HPP_

rclpy/src/rclpy/service.cpp

@@ -22,6 +22,7 @@
 
 #include "exceptions.hpp"
 #include "service.hpp"
+#include "utils.hpp"
 
 namespace rclpy
 {
@@ -86,6 +87,11 @@ Service::Service(
   }
 }
 
+Service::Service(
+  Node & node, std::shared_ptr<rcl_service_t> rcl_service)
+: node_(node), rcl_service_(rcl_service)
+{}
+
 void
 Service::service_send_response(py::object pyresponse, rmw_request_id_t * header)
 {
@@ -104,7 +110,6 @@ py::tuple
 Service::service_take_request(py::object pyrequest_type)
 {
   auto taken_request = create_from_py(pyrequest_type);
-
   rmw_service_info_t header;
 
   py::tuple result_tuple(2);
@@ -122,6 +127,20 @@ Service::service_take_request(py::object pyrequest_type)
 
   return result_tuple;
 }
+
+const char *
+Service::get_service_name()
+{
+  return rcl_service_get_service_name(rcl_service_.get());
+}
+
+py::dict
+Service::get_qos_profile()
+{
+  const auto * options = rcl_service_get_options(rcl_service_.get());
+  return rclpy::convert_to_qos_dict(&options->qos);
+}
+
 void
 define_service(py::object module)
 {
@@ -132,6 +151,12 @@ define_service(py::object module)
       return reinterpret_cast<size_t>(service.rcl_ptr());
     },
     "Get the address of the entity as an integer")
+  .def_property_readonly(
+    "name", &Service::get_service_name,
+    "Get the name of the service")
+  .def_property_readonly(
+    "qos", &Service::get_qos_profile,
+    "Get the qos profile of the service")
   .def(
     "service_send_response", &Service::service_send_response,
     "Send a response")

rclpy/src/rclpy/service.hpp

@@ -55,6 +55,9 @@ class Service : public Destroyable, public std::enable_shared_from_this<Service>
     Node & node, py::object pysrv_type, std::string service_name,
     py::object pyqos_profile);
 
+  Service(
+    Node & node, std::shared_ptr<rcl_service_t> rcl_service);
+
   ~Service() = default;
 
   /// Publish a response message
@@ -88,6 +91,12 @@ class Service : public Destroyable, public std::enable_shared_from_this<Service>
     return rcl_service_.get();
   }
 
+  const char *
+  get_service_name();
+
+  py::dict
+  get_qos_profile();
+
   /// Force an early destruction of this object
   void
   destroy() override;

rclpy/test/test_lifecycle.py

@@ -0,0 +1,207 @@
+# 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 threading import Thread
+from unittest import mock
+
+import lifecycle_msgs.msg
+import lifecycle_msgs.srv
+
+import pytest
+
+import rclpy
+from rclpy.executors import SingleThreadedExecutor
+from rclpy.impl.implementation_singleton import rclpy_implementation as _rclpy
+from rclpy.lifecycle import LifecycleNode
+from rclpy.lifecycle import TransitionCallbackReturn
+from rclpy.node import Node
+from rclpy.publisher import Publisher
+
+from test_msgs.msg import BasicTypes
+
+
+def test_lifecycle_node_init():
+    rclpy.init()
+    node = LifecycleNode('test_lifecycle_node_init_1')
+    assert node
+    node.destroy_node()
+    node = LifecycleNode('test_lifecycle_node_init_2', enable_communication_interface=False)
+    assert node
+    # Make sure services were not created
+    assert not hasattr(node, '_service_change_state')
+    assert not hasattr(node, '_service_get_state')
+    assert not hasattr(node, '_service_get_available_states')
+    assert not hasattr(node, '_service_get_available_transitions')
+    assert not hasattr(node, '_service_get_transition_graph')
+    # Make sure also that the services were not created in the pybind11 plugin
+    assert not node._state_machine.service_change_state
+    assert not node._state_machine.service_get_state
+    assert not node._state_machine.service_get_available_states
+    assert not node._state_machine.service_get_available_transitions
+    assert not node._state_machine.service_get_transition_graph
+    node.destroy_node()
+    with pytest.raises(TypeError):
+        LifecycleNode('test_lifecycle_node_init_3', enable_communication_interface='asd')
+
+
+def test_lifecycle_state_transitions():
+    node = LifecycleNode(
+        'test_lifecycle_state_transitions_1', enable_communication_interface=False)
+    # normal transitions
+    assert node.trigger_configure() == TransitionCallbackReturn.SUCCESS
+    # test many times back to back, to make sure it works robustly
+    for _ in range(10):
+        assert node.trigger_cleanup() == TransitionCallbackReturn.SUCCESS
+        assert node.trigger_configure() == TransitionCallbackReturn.SUCCESS
+    for _ in range(10):
+        assert node.trigger_activate() == TransitionCallbackReturn.SUCCESS
+        assert node.trigger_deactivate() == TransitionCallbackReturn.SUCCESS
+    assert node.trigger_cleanup() == TransitionCallbackReturn.SUCCESS
+    # some that are not possible from the current state
+    with pytest.raises(_rclpy.RCLError):
+        node.trigger_activate()
+    with pytest.raises(_rclpy.RCLError):
+        node.trigger_deactivate()
+    assert node.trigger_shutdown() == TransitionCallbackReturn.SUCCESS
+    with pytest.raises(_rclpy.RCLError):
+        node.trigger_shutdown()
+    node.destroy_node()
+    # Again but trigger shutdown from 'inactive' instead of 'unconfigured'
+    node = LifecycleNode(
+        'test_lifecycle_state_transitions_2', enable_communication_interface=False)
+    assert node.trigger_shutdown() == TransitionCallbackReturn.SUCCESS
+    # Again but trigger shutdown from 'active'
+    node = LifecycleNode(
+        'test_lifecycle_state_transitions_3', enable_communication_interface=False)
+    assert node.trigger_configure() == TransitionCallbackReturn.SUCCESS
+    assert node.trigger_activate() == TransitionCallbackReturn.SUCCESS
+    assert node.trigger_shutdown() == TransitionCallbackReturn.SUCCESS
+
+    class ErrorOnConfigureHandledCorrectlyNode(LifecycleNode):
+
+        def on_configure(self):
+            return TransitionCallbackReturn.ERROR
+
+        def __init__(self, *args, **kwargs):
+            super().__init__(*args, **kwargs)
+
+    node = ErrorOnConfigureHandledCorrectlyNode(
+        'test_lifecycle_state_transitions_2', enable_communication_interface=False)
+    assert node._state_machine.current_state[1] == 'unconfigured'
+    assert node.trigger_configure() == TransitionCallbackReturn.ERROR
+    assert node._state_machine.current_state[1] == 'unconfigured'
+
+    class ErrorOnConfigureHandledInCorrectlyNode(LifecycleNode):
+
+        def on_configure(self):
+            return TransitionCallbackReturn.ERROR
+
+        def on_error(self):
+            return TransitionCallbackReturn.ERROR
+
+        def __init__(self, *args, **kwargs):
+            super().__init__(*args, **kwargs)
+
+    node = ErrorOnConfigureHandledInCorrectlyNode(
+        'test_lifecycle_state_transitions_3', enable_communication_interface=False)
+    assert node.trigger_configure() == TransitionCallbackReturn.ERROR
+    assert node._state_machine.current_state[1] == 'finalized'
+
+
+def test_lifecycle_services(request):
+    lc_node_name = 'test_lifecycle_services_lifecycle'
+    lc_node = LifecycleNode(lc_node_name)
+    client_node = Node('test_lifecycle_services_client')
+    get_state_cli = client_node.create_client(
+        lifecycle_msgs.srv.GetState,
+        f'/{lc_node_name}/get_state')
+    change_state_cli = client_node.create_client(
+        lifecycle_msgs.srv.ChangeState,
+        f'/{lc_node_name}/change_state')
+    get_available_states_cli = client_node.create_client(
+        lifecycle_msgs.srv.GetAvailableStates,
+        f'/{lc_node_name}/get_available_states')
+    get_available_transitions_cli = client_node.create_client(
+        lifecycle_msgs.srv.GetAvailableTransitions,
+        f'/{lc_node_name}/get_available_transitions')
+    get_transition_graph_cli = client_node.create_client(
+        lifecycle_msgs.srv.GetAvailableTransitions,
+        f'/{lc_node_name}/get_transition_graph')
+    for cli in (
+        get_state_cli,
+        change_state_cli,
+        get_available_states_cli,
+        get_available_transitions_cli,
+        get_transition_graph_cli,
+    ):
+        assert cli.wait_for_service(5.)
+    # lunch a thread to spin the executor, so we can make sync service calls easily
+    executor = SingleThreadedExecutor()
+    executor.add_node(client_node)
+    executor.add_node(lc_node)
+    thread = Thread(target=executor.spin)
+    thread.start()
+
+    def cleanup():
+        # Stop executor and join thread.
+        # This cleanup is run even if an assertion fails.
+        executor.shutdown()
+        thread.join()
+    request.addfinalizer(cleanup)
+
+    # test all services
+    req = lifecycle_msgs.srv.GetState.Request()
+    resp = get_state_cli.call(req)
+    assert resp.current_state.label == 'unconfigured'
+    req = lifecycle_msgs.srv.ChangeState.Request()
+    req.transition.id = lifecycle_msgs.msg.Transition.TRANSITION_CONFIGURE
+    resp = change_state_cli.call(req)
+    assert resp.success
+    req = lifecycle_msgs.srv.GetState.Request()
+    resp = get_state_cli.call(req)
+    assert resp.current_state.label == 'inactive'
+    req = lifecycle_msgs.srv.GetAvailableStates.Request()
+    resp = get_available_states_cli.call(req)
+    states_labels = {state.label for state in resp.available_states}
+    assert states_labels == {
+        'unknown', 'unconfigured', 'inactive', 'active', 'finalized', 'configuring', 'cleaningup',
+        'shuttingdown', 'activating', 'deactivating', 'errorprocessing', ''
+    }
+    req = lifecycle_msgs.srv.GetAvailableTransitions.Request()
+    resp = get_available_transitions_cli.call(req)
+    transitions_labels = {
+        transition_def.transition.label for transition_def in resp.available_transitions}
+    assert transitions_labels == {'activate', 'cleanup', 'shutdown'}
+    req = lifecycle_msgs.srv.GetAvailableTransitions.Request()
+    resp = get_transition_graph_cli.call(req)
+    transitions_labels = {
+        transition_def.transition.label for transition_def in resp.available_transitions}
+    assert transitions_labels == {
+        'configure', 'activate', 'cleanup', 'shutdown', 'deactivate', 'transition_error',
+        'transition_failure', 'transition_success'
+    }
+
+
+def test_lifecycle_publisher():
+    node = LifecycleNode('test_lifecycle_publisher', enable_communication_interface=False)
+    with mock.patch.object(Publisher, 'publish') as mock_publish:
+        pub = node.create_lifecycle_publisher(BasicTypes, 'test_lifecycle_publisher_topic', 10)
+        pub.publish(BasicTypes())
+        mock_publish.assert_not_called()
+        assert node.trigger_configure() == TransitionCallbackReturn.SUCCESS
+        node.trigger_activate()
+        msg = BasicTypes()
+        pub.publish(msg)
+        mock_publish.assert_called()
+        mock_publish.assert_called_with(pub, msg)