From 863eb53dc5ecf67b5a2145fd9b82734663b61cb5 Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Wed, 5 Nov 2025 15:05:57 +0100 Subject: [PATCH 1/4] footprint cost for scoring cost critic Signed-off-by: Tony Najjar --- .../critics/cost_critic.hpp | 33 +---------------- .../src/critics/cost_critic.cpp | 36 ++++++++++++++++--- 2 files changed, 32 insertions(+), 37 deletions(-) diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/critics/cost_critic.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/critics/cost_critic.hpp index c285a14b664..f97d05f6965 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/critics/cost_critic.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/critics/cost_critic.hpp @@ -48,38 +48,6 @@ class CostCritic : public CriticFunction void score(CriticData & data) override; protected: - /** - * @brief Checks if cost represents a collision - * @param cost Point cost at pose center - * @param x X of pose - * @param y Y of pose - * @param theta theta of pose - * @return bool if in collision - */ - inline bool inCollision(float cost, float x, float y, float theta) - { - // If consider_footprint_ check footprint scort for collision - float score_cost = cost; - if (consider_footprint_ && - (cost >= possible_collision_cost_ || possible_collision_cost_ < 1.0f)) - { - score_cost = static_cast(collision_checker_.footprintCostAtPose( - static_cast(x), static_cast(y), static_cast(theta), - costmap_ros_->getRobotFootprint())); - } - - switch (static_cast(score_cost)) { - case (nav2_costmap_2d::LETHAL_OBSTACLE): - return true; - case (nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE): - return consider_footprint_ ? false : true; - case (nav2_costmap_2d::NO_INFORMATION): - return is_tracking_unknown_ ? false : true; - } - - return false; - } - /** * @brief Find the min cost of the inflation decay function for which the robot MAY be * in collision in any orientation @@ -134,6 +102,7 @@ class CostCritic : public CriticFunction float collision_cost_{0.0f}; float critical_cost_{0.0f}; unsigned int near_collision_cost_{253}; + unsigned int near_collision_cost_footprint_{253}; float weight_{0}; unsigned int trajectory_point_step_; diff --git a/nav2_mppi_controller/src/critics/cost_critic.cpp b/nav2_mppi_controller/src/critics/cost_critic.cpp index 08681b144b6..78a0c83644c 100644 --- a/nav2_mppi_controller/src/critics/cost_critic.cpp +++ b/nav2_mppi_controller/src/critics/cost_critic.cpp @@ -193,19 +193,45 @@ void CostCritic::score(CriticData & data) } } - if (inCollision(pose_cost, Tx, Ty, traj_yaw(i, j))) { + float cost_for_scoring = pose_cost; + bool use_footprint_cost = consider_footprint_ && + (pose_cost >= possible_collision_cost_ || possible_collision_cost_ < 1.0f); + + if (use_footprint_cost) + { + cost_for_scoring = static_cast(collision_checker_.footprintCostAtPose( + static_cast(Tx), static_cast(Ty), static_cast(traj_yaw(i, j)), + costmap_ros_->getRobotFootprint())); + } + + // Check for collision based on the appropriate cost + bool in_collision = false; + + switch (static_cast(cost_for_scoring)) { + case (nav2_costmap_2d::LETHAL_OBSTACLE): + in_collision = true; + break; + case (nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE): + in_collision = use_footprint_cost ? false : true; + break; + case (nav2_costmap_2d::NO_INFORMATION): + in_collision = is_tracking_unknown_ ? false : true; + break; + default: + in_collision = false; + } + + if (in_collision) { traj_cost = collision_cost_; trajectory_collide = true; break; } // Let near-collision trajectory points be punished severely - // Note that we collision check based on the footprint actual, - // but score based on the center-point cost regardless - if (pose_cost >= static_cast(near_collision_cost_)) { + if (cost_for_scoring >= near_collision_cost_) { traj_cost += critical_cost_; } else if (!near_goal) { // Generally prefer trajectories further from obstacles - traj_cost += pose_cost; + traj_cost += cost_for_scoring; } } From 6fcf2fd1b8994537134b09a8056478df9f06fca7 Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Wed, 5 Nov 2025 15:10:18 +0100 Subject: [PATCH 2/4] remove unused param Signed-off-by: Tony Najjar --- .../include/nav2_mppi_controller/critics/cost_critic.hpp | 1 - 1 file changed, 1 deletion(-) diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/critics/cost_critic.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/critics/cost_critic.hpp index f97d05f6965..5fc0e80fa94 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/critics/cost_critic.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/critics/cost_critic.hpp @@ -102,7 +102,6 @@ class CostCritic : public CriticFunction float collision_cost_{0.0f}; float critical_cost_{0.0f}; unsigned int near_collision_cost_{253}; - unsigned int near_collision_cost_footprint_{253}; float weight_{0}; unsigned int trajectory_point_step_; From 0ff64bf8a03c2a5f3e6527ae236d0828274d3c1e Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Wed, 5 Nov 2025 15:51:09 +0100 Subject: [PATCH 3/4] lint Signed-off-by: Tony Najjar --- nav2_mppi_controller/src/critics/cost_critic.cpp | 9 ++++----- 1 file changed, 4 insertions(+), 5 deletions(-) diff --git a/nav2_mppi_controller/src/critics/cost_critic.cpp b/nav2_mppi_controller/src/critics/cost_critic.cpp index 78a0c83644c..43e1eeb6f64 100644 --- a/nav2_mppi_controller/src/critics/cost_critic.cpp +++ b/nav2_mppi_controller/src/critics/cost_critic.cpp @@ -197,16 +197,15 @@ void CostCritic::score(CriticData & data) bool use_footprint_cost = consider_footprint_ && (pose_cost >= possible_collision_cost_ || possible_collision_cost_ < 1.0f); - if (use_footprint_cost) - { + if (use_footprint_cost) { cost_for_scoring = static_cast(collision_checker_.footprintCostAtPose( static_cast(Tx), static_cast(Ty), static_cast(traj_yaw(i, j)), costmap_ros_->getRobotFootprint())); } - + // Check for collision based on the appropriate cost bool in_collision = false; - + switch (static_cast(cost_for_scoring)) { case (nav2_costmap_2d::LETHAL_OBSTACLE): in_collision = true; @@ -220,7 +219,7 @@ void CostCritic::score(CriticData & data) default: in_collision = false; } - + if (in_collision) { traj_cost = collision_cost_; trajectory_collide = true; From 4096a38d15e1c72a906f49b5389f9fcee7a8d80a Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Wed, 5 Nov 2025 17:15:42 +0100 Subject: [PATCH 4/4] fix near_goal exception Signed-off-by: Tony Najjar --- nav2_mppi_controller/src/critics/cost_critic.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nav2_mppi_controller/src/critics/cost_critic.cpp b/nav2_mppi_controller/src/critics/cost_critic.cpp index 43e1eeb6f64..e0271d8911c 100644 --- a/nav2_mppi_controller/src/critics/cost_critic.cpp +++ b/nav2_mppi_controller/src/critics/cost_critic.cpp @@ -227,7 +227,7 @@ void CostCritic::score(CriticData & data) } // Let near-collision trajectory points be punished severely - if (cost_for_scoring >= near_collision_cost_) { + if (cost_for_scoring >= near_collision_cost_ && !near_goal) { traj_cost += critical_cost_; } else if (!near_goal) { // Generally prefer trajectories further from obstacles traj_cost += cost_for_scoring;