Deferrable + Cancelable lifecycle change_state transition function implementation

rclcpp_lifecycle/CMakeLists.txt

@@ -22,8 +22,12 @@ find_package(rosidl_typesupport_cpp REQUIRED)
 
 ### CPP High level library
 add_library(rclcpp_lifecycle
+  src/change_state_handler_impl.cpp
   src/lifecycle_node.cpp
+  src/lifecycle_node_entities_manager.cpp
   src/lifecycle_node_interface_impl.cpp
+  src/lifecycle_node_state_manager.cpp
+  src/lifecycle_node_state_services_manager.cpp
   src/managed_entity.cpp
   src/node_interfaces/lifecycle_node_interface.cpp
   src/state.cpp
@@ -115,6 +119,14 @@ if(BUILD_TESTING)
       ${rcl_interfaces_TARGETS}
       rclcpp::rclcpp)
   endif()
+  ament_add_gtest(test_lifecycle_async_transitions test/test_lifecycle_async_transitions.cpp TIMEOUT 120)
+  if(TARGET test_lifecycle_async_transitions)
+    target_link_libraries(test_lifecycle_async_transitions
+      ${PROJECT_NAME}
+      mimick
+      ${rcl_interfaces_TARGETS}
+      rclcpp::rclcpp)
+  endif()
   ament_add_gtest(test_service test/test_service.cpp TIMEOUT 120)
   if(TARGET test_service)
     target_link_libraries(test_service

rclcpp_lifecycle/include/rclcpp_lifecycle/change_state_handler.hpp

@@ -0,0 +1,60 @@
+// Copyright 2023 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 RCLCPP_LIFECYCLE__CHANGE_STATE_HANDLER_HPP_
+#define RCLCPP_LIFECYCLE__CHANGE_STATE_HANDLER_HPP_
+
+#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
+
+namespace rclcpp_lifecycle
+{
+/// The object passed to asynchronous change_state user transition functions
+class ChangeStateHandler
+{
+public:
+  /// Continues the change state process handling proper callback order
+  /** Used within the user defined transition callback to continue the change state process
+   *  similar to a service call response
+   *  Note this only allows sending a single response callback per object
+   *  and will not send further responses if called mutiple times on the object
+   * \param[in] cb_return_code result of user defined transition callback
+   * \return true if the response was successfully sent
+   */
+  virtual bool send_callback_resp(
+    node_interfaces::LifecycleNodeInterface::CallbackReturn cb_return_code) = 0;
+
+  /// Updates the state machine based on the handling of a cancelled transition
+  /**
+   * \param[in] success true if the transition cancel request was successfully handled
+   * \return true if the response was successfully sent to the state handler
+  */
+  virtual bool handle_canceled(bool success) = 0;
+
+  /// Check to see if a send_callback_resp has been cancelled
+  /**
+   * @return true if response has been cancelled
+   */
+  virtual bool is_canceling() const = 0;
+
+  // Check to see if the response has been sent
+  /**
+   * @return true if response has not been sent
+   */
+  virtual bool is_executing() const = 0;
+
+  virtual ~ChangeStateHandler() = default;
+};
+}  // namespace rclcpp_lifecycle
+
+#endif  // RCLCPP_LIFECYCLE__CHANGE_STATE_HANDLER_HPP_

rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp

@@ -90,6 +90,7 @@
 #include "rclcpp_lifecycle/state.hpp"
 #include "rclcpp_lifecycle/transition.hpp"
 #include "rclcpp_lifecycle/visibility_control.h"
+#include "rclcpp_lifecycle/change_state_handler.hpp"
 
 namespace rclcpp_lifecycle
 {
@@ -1004,7 +1005,19 @@ class LifecycleNode : public node_interfaces::LifecycleNodeInterface,
    */
   RCLCPP_LIFECYCLE_PUBLIC
   bool
-  register_on_configure(std::function<LifecycleNodeInterface::CallbackReturn(const State &)> fcn);
+  register_on_configure(
+    std::function<LifecycleNodeInterface::CallbackReturn(const State &)> fcn);
+
+  /// Register the configure async callback
+  /**
+   * This callback will be called when the transition to this state is triggered
+   * \param[in] fcn callback function to call
+   * \return always true
+   */
+  RCLCPP_LIFECYCLE_PUBLIC
+  bool
+  register_async_on_configure(
+    std::function<void(const State &, std::shared_ptr<ChangeStateHandler>)> fcn);
 
   /// Register the cleanup callback
   /**
@@ -1014,7 +1027,19 @@ class LifecycleNode : public node_interfaces::LifecycleNodeInterface,
    */
   RCLCPP_LIFECYCLE_PUBLIC
   bool
-  register_on_cleanup(std::function<LifecycleNodeInterface::CallbackReturn(const State &)> fcn);
+  register_on_cleanup(
+    std::function<LifecycleNodeInterface::CallbackReturn(const State &)> fcn);
+
+  /// Register the cleanup async callback
+  /**
+   * This callback will be called when the transition to this state is triggered
+   * \param[in] fcn callback function to call
+   * \return always true
+   */
+  RCLCPP_LIFECYCLE_PUBLIC
+  bool
+  register_async_on_cleanup(
+    std::function<void(const State &, std::shared_ptr<ChangeStateHandler>)> fcn);
 
   /// Register the shutdown callback
   /**
@@ -1024,7 +1049,19 @@ class LifecycleNode : public node_interfaces::LifecycleNodeInterface,
    */
   RCLCPP_LIFECYCLE_PUBLIC
   bool
-  register_on_shutdown(std::function<LifecycleNodeInterface::CallbackReturn(const State &)> fcn);
+  register_on_shutdown(
+    std::function<LifecycleNodeInterface::CallbackReturn(const State &)> fcn);
+
+  /// Register the shutdown async callback
+  /**
+   * This callback will be called when the transition to this state is triggered
+   * \param[in] fcn callback function to call
+   * \return always true
+   */
+  RCLCPP_LIFECYCLE_PUBLIC
+  bool
+  register_async_on_shutdown(
+    std::function<void(const State &, std::shared_ptr<ChangeStateHandler>)> fcn);
 
   /// Register the activate callback
   /**
@@ -1034,7 +1071,19 @@ class LifecycleNode : public node_interfaces::LifecycleNodeInterface,
    */
   RCLCPP_LIFECYCLE_PUBLIC
   bool
-  register_on_activate(std::function<LifecycleNodeInterface::CallbackReturn(const State &)> fcn);
+  register_on_activate(
+    std::function<LifecycleNodeInterface::CallbackReturn(const State &)> fcn);
+
+  /// Register the activate async callback
+  /**
+   * This callback will be called when the transition to this state is triggered
+   * \param[in] fcn callback function to call
+   * \return always true
+   */
+  RCLCPP_LIFECYCLE_PUBLIC
+  bool
+  register_async_on_activate(
+    std::function<void(const State &, std::shared_ptr<ChangeStateHandler>)> fcn);
 
   /// Register the deactivate callback
   /**
@@ -1044,7 +1093,19 @@ class LifecycleNode : public node_interfaces::LifecycleNodeInterface,
    */
   RCLCPP_LIFECYCLE_PUBLIC
   bool
-  register_on_deactivate(std::function<LifecycleNodeInterface::CallbackReturn(const State &)> fcn);
+  register_on_deactivate(
+    std::function<LifecycleNodeInterface::CallbackReturn(const State &)> fcn);
+
+  /// Register the deactivate async callback
+  /**
+   * This callback will be called when the transition to this state is triggered
+   * \param[in] fcn callback function to call
+   * \return always true
+   */
+  RCLCPP_LIFECYCLE_PUBLIC
+  bool
+  register_async_on_deactivate(
+    std::function<void(const State &, std::shared_ptr<ChangeStateHandler>)> fcn);
 
   /// Register the error callback
   /**
@@ -1054,7 +1115,19 @@ class LifecycleNode : public node_interfaces::LifecycleNodeInterface,
    */
   RCLCPP_LIFECYCLE_PUBLIC
   bool
-  register_on_error(std::function<LifecycleNodeInterface::CallbackReturn(const State &)> fcn);
+  register_on_error(
+    std::function<LifecycleNodeInterface::CallbackReturn(const State &)> fcn);
+
+  /// Register the error async callback
+  /**
+   * This callback will be called when the transition to this state is triggered
+   * \param[in] fcn callback function to call
+   * \return always true
+   */
+  RCLCPP_LIFECYCLE_PUBLIC
+  bool
+  register_async_on_error(
+    std::function<void(const State &, std::shared_ptr<ChangeStateHandler>)> fcn);
 
   RCLCPP_LIFECYCLE_PUBLIC
   CallbackReturn

rclcpp_lifecycle/src/change_state_handler_impl.cpp

@@ -0,0 +1,81 @@
+// Copyright 2023 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 <memory>
+
+#include "change_state_handler_impl.hpp"
+
+namespace rclcpp_lifecycle
+{
+
+ChangeStateHandlerImpl::ChangeStateHandlerImpl(
+  const std::weak_ptr<LifecycleNodeStateManager> state_manager_hdl)
+: state_manager_hdl_(state_manager_hdl)
+{
+}
+
+bool
+ChangeStateHandlerImpl::send_callback_resp(
+  node_interfaces::LifecycleNodeInterface::CallbackReturn cb_return_code)
+{
+  if (!is_canceling() && is_executing()) {
+    auto state_manager_hdl = state_manager_hdl_.lock();
+    if (state_manager_hdl) {
+      response_sent_.store(true);
+      state_manager_hdl->process_callback_resp(cb_return_code);
+      return true;
+    }
+  }
+  return false;
+}
+
+bool
+ChangeStateHandlerImpl::handle_canceled(bool success)
+{
+  if (is_canceling() && is_executing()) {
+    auto state_manager_hdl = state_manager_hdl_.lock();
+    if (state_manager_hdl) {
+      response_sent_.store(true);
+      state_manager_hdl->user_handled_transition_cancel(success);
+      return true;
+    }
+  }
+  return false;
+}
+
+bool
+ChangeStateHandlerImpl::is_canceling() const
+{
+  return transition_is_cancelled_.load();
+}
+
+bool
+ChangeStateHandlerImpl::is_executing() const
+{
+  return !response_sent_.load();
+}
+
+void
+ChangeStateHandlerImpl::cancel_transition()
+{
+  transition_is_cancelled_.store(true);
+}
+
+void
+ChangeStateHandlerImpl::invalidate()
+{
+  response_sent_.store(true);
+}
+
+}  // namespace rclcpp_lifecycle

rclcpp_lifecycle/src/change_state_handler_impl.hpp

@@ -0,0 +1,59 @@
+// Copyright 2023 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 CHANGE_STATE_HANDLER_IMPL_HPP_
+#define CHANGE_STATE_HANDLER_IMPL_HPP_
+
+#include <memory>
+#include <atomic>
+
+#include "rclcpp_lifecycle/change_state_handler.hpp"
+#include "rclcpp/node_interfaces/node_base_interface.hpp"
+#include "lifecycle_node_state_manager.hpp"
+
+namespace rclcpp_lifecycle
+{
+
+class ChangeStateHandlerImpl : public ChangeStateHandler
+{
+public:
+  explicit ChangeStateHandlerImpl(const std::weak_ptr<LifecycleNodeStateManager> state_manager_hdl);
+
+  bool send_callback_resp(
+    node_interfaces::LifecycleNodeInterface::CallbackReturn cb_return_code) override;
+
+  bool handle_canceled(bool success) override;
+
+  bool is_canceling() const override;
+
+  bool is_executing() const override;
+
+  /**
+   * @brief Marks this transition as cancelled. It is up to the user to check if the transition
+   *       has been cancelled and attempt to handle it.
+   */
+  void cancel_transition();
+
+  /**
+   * @brief Invalidate the handler by setting the response_sent_ flag to true
+   */
+  void invalidate();
+
+private:
+  std::weak_ptr<LifecycleNodeStateManager> state_manager_hdl_;
+  std::atomic<bool> response_sent_{false};
+  std::atomic<bool> transition_is_cancelled_{false};
+};
+}  // namespace rclcpp_lifecycle
+#endif  // CHANGE_STATE_HANDLER_IMPL_HPP_