Backport #4996 (#5039)

Signed-off-by: mini-1235 <mauricepurnawan@gmail.com>

Author: mini-1235

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/exceptions.hpp"
 
 namespace mppi::critics
 {
@@ -52,6 +53,18 @@ void CostCritic::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 cost critic.");
+    if (costmap_ros_->getUseRadius()) {
+      throw nav2_core::PlannerException(
+              "Considering footprint in collision checking but no robot footprint provided in the "
+              "costmap.");
+    }
+  }
+
   RCLCPP_INFO(
     logger_,
     "InflationCostCritic instantiated with %d power and %f / %f weights. "

nav2_mppi_controller/src/critics/obstacles_critic.cpp

@@ -14,6 +14,7 @@
 
 #include <cmath>
 #include "nav2_mppi_controller/critics/obstacles_critic.hpp"
+#include "nav2_core/exceptions.hpp"
 
 namespace mppi::critics
 {
@@ -42,6 +43,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::PlannerException(
+              "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

@@ -31,6 +31,7 @@
 #include "nav2_mppi_controller/critics/prefer_forward_critic.hpp"
 #include "nav2_mppi_controller/critics/twirling_critic.hpp"
 #include "nav2_mppi_controller/critics/velocity_deadband_critic.hpp"
+#include "nav2_core/exceptions.hpp"
 #include "utils_test.cpp"  // NOLINT
 
 // Tests the various critic plugin functions
@@ -115,6 +116,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", "", "dummy_costmap");
+  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::PlannerException
+  );
+}
+
+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", "", "dummy_costmap");
+  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", "", "dummy_costmap");
+  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::PlannerException
+  );
+}
+
+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", "", "dummy_costmap");
+  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