Refactor parsing function to common, add for rest of layers

Signed-off-by: Luca Della Vedova <[email protected]>

Author: luca-della-vedova

nav2_costmap_2d/include/nav2_costmap_2d/costmap_layer.hpp

@@ -38,6 +38,8 @@
 #ifndef NAV2_COSTMAP_2D__COSTMAP_LAYER_HPP_
 #define NAV2_COSTMAP_2D__COSTMAP_LAYER_HPP_
 
+#include <string>
+
 #include <rclcpp/rclcpp.hpp>
 #include <nav2_costmap_2d/layer.hpp>
 #include <nav2_costmap_2d/layered_costmap.hpp>
@@ -192,6 +194,23 @@ 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_;
 };

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_,

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,

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_);

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