From cce5dc73749b0d901a5e0bc917c39b80926fac16 Mon Sep 17 00:00:00 2001
From: Nisarg Panchal <71684502+Nisarg236@users.noreply.github.com>
Date: Sat, 7 Dec 2024 00:58:49 +0530
Subject: [PATCH] Update obstacle_layer.cpp

---
 costmap_2d/plugins/obstacle_layer.cpp | 15 ++++++++-------
 1 file changed, 8 insertions(+), 7 deletions(-)

diff --git a/costmap_2d/plugins/obstacle_layer.cpp b/costmap_2d/plugins/obstacle_layer.cpp
index 844145098..ab9af72b8 100644
--- a/costmap_2d/plugins/obstacle_layer.cpp
+++ b/costmap_2d/plugins/obstacle_layer.cpp
@@ -465,30 +465,31 @@ std::vector<geometry_msgs::Point> ObstacleLayer::getReducedFootprint(const std::
 {
     std::vector<geometry_msgs::Point> reduced_footprint;
 
-    for (size_t i = 0; i < original_footprint.size(); ++i)
+    for (const auto& p : original_footprint)
     {
-        const auto& p = original_footprint[i];
         double dx = p.x - footprint_center_.x;
         double dy = p.y - footprint_center_.y;
 
         double distance = sqrt(dx * dx + dy * dy);
-        if (distance > reduction_size)
+        geometry_msgs::Point new_point;
+
+        if (distance > 0)
         {
             double scale = (distance - reduction_size) / distance;
-            geometry_msgs::Point new_point;
             new_point.x = footprint_center_.x + dx * scale;
             new_point.y = footprint_center_.y + dy * scale;
-            reduced_footprint.push_back(new_point);
         }
         else
         {
-            reduced_footprint.push_back(footprint_center_);
+            new_point = footprint_center_;
         }
-    }
 
+        reduced_footprint.push_back(new_point);
+    }
     return reduced_footprint;
 }
 
+
 void ObstacleLayer::updateFootprint(double robot_x, double robot_y, double robot_yaw, double* min_x, double* min_y,
                                     double* max_x, double* max_y)
 {