From a0eff3c52dcf63ad499da0f88daa2df9bceed220 Mon Sep 17 00:00:00 2001 From: yadunund Date: Mon, 6 Jan 2025 10:12:43 -0800 Subject: [PATCH 1/2] Pin working version of rclrs (#50) * Pin working version of rclrs Signed-off-by: Yadunund * Pin home in Cargo.toml Signed-off-by: Luca Della Vedova --------- Signed-off-by: Yadunund Signed-off-by: Luca Della Vedova Co-authored-by: Luca Della Vedova --- .github/workflows/nexus_workcell_editor.yaml | 2 +- nexus_workcell_editor/Cargo.toml | 2 ++ nexus_workcell_editor/README.md | 9 +++------ 3 files changed, 6 insertions(+), 7 deletions(-) diff --git a/.github/workflows/nexus_workcell_editor.yaml b/.github/workflows/nexus_workcell_editor.yaml index 73e5357..67f713a 100644 --- a/.github/workflows/nexus_workcell_editor.yaml +++ b/.github/workflows/nexus_workcell_editor.yaml @@ -34,7 +34,7 @@ jobs: run: | git clone https://github.com/ros2-rust/ros2_rust.git cd ros2_rust - git checkout f45a66f47dc727e3ccb13037a6c57923af1446c7 + git checkout 9a845c17873cbdf49e8017d5f0af6d8f795589cc cd .. vcs import . < ros2_rust/ros2_rust_jazzy.repos - name: rosdep diff --git a/nexus_workcell_editor/Cargo.toml b/nexus_workcell_editor/Cargo.toml index 899d193..510d6d7 100644 --- a/nexus_workcell_editor/Cargo.toml +++ b/nexus_workcell_editor/Cargo.toml @@ -10,6 +10,8 @@ name = "nexus_workcell_editor" [dependencies] bevy = "0.12" bevy_egui = "0.23" +# TODO(luca) Fix upstream by removing the open_url feature from bevy_egui +home = "=0.5.9" # TODO(luca) Just use the version used by site editor once released bevy_impulse = { git = "https://github.com/open-rmf/bevy_impulse", branch = "main" } clap = { version = "4.0.10", features = ["color", "derive", "help", "usage", "suggestions"] } diff --git a/nexus_workcell_editor/README.md b/nexus_workcell_editor/README.md index 51e7be6..8ef4dfb 100644 --- a/nexus_workcell_editor/README.md +++ b/nexus_workcell_editor/README.md @@ -4,18 +4,15 @@ A GUI for assembling workcells from components that is built off [rmf_site](http ## Setup -Install rustup from the Rust website: https://www.rust-lang.org/tools/install - -``` -curl --proto '=https' --tlsv1.2 -sSf https://sh.rustup.rs | sh -``` - Follow instructions [here](https://github.com/ros2-rust/ros2_rust) to setup ros2_rust. +> Note: Checkout `9a845c17873cbdf49e8017d5f0af6d8f795589cc` commit to include fix for https://github.com/ros2-rust/ros2_rust/issues/449. + ## Build ``` # source the ros distro and ros2_rust workspace. cd ~/ws_nexus +rosdep install --from-paths src --ignore-src --rosdistro # Replace with supported ROS 2 distro, eg. jazzy. colcon build ``` From 5ffa61ae97f76d9b6e3708cee59ad57f1c63fe6f Mon Sep 17 00:00:00 2001 From: Luca Della Vedova Date: Wed, 8 Jan 2025 12:38:38 +0800 Subject: [PATCH 2/2] Fix registration logic in lifecycle manager (#52) * Fix registration logic in lifecycle manager Signed-off-by: Luca Della Vedova * Make sure to use state, not transition Signed-off-by: Luca Della Vedova * Remove _system_active in favor of lifecycle state Signed-off-by: Luca Della Vedova * Convert target state to optional Signed-off-by: Luca Della Vedova * Add warning Signed-off-by: Luca Della Vedova * Update nexus_lifecycle_manager/include/nexus_lifecycle_manager/lifecycle_manager.hpp Co-authored-by: yadunund --------- Signed-off-by: Luca Della Vedova Co-authored-by: yadunund --- .../lifecycle_manager.hpp | 117 ++++++------------ 1 file changed, 37 insertions(+), 80 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..4fa945b 100644 --- a/nexus_lifecycle_manager/include/nexus_lifecycle_manager/lifecycle_manager.hpp +++ b/nexus_lifecycle_manager/include/nexus_lifecycle_manager/lifecycle_manager.hpp @@ -87,10 +87,9 @@ class Implementation std::shared_ptr>> _node_clients; - 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 + std::optional _target_state = std::nullopt; std::shared_ptr spin_thread_; // Callback group used by services @@ -117,11 +116,17 @@ class Implementation { std::optional state = client->get_state(); if (!state.has_value()) + { ok = false; + RCLCPP_WARN( + this->node_->get_logger(), "Failed to get state for node [%s]. Skipping state change...", + name.c_str()); + 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(); } return ok; } @@ -131,7 +136,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() @@ -164,14 +169,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(); @@ -181,8 +183,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) || @@ -199,8 +199,6 @@ class Implementation } bool pause() { - this->_system_active = false; - this->message("Pausing managed nodes..."); if (!this->changeStateForAllNodes(Transition::TRANSITION_DEACTIVATE)) { @@ -226,7 +224,6 @@ class Implementation } this->message("Managed nodes are active"); - this->_system_active = true; return true; } @@ -336,78 +333,34 @@ 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 + if (this->_target_state.has_value()) { - if (_node_clients.size() > 0) + std::vector solution = transitionGraph.dijkstra(currentState, this->_target_state.value()); + for (unsigned int i = 0; i < solution.size(); 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)) + if (!ChangeState(name, solution[i])) { - message("The state of new node to add was not recheable, " - "Keep the new node is unconfigure state"); + RCLCPP_ERROR( + this->node_->get_logger(), + "Not able to transition the node [%s]. This node is not inserted", + name.c_str()); 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; - } - } } - _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 +418,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 = State::PRIMARY_STATE_ACTIVE; + } _pimpl->_service_request_timeout = std::chrono::seconds( service_request_timeout); @@ -552,7 +509,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_)