From 4dd441020fcfda862c12180985e6b668b56ae1bd Mon Sep 17 00:00:00 2001 From: Thomas Groechel Date: Mon, 12 Jun 2023 17:57:38 -0500 Subject: [PATCH] Sq: Async+Cancelable Lifecycle Transitions Signed-off-by: Tom Groechel --- rclcpp_lifecycle/CMakeLists.txt | 9 + .../rclcpp_lifecycle/change_state_handler.hpp | 60 ++ .../rclcpp_lifecycle/lifecycle_node.hpp | 67 ++ .../src/change_state_handler_impl.cpp | 81 +++ .../src/change_state_handler_impl.hpp | 59 ++ rclcpp_lifecycle/src/lifecycle_node.cpp | 47 ++ .../src/lifecycle_node_interface_impl.cpp | 9 + .../src/lifecycle_node_interface_impl.hpp | 5 + .../src/lifecycle_node_state_manager.cpp | 169 ++++- .../src/lifecycle_node_state_manager.hpp | 39 ++ .../lifecycle_node_state_services_manager.cpp | 52 ++ .../lifecycle_node_state_services_manager.hpp | 15 + .../test/test_lifecycle_async_transitions.cpp | 588 ++++++++++++++++++ .../test/test_lifecycle_service_client.cpp | 32 +- .../test/test_register_custom_callbacks.cpp | 111 ++++ 15 files changed, 1334 insertions(+), 9 deletions(-) create mode 100644 rclcpp_lifecycle/include/rclcpp_lifecycle/change_state_handler.hpp create mode 100644 rclcpp_lifecycle/src/change_state_handler_impl.cpp create mode 100644 rclcpp_lifecycle/src/change_state_handler_impl.hpp create mode 100644 rclcpp_lifecycle/test/test_lifecycle_async_transitions.cpp diff --git a/rclcpp_lifecycle/CMakeLists.txt b/rclcpp_lifecycle/CMakeLists.txt index 4ffff3cab8..3bbde37682 100644 --- a/rclcpp_lifecycle/CMakeLists.txt +++ b/rclcpp_lifecycle/CMakeLists.txt @@ -22,6 +22,7 @@ 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 @@ -118,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 diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/change_state_handler.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/change_state_handler.hpp new file mode 100644 index 0000000000..f3f4df1d39 --- /dev/null +++ b/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_ diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp index 3b1b175b5a..1185350d11 100644 --- a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp +++ b/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 { @@ -1007,6 +1008,17 @@ class LifecycleNode : public node_interfaces::LifecycleNodeInterface, register_on_configure( std::function 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)> fcn); + /// Register the cleanup callback /** * This callback will be called when the transition to this state is triggered @@ -1018,6 +1030,17 @@ class LifecycleNode : public node_interfaces::LifecycleNodeInterface, register_on_cleanup( std::function 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)> fcn); + /// Register the shutdown callback /** * This callback will be called when the transition to this state is triggered @@ -1029,6 +1052,17 @@ class LifecycleNode : public node_interfaces::LifecycleNodeInterface, register_on_shutdown( std::function 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)> fcn); + /// Register the activate callback /** * This callback will be called when the transition to this state is triggered @@ -1040,6 +1074,17 @@ class LifecycleNode : public node_interfaces::LifecycleNodeInterface, register_on_activate( std::function 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)> fcn); + /// Register the deactivate callback /** * This callback will be called when the transition to this state is triggered @@ -1051,6 +1096,17 @@ class LifecycleNode : public node_interfaces::LifecycleNodeInterface, register_on_deactivate( std::function 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)> fcn); + /// Register the error callback /** * This callback will be called when the transition to this state is triggered @@ -1062,6 +1118,17 @@ class LifecycleNode : public node_interfaces::LifecycleNodeInterface, register_on_error( std::function 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)> fcn); + RCLCPP_LIFECYCLE_PUBLIC CallbackReturn on_activate(const State & previous_state) override; diff --git a/rclcpp_lifecycle/src/change_state_handler_impl.cpp b/rclcpp_lifecycle/src/change_state_handler_impl.cpp new file mode 100644 index 0000000000..b7bb7074cc --- /dev/null +++ b/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 + +#include "change_state_handler_impl.hpp" + +namespace rclcpp_lifecycle +{ + +ChangeStateHandlerImpl::ChangeStateHandlerImpl( + const std::weak_ptr 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 diff --git a/rclcpp_lifecycle/src/change_state_handler_impl.hpp b/rclcpp_lifecycle/src/change_state_handler_impl.hpp new file mode 100644 index 0000000000..add906a0d0 --- /dev/null +++ b/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 +#include + +#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 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 state_manager_hdl_; + std::atomic response_sent_{false}; + std::atomic transition_is_cancelled_{false}; +}; +} // namespace rclcpp_lifecycle +#endif // CHANGE_STATE_HANDLER_IMPL_HPP_ diff --git a/rclcpp_lifecycle/src/lifecycle_node.cpp b/rclcpp_lifecycle/src/lifecycle_node.cpp index 041d13bfa8..0bac06749e 100644 --- a/rclcpp_lifecycle/src/lifecycle_node.cpp +++ b/rclcpp_lifecycle/src/lifecycle_node.cpp @@ -501,6 +501,14 @@ LifecycleNode::register_on_configure( lifecycle_msgs::msg::State::TRANSITION_STATE_CONFIGURING, fcn); } +bool +LifecycleNode::register_async_on_configure( + std::function)> fcn) +{ + return impl_->register_async_callback( + lifecycle_msgs::msg::State::TRANSITION_STATE_CONFIGURING, fcn); +} + bool LifecycleNode::register_on_cleanup( std::function fcn) @@ -509,6 +517,14 @@ LifecycleNode::register_on_cleanup( lifecycle_msgs::msg::State::TRANSITION_STATE_CLEANINGUP, fcn); } +bool +LifecycleNode::register_async_on_cleanup( + std::function)> fcn) +{ + return impl_->register_async_callback( + lifecycle_msgs::msg::State::TRANSITION_STATE_CLEANINGUP, fcn); +} + bool LifecycleNode::register_on_shutdown( std::function fcn) @@ -517,6 +533,14 @@ LifecycleNode::register_on_shutdown( lifecycle_msgs::msg::State::TRANSITION_STATE_SHUTTINGDOWN, fcn); } +bool +LifecycleNode::register_async_on_shutdown( + std::function)> fcn) +{ + return impl_->register_async_callback( + lifecycle_msgs::msg::State::TRANSITION_STATE_SHUTTINGDOWN, fcn); +} + bool LifecycleNode::register_on_activate( std::function fcn) @@ -525,6 +549,14 @@ LifecycleNode::register_on_activate( lifecycle_msgs::msg::State::TRANSITION_STATE_ACTIVATING, fcn); } +bool +LifecycleNode::register_async_on_activate( + std::function)> fcn) +{ + return impl_->register_async_callback( + lifecycle_msgs::msg::State::TRANSITION_STATE_ACTIVATING, fcn); +} + bool LifecycleNode::register_on_deactivate( std::function fcn) @@ -533,6 +565,14 @@ LifecycleNode::register_on_deactivate( lifecycle_msgs::msg::State::TRANSITION_STATE_DEACTIVATING, fcn); } +bool +LifecycleNode::register_async_on_deactivate( + std::function)> fcn) +{ + return impl_->register_async_callback( + lifecycle_msgs::msg::State::TRANSITION_STATE_DEACTIVATING, fcn); +} + bool LifecycleNode::register_on_error( std::function fcn) @@ -541,6 +581,13 @@ LifecycleNode::register_on_error( lifecycle_msgs::msg::State::TRANSITION_STATE_ERRORPROCESSING, fcn); } +bool +LifecycleNode::register_async_on_error( + std::function)> fcn) +{ + return impl_->register_async_callback( + lifecycle_msgs::msg::State::TRANSITION_STATE_ERRORPROCESSING, fcn); +} const State & LifecycleNode::get_current_state() const diff --git a/rclcpp_lifecycle/src/lifecycle_node_interface_impl.cpp b/rclcpp_lifecycle/src/lifecycle_node_interface_impl.cpp index 49e8d8c568..9f815ad929 100644 --- a/rclcpp_lifecycle/src/lifecycle_node_interface_impl.cpp +++ b/rclcpp_lifecycle/src/lifecycle_node_interface_impl.cpp @@ -156,6 +156,15 @@ LifecycleNode::LifecycleNodeInterfaceImpl::trigger_transition( return get_current_state(); } +bool +LifecycleNode::LifecycleNodeInterfaceImpl::register_async_callback( + std::uint8_t lifecycle_transition, + std::function)> & cb) +{ + return state_manager_hdl_->register_async_callback(lifecycle_transition, cb); +} + + void LifecycleNode::LifecycleNodeInterfaceImpl::add_managed_entity( std::weak_ptr managed_entity) diff --git a/rclcpp_lifecycle/src/lifecycle_node_interface_impl.hpp b/rclcpp_lifecycle/src/lifecycle_node_interface_impl.hpp index eb01ce4728..fdbef7701e 100644 --- a/rclcpp_lifecycle/src/lifecycle_node_interface_impl.hpp +++ b/rclcpp_lifecycle/src/lifecycle_node_interface_impl.hpp @@ -62,6 +62,11 @@ class LifecycleNode::LifecycleNodeInterfaceImpl final std::uint8_t lifecycle_transition, std::function & cb); + bool + register_async_callback( + std::uint8_t lifecycle_transition, + std::function)> & cb); + const State & get_current_state() const; diff --git a/rclcpp_lifecycle/src/lifecycle_node_state_manager.cpp b/rclcpp_lifecycle/src/lifecycle_node_state_manager.cpp index ab394c556a..8c7967ba2d 100644 --- a/rclcpp_lifecycle/src/lifecycle_node_state_manager.cpp +++ b/rclcpp_lifecycle/src/lifecycle_node_state_manager.cpp @@ -18,6 +18,7 @@ #include "lifecycle_node_state_services_manager.hpp" #include "lifecycle_node_state_manager.hpp" +#include "change_state_handler_impl.hpp" namespace rclcpp_lifecycle { @@ -84,6 +85,23 @@ LifecycleNodeStateManager::register_callback( std::function & cb) { cb_map_[lifecycle_transition] = cb; + auto it = async_cb_map_.find(lifecycle_transition); + if (it != async_cb_map_.end()) { + async_cb_map_.erase(it); + } + return true; +} + +bool +LifecycleNodeStateManager::register_async_callback( + std::uint8_t lifecycle_transition, + std::function)> & cb) +{ + async_cb_map_[lifecycle_transition] = cb; + auto it = cb_map_.find(lifecycle_transition); + if (it != cb_map_.end()) { + cb_map_.erase(it); + } return true; } @@ -204,10 +222,14 @@ LifecycleNodeStateManager::change_state( update_current_state_(); } - cb_return_code_ = execute_callback( - current_state_id, - pre_transition_primary_state_); - process_callback_resp(cb_return_code_); + if (is_async_callback(current_state_id)) { + execute_async_callback(current_state_id, pre_transition_primary_state_); + } else { + cb_return_code_ = execute_callback( + current_state_id, + pre_transition_primary_state_); + process_callback_resp(cb_return_code_); + } return rcl_ret_; } @@ -216,6 +238,9 @@ void LifecycleNodeStateManager::process_callback_resp( node_interfaces::LifecycleNodeInterface::CallbackReturn cb_return_code) { + // we have received a response from the user callback so we can invalidate the handler + invalidate_change_state_handler(); + uint8_t current_state_id = get_current_state_id(); if (in_non_error_transition_state(current_state_id)) { if (transition_cb_completed_) { @@ -247,6 +272,61 @@ LifecycleNodeStateManager::process_callback_resp( } } +bool +LifecycleNodeStateManager::is_cancelling_transition() const +{ + return is_cancelling_transition_.load(); +} + +void +LifecycleNodeStateManager::cancel_transition( + std::function)> callback, + std::shared_ptr header) +{ + if (!is_transitioning()) { + if (callback) { + callback("Not in a transition, cannot cancel", false, header); + } + return; + } else if (is_cancelling_transition()) { + if (callback) { + callback("Already cancelling transition", false, header); + } + return; + } else if (!is_running_async_callback()) { + if (callback) { + callback("Not running async transition callback, cannot cancel", false, header); + } + return; + } + + is_cancelling_transition_.store(true); + send_cancel_transition_resp_cb_ = callback; + cancel_transition_header_ = header; + mark_transition_as_cancelled(); +} + +void +LifecycleNodeStateManager::user_handled_transition_cancel(bool success) +{ + if (!is_cancelling_transition()) { + RCUTILS_LOG_WARN("Received user handled cancel but not in a cancel transition"); + return; + } + + if (success) { + finalize_cancel_transition("", true); + // If the user successfully "unwound" the transition and handled the cancel + // successfully, we proceed as if the transition returned a FAILURE. + // This allows us to use the same logic as if the user returned a FAILURE + // which is often recovering to the prior primary state. + process_callback_resp( + node_interfaces::LifecycleNodeInterface::CallbackReturn::FAILURE); + } else { + finalize_cancel_transition("User handled cancel but did not succeed", false); + } +} + int LifecycleNodeStateManager::get_transition_id_from_request( const ChangeStateSrv::Request::SharedPtr req) @@ -321,10 +401,14 @@ LifecycleNodeStateManager::process_user_callback_resp( // TODO(karsten1987): iterate over possible ret value if (cb_return_code == node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR) { RCUTILS_LOG_WARN("Error occurred while calling transition function, calling on_error."); - auto error_cb_code = execute_callback( - current_state_id, - pre_transition_primary_state_); - process_callback_resp(error_cb_code); + if (is_async_callback(current_state_id)) { + execute_async_callback(current_state_id, pre_transition_primary_state_); + } else { + auto error_cb_code = execute_callback( + current_state_id, + pre_transition_primary_state_); + process_callback_resp(error_cb_code); + } } else { finalize_change_state( cb_return_code == node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS); @@ -389,6 +473,37 @@ LifecycleNodeStateManager::execute_callback( return cb_success; } + +bool +LifecycleNodeStateManager::is_async_callback( + unsigned int cb_id) const +{ + return async_cb_map_.find(static_cast(cb_id)) != async_cb_map_.end(); +} + +void +LifecycleNodeStateManager::execute_async_callback( + unsigned int cb_id, + const State & previous_state) +{ + auto it = async_cb_map_.find(static_cast(cb_id)); + if (it != async_cb_map_.end()) { + auto callback = it->second; + callback(State(previous_state), create_new_change_state_handler()); + } else { + // in case no callback was attached, we forward directly + process_callback_resp( + node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS); + } +} + +std::shared_ptr +LifecycleNodeStateManager::create_new_change_state_handler() +{ + change_state_hdl_ = std::make_shared(weak_from_this()); + return change_state_hdl_; +} + const char * LifecycleNodeStateManager::get_label_for_return_code( node_interfaces::LifecycleNodeInterface::CallbackReturn cb_return_code) @@ -440,6 +555,42 @@ LifecycleNodeStateManager::in_error_transition_state( return current_state_id == lifecycle_msgs::msg::State::TRANSITION_STATE_ERRORPROCESSING; } +bool +LifecycleNodeStateManager::is_running_async_callback() const +{ + return change_state_hdl_ && change_state_hdl_->is_executing(); +} + +void +LifecycleNodeStateManager::invalidate_change_state_handler() +{ + if (change_state_hdl_) { + change_state_hdl_->invalidate(); + change_state_hdl_.reset(); + } +} + +bool +LifecycleNodeStateManager::mark_transition_as_cancelled() +{ + if (change_state_hdl_) { + change_state_hdl_->cancel_transition(); + return true; + } + return false; +} + +void +LifecycleNodeStateManager::finalize_cancel_transition(const std::string & error_msg, bool success) +{ + if (send_cancel_transition_resp_cb_) { + send_cancel_transition_resp_cb_(error_msg, success, cancel_transition_header_); + cancel_transition_header_.reset(); + send_cancel_transition_resp_cb_ = nullptr; + } + is_cancelling_transition_.store(false); +} + LifecycleNodeStateManager::~LifecycleNodeStateManager() { rcl_node_t * node_handle = node_base_interface_->get_rcl_node_handle(); @@ -454,7 +605,9 @@ LifecycleNodeStateManager::~LifecycleNodeStateManager() "failed to destroy rcl_state_machine"); } send_change_state_resp_cb_ = nullptr; + send_cancel_transition_resp_cb_ = nullptr; change_state_header_.reset(); + cancel_transition_header_.reset(); node_base_interface_.reset(); } diff --git a/rclcpp_lifecycle/src/lifecycle_node_state_manager.hpp b/rclcpp_lifecycle/src/lifecycle_node_state_manager.hpp index 36304902f5..7918d0355b 100644 --- a/rclcpp_lifecycle/src/lifecycle_node_state_manager.hpp +++ b/rclcpp_lifecycle/src/lifecycle_node_state_manager.hpp @@ -27,6 +27,7 @@ #include "rclcpp_lifecycle/lifecycle_node.hpp" #include "lifecycle_msgs/msg/transition_event.hpp" +#include "lifecycle_msgs/srv/cancel_transition.hpp" #include "lifecycle_msgs/srv/change_state.hpp" #include "lifecycle_msgs/srv/get_state.hpp" #include "lifecycle_msgs/srv/get_available_states.hpp" @@ -50,6 +51,7 @@ class LifecycleNodeStateManager { public: using ChangeStateSrv = lifecycle_msgs::srv::ChangeState; + using CancelTransitionSrv = lifecycle_msgs::srv::CancelTransition; void init( const std::shared_ptr node_base_interface, @@ -63,6 +65,11 @@ class LifecycleNodeStateManager std::uint8_t lifecycle_transition, std::function & cb); + bool + register_async_callback( + std::uint8_t lifecycle_transition, + std::function)> & cb); + const State & get_current_state() const; std::vector get_available_states() const; @@ -85,6 +92,14 @@ class LifecycleNodeStateManager void process_callback_resp( node_interfaces::LifecycleNodeInterface::CallbackReturn cb_return_code); + bool is_cancelling_transition() const; + + void cancel_transition( + std::function)> callback = nullptr, + const std::shared_ptr header = nullptr); + + void user_handled_transition_cancel(bool success); + /** * @brief Gets the transition id prioritizing request->transition.label over * request->transition.id if the label is set @@ -110,6 +125,9 @@ class LifecycleNodeStateManager std::map< std::uint8_t, std::function> cb_map_; + std::map< + std::uint8_t, + std::function)>> async_cb_map_; /*ChangeState Members*/ std::shared_ptr change_state_hdl_; @@ -136,6 +154,12 @@ class LifecycleNodeStateManager node_interfaces::LifecycleNodeInterface::CallbackReturn execute_callback(unsigned int cb_id, const State & previous_state) const; + bool is_async_callback(unsigned int cb_id) const; + + void execute_async_callback(unsigned int cb_id, const State & previous_state); + + std::shared_ptr create_new_change_state_handler(); + const char * get_label_for_return_code(node_interfaces::LifecycleNodeInterface::CallbackReturn cb_return_code); @@ -148,6 +172,21 @@ class LifecycleNodeStateManager bool in_non_error_transition_state(uint8_t) const; bool in_error_transition_state(uint8_t) const; + + /*CancelTransition Members*/ + std::function)> send_cancel_transition_resp_cb_; + std::shared_ptr cancel_transition_header_; + std::atomic is_cancelling_transition_{false}; + + /*CancelTransition Helpers*/ + bool is_running_async_callback() const; + + void invalidate_change_state_handler(); + + bool mark_transition_as_cancelled(); + + void finalize_cancel_transition(const std::string & error_msg, bool success); }; } // namespace rclcpp_lifecycle #endif // LIFECYCLE_NODE_STATE_MANAGER_HPP_ diff --git a/rclcpp_lifecycle/src/lifecycle_node_state_services_manager.cpp b/rclcpp_lifecycle/src/lifecycle_node_state_services_manager.cpp index b0f40facfa..7da2705141 100644 --- a/rclcpp_lifecycle/src/lifecycle_node_state_services_manager.cpp +++ b/rclcpp_lifecycle/src/lifecycle_node_state_services_manager.cpp @@ -117,6 +117,27 @@ LifecycleNodeStateServicesManager::LifecycleNodeStateServicesManager( std::dynamic_pointer_cast(srv_get_transition_graph_), nullptr); } + + { // cancel_transition + auto cb = std::bind( + &LifecycleNodeStateServicesManager::on_cancel_transition, this, + std::placeholders::_1, std::placeholders::_2); + rclcpp::AnyServiceCallback any_cb; + any_cb.set(std::move(cb)); + + rcl_service_options_t cancel_srv_options = rcl_service_get_default_options(); + + srv_cancel_transition_ = + std::make_shared>( + node_base_interface->get_shared_rcl_node_handle(), + std::string(node_base_interface->get_fully_qualified_name()) + "/cancel_transition", + any_cb, + cancel_srv_options); + + node_services_interface->add_service( + std::dynamic_pointer_cast(srv_cancel_transition_), + nullptr); + } } } @@ -130,6 +151,18 @@ LifecycleNodeStateServicesManager::send_change_state_resp( srv_change_state_->send_response(*header, *resp); } +void +LifecycleNodeStateServicesManager::send_cancel_transition_resp( + const std::string & error_msg, + bool success, + const std::shared_ptr header) const +{ + auto resp = std::make_unique(); + resp->success = success; + resp->error_msg = error_msg; + srv_cancel_transition_->send_response(*header, *resp); +} + void LifecycleNodeStateServicesManager::on_change_state( const std::shared_ptr header, @@ -223,6 +256,25 @@ LifecycleNodeStateServicesManager::on_get_transition_graph( } } +void +LifecycleNodeStateServicesManager::on_cancel_transition( + const std::shared_ptr header, + const std::shared_ptr req) const +{ + (void) req; + auto state_hdl = state_manager_hdl_.lock(); + if (state_hdl) { + state_hdl->throw_runtime_error_on_uninitialized_state_machine("cancel transition"); + state_hdl->cancel_transition( + std::bind( + &LifecycleNodeStateServicesManager::send_cancel_transition_resp, this, + std::placeholders::_1, std::placeholders::_2, std::placeholders::_3), + header); + } else { + send_cancel_transition_resp("LifecycleNodeStateManager is not available.", false, header); + } +} + void LifecycleNodeStateServicesManager::copy_transitions_vector_to_resp( const std::vector transition_vec, diff --git a/rclcpp_lifecycle/src/lifecycle_node_state_services_manager.hpp b/rclcpp_lifecycle/src/lifecycle_node_state_services_manager.hpp index d469dd1180..afacdf4fd3 100644 --- a/rclcpp_lifecycle/src/lifecycle_node_state_services_manager.hpp +++ b/rclcpp_lifecycle/src/lifecycle_node_state_services_manager.hpp @@ -22,6 +22,7 @@ #include "rcl_lifecycle/rcl_lifecycle.h" #include "lifecycle_msgs/msg/transition_event.hpp" +#include "lifecycle_msgs/srv/cancel_transition.hpp" #include "lifecycle_msgs/srv/change_state.hpp" #include "lifecycle_msgs/srv/get_state.hpp" #include "lifecycle_msgs/srv/get_available_states.hpp" @@ -38,6 +39,7 @@ namespace rclcpp_lifecycle { class LifecycleNodeStateServicesManager { + using CancelTransitionSrv = lifecycle_msgs::srv::CancelTransition; using ChangeStateSrv = lifecycle_msgs::srv::ChangeState; using GetStateSrv = lifecycle_msgs::srv::GetState; using GetAvailableStatesSrv = lifecycle_msgs::srv::GetAvailableStates; @@ -54,6 +56,11 @@ class LifecycleNodeStateServicesManager bool success, const std::shared_ptr header) const; + void send_cancel_transition_resp( + const std::string & error_msg, + bool success, + const std::shared_ptr header) const; + private: void on_change_state( @@ -84,6 +91,11 @@ class LifecycleNodeStateServicesManager const std::shared_ptr req, std::shared_ptr resp) const; + void + on_cancel_transition( + const std::shared_ptr header, + const std::shared_ptr req) const; + void copy_transitions_vector_to_resp( const std::vector transition_vec, @@ -97,12 +109,15 @@ class LifecycleNodeStateServicesManager std::shared_ptr>; using GetTransitionGraphSrvPtr = std::shared_ptr>; + using CancelTransitionSrvPtr = + std::shared_ptr>; ChangeStateSrvPtr srv_change_state_; GetStateSrvPtr srv_get_state_; GetAvailableStatesSrvPtr srv_get_available_states_; GetAvailableTransitionsSrvPtr srv_get_available_transitions_; GetTransitionGraphSrvPtr srv_get_transition_graph_; + CancelTransitionSrvPtr srv_cancel_transition_; std::weak_ptr state_manager_hdl_; }; diff --git a/rclcpp_lifecycle/test/test_lifecycle_async_transitions.cpp b/rclcpp_lifecycle/test/test_lifecycle_async_transitions.cpp new file mode 100644 index 0000000000..013b0fb4cb --- /dev/null +++ b/rclcpp_lifecycle/test/test_lifecycle_async_transitions.cpp @@ -0,0 +1,588 @@ +// Copyright 2020 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. + +/** + * Service client test was adopted from: + * https://github.com/ros2/demos/blob/master/lifecycle/src/lifecycle_service_client.cpp + */ + +#include +#include +#include +#include +#include +#include +#include + +#include "lifecycle_msgs/msg/state.hpp" +#include "lifecycle_msgs/msg/transition.hpp" +#include "lifecycle_msgs/msg/transition_description.hpp" +#include "lifecycle_msgs/srv/cancel_transition.hpp" +#include "lifecycle_msgs/srv/change_state.hpp" +#include "lifecycle_msgs/srv/get_state.hpp" + +#include "rcl_lifecycle/rcl_lifecycle.h" + +#include "rclcpp/node_interfaces/node_graph.hpp" +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" + +#include "rcpputils/scope_exit.hpp" + +#include "./mocking_utils/patch.hpp" + +using namespace std::chrono_literals; +using lifecycle_msgs::msg::State; + +constexpr char const * ping_pong_node_name = "ping_pong_node"; + +constexpr char const * node_get_state_topic = "/ping_pong_node/get_state"; +constexpr char const * node_change_state_topic = "/ping_pong_node/change_state"; +constexpr char const * node_cancel_transition_topic = + "/ping_pong_node/cancel_transition"; + +const lifecycle_msgs::msg::State unknown_state = lifecycle_msgs::msg::State(); + +class PingPongAsyncLCNode : public rclcpp_lifecycle::LifecycleNode +{ +public: + /** + * @brief `PingPong` refers to transition callback FAILURE->SUCCESS->... returns + * It has 2 notable exceptions: + * - on_shutdown_async ping pongs ERROR -> SUCCESS (FAILURE is undefined for this transition) + * - on_error_async always returns SUCCESS + * It also contains a function to switch to a detached thread wrapper of each callback + */ + explicit PingPongAsyncLCNode(std::string node_name) + : rclcpp_lifecycle::LifecycleNode(std::move(node_name)) + { + register_async_on_configure( + std::bind( + &PingPongAsyncLCNode::on_configure_async, + this, std::placeholders::_1, + std::placeholders::_2)); + register_async_on_activate( + std::bind( + &PingPongAsyncLCNode::on_activate_async, + this, std::placeholders::_1, + std::placeholders::_2)); + register_async_on_deactivate( + std::bind( + &PingPongAsyncLCNode::on_deactivate_async, + this, std::placeholders::_1, + std::placeholders::_2)); + register_async_on_cleanup( + std::bind( + &PingPongAsyncLCNode::on_cleanup_async, + this, std::placeholders::_1, + std::placeholders::_2)); + register_async_on_shutdown( + std::bind( + &PingPongAsyncLCNode::on_shutdown_async, + this, std::placeholders::_1, + std::placeholders::_2)); + register_async_on_error( + std::bind( + &PingPongAsyncLCNode::on_error_async, + this, std::placeholders::_1, + std::placeholders::_2)); + } + + void switch_to_detached_thread_callbacks() + { + register_async_on_configure( + std::bind( + &PingPongAsyncLCNode::on_configure_async_dt, + this, std::placeholders::_1, + std::placeholders::_2)); + register_async_on_activate( + std::bind( + &PingPongAsyncLCNode::on_activate_async_dt, + this, std::placeholders::_1, + std::placeholders::_2)); + register_async_on_deactivate( + std::bind( + &PingPongAsyncLCNode::on_deactivate_async_dt, + this, std::placeholders::_1, + std::placeholders::_2)); + register_async_on_cleanup( + std::bind( + &PingPongAsyncLCNode::on_cleanup_async_dt, + this, std::placeholders::_1, + std::placeholders::_2)); + register_async_on_shutdown( + std::bind( + &PingPongAsyncLCNode::on_shutdown_async_dt, + this, std::placeholders::_1, + std::placeholders::_2)); + register_async_on_error( + std::bind( + &PingPongAsyncLCNode::on_error_async_dt, + this, std::placeholders::_1, + std::placeholders::_2)); + } + + size_t number_of_callbacks = 0; + +protected: + // Simulates fail -> success -> fail -> success -> ... + bool ret_success{false}; + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn + ping_pong_ret_fail_success() + { + auto ret = ret_success ? + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS : + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::FAILURE; + ret_success = !ret_success; + return ret; + } + + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn + ping_pong_ret_error_success() + { + auto ret = ret_success ? + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS : + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; + ret_success = !ret_success; + return ret; + } + + // Async callbacks + void + on_configure_async( + const rclcpp_lifecycle::State &, + std::shared_ptr change_state_hdl) + { + EXPECT_EQ(State::TRANSITION_STATE_CONFIGURING, get_current_state().id()); + EXPECT_TRUE(change_state_hdl != nullptr); + ++number_of_callbacks; + change_state_hdl->send_callback_resp(ping_pong_ret_fail_success()); + } + + void + on_activate_async( + const rclcpp_lifecycle::State &, + std::shared_ptr change_state_hdl) + { + EXPECT_EQ(State::TRANSITION_STATE_ACTIVATING, get_current_state().id()); + EXPECT_TRUE(change_state_hdl != nullptr); + ++number_of_callbacks; + change_state_hdl->send_callback_resp(ping_pong_ret_fail_success()); + } + + void + on_deactivate_async( + const rclcpp_lifecycle::State &, + std::shared_ptr change_state_hdl) + { + EXPECT_EQ(State::TRANSITION_STATE_DEACTIVATING, get_current_state().id()); + EXPECT_TRUE(change_state_hdl != nullptr); + ++number_of_callbacks; + change_state_hdl->send_callback_resp(ping_pong_ret_fail_success()); + } + + void + on_cleanup_async( + const rclcpp_lifecycle::State &, + std::shared_ptr change_state_hdl) + { + EXPECT_EQ(State::TRANSITION_STATE_CLEANINGUP, get_current_state().id()); + EXPECT_TRUE(change_state_hdl != nullptr); + ++number_of_callbacks; + change_state_hdl->send_callback_resp(ping_pong_ret_fail_success()); + } + + void on_shutdown_async( + const rclcpp_lifecycle::State &, + std::shared_ptr change_state_hdl) + { + EXPECT_EQ(State::TRANSITION_STATE_SHUTTINGDOWN, get_current_state().id()); + EXPECT_TRUE(change_state_hdl != nullptr); + ++number_of_callbacks; + change_state_hdl->send_callback_resp(ping_pong_ret_error_success()); + } + + void on_error_async( + const rclcpp_lifecycle::State &, + std::shared_ptr change_state_hdl) + { + EXPECT_EQ(State::TRANSITION_STATE_ERRORPROCESSING, get_current_state().id()); + EXPECT_TRUE(change_state_hdl != nullptr); + ++number_of_callbacks; + change_state_hdl->send_callback_resp( + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS); + } + + // Detached thread callbacks + void + on_configure_async_dt( + const rclcpp_lifecycle::State & s, + std::shared_ptr change_state_hdl) + { + std::thread t(&PingPongAsyncLCNode::on_configure_async, this, + s, change_state_hdl); + t.detach(); + } + + void + on_activate_async_dt( + const rclcpp_lifecycle::State & s, + std::shared_ptr change_state_hdl) + { + std::thread t(&PingPongAsyncLCNode::on_activate_async, this, + s, change_state_hdl); + t.detach(); + } + + void + on_deactivate_async_dt( + const rclcpp_lifecycle::State & s, + std::shared_ptr change_state_hdl) + { + std::thread t(&PingPongAsyncLCNode::on_deactivate_async, this, + s, change_state_hdl); + t.detach(); + } + + void + on_cleanup_async_dt( + const rclcpp_lifecycle::State & s, + std::shared_ptr change_state_hdl) + { + std::thread t(&PingPongAsyncLCNode::on_cleanup_async, this, + s, change_state_hdl); + t.detach(); + } + + void on_shutdown_async_dt( + const rclcpp_lifecycle::State & s, + std::shared_ptr change_state_hdl) + { + std::thread t(&PingPongAsyncLCNode::on_shutdown_async, this, + s, change_state_hdl); + t.detach(); + } + + void on_error_async_dt( + const rclcpp_lifecycle::State & s, + std::shared_ptr change_state_hdl) + { + std::thread t(&PingPongAsyncLCNode::on_error_async, this, + s, change_state_hdl); + t.detach(); + } +}; + + +class LifecycleServiceClient : public rclcpp::Node +{ +public: + explicit LifecycleServiceClient(std::string node_name) + : Node(node_name) + { + client_get_state_ = this->create_client( + node_get_state_topic); + client_change_state_ = this->create_client( + node_change_state_topic); + client_cancel_transition_ = this->create_client( + node_cancel_transition_topic); + } + + lifecycle_msgs::msg::State + get_state(std::chrono::seconds time_out = 1s) + { + auto request = std::make_shared(); + + if (!client_get_state_->wait_for_service(time_out)) { + return unknown_state; + } + + auto future_result = client_get_state_->async_send_request(request); + auto future_status = future_result.wait_for(time_out); + + if (future_status != std::future_status::ready) { + return unknown_state; + } + + auto result = future_result.get(); + if (result) { + return result->current_state; + } else { + return unknown_state; + } + } + + bool + change_state(std::uint8_t transition, std::chrono::seconds time_out = 1s) + { + auto request = std::make_shared(); + request->transition.id = transition; + + if (!client_change_state_->wait_for_service(time_out)) { + return false; + } + + auto future_result = client_change_state_->async_send_request(request); + auto future_status = future_result.wait_for(time_out); + + if (future_status != std::future_status::ready) { + return false; + } + + return future_result.get()->success; + } + + bool cancel_transition(std::chrono::seconds time_out = 1s) + { + auto request = std::make_shared(); + + if (!client_cancel_transition_->wait_for_service(time_out)) { + return false; + } + + auto future_result = client_cancel_transition_->async_send_request(request); + auto future_status = future_result.wait_for(time_out); + + if (future_status != std::future_status::ready) { + return false; + } + + return future_result.get()->success; + } + +private: + std::shared_ptr> client_get_state_; + std::shared_ptr> client_change_state_; + std::shared_ptr> + client_cancel_transition_; +}; + + +class TestLifecycleAsyncTransitions : public ::testing::Test +{ +protected: + PingPongAsyncLCNode * lifecycle_node() {return lifecycle_node_.get();} + LifecycleServiceClient * lifecycle_client() {return lifecycle_client_.get();} + +private: + void SetUp() override + { + rclcpp::init(0, nullptr); + lifecycle_node_ = std::make_shared(ping_pong_node_name); + lifecycle_client_ = std::make_shared("client"); + spinner_ = std::thread(&TestLifecycleAsyncTransitions::spin, this); + } + + void TearDown() override + { + { + std::lock_guard guard(shutdown_mutex_); + rclcpp::shutdown(); + } + spinner_.join(); + } + + void spin() + { + while (true) { + { + std::lock_guard guard(shutdown_mutex_); + if (!rclcpp::ok()) { + break; + } + rclcpp::spin_some(lifecycle_node_->get_node_base_interface()); + rclcpp::spin_some(lifecycle_client_); + } + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + } + } + + std::shared_ptr lifecycle_node_; + std::shared_ptr lifecycle_client_; + std::mutex shutdown_mutex_; + std::thread spinner_; +}; + + +TEST_F(TestLifecycleAsyncTransitions, lifecycle_async_transitions_w_immediate_ret) { + EXPECT_EQ( + lifecycle_client()->get_state().id, + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); + + // Configure 2x + EXPECT_FALSE( + lifecycle_client()->change_state( + lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE)); + EXPECT_EQ( + lifecycle_client()->get_state().id, + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); + EXPECT_TRUE( + lifecycle_client()->change_state( + lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE)); + EXPECT_EQ( + lifecycle_client()->get_state().id, + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); + + // Activate 2x + EXPECT_FALSE( + lifecycle_client()->change_state( + lifecycle_msgs::msg::Transition::TRANSITION_ACTIVATE)); + EXPECT_EQ( + lifecycle_client()->get_state().id, + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); + EXPECT_TRUE( + lifecycle_client()->change_state( + lifecycle_msgs::msg::Transition::TRANSITION_ACTIVATE)); + EXPECT_EQ( + lifecycle_client()->get_state().id, + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + + // Deactivate 2x + EXPECT_FALSE( + lifecycle_client()->change_state( + lifecycle_msgs::msg::Transition::TRANSITION_DEACTIVATE)); + EXPECT_EQ( + lifecycle_client()->get_state().id, + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + EXPECT_TRUE( + lifecycle_client()->change_state( + lifecycle_msgs::msg::Transition::TRANSITION_DEACTIVATE)); + EXPECT_EQ( + lifecycle_client()->get_state().id, + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); + + // Cleanup 2x + EXPECT_FALSE( + lifecycle_client()->change_state( + lifecycle_msgs::msg::Transition::TRANSITION_CLEANUP)); + EXPECT_EQ( + lifecycle_client()->get_state().id, + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); + EXPECT_TRUE( + lifecycle_client()->change_state( + lifecycle_msgs::msg::Transition::TRANSITION_CLEANUP)); + EXPECT_EQ( + lifecycle_client()->get_state().id, + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); + + EXPECT_EQ( + lifecycle_client()->get_state().id, + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); + + // Shutdown(Error) -> ErrorProcessing(Success) + EXPECT_TRUE( + lifecycle_client()->change_state( + lifecycle_msgs::msg::Transition::TRANSITION_UNCONFIGURED_SHUTDOWN)); + EXPECT_EQ( + lifecycle_client()->get_state().id, + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); + + // Shutdown(Success) + EXPECT_TRUE( + lifecycle_client()->change_state( + lifecycle_msgs::msg::Transition::TRANSITION_UNCONFIGURED_SHUTDOWN)); + EXPECT_EQ( + lifecycle_client()->get_state().id, + lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED); + + // 2 * change_state calls + 1 ErrorProcessing + EXPECT_EQ(lifecycle_node()->number_of_callbacks, 11); +} + +TEST_F(TestLifecycleAsyncTransitions, lifecycle_async_transitions_w_detached_thread) { + lifecycle_node()->switch_to_detached_thread_callbacks(); + + EXPECT_EQ( + lifecycle_client()->get_state().id, + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); + + // Configure 2x + EXPECT_FALSE( + lifecycle_client()->change_state( + lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE)); + EXPECT_EQ( + lifecycle_client()->get_state().id, + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); + EXPECT_TRUE( + lifecycle_client()->change_state( + lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE)); + EXPECT_EQ( + lifecycle_client()->get_state().id, + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); + + // Activate 2x + EXPECT_FALSE( + lifecycle_client()->change_state( + lifecycle_msgs::msg::Transition::TRANSITION_ACTIVATE)); + EXPECT_EQ( + lifecycle_client()->get_state().id, + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); + EXPECT_TRUE( + lifecycle_client()->change_state( + lifecycle_msgs::msg::Transition::TRANSITION_ACTIVATE)); + EXPECT_EQ( + lifecycle_client()->get_state().id, + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + + // Deactivate 2x + EXPECT_FALSE( + lifecycle_client()->change_state( + lifecycle_msgs::msg::Transition::TRANSITION_DEACTIVATE)); + EXPECT_EQ( + lifecycle_client()->get_state().id, + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + EXPECT_TRUE( + lifecycle_client()->change_state( + lifecycle_msgs::msg::Transition::TRANSITION_DEACTIVATE)); + EXPECT_EQ( + lifecycle_client()->get_state().id, + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); + + // Cleanup 2x + EXPECT_FALSE( + lifecycle_client()->change_state( + lifecycle_msgs::msg::Transition::TRANSITION_CLEANUP)); + EXPECT_EQ( + lifecycle_client()->get_state().id, + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); + EXPECT_TRUE( + lifecycle_client()->change_state( + lifecycle_msgs::msg::Transition::TRANSITION_CLEANUP)); + EXPECT_EQ( + lifecycle_client()->get_state().id, + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); + + EXPECT_EQ( + lifecycle_client()->get_state().id, + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); + + // Shutdown(Error) -> ErrorProcessing(Success) + EXPECT_TRUE( + lifecycle_client()->change_state( + lifecycle_msgs::msg::Transition::TRANSITION_UNCONFIGURED_SHUTDOWN)); + EXPECT_EQ( + lifecycle_client()->get_state().id, + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); + + // Shutdown(Success) + EXPECT_TRUE( + lifecycle_client()->change_state( + lifecycle_msgs::msg::Transition::TRANSITION_UNCONFIGURED_SHUTDOWN)); + EXPECT_EQ( + lifecycle_client()->get_state().id, + lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED); + + // 2 * change_state calls + 1 ErrorProcessing + EXPECT_EQ(lifecycle_node()->number_of_callbacks, 11); +} diff --git a/rclcpp_lifecycle/test/test_lifecycle_service_client.cpp b/rclcpp_lifecycle/test/test_lifecycle_service_client.cpp index 31338fa075..451cdc6bef 100644 --- a/rclcpp_lifecycle/test/test_lifecycle_service_client.cpp +++ b/rclcpp_lifecycle/test/test_lifecycle_service_client.cpp @@ -28,6 +28,7 @@ #include "lifecycle_msgs/msg/state.hpp" #include "lifecycle_msgs/msg/transition.hpp" #include "lifecycle_msgs/msg/transition_description.hpp" +#include "lifecycle_msgs/srv/cancel_transition.hpp" #include "lifecycle_msgs/srv/change_state.hpp" #include "lifecycle_msgs/srv/get_available_states.hpp" #include "lifecycle_msgs/srv/get_available_transitions.hpp" @@ -54,7 +55,8 @@ constexpr char const * node_get_available_transitions_topic = "/lc_talker/get_available_transitions"; constexpr char const * node_get_transition_graph_topic = "/lc_talker/get_transition_graph"; - +constexpr char const * node_cancel_transition_topic = + "/lc_talker/cancel_transition"; const lifecycle_msgs::msg::State unknown_state = lifecycle_msgs::msg::State(); class EmptyLifecycleNode : public rclcpp_lifecycle::LifecycleNode @@ -83,6 +85,8 @@ class LifecycleServiceClient : public rclcpp::Node node_get_state_topic); client_change_state_ = this->create_client( node_change_state_topic); + client_cancel_transition_ = this->create_client( + node_cancel_transition_topic); } lifecycle_msgs::msg::State @@ -200,6 +204,24 @@ class LifecycleServiceClient : public rclcpp::Node return std::vector(); } + bool cancel_transition(std::chrono::seconds time_out = 1s) + { + auto request = std::make_shared(); + + if (!client_cancel_transition_->wait_for_service(time_out)) { + return false; + } + + auto future_result = client_cancel_transition_->async_send_request(request); + auto future_status = future_result.wait_for(time_out); + + if (future_status != std::future_status::ready) { + return false; + } + + return future_result.get()->success; + } + private: std::shared_ptr> client_get_available_states_; @@ -209,6 +231,8 @@ class LifecycleServiceClient : public rclcpp::Node client_get_transition_graph_; std::shared_ptr> client_get_state_; std::shared_ptr> client_change_state_; + std::shared_ptr> + client_cancel_transition_; }; @@ -475,4 +499,10 @@ TEST_F(TestLifecycleServiceClientRCLErrors, call_services_rcl_errors) { rclcpp::spin_some(lifecycle_client); EXPECT_THROW( rclcpp::spin_some(lifecycle_node->get_node_base_interface()), std::runtime_error); + + // on_cancel_transition + lifecycle_client->cancel_transition(); + rclcpp::spin_some(lifecycle_client); + EXPECT_THROW( + rclcpp::spin_some(lifecycle_node->get_node_base_interface()), std::runtime_error); } diff --git a/rclcpp_lifecycle/test/test_register_custom_callbacks.cpp b/rclcpp_lifecycle/test/test_register_custom_callbacks.cpp index 2a46098ee9..2c3cd3f5a6 100644 --- a/rclcpp_lifecycle/test/test_register_custom_callbacks.cpp +++ b/rclcpp_lifecycle/test/test_register_custom_callbacks.cpp @@ -133,6 +133,71 @@ class CustomLifecycleNode : public rclcpp_lifecycle::LifecycleNode ++number_of_callbacks; return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; } + + // Asynchronous callbacks + void + on_custom_configure_async( + const rclcpp_lifecycle::State & previous_state, + std::shared_ptr change_state_hdl) + { + EXPECT_EQ(State::PRIMARY_STATE_UNCONFIGURED, previous_state.id()); + EXPECT_EQ(State::TRANSITION_STATE_CONFIGURING, get_current_state().id()); + EXPECT_TRUE(change_state_hdl != nullptr); + ++number_of_callbacks; + change_state_hdl->send_callback_resp( + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS); + } + + void + on_custom_activate_async( + const rclcpp_lifecycle::State & previous_state, + std::shared_ptr change_state_hdl) + { + EXPECT_EQ(State::PRIMARY_STATE_INACTIVE, previous_state.id()); + EXPECT_EQ(State::TRANSITION_STATE_ACTIVATING, get_current_state().id()); + EXPECT_TRUE(change_state_hdl != nullptr); + ++number_of_callbacks; + change_state_hdl->send_callback_resp( + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS); + } + + void + on_custom_deactivate_async( + const rclcpp_lifecycle::State & previous_state, + std::shared_ptr change_state_hdl) + { + EXPECT_EQ(State::PRIMARY_STATE_ACTIVE, previous_state.id()); + EXPECT_EQ(State::TRANSITION_STATE_DEACTIVATING, get_current_state().id()); + EXPECT_TRUE(change_state_hdl != nullptr); + ++number_of_callbacks; + change_state_hdl->send_callback_resp( + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS); + } + + void + on_custom_cleanup_async( + const rclcpp_lifecycle::State & previous_state, + std::shared_ptr change_state_hdl) + { + EXPECT_EQ(State::PRIMARY_STATE_INACTIVE, previous_state.id()); + EXPECT_EQ(State::TRANSITION_STATE_CLEANINGUP, get_current_state().id()); + EXPECT_TRUE(change_state_hdl != nullptr); + ++number_of_callbacks; + change_state_hdl->send_callback_resp( + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS); + } + + void + on_custom_shutdown_async( + const rclcpp_lifecycle::State &, + std::shared_ptr change_state_hdl) + { + EXPECT_EQ(State::TRANSITION_STATE_SHUTTINGDOWN, get_current_state().id()); + EXPECT_TRUE(change_state_hdl != nullptr); + ++number_of_callbacks; + change_state_hdl->send_callback_resp( + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS); + } }; TEST_F(TestRegisterCustomCallbacks, custom_synchronous_callbacks) { @@ -179,3 +244,49 @@ TEST_F(TestRegisterCustomCallbacks, custom_synchronous_callbacks) { // check if all callbacks were successfully overwritten EXPECT_EQ(5u, test_node->number_of_callbacks); } + + +TEST_F(TestRegisterCustomCallbacks, custom_asynchronous_callbacks) { + auto test_node = std::make_shared("testnode"); + + test_node->register_async_on_configure( + std::bind( + &CustomLifecycleNode::on_custom_configure_async, + test_node.get(), std::placeholders::_1, std::placeholders::_2)); + test_node->register_async_on_cleanup( + std::bind( + &CustomLifecycleNode::on_custom_cleanup_async, + test_node.get(), std::placeholders::_1, std::placeholders::_2)); + test_node->register_async_on_shutdown( + std::bind( + &CustomLifecycleNode::on_custom_shutdown_async, + test_node.get(), std::placeholders::_1, std::placeholders::_2)); + test_node->register_async_on_activate( + std::bind( + &CustomLifecycleNode::on_custom_activate_async, + test_node.get(), std::placeholders::_1, std::placeholders::_2)); + test_node->register_async_on_deactivate( + std::bind( + &CustomLifecycleNode::on_custom_deactivate_async, + test_node.get(), std::placeholders::_1, std::placeholders::_2)); + + EXPECT_EQ(State::PRIMARY_STATE_UNCONFIGURED, test_node->get_current_state().id()); + EXPECT_EQ( + State::PRIMARY_STATE_INACTIVE, test_node->trigger_transition( + rclcpp_lifecycle::Transition(Transition::TRANSITION_CONFIGURE)).id()); + EXPECT_EQ( + State::PRIMARY_STATE_ACTIVE, test_node->trigger_transition( + rclcpp_lifecycle::Transition(Transition::TRANSITION_ACTIVATE)).id()); + EXPECT_EQ( + State::PRIMARY_STATE_INACTIVE, test_node->trigger_transition( + rclcpp_lifecycle::Transition(Transition::TRANSITION_DEACTIVATE)).id()); + EXPECT_EQ( + State::PRIMARY_STATE_UNCONFIGURED, test_node->trigger_transition( + rclcpp_lifecycle::Transition(Transition::TRANSITION_CLEANUP)).id()); + EXPECT_EQ( + State::PRIMARY_STATE_FINALIZED, test_node->trigger_transition( + rclcpp_lifecycle::Transition(Transition::TRANSITION_UNCONFIGURED_SHUTDOWN)).id()); + + // check if all callbacks were successfully overwritten + EXPECT_EQ(5u, test_node->number_of_callbacks); +}