Adding Smac Planner Tolerances for Feasible Planners

nav2_smac_planner/README.md

@@ -103,12 +103,12 @@ planner_server:
 
     GridBased:
       plugin: "nav2_smac_planner/SmacPlannerHybrid"
-      tolerance: 0.5                      # tolerance for planning if unable to reach exact pose, in meters, for 2D node
+      tolerance: 0.5                      # tolerance for planning if unable to reach exact pose, in meters
       downsample_costmap: false           # whether or not to downsample the map
       downsampling_factor: 1              # multiplier for the resolution of the costmap layer (e.g. 2 on a 5cm costmap would be 10cm)
       allow_unknown: false                # allow traveling in unknown space
       max_iterations: 1000000             # maximum total iterations to search for before failing (in case unreachable), set to -1 to disable
-      max_on_approach_iterations: 1000    # maximum number of iterations to attempt to reach goal once in tolerance, 2D only
+      max_on_approach_iterations: 1000    # maximum number of iterations to attempt to reach goal once in tolerance
       max_planning_time: 3.5              # max time in s for planner to plan, smooth, and upsample. Will scale maximum smoothing and upsampling times based on remaining time after planning.
       motion_model_for_search: "DUBIN"    # For Hybrid Dubin, Redds-Shepp
       cost_travel_multiplier: 2.0         # For 2D: Cost multiplier to apply to search to steer away from high cost areas. Larger values will place in the center of aisles more exactly (if non-`FREE` cost potential field exists) but take slightly longer to compute. To optimize for speed, a value of 1.0 is reasonable. A reasonable tradeoff value is 2.0. A value of 0.0 effective disables steering away from obstacles and acts like a naive binary search A*.

nav2_smac_planner/include/nav2_smac_planner/smac_planner_hybrid.hpp

@@ -111,6 +111,7 @@ class SmacPlannerHybrid : public nav2_core::GlobalPlanner
   unsigned int _angle_quantizations;
   bool _allow_unknown;
   int _max_iterations;
+  int _max_on_approach_iterations;
   SearchInfo _search_info;
   double _max_planning_time;
   double _lookup_table_size;

nav2_smac_planner/include/nav2_smac_planner/smac_planner_lattice.hpp

@@ -106,6 +106,7 @@ class SmacPlannerLattice : public nav2_core::GlobalPlanner
   SearchInfo _search_info;
   bool _allow_unknown;
   int _max_iterations;
+  int _max_on_approach_iterations;
   float _tolerance;
   rclcpp_lifecycle::LifecyclePublisher<nav_msgs::msg::Path>::SharedPtr _raw_plan_publisher;
   double _max_planning_time;

nav2_smac_planner/src/a_star.cpp

@@ -326,6 +326,11 @@ bool AStarAlgorithm<NodeT>::createPath(
     }
   }
 
+  if (_best_heuristic_node.first < getToleranceHeuristic()) {
+    // If we run out of serach options, return the path that is closest, if within tolerance.
+    return _graph.at(_best_heuristic_node.second).backtracePath(path);
+  }
+
   return false;
 }
 

nav2_smac_planner/src/smac_planner_hybrid.cpp

@@ -80,12 +80,18 @@ void SmacPlannerHybrid::configure(
   _angle_bin_size = 2.0 * M_PI / angle_quantizations;
   _angle_quantizations = static_cast<unsigned int>(angle_quantizations);
 
+  nav2_util::declare_parameter_if_not_declared(
+    node, name + ".tolerance", rclcpp::ParameterValue(0.25));
+  _tolerance = static_cast<float>(node->get_parameter(name + ".tolerance").as_double());
   nav2_util::declare_parameter_if_not_declared(
     node, name + ".allow_unknown", rclcpp::ParameterValue(true));
   node->get_parameter(name + ".allow_unknown", _allow_unknown);
   nav2_util::declare_parameter_if_not_declared(
     node, name + ".max_iterations", rclcpp::ParameterValue(1000000));
   node->get_parameter(name + ".max_iterations", _max_iterations);
+  nav2_util::declare_parameter_if_not_declared(
+    node, name + ".max_on_approach_iterations", rclcpp::ParameterValue(1000));
+  node->get_parameter(name + ".max_on_approach_iterations", _max_on_approach_iterations);
   nav2_util::declare_parameter_if_not_declared(
     node, name + ".smooth_path", rclcpp::ParameterValue(true));
   node->get_parameter(name + ".smooth_path", smooth_path);
@@ -139,6 +145,13 @@ void SmacPlannerHybrid::configure(
       _motion_model_for_search.c_str());
   }
 
+  if (_max_on_approach_iterations <= 0) {
+    RCLCPP_INFO(
+      _logger, "On approach iteration selected as <= 0, "
+      "disabling tolerance and on approach iterations.");
+    _max_on_approach_iterations = std::numeric_limits<int>::max();
+  }
+
   if (_max_iterations <= 0) {
     RCLCPP_INFO(
       _logger, "maximum iteration selected as <= 0, "
@@ -180,7 +193,7 @@ void SmacPlannerHybrid::configure(
   _a_star->initialize(
     _allow_unknown,
     _max_iterations,
-    std::numeric_limits<int>::max(),
+    _max_on_approach_iterations,
     _max_planning_time,
     _lookup_table_dim,
     _angle_quantizations);
@@ -205,10 +218,11 @@ void SmacPlannerHybrid::configure(
 
   RCLCPP_INFO(
     _logger, "Configured plugin %s of type SmacPlannerHybrid with "
-    "maximum iterations %i, and %s. Using motion model: %s.",
-    _name.c_str(), _max_iterations,
+    "maximum iterations %i, max on approach iterations %i, and %s. Tolerance %.2f."
+    "Using motion model: %s.",
+    _name.c_str(), _max_iterations, _max_on_approach_iterations,
     _allow_unknown ? "allowing unknown traversal" : "not allowing unknown traversal",
-    toString(_motion_model).c_str());
+    _tolerance, toString(_motion_model).c_str());
 }
 
 void SmacPlannerHybrid::activate()
@@ -315,7 +329,9 @@ nav_msgs::msg::Path SmacPlannerHybrid::createPlan(
   int num_iterations = 0;
   std::string error;
   try {
-    if (!_a_star->createPath(path, num_iterations, 0.0)) {
+    if (!_a_star->createPath(
+        path, num_iterations, _tolerance / static_cast<float>(costmap->getResolution())))
+    {
       if (num_iterations < _a_star->getMaxIterations()) {
         error = std::string("no valid path found");
       } else {
@@ -392,6 +408,8 @@ SmacPlannerHybrid::dynamicParametersCallback(std::vector<rclcpp::Parameter> para
       if (name == _name + ".max_planning_time") {
         reinit_a_star = true;
         _max_planning_time = parameter.as_double();
+      } else if (name == _name + ".tolerance") {
+        _tolerance = static_cast<float>(parameter.as_double());
       } else if (name == _name + ".lookup_table_size") {
         reinit_a_star = true;
         _lookup_table_size = parameter.as_double();
@@ -452,6 +470,15 @@ SmacPlannerHybrid::dynamicParametersCallback(std::vector<rclcpp::Parameter> para
             "disabling maximum iterations.");
           _max_iterations = std::numeric_limits<int>::max();
         }
+      } else if (name == _name + ".max_on_approach_iterations") {
+        reinit_a_star = true;
+        _max_on_approach_iterations = parameter.as_int();
+        if (_max_on_approach_iterations <= 0) {
+          RCLCPP_INFO(
+            _logger, "On approach iteration selected as <= 0, "
+            "disabling tolerance and on approach iterations.");
+          _max_on_approach_iterations = std::numeric_limits<int>::max();
+        }
       } else if (name == _name + ".angle_quantization_bins") {
         reinit_collision_checker = true;
         reinit_a_star = true;
@@ -504,7 +531,7 @@ SmacPlannerHybrid::dynamicParametersCallback(std::vector<rclcpp::Parameter> para
       _a_star->initialize(
         _allow_unknown,
         _max_iterations,
-        std::numeric_limits<int>::max(),
+        _max_on_approach_iterations,
         _max_planning_time,
         _lookup_table_dim,
         _angle_quantizations);

nav2_smac_planner/src/smac_planner_lattice.cpp

@@ -65,12 +65,18 @@ void SmacPlannerLattice::configure(
   double analytic_expansion_max_length_m;
   bool smooth_path;
 
+  nav2_util::declare_parameter_if_not_declared(
+    node, name + ".tolerance", rclcpp::ParameterValue(0.25));
+  _tolerance = static_cast<float>(node->get_parameter(name + ".tolerance").as_double());
   nav2_util::declare_parameter_if_not_declared(
     node, name + ".allow_unknown", rclcpp::ParameterValue(true));
   node->get_parameter(name + ".allow_unknown", _allow_unknown);
   nav2_util::declare_parameter_if_not_declared(
     node, name + ".max_iterations", rclcpp::ParameterValue(1000000));
   node->get_parameter(name + ".max_iterations", _max_iterations);
+  nav2_util::declare_parameter_if_not_declared(
+    node, name + ".max_on_approach_iterations", rclcpp::ParameterValue(1000));
+  node->get_parameter(name + ".max_on_approach_iterations", _max_on_approach_iterations);
   nav2_util::declare_parameter_if_not_declared(
     node, name + ".smooth_path", rclcpp::ParameterValue(true));
   node->get_parameter(name + ".smooth_path", smooth_path);
@@ -126,6 +132,13 @@ void SmacPlannerLattice::configure(
     _metadata.min_turning_radius / (_costmap->getResolution());
   _motion_model = MotionModel::STATE_LATTICE;
 
+  if (_max_on_approach_iterations <= 0) {
+    RCLCPP_INFO(
+      _logger, "On approach iteration selected as <= 0, "
+      "disabling tolerance and on approach iterations.");
+    _max_on_approach_iterations = std::numeric_limits<int>::max();
+  }
+
   if (_max_iterations <= 0) {
     RCLCPP_INFO(
       _logger, "maximum iteration selected as <= 0, "
@@ -167,7 +180,7 @@ void SmacPlannerLattice::configure(
   _a_star->initialize(
     _allow_unknown,
     _max_iterations,
-    std::numeric_limits<int>::max(),
+    _max_on_approach_iterations,
     _max_planning_time,
     lookup_table_dim,
     _metadata.number_of_headings);
@@ -182,11 +195,11 @@ void SmacPlannerLattice::configure(
 
   RCLCPP_INFO(
     _logger, "Configured plugin %s of type SmacPlannerLattice with "
-    "maximum iterations %i, "
-    "and %s. Using motion model: %s. State lattice file: %s.",
-    _name.c_str(), _max_iterations,
+    "maximum iterations %i, max on approach iterations %i, "
+    "and %s. Tolerance %.2f. Using motion model: %s. State lattice file: %s.",
+    _name.c_str(), _max_iterations, _max_on_approach_iterations,
     _allow_unknown ? "allowing unknown traversal" : "not allowing unknown traversal",
-    toString(_motion_model).c_str(), _search_info.lattice_filepath.c_str());
+    _tolerance, toString(_motion_model).c_str(), _search_info.lattice_filepath.c_str());
 }
 
 void SmacPlannerLattice::activate()
@@ -262,7 +275,9 @@ nav_msgs::msg::Path SmacPlannerLattice::createPlan(
   int num_iterations = 0;
   std::string error;
   try {
-    if (!_a_star->createPath(path, num_iterations, 0 /*no tolerance*/)) {
+    if (!_a_star->createPath(
+        path, num_iterations, _tolerance / static_cast<float>(_costmap->getResolution())))
+    {
       if (num_iterations < _a_star->getMaxIterations()) {
         error = std::string("no valid path found");
       } else {
@@ -349,6 +364,8 @@ SmacPlannerLattice::dynamicParametersCallback(std::vector<rclcpp::Parameter> par
       if (name == _name + ".max_planning_time") {
         reinit_a_star = true;
         _max_planning_time = parameter.as_double();
+      } else if (name == _name + ".tolerance") {
+        _tolerance = static_cast<float>(parameter.as_double());
       } else if (name == _name + ".lookup_table_size") {
         reinit_a_star = true;
         _lookup_table_size = parameter.as_double();
@@ -403,6 +420,15 @@ SmacPlannerLattice::dynamicParametersCallback(std::vector<rclcpp::Parameter> par
           _max_iterations = std::numeric_limits<int>::max();
         }
       }
+    } else if (name == _name + ".max_on_approach_iterations") {
+      reinit_a_star = true;
+      _max_on_approach_iterations = parameter.as_int();
+      if (_max_on_approach_iterations <= 0) {
+        RCLCPP_INFO(
+          _logger, "On approach iteration selected as <= 0, "
+          "disabling tolerance and on approach iterations.");
+        _max_on_approach_iterations = std::numeric_limits<int>::max();
+      }
     } else if (type == ParameterType::PARAMETER_STRING) {
       if (name == _name + ".lattice_filepath") {
         reinit_a_star = true;
@@ -453,7 +479,7 @@ SmacPlannerLattice::dynamicParametersCallback(std::vector<rclcpp::Parameter> par
       _a_star->initialize(
         _allow_unknown,
         _max_iterations,
-        std::numeric_limits<int>::max(),
+        _max_on_approach_iterations,
         _max_planning_time,
         lookup_table_dim,
         _metadata.number_of_headings);

nav2_smac_planner/test/test_smac_hybrid.cpp

@@ -110,12 +110,14 @@ TEST(SmacTest, test_smac_se2_reconfigure)
       rclcpp::Parameter("test.change_penalty", 1.0),
       rclcpp::Parameter("test.non_straight_penalty", 2.0),
       rclcpp::Parameter("test.cost_penalty", 2.0),
+      rclcpp::Parameter("test.tolerance", 0.2),
       rclcpp::Parameter("test.retrospective_penalty", 0.2),
       rclcpp::Parameter("test.analytic_expansion_ratio", 4.0),
       rclcpp::Parameter("test.max_planning_time", 10.0),
       rclcpp::Parameter("test.lookup_table_size", 30.0),
       rclcpp::Parameter("test.smooth_path", false),
       rclcpp::Parameter("test.analytic_expansion_max_length", 42.0),
+      rclcpp::Parameter("test.max_on_approach_iterations", 42),
       rclcpp::Parameter("test.motion_model_for_search", std::string("REEDS_SHEPP"))});
 
   rclcpp::spin_until_future_complete(
@@ -134,11 +136,13 @@ TEST(SmacTest, test_smac_se2_reconfigure)
   EXPECT_EQ(nodeSE2->get_parameter("test.non_straight_penalty").as_double(), 2.0);
   EXPECT_EQ(nodeSE2->get_parameter("test.cost_penalty").as_double(), 2.0);
   EXPECT_EQ(nodeSE2->get_parameter("test.retrospective_penalty").as_double(), 0.2);
+  EXPECT_EQ(nodeSE2->get_parameter("test.tolerance").as_double(), 0.2);
   EXPECT_EQ(nodeSE2->get_parameter("test.analytic_expansion_ratio").as_double(), 4.0);
   EXPECT_EQ(nodeSE2->get_parameter("test.smooth_path").as_bool(), false);
   EXPECT_EQ(nodeSE2->get_parameter("test.max_planning_time").as_double(), 10.0);
   EXPECT_EQ(nodeSE2->get_parameter("test.lookup_table_size").as_double(), 30.0);
   EXPECT_EQ(nodeSE2->get_parameter("test.analytic_expansion_max_length").as_double(), 42.0);
+  EXPECT_EQ(nodeSE2->get_parameter("test.max_on_approach_iterations").as_int(), 42);
   EXPECT_EQ(
     nodeSE2->get_parameter("test.motion_model_for_search").as_string(),
     std::string("REEDS_SHEPP"));

nav2_smac_planner/test/test_smac_lattice.cpp

@@ -124,7 +124,9 @@ TEST(SmacTest, test_smac_lattice_reconfigure)
       rclcpp::Parameter("test.lookup_table_size", 30.0),
       rclcpp::Parameter("test.smooth_path", false),
       rclcpp::Parameter("test.analytic_expansion_max_length", 42.0),
+      rclcpp::Parameter("test.tolerance", 42.0),
       rclcpp::Parameter("test.rotation_penalty", 42.0),
+      rclcpp::Parameter("test.max_on_approach_iterations", 42),
       rclcpp::Parameter("test.allow_reverse_expansion", true)});
 
   try {