diff --git a/costmap_2d/include/costmap_2d/costmap_math.h b/costmap_2d/include/costmap_2d/costmap_math.h
index 71fe96c45f..81cdfc3a08 100644
--- a/costmap_2d/include/costmap_2d/costmap_math.h
+++ b/costmap_2d/include/costmap_2d/costmap_math.h
@@ -66,4 +66,6 @@ bool intersects(std::vector<geometry_msgs::Point>& polygon, float testx, float t
 
 bool intersects(std::vector<geometry_msgs::Point>& polygon1, std::vector<geometry_msgs::Point>& polygon2);
 
+double orientation(double x0, double y0, double x1, double y1);
+
 #endif  // COSTMAP_2D_COSTMAP_MATH_H_
diff --git a/costmap_2d/include/costmap_2d/footprint.h b/costmap_2d/include/costmap_2d/footprint.h
index 6b1d1bec1b..407318974e 100644
--- a/costmap_2d/include/costmap_2d/footprint.h
+++ b/costmap_2d/include/costmap_2d/footprint.h
@@ -57,6 +57,16 @@ namespace costmap_2d
 void calculateMinAndMaxDistances(const std::vector<geometry_msgs::Point>& footprint,
                                  double& min_dist, double& max_dist);
 
+/**
+ * @brief Calculate the orientation at which the footprint will sweep the smallest area when moving along +x direction
+ * @warning This function only works under two assumptions:
+ *          * the footprint is symmetric wrt the x axis
+ *          * the closest edge is approximately parallel to either x or y axis
+ * @param footprint The footprint to examine
+ * @return Minimum footprint sweeping area orientation
+ */
+double minSweepingAreaOrientation(const std::vector<geometry_msgs::Point>& footprint);
+
 /**
  * @brief Convert Point32 to Point
  */
diff --git a/costmap_2d/src/costmap_math.cpp b/costmap_2d/src/costmap_math.cpp
index 97f7f50699..f7e978d9bd 100644
--- a/costmap_2d/src/costmap_math.cpp
+++ b/costmap_2d/src/costmap_math.cpp
@@ -87,3 +87,10 @@ bool intersects(std::vector<geometry_msgs::Point>& polygon1, std::vector<geometr
 {
   return intersects_helper(polygon1, polygon2) || intersects_helper(polygon2, polygon1);
 }
+
+double orientation(double x0, double y0, double x1, double y1)
+{
+  const double dx = x1 - x0;
+  const double dy = y1 - y0;
+  return atan2(dy, dx);
+}
\ No newline at end of file
diff --git a/costmap_2d/src/footprint.cpp b/costmap_2d/src/footprint.cpp
index dc51700a2f..d9d3558fcf 100644
--- a/costmap_2d/src/footprint.cpp
+++ b/costmap_2d/src/footprint.cpp
@@ -66,6 +66,39 @@ void calculateMinAndMaxDistances(const std::vector<geometry_msgs::Point>& footpr
   max_dist = std::max(max_dist, std::max(vertex_dist, edge_dist));
 }
 
+double minSweepingAreaOrientation(const std::vector<geometry_msgs::Point>& footprint)
+{
+  double min_dist = std::numeric_limits<double>::max();
+  std::array<geometry_msgs::Point, 2> closest_edge;
+
+  if (footprint.size() <= 2)
+  {
+    return NAN;
+  }
+
+  // check the distance from the robot center point to each footprint edged and keep the closest one
+  for (unsigned int i = 0; i < footprint.size() - 1; ++i)
+  {
+    double edge_dist = distanceToLine(0, 0, footprint[i].x, footprint[i].y, footprint[i + 1].x, footprint[i + 1].y);
+    if (edge_dist < min_dist)
+    {
+      min_dist = edge_dist;
+      closest_edge = { footprint[i], footprint[i + 1] };
+    }
+  }
+
+  // we also need to do the last vertex and the first vertex
+  if (distanceToLine(0, 0, footprint.back().x, footprint.back().y, footprint.front().x, footprint.front().y) < min_dist)
+  {
+    closest_edge = { footprint.back(), footprint.front() };
+  }
+
+  // return the orientation of the closest edge, directed from back to front (+x axis direction)
+  std::sort(closest_edge.begin(), closest_edge.end(),
+            [](const geometry_msgs::Point& p1, const geometry_msgs::Point& p2) { return p1.x < p2.x; });
+  return orientation(closest_edge.front().x, closest_edge.front().y, closest_edge.back().x, closest_edge.back().y);
+}
+
 geometry_msgs::Point32 toPoint32(geometry_msgs::Point pt)
 {
   geometry_msgs::Point32 point32;