From 0c958dc994f5e618fef2471c0851a2bc3b19b1d5 Mon Sep 17 00:00:00 2001 From: Luca Della Vedova Date: Mon, 25 Nov 2024 12:55:24 +0800 Subject: [PATCH] Use private parameter for parent namespace Signed-off-by: Luca Della Vedova --- nav2_costmap_2d/src/costmap_2d_ros.cpp | 7 +++---- nav2_costmap_2d/src/costmap_layer.cpp | 9 ++++++--- 2 files changed, 9 insertions(+), 7 deletions(-) diff --git a/nav2_costmap_2d/src/costmap_2d_ros.cpp b/nav2_costmap_2d/src/costmap_2d_ros.cpp index fa11b60ebf..c51b26f965 100644 --- a/nav2_costmap_2d/src/costmap_2d_ros.cpp +++ b/nav2_costmap_2d/src/costmap_2d_ros.cpp @@ -70,8 +70,8 @@ Costmap2DROS::Costmap2DROS(const rclcpp::NodeOptions & options) "nav2_costmap_2d::ObstacleLayer", "nav2_costmap_2d::InflationLayer"} { - declare_parameter("map_topic", rclcpp::ParameterValue(std::string("map"))); is_lifecycle_follower_ = false; + declare_parameter("__parent_namespace", std::string(get_namespace())); init(); } @@ -99,9 +99,7 @@ Costmap2DROS::Costmap2DROS( "nav2_costmap_2d::ObstacleLayer", "nav2_costmap_2d::InflationLayer"} { - declare_parameter( - "map_topic", rclcpp::ParameterValue( - (parent_namespace_ == "/" ? "/" : parent_namespace_ + "/") + std::string("map"))); + declare_parameter("__parent_namespace", parent_namespace); init(); } @@ -114,6 +112,7 @@ void Costmap2DROS::init() declare_parameter("footprint_padding", rclcpp::ParameterValue(0.01f)); declare_parameter("footprint", rclcpp::ParameterValue(std::string("[]"))); declare_parameter("global_frame", rclcpp::ParameterValue(std::string("map"))); + declare_parameter("map_topic", rclcpp::ParameterValue(std::string("map"))); declare_parameter("height", rclcpp::ParameterValue(5)); declare_parameter("width", rclcpp::ParameterValue(5)); declare_parameter("lethal_cost_threshold", rclcpp::ParameterValue(100)); diff --git a/nav2_costmap_2d/src/costmap_layer.cpp b/nav2_costmap_2d/src/costmap_layer.cpp index 93f20f664d..6d41affe67 100644 --- a/nav2_costmap_2d/src/costmap_layer.cpp +++ b/nav2_costmap_2d/src/costmap_layer.cpp @@ -273,9 +273,12 @@ std::string CostmapLayer::joinWithParentNamespace(const std::string & topic) } if (topic[0] != '/') { - std::string node_namespace = node->get_namespace(); - std::string parent_namespace = node_namespace.substr(0, node_namespace.rfind("/")); - return parent_namespace + "/" + topic; + auto parent_param = node->get_parameter("__parent_namespace"); + if (parent_param.get_type() != rclcpp::ParameterType::PARAMETER_STRING) { + throw std::runtime_error{"__parent_namespace parameter not found or wrong type"}; + } + auto parent_ns = parent_param.as_string(); + return (parent_ns == "/" ? "/" : parent_ns + "/") + topic; } return topic;