From 780b4749784e487a2c254706a7ed955bec0272b4 Mon Sep 17 00:00:00 2001 From: Nick Lamprianidis Date: Thu, 8 Jun 2023 01:40:25 +0200 Subject: [PATCH] Fix duplicate node The node created for the costmap converter was not properly set up. The name passed to the node constructor had no effect. It is now passed as a ros argument instead. --- teb_local_planner/src/teb_local_planner_ros.cpp | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) diff --git a/teb_local_planner/src/teb_local_planner_ros.cpp b/teb_local_planner/src/teb_local_planner_ros.cpp index 6060d8b0..07061a01 100644 --- a/teb_local_planner/src/teb_local_planner_ros.cpp +++ b/teb_local_planner/src/teb_local_planner_ros.cpp @@ -85,10 +85,14 @@ void TebLocalPlannerROS::initialize(nav2_util::LifecycleNode::SharedPtr node) // check if the plugin is already initialized if(!initialized_) { + auto options = rclcpp::NodeOptions().arguments( + {"--ros-args", "-r", + std::string("__node:=") + node->get_name() + "_" + name_ + + "_costmap_converter_rclcpp_node", + "--"}); + intra_proc_node_ = std::make_shared("_", options); + // declare parameters (ros2-dashing) - intra_proc_node_.reset( - new rclcpp::Node("costmap_converter", node->get_namespace(), - rclcpp::NodeOptions())); cfg_->declareParameters(node, name_); // get parameters of TebConfig via the nodehandle and override the default config