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_