Skip to content

Commit

Permalink
Handle timeout and cancel check on the same branch and default to 500…
Browse files Browse the repository at this point in the history
…0 iterations

Signed-off-by: Kemal Bektas <kemal.bektas@node-robotics.com>
  • Loading branch information
Kemal Bektas committed Feb 28, 2024
1 parent 8f16980 commit 2aaa7db
Show file tree
Hide file tree
Showing 11 changed files with 20 additions and 23 deletions.
2 changes: 1 addition & 1 deletion nav2_navfn_planner/include/nav2_navfn_planner/navfn.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -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 */
/**
Expand Down
2 changes: 1 addition & 1 deletion nav2_smac_planner/README.md
Original file line number Diff line number Diff line change
Expand Up @@ -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*.
Expand Down
5 changes: 2 additions & 3 deletions nav2_smac_planner/include/nav2_smac_planner/a_star.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -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
*/
Expand Down Expand Up @@ -254,8 +255,6 @@ class AStarAlgorithm
*/
void clearStart();

int _timing_interval = 5000;


bool _traverse_unknown;
int _max_iterations;
Expand Down
14 changes: 6 additions & 8 deletions nav2_smac_planner/src/a_star.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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),
Expand Down Expand Up @@ -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");

Check warning on line 297 in nav2_smac_planner/src/a_star.cpp

View check run for this annotation

Codecov / codecov/patch

nav2_smac_planner/src/a_star.cpp#L297

Added line #L297 was not covered by tests
}
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();

Expand Down
2 changes: 1 addition & 1 deletion nav2_smac_planner/src/smac_planner_2d.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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));
Expand Down
2 changes: 1 addition & 1 deletion nav2_smac_planner/src/smac_planner_hybrid.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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));
Expand Down
2 changes: 1 addition & 1 deletion nav2_smac_planner/src/smac_planner_lattice.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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));
Expand Down
8 changes: 4 additions & 4 deletions nav2_smac_planner/test/test_a_star.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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;

Expand Down Expand Up @@ -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;

Expand Down Expand Up @@ -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;

Expand Down Expand Up @@ -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;

Expand Down
2 changes: 1 addition & 1 deletion nav2_smac_planner/test/test_smoother.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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;

Expand Down
2 changes: 1 addition & 1 deletion nav2_theta_star_planner/src/theta_star.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down
2 changes: 1 addition & 1 deletion nav2_theta_star_planner/src/theta_star_planner.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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(
Expand Down

0 comments on commit 2aaa7db

Please sign in to comment.