From 7eeed40b247ec194f4f4b1fe9adeb275f4a5effc Mon Sep 17 00:00:00 2001 From: Tomohito Ando Date: Wed, 27 Aug 2025 15:17:02 +0900 Subject: [PATCH 1/4] feat: deduplicate node loading if FQN already exists Signed-off-by: Tomohito Ando --- rclcpp_components/src/component_manager.cpp | 29 +++++++++++++++++++++ 1 file changed, 29 insertions(+) diff --git a/rclcpp_components/src/component_manager.cpp b/rclcpp_components/src/component_manager.cpp index 7b77af9c92..e0b3afe78e 100644 --- a/rclcpp_components/src/component_manager.cpp +++ b/rclcpp_components/src/component_manager.cpp @@ -223,6 +223,35 @@ ComponentManager::on_load_node( (void) request_header; try { + // check if node already exists in the container + if (!request->node_name.empty()) { + const std::string & ns = request->node_namespace; + std::string requested_fqn; + if (ns.empty() || ns == "/") { + requested_fqn = "/" + request->node_name; + } else if (ns.back() == '/') { + requested_fqn = ns + request->node_name; + } else { + requested_fqn = ns + "/" + request->node_name; + } + + // scan existing nodes + for (auto & kv : node_wrappers_) { + const auto base = kv.second.get_node_base_interface(); + if (base && base->get_fully_qualified_name() == requested_fqn) { + // already exists -> return existing instance + response->full_node_name = requested_fqn; + response->unique_id = kv.first; + response->success = true; + RCLCPP_WARN( + get_logger(), + "[LoadNode] Deduplicated : node '%s' already exists. Skipping load.", + requested_fqn.c_str()); + return; + } + } + } + auto resources = get_component_resources(request->package_name); for (const auto & resource : resources) { From 97ae27940975f40f5de31c5e54be04f2cad87386 Mon Sep 17 00:00:00 2001 From: Tomohito Ando Date: Tue, 2 Sep 2025 19:47:13 +0900 Subject: [PATCH 2/4] extract duplication check logic into separate method Signed-off-by: Tomohito Ando --- .../rclcpp_components/component_manager.hpp | 12 ++++ rclcpp_components/src/component_manager.cpp | 66 +++++++++++-------- 2 files changed, 52 insertions(+), 26 deletions(-) diff --git a/rclcpp_components/include/rclcpp_components/component_manager.hpp b/rclcpp_components/include/rclcpp_components/component_manager.hpp index 6238284278..41f4b2bff2 100644 --- a/rclcpp_components/include/rclcpp_components/component_manager.hpp +++ b/rclcpp_components/include/rclcpp_components/component_manager.hpp @@ -165,6 +165,18 @@ class ComponentManager : public rclcpp::Node virtual void remove_node_from_executor(uint64_t node_id); + /// Check if a node with the same name already exists in the container + /** + * \param request information with the node to load + * \param response response to be filled if node already exists + * \return true if node already exists, false otherwise + */ + RCLCPP_COMPONENTS_PUBLIC + virtual bool + check_node_duplication( + const std::shared_ptr request, + std::shared_ptr response); + /// Service callback to load a new node in the component /** * This function allows to add parameters, remap rules, a specific node, name a namespace diff --git a/rclcpp_components/src/component_manager.cpp b/rclcpp_components/src/component_manager.cpp index e0b3afe78e..c694e79dcd 100644 --- a/rclcpp_components/src/component_manager.cpp +++ b/rclcpp_components/src/component_manager.cpp @@ -214,6 +214,44 @@ ComponentManager::remove_node_from_executor(uint64_t node_id) } } +bool +ComponentManager::check_node_duplication( + const std::shared_ptr request, + std::shared_ptr response) +{ + if (request->node_name.empty()) { + return false; + } + + const std::string & ns = request->node_namespace; + std::string requested_fqn; + if (ns.empty() || ns == "/") { + requested_fqn = "/" + request->node_name; + } else if (ns.back() == '/') { + requested_fqn = ns + request->node_name; + } else { + requested_fqn = ns + "/" + request->node_name; + } + + // scan existing nodes + for (auto & kv : node_wrappers_) { + const auto base = kv.second.get_node_base_interface(); + if (base && base->get_fully_qualified_name() == requested_fqn) { + // already exists -> return existing instance + response->full_node_name = requested_fqn; + response->unique_id = kv.first; + response->success = true; + RCLCPP_WARN( + get_logger(), + "[LoadNode] Deduplicated : node '%s' already exists. Skipping load.", + requested_fqn.c_str()); + return true; + } + } + + return false; +} + void ComponentManager::on_load_node( const std::shared_ptr request_header, @@ -224,32 +262,8 @@ ComponentManager::on_load_node( try { // check if node already exists in the container - if (!request->node_name.empty()) { - const std::string & ns = request->node_namespace; - std::string requested_fqn; - if (ns.empty() || ns == "/") { - requested_fqn = "/" + request->node_name; - } else if (ns.back() == '/') { - requested_fqn = ns + request->node_name; - } else { - requested_fqn = ns + "/" + request->node_name; - } - - // scan existing nodes - for (auto & kv : node_wrappers_) { - const auto base = kv.second.get_node_base_interface(); - if (base && base->get_fully_qualified_name() == requested_fqn) { - // already exists -> return existing instance - response->full_node_name = requested_fqn; - response->unique_id = kv.first; - response->success = true; - RCLCPP_WARN( - get_logger(), - "[LoadNode] Deduplicated : node '%s' already exists. Skipping load.", - requested_fqn.c_str()); - return; - } - } + if (check_node_duplication(request, response)) { + return; } auto resources = get_component_resources(request->package_name); From 7b69d4695a512f3a2a1f14aa9d4756ac14df35f5 Mon Sep 17 00:00:00 2001 From: Tomohito Ando Date: Tue, 2 Sep 2025 19:56:25 +0900 Subject: [PATCH 3/4] use std::find_if instead of for loop Signed-off-by: Tomohito Ando --- rclcpp_components/src/component_manager.cpp | 31 ++++++++++++--------- 1 file changed, 18 insertions(+), 13 deletions(-) diff --git a/rclcpp_components/src/component_manager.cpp b/rclcpp_components/src/component_manager.cpp index c694e79dcd..c979b0291d 100644 --- a/rclcpp_components/src/component_manager.cpp +++ b/rclcpp_components/src/component_manager.cpp @@ -234,19 +234,24 @@ ComponentManager::check_node_duplication( } // scan existing nodes - for (auto & kv : node_wrappers_) { - const auto base = kv.second.get_node_base_interface(); - if (base && base->get_fully_qualified_name() == requested_fqn) { - // already exists -> return existing instance - response->full_node_name = requested_fqn; - response->unique_id = kv.first; - response->success = true; - RCLCPP_WARN( - get_logger(), - "[LoadNode] Deduplicated : node '%s' already exists. Skipping load.", - requested_fqn.c_str()); - return true; - } + auto existing_node = std::find_if( + node_wrappers_.begin(), + node_wrappers_.end(), + [&requested_fqn](auto & kv) { + const auto base = kv.second.get_node_base_interface(); + return base && base->get_fully_qualified_name() == requested_fqn; + }); + + if (existing_node != node_wrappers_.end()) { + // already exists -> return existing instance + response->full_node_name = requested_fqn; + response->unique_id = existing_node->first; + response->success = true; + RCLCPP_WARN( + get_logger(), + "[LoadNode] Deduplicated : node '%s' already exists. Skipping load.", + requested_fqn.c_str()); + return true; } return false; From 1c318fdbb39546521efe0c371e79d04e7aa72cf8 Mon Sep 17 00:00:00 2001 From: Tomohito Ando Date: Wed, 3 Sep 2025 08:14:14 +0900 Subject: [PATCH 4/4] refactor: move check_node_duplication to independent function Signed-off-by: Tomohito Ando --- .../rclcpp_components/component_manager.hpp | 12 --- rclcpp_components/src/component_manager.cpp | 101 ++++++++++-------- 2 files changed, 57 insertions(+), 56 deletions(-) diff --git a/rclcpp_components/include/rclcpp_components/component_manager.hpp b/rclcpp_components/include/rclcpp_components/component_manager.hpp index 41f4b2bff2..6238284278 100644 --- a/rclcpp_components/include/rclcpp_components/component_manager.hpp +++ b/rclcpp_components/include/rclcpp_components/component_manager.hpp @@ -165,18 +165,6 @@ class ComponentManager : public rclcpp::Node virtual void remove_node_from_executor(uint64_t node_id); - /// Check if a node with the same name already exists in the container - /** - * \param request information with the node to load - * \param response response to be filled if node already exists - * \return true if node already exists, false otherwise - */ - RCLCPP_COMPONENTS_PUBLIC - virtual bool - check_node_duplication( - const std::shared_ptr request, - std::shared_ptr response); - /// Service callback to load a new node in the component /** * This function allows to add parameters, remap rules, a specific node, name a namespace diff --git a/rclcpp_components/src/component_manager.cpp b/rclcpp_components/src/component_manager.cpp index c979b0291d..d5871deb72 100644 --- a/rclcpp_components/src/component_manager.cpp +++ b/rclcpp_components/src/component_manager.cpp @@ -29,6 +29,62 @@ using namespace std::placeholders; namespace rclcpp_components { +namespace +{ + /// Check if a node with the same name already exists in the container + /** + * \param request information with the node to load + * \param node_wrappers map of nodes in the container + * \param logger logger + * \param response response to be filled if node already exists + * \return true if node already exists, false otherwise + */ +bool check_node_duplication( + const std::shared_ptr request, + const std::map & node_wrappers, + const rclcpp::Logger & logger, + std::shared_ptr response) +{ + if (request->node_name.empty()) { + return false; + } + + const std::string & ns = request->node_namespace; + std::string requested_fqn; + if (ns.empty() || ns == "/") { + requested_fqn = "/" + request->node_name; + } else if (ns.back() == '/') { + requested_fqn = ns + request->node_name; + } else { + requested_fqn = ns + "/" + request->node_name; + } + + // scan existing nodes + auto existing_node = std::find_if( + node_wrappers.cbegin(), + node_wrappers.cend(), + [&requested_fqn](const auto & kv) { + // Copy to call non-const method from const reference + auto wrapper = kv.second; + const auto base = wrapper.get_node_base_interface(); + return base && base->get_fully_qualified_name() == requested_fqn; + }); + + if (existing_node != node_wrappers.cend()) { + // already exists -> return existing instance + response->full_node_name = requested_fqn; + response->unique_id = existing_node->first; + response->success = true; + RCLCPP_WARN( + logger, + "[LoadNode] Deduplicated : node '%s' already exists. Skipping load.", + requested_fqn.c_str()); + return true; + } + + return false; +} +} // namespace (anonymous) ComponentManager::ComponentManager( std::weak_ptr executor, @@ -214,49 +270,6 @@ ComponentManager::remove_node_from_executor(uint64_t node_id) } } -bool -ComponentManager::check_node_duplication( - const std::shared_ptr request, - std::shared_ptr response) -{ - if (request->node_name.empty()) { - return false; - } - - const std::string & ns = request->node_namespace; - std::string requested_fqn; - if (ns.empty() || ns == "/") { - requested_fqn = "/" + request->node_name; - } else if (ns.back() == '/') { - requested_fqn = ns + request->node_name; - } else { - requested_fqn = ns + "/" + request->node_name; - } - - // scan existing nodes - auto existing_node = std::find_if( - node_wrappers_.begin(), - node_wrappers_.end(), - [&requested_fqn](auto & kv) { - const auto base = kv.second.get_node_base_interface(); - return base && base->get_fully_qualified_name() == requested_fqn; - }); - - if (existing_node != node_wrappers_.end()) { - // already exists -> return existing instance - response->full_node_name = requested_fqn; - response->unique_id = existing_node->first; - response->success = true; - RCLCPP_WARN( - get_logger(), - "[LoadNode] Deduplicated : node '%s' already exists. Skipping load.", - requested_fqn.c_str()); - return true; - } - - return false; -} - void ComponentManager::on_load_node( const std::shared_ptr request_header, @@ -267,7 +280,7 @@ ComponentManager::on_load_node( try { // check if node already exists in the container - if (check_node_duplication(request, response)) { + if (check_node_duplication(request, node_wrappers_, get_logger(), response)) { return; }