Update padFootprint function

costmap_2d/include/costmap_2d/footprint.h

@@ -99,6 +99,13 @@ void transformFootprint(double x, double y, double theta, const std::vector<geom
 void transformFootprint(double x, double y, double theta, const std::vector<geometry_msgs::Point>& footprint_spec,
                         geometry_msgs::PolygonStamped & oriented_footprint);
 
+/**
+ * @brief  Calculate the centroid of a vector of 2D points.
+ * @param  footprint A vector of points (type geometry_msgs::Point) representing a 2D polygon (footprint).
+ * @return The centroid of the footprint as a geometry_msgs::Point.
+ */
+geometry_msgs::Point calculateCentroid(const std::vector<geometry_msgs::Point>& footprint);
+
 /**
  * @brief Adds the specified amount of padding to the footprint (in place)
  */

costmap_2d/src/footprint.cpp

@@ -135,14 +135,40 @@ void transformFootprint(double x, double y, double theta, const std::vector<geom
   }
 }
 
+geometry_msgs::Point calculateCentroid(const std::vector<geometry_msgs::Point>& footprint)
+{
+    geometry_msgs::Point center;
+    double sum_x = 0.0, sum_y = 0.0;
+
+    for (const auto& point : footprint)
+    {
+        sum_x += point.x;
+        sum_y += point.y;
+    }
+
+    if (!footprint.empty()) {
+        center.x = sum_x / footprint.size();
+        center.y = sum_y / footprint.size();
+    }
+
+    return center;
+}
+
 void padFootprint(std::vector<geometry_msgs::Point>& footprint, double padding)
 {
-  // pad footprint in place
-  for (unsigned int i = 0; i < footprint.size(); i++)
+  geometry_msgs::Point footprint_center = calculateCentroid(footprint);
+
+  for (auto& point : footprint)
   {
-    geometry_msgs::Point& pt = footprint[ i ];
-    pt.x += sign0(pt.x) * padding;
-    pt.y += sign0(pt.y) * padding;
+    double dx = point.x - footprint_center.x;
+    double dy = point.y - footprint_center.y;
+    double distance = std::hypot(dx, dy);
+
+    if (distance > 0.0)
+    {
+      point.x += padding * (dx / distance);
+      point.y += padding * (dy / distance);
+    }
   }
 }
 

costmap_2d/test/footprint_tests.cpp

@@ -70,17 +70,17 @@ TEST( Costmap2DROS, padded_footprint_from_string_param )
   std::vector<geometry_msgs::Point> footprint = cm.getRobotFootprint();
   EXPECT_EQ( 3, footprint.size() );
 
-  EXPECT_EQ( 1.5f, footprint[ 0 ].x );
-  EXPECT_EQ( 1.5f, footprint[ 0 ].y );
-  EXPECT_EQ( 0.0f, footprint[ 0 ].z );
+  EXPECT_NEAR( 1.44721f, footprint[ 0 ].x, 0.0001f);
+  EXPECT_NEAR( 1.22361f, footprint[ 0 ].y, 0.0001f);
+  EXPECT_EQ( 0.0f, footprint[ 0 ].z);
 
-  EXPECT_EQ( -1.5f, footprint[ 1 ].x );
-  EXPECT_EQ( 1.5f, footprint[ 1 ].y );
-  EXPECT_EQ( 0.0f, footprint[ 1 ].z );
+  EXPECT_NEAR( -1.35355f, footprint[ 1 ].x, 0.0001f);
+  EXPECT_NEAR( 1.35355f, footprint[ 1 ].y, 0.0001f);
+  EXPECT_EQ( 0.0f, footprint[ 1 ].z);
 
-  EXPECT_EQ( -1.5f, footprint[ 2 ].x );
-  EXPECT_EQ( -1.5f, footprint[ 2 ].y );
-  EXPECT_EQ( 0.0f, footprint[ 2 ].z );
+  EXPECT_NEAR( -1.22361f, footprint[ 2 ].x, 0.0001f);
+  EXPECT_NEAR( -1.44721f, footprint[ 2 ].y, 0.0001f);
+  EXPECT_EQ( 0.0f, footprint[ 2 ].z);
 }
 
 TEST( Costmap2DROS, radius_param )
@@ -155,7 +155,7 @@ TEST( Costmap2DROS, footprint_empty )
   // With no specification of footprint or radius, defaults to 0.46 meter radius plus 0.01 meter padding.
   EXPECT_EQ( 16, footprint.size() );
 
-  EXPECT_NEAR( 0.47f, footprint[ 0 ].x, 0.0001 );
+  EXPECT_NEAR( 0.47f, footprint[ 0 ].x, 0.001 );
   EXPECT_NEAR( 0.0f, footprint[ 0 ].y, 0.0001 );
   EXPECT_EQ( 0.0f, footprint[ 0 ].z );
 }