From 7c8222d54cb1c1f84b65e8a75d4bbf714681b287 Mon Sep 17 00:00:00 2001 From: Marco Bassa Date: Fri, 30 May 2025 09:41:06 +0200 Subject: [PATCH 1/2] Passing parent node options from controller_server to costmap node Signed-off-by: Marco Bassa --- nav2_controller/src/controller_server.cpp | 2 +- .../nav2_costmap_2d/costmap_2d_ros.hpp | 24 ++++++++++- nav2_costmap_2d/src/costmap_2d_ros.cpp | 41 +++++++++++++++---- 3 files changed, 58 insertions(+), 9 deletions(-) diff --git a/nav2_controller/src/controller_server.cpp b/nav2_controller/src/controller_server.cpp index 2597c277996..ba58dc67d29 100644 --- a/nav2_controller/src/controller_server.cpp +++ b/nav2_controller/src/controller_server.cpp @@ -68,7 +68,7 @@ ControllerServer::ControllerServer(const rclcpp::NodeOptions & options) // The costmap node is used in the implementation of the controller costmap_ros_ = std::make_shared( "local_costmap", std::string{get_namespace()}, - get_parameter("use_sim_time").as_bool()); + get_parameter("use_sim_time").as_bool(), options); } ControllerServer::~ControllerServer() 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 b7f3bf80c83..8bd71add71d 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 @@ -91,7 +91,8 @@ class Costmap2DROS : public nav2_util::LifecycleNode explicit Costmap2DROS( const std::string & name, const std::string & parent_namespace = "/", - const bool & use_sim_time = false); + const bool & use_sim_time = false, + const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); /** * @brief Common initialization for constructors @@ -430,6 +431,27 @@ class Costmap2DROS : public nav2_util::LifecycleNode dynamicParametersCallback(std::vector parameters); }; +// 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 + */ +rclcpp::NodeOptions getChildNodeOptions( + const std::string & name, + const std::string & parent_namespace, + const bool & use_sim_time, + const rclcpp::NodeOptions & parent_options); + } // namespace nav2_costmap_2d #endif // NAV2_COSTMAP_2D__COSTMAP_2D_ROS_HPP_ diff --git a/nav2_costmap_2d/src/costmap_2d_ros.cpp b/nav2_costmap_2d/src/costmap_2d_ros.cpp index d2a8646507a..9c3edb21df6 100644 --- a/nav2_costmap_2d/src/costmap_2d_ros.cpp +++ b/nav2_costmap_2d/src/costmap_2d_ros.cpp @@ -71,21 +71,48 @@ 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, + const bool & use_sim_time, + const rclcpp::NodeOptions & parent_options) +{ + std::vector new_arguments = parent_options.arguments(); + replaceOrAddArgument(new_arguments, "-r", "__ns", + "__ns:=" + nav2_util::add_namespaces(parent_namespace, name)); + replaceOrAddArgument(new_arguments, "-r", "__node", name + ":" + "__node:=" + name); + replaceOrAddArgument(new_arguments, "-p", "use_sim_time", + "use_sim_time:=" + std::string(use_sim_time ? "true" : "false")); + return rclcpp::NodeOptions().arguments(new_arguments); +} + Costmap2DROS::Costmap2DROS( const std::string & name, const std::string & parent_namespace, - const bool & use_sim_time) + const bool & use_sim_time, + const rclcpp::NodeOptions & options) : nav2_util::LifecycleNode(name, "", // NodeOption arguments take precedence over the ones provided on the command line // use this to make sure the node is placed on the provided namespace // TODO(orduno) Pass a sub-node instead of creating a new node for better handling // of the namespaces - rclcpp::NodeOptions().arguments({ - "--ros-args", "-r", std::string("__ns:=") + - nav2_util::add_namespaces(parent_namespace, name), - "--ros-args", "-r", name + ":" + std::string("__node:=") + name, - "--ros-args", "-p", "use_sim_time:=" + std::string(use_sim_time ? "true" : "false"), - })), + getChildNodeOptions(name, parent_namespace, use_sim_time, options) +), name_(name), default_plugins_{"static_layer", "obstacle_layer", "inflation_layer"}, default_types_{ From cfb4fca6e6817f8a16ef15f06049b987f76a643f Mon Sep 17 00:00:00 2001 From: Marco Bassa Date: Fri, 30 May 2025 10:26:01 +0200 Subject: [PATCH 2/2] Passing parent node options to global costmap Signed-off-by: Marco Bassa --- nav2_planner/src/planner_server.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nav2_planner/src/planner_server.cpp b/nav2_planner/src/planner_server.cpp index ac1d1efd89d..15b1001faab 100644 --- a/nav2_planner/src/planner_server.cpp +++ b/nav2_planner/src/planner_server.cpp @@ -64,7 +64,7 @@ PlannerServer::PlannerServer(const rclcpp::NodeOptions & options) // Setup the global costmap costmap_ros_ = std::make_shared( "global_costmap", std::string{get_namespace()}, - get_parameter("use_sim_time").as_bool()); + get_parameter("use_sim_time").as_bool(), options); } PlannerServer::~PlannerServer()