From 98af3b9a28d9180ba22b17573f01cccaf7cbe04b Mon Sep 17 00:00:00 2001
From: LeifHookedWireless <leif@hookedwireless.com>
Date: Mon, 6 Nov 2023 16:34:55 -0500
Subject: [PATCH] Fix for  robot footprint collision in obstacles critic
 (#3878)

* Inscribed/Circumscribed costs must be updated to take into account the
current shape of the robot.
Was previous only being called once in initialize().

* Add early return to avoid calculations if footprint has not changed.

* Only update radius if using footprint.
Add perf timers.

* Remove perf timers.

* Update comments.

---------

Co-authored-by: Leif Terry <leif@peanutrobotics.com>
---
 .../critics/obstacles_critic.hpp               |  1 +
 .../src/critics/obstacles_critic.cpp           | 18 ++++++++++++++++--
 2 files changed, 17 insertions(+), 2 deletions(-)

diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/critics/obstacles_critic.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/critics/obstacles_critic.hpp
index fe17906bae..fd6eeab012 100644
--- a/nav2_mppi_controller/include/nav2_mppi_controller/critics/obstacles_critic.hpp
+++ b/nav2_mppi_controller/include/nav2_mppi_controller/critics/obstacles_critic.hpp
@@ -92,6 +92,7 @@ class ObstaclesCritic : public CriticFunction
   float possibly_inscribed_cost_;
   float collision_margin_distance_;
   float near_goal_distance_;
+  float circumscribed_cost_{0}, circumscribed_radius_{0};
 
   unsigned int power_{0};
   float repulsion_weight_, critical_weight_{0};
diff --git a/nav2_mppi_controller/src/critics/obstacles_critic.cpp b/nav2_mppi_controller/src/critics/obstacles_critic.cpp
index 90fa92a57a..b8d6c89870 100644
--- a/nav2_mppi_controller/src/critics/obstacles_critic.cpp
+++ b/nav2_mppi_controller/src/critics/obstacles_critic.cpp
@@ -55,6 +55,13 @@ float ObstaclesCritic::findCircumscribedCost(
 {
   double result = -1.0;
   bool inflation_layer_found = false;
+
+  const double circum_radius = costmap->getLayeredCostmap()->getCircumscribedRadius();
+  if (static_cast<float>(circum_radius) == circumscribed_radius_) {
+    // early return if footprint size is unchanged
+    return circumscribed_cost_;
+  }
+
   // check if the costmap has an inflation layer
   for (auto layer = costmap->getLayeredCostmap()->getPlugins()->begin();
     layer != costmap->getLayeredCostmap()->getPlugins()->end();
@@ -66,7 +73,6 @@ float ObstaclesCritic::findCircumscribedCost(
     }
 
     inflation_layer_found = true;
-    const double circum_radius = costmap->getLayeredCostmap()->getCircumscribedRadius();
     const double resolution = costmap->getCostmap()->getResolution();
     result = inflation_layer->computeCost(circum_radius / resolution);
     inflation_scale_factor_ = static_cast<float>(inflation_layer->getCostScalingFactor());
@@ -83,7 +89,10 @@ float ObstaclesCritic::findCircumscribedCost(
       "significantly slow down planning times and not avoid anything but absolute collisions!");
   }
 
-  return static_cast<float>(result);
+  circumscribed_radius_ = static_cast<float>(circum_radius);
+  circumscribed_cost_ = static_cast<float>(result);
+
+  return circumscribed_cost_;
 }
 
 float ObstaclesCritic::distanceToObstacle(const CollisionCost & cost)
@@ -108,6 +117,11 @@ void ObstaclesCritic::score(CriticData & data)
     return;
   }
 
+  if (consider_footprint_) {
+    // footprint may have changed since initialization if user has dynamic footprints
+    possibly_inscribed_cost_ = findCircumscribedCost(costmap_ros_);
+  }
+
   // If near the goal, don't apply the preferential term since the goal is near obstacles
   bool near_goal = false;
   if (utils::withinPositionGoalTolerance(near_goal_distance_, data.state.pose.pose, data.path)) {