From 8cee5b64ad203009a8d9206a12b3d3d8e0523d51 Mon Sep 17 00:00:00 2001 From: LeifHookedWireless 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 (cherry picked from commit 98af3b9a28d9180ba22b17573f01cccaf7cbe04b) --- .../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(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)) {