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

current shape of the robot. Was previous only being called once in initialize().
ros-navigation · Oct 13, 2023 · fa09c78 · fa09c78
1 parent 57dd8d4
commit fa09c78
Showing 1 changed file with 2 additions and 0 deletions.
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)) {