adding error warnings around incorrect inflation layer setups in MPPI and Smac which impact performance substantially

nav2_mppi_controller/src/critics/obstacles_critic.cpp

@@ -31,6 +31,17 @@ void ObstaclesCritic::initialize()
 
   collision_checker_.setCostmap(costmap_);
   possibly_inscribed_cost_ = findCircumscribedCost(costmap_ros_);
+
+  if (possibly_inscribed_cost_ < 1) {
+    RCLCPP_ERROR(
+      logger_,
+      "Inflation layer either not found or inflation is not set sufficiently for "
+      "optimized non-circular collision checking capabilities. It is HIGHLY recommended to set"
+      " the inflation radius to be at MINIMUM half of the robot's largest cross-section. See "
+      "github.com/ros-planning/navigation2/tree/main/nav2_smac_planner#potential-fields"
+      " for full instructions. This will substantially impact run-time performance.");
+  }
+
   RCLCPP_INFO(
     logger_,
     "ObstaclesCritic instantiated with %d power and %f / %f weights. "
@@ -183,7 +194,7 @@ CollisionCost ObstaclesCritic::costAtPose(float x, float y, float theta)
   collision_checker_.worldToMap(x, y, x_i, y_i);
   cost = collision_checker_.pointCost(x_i, y_i);
 
-  if (consider_footprint_ && cost >= possibly_inscribed_cost_) {
+  if (consider_footprint_ && (cost >= possibly_inscribed_cost_ || possibly_inscribed_cost_ < 1)) {
     cost = static_cast<float>(collision_checker_.footprintCostAtPose(
         x, y, theta, costmap_ros_->getRobotFootprint()));
     collision_cost.using_footprint = true;

nav2_smac_planner/include/nav2_smac_planner/collision_checker.hpp

@@ -14,6 +14,7 @@
 #include <vector>
 #include "nav2_costmap_2d/footprint_collision_checker.hpp"
 #include "nav2_smac_planner/constants.hpp"
+#include "rclcpp_lifecycle/lifecycle_node.hpp"
 
 #ifndef NAV2_SMAC_PLANNER__COLLISION_CHECKER_HPP_
 #define NAV2_SMAC_PLANNER__COLLISION_CHECKER_HPP_
@@ -34,11 +35,13 @@ class GridCollisionChecker
    * for use when regular bin intervals are appropriate
    * @param costmap The costmap to collision check against
    * @param num_quantizations The number of quantizations to precompute footprint
+   * @param node Node to extract clock and logger from
    * orientations for to speed up collision checking
    */
   GridCollisionChecker(
     nav2_costmap_2d::Costmap2D * costmap,
-    unsigned int num_quantizations);
+    unsigned int num_quantizations,
+    rclcpp_lifecycle::LifecycleNode::SharedPtr node);
 
   /**
    * @brief A constructor for nav2_smac_planner::GridCollisionChecker
@@ -117,6 +120,8 @@ class GridCollisionChecker
   bool footprint_is_radius_;
   std::vector<float> angles_;
   double possible_inscribed_cost_{-1};
+  rclcpp::Logger logger_{rclcpp::get_logger("SmacPlannerCollisionChecker")};
+  rclcpp::Clock::SharedPtr clock_;
 };
 
 }  // namespace nav2_smac_planner

nav2_smac_planner/src/collision_checker.cpp

@@ -19,9 +19,15 @@ namespace nav2_smac_planner
 
 GridCollisionChecker::GridCollisionChecker(
   nav2_costmap_2d::Costmap2D * costmap,
-  unsigned int num_quantizations)
+  unsigned int num_quantizations,
+  rclcpp_lifecycle::LifecycleNode::SharedPtr node)
 : FootprintCollisionChecker(costmap)
 {
+  if (node) {
+    clock_ = node->get_clock();
+    logger_ = node->get_logger();
+  }
+
   // Convert number of regular bins into angles
   float bin_size = 2 * M_PI / static_cast<float>(num_quantizations);
   angles_.reserve(num_quantizations);
@@ -104,7 +110,17 @@ bool GridCollisionChecker::inCollision(
       static_cast<unsigned int>(x), static_cast<unsigned int>(y));
 
     if (footprint_cost_ < possible_inscribed_cost_) {
-      return false;
+      if (possible_inscribed_cost_ > 0) {
+        return false;
+      } else {
+        RCLCPP_ERROR_THROTTLE(
+          logger_, *clock_, 1000,
+          "Inflation layer either not found or inflation is not set sufficiently for "
+          "optimized non-circular collision checking capabilities. It is HIGHLY recommended to set"
+          " the inflation radius to be at MINIMUM half of the robot's largest cross-section. See "
+          "github.com/ros-planning/navigation2/tree/main/nav2_smac_planner#potential-fields"
+          " for full instructions. This will substantially impact run-time performance.");
+      }
     }
 
     // If its inscribed, in collision, or unknown in the middle,

nav2_smac_planner/src/smac_planner_2d.cpp

@@ -31,7 +31,7 @@ using std::placeholders::_1;
 
 SmacPlanner2D::SmacPlanner2D()
 : _a_star(nullptr),
-  _collision_checker(nullptr, 1),
+  _collision_checker(nullptr, 1, nullptr),
   _smoother(nullptr),
   _costmap(nullptr),
   _costmap_downsampler(nullptr)
@@ -108,7 +108,7 @@ void SmacPlanner2D::configure(
   }
 
   // Initialize collision checker
-  _collision_checker = GridCollisionChecker(_costmap, 1 /*for 2D, most be 1*/);
+  _collision_checker = GridCollisionChecker(_costmap, 1 /*for 2D, most be 1*/, node);
   _collision_checker.setFootprint(
     costmap_ros->getRobotFootprint(),
     true /*for 2D, most use radius*/,

nav2_smac_planner/src/smac_planner_hybrid.cpp

@@ -32,7 +32,7 @@ using std::placeholders::_1;
 
 SmacPlannerHybrid::SmacPlannerHybrid()
 : _a_star(nullptr),
-  _collision_checker(nullptr, 1),
+  _collision_checker(nullptr, 1, nullptr),
   _smoother(nullptr),
   _costmap(nullptr),
   _costmap_downsampler(nullptr)
@@ -186,7 +186,7 @@ void SmacPlannerHybrid::configure(
   }
 
   // Initialize collision checker
-  _collision_checker = GridCollisionChecker(_costmap, _angle_quantizations);
+  _collision_checker = GridCollisionChecker(_costmap, _angle_quantizations, node);
   _collision_checker.setFootprint(
     _costmap_ros->getRobotFootprint(),
     _costmap_ros->getUseRadius(),
@@ -552,6 +552,8 @@ SmacPlannerHybrid::dynamicParametersCallback(std::vector<rclcpp::Parameter> para
       _lookup_table_dim += 1.0;
     }
 
+    auto node = _node.lock();
+
     // Re-Initialize A* template
     if (reinit_a_star) {
       _a_star = std::make_unique<AStarAlgorithm<NodeHybrid>>(_motion_model, _search_info);
@@ -567,7 +569,6 @@ SmacPlannerHybrid::dynamicParametersCallback(std::vector<rclcpp::Parameter> para
     // Re-Initialize costmap downsampler
     if (reinit_downsampler) {
       if (_downsample_costmap && _downsampling_factor > 1) {
-        auto node = _node.lock();
         std::string topic_name = "downsampled_costmap";
         _costmap_downsampler = std::make_unique<CostmapDownsampler>();
         _costmap_downsampler->on_configure(
@@ -577,7 +578,7 @@ SmacPlannerHybrid::dynamicParametersCallback(std::vector<rclcpp::Parameter> para
 
     // Re-Initialize collision checker
     if (reinit_collision_checker) {
-      _collision_checker = GridCollisionChecker(_costmap, _angle_quantizations);
+      _collision_checker = GridCollisionChecker(_costmap, _angle_quantizations, node);
       _collision_checker.setFootprint(
         _costmap_ros->getRobotFootprint(),
         _costmap_ros->getUseRadius(),
@@ -586,7 +587,6 @@ SmacPlannerHybrid::dynamicParametersCallback(std::vector<rclcpp::Parameter> para
 
     // Re-Initialize smoother
     if (reinit_smoother) {
-      auto node = _node.lock();
       SmootherParams params;
       params.get(node, _name);
       _smoother = std::make_unique<Smoother>(params);

nav2_smac_planner/src/smac_planner_lattice.cpp

@@ -31,7 +31,7 @@ using rcl_interfaces::msg::ParameterType;
 
 SmacPlannerLattice::SmacPlannerLattice()
 : _a_star(nullptr),
-  _collision_checker(nullptr, 1),
+  _collision_checker(nullptr, 1, nullptr),
   _smoother(nullptr),
   _costmap(nullptr)
 {
@@ -169,7 +169,7 @@ void SmacPlannerLattice::configure(
   // increments causing "wobbly" checks that could cause larger robots to virtually show collisions
   // in valid configurations. This approximation helps to bound orientation error for all checks
   // in exchange for slight inaccuracies in the collision headings in terminal search states.
-  _collision_checker = GridCollisionChecker(_costmap, 72u);
+  _collision_checker = GridCollisionChecker(_costmap, 72u, node);
   _collision_checker.setFootprint(
     costmap_ros->getRobotFootprint(),
     costmap_ros->getUseRadius(),

nav2_smac_planner/test/test_a_star.cpp

@@ -39,6 +39,7 @@ RclCppFixture g_rclcppfixture;
 
 TEST(AStarTest, test_a_star_2d)
 {
+  auto lnode = std::make_shared<rclcpp_lifecycle::LifecycleNode>("test");
   nav2_smac_planner::SearchInfo info;
   nav2_smac_planner::AStarAlgorithm<nav2_smac_planner::Node2D> a_star(
     nav2_smac_planner::MotionModel::TWOD, info);
@@ -62,7 +63,7 @@ TEST(AStarTest, test_a_star_2d)
 
   // functional case testing
   std::unique_ptr<nav2_smac_planner::GridCollisionChecker> checker =
-    std::make_unique<nav2_smac_planner::GridCollisionChecker>(costmapA, 1);
+    std::make_unique<nav2_smac_planner::GridCollisionChecker>(costmapA, 1, lnode);
   checker->setFootprint(nav2_costmap_2d::Footprint(), true, 0.0);
   a_star.setCollisionChecker(checker.get());
   a_star.setStart(20u, 20u, 0);
@@ -122,6 +123,7 @@ TEST(AStarTest, test_a_star_2d)
 
 TEST(AStarTest, test_a_star_se2)
 {
+  auto lnode = std::make_shared<rclcpp_lifecycle::LifecycleNode>("test");
   nav2_smac_planner::SearchInfo info;
   info.change_penalty = 0.1;
   info.non_straight_penalty = 1.1;
@@ -152,7 +154,7 @@ TEST(AStarTest, test_a_star_se2)
   }
 
   std::unique_ptr<nav2_smac_planner::GridCollisionChecker> checker =
-    std::make_unique<nav2_smac_planner::GridCollisionChecker>(costmapA, size_theta);
+    std::make_unique<nav2_smac_planner::GridCollisionChecker>(costmapA, size_theta, lnode);
   checker->setFootprint(nav2_costmap_2d::Footprint(), true, 0.0);
 
   // functional case testing
@@ -178,6 +180,7 @@ TEST(AStarTest, test_a_star_se2)
 
 TEST(AStarTest, test_a_star_lattice)
 {
+  auto lnode = std::make_shared<rclcpp_lifecycle::LifecycleNode>("test");
   nav2_smac_planner::SearchInfo info;
   info.change_penalty = 0.05;
   info.non_straight_penalty = 1.05;
@@ -213,7 +216,7 @@ TEST(AStarTest, test_a_star_lattice)
   }
 
   std::unique_ptr<nav2_smac_planner::GridCollisionChecker> checker =
-    std::make_unique<nav2_smac_planner::GridCollisionChecker>(costmapA, size_theta);
+    std::make_unique<nav2_smac_planner::GridCollisionChecker>(costmapA, size_theta, lnode);
   checker->setFootprint(nav2_costmap_2d::Footprint(), true, 0.0);
 
   // functional case testing
@@ -225,7 +228,7 @@ TEST(AStarTest, test_a_star_lattice)
 
   // check path is the right size and collision free
   EXPECT_EQ(num_it, 21);
-  EXPECT_EQ(path.size(), 49u);
+  EXPECT_GT(path.size(), 47u);
   for (unsigned int i = 0; i != path.size(); i++) {
     EXPECT_EQ(costmapA->getCost(path[i].x, path[i].y), 0);
   }
@@ -239,6 +242,7 @@ TEST(AStarTest, test_a_star_lattice)
 
 TEST(AStarTest, test_se2_single_pose_path)
 {
+  auto lnode = std::make_shared<rclcpp_lifecycle::LifecycleNode>("test");
   nav2_smac_planner::SearchInfo info;
   info.change_penalty = 0.1;
   info.non_straight_penalty = 1.1;
@@ -263,7 +267,7 @@ TEST(AStarTest, test_se2_single_pose_path)
     new nav2_costmap_2d::Costmap2D(100, 100, 0.1, 0.0, 0.0, 0);
 
   std::unique_ptr<nav2_smac_planner::GridCollisionChecker> checker =
-    std::make_unique<nav2_smac_planner::GridCollisionChecker>(costmapA, size_theta);
+    std::make_unique<nav2_smac_planner::GridCollisionChecker>(costmapA, size_theta, lnode);
   checker->setFootprint(nav2_costmap_2d::Footprint(), true, 0.0);
 
   // functional case testing

nav2_smac_planner/test/test_collision_checker.cpp

@@ -19,11 +19,21 @@
 
 #include "gtest/gtest.h"
 #include "nav2_smac_planner/collision_checker.hpp"
+#include "nav2_util/lifecycle_node.hpp"
 
 using namespace nav2_costmap_2d;  // NOLINT
 
+class RclCppFixture
+{
+public:
+  RclCppFixture() {rclcpp::init(0, nullptr);}
+  ~RclCppFixture() {rclcpp::shutdown();}
+};
+RclCppFixture g_rclcppfixture;
+
 TEST(collision_footprint, test_basic)
 {
+  auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("testA");
   nav2_costmap_2d::Costmap2D * costmap_ = new nav2_costmap_2d::Costmap2D(100, 100, 0.1, 0, 0, 0);
 
   geometry_msgs::msg::Point p1;
@@ -41,7 +51,7 @@ TEST(collision_footprint, test_basic)
 
   nav2_costmap_2d::Footprint footprint = {p1, p2, p3, p4};
 
-  nav2_smac_planner::GridCollisionChecker collision_checker(costmap_, 72);
+  nav2_smac_planner::GridCollisionChecker collision_checker(costmap_, 72, node);
   collision_checker.setFootprint(footprint, false /*use footprint*/, 0.0);
   collision_checker.inCollision(5.0, 5.0, 0.0, false);
   float cost = collision_checker.getCost();
@@ -51,9 +61,10 @@ TEST(collision_footprint, test_basic)
 
 TEST(collision_footprint, test_point_cost)
 {
+  auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("testB");
   nav2_costmap_2d::Costmap2D * costmap_ = new nav2_costmap_2d::Costmap2D(100, 100, 0.1, 0, 0, 0);
 
-  nav2_smac_planner::GridCollisionChecker collision_checker(costmap_, 72);
+  nav2_smac_planner::GridCollisionChecker collision_checker(costmap_, 72, node);
   nav2_costmap_2d::Footprint footprint;
   collision_checker.setFootprint(footprint, true /*radius / pointcose*/, 0.0);
 
@@ -65,9 +76,10 @@ TEST(collision_footprint, test_point_cost)
 
 TEST(collision_footprint, test_world_to_map)
 {
+  auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("testC");
   nav2_costmap_2d::Costmap2D * costmap_ = new nav2_costmap_2d::Costmap2D(100, 100, 0.1, 0, 0, 0);
 
-  nav2_smac_planner::GridCollisionChecker collision_checker(costmap_, 72);
+  nav2_smac_planner::GridCollisionChecker collision_checker(costmap_, 72, node);
   nav2_costmap_2d::Footprint footprint;
   collision_checker.setFootprint(footprint, true /*radius / point cost*/, 0.0);
 
@@ -90,6 +102,7 @@ TEST(collision_footprint, test_world_to_map)
 
 TEST(collision_footprint, test_footprint_at_pose_with_movement)
 {
+  auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("testD");
   nav2_costmap_2d::Costmap2D * costmap_ = new nav2_costmap_2d::Costmap2D(100, 100, 0.1, 0, 0, 254);
 
   for (unsigned int i = 40; i <= 60; ++i) {
@@ -113,7 +126,7 @@ TEST(collision_footprint, test_footprint_at_pose_with_movement)
 
   nav2_costmap_2d::Footprint footprint = {p1, p2, p3, p4};
 
-  nav2_smac_planner::GridCollisionChecker collision_checker(costmap_, 72);
+  nav2_smac_planner::GridCollisionChecker collision_checker(costmap_, 72, node);
   collision_checker.setFootprint(footprint, false /*use footprint*/, 0.0);
 
   collision_checker.inCollision(50, 50, 0.0, false);
@@ -132,6 +145,7 @@ TEST(collision_footprint, test_footprint_at_pose_with_movement)
 
 TEST(collision_footprint, test_point_and_line_cost)
 {
+  auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("testE");
   nav2_costmap_2d::Costmap2D * costmap_ = new nav2_costmap_2d::Costmap2D(
     100, 100, 0.10000, 0, 0.0, 128.0);
 
@@ -153,7 +167,7 @@ TEST(collision_footprint, test_point_and_line_cost)
 
   nav2_costmap_2d::Footprint footprint = {p1, p2, p3, p4};
 
-  nav2_smac_planner::GridCollisionChecker collision_checker(costmap_, 72);
+  nav2_smac_planner::GridCollisionChecker collision_checker(costmap_, 72, node);
   collision_checker.setFootprint(footprint, false /*use footprint*/, 0.0);
 
   collision_checker.inCollision(50, 50, 0.0, false);

nav2_smac_planner/test/test_node2d.cpp

@@ -34,9 +34,10 @@ RclCppFixture g_rclcppfixture;
 
 TEST(Node2DTest, test_node_2d)
 {
+  auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("test");
   nav2_costmap_2d::Costmap2D costmapA(10, 10, 0.05, 0.0, 0.0, 0);
   std::unique_ptr<nav2_smac_planner::GridCollisionChecker> checker =
-    std::make_unique<nav2_smac_planner::GridCollisionChecker>(&costmapA, 72);
+    std::make_unique<nav2_smac_planner::GridCollisionChecker>(&costmapA, 72, node);
   checker->setFootprint(nav2_costmap_2d::Footprint(), true, 0.0);
 
   // test construction
@@ -101,6 +102,7 @@ TEST(Node2DTest, test_node_2d)
 
 TEST(Node2DTest, test_node_2d_neighbors)
 {
+  auto lnode = std::make_shared<rclcpp_lifecycle::LifecycleNode>("test");
   nav2_smac_planner::SearchInfo info;
   unsigned int size_x = 10u;
   unsigned int size_y = 10u;
@@ -122,7 +124,7 @@ TEST(Node2DTest, test_node_2d_neighbors)
 
   nav2_costmap_2d::Costmap2D costmapA(10, 10, 0.05, 0.0, 0.0, 0);
   std::unique_ptr<nav2_smac_planner::GridCollisionChecker> checker =
-    std::make_unique<nav2_smac_planner::GridCollisionChecker>(&costmapA, 72);
+    std::make_unique<nav2_smac_planner::GridCollisionChecker>(&costmapA, 72, lnode);
   unsigned char cost = static_cast<unsigned int>(1);
   nav2_smac_planner::Node2D * node = new nav2_smac_planner::Node2D(1);
   node->setCost(cost);