diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/footprint.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/footprint.hpp
index 6590a4682ec..4c73d671d0e 100644
--- a/nav2_costmap_2d/include/nav2_costmap_2d/footprint.hpp
+++ b/nav2_costmap_2d/include/nav2_costmap_2d/footprint.hpp
@@ -96,6 +96,13 @@ void transformFootprint(
   const std::vector<geometry_msgs::msg::Point> & footprint_spec,
   std::vector<geometry_msgs::msg::Point> & 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::msg::Point calculateCentroid(const std::vector<geometry_msgs::msg::Point>& footprint);
+
 /**
  * @brief  Given a pose and base footprint, build the oriented footprint of the robot (PolygonStamped)
  * @param  x The x position of the robot
diff --git a/nav2_costmap_2d/src/footprint.cpp b/nav2_costmap_2d/src/footprint.cpp
index 319702c5bde..f7f4a8592ab 100644
--- a/nav2_costmap_2d/src/footprint.cpp
+++ b/nav2_costmap_2d/src/footprint.cpp
@@ -142,13 +142,40 @@ void transformFootprint(
   }
 }
 
+geometry_msgs::msg::Point calculateCentroid(const std::vector<geometry_msgs::msg::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::msg::Point> & footprint, double padding)
 {
-  // pad footprint in place
-  for (unsigned int i = 0; i < footprint.size(); i++) {
-    geometry_msgs::msg::Point & pt = footprint[i];
-    pt.x += sign0(pt.x) * padding;
-    pt.y += sign0(pt.y) * padding;
+  geometry_msgs::msg::Point footprint_center = calculateCentroid(footprint);
+
+  for (auto& point : footprint)
+  {
+    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);
+    }
   }
 }
 
diff --git a/nav2_costmap_2d/test/integration/footprint_tests.cpp b/nav2_costmap_2d/test/integration/footprint_tests.cpp
index 0cab6fb818a..3b550976746 100644
--- a/nav2_costmap_2d/test/integration/footprint_tests.cpp
+++ b/nav2_costmap_2d/test/integration/footprint_tests.cpp
@@ -153,16 +153,16 @@ TEST_F(TestNode, padded_footprint_from_string_param)
   std::vector<geometry_msgs::msg::Point> footprint = footprint_tester_->getRobotFootprint();
   EXPECT_EQ(3u, footprint.size());
 
-  EXPECT_EQ(1.5f, footprint[0].x);
-  EXPECT_EQ(1.5f, footprint[0].y);
+  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_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_NEAR(-1.22361f, footprint[2].x, 0.0001f);
+  EXPECT_NEAR(-1.44721f, footprint[2].y, 0.0001f);
   EXPECT_EQ(0.0f, footprint[2].z);
 }