From 3fa85bcced799001c51ccb7888d4d44fd7e747cb Mon Sep 17 00:00:00 2001
From: corot <jorge.santos@rapyuta-robotics.com>
Date: Fri, 29 Jul 2022 11:40:07 +0200
Subject: [PATCH 1/2] Add function to calculate the orientation at which the
 footprint sweeps the smallest area

---
 costmap_2d/include/costmap_2d/costmap_math.h |  2 ++
 costmap_2d/include/costmap_2d/footprint.h    |  8 +++++
 costmap_2d/src/costmap_math.cpp              |  7 +++++
 costmap_2d/src/footprint.cpp                 | 33 ++++++++++++++++++++
 4 files changed, 50 insertions(+)

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..01911b97a3 100644
--- a/costmap_2d/include/costmap_2d/footprint.h
+++ b/costmap_2d/include/costmap_2d/footprint.h
@@ -57,6 +57,14 @@ 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
+
+ * @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..cf036c75a8 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::vector<geometry_msgs::Point> 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;

From 2fb17cea2b45a6c1ade74656c6103091dc94ad75 Mon Sep 17 00:00:00 2001
From: corot <jorge.santos@rapyuta-robotics.com>
Date: Mon, 1 Aug 2022 10:16:56 +0200
Subject: [PATCH 2/2] Address PR reviews comments

---
 costmap_2d/include/costmap_2d/footprint.h | 4 +++-
 costmap_2d/src/footprint.cpp              | 2 +-
 2 files changed, 4 insertions(+), 2 deletions(-)

diff --git a/costmap_2d/include/costmap_2d/footprint.h b/costmap_2d/include/costmap_2d/footprint.h
index 01911b97a3..407318974e 100644
--- a/costmap_2d/include/costmap_2d/footprint.h
+++ b/costmap_2d/include/costmap_2d/footprint.h
@@ -59,7 +59,9 @@ void calculateMinAndMaxDistances(const std::vector<geometry_msgs::Point>& footpr
 
 /**
  * @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
  */
diff --git a/costmap_2d/src/footprint.cpp b/costmap_2d/src/footprint.cpp
index cf036c75a8..d9d3558fcf 100644
--- a/costmap_2d/src/footprint.cpp
+++ b/costmap_2d/src/footprint.cpp
@@ -69,7 +69,7 @@ void calculateMinAndMaxDistances(const std::vector<geometry_msgs::Point>& footpr
 double minSweepingAreaOrientation(const std::vector<geometry_msgs::Point>& footprint)
 {
   double min_dist = std::numeric_limits<double>::max();
-  std::vector<geometry_msgs::Point> closest_edge;
+  std::array<geometry_msgs::Point, 2> closest_edge;
 
   if (footprint.size() <= 2)
   {