From 20fcb3266f679a7b620f13e5ce4703c39ff01bb5 Mon Sep 17 00:00:00 2001 From: Patrick Roncagliolo Date: Fri, 27 Jun 2025 12:14:44 +0200 Subject: [PATCH 1/2] `bt_navigator_***_rclcpp_node` - Namespacing + NodeOptions forwarding Closes #5242 Inspired by: #5202 - `bt_navigator_***_rclcpp_node` now gets the same namespace of `bt_navigator` - `bt_navigator_***_rclcpp_node` now gets the same node options of `bt_navigator` (apart from node name, that is still forced to respect the pattern `bt_navigator_***_rclcpp_node` ) Signed-off-by: Patrick Roncagliolo --- .../bt_action_server_impl.hpp | 30 +++++++++++++------ 1 file changed, 21 insertions(+), 9 deletions(-) diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp index 17e4f66bcba..4cf3d8026ee 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp @@ -125,6 +125,21 @@ template BtActionServer::~BtActionServer() {} +void replaceOrAddArgument( + std::vector & arguments, const std::string & option, + const std::string & arg_name, const std::string & new_argument) +{ + auto argument = std::find_if(arguments.begin(), arguments.end(), + [arg_name](const std::string & value){return value.find(arg_name) != std::string::npos;}); + if (argument != arguments.end()) { + *argument = new_argument; + } else { + arguments.push_back("--ros-args"); + arguments.push_back(option); + arguments.push_back(new_argument); + } +} + template bool BtActionServer::on_configure() { @@ -137,15 +152,12 @@ bool BtActionServer::on_configure() std::string client_node_name = action_name_; std::replace(client_node_name.begin(), client_node_name.end(), '/', '_'); // Use suffix '_rclcpp_node' to keep parameter file consistency #1773 - auto options = rclcpp::NodeOptions().arguments( - {"--ros-args", - "-r", - std::string("__node:=") + - std::string(node->get_name()) + "_" + client_node_name + "_rclcpp_node", - "-p", - "use_sim_time:=" + - std::string(node->get_parameter("use_sim_time").as_bool() ? "true" : "false"), - "--"}); + + auto new_arguments = node->get_node_options().arguments(); + replaceOrAddArgument(new_arguments, "-r", "__node", std::string("__node:=") + + std::string(node->get_name()) + "_" + client_node_name + "_rclcpp_node"); + auto options = node->get_node_options(); + options = options.arguments(new_arguments); // Support for handling the topic-based goal pose from rviz client_node_ = std::make_shared("_", options); From b25f3f070cb76cc5b1b789c145d65ad850e4189e Mon Sep 17 00:00:00 2001 From: Patrick Roncagliolo Date: Sat, 28 Jun 2025 11:17:40 +0200 Subject: [PATCH 2/2] Deduplicate `replaceOrAddArgument` implementation Moved to `nav2_ros_common/node_utils.hpp`, namespace `nav2` Signed-off-by: Patrick Roncagliolo --- .../bt_action_server_impl.hpp | 17 +-------------- .../nav2_costmap_2d/costmap_2d_ros.hpp | 9 -------- nav2_costmap_2d/src/costmap_2d_ros.cpp | 21 +++---------------- .../include/nav2_ros_common/node_utils.hpp | 20 ++++++++++++++++++ 4 files changed, 24 insertions(+), 43 deletions(-) diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp index 4cf3d8026ee..184f3d94aca 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp @@ -125,21 +125,6 @@ template BtActionServer::~BtActionServer() {} -void replaceOrAddArgument( - std::vector & arguments, const std::string & option, - const std::string & arg_name, const std::string & new_argument) -{ - auto argument = std::find_if(arguments.begin(), arguments.end(), - [arg_name](const std::string & value){return value.find(arg_name) != std::string::npos;}); - if (argument != arguments.end()) { - *argument = new_argument; - } else { - arguments.push_back("--ros-args"); - arguments.push_back(option); - arguments.push_back(new_argument); - } -} - template bool BtActionServer::on_configure() { @@ -154,7 +139,7 @@ bool BtActionServer::on_configure() // Use suffix '_rclcpp_node' to keep parameter file consistency #1773 auto new_arguments = node->get_node_options().arguments(); - replaceOrAddArgument(new_arguments, "-r", "__node", std::string("__node:=") + + nav2::replaceOrAddArgument(new_arguments, "-r", "__node", std::string("__node:=") + std::string(node->get_name()) + "_" + client_node_name + "_rclcpp_node"); auto options = node->get_node_options(); options = options.arguments(new_arguments); diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp index 878b7e6ecc5..1bf1479c89d 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp @@ -432,15 +432,6 @@ class Costmap2DROS : public nav2::LifecycleNode // free functions -/** - * @brief Given a specified argument name, replaces it with the specified - * new value. If the argument is not in the existing list, a new argument - * is created with the specified option. - */ -void replaceOrAddArgument( - std::vector & arguments, const std::string & option, - const std::string & arg_name, const std::string & new_argument); - /** * @brief Given the node options of a parent node, expands of replaces * the fields for the node name, namespace and use_sim_time diff --git a/nav2_costmap_2d/src/costmap_2d_ros.cpp b/nav2_costmap_2d/src/costmap_2d_ros.cpp index 01f49894a06..22f32ae88c1 100644 --- a/nav2_costmap_2d/src/costmap_2d_ros.cpp +++ b/nav2_costmap_2d/src/costmap_2d_ros.cpp @@ -71,21 +71,6 @@ Costmap2DROS::Costmap2DROS(const rclcpp::NodeOptions & options) init(); } -void replaceOrAddArgument( - std::vector & arguments, const std::string & option, - const std::string & arg_name, const std::string & new_argument) -{ - auto argument = std::find_if(arguments.begin(), arguments.end(), - [arg_name](const std::string & value){return value.find(arg_name) != std::string::npos;}); - if (argument != arguments.end()) { - *argument = new_argument; - } else { - arguments.push_back("--ros-args"); - arguments.push_back(option); - arguments.push_back(new_argument); - } -} - rclcpp::NodeOptions getChildNodeOptions( const std::string & name, const std::string & parent_namespace, @@ -93,10 +78,10 @@ rclcpp::NodeOptions getChildNodeOptions( const rclcpp::NodeOptions & parent_options) { std::vector new_arguments = parent_options.arguments(); - replaceOrAddArgument(new_arguments, "-r", "__ns", + nav2::replaceOrAddArgument(new_arguments, "-r", "__ns", "__ns:=" + nav2::add_namespaces(parent_namespace, name)); - replaceOrAddArgument(new_arguments, "-r", "__node", name + ":" + "__node:=" + name); - replaceOrAddArgument(new_arguments, "-p", "use_sim_time", + nav2::replaceOrAddArgument(new_arguments, "-r", "__node", name + ":" + "__node:=" + name); + nav2::replaceOrAddArgument(new_arguments, "-p", "use_sim_time", "use_sim_time:=" + std::string(use_sim_time ? "true" : "false")); return rclcpp::NodeOptions().arguments(new_arguments); } diff --git a/nav2_ros_common/include/nav2_ros_common/node_utils.hpp b/nav2_ros_common/include/nav2_ros_common/node_utils.hpp index 6cf10f79d69..469a9b34e7c 100644 --- a/nav2_ros_common/include/nav2_ros_common/node_utils.hpp +++ b/nav2_ros_common/include/nav2_ros_common/node_utils.hpp @@ -378,6 +378,26 @@ inline void setIntrospectionMode( #endif } +/** + * @brief Given a specified argument name, replaces it with the specified + * new value. If the argument is not in the existing list, a new argument + * is created with the specified option. + */ +inline void replaceOrAddArgument( + std::vector & arguments, const std::string & option, + const std::string & arg_name, const std::string & new_argument) +{ + auto argument = std::find_if(arguments.begin(), arguments.end(), + [arg_name](const std::string & value){return value.find(arg_name) != std::string::npos;}); + if (argument != arguments.end()) { + *argument = new_argument; + } else { + arguments.push_back("--ros-args"); + arguments.push_back(option); + arguments.push_back(new_argument); + } +} + } // namespace nav2 #endif // NAV2_ROS_COMMON__NODE_UTILS_HPP_