diff --git a/nav2_navfn_planner/include/nav2_navfn_planner/navfn.hpp b/nav2_navfn_planner/include/nav2_navfn_planner/navfn.hpp index 6860f41de6b..5993767e834 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 */ - int cancel_check_interval; + static constexpr int terminal_checking_interval = 500; /** goal and start positions */ /** diff --git a/nav2_navfn_planner/src/navfn.cpp b/nav2_navfn_planner/src/navfn.cpp index cb1dd4054c2..2cce713b0f2 100644 --- a/nav2_navfn_planner/src/navfn.cpp +++ b/nav2_navfn_planner/src/navfn.cpp @@ -582,7 +582,7 @@ NavFn::propNavFnDijkstra(int cycles, std::function 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 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"); } diff --git a/nav2_smac_planner/README.md b/nav2_smac_planner/README.md index 3a992088b69..5dcdfff2e98 100644 --- a/nav2_smac_planner/README.md +++ b/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*. 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 485024d84f9..550e189bbe3 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp @@ -88,6 +88,7 @@ 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 */ @@ -95,6 +96,7 @@ class AStarAlgorithm 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; diff --git a/nav2_smac_planner/include/nav2_smac_planner/smac_planner_2d.hpp b/nav2_smac_planner/include/nav2_smac_planner/smac_planner_2d.hpp index 81a9a0e2e54..059871d8677 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/smac_planner_2d.hpp +++ b/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; diff --git a/nav2_smac_planner/include/nav2_smac_planner/smac_planner_hybrid.hpp b/nav2_smac_planner/include/nav2_smac_planner/smac_planner_hybrid.hpp index a93dbbba9de..157bbcea839 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/smac_planner_hybrid.hpp +++ b/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; diff --git a/nav2_smac_planner/include/nav2_smac_planner/smac_planner_lattice.hpp b/nav2_smac_planner/include/nav2_smac_planner/smac_planner_lattice.hpp index 11a34c0ece9..57e225f2876 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/smac_planner_lattice.hpp +++ b/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::SharedPtr _raw_plan_publisher; double _max_planning_time; diff --git a/nav2_smac_planner/src/a_star.cpp b/nav2_smac_planner/src/a_star.cpp index fb9c998e65e..968b5aec6e0 100644 --- a/nav2_smac_planner/src/a_star.cpp +++ b/nav2_smac_planner/src/a_star.cpp @@ -37,6 +37,7 @@ AStarAlgorithm::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,6 +60,7 @@ void AStarAlgorithm::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) @@ -66,6 +68,7 @@ void AStarAlgorithm::initialize( _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,6 +81,7 @@ void AStarAlgorithm::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) @@ -85,6 +89,7 @@ void AStarAlgorithm::initialize( _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::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"); } diff --git a/nav2_smac_planner/src/smac_planner_2d.cpp b/nav2_smac_planner/src/smac_planner_2d.cpp index 221a2a81f45..9f1387965e5 100644 --- a/nav2_smac_planner/src/smac_planner_2d.cpp +++ b/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 paramete "disabling tolerance and on approach iterations."); _max_on_approach_iterations = std::numeric_limits::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 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*/); diff --git a/nav2_smac_planner/src/smac_planner_hybrid.cpp b/nav2_smac_planner/src/smac_planner_hybrid.cpp index fe705f773e6..717ceab2124 100644 --- a/nav2_smac_planner/src/smac_planner_hybrid.cpp +++ b/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 para "disabling tolerance and on approach iterations."); _max_on_approach_iterations = std::numeric_limits::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 para _allow_unknown, _max_iterations, _max_on_approach_iterations, + _terminal_checking_interval, _max_planning_time, _lookup_table_dim, _angle_quantizations); diff --git a/nav2_smac_planner/src/smac_planner_lattice.cpp b/nav2_smac_planner/src/smac_planner_lattice.cpp index ede2a211ea0..c7a60b2255b 100644 --- a/nav2_smac_planner/src/smac_planner_lattice.cpp +++ b/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 par "disabling maximum iterations."); _max_iterations = std::numeric_limits::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::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::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 par _allow_unknown, _max_iterations, _max_on_approach_iterations, + _terminal_checking_interval, _max_planning_time, lookup_table_dim, _metadata.number_of_headings); diff --git a/nav2_smac_planner/test/test_a_star.cpp b/nav2_smac_planner/test/test_a_star.cpp index 181de13004a..b5ff6db0dc5 100644 --- a/nav2_smac_planner/test/test_a_star.cpp +++ b/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 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); diff --git a/nav2_smac_planner/test/test_smoother.cpp b/nav2_smac_planner/test/test_smoother.cpp index e7da3a88740..b3cb5a55627 100644 --- a/nav2_smac_planner/test/test_smoother.cpp +++ b/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::max(), max_planning_time, 401, size_theta); + false, max_iterations, + std::numeric_limits::max(), terminal_checking_interval, max_planning_time, 401, + size_theta); // Convert raw costmap into a costmap ros object auto costmap_ros = std::make_shared();