separate function for centroid and use std::hypot

rapyuta-robotics · Jan 6, 2025 · b0f6775 · b0f6775
1 parent 7f99c9c
commit b0f6775
Show file tree

Hide file tree

Showing 2 changed files with 28 additions and 12 deletions.
diff --git a/costmap_2d/include/costmap_2d/footprint.h b/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)
  */

diff --git a/costmap_2d/src/footprint.cpp b/costmap_2d/src/footprint.cpp
@@ -135,25 +135,34 @@ void transformFootprint(double x, double y, double theta, const std::vector<geom
   }
 }
 
-void padFootprint(std::vector<geometry_msgs::Point>& footprint, double padding)
+geometry_msgs::Point calculateCentroid(const std::vector<geometry_msgs::Point>& footprint)
 {
-  geometry_msgs::Point footprint_center;
-  double sum_x = 0.0, sum_y = 0.0;
+    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;
-  }
-  footprint_center.x = sum_x / footprint.size();
-  footprint_center.y = sum_y / footprint.size();
+    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)
+{
+  geometry_msgs::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::sqrt(dx * dx + dy * dy);
+    double distance = std::hypot(dx, dy);
 
     if (distance > 0.0)
     {