diff --git a/rclcpp_lifecycle/CMakeLists.txt b/rclcpp_lifecycle/CMakeLists.txt index f46f993b21..4ffff3cab8 100644 --- a/rclcpp_lifecycle/CMakeLists.txt +++ b/rclcpp_lifecycle/CMakeLists.txt @@ -23,7 +23,10 @@ find_package(rosidl_typesupport_cpp REQUIRED) ### CPP High level library add_library(rclcpp_lifecycle 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 diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp index 197ecf5c19..3b1b175b5a 100644 --- a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp @@ -1004,7 +1004,8 @@ class LifecycleNode : public node_interfaces::LifecycleNodeInterface, */ RCLCPP_LIFECYCLE_PUBLIC bool - register_on_configure(std::function fcn); + register_on_configure( + std::function fcn); /// Register the cleanup callback /** @@ -1014,7 +1015,8 @@ class LifecycleNode : public node_interfaces::LifecycleNodeInterface, */ RCLCPP_LIFECYCLE_PUBLIC bool - register_on_cleanup(std::function fcn); + register_on_cleanup( + std::function fcn); /// Register the shutdown callback /** @@ -1024,7 +1026,8 @@ class LifecycleNode : public node_interfaces::LifecycleNodeInterface, */ RCLCPP_LIFECYCLE_PUBLIC bool - register_on_shutdown(std::function fcn); + register_on_shutdown( + std::function fcn); /// Register the activate callback /** @@ -1034,7 +1037,8 @@ class LifecycleNode : public node_interfaces::LifecycleNodeInterface, */ RCLCPP_LIFECYCLE_PUBLIC bool - register_on_activate(std::function fcn); + register_on_activate( + std::function fcn); /// Register the deactivate callback /** @@ -1044,7 +1048,8 @@ class LifecycleNode : public node_interfaces::LifecycleNodeInterface, */ RCLCPP_LIFECYCLE_PUBLIC bool - register_on_deactivate(std::function fcn); + register_on_deactivate( + std::function fcn); /// Register the error callback /** @@ -1054,7 +1059,8 @@ class LifecycleNode : public node_interfaces::LifecycleNodeInterface, */ RCLCPP_LIFECYCLE_PUBLIC bool - register_on_error(std::function fcn); + register_on_error( + std::function fcn); RCLCPP_LIFECYCLE_PUBLIC CallbackReturn diff --git a/rclcpp_lifecycle/src/lifecycle_node.cpp b/rclcpp_lifecycle/src/lifecycle_node.cpp index 4c0b94cb42..041d13bfa8 100644 --- a/rclcpp_lifecycle/src/lifecycle_node.cpp +++ b/rclcpp_lifecycle/src/lifecycle_node.cpp @@ -541,6 +541,7 @@ LifecycleNode::register_on_error( 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..49e8d8c568 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,7 +152,7 @@ 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(); } @@ -549,36 +160,26 @@ 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..eb01ce4728 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, @@ -76,6 +74,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 +108,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..ab394c556a --- /dev/null +++ b/rclcpp_lifecycle/src/lifecycle_node_state_manager.cpp @@ -0,0 +1,461 @@ +// 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" + +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; + 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_(); + } + + 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) +{ + 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(); + } +} + +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."); + 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; +} + +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; +} + +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; + change_state_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..36304902f5 --- /dev/null +++ b/rclcpp_lifecycle/src/lifecycle_node_state_manager.hpp @@ -0,0 +1,153 @@ +// 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/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; + + 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); + + 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); + + /** + * @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_; + + /*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; + + 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; +}; +} // 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..b0f40facfa --- /dev/null +++ b/rclcpp_lifecycle/src/lifecycle_node_state_services_manager.cpp @@ -0,0 +1,245 @@ +// 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); + } + } +} + +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::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::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..d469dd1180 --- /dev/null +++ b/rclcpp_lifecycle/src/lifecycle_node_state_services_manager.hpp @@ -0,0 +1,111 @@ +// 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/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 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; + +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 + 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>; + + ChangeStateSrvPtr srv_change_state_; + GetStateSrvPtr srv_get_state_; + GetAvailableStatesSrvPtr srv_get_available_states_; + GetAvailableTransitionsSrvPtr srv_get_available_transitions_; + GetTransitionGraphSrvPtr srv_get_transition_graph_; + + std::weak_ptr state_manager_hdl_; +}; + +} // namespace rclcpp_lifecycle +#endif // LIFECYCLE_NODE_STATE_SERVICES_MANAGER_HPP_ diff --git a/rclcpp_lifecycle/test/test_lifecycle_service_client.cpp b/rclcpp_lifecycle/test/test_lifecycle_service_client.cpp index 3698c807f6..31338fa075 100644 --- a/rclcpp_lifecycle/test/test_lifecycle_service_client.cpp +++ b/rclcpp_lifecycle/test/test_lifecycle_service_client.cpp @@ -54,6 +54,7 @@ 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"; + const lifecycle_msgs::msg::State unknown_state = lifecycle_msgs::msg::State(); class EmptyLifecycleNode : public rclcpp_lifecycle::LifecycleNode diff --git a/rclcpp_lifecycle/test/test_register_custom_callbacks.cpp b/rclcpp_lifecycle/test/test_register_custom_callbacks.cpp index 3c146e4207..2a46098ee9 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) { @@ -134,7 +135,7 @@ class CustomLifecycleNode : public rclcpp_lifecycle::LifecycleNode } }; -TEST_F(TestRegisterCustomCallbacks, custom_callbacks) { +TEST_F(TestRegisterCustomCallbacks, custom_synchronous_callbacks) { auto test_node = std::make_shared("testnode"); test_node->register_on_configure(