Used utility function to simplify namespace calls.

Signed-off-by: Leander Stephen D'Souza <leanderdsouza1234@gmail.com>

Author: leander-dsouza

nav2_costmap_2d/include/nav2_costmap_2d/costmap_layer.hpp

@@ -194,24 +194,6 @@ 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`.
-   * Use global topic `/scan` if you do not wish the node namespace to apply
-   *
-   * @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

@@ -46,6 +46,7 @@
 #include "pluginlib/class_list_macros.hpp"
 #include "sensor_msgs/point_cloud2_iterator.hpp"
 #include "nav2_costmap_2d/costmap_math.hpp"
+#include "nav2_costmap_2d/costmap_utils.hpp"
 #include "rclcpp/version.h"
 
 PLUGINLIB_EXPORT_CLASS(nav2_costmap_2d::ObstacleLayer, nav2_costmap_2d::Layer)
@@ -190,7 +191,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);
 
-    topic = joinWithParentNamespace(topic);
+    topic = joinWithParentNamespace(node_, topic);
 
     RCLCPP_DEBUG(
       logger_,

nav2_costmap_2d/plugins/range_sensor_layer.cpp

@@ -43,6 +43,7 @@
 #include "pluginlib/class_list_macros.hpp"
 #include "geometry_msgs/msg/point_stamped.hpp"
 #include "nav2_costmap_2d/range_sensor_layer.hpp"
+#include "nav2_costmap_2d/costmap_utils.hpp"
 
 PLUGINLIB_EXPORT_CLASS(nav2_costmap_2d::RangeSensorLayer, nav2_costmap_2d::Layer)
 
@@ -130,7 +131,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);
+    topic_name = joinWithParentNamespace(node_, topic_name);
     if (input_sensor_type == InputSensorType::VARIABLE) {
       processRangeMessageFunc_ = std::bind(
         &RangeSensorLayer::processVariableRangeMsg, this,

nav2_costmap_2d/plugins/static_layer.cpp

@@ -46,6 +46,7 @@
 #include "tf2/convert.hpp"
 #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
 #include "nav2_util/validate_messages.hpp"
+#include "nav2_costmap_2d/costmap_utils.hpp"
 
 PLUGINLIB_EXPORT_CLASS(nav2_costmap_2d::StaticLayer, nav2_costmap_2d::Layer)
 
@@ -150,7 +151,7 @@ StaticLayer::getParameters()
   node->get_parameter(name_ + "." + "footprint_clearing_enabled", footprint_clearing_enabled_);
   node->get_parameter(name_ + "." + "restore_cleared_footprint", restore_cleared_footprint_);
   node->get_parameter(name_ + "." + "map_topic", map_topic_);
-  map_topic_ = joinWithParentNamespace(map_topic_);
+  map_topic_ = joinWithParentNamespace(node, map_topic_);
   node->get_parameter(
     name_ + "." + "map_subscribe_transient_local",
     map_subscribe_transient_local_);

nav2_costmap_2d/src/costmap_layer.cpp

@@ -273,20 +273,4 @@ 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