diff --git a/rclcpp_lifecycle/CMakeLists.txt b/rclcpp_lifecycle/CMakeLists.txt index f46f993b21..3bbde37682 100644 --- a/rclcpp_lifecycle/CMakeLists.txt +++ b/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 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 197ecf5c19..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 { @@ -1004,7 +1005,19 @@ class LifecycleNode : public node_interfaces::LifecycleNodeInterface, */ RCLCPP_LIFECYCLE_PUBLIC bool - register_on_configure(std::function fcn); + 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 /** @@ -1014,7 +1027,19 @@ class LifecycleNode : public node_interfaces::LifecycleNodeInterface, */ RCLCPP_LIFECYCLE_PUBLIC bool - register_on_cleanup(std::function fcn); + 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 /** @@ -1024,7 +1049,19 @@ class LifecycleNode : public node_interfaces::LifecycleNodeInterface, */ RCLCPP_LIFECYCLE_PUBLIC bool - register_on_shutdown(std::function fcn); + 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 /** @@ -1034,7 +1071,19 @@ class LifecycleNode : public node_interfaces::LifecycleNodeInterface, */ RCLCPP_LIFECYCLE_PUBLIC bool - register_on_activate(std::function fcn); + 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 /** @@ -1044,7 +1093,19 @@ class LifecycleNode : public node_interfaces::LifecycleNodeInterface, */ RCLCPP_LIFECYCLE_PUBLIC bool - register_on_deactivate(std::function fcn); + 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 /** @@ -1054,7 +1115,19 @@ class LifecycleNode : public node_interfaces::LifecycleNodeInterface, */ RCLCPP_LIFECYCLE_PUBLIC bool - register_on_error(std::function fcn); + 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 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 4c0b94cb42..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,14 @@ 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_entities_manager.cpp b/rclcpp_lifecycle/src/lifecycle_node_entities_manager.cpp new file mode 100644 index 0000000000..e9e70b8bad --- /dev/null +++ b/rclcpp_lifecycle/src/lifecycle_node_entities_manager.cpp @@ -0,0 +1,55 @@ +// Copyright 2016 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 "lifecycle_node_entities_manager.hpp" + +namespace rclcpp_lifecycle +{ + +void +LifecycleNodeEntitiesManager::on_activate() const +{ + for (const auto & weak_entity : weak_managed_entities_) { + auto entity = weak_entity.lock(); + if (entity) { + entity->on_activate(); + } + } +} + +void +LifecycleNodeEntitiesManager::on_deactivate() const +{ + for (const auto & weak_entity : weak_managed_entities_) { + auto entity = weak_entity.lock(); + if (entity) { + entity->on_deactivate(); + } + } +} + +void +LifecycleNodeEntitiesManager::add_managed_entity( + std::weak_ptr managed_entity) +{ + weak_managed_entities_.push_back(managed_entity); +} + +void +LifecycleNodeEntitiesManager::add_timer_handle( + std::shared_ptr timer) +{ + weak_timers_.push_back(timer); +} + +} // namespace rclcpp_lifecycle diff --git a/rclcpp_lifecycle/src/lifecycle_node_entities_manager.hpp b/rclcpp_lifecycle/src/lifecycle_node_entities_manager.hpp new file mode 100644 index 0000000000..b64c57a3af --- /dev/null +++ b/rclcpp_lifecycle/src/lifecycle_node_entities_manager.hpp @@ -0,0 +1,47 @@ +// 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 LIFECYCLE_NODE_ENTITIES_MANAGER_HPP_ +#define LIFECYCLE_NODE_ENTITIES_MANAGER_HPP_ + +#include +#include +#include "rclcpp_lifecycle/managed_entity.hpp" +#include + +namespace rclcpp_lifecycle +{ + +class LifecycleNodeEntitiesManager +{ +public: + void + on_activate() const; + + void + on_deactivate() const; + + void + add_managed_entity(std::weak_ptr managed_entity); + + void + add_timer_handle(std::shared_ptr timer); + +private: + std::vector> weak_managed_entities_; + std::vector> weak_timers_; +}; + +} // namespace rclcpp_lifecycle +#endif // LIFECYCLE_NODE_ENTITIES_MANAGER_HPP_ diff --git a/rclcpp_lifecycle/src/lifecycle_node_interface_impl.cpp b/rclcpp_lifecycle/src/lifecycle_node_interface_impl.cpp index 5c5f7797e1..9f815ad929 100644 --- a/rclcpp_lifecycle/src/lifecycle_node_interface_impl.cpp +++ b/rclcpp_lifecycle/src/lifecycle_node_interface_impl.cpp @@ -58,138 +58,24 @@ LifecycleNode::LifecycleNodeInterfaceImpl::LifecycleNodeInterfaceImpl( LifecycleNode::LifecycleNodeInterfaceImpl::~LifecycleNodeInterfaceImpl() { - rcl_node_t * node_handle = node_base_interface_->get_rcl_node_handle(); - rcl_ret_t ret; - { - std::lock_guard lock(state_machine_mutex_); - ret = rcl_lifecycle_state_machine_fini(&state_machine_, node_handle); - } - if (ret != RCL_RET_OK) { - RCUTILS_LOG_FATAL_NAMED( - "rclcpp_lifecycle", - "failed to destroy rcl_state_machine"); - } } void LifecycleNode::LifecycleNodeInterfaceImpl::init(bool enable_communication_interface) { - rcl_node_t * node_handle = node_base_interface_->get_rcl_node_handle(); - const rcl_node_options_t * node_options = - rcl_node_get_options(node_base_interface_->get_rcl_node_handle()); - auto state_machine_options = rcl_lifecycle_get_default_state_machine_options(); - state_machine_options.enable_com_interface = enable_communication_interface; - state_machine_options.allocator = node_options->allocator; - - // The call to initialize the state machine takes - // currently five different typesupports for all publishers/services - // created within the RCL_LIFECYCLE structure. - // The publisher takes a C-Typesupport since the publishing (i.e. creating - // the message) is done fully in RCL. - // Services are handled in C++, so that it needs a C++ typesupport structure. - std::lock_guard lock(state_machine_mutex_); - state_machine_ = rcl_lifecycle_get_zero_initialized_state_machine(); - rcl_ret_t ret = rcl_lifecycle_state_machine_init( - &state_machine_, - node_handle, - ROSIDL_GET_MSG_TYPE_SUPPORT(lifecycle_msgs, msg, TransitionEvent), - rosidl_typesupport_cpp::get_service_type_support_handle(), - rosidl_typesupport_cpp::get_service_type_support_handle(), - rosidl_typesupport_cpp::get_service_type_support_handle(), - rosidl_typesupport_cpp::get_service_type_support_handle(), - rosidl_typesupport_cpp::get_service_type_support_handle(), - &state_machine_options); - if (ret != RCL_RET_OK) { - throw std::runtime_error( - std::string("Couldn't initialize state machine for node ") + - node_base_interface_->get_name()); - } - - current_state_ = State(state_machine_.current_state); - + state_manager_hdl_ = std::make_shared(); + state_manager_hdl_->init( + node_base_interface_, + enable_communication_interface + ); if (enable_communication_interface) { - { // change_state - auto cb = std::bind( - &LifecycleNode::LifecycleNodeInterfaceImpl::on_change_state, this, - std::placeholders::_1, std::placeholders::_2, std::placeholders::_3); - rclcpp::AnyServiceCallback any_cb; - any_cb.set(std::move(cb)); - - srv_change_state_ = std::make_shared>( - node_base_interface_->get_shared_rcl_node_handle(), - &state_machine_.com_interface.srv_change_state, - any_cb); - node_services_interface_->add_service( - std::dynamic_pointer_cast(srv_change_state_), - nullptr); - } - - { // get_state - auto cb = std::bind( - &LifecycleNode::LifecycleNodeInterfaceImpl::on_get_state, this, - std::placeholders::_1, std::placeholders::_2, std::placeholders::_3); - rclcpp::AnyServiceCallback any_cb; - any_cb.set(std::move(cb)); - - srv_get_state_ = std::make_shared>( - node_base_interface_->get_shared_rcl_node_handle(), - &state_machine_.com_interface.srv_get_state, - any_cb); - node_services_interface_->add_service( - std::dynamic_pointer_cast(srv_get_state_), - nullptr); - } - - { // get_available_states - auto cb = std::bind( - &LifecycleNode::LifecycleNodeInterfaceImpl::on_get_available_states, this, - std::placeholders::_1, std::placeholders::_2, std::placeholders::_3); - rclcpp::AnyServiceCallback any_cb; - any_cb.set(std::move(cb)); - - srv_get_available_states_ = std::make_shared>( - node_base_interface_->get_shared_rcl_node_handle(), - &state_machine_.com_interface.srv_get_available_states, - any_cb); - node_services_interface_->add_service( - std::dynamic_pointer_cast(srv_get_available_states_), - nullptr); - } - - { // get_available_transitions - auto cb = std::bind( - &LifecycleNode::LifecycleNodeInterfaceImpl::on_get_available_transitions, this, - std::placeholders::_1, std::placeholders::_2, std::placeholders::_3); - rclcpp::AnyServiceCallback any_cb; - any_cb.set(std::move(cb)); - - srv_get_available_transitions_ = - std::make_shared>( - node_base_interface_->get_shared_rcl_node_handle(), - &state_machine_.com_interface.srv_get_available_transitions, - any_cb); - node_services_interface_->add_service( - std::dynamic_pointer_cast(srv_get_available_transitions_), - nullptr); - } - - { // get_transition_graph - auto cb = std::bind( - &LifecycleNode::LifecycleNodeInterfaceImpl::on_get_transition_graph, this, - std::placeholders::_1, std::placeholders::_2, std::placeholders::_3); - rclcpp::AnyServiceCallback any_cb; - any_cb.set(std::move(cb)); - - srv_get_transition_graph_ = - std::make_shared>( - node_base_interface_->get_shared_rcl_node_handle(), - &state_machine_.com_interface.srv_get_transition_graph, - any_cb); - node_services_interface_->add_service( - std::dynamic_pointer_cast(srv_get_transition_graph_), - nullptr); - } + state_services_manager_hdl_ = std::make_unique( + node_base_interface_, + node_services_interface_, + state_manager_hdl_ + ); } + managed_entities_manager_hdl_ = std::make_unique(); } bool @@ -197,193 +83,31 @@ LifecycleNode::LifecycleNodeInterfaceImpl::register_callback( std::uint8_t lifecycle_transition, std::function & cb) { - cb_map_[lifecycle_transition] = cb; - return true; -} - -void -LifecycleNode::LifecycleNodeInterfaceImpl::on_change_state( - const std::shared_ptr header, - const std::shared_ptr req, - std::shared_ptr resp) -{ - (void)header; - std::uint8_t transition_id; - { - std::lock_guard lock(state_machine_mutex_); - if (rcl_lifecycle_state_machine_is_initialized(&state_machine_) != RCL_RET_OK) { - throw std::runtime_error("Can't get state. State machine is not initialized."); - } - - transition_id = req->transition.id; - // if there's a label attached to the request, - // we check the transition attached to this label. - // we further can't compare the id of the looked up transition - // because ros2 service call defaults all intergers to zero. - // that means if we call ros2 service call ... {transition: {label: shutdown}} - // the id of the request is 0 (zero) whereas the id from the lookup up transition - // can be different. - // the result of this is that the label takes presedence of the id. - if (req->transition.label.size() != 0) { - auto rcl_transition = rcl_lifecycle_get_transition_by_label( - state_machine_.current_state, req->transition.label.c_str()); - if (rcl_transition == nullptr) { - resp->success = false; - return; - } - transition_id = static_cast(rcl_transition->id); - } - } - - node_interfaces::LifecycleNodeInterface::CallbackReturn cb_return_code; - auto ret = change_state(transition_id, cb_return_code); - (void) ret; - // TODO(karsten1987): Lifecycle msgs have to be extended to keep both returns - // 1. return is the actual transition - // 2. return is whether an error occurred or not - resp->success = - (cb_return_code == node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS); -} - -void -LifecycleNode::LifecycleNodeInterfaceImpl::on_get_state( - const std::shared_ptr header, - const std::shared_ptr req, - std::shared_ptr resp) const -{ - (void)header; - (void)req; - std::lock_guard lock(state_machine_mutex_); - if (rcl_lifecycle_state_machine_is_initialized(&state_machine_) != RCL_RET_OK) { - throw std::runtime_error( - "Can't get state. State machine is not initialized."); - } - resp->current_state.id = static_cast(state_machine_.current_state->id); - resp->current_state.label = state_machine_.current_state->label; -} - -void -LifecycleNode::LifecycleNodeInterfaceImpl::on_get_available_states( - const std::shared_ptr header, - const std::shared_ptr req, - std::shared_ptr resp) const -{ - (void)header; - (void)req; - std::lock_guard lock(state_machine_mutex_); - if (rcl_lifecycle_state_machine_is_initialized(&state_machine_) != RCL_RET_OK) { - throw std::runtime_error( - "Can't get available states. State machine is not initialized."); - } - - resp->available_states.resize(state_machine_.transition_map.states_size); - for (unsigned int i = 0; i < state_machine_.transition_map.states_size; ++i) { - resp->available_states[i].id = - static_cast(state_machine_.transition_map.states[i].id); - resp->available_states[i].label = - static_cast(state_machine_.transition_map.states[i].label); - } -} - -void -LifecycleNode::LifecycleNodeInterfaceImpl::on_get_available_transitions( - const std::shared_ptr header, - const std::shared_ptr req, - std::shared_ptr resp) const -{ - (void)header; - (void)req; - std::lock_guard lock(state_machine_mutex_); - if (rcl_lifecycle_state_machine_is_initialized(&state_machine_) != RCL_RET_OK) { - throw std::runtime_error( - "Can't get available transitions. State machine is not initialized."); - } - - resp->available_transitions.resize(state_machine_.current_state->valid_transition_size); - for (unsigned int i = 0; i < state_machine_.current_state->valid_transition_size; ++i) { - lifecycle_msgs::msg::TransitionDescription & trans_desc = resp->available_transitions[i]; - - auto rcl_transition = state_machine_.current_state->valid_transitions[i]; - trans_desc.transition.id = static_cast(rcl_transition.id); - trans_desc.transition.label = rcl_transition.label; - trans_desc.start_state.id = static_cast(rcl_transition.start->id); - trans_desc.start_state.label = rcl_transition.start->label; - trans_desc.goal_state.id = static_cast(rcl_transition.goal->id); - trans_desc.goal_state.label = rcl_transition.goal->label; - } -} - -void -LifecycleNode::LifecycleNodeInterfaceImpl::on_get_transition_graph( - const std::shared_ptr header, - const std::shared_ptr req, - std::shared_ptr resp) const -{ - (void)header; - (void)req; - std::lock_guard lock(state_machine_mutex_); - if (rcl_lifecycle_state_machine_is_initialized(&state_machine_) != RCL_RET_OK) { - throw std::runtime_error( - "Can't get available transitions. State machine is not initialized."); - } - - resp->available_transitions.resize(state_machine_.transition_map.transitions_size); - for (unsigned int i = 0; i < state_machine_.transition_map.transitions_size; ++i) { - lifecycle_msgs::msg::TransitionDescription & trans_desc = resp->available_transitions[i]; - - auto rcl_transition = state_machine_.transition_map.transitions[i]; - trans_desc.transition.id = static_cast(rcl_transition.id); - trans_desc.transition.label = rcl_transition.label; - trans_desc.start_state.id = static_cast(rcl_transition.start->id); - trans_desc.start_state.label = rcl_transition.start->label; - trans_desc.goal_state.id = static_cast(rcl_transition.goal->id); - trans_desc.goal_state.label = rcl_transition.goal->label; - } + return state_manager_hdl_->register_callback(lifecycle_transition, cb); } const State & LifecycleNode::LifecycleNodeInterfaceImpl::get_current_state() const { - return current_state_; + return state_manager_hdl_->get_current_state(); } std::vector LifecycleNode::LifecycleNodeInterfaceImpl::get_available_states() const { - std::vector states; - std::lock_guard lock(state_machine_mutex_); - states.reserve(state_machine_.transition_map.states_size); - - for (unsigned int i = 0; i < state_machine_.transition_map.states_size; ++i) { - states.emplace_back(&state_machine_.transition_map.states[i]); - } - return states; + return state_manager_hdl_->get_available_states(); } std::vector LifecycleNode::LifecycleNodeInterfaceImpl::get_available_transitions() const { - std::vector transitions; - std::lock_guard lock(state_machine_mutex_); - transitions.reserve(state_machine_.current_state->valid_transition_size); - - for (unsigned int i = 0; i < state_machine_.current_state->valid_transition_size; ++i) { - transitions.emplace_back(&state_machine_.current_state->valid_transitions[i]); - } - return transitions; + return state_manager_hdl_->get_available_transitions(); } std::vector LifecycleNode::LifecycleNodeInterfaceImpl::get_transition_graph() const { - std::vector transitions; - std::lock_guard lock(state_machine_mutex_); - transitions.reserve(state_machine_.transition_map.transitions_size); - - for (unsigned int i = 0; i < state_machine_.transition_map.transitions_size; ++i) { - transitions.emplace_back(&state_machine_.transition_map.transitions[i]); - } - return transitions; + return state_manager_hdl_->get_transition_graph(); } rcl_ret_t @@ -391,116 +115,7 @@ LifecycleNode::LifecycleNodeInterfaceImpl::change_state( std::uint8_t transition_id, node_interfaces::LifecycleNodeInterface::CallbackReturn & cb_return_code) { - constexpr bool publish_update = true; - State initial_state; - unsigned int current_state_id; - - { - std::lock_guard lock(state_machine_mutex_); - if (rcl_lifecycle_state_machine_is_initialized(&state_machine_) != RCL_RET_OK) { - RCUTILS_LOG_ERROR( - "Unable to change state for state machine for %s: %s", - node_base_interface_->get_name(), rcl_get_error_string().str); - return RCL_RET_ERROR; - } - - // keep the initial state to pass to a transition callback - initial_state = State(state_machine_.current_state); - - if ( - rcl_lifecycle_trigger_transition_by_id( - &state_machine_, transition_id, publish_update) != RCL_RET_OK) - { - RCUTILS_LOG_ERROR( - "Unable to start transition %u from current state %s: %s", - transition_id, state_machine_.current_state->label, rcl_get_error_string().str); - rcutils_reset_error(); - return RCL_RET_ERROR; - } - current_state_id = state_machine_.current_state->id; - } - - // Update the internal current_state_ - current_state_ = State(state_machine_.current_state); - - auto get_label_for_return_code = - [](node_interfaces::LifecycleNodeInterface::CallbackReturn cb_return_code) -> const char *{ - auto cb_id = static_cast(cb_return_code); - if (cb_id == lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_SUCCESS) { - return rcl_lifecycle_transition_success_label; - } else if (cb_id == lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_FAILURE) { - return rcl_lifecycle_transition_failure_label; - } - return rcl_lifecycle_transition_error_label; - }; - - cb_return_code = execute_callback(current_state_id, initial_state); - auto transition_label = get_label_for_return_code(cb_return_code); - - { - std::lock_guard lock(state_machine_mutex_); - if ( - rcl_lifecycle_trigger_transition_by_label( - &state_machine_, transition_label, publish_update) != RCL_RET_OK) - { - RCUTILS_LOG_ERROR( - "Failed to finish transition %u. Current state is now: %s (%s)", - transition_id, state_machine_.current_state->label, rcl_get_error_string().str); - rcutils_reset_error(); - return RCL_RET_ERROR; - } - current_state_id = state_machine_.current_state->id; - } - - // Update the internal current_state_ - current_state_ = State(state_machine_.current_state); - - // error handling ?! - // TODO(karsten1987): iterate over possible ret value - if (cb_return_code == node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR) { - RCUTILS_LOG_WARN("Error occurred while doing error handling."); - - auto error_cb_code = execute_callback(current_state_id, initial_state); - auto error_cb_label = get_label_for_return_code(error_cb_code); - std::lock_guard lock(state_machine_mutex_); - if ( - rcl_lifecycle_trigger_transition_by_label( - &state_machine_, error_cb_label, publish_update) != RCL_RET_OK) - { - RCUTILS_LOG_ERROR("Failed to call cleanup on error state: %s", rcl_get_error_string().str); - rcutils_reset_error(); - return RCL_RET_ERROR; - } - } - - // Update the internal current_state_ - current_state_ = State(state_machine_.current_state); - - // This true holds in both cases where the actual callback - // was successful or not, since at this point we have a valid transistion - // to either a new primary state or error state - return RCL_RET_OK; -} - -node_interfaces::LifecycleNodeInterface::CallbackReturn -LifecycleNode::LifecycleNodeInterfaceImpl::execute_callback( - unsigned int cb_id, const State & previous_state) const -{ - // in case no callback was attached, we forward directly - auto cb_success = node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; - - auto it = cb_map_.find(static_cast(cb_id)); - if (it != cb_map_.end()) { - auto callback = it->second; - try { - cb_success = callback(State(previous_state)); - } catch (const std::exception & e) { - RCUTILS_LOG_ERROR("Caught exception in callback for transition %d", it->first); - RCUTILS_LOG_ERROR("Original error: %s", e.what()); - cb_success = node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; - } - } - return cb_success; + return state_manager_hdl_->change_state(transition_id, cb_return_code); } const State & LifecycleNode::LifecycleNodeInterfaceImpl::trigger_transition( @@ -514,15 +129,11 @@ const State & LifecycleNode::LifecycleNodeInterfaceImpl::trigger_transition( const char * transition_label, node_interfaces::LifecycleNodeInterface::CallbackReturn & cb_return_code) { - const rcl_lifecycle_transition_t * transition; - { - std::lock_guard lock(state_machine_mutex_); + const rcl_lifecycle_transition_t * transition = + state_manager_hdl_->get_transition_by_label(transition_label); - transition = - rcl_lifecycle_get_transition_by_label(state_machine_.current_state, transition_label); - } if (transition) { - change_state(static_cast(transition->id), cb_return_code); + state_manager_hdl_->change_state(static_cast(transition->id), cb_return_code); } return get_current_state(); } @@ -531,7 +142,7 @@ const State & LifecycleNode::LifecycleNodeInterfaceImpl::trigger_transition(uint8_t transition_id) { node_interfaces::LifecycleNodeInterface::CallbackReturn error; - change_state(transition_id, error); + state_manager_hdl_->change_state(transition_id, error); (void) error; return get_current_state(); } @@ -541,44 +152,43 @@ LifecycleNode::LifecycleNodeInterfaceImpl::trigger_transition( uint8_t transition_id, node_interfaces::LifecycleNodeInterface::CallbackReturn & cb_return_code) { - change_state(transition_id, cb_return_code); + state_manager_hdl_->change_state(transition_id, cb_return_code); 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) { - weak_managed_entities_.push_back(managed_entity); + managed_entities_manager_hdl_->add_managed_entity(managed_entity); } void LifecycleNode::LifecycleNodeInterfaceImpl::add_timer_handle( std::shared_ptr timer) { - weak_timers_.push_back(timer); + managed_entities_manager_hdl_->add_timer_handle(timer); } void LifecycleNode::LifecycleNodeInterfaceImpl::on_activate() const { - for (const auto & weak_entity : weak_managed_entities_) { - auto entity = weak_entity.lock(); - if (entity) { - entity->on_activate(); - } - } + managed_entities_manager_hdl_->on_activate(); } void LifecycleNode::LifecycleNodeInterfaceImpl::on_deactivate() const { - for (const auto & weak_entity : weak_managed_entities_) { - auto entity = weak_entity.lock(); - if (entity) { - entity->on_deactivate(); - } - } + managed_entities_manager_hdl_->on_deactivate(); } } // namespace rclcpp_lifecycle diff --git a/rclcpp_lifecycle/src/lifecycle_node_interface_impl.hpp b/rclcpp_lifecycle/src/lifecycle_node_interface_impl.hpp index d09f44831c..fdbef7701e 100644 --- a/rclcpp_lifecycle/src/lifecycle_node_interface_impl.hpp +++ b/rclcpp_lifecycle/src/lifecycle_node_interface_impl.hpp @@ -38,17 +38,15 @@ #include "rmw/types.h" +#include "lifecycle_node_state_manager.hpp" +#include "lifecycle_node_state_services_manager.hpp" +#include "lifecycle_node_entities_manager.hpp" + namespace rclcpp_lifecycle { class LifecycleNode::LifecycleNodeInterfaceImpl final { - using ChangeStateSrv = lifecycle_msgs::srv::ChangeState; - using GetStateSrv = lifecycle_msgs::srv::GetState; - using GetAvailableStatesSrv = lifecycle_msgs::srv::GetAvailableStates; - using GetAvailableTransitionsSrv = lifecycle_msgs::srv::GetAvailableTransitions; - using TransitionEventMsg = lifecycle_msgs::msg::TransitionEvent; - public: LifecycleNodeInterfaceImpl( std::shared_ptr node_base_interface, @@ -64,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; @@ -76,6 +79,11 @@ class LifecycleNode::LifecycleNodeInterfaceImpl final std::vector get_transition_graph() const; + rcl_ret_t + change_state( + std::uint8_t transition_id, + node_interfaces::LifecycleNodeInterface::CallbackReturn & cb_return_code); + const State & trigger_transition(uint8_t transition_id); @@ -105,73 +113,17 @@ class LifecycleNode::LifecycleNodeInterfaceImpl final private: RCLCPP_DISABLE_COPY(LifecycleNodeInterfaceImpl) - void - on_change_state( - const std::shared_ptr header, - const std::shared_ptr req, - std::shared_ptr resp); - - void - on_get_state( - const std::shared_ptr header, - const std::shared_ptr req, - std::shared_ptr resp) const; - - void - on_get_available_states( - const std::shared_ptr header, - const std::shared_ptr req, - std::shared_ptr resp) const; - - void - on_get_available_transitions( - const std::shared_ptr header, - const std::shared_ptr req, - std::shared_ptr resp) const; - - void - on_get_transition_graph( - const std::shared_ptr header, - const std::shared_ptr req, - std::shared_ptr resp) const; - - rcl_ret_t - change_state( - std::uint8_t transition_id, - node_interfaces::LifecycleNodeInterface::CallbackReturn & cb_return_code); - - node_interfaces::LifecycleNodeInterface::CallbackReturn - execute_callback(unsigned int cb_id, const State & previous_state) const; - - mutable std::recursive_mutex state_machine_mutex_; - rcl_lifecycle_state_machine_t state_machine_; - State current_state_; - std::map< - std::uint8_t, - std::function> cb_map_; - using NodeBasePtr = std::shared_ptr; using NodeServicesPtr = std::shared_ptr; - using ChangeStateSrvPtr = std::shared_ptr>; - using GetStateSrvPtr = std::shared_ptr>; - using GetAvailableStatesSrvPtr = - std::shared_ptr>; - using GetAvailableTransitionsSrvPtr = - std::shared_ptr>; - using GetTransitionGraphSrvPtr = - std::shared_ptr>; + using NodeTimersPtr = std::shared_ptr; NodeBasePtr node_base_interface_; NodeServicesPtr node_services_interface_; - ChangeStateSrvPtr srv_change_state_; - GetStateSrvPtr srv_get_state_; - GetAvailableStatesSrvPtr srv_get_available_states_; - GetAvailableTransitionsSrvPtr srv_get_available_transitions_; - GetTransitionGraphSrvPtr srv_get_transition_graph_; - - // to controllable things - std::vector> weak_managed_entities_; - std::vector> weak_timers_; + NodeTimersPtr node_timers_interface_; + + std::shared_ptr state_manager_hdl_; + std::unique_ptr state_services_manager_hdl_; + std::unique_ptr managed_entities_manager_hdl_; }; } // namespace rclcpp_lifecycle diff --git a/rclcpp_lifecycle/src/lifecycle_node_state_manager.cpp b/rclcpp_lifecycle/src/lifecycle_node_state_manager.cpp new file mode 100644 index 0000000000..8c7967ba2d --- /dev/null +++ b/rclcpp_lifecycle/src/lifecycle_node_state_manager.cpp @@ -0,0 +1,614 @@ +// 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 +#include + +#include "lifecycle_node_state_services_manager.hpp" +#include "lifecycle_node_state_manager.hpp" +#include "change_state_handler_impl.hpp" + +namespace rclcpp_lifecycle +{ +void +LifecycleNodeStateManager::init( + const std::shared_ptr node_base_interface, + bool enable_communication_interface) +{ + using ChangeStateSrv = lifecycle_msgs::srv::ChangeState; + using GetStateSrv = lifecycle_msgs::srv::GetState; + using GetAvailableStatesSrv = lifecycle_msgs::srv::GetAvailableStates; + using GetAvailableTransitionsSrv = lifecycle_msgs::srv::GetAvailableTransitions; + + node_base_interface_ = node_base_interface; + + rcl_node_t * node_handle = node_base_interface_->get_rcl_node_handle(); + const rcl_node_options_t * node_options = + rcl_node_get_options(node_base_interface_->get_rcl_node_handle()); + auto state_machine_options = rcl_lifecycle_get_default_state_machine_options(); + state_machine_options.enable_com_interface = enable_communication_interface; + state_machine_options.allocator = node_options->allocator; + + // The call to initialize the state machine takes + // currently five different typesupports for all publishers/services + // created within the RCL_LIFECYCLE structure. + // The publisher takes a C-Typesupport since the publishing (i.e. creating + // the message) is done fully in RCL. + // Services are handled in C++, so that it needs a C++ typesupport structure. + std::lock_guard lock(state_machine_mutex_); + state_machine_ = rcl_lifecycle_get_zero_initialized_state_machine(); + rcl_ret_t ret = rcl_lifecycle_state_machine_init( + &state_machine_, + node_handle, + ROSIDL_GET_MSG_TYPE_SUPPORT(lifecycle_msgs, msg, TransitionEvent), + rosidl_typesupport_cpp::get_service_type_support_handle(), + rosidl_typesupport_cpp::get_service_type_support_handle(), + rosidl_typesupport_cpp::get_service_type_support_handle(), + rosidl_typesupport_cpp::get_service_type_support_handle(), + rosidl_typesupport_cpp::get_service_type_support_handle(), + &state_machine_options); + if (ret != RCL_RET_OK) { + throw std::runtime_error( + std::string("Couldn't initialize state machine for node ") + + node_base_interface_->get_name()); + } + + update_current_state_(); +} + +void +LifecycleNodeStateManager::throw_runtime_error_on_uninitialized_state_machine( + const std::string & attempted_action) const +{ + std::lock_guard lock(state_machine_mutex_); + if (rcl_lifecycle_state_machine_is_initialized(&state_machine_) != RCL_RET_OK) { + throw std::runtime_error( + "Can't " + attempted_action + ". State machine is not initialized."); + } +} + +bool +LifecycleNodeStateManager::register_callback( + std::uint8_t lifecycle_transition, + 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; +} + +const State & +LifecycleNodeStateManager::get_current_state() const +{ + return current_state_; +} + +std::vector +LifecycleNodeStateManager::get_available_states() const +{ + std::vector states; + std::lock_guard lock(state_machine_mutex_); + states.reserve(state_machine_.transition_map.states_size); + + for (unsigned int i = 0; i < state_machine_.transition_map.states_size; ++i) { + states.emplace_back(&state_machine_.transition_map.states[i]); + } + return states; +} + +std::vector +LifecycleNodeStateManager::get_available_transitions() const +{ + std::vector transitions; + std::lock_guard lock(state_machine_mutex_); + transitions.reserve(state_machine_.current_state->valid_transition_size); + + for (unsigned int i = 0; i < state_machine_.current_state->valid_transition_size; ++i) { + transitions.emplace_back(&state_machine_.current_state->valid_transitions[i]); + } + return transitions; +} + +std::vector +LifecycleNodeStateManager::get_transition_graph() const +{ + std::vector transitions; + std::lock_guard lock(state_machine_mutex_); + transitions.reserve(state_machine_.transition_map.transitions_size); + + for (unsigned int i = 0; i < state_machine_.transition_map.transitions_size; ++i) { + transitions.emplace_back(&state_machine_.transition_map.transitions[i]); + } + return transitions; +} + +bool +LifecycleNodeStateManager::is_transitioning() const +{ + return is_transitioning_.load(); +} + +rcl_ret_t +LifecycleNodeStateManager::change_state( + uint8_t transition_id, + node_interfaces::LifecycleNodeInterface::CallbackReturn & cb_return_code +) +{ + rcl_ret_t ret = change_state(transition_id); + cb_return_code = cb_return_code_; + return ret; +} + +rcl_ret_t +LifecycleNodeStateManager::change_state( + uint8_t transition_id, + std::function)> callback /*= nullptr*/, + const std::shared_ptr header /* = nullptr*/) +{ + if (is_transitioning()) { + RCUTILS_LOG_ERROR( + "%s currently in transition, failing requested transition id %d.", + node_base_interface_->get_name(), + transition_id); + if (callback) { + callback(false, header); + } + return RCL_RET_ERROR; + } + + is_transitioning_.store(true); + send_change_state_resp_cb_ = callback; + change_state_header_ = header; + transition_id_ = transition_id; + rcl_ret_ = RCL_RET_OK; + transition_cb_completed_ = false; + on_error_cb_completed_ = false; + + unsigned int current_state_id; + { + std::lock_guard lock(state_machine_mutex_); + constexpr bool publish_update = true; + if (rcl_lifecycle_state_machine_is_initialized(&state_machine_) != RCL_RET_OK) { + RCUTILS_LOG_ERROR( + "Unable to change state for state machine for %s: %s", + node_base_interface_->get_name(), rcl_get_error_string().str); + rcl_ret_error(); + return rcl_ret_; + } + + pre_transition_primary_state_ = State(state_machine_.current_state); + + if ( + rcl_lifecycle_trigger_transition_by_id( + &state_machine_, transition_id_, publish_update) != RCL_RET_OK) + { + RCUTILS_LOG_ERROR( + "Unable to start transition %u from current state %s: %s", + transition_id_, state_machine_.current_state->label, + rcl_get_error_string().str); + rcutils_reset_error(); + rcl_ret_error(); + return rcl_ret_; + } + current_state_id = get_current_state_id(); + update_current_state_(); + } + + 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_; +} + +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_) { + RCUTILS_LOG_ERROR( + "process_callback_resp recursively running user" + " transition function with id%d", + current_state_id); + rcl_ret_error(); + return; + } + transition_cb_completed_ = true; + process_user_callback_resp(cb_return_code); + } else if (in_error_transition_state(current_state_id)) { + if (on_error_cb_completed_) { + RCUTILS_LOG_ERROR( + "process_callback_resp recursively running user" + " transition function with id%d", + current_state_id); + rcl_ret_error(); + return; + } + on_error_cb_completed_ = true; + process_on_error_resp(cb_return_code); + } else { + RCUTILS_LOG_ERROR( + "process_callback_resp failed for %s: not in a transition state", + node_base_interface_->get_name()); + rcl_ret_error(); + } +} + +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) +{ + std::lock_guard lock(state_machine_mutex_); + + // Use transition.label over transition.id if transition.label exits + // label has higher precedence to the id due to ROS 2 defaulting integers to 0 + // e.g.: srv call of {transition: {label: configure}} + // transition.id = 0 -> would be equiv to "create" + // transition.label = "configure" -> id is 1, use this + if (req->transition.label.size() != 0) { + auto rcl_transition = rcl_lifecycle_get_transition_by_label( + state_machine_.current_state, req->transition.label.c_str()); + if (rcl_transition == nullptr) { + // send fail response printing out the requested label + RCUTILS_LOG_ERROR( + "ChangeState srv request failed: Transition label '%s'" + " is not available in the current state '%s'", + req->transition.label.c_str(), state_machine_.current_state->label); + return -1; + } + return static_cast(rcl_transition->id); + } + return req->transition.id; +} + +const rcl_lifecycle_transition_t * +LifecycleNodeStateManager::get_transition_by_label(const char * label) const +{ + std::lock_guard lock(state_machine_mutex_); + return + rcl_lifecycle_get_transition_by_label(state_machine_.current_state, label); +} + +rcl_lifecycle_com_interface_t & +LifecycleNodeStateManager::get_rcl_com_interface() +{ + return state_machine_.com_interface; +} + +void +LifecycleNodeStateManager::process_user_callback_resp( + node_interfaces::LifecycleNodeInterface::CallbackReturn cb_return_code) +{ + constexpr bool publish_update = true; + unsigned int current_state_id; + + cb_return_code_ = cb_return_code; + auto transition_label = get_label_for_return_code(cb_return_code); + { + std::lock_guard lock(state_machine_mutex_); + if ( + rcl_lifecycle_trigger_transition_by_label( + &state_machine_, transition_label, publish_update) != RCL_RET_OK) + { + RCUTILS_LOG_ERROR( + "Failed to finish transition %u: Current state is now: %s (%s)", + transition_id_, + state_machine_.current_state->label, + rcl_get_error_string().str); + rcutils_reset_error(); + rcl_ret_error(); + return; + } + current_state_id = get_current_state_id(); + + update_current_state_(); + } + + // error handling ?! + // 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."); + 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); + } +} + +void +LifecycleNodeStateManager::process_on_error_resp( + node_interfaces::LifecycleNodeInterface::CallbackReturn error_cb_code) +{ + constexpr bool publish_update = true; + auto error_cb_label = get_label_for_return_code(error_cb_code); + std::lock_guard lock(state_machine_mutex_); + if ( + rcl_lifecycle_trigger_transition_by_label( + &state_machine_, error_cb_label, publish_update) != RCL_RET_OK) + { + RCUTILS_LOG_ERROR("Failed to call cleanup on error state: %s", rcl_get_error_string().str); + rcutils_reset_error(); + rcl_ret_error(); + return; + } + finalize_change_state( + error_cb_code == node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS); +} + +void +LifecycleNodeStateManager::finalize_change_state(bool success) +{ + // TODO(karsten1987): Lifecycle msgs have to be extended to keep both returns + // 1. return is the actual transition + // 2. return is whether an error occurred or not + rcl_ret_ = success ? RCL_RET_OK : RCL_RET_ERROR; + update_current_state_(); + + if (send_change_state_resp_cb_) { + send_change_state_resp_cb_(success, change_state_header_); + change_state_header_.reset(); + send_change_state_resp_cb_ = nullptr; + } + is_transitioning_.store(false); +} + +node_interfaces::LifecycleNodeInterface::CallbackReturn +LifecycleNodeStateManager::execute_callback( + unsigned int cb_id, const State & previous_state) const +{ + // in case no callback was attached, we forward directly + auto cb_success = node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; + + auto it = cb_map_.find(static_cast(cb_id)); + if (it != cb_map_.end()) { + auto callback = it->second; + try { + cb_success = callback(State(previous_state)); + } catch (const std::exception & e) { + RCUTILS_LOG_ERROR("Caught exception in callback for transition %d", it->first); + RCUTILS_LOG_ERROR("Original error: %s", e.what()); + cb_success = node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; + } + } + 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) +{ + auto cb_id = static_cast(cb_return_code); + if (cb_id == lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_SUCCESS) { + return rcl_lifecycle_transition_success_label; + } else if (cb_id == lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_FAILURE) { + return rcl_lifecycle_transition_failure_label; + } + return rcl_lifecycle_transition_error_label; +} + +void +LifecycleNodeStateManager::rcl_ret_error() +{ + finalize_change_state(false); +} + +void +LifecycleNodeStateManager::update_current_state_() +{ + std::lock_guard lock(state_machine_mutex_); + current_state_ = State(state_machine_.current_state); +} + +uint8_t +LifecycleNodeStateManager::get_current_state_id() const +{ + std::lock_guard lock(state_machine_mutex_); + return state_machine_.current_state->id; +} + +bool +LifecycleNodeStateManager::in_non_error_transition_state( + uint8_t current_state_id) const +{ + return current_state_id == lifecycle_msgs::msg::State::TRANSITION_STATE_CONFIGURING || + current_state_id == lifecycle_msgs::msg::State::TRANSITION_STATE_CLEANINGUP || + current_state_id == lifecycle_msgs::msg::State::TRANSITION_STATE_SHUTTINGDOWN || + current_state_id == lifecycle_msgs::msg::State::TRANSITION_STATE_ACTIVATING || + current_state_id == lifecycle_msgs::msg::State::TRANSITION_STATE_DEACTIVATING; +} + +bool +LifecycleNodeStateManager::in_error_transition_state( + uint8_t current_state_id) const +{ + 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(); + rcl_ret_t ret; + { + std::lock_guard lock(state_machine_mutex_); + ret = rcl_lifecycle_state_machine_fini(&state_machine_, node_handle); + } + if (ret != RCL_RET_OK) { + RCUTILS_LOG_FATAL_NAMED( + "rclcpp_lifecycle", + "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(); +} + +} // namespace rclcpp_lifecycle diff --git a/rclcpp_lifecycle/src/lifecycle_node_state_manager.hpp b/rclcpp_lifecycle/src/lifecycle_node_state_manager.hpp new file mode 100644 index 0000000000..7918d0355b --- /dev/null +++ b/rclcpp_lifecycle/src/lifecycle_node_state_manager.hpp @@ -0,0 +1,192 @@ +// 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 LIFECYCLE_NODE_STATE_MANAGER_HPP_ +#define LIFECYCLE_NODE_STATE_MANAGER_HPP_ + +#include +#include +#include +#include +#include +#include +#include +#include + +#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" +#include "lifecycle_msgs/srv/get_available_transitions.hpp" + +#include "rcl_lifecycle/rcl_lifecycle.h" + +#include "rclcpp/macros.hpp" +#include "rclcpp/node_interfaces/node_base_interface.hpp" + +#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" + +#include "rmw/types.h" + +namespace rclcpp_lifecycle +{ +class ChangeStateHandlerImpl; // forward declaration + +class LifecycleNodeStateManager + : public std::enable_shared_from_this +{ +public: + using ChangeStateSrv = lifecycle_msgs::srv::ChangeState; + using CancelTransitionSrv = lifecycle_msgs::srv::CancelTransition; + + void init( + const std::shared_ptr node_base_interface, + bool enable_communication_interface); + + void throw_runtime_error_on_uninitialized_state_machine(const std::string & attempted_action) + const; + + bool + register_callback( + 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; + + std::vector get_available_transitions() const; + + std::vector get_transition_graph() const; + + bool is_transitioning() const; + + rcl_ret_t change_state( + uint8_t transition_id, + node_interfaces::LifecycleNodeInterface::CallbackReturn & cb_return_code); + + rcl_ret_t change_state( + uint8_t transition_id, + std::function)> callback = nullptr, + const std::shared_ptr header = nullptr); + + 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 + * Throws exception if state_machine_ is not initialized + * @return the transition id, returns -1 if the transition does not exist + */ + int get_transition_id_from_request(const ChangeStateSrv::Request::SharedPtr req); + + const rcl_lifecycle_transition_t * get_transition_by_label(const char * label) const; + + rcl_lifecycle_com_interface_t & get_rcl_com_interface(); + + virtual ~LifecycleNodeStateManager(); + +private: + /*NodeInterfaces*/ + std::shared_ptr node_base_interface_; + + /*StateMachine & Callback Maps*/ + mutable std::recursive_mutex state_machine_mutex_; + rcl_lifecycle_state_machine_t state_machine_; + State current_state_; + 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_; + std::function)> send_change_state_resp_cb_; + std::shared_ptr change_state_header_; + std::atomic is_transitioning_{false}; + State pre_transition_primary_state_; + uint8_t transition_id_; + bool transition_cb_completed_; + bool on_error_cb_completed_; + + node_interfaces::LifecycleNodeInterface::CallbackReturn cb_return_code_; + rcl_ret_t rcl_ret_; + + /*ChangeState Helpers*/ + void process_user_callback_resp( + node_interfaces::LifecycleNodeInterface::CallbackReturn cb_return_code); + + void process_on_error_resp( + node_interfaces::LifecycleNodeInterface::CallbackReturn error_cb_code); + + void finalize_change_state(bool success); + + 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); + + void rcl_ret_error(); + + void update_current_state_(); + + uint8_t get_current_state_id() const; + + 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 new file mode 100644 index 0000000000..7da2705141 --- /dev/null +++ b/rclcpp_lifecycle/src/lifecycle_node_state_services_manager.cpp @@ -0,0 +1,297 @@ +// 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 +#include + +#include "lifecycle_node_state_services_manager.hpp" +#include "lifecycle_node_state_manager.hpp" + +#include "rclcpp/node_interfaces/node_base_interface.hpp" +#include "rclcpp/node_interfaces/node_services_interface.hpp" +#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" + +namespace rclcpp_lifecycle +{ + +LifecycleNodeStateServicesManager::LifecycleNodeStateServicesManager( + std::shared_ptr node_base_interface, + std::shared_ptr node_services_interface, + const std::weak_ptr state_manager_hdl +) +: state_manager_hdl_(state_manager_hdl) +{ + if (auto state_manager_hdl = state_manager_hdl_.lock()) { + rcl_lifecycle_com_interface_t & state_machine_com_interface = + state_manager_hdl->get_rcl_com_interface(); + { // change_state + auto cb = std::bind( + &LifecycleNodeStateServicesManager::on_change_state, this, + std::placeholders::_1, std::placeholders::_2); + rclcpp::AnyServiceCallback any_cb; + any_cb.set(std::move(cb)); + + srv_change_state_ = std::make_shared>( + node_base_interface->get_shared_rcl_node_handle(), + &state_machine_com_interface.srv_change_state, + any_cb); + node_services_interface->add_service( + std::dynamic_pointer_cast(srv_change_state_), + nullptr); + } + + { // get_state + auto cb = std::bind( + &LifecycleNodeStateServicesManager::on_get_state, this, + std::placeholders::_1, std::placeholders::_2, std::placeholders::_3); + rclcpp::AnyServiceCallback any_cb; + any_cb.set(std::move(cb)); + + srv_get_state_ = std::make_shared>( + node_base_interface->get_shared_rcl_node_handle(), + &state_machine_com_interface.srv_get_state, + any_cb); + node_services_interface->add_service( + std::dynamic_pointer_cast(srv_get_state_), + nullptr); + } + + { // get_available_states + auto cb = std::bind( + &LifecycleNodeStateServicesManager::on_get_available_states, this, + std::placeholders::_1, std::placeholders::_2, std::placeholders::_3); + rclcpp::AnyServiceCallback any_cb; + any_cb.set(std::move(cb)); + + srv_get_available_states_ = std::make_shared>( + node_base_interface->get_shared_rcl_node_handle(), + &state_machine_com_interface.srv_get_available_states, + any_cb); + node_services_interface->add_service( + std::dynamic_pointer_cast(srv_get_available_states_), + nullptr); + } + + { // get_available_transitions + auto cb = std::bind( + &LifecycleNodeStateServicesManager::on_get_available_transitions, this, + std::placeholders::_1, std::placeholders::_2, std::placeholders::_3); + rclcpp::AnyServiceCallback any_cb; + any_cb.set(std::move(cb)); + + srv_get_available_transitions_ = + std::make_shared>( + node_base_interface->get_shared_rcl_node_handle(), + &state_machine_com_interface.srv_get_available_transitions, + any_cb); + node_services_interface->add_service( + std::dynamic_pointer_cast(srv_get_available_transitions_), + nullptr); + } + + { // get_transition_graph + auto cb = std::bind( + &LifecycleNodeStateServicesManager::on_get_transition_graph, this, + std::placeholders::_1, std::placeholders::_2, std::placeholders::_3); + rclcpp::AnyServiceCallback any_cb; + any_cb.set(std::move(cb)); + + srv_get_transition_graph_ = + std::make_shared>( + node_base_interface->get_shared_rcl_node_handle(), + &state_machine_com_interface.srv_get_transition_graph, + any_cb); + node_services_interface->add_service( + 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); + } + } +} + +void +LifecycleNodeStateServicesManager::send_change_state_resp( + bool success, + const std::shared_ptr header) const +{ + auto resp = std::make_unique(); + resp->success = success; + 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, + const std::shared_ptr req) +{ + auto state_hdl = state_manager_hdl_.lock(); + if (state_hdl) { + state_hdl->throw_runtime_error_on_uninitialized_state_machine("change state"); + int transition_id = state_hdl->get_transition_id_from_request(req); + if (transition_id < 0) { + send_change_state_resp(false, header); + } else { + state_hdl->change_state( + transition_id, + std::bind( + &LifecycleNodeStateServicesManager::send_change_state_resp, + this, std::placeholders::_1, std::placeholders::_2), + header); + } + } else { /*Unable to lock StateManager*/ + send_change_state_resp(false, header); + } +} + +void +LifecycleNodeStateServicesManager::on_get_state( + const std::shared_ptr header, + const std::shared_ptr req, + std::shared_ptr resp) const +{ + (void)header; + (void)req; + auto state_hdl = state_manager_hdl_.lock(); + if (state_hdl) { + state_hdl->throw_runtime_error_on_uninitialized_state_machine("get state"); + const State & current_state = state_hdl->get_current_state(); + resp->current_state.id = current_state.id(); + resp->current_state.label = current_state.label(); + } +} + +void +LifecycleNodeStateServicesManager::on_get_available_states( + const std::shared_ptr header, + const std::shared_ptr req, + std::shared_ptr resp) const +{ + (void)header; + (void)req; + auto state_hdl = state_manager_hdl_.lock(); + if (state_hdl) { + state_hdl->throw_runtime_error_on_uninitialized_state_machine("get available states"); + std::vector available_states = state_hdl->get_available_states(); + resp->available_states.resize(available_states.size()); + for (unsigned int i = 0; i < available_states.size(); ++i) { + resp->available_states[i].id = available_states[i].id(); + resp->available_states[i].label = available_states[i].label(); + } + } +} + +void +LifecycleNodeStateServicesManager::on_get_available_transitions( + const std::shared_ptr header, + const std::shared_ptr req, + std::shared_ptr resp) const +{ + (void)header; + (void)req; + auto state_hdl = state_manager_hdl_.lock(); + if (state_hdl) { + state_hdl->throw_runtime_error_on_uninitialized_state_machine("get available transitions"); + std::vector available_transitions = state_hdl->get_available_transitions(); + copy_transitions_vector_to_resp(available_transitions, resp); + } +} + +void +LifecycleNodeStateServicesManager::on_get_transition_graph( + const std::shared_ptr header, + const std::shared_ptr req, + std::shared_ptr resp) const +{ + (void)header; + (void)req; + auto state_hdl = state_manager_hdl_.lock(); + if (state_hdl) { + state_hdl->throw_runtime_error_on_uninitialized_state_machine("get transition graph"); + std::vector available_transitions = state_hdl->get_transition_graph(); + copy_transitions_vector_to_resp(available_transitions, resp); + } +} + +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, + std::shared_ptr resp) const +{ + resp->available_transitions.resize(transition_vec.size()); + for (unsigned int i = 0; i < transition_vec.size(); ++i) { + lifecycle_msgs::msg::TransitionDescription & trans_desc = resp->available_transitions[i]; + + Transition transition = transition_vec[i]; + trans_desc.transition.id = transition.id(); + trans_desc.transition.label = transition.label(); + trans_desc.start_state.id = transition.start_state().id(); + trans_desc.start_state.label = transition.start_state().label(); + trans_desc.goal_state.id = transition.goal_state().id(); + trans_desc.goal_state.label = transition.goal_state().label(); + } +} + +} // namespace rclcpp_lifecycle diff --git a/rclcpp_lifecycle/src/lifecycle_node_state_services_manager.hpp b/rclcpp_lifecycle/src/lifecycle_node_state_services_manager.hpp new file mode 100644 index 0000000000..afacdf4fd3 --- /dev/null +++ b/rclcpp_lifecycle/src/lifecycle_node_state_services_manager.hpp @@ -0,0 +1,126 @@ +// 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 LIFECYCLE_NODE_STATE_SERVICES_MANAGER_HPP_ +#define LIFECYCLE_NODE_STATE_SERVICES_MANAGER_HPP_ + +#include +#include +#include + +#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" +#include "lifecycle_msgs/srv/get_available_transitions.hpp" + +#include "rclcpp/node_interfaces/node_base_interface.hpp" +#include "rclcpp/node_interfaces/node_services_interface.hpp" +#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" + +#include "rclcpp_lifecycle/transition.hpp" +#include "lifecycle_node_state_manager.hpp" + +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; + using GetAvailableTransitionsSrv = lifecycle_msgs::srv::GetAvailableTransitions; + using TransitionEventMsg = lifecycle_msgs::msg::TransitionEvent; + +public: + LifecycleNodeStateServicesManager( + std::shared_ptr node_base_interface, + std::shared_ptr node_services_interface, + const std::weak_ptr state_manager_hdl); + + void send_change_state_resp( + 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( + const std::shared_ptr header, + const std::shared_ptr req); + + void + on_get_state( + const std::shared_ptr header, + const std::shared_ptr req, + std::shared_ptr resp) const; + + void + on_get_available_states( + const std::shared_ptr header, + const std::shared_ptr req, + std::shared_ptr resp) const; + + void + on_get_available_transitions( + const std::shared_ptr header, + const std::shared_ptr req, + std::shared_ptr resp) const; + + void + on_get_transition_graph( + const std::shared_ptr header, + 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, + std::shared_ptr resp) const; + + using ChangeStateSrvPtr = std::shared_ptr>; + using GetStateSrvPtr = std::shared_ptr>; + using GetAvailableStatesSrvPtr = + std::shared_ptr>; + using GetAvailableTransitionsSrvPtr = + 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_; +}; + +} // namespace rclcpp_lifecycle +#endif // LIFECYCLE_NODE_STATE_SERVICES_MANAGER_HPP_ 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 3698c807f6..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,6 +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 @@ -82,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 @@ -199,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_; @@ -208,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_; }; @@ -474,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 3c146e4207..2c3cd3f5a6 100644 --- a/rclcpp_lifecycle/test/test_register_custom_callbacks.cpp +++ b/rclcpp_lifecycle/test/test_register_custom_callbacks.cpp @@ -89,6 +89,7 @@ class CustomLifecycleNode : public rclcpp_lifecycle::LifecycleNode // Custom callbacks public: + // Synchronous callbacks rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_custom_configure(const rclcpp_lifecycle::State & previous_state) { @@ -132,9 +133,74 @@ 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_callbacks) { +TEST_F(TestRegisterCustomCallbacks, custom_synchronous_callbacks) { auto test_node = std::make_shared("testnode"); test_node->register_on_configure( @@ -178,3 +244,49 @@ TEST_F(TestRegisterCustomCallbacks, custom_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); +}