Fix for robot footprint collision in obstacles critic (#3878) (#3947)

* 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 <[email protected]>
(cherry picked from commit 98af3b9)

Co-authored-by: LeifHookedWireless <[email protected]>

Author: mergify[bot]

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};

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);
     auto getParam = parameters_handler_->getParamGetter(name_);
@@ -84,7 +90,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)
@@ -109,6 +118,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)) {