From 2aaa7db84fe53623db4b09a8767ff5607872ee28 Mon Sep 17 00:00:00 2001 From: Kemal Bektas Date: Wed, 28 Feb 2024 10:25:58 +0100 Subject: [PATCH] Handle timeout and cancel check on the same branch and default to 5000 iterations Signed-off-by: Kemal Bektas --- .../include/nav2_navfn_planner/navfn.hpp | 2 +- nav2_smac_planner/README.md | 2 +- .../include/nav2_smac_planner/a_star.hpp | 5 ++--- nav2_smac_planner/src/a_star.cpp | 14 ++++++-------- nav2_smac_planner/src/smac_planner_2d.cpp | 2 +- nav2_smac_planner/src/smac_planner_hybrid.cpp | 2 +- nav2_smac_planner/src/smac_planner_lattice.cpp | 2 +- nav2_smac_planner/test/test_a_star.cpp | 8 ++++---- nav2_smac_planner/test/test_smoother.cpp | 2 +- nav2_theta_star_planner/src/theta_star.cpp | 2 +- nav2_theta_star_planner/src/theta_star_planner.cpp | 2 +- 11 files changed, 20 insertions(+), 23 deletions(-) diff --git a/nav2_navfn_planner/include/nav2_navfn_planner/navfn.hpp b/nav2_navfn_planner/include/nav2_navfn_planner/navfn.hpp index 5993767e834..966a8458521 100644 --- 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 index 5dcdfff2e98..6e92209068d 100644 --- 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 index 550e189bbe3..668aff5e4d8 100644 --- 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 index 968b5aec6e0..1f0d7207631 100644 --- a/nav2_smac_planner/src/a_star.cpp +++ b/nav2_smac_planner/src/a_star.cpp @@ -37,7 +37,7 @@ AStarAlgorithm::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,8 +291,11 @@ bool AStarAlgorithm::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 planning_duration = std::chrono::duration_cast>(steady_clock::now() - start_time); if (static_cast(planning_duration.count()) >= _max_planning_time) { @@ -300,11 +303,6 @@ bool AStarAlgorithm::createPath( } } - // 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 index 9f1387965e5..38fd12b9e17 100644 --- 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 index 717ceab2124..b0bdf042dc2 100644 --- 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 index c7a60b2255b..cd98cf64408 100644 --- 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 index 821fa4c9e51..bdcf37c7215 100644 --- 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 index b3cb5a55627..616c674595a 100644 --- 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 index 018f50b2b4c..ba3629bcbfa 100644 --- 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 index 461601816b9..61064efbf45 100644 --- 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(