Handle timeout and cancel check on the same branch and default to 500…

…0 iterations Signed-off-by: Kemal Bektas <[email protected]>
ros-navigation · Feb 28, 2024 · 2aaa7db · 2aaa7db
1 parent 8f16980
commit 2aaa7db
Show file tree

Hide file tree

Showing 11 changed files with 20 additions and 23 deletions.
diff --git a/nav2_navfn_planner/include/nav2_navfn_planner/navfn.hpp b/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 */
-  static constexpr int terminal_checking_interval = 500;
+  static constexpr int terminal_checking_interval = 5000;
 
   /** goal and start positions */
   /**

diff --git a/nav2_smac_planner/README.md b/nav2_smac_planner/README.md
@@ -109,7 +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
+      terminal_checking_interval: 5000     # number of iterations between checking if the goal has been cancelled or planner timed out
       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*.

diff --git a/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp b/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp
@@ -88,7 +88,8 @@ 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 terminal_checking_interval Number of iterations to check if the task has been canceled or
+   * or planning time exceeded
    * @param max_planning_time Maximum time (in seconds) to wait for a plan, createPath returns
    * false after this timeout
    */
@@ -254,8 +255,6 @@ class AStarAlgorithm
    */
   void clearStart();
 
-  int _timing_interval = 5000;
-
 
   bool _traverse_unknown;
   int _max_iterations;

diff --git a/nav2_smac_planner/src/a_star.cpp b/nav2_smac_planner/src/a_star.cpp
@@ -37,7 +37,7 @@ AStarAlgorithm<NodeT>::AStarAlgorithm(
   const SearchInfo & search_info)
 : _traverse_unknown(true),
   _max_iterations(0),
-  _terminal_checking_interval(500),
+  _terminal_checking_interval(5000),
   _max_planning_time(0),
   _x_size(0),
   _y_size(0),
@@ -291,20 +291,18 @@ bool AStarAlgorithm<NodeT>::createPath(
     };
 
   while (iterations < getMaxIterations() && !_queue.empty()) {
-    // Check for planning timeout only on every Nth iteration
-    if (iterations % _timing_interval == 0) {
+    // Check for planning timeout and cancel only on every Nth iteration
+    if (iterations % _terminal_checking_interval == 0) {
+      if (cancel_checker()) {
+        throw nav2_core::PlannerCancelled("Planner was cancelled");
+      }
       std::chrono::duration<double> planning_duration =
         std::chrono::duration_cast<std::chrono::duration<double>>(steady_clock::now() - start_time);
       if (static_cast<double>(planning_duration.count()) >= _max_planning_time) {
         return false;
       }
     }
 
-    // Check for cancel every Nth iteration
-    if (iterations % _terminal_checking_interval == 0 && cancel_checker()) {
-      throw nav2_core::PlannerCancelled("Planner was cancelled");
-    }
-
     // 1) Pick Nbest from O s.t. min(f(Nbest)), remove from queue
     current_node = getNextNode();
 

diff --git a/nav2_smac_planner/src/smac_planner_2d.cpp b/nav2_smac_planner/src/smac_planner_2d.cpp
@@ -84,7 +84,7 @@ void SmacPlanner2D::configure(
     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, name + ".terminal_checking_interval", rclcpp::ParameterValue(5000));
   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));

diff --git a/nav2_smac_planner/src/smac_planner_hybrid.cpp b/nav2_smac_planner/src/smac_planner_hybrid.cpp
@@ -95,7 +95,7 @@ void SmacPlannerHybrid::configure(
     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, name + ".terminal_checking_interval", rclcpp::ParameterValue(5000));
   node->get_parameter(name + ".terminal_checking_interval", _terminal_checking_interval);
   nav2_util::declare_parameter_if_not_declared(
     node, name + ".smooth_path", rclcpp::ParameterValue(true));

diff --git a/nav2_smac_planner/src/smac_planner_lattice.cpp b/nav2_smac_planner/src/smac_planner_lattice.cpp
@@ -78,7 +78,7 @@ void SmacPlannerLattice::configure(
     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, name + ".terminal_checking_interval", rclcpp::ParameterValue(5000));
   node->get_parameter(name + ".terminal_checking_interval", _terminal_checking_interval);
   nav2_util::declare_parameter_if_not_declared(
     node, name + ".smooth_path", rclcpp::ParameterValue(true));

diff --git a/nav2_smac_planner/test/test_a_star.cpp b/nav2_smac_planner/test/test_a_star.cpp
@@ -47,7 +47,7 @@ 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;
+  int terminal_checking_interval = 5000;
   double max_planning_time = 120.0;
   int num_it = 0;
 
@@ -159,7 +159,7 @@ TEST(AStarTest, test_a_star_se2)
   int max_iterations = 10000;
   float tolerance = 10.0;
   int it_on_approach = 10;
-  int terminal_checking_interval = 500;
+  int terminal_checking_interval = 5000;
   double max_planning_time = 120.0;
   int num_it = 0;
 
@@ -239,7 +239,7 @@ TEST(AStarTest, test_a_star_lattice)
   int max_iterations = 10000;
   float tolerance = 10.0;
   int it_on_approach = 10;
-  int terminal_checking_interval = 500;
+  int terminal_checking_interval = 5000;
   double max_planning_time = 120.0;
   int num_it = 0;
 
@@ -310,7 +310,7 @@ 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;
+  int terminal_checking_interval = 5000;
   double max_planning_time = 120.0;
   int num_it = 0;
 

diff --git a/nav2_smac_planner/test/test_smoother.cpp b/nav2_smac_planner/test/test_smoother.cpp
@@ -89,7 +89,7 @@ TEST(SmootherTest, test_full_smoother)
   int max_iterations = 10000;
   float tolerance = 10.0;
   int it_on_approach = 10;
-  int terminal_checking_interval = 500;
+  int terminal_checking_interval = 5000;
   double max_planning_time = 120.0;
   int num_it = 0;
 

diff --git a/nav2_theta_star_planner/src/theta_star.cpp b/nav2_theta_star_planner/src/theta_star.cpp
@@ -27,7 +27,7 @@ ThetaStar::ThetaStar()
   allow_unknown_(true),
   size_x_(0),
   size_y_(0),
-  terminal_checking_interval_(500),
+  terminal_checking_interval_(5000),
   index_generated_(0)
 {
   exp_node = new tree_node;

diff --git a/nav2_theta_star_planner/src/theta_star_planner.cpp b/nav2_theta_star_planner/src/theta_star_planner.cpp
@@ -60,7 +60,7 @@ void ThetaStarPlanner::configure(
   planner_->w_heuristic_cost_ = planner_->w_euc_cost_ < 1.0 ? planner_->w_euc_cost_ : 1.0;
 
   nav2_util::declare_parameter_if_not_declared(
-    node, name_ + ".terminal_checking_interval", rclcpp::ParameterValue(500));
+    node, name_ + ".terminal_checking_interval", rclcpp::ParameterValue(5000));
   node->get_parameter(name_ + ".terminal_checking_interval", planner_->terminal_checking_interval_);
 
   nav2_util::declare_parameter_if_not_declared(