Merge pull request #5 from idealworks/fix/costmap_footprint_topic

[nav2_costmap_2d] Add a footprint topic parameter

Author: nourazzii

nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp

@@ -383,6 +383,7 @@ class Costmap2DROS : public nav2_util::LifecycleNode
   void getParameters();
   bool always_send_full_costmap_{false};
   std::string footprint_;
+  std::string footprint_topic_;
   float footprint_padding_{0};
   std::string global_frame_;                ///< The global frame for the costmap
   int map_height_meters_{0};

nav2_costmap_2d/src/costmap_2d_ros.cpp

@@ -104,6 +104,7 @@ void Costmap2DROS::init()
   declare_parameter("map_vis_z", rclcpp::ParameterValue(0.0));
   declare_parameter("footprint_padding", rclcpp::ParameterValue(0.01f));
   declare_parameter("footprint", rclcpp::ParameterValue(std::string("[]")));
+  declare_parameter("footprint_topic", rclcpp::ParameterValue(std::string("footprint")));
   declare_parameter("global_frame", rclcpp::ParameterValue(std::string("map")));
   declare_parameter("height", rclcpp::ParameterValue(5));
   declare_parameter("width", rclcpp::ParameterValue(5));
@@ -212,7 +213,7 @@ Costmap2DROS::on_configure(const rclcpp_lifecycle::State & /*state*/)
 
   // Create the publishers and subscribers
   footprint_sub_ = create_subscription<geometry_msgs::msg::Polygon>(
-    "footprint",
+    footprint_topic_,
     rclcpp::SystemDefaultsQoS(),
     std::bind(&Costmap2DROS::setRobotFootprintPolygon, this, std::placeholders::_1));
 
@@ -393,6 +394,7 @@ Costmap2DROS::getParameters()
   get_parameter("always_send_full_costmap", always_send_full_costmap_);
   get_parameter("map_vis_z", map_vis_z_);
   get_parameter("footprint", footprint_);
+  get_parameter("footprint_topic", footprint_topic_);
   get_parameter("footprint_padding", footprint_padding_);
   get_parameter("global_frame", global_frame_);
   get_parameter("height", map_height_meters_);