From 9e4d2f4c9488c124a519e13a88fb8b4e7000fec5 Mon Sep 17 00:00:00 2001 From: Luca Della Vedova Date: Tue, 7 Jan 2025 11:55:02 +0800 Subject: [PATCH 1/6] Fix registration logic in lifecycle manager Signed-off-by: Luca Della Vedova --- .../lifecycle_manager.hpp | 98 ++++++------------- 1 file changed, 28 insertions(+), 70 deletions(-) diff --git a/nexus_lifecycle_manager/include/nexus_lifecycle_manager/lifecycle_manager.hpp b/nexus_lifecycle_manager/include/nexus_lifecycle_manager/lifecycle_manager.hpp index 1c07a7a..be8233c 100644 --- a/nexus_lifecycle_manager/include/nexus_lifecycle_manager/lifecycle_manager.hpp +++ b/nexus_lifecycle_manager/include/nexus_lifecycle_manager/lifecycle_manager.hpp @@ -89,8 +89,8 @@ class Implementation bool _system_active{false}; std::chrono::seconds _service_request_timeout{10s}; - // autostart, if set to true, will transition the first added node to ACTIVE - bool _autostart{false}; + // Lifecycle state that managed nodes should be in + uint8_t _target_state = 0; std::shared_ptr spin_thread_; // Callback group used by services @@ -123,6 +123,7 @@ class Implementation transitionGraph.atGoalState(state.value(), transition) || client->change_state(transition)); } + this->_target_state = transition; return ok; } @@ -336,78 +337,31 @@ class Implementation bool add_node(const std::string& name) { - // If this is the first node, then the node will be transitioned to active - if (this->_node_clients.size() == 0) + uint8_t currentState; + if (!this->GetState(name, currentState)) { - uint8_t currentState; - if (!this->GetState(name, currentState)) - { - message("The state of current nodes were not recheable, " - "Keep the new node is unconfigure state"); - return false; - } - - if (_autostart) - { - std::vector solution = transitionGraph.dijkstra(currentState, 3); - for (unsigned int i = 0; i < solution.size(); i++) - { - if (!ChangeState(name, solution[i])) - { - RCLCPP_ERROR( - this->node_->get_logger(), - "Not able to transition the node [%s]. This node is not inserted", - name.c_str()); - return false; - } - currentState = solution[i]; - } - } - _node_clients.insert_or_assign(name, - std::make_shared>( - name)); - return true; + message("The state of new node to add was not reacheable. " + "This node is not inserted"); + return false; } - else + + // If the target state is different from the current, transition this node to it + std::vector solution = transitionGraph.dijkstra(currentState, this->_target_state); + for (unsigned int i = 0; i < solution.size(); i++) { - if (_node_clients.size() > 0) + if (!ChangeState(name, solution[i])) { - uint8_t targetState; - if (!this->GetState(this->_node_clients.begin()->first, targetState)) - { - message("The state of current nodes were not recheable, " - "Keep the new node is unconfigure state"); - return false; - } - - uint8_t currentState; - if (!this->GetState(name, currentState)) - { - message("The state of new node to add was not recheable, " - "Keep the new node is unconfigure state"); - return false; - } - - std::vector solution = transitionGraph.dijkstra(currentState, - targetState); - for (unsigned int i = 0; i < solution.size(); i++) - { - if (!ChangeState(name, solution[i])) - { - RCLCPP_ERROR( - this->node_->get_logger(), - "Not able to transition the node [%s]. This node is not inserted", - name.c_str()); - return false; - } - } + RCLCPP_ERROR( + this->node_->get_logger(), + "Not able to transition the node [%s]. This node is not inserted", + name.c_str()); + return false; } - _node_clients.insert_or_assign(name, - std::make_shared>( - name)); - return true; } - return false; + _node_clients.insert_or_assign(name, + std::make_shared>( + name)); + return true; } bool change_state(const std::string& node_name, const std::uint8_t goal) @@ -465,7 +419,11 @@ class LifecycleManager { _pimpl = rmf_utils::make_impl>(); _pimpl->_are_services_active = activate_services; - _pimpl->_autostart = autostart; + // Set the target state to active if autostart is enabled + if (autostart) + { + _pimpl->_target_state = 3; + } _pimpl->_service_request_timeout = std::chrono::seconds( service_request_timeout); @@ -552,7 +510,7 @@ class LifecycleManager return _pimpl->add_node(node_name_); } - /// \brief REmove a node from the list + /// \brief Remove a node from the list /// \param[in] node_name_ Node name to remove /// \return True if the node name was remove from the list, false otherwise bool removeNodeName(const std::string& node_name_) From da8e4258cded6c6bae4c50e356fb2b6ea158cab7 Mon Sep 17 00:00:00 2001 From: Luca Della Vedova Date: Tue, 7 Jan 2025 12:28:15 +0800 Subject: [PATCH 2/6] Make sure to use state, not transition Signed-off-by: Luca Della Vedova --- .../nexus_lifecycle_manager/lifecycle_manager.hpp | 14 ++++++++------ 1 file changed, 8 insertions(+), 6 deletions(-) diff --git a/nexus_lifecycle_manager/include/nexus_lifecycle_manager/lifecycle_manager.hpp b/nexus_lifecycle_manager/include/nexus_lifecycle_manager/lifecycle_manager.hpp index be8233c..f9b6874 100644 --- a/nexus_lifecycle_manager/include/nexus_lifecycle_manager/lifecycle_manager.hpp +++ b/nexus_lifecycle_manager/include/nexus_lifecycle_manager/lifecycle_manager.hpp @@ -90,7 +90,7 @@ class Implementation bool _system_active{false}; std::chrono::seconds _service_request_timeout{10s}; // Lifecycle state that managed nodes should be in - uint8_t _target_state = 0; + uint8_t _target_state = State::PRIMARY_STATE_UNCONFIGURED; std::shared_ptr spin_thread_; // Callback group used by services @@ -117,13 +117,15 @@ class Implementation { std::optional state = client->get_state(); if (!state.has_value()) + { ok = false; + continue; + } - ok = ok && ( - transitionGraph.atGoalState(state.value(), transition) || - client->change_state(transition)); + ok &= transitionGraph.atGoalState(state.value(), transition) || + client->change_state(transition); + this->_target_state = state.value(); } - this->_target_state = transition; return ok; } @@ -422,7 +424,7 @@ class LifecycleManager // Set the target state to active if autostart is enabled if (autostart) { - _pimpl->_target_state = 3; + _pimpl->_target_state = State::PRIMARY_STATE_ACTIVE; } _pimpl->_service_request_timeout = std::chrono::seconds( service_request_timeout); From 410b119d3d0269241af25b67917f7c7e8c9297aa Mon Sep 17 00:00:00 2001 From: Luca Della Vedova Date: Tue, 7 Jan 2025 12:28:35 +0800 Subject: [PATCH 3/6] Remove _system_active in favor of lifecycle state Signed-off-by: Luca Della Vedova --- .../nexus_lifecycle_manager/lifecycle_manager.hpp | 11 +---------- 1 file changed, 1 insertion(+), 10 deletions(-) diff --git a/nexus_lifecycle_manager/include/nexus_lifecycle_manager/lifecycle_manager.hpp b/nexus_lifecycle_manager/include/nexus_lifecycle_manager/lifecycle_manager.hpp index f9b6874..4d786b7 100644 --- a/nexus_lifecycle_manager/include/nexus_lifecycle_manager/lifecycle_manager.hpp +++ b/nexus_lifecycle_manager/include/nexus_lifecycle_manager/lifecycle_manager.hpp @@ -87,7 +87,6 @@ class Implementation std::shared_ptr>> _node_clients; - bool _system_active{false}; std::chrono::seconds _service_request_timeout{10s}; // Lifecycle state that managed nodes should be in uint8_t _target_state = State::PRIMARY_STATE_UNCONFIGURED; @@ -134,7 +133,7 @@ class Implementation const std::shared_ptr /*request*/, std::shared_ptr response) { - response->success = this->_system_active; + response->success = this->_target_state == State::PRIMARY_STATE_ACTIVE; } void shutdownAllNodes() @@ -167,14 +166,11 @@ class Implementation return false; } this->message("Managed nodes are active"); - this->_system_active = true; return true; } bool shutdown() { - this->_system_active = false; - this->message("Shutting down managed nodes..."); this->shutdownAllNodes(); this->destroyLifecycleServiceClients(); @@ -184,8 +180,6 @@ class Implementation bool reset() { - this->_system_active = false; - this->message("Resetting managed nodes..."); // Should transition in reverse order if (!this->changeStateForAllNodes(Transition::TRANSITION_DEACTIVATE) || @@ -202,8 +196,6 @@ class Implementation } bool pause() { - this->_system_active = false; - this->message("Pausing managed nodes..."); if (!this->changeStateForAllNodes(Transition::TRANSITION_DEACTIVATE)) { @@ -229,7 +221,6 @@ class Implementation } this->message("Managed nodes are active"); - this->_system_active = true; return true; } From c3200d5f2abdd57339699e461b41b552a9d0e163 Mon Sep 17 00:00:00 2001 From: Luca Della Vedova Date: Tue, 7 Jan 2025 18:06:01 +0800 Subject: [PATCH 4/6] Convert target state to optional Signed-off-by: Luca Della Vedova --- .../lifecycle_manager.hpp | 21 +++++++++++-------- 1 file changed, 12 insertions(+), 9 deletions(-) diff --git a/nexus_lifecycle_manager/include/nexus_lifecycle_manager/lifecycle_manager.hpp b/nexus_lifecycle_manager/include/nexus_lifecycle_manager/lifecycle_manager.hpp index 4d786b7..5dcb2e7 100644 --- a/nexus_lifecycle_manager/include/nexus_lifecycle_manager/lifecycle_manager.hpp +++ b/nexus_lifecycle_manager/include/nexus_lifecycle_manager/lifecycle_manager.hpp @@ -89,7 +89,7 @@ class Implementation std::chrono::seconds _service_request_timeout{10s}; // Lifecycle state that managed nodes should be in - uint8_t _target_state = State::PRIMARY_STATE_UNCONFIGURED; + std::optional _target_state = std::nullopt; std::shared_ptr spin_thread_; // Callback group used by services @@ -339,16 +339,19 @@ class Implementation } // If the target state is different from the current, transition this node to it - std::vector solution = transitionGraph.dijkstra(currentState, this->_target_state); - for (unsigned int i = 0; i < solution.size(); i++) + if (this->_target_state.has_value()) { - if (!ChangeState(name, solution[i])) + std::vector solution = transitionGraph.dijkstra(currentState, this->_target_state.value()); + for (unsigned int i = 0; i < solution.size(); i++) { - RCLCPP_ERROR( - this->node_->get_logger(), - "Not able to transition the node [%s]. This node is not inserted", - name.c_str()); - return false; + if (!ChangeState(name, solution[i])) + { + RCLCPP_ERROR( + this->node_->get_logger(), + "Not able to transition the node [%s]. This node is not inserted", + name.c_str()); + return false; + } } } _node_clients.insert_or_assign(name, From 967fade404187ce19724bd929b71588bcdf89cb6 Mon Sep 17 00:00:00 2001 From: Luca Della Vedova Date: Wed, 8 Jan 2025 10:11:18 +0800 Subject: [PATCH 5/6] Add warning Signed-off-by: Luca Della Vedova --- .../include/nexus_lifecycle_manager/lifecycle_manager.hpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/nexus_lifecycle_manager/include/nexus_lifecycle_manager/lifecycle_manager.hpp b/nexus_lifecycle_manager/include/nexus_lifecycle_manager/lifecycle_manager.hpp index 5dcb2e7..0ea4697 100644 --- a/nexus_lifecycle_manager/include/nexus_lifecycle_manager/lifecycle_manager.hpp +++ b/nexus_lifecycle_manager/include/nexus_lifecycle_manager/lifecycle_manager.hpp @@ -118,6 +118,9 @@ class Implementation if (!state.has_value()) { ok = false; + RCLCPP_WARN( + this->node_->get_logger(), "Failed to get state for node [%s]", + name.c_str()); continue; } From 40e147371578e90508f4421d9461c618b878b469 Mon Sep 17 00:00:00 2001 From: Luca Della Vedova Date: Wed, 8 Jan 2025 10:23:06 +0800 Subject: [PATCH 6/6] Update nexus_lifecycle_manager/include/nexus_lifecycle_manager/lifecycle_manager.hpp Co-authored-by: yadunund --- .../include/nexus_lifecycle_manager/lifecycle_manager.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nexus_lifecycle_manager/include/nexus_lifecycle_manager/lifecycle_manager.hpp b/nexus_lifecycle_manager/include/nexus_lifecycle_manager/lifecycle_manager.hpp index 0ea4697..4fa945b 100644 --- a/nexus_lifecycle_manager/include/nexus_lifecycle_manager/lifecycle_manager.hpp +++ b/nexus_lifecycle_manager/include/nexus_lifecycle_manager/lifecycle_manager.hpp @@ -119,7 +119,7 @@ class Implementation { ok = false; RCLCPP_WARN( - this->node_->get_logger(), "Failed to get state for node [%s]", + this->node_->get_logger(), "Failed to get state for node [%s]. Skipping state change...", name.c_str()); continue; }