Inscribed/Circumscribed costs must be updated to take into account the

Leif Terry · Leif Terry · commit fa09c7862495 · 2023-10-12T20:13:49.000-04:00
current shape of the robot.
Was previous only being called once in initialize().
diff --git a/nav2_mppi_controller/src/critics/obstacles_critic.cpp b/nav2_mppi_controller/src/critics/obstacles_critic.cpp
@@ -108,6 +108,8 @@ void ObstaclesCritic::score(CriticData & data)
     return;
   }
 
+  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)) {

Original file line number	Diff line number	Diff line change
`@@ -108,6 +108,8 @@ void ObstaclesCritic::score(CriticData & data)`
`108`	`108`	`return;`
`109`	`109`	`}`
`110`	`110`
	`111`	`+ possibly_inscribed_cost_ = findCircumscribedCost(costmap_ros_);`
	`112`	`+`
`111`	`113`	`// If near the goal, don't apply the preferential term since the goal is near obstacles`
`112`	`114`	`bool near_goal = false;`
`113`	`115`	`if (utils::withinPositionGoalTolerance(near_goal_distance_, data.state.pose.pose, data.path)) {`