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..8cd4fac4d5d 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_layer.hpp +++ b/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 + #include #include #include @@ -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_; }; 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