Support stamped footprints in costmap_2d (#5345)

Signed-off-by: Sushant Chavan <gitecsvc@gmail.com>

Author: Sushant-Chavan

nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp

@@ -313,7 +313,7 @@ class Costmap2DROS : public nav2::LifecycleNode
    * layered_costmap_->setFootprint().  Also saves the unpadded
    * footprint, which is available from
    * getUnpaddedRobotFootprint(). */
-  void setRobotFootprintPolygon(const geometry_msgs::msg::Polygon::SharedPtr footprint);
+  void setRobotFootprintPolygon(const geometry_msgs::msg::Polygon & footprint);
 
   std::shared_ptr<tf2_ros::Buffer> getTfBuffer() {return tf_buffer_;}
 
@@ -351,6 +351,7 @@ class Costmap2DROS : public nav2::LifecycleNode
   std::vector<std::unique_ptr<Costmap2DPublisher>> layer_publishers_;
 
   nav2::Subscription<geometry_msgs::msg::Polygon>::SharedPtr footprint_sub_;
+  nav2::Subscription<geometry_msgs::msg::PolygonStamped>::SharedPtr footprint_stamped_sub_;
   nav2::Subscription<rcl_interfaces::msg::ParameterEvent>::SharedPtr parameter_sub_;
 
   // Dedicated callback group and executor for tf timer_interface and message filter
@@ -407,6 +408,8 @@ class Costmap2DROS : public nav2::LifecycleNode
   double transform_tolerance_{0};           ///< The timeout before transform errors
   double initial_transform_timeout_{0};   ///< The timeout before activation of the node errors
   double map_vis_z_{0};                 ///< The height of map, allows to avoid flickering at -0.008
+  /// If true, the footprint subscriber expects a PolygonStamped msg
+  bool subscribe_to_stamped_footprint_{false};
 
   bool is_lifecycle_follower_{true};   ///< whether is a child-LifecycleNode or an independent node
 

nav2_costmap_2d/include/nav2_costmap_2d/footprint.hpp

@@ -65,23 +65,23 @@ std::pair<double, double> calculateMinAndMaxDistances(
 /**
  * @brief Convert Point32 to Point
  */
-geometry_msgs::msg::Point toPoint(geometry_msgs::msg::Point32 pt);
+geometry_msgs::msg::Point toPoint(const geometry_msgs::msg::Point32 & pt);
 
 /**
  * @brief Convert Point to Point32
  */
-geometry_msgs::msg::Point32 toPoint32(geometry_msgs::msg::Point pt);
+geometry_msgs::msg::Point32 toPoint32(const geometry_msgs::msg::Point & pt);
 
 /**
  * @brief Convert vector of Points to Polygon msg
  */
-geometry_msgs::msg::Polygon toPolygon(std::vector<geometry_msgs::msg::Point> pts);
+geometry_msgs::msg::Polygon toPolygon(const std::vector<geometry_msgs::msg::Point> & pts);
 
 /**
  * @brief Convert Polygon msg to vector of Points.
  */
 std::vector<geometry_msgs::msg::Point> toPointVector(
-  geometry_msgs::msg::Polygon::SharedPtr polygon);
+  const geometry_msgs::msg::Polygon & polygon);
 
 /**
  * @brief  Given a pose and base footprint, build the oriented footprint of the robot (list of Points)

nav2_costmap_2d/src/costmap_2d_ros.cpp

@@ -133,6 +133,7 @@ void Costmap2DROS::init()
   declare_parameter("unknown_cost_value", rclcpp::ParameterValue(static_cast<unsigned char>(0xff)));
   declare_parameter("update_frequency", rclcpp::ParameterValue(5.0));
   declare_parameter("use_maximum", rclcpp::ParameterValue(false));
+  declare_parameter("subscribe_to_stamped_footprint", rclcpp::ParameterValue(false));
 }
 
 Costmap2DROS::~Costmap2DROS()
@@ -218,9 +219,15 @@ Costmap2DROS::on_configure(const rclcpp_lifecycle::State & /*state*/)
   }
 
   // Create the publishers and subscribers
-  footprint_sub_ = create_subscription<geometry_msgs::msg::Polygon>(
-    "footprint",
-    std::bind(&Costmap2DROS::setRobotFootprintPolygon, this, std::placeholders::_1));
+  if (subscribe_to_stamped_footprint_) {
+    footprint_stamped_sub_ = create_subscription<geometry_msgs::msg::PolygonStamped>(
+      "footprint", [this](const geometry_msgs::msg::PolygonStamped::SharedPtr footprint)
+      {setRobotFootprintPolygon(footprint->polygon);});
+  } else {
+    footprint_sub_ = create_subscription<geometry_msgs::msg::Polygon>(
+      "footprint", [this](const geometry_msgs::msg::Polygon::SharedPtr footprint)
+      {setRobotFootprintPolygon(*footprint);});
+  }
 
   footprint_pub_ = create_publisher<geometry_msgs::msg::PolygonStamped>(
     "published_footprint");
@@ -415,6 +422,7 @@ Costmap2DROS::getParameters()
   get_parameter("width", map_width_meters_);
   get_parameter("plugins", plugin_names_);
   get_parameter("filters", filter_names_);
+  get_parameter("subscribe_to_stamped_footprint", subscribe_to_stamped_footprint_);
 
   auto node = shared_from_this();
 
@@ -483,7 +491,7 @@ Costmap2DROS::setRobotFootprint(const std::vector<geometry_msgs::msg::Point> & p
 
 void
 Costmap2DROS::setRobotFootprintPolygon(
-  const geometry_msgs::msg::Polygon::SharedPtr footprint)
+  const geometry_msgs::msg::Polygon & footprint)
 {
   setRobotFootprint(toPointVector(footprint));
 }

nav2_costmap_2d/src/footprint.cpp

@@ -71,7 +71,7 @@ std::pair<double, double> calculateMinAndMaxDistances(
   return std::pair<double, double>(min_dist, max_dist);
 }
 
-geometry_msgs::msg::Point32 toPoint32(geometry_msgs::msg::Point pt)
+geometry_msgs::msg::Point32 toPoint32(const geometry_msgs::msg::Point & pt)
 {
   geometry_msgs::msg::Point32 point32;
   point32.x = pt.x;
@@ -80,7 +80,7 @@ geometry_msgs::msg::Point32 toPoint32(geometry_msgs::msg::Point pt)
   return point32;
 }
 
-geometry_msgs::msg::Point toPoint(geometry_msgs::msg::Point32 pt)
+geometry_msgs::msg::Point toPoint(const geometry_msgs::msg::Point32 & pt)
 {
   geometry_msgs::msg::Point point;
   point.x = pt.x;
@@ -89,20 +89,22 @@ geometry_msgs::msg::Point toPoint(geometry_msgs::msg::Point32 pt)
   return point;
 }
 
-geometry_msgs::msg::Polygon toPolygon(std::vector<geometry_msgs::msg::Point> pts)
+geometry_msgs::msg::Polygon toPolygon(const std::vector<geometry_msgs::msg::Point> & pts)
 {
   geometry_msgs::msg::Polygon polygon;
-  for (unsigned int i = 0; i < pts.size(); i++) {
-    polygon.points.push_back(toPoint32(pts[i]));
+  polygon.points.reserve(pts.size());
+  for (const auto & pt : pts) {
+    polygon.points.push_back(toPoint32(pt));
   }
   return polygon;
 }
 
-std::vector<geometry_msgs::msg::Point> toPointVector(geometry_msgs::msg::Polygon::SharedPtr polygon)
+std::vector<geometry_msgs::msg::Point> toPointVector(const geometry_msgs::msg::Polygon & polygon)
 {
   std::vector<geometry_msgs::msg::Point> pts;
-  for (unsigned int i = 0; i < polygon->points.size(); i++) {
-    pts.push_back(toPoint(polygon->points[i]));
+  pts.reserve(polygon.points.size());
+  for (const auto & point : polygon.points) {
+    pts.push_back(toPoint(point));
   }
   return pts;
 }

nav2_costmap_2d/src/footprint_subscriber.cpp

@@ -35,8 +35,7 @@ FootprintSubscriber::getFootprintRaw(
   }
 
   auto current_footprint = std::atomic_load(&footprint_);
-  footprint = toPointVector(
-    std::make_shared<geometry_msgs::msg::Polygon>(current_footprint->polygon));
+  footprint = toPointVector(current_footprint->polygon);
   footprint_header = current_footprint->header;
 
   return true;