Add near collision cost and warnings for misaligned parameter settings in MPPI critics

nav2_bringup/params/nav2_params.yaml

@@ -161,8 +161,9 @@ controller_server:
         enabled: true
         cost_power: 1
         cost_weight: 3.81
+        near_collision_cost: 253
         critical_cost: 300.0
-        consider_footprint: true
+        consider_footprint: false
         collision_cost: 1000000.0
         near_goal_distance: 1.0
         trajectory_point_step: 2

nav2_mppi_controller/include/nav2_mppi_controller/critics/cost_critic.hpp

@@ -133,6 +133,7 @@ class CostCritic : public CriticFunction
   float circumscribed_cost_{0.0f};
   float collision_cost_{0.0f};
   float critical_cost_{0.0f};
+  unsigned int near_collision_cost_{253};
   float weight_{0};
   unsigned int trajectory_point_step_;
 

nav2_mppi_controller/src/critics/cost_critic.cpp

@@ -15,6 +15,7 @@
 
 #include <cmath>
 #include "nav2_mppi_controller/critics/cost_critic.hpp"
+#include "nav2_core/controller_exceptions.hpp"
 
 namespace mppi::critics
 {
@@ -26,6 +27,7 @@
   getParam(power_, "cost_power", 1);
   getParam(weight_, "cost_weight", 3.81f);
   getParam(critical_cost_, "critical_cost", 300.0f);
+  getParam(near_collision_cost_, "near_collision_cost", 253);
   getParam(collision_cost_, "collision_cost", 1000000.0f);
   getParam(near_goal_distance_, "near_goal_distance", 0.5f);
   getParam(inflation_layer_name_, "inflation_layer_name", std::string(""));
@@ -54,6 +56,22 @@
       " for full instructions. This will substantially impact run-time performance.");
   }
 
+  if (costmap_ros_->getUseRadius() == consider_footprint_) {
+    RCLCPP_WARN(
+    logger_,
+    "Inconsistent configuration in collision checking. Please verify the robot's shape settings "
+    "in both the costmap and the cost critic.");
+    if (costmap_ros_->getUseRadius()) {
+      throw nav2_core::ControllerException(
+      "Considering footprint in collision checking but no robot footprint provided in the "
+      "costmap.");
+    }
+  }
+
+  if(near_collision_cost_ > 253) {
+    RCLCPP_WARN(logger_, "Near collision cost is set higher than INSCRIBED_INFLATED_OBSTACLE");
+  }
+
   RCLCPP_INFO(
     logger_,
     "InflationCostCritic instantiated with %d power and %f / %f weights. "
@@ -153,28 +171,24 @@
       // The footprintCostAtPose will always return "INSCRIBED" if footprint is over it
       // So the center point has more information than the footprint
       if (!worldToMapFloat(Tx, Ty, x_i, y_i)) {
-        if (!is_tracking_unknown_) {
-          traj_cost = collision_cost_;
-          trajectory_collide = true;
-          break;
-        }
         pose_cost = 255.0f;  // NO_INFORMATION in float
       } else {
         pose_cost = static_cast<float>(costmap->getCost(getIndex(x_i, y_i)));
         if (pose_cost < 1.0f) {
           continue;  // In free space
         }
-        if (inCollision(pose_cost, Tx, Ty, traj_yaw(i, j))) {
-          traj_cost = collision_cost_;
-          trajectory_collide = true;
-          break;
-        }
+      }
+
+      if (inCollision(pose_cost, Tx, Ty, traj_yaw(i, j))) {
+        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 >= 253.0f /*INSCRIBED_INFLATED_OBSTACLE in float*/) {
+      if (pose_cost >= static_cast<float>(near_collision_cost_)) {
         traj_cost += critical_cost_;
       } else if (!near_goal) {  // Generally prefer trajectories further from obstacles
         traj_cost += pose_cost;

nav2_mppi_controller/src/critics/obstacles_critic.cpp

@@ -16,6 +16,8 @@
 #include <cmath>
 #include "nav2_mppi_controller/critics/obstacles_critic.hpp"
 #include "nav2_costmap_2d/inflation_layer.hpp"
+#include "nav2_core/controller_exceptions.hpp"
+
 namespace mppi::critics
 {
 
@@ -44,6 +46,18 @@ void ObstaclesCritic::initialize()
       " for full instructions. This will substantially impact run-time performance.");
   }
 
+  if (costmap_ros_->getUseRadius() == consider_footprint_) {
+    RCLCPP_WARN(
+    logger_,
+    "Inconsistent configuration in collision checking. Please verify the robot's shape settings "
+    "in both the costmap and the obstacle critic.");
+    if (costmap_ros_->getUseRadius()) {
+      throw nav2_core::ControllerException(
+      "Considering footprint in collision checking but no robot footprint provided in the "
+      "costmap.");
+    }
+  }
+
   RCLCPP_INFO(
     logger_,
     "ObstaclesCritic instantiated with %d power and %f / %f weights. "

nav2_mppi_controller/test/critics_tests.cpp

@@ -126,6 +126,81 @@ TEST(CriticTests, ConstraintsCritic)
   EXPECT_NEAR(costs(1), 0.48, 0.01);
 }
 
+TEST(CriticTests, ObstacleCriticMisalignedParams) {
+  auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("my_node");
+  auto costmap_ros = std::make_shared<nav2_costmap_2d::Costmap2DROS>(
+    "dummy_costmap", "", true);
+  ParametersHandler param_handler(node);
+  auto getParam = param_handler.getParamGetter("critic");
+  bool consider_footprint;
+  getParam(consider_footprint, "consider_footprint", true);
+
+  rclcpp_lifecycle::State lstate;
+  costmap_ros->on_configure(lstate);
+
+  ObstaclesCritic critic;
+  // Expect throw when settings mismatched
+  EXPECT_THROW(
+    critic.on_configure(node, "mppi", "critic", costmap_ros, &param_handler),
+    nav2_core::ControllerException
+  );
+}
+
+TEST(CriticTests, ObstacleCriticAlignedParams) {
+  auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("my_node");
+  auto costmap_ros = std::make_shared<nav2_costmap_2d::Costmap2DROS>(
+    "dummy_costmap", "", true);
+  ParametersHandler param_handler(node);
+  auto getParam = param_handler.getParamGetter("critic");
+  bool consider_footprint;
+  getParam(consider_footprint, "consider_footprint", false);
+
+  rclcpp_lifecycle::State lstate;
+  costmap_ros->on_configure(lstate);
+
+  ObstaclesCritic critic;
+  critic.on_configure(node, "mppi", "critic", costmap_ros, &param_handler);
+  EXPECT_EQ(critic.getName(), "critic");
+}
+
+
+TEST(CriticTests, CostCriticMisAlignedParams) {
+  // Standard preamble
+  auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("my_node");
+  auto costmap_ros = std::make_shared<nav2_costmap_2d::Costmap2DROS>(
+    "dummy_costmap", "", true);
+  ParametersHandler param_handler(node);
+  rclcpp_lifecycle::State lstate;
+   auto getParam = param_handler.getParamGetter("critic");
+  bool consider_footprint;
+  getParam(consider_footprint, "consider_footprint", true);
+  costmap_ros->on_configure(lstate);
+
+  CostCritic critic;
+  // Expect throw when settings mismatched
+  EXPECT_THROW(
+    critic.on_configure(node, "mppi", "critic", costmap_ros, &param_handler),
+    nav2_core::ControllerException
+  );
+}
+
+TEST(CriticTests, CostCriticAlignedParams) {
+  // Standard preamble
+  auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("my_node");
+  auto costmap_ros = std::make_shared<nav2_costmap_2d::Costmap2DROS>(
+    "dummy_costmap", "", true);
+  ParametersHandler param_handler(node);
+  rclcpp_lifecycle::State lstate;
+   auto getParam = param_handler.getParamGetter("critic");
+  bool consider_footprint;
+  getParam(consider_footprint, "consider_footprint", false);
+  costmap_ros->on_configure(lstate);
+
+  CostCritic critic;
+  critic.on_configure(node, "mppi", "critic", costmap_ros, &param_handler);
+  EXPECT_EQ(critic.getName(), "critic");
+}
+
 TEST(CriticTests, GoalAngleCritic)
 {
   // Standard preamble

nav2_system_tests/src/gps_navigation/nav2_no_map_params.yaml

@@ -109,8 +109,9 @@ controller_server:
         enabled: true
         cost_power: 1
         cost_weight: 3.81
+        near_collision_cost: 253
         critical_cost: 300.0
-        consider_footprint: true
+        consider_footprint: false
         collision_cost: 1000000.0
         near_goal_distance: 1.0
         trajectory_point_step: 2