Add terminal_checking_interval parameter

Signed-off-by: Kemal Bektas <[email protected]>

nav2_navfn_planner/include/nav2_navfn_planner/navfn.hpp

@@ -183,7 +183,7 @@ class NavFn
   float priInc;  /**< priority threshold increment */
 
   /**< number of cycles between checks for cancellation */
-  int cancel_check_interval;
+  static constexpr int terminal_checking_interval = 500;
 
   /** goal and start positions */
   /**

nav2_navfn_planner/src/navfn.cpp

@@ -582,7 +582,7 @@ NavFn::propNavFnDijkstra(int cycles, std::function<bool()> cancelChecker, bool a
   int startCell = start[1] * nx + start[0];
 
   for (; cycle < cycles; cycle++) {  // go for this many cycles, unless interrupted
-    if (cycle % cancel_check_interval == 0 && cancelChecker()) {
+    if (cycle % terminal_checking_interval == 0 && cancelChecker()) {
       throw nav2_core::PlannerCancelled("Planner was cancelled");
     }
 
@@ -672,7 +672,7 @@ NavFn::propNavFnAstar(int cycles, std::function<bool()> cancelChecker)
 
   // do main cycle
   for (; cycle < cycles; cycle++) {  // go for this many cycles, unless interrupted
-    if (cycle % cancel_check_interval == 0 && cancelChecker()) {
+    if (cycle % terminal_checking_interval == 0 && cancelChecker()) {
       throw nav2_core::PlannerCancelled("Planner was cancelled");
     }
 

nav2_smac_planner/README.md

@@ -109,6 +109,7 @@ planner_server:
       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
+      terminal_checking_interval: 500     # number of iterations between checking if the goal has been cancelled
       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/a_star.hpp

@@ -88,13 +88,15 @@ class AStarAlgorithm
    * @param max_on_approach_iterations Maximum number of iterations before returning a valid
    * path once within thresholds to refine path
    * comes at more compute time but smoother paths.
+   * @param terminal_checking_interval Number of iterations to check if the task has been canceled
    * @param max_planning_time Maximum time (in seconds) to wait for a plan, createPath returns
    * false after this timeout
    */
   void initialize(
     const bool & allow_unknown,
     int & max_iterations,
     const int & max_on_approach_iterations,
+    const int & terminal_checking_interval,
     const double & max_planning_time,
     const float & lookup_table_size,
     const unsigned int & dim_3_size);
@@ -253,12 +255,12 @@ class AStarAlgorithm
   void clearStart();
 
   int _timing_interval = 5000;
-  int _cancel_check_interval = 500;
 
 
   bool _traverse_unknown;
   int _max_iterations;
   int _max_on_approach_iterations;
+  int _terminal_checking_interval;
   double _max_planning_time;
   float _tolerance;
   unsigned int _x_size;

nav2_smac_planner/include/nav2_smac_planner/smac_planner_2d.hpp

@@ -114,6 +114,7 @@ class SmacPlanner2D : public nav2_core::GlobalPlanner
   bool _allow_unknown;
   int _max_iterations;
   int _max_on_approach_iterations;
+  int _terminal_checking_interval;
   bool _use_final_approach_orientation;
   SearchInfo _search_info;
   std::string _motion_model_for_search;

nav2_smac_planner/include/nav2_smac_planner/smac_planner_hybrid.hpp

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

nav2_smac_planner/include/nav2_smac_planner/smac_planner_lattice.hpp

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

nav2_smac_planner/src/a_star.cpp

@@ -37,6 +37,7 @@ AStarAlgorithm<NodeT>::AStarAlgorithm(
   const SearchInfo & search_info)
 : _traverse_unknown(true),
   _max_iterations(0),
+  _terminal_checking_interval(500),
   _max_planning_time(0),
   _x_size(0),
   _y_size(0),
@@ -59,13 +60,15 @@ void AStarAlgorithm<NodeT>::initialize(
   const bool & allow_unknown,
   int & max_iterations,
   const int & max_on_approach_iterations,
+  const int & terminal_checking_interval,
   const double & max_planning_time,
   const float & lookup_table_size,
   const unsigned int & dim_3_size)
 {
   _traverse_unknown = allow_unknown;
   _max_iterations = max_iterations;
   _max_on_approach_iterations = max_on_approach_iterations;
+  _terminal_checking_interval = terminal_checking_interval;
   _max_planning_time = max_planning_time;
   NodeT::precomputeDistanceHeuristic(lookup_table_size, _motion_model, dim_3_size, _search_info);
   _dim3_size = dim_3_size;
@@ -78,13 +81,15 @@ void AStarAlgorithm<Node2D>::initialize(
   const bool & allow_unknown,
   int & max_iterations,
   const int & max_on_approach_iterations,
+  const int & terminal_checking_interval,
   const double & max_planning_time,
   const float & /*lookup_table_size*/,
   const unsigned int & dim_3_size)
 {
   _traverse_unknown = allow_unknown;
   _max_iterations = max_iterations;
   _max_on_approach_iterations = max_on_approach_iterations;
+  _terminal_checking_interval = terminal_checking_interval;
   _max_planning_time = max_planning_time;
 
   if (dim_3_size != 1) {
@@ -296,7 +301,7 @@ bool AStarAlgorithm<NodeT>::createPath(
     }
 
     // Check for cancel every Nth iteration
-    if (iterations % _cancel_check_interval == 0 && cancel_checker()) {
+    if (iterations % _terminal_checking_interval == 0 && cancel_checker()) {
       throw nav2_core::PlannerCancelled("Planner was cancelled");
     }
 

nav2_smac_planner/src/smac_planner_2d.cpp

@@ -83,6 +83,9 @@ void SmacPlanner2D::configure(
   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 + ".terminal_checking_interval", rclcpp::ParameterValue(500));
+  node->get_parameter(name + ".terminal_checking_interval", _terminal_checking_interval);
   nav2_util::declare_parameter_if_not_declared(
     node, name + ".use_final_approach_orientation", rclcpp::ParameterValue(false));
   node->get_parameter(name + ".use_final_approach_orientation", _use_final_approach_orientation);
@@ -120,6 +123,7 @@ void SmacPlanner2D::configure(
     _allow_unknown,
     _max_iterations,
     _max_on_approach_iterations,
+    _terminal_checking_interval,
     _max_planning_time,
     0.0 /*unused for 2D*/,
     1.0 /*unused for 2D*/);
@@ -378,6 +382,9 @@ SmacPlanner2D::dynamicParametersCallback(std::vector<rclcpp::Parameter> paramete
             "disabling tolerance and on approach iterations.");
           _max_on_approach_iterations = std::numeric_limits<int>::max();
         }
+      } else if (name == _name + ".terminal_checking_interval") {
+        reinit_a_star = true;
+        _terminal_checking_interval = parameter.as_int();
       }
     }
   }
@@ -391,6 +398,7 @@ SmacPlanner2D::dynamicParametersCallback(std::vector<rclcpp::Parameter> paramete
         _allow_unknown,
         _max_iterations,
         _max_on_approach_iterations,
+        _terminal_checking_interval,
         _max_planning_time,
         0.0 /*unused for 2D*/,
         1.0 /*unused for 2D*/);

nav2_smac_planner/src/smac_planner_hybrid.cpp

@@ -94,6 +94,9 @@ void SmacPlannerHybrid::configure(
   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 + ".terminal_checking_interval", rclcpp::ParameterValue(500));
+  node->get_parameter(name + ".terminal_checking_interval", _terminal_checking_interval);
   nav2_util::declare_parameter_if_not_declared(
     node, name + ".smooth_path", rclcpp::ParameterValue(true));
   node->get_parameter(name + ".smooth_path", smooth_path);
@@ -228,6 +231,7 @@ void SmacPlannerHybrid::configure(
     _allow_unknown,
     _max_iterations,
     _max_on_approach_iterations,
+    _terminal_checking_interval,
     _max_planning_time,
     _lookup_table_dim,
     _angle_quantizations);
@@ -599,6 +603,9 @@ SmacPlannerHybrid::dynamicParametersCallback(std::vector<rclcpp::Parameter> para
             "disabling tolerance and on approach iterations.");
           _max_on_approach_iterations = std::numeric_limits<int>::max();
         }
+      } else if (name == _name + ".terminal_checking_interval") {
+        reinit_a_star = true;
+        _terminal_checking_interval = parameter.as_int();
       } else if (name == _name + ".angle_quantization_bins") {
         reinit_collision_checker = true;
         reinit_a_star = true;
@@ -654,6 +661,7 @@ SmacPlannerHybrid::dynamicParametersCallback(std::vector<rclcpp::Parameter> para
         _allow_unknown,
         _max_iterations,
         _max_on_approach_iterations,
+        _terminal_checking_interval,
         _max_planning_time,
         _lookup_table_dim,
         _angle_quantizations);

nav2_smac_planner/src/smac_planner_lattice.cpp

@@ -77,6 +77,9 @@ void SmacPlannerLattice::configure(
   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 + ".terminal_checking_interval", rclcpp::ParameterValue(500));
+  node->get_parameter(name + ".terminal_checking_interval", _terminal_checking_interval);
   nav2_util::declare_parameter_if_not_declared(
     node, name + ".smooth_path", rclcpp::ParameterValue(true));
   node->get_parameter(name + ".smooth_path", smooth_path);
@@ -193,6 +196,7 @@ void SmacPlannerLattice::configure(
     _allow_unknown,
     _max_iterations,
     _max_on_approach_iterations,
+    _terminal_checking_interval,
     _max_planning_time,
     lookup_table_dim,
     _metadata.number_of_headings);
@@ -505,15 +509,18 @@ SmacPlannerLattice::dynamicParametersCallback(std::vector<rclcpp::Parameter> par
             "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 + ".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 + ".terminal_checking_interval") {
+        reinit_a_star = true;
+        _terminal_checking_interval = parameter.as_int();
       }
     } else if (type == ParameterType::PARAMETER_STRING) {
       if (name == _name + ".lattice_filepath") {
@@ -566,6 +573,7 @@ SmacPlannerLattice::dynamicParametersCallback(std::vector<rclcpp::Parameter> par
         _allow_unknown,
         _max_iterations,
         _max_on_approach_iterations,
+        _terminal_checking_interval,
         _max_planning_time,
         lookup_table_dim,
         _metadata.number_of_headings);

nav2_smac_planner/test/test_a_star.cpp

@@ -47,10 +47,13 @@ TEST(AStarTest, test_a_star_2d)
   float tolerance = 0.0;
   float some_tolerance = 20.0;
   int it_on_approach = 10;
+  int terminal_checking_interval = 500;
   double max_planning_time = 120.0;
   int num_it = 0;
 
-  a_star.initialize(false, max_iterations, it_on_approach, max_planning_time, 0.0, 1);
+  a_star.initialize(
+    false, max_iterations, it_on_approach, terminal_checking_interval,
+    max_planning_time, 0.0, 1);
 
   nav2_costmap_2d::Costmap2D * costmapA =
     new nav2_costmap_2d::Costmap2D(100, 100, 0.1, 0.0, 0.0, 0);
@@ -96,7 +99,9 @@ TEST(AStarTest, test_a_star_2d)
   // failure cases with invalid inputs
   nav2_smac_planner::AStarAlgorithm<nav2_smac_planner::Node2D> a_star_2(
     nav2_smac_planner::MotionModel::TWOD, info);
-  a_star_2.initialize(false, max_iterations, it_on_approach, max_planning_time, 0, 1);
+  a_star_2.initialize(
+    false, max_iterations, it_on_approach, terminal_checking_interval,
+    max_planning_time, 0, 1);
   num_it = 0;
   EXPECT_THROW(
     a_star_2.createPath(
@@ -154,10 +159,13 @@ TEST(AStarTest, test_a_star_se2)
   int max_iterations = 10000;
   float tolerance = 10.0;
   int it_on_approach = 10;
+  int terminal_checking_interval = 500;
   double max_planning_time = 120.0;
   int num_it = 0;
 
-  a_star.initialize(false, max_iterations, it_on_approach, max_planning_time, 401, size_theta);
+  a_star.initialize(
+    false, max_iterations, it_on_approach, terminal_checking_interval,
+    max_planning_time, 401, size_theta);
 
   nav2_costmap_2d::Costmap2D * costmapA =
     new nav2_costmap_2d::Costmap2D(100, 100, 0.1, 0.0, 0.0, 0);
@@ -299,10 +307,13 @@ TEST(AStarTest, test_se2_single_pose_path)
   int max_iterations = 100;
   float tolerance = 10.0;
   int it_on_approach = 10;
+  int terminal_checking_interval = 500;
   double max_planning_time = 120.0;
   int num_it = 0;
 
-  a_star.initialize(false, max_iterations, it_on_approach, max_planning_time, 401, size_theta);
+  a_star.initialize(
+    false, max_iterations, it_on_approach, terminal_checking_interval,
+    max_planning_time, 401, size_theta);
 
   nav2_costmap_2d::Costmap2D * costmapA =
     new nav2_costmap_2d::Costmap2D(100, 100, 0.1, 0.0, 0.0, 0);

nav2_smac_planner/test/test_smoother.cpp

@@ -89,11 +89,14 @@ TEST(SmootherTest, test_full_smoother)
   int max_iterations = 10000;
   float tolerance = 10.0;
   int it_on_approach = 10;
+  int terminal_checking_interval = 500;
   double max_planning_time = 120.0;
   int num_it = 0;
 
   a_star.initialize(
-    false, max_iterations, std::numeric_limits<int>::max(), max_planning_time, 401, size_theta);
+    false, max_iterations,
+    std::numeric_limits<int>::max(), terminal_checking_interval, max_planning_time, 401,
+    size_theta);
 
   // Convert raw costmap into a costmap ros object
   auto costmap_ros = std::make_shared<nav2_costmap_2d::Costmap2DROS>();