From fa09c7862495ad9d1e20b6a246497656afeda0c1 Mon Sep 17 00:00:00 2001 From: Leif Terry Date: Wed, 11 Oct 2023 16:44:04 -0400 Subject: [PATCH 1/5] Inscribed/Circumscribed costs must be updated to take into account the current shape of the robot. Was previous only being called once in initialize(). --- nav2_mppi_controller/src/critics/obstacles_critic.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/nav2_mppi_controller/src/critics/obstacles_critic.cpp b/nav2_mppi_controller/src/critics/obstacles_critic.cpp index 80ac77e10f5..b3a4351443a 100644 --- 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)) { From c201121de901b0cc8c77657d6920dc06181a526e Mon Sep 17 00:00:00 2001 From: Leif Terry Date: Fri, 3 Nov 2023 17:01:01 -0400 Subject: [PATCH 2/5] Add early return to avoid calculations if footprint has not changed. --- .../critics/obstacles_critic.hpp | 1 + .../src/critics/obstacles_critic.cpp | 12 ++++++++++-- 2 files changed, 11 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 fe17906bae4..fd6eeab0128 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 bcf5e7fa009..c2a9d6fa49a 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,9 @@ 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) From 9d6076ee0bd845e595e10280c8ac6533fa6cd7ca Mon Sep 17 00:00:00 2001 From: Leif Terry Date: Sat, 4 Nov 2023 16:29:02 -0400 Subject: [PATCH 3/5] Only update radius if using footprint. Add perf timers. --- .../src/critics/obstacles_critic.cpp | 20 +++++++++++++++++-- 1 file changed, 18 insertions(+), 2 deletions(-) diff --git a/nav2_mppi_controller/src/critics/obstacles_critic.cpp b/nav2_mppi_controller/src/critics/obstacles_critic.cpp index c2a9d6fa49a..f33143c13b3 100644 --- a/nav2_mppi_controller/src/critics/obstacles_critic.cpp +++ b/nav2_mppi_controller/src/critics/obstacles_critic.cpp @@ -91,6 +91,7 @@ float ObstaclesCritic::findCircumscribedCost( circumscribed_radius_ = static_cast(circum_radius); circumscribed_cost_ = static_cast(result); + return circumscribed_cost_; } @@ -115,8 +116,17 @@ void ObstaclesCritic::score(CriticData & data) if (!enabled_) { return; } - - possibly_inscribed_cost_ = findCircumscribedCost(costmap_ros_); + auto parent = parent_.lock(); + auto t0 = parent->now(); // xx!! + if (consider_footprint_) { + // footprint has almost certainly changed since initialize() + possibly_inscribed_cost_ = findCircumscribedCost(costmap_ros_); + } + auto t1 = parent->now(); + auto dt = (t1 - t0).seconds(); + static double fcc_dt = 0.05; + fcc_dt = 0.9 * fcc_dt + 0.1 * dt; + RCLCPP_WARN_THROTTLE(logger_, *(parent->get_clock()), 500, "findCircumscribedCost took %f ms (%f)", 1000.0*dt, 1000.0*fcc_dt); // If near the goal, don't apply the preferential term since the goal is near obstacles bool near_goal = false; @@ -170,6 +180,12 @@ void ObstaclesCritic::score(CriticData & data) (repulsion_weight_ * repulsive_cost / traj_len), power_); data.fail_flag = all_trajectories_collide; + + auto t2 = parent->now(); + dt = (t2 - t0).seconds(); + static double score_dt = 0.05; + score_dt = 0.9 * score_dt + 0.1 * dt; + RCLCPP_WARN_THROTTLE(logger_, *(parent->get_clock()), 500, "score took %f ms (%f)", 1000.0*dt, 1000.0*score_dt); } /** From 69293f40e875a9782d79ddf2506178d55440440f Mon Sep 17 00:00:00 2001 From: Leif Terry Date: Sun, 5 Nov 2023 14:38:43 -0500 Subject: [PATCH 4/5] Remove perf timers. --- .../src/critics/obstacles_critic.cpp | 15 ++------------- 1 file changed, 2 insertions(+), 13 deletions(-) diff --git a/nav2_mppi_controller/src/critics/obstacles_critic.cpp b/nav2_mppi_controller/src/critics/obstacles_critic.cpp index f33143c13b3..75118c73b08 100644 --- a/nav2_mppi_controller/src/critics/obstacles_critic.cpp +++ b/nav2_mppi_controller/src/critics/obstacles_critic.cpp @@ -116,17 +116,12 @@ void ObstaclesCritic::score(CriticData & data) if (!enabled_) { return; } - auto parent = parent_.lock(); - auto t0 = parent->now(); // xx!! + if (consider_footprint_) { // footprint has almost certainly changed since initialize() + // and may continue to change possibly_inscribed_cost_ = findCircumscribedCost(costmap_ros_); } - auto t1 = parent->now(); - auto dt = (t1 - t0).seconds(); - static double fcc_dt = 0.05; - fcc_dt = 0.9 * fcc_dt + 0.1 * dt; - RCLCPP_WARN_THROTTLE(logger_, *(parent->get_clock()), 500, "findCircumscribedCost took %f ms (%f)", 1000.0*dt, 1000.0*fcc_dt); // If near the goal, don't apply the preferential term since the goal is near obstacles bool near_goal = false; @@ -180,12 +175,6 @@ void ObstaclesCritic::score(CriticData & data) (repulsion_weight_ * repulsive_cost / traj_len), power_); data.fail_flag = all_trajectories_collide; - - auto t2 = parent->now(); - dt = (t2 - t0).seconds(); - static double score_dt = 0.05; - score_dt = 0.9 * score_dt + 0.1 * dt; - RCLCPP_WARN_THROTTLE(logger_, *(parent->get_clock()), 500, "score took %f ms (%f)", 1000.0*dt, 1000.0*score_dt); } /** From 8ec14e03567352fe09959fad1c443fc9fd3f8f9a Mon Sep 17 00:00:00 2001 From: Leif Terry Date: Mon, 6 Nov 2023 15:14:28 -0500 Subject: [PATCH 5/5] Update comments. --- nav2_mppi_controller/src/critics/obstacles_critic.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/nav2_mppi_controller/src/critics/obstacles_critic.cpp b/nav2_mppi_controller/src/critics/obstacles_critic.cpp index 75118c73b08..b8d6c898709 100644 --- a/nav2_mppi_controller/src/critics/obstacles_critic.cpp +++ b/nav2_mppi_controller/src/critics/obstacles_critic.cpp @@ -118,8 +118,7 @@ void ObstaclesCritic::score(CriticData & data) } if (consider_footprint_) { - // footprint has almost certainly changed since initialize() - // and may continue to change + // footprint may have changed since initialization if user has dynamic footprints possibly_inscribed_cost_ = findCircumscribedCost(costmap_ros_); }