From bc4093837519dcbd13f786a2b358a8070d496a3a Mon Sep 17 00:00:00 2001
From: Luca Della Vedova <lucadv@intrinsic.ai>
Date: Wed, 20 Nov 2024 12:17:55 +0800
Subject: [PATCH] Refactor parsing function to common, add for rest of layers

Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>
---
 .../include/nav2_costmap_2d/costmap_layer.hpp    | 16 ++++++++++++++++
 nav2_costmap_2d/plugins/obstacle_layer.cpp       | 12 +-----------
 nav2_costmap_2d/plugins/range_sensor_layer.cpp   |  1 +
 nav2_costmap_2d/plugins/static_layer.cpp         |  1 +
 nav2_costmap_2d/src/costmap_layer.cpp            | 16 ++++++++++++++++
 5 files changed, 35 insertions(+), 11 deletions(-)

diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_layer.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_layer.hpp
index 43a82e0cde1..5e0924550ae 100644
--- a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_layer.hpp
+++ b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_layer.hpp
@@ -192,6 +192,22 @@ class CostmapLayer : public Layer, public Costmap2D
  */
   CombinationMethod combination_method_from_int(const int value);
 
+  /**
+   * Joins the specified topic with the parent namespace of the costmap node.
+   * If the topic has an absolute path, it is returned instead.
+   *
+   * This is necessary for user defined relative topics to work as expected since costmap layers
+   * add a an additional `costmap_name` namespace to the topic.
+   * For example:
+   *   * User chosen namespace is `tb4`.
+   *   * User chosen topic is `scan`.
+   *   * Costmap node namespace will be `/tb4/global_costmap`.
+   *   * Without this function, the topic would be `/tb4/global_costmap/scan`.
+   *   * With this function, topic will be remapped to `/tb4/scan`.
+   *
+   * @param topic the topic to parse
+   */
+  std::string joinWithParentNamespace(const std::string & topic);
 private:
   double extra_min_x_, extra_max_x_, extra_min_y_, extra_max_y_;
 };
diff --git a/nav2_costmap_2d/plugins/obstacle_layer.cpp b/nav2_costmap_2d/plugins/obstacle_layer.cpp
index eb78fbcc510..3e20092a9ce 100644
--- a/nav2_costmap_2d/plugins/obstacle_layer.cpp
+++ b/nav2_costmap_2d/plugins/obstacle_layer.cpp
@@ -189,17 +189,7 @@ void ObstacleLayer::onInitialize()
     node->get_parameter(name_ + "." + source + "." + "raytrace_min_range", raytrace_min_range);
     node->get_parameter(name_ + "." + source + "." + "raytrace_max_range", raytrace_max_range);
 
-    // If the user passed a relative topic, use it as relative to this node's parent namespace.
-    // For example:
-    //   * User chosen namespace is `tb4`.
-    //   * User chosen topic is `scan`.
-    //   * Node namespace will be `/tb4/global_costmap`.
-    //   * Topic will be remapped to `/tb4/scan` rather than `/tb4/global_costmap/scan`
-    if (topic[0] != '/') {
-      std::string node_namespace = node->get_namespace();
-      std::string parent_namespace = node_namespace.substr(0, node_namespace.rfind("/"));
-      topic = parent_namespace + "/" + topic;
-    }
+    topic = joinWithParentNamespace(topic);
 
     RCLCPP_DEBUG(
       logger_,
diff --git a/nav2_costmap_2d/plugins/range_sensor_layer.cpp b/nav2_costmap_2d/plugins/range_sensor_layer.cpp
index ffbe8a3ef8b..dc28d4f4342 100644
--- a/nav2_costmap_2d/plugins/range_sensor_layer.cpp
+++ b/nav2_costmap_2d/plugins/range_sensor_layer.cpp
@@ -130,6 +130,7 @@ void RangeSensorLayer::onInitialize()
 
   // Traverse the topic names list subscribing to all of them with the same callback method
   for (auto & topic_name : topic_names) {
+    topic_name = joinWithParentNamespace(topic_name);
     if (input_sensor_type == InputSensorType::VARIABLE) {
       processRangeMessageFunc_ = std::bind(
         &RangeSensorLayer::processVariableRangeMsg, this,
diff --git a/nav2_costmap_2d/plugins/static_layer.cpp b/nav2_costmap_2d/plugins/static_layer.cpp
index 67b7fbd5ec9..249fd9effdc 100644
--- a/nav2_costmap_2d/plugins/static_layer.cpp
+++ b/nav2_costmap_2d/plugins/static_layer.cpp
@@ -155,6 +155,7 @@ StaticLayer::getParameters()
   } else {
     map_topic_ = global_map_topic;
   }
+  map_topic_ = joinWithParentNamespace(map_topic_);
   node->get_parameter(
     name_ + "." + "map_subscribe_transient_local",
     map_subscribe_transient_local_);
diff --git a/nav2_costmap_2d/src/costmap_layer.cpp b/nav2_costmap_2d/src/costmap_layer.cpp
index 8dbce5017f0..93f20f664dc 100644
--- a/nav2_costmap_2d/src/costmap_layer.cpp
+++ b/nav2_costmap_2d/src/costmap_layer.cpp
@@ -264,4 +264,20 @@ CombinationMethod CostmapLayer::combination_method_from_int(const int value)
       return CombinationMethod::Max;
   }
 }
+
+std::string CostmapLayer::joinWithParentNamespace(const std::string & topic)
+{
+  auto node = node_.lock();
+  if (!node) {
+    throw std::runtime_error{"Failed to lock node"};
+  }
+
+  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;
+  }
+
+  return topic;
+}
 }  // namespace nav2_costmap_2d