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(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(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(result); + circumscribed_radius_ = static_cast(circum_radius); + circumscribed_cost_ = static_cast(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)) {