Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Stop planner if the goal is cancelled #4148

Merged
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Jump to
Jump to file
Failed to load files.
Diff view
Diff view
4 changes: 3 additions & 1 deletion nav2_core/include/nav2_core/global_planner.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -71,11 +71,13 @@ class GlobalPlanner
* @brief Method create the plan from a starting and ending goal.
* @param start The starting pose of the robot
* @param goal The goal pose of the robot
* @param cancel_checker Function to check if the action has been canceled
* @return The sequence of poses to get from start to goal, if any
*/
virtual nav_msgs::msg::Path createPlan(
const geometry_msgs::msg::PoseStamped & start,
const geometry_msgs::msg::PoseStamped & goal) = 0;
const geometry_msgs::msg::PoseStamped & goal,
std::function<bool()> cancel_checker) = 0;
};

} // namespace nav2_core
Expand Down
7 changes: 7 additions & 0 deletions nav2_core/include/nav2_core/planner_exceptions.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -113,6 +113,13 @@ class NoViapointsGiven : public PlannerException
: PlannerException(description) {}
};

class PlannerCancelled : public PlannerException
{
public:
explicit PlannerCancelled(const std::string & description)
: PlannerException(description) {}
};

} // namespace nav2_core

#endif // NAV2_CORE__PLANNER_EXCEPTIONS_HPP_
18 changes: 13 additions & 5 deletions nav2_navfn_planner/include/nav2_navfn_planner/navfn.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -43,6 +43,7 @@
#include <stdint.h>
#include <string.h>
#include <stdio.h>
#include <functional>

namespace nav2_navfn_planner
{
Expand Down Expand Up @@ -131,14 +132,16 @@ class NavFn

/**
* @brief Calculates a plan using the A* heuristic, returns true if one is found
* @param cancelChecker Function to check if the task has been canceled
* @return True if a plan is found, false otherwise
*/
bool calcNavFnAstar();
bool calcNavFnAstar(std::function<bool()> cancelChecker);

/**
* @brief Caclulates the full navigation function using Dijkstra
* @brief Calculates the full navigation function using Dijkstra
* @param cancelChecker Function to check if the task has been canceled
*/
bool calcNavFnDijkstra(bool atStart = false);
bool calcNavFnDijkstra(std::function<bool()> cancelChecker, bool atStart = false);

/**
* @brief Accessor for the x-coordinates of a path
Expand Down Expand Up @@ -179,6 +182,9 @@ class NavFn
float curT; /**< current threshold */
float priInc; /**< priority threshold increment */

/**< number of cycles between checks for cancellation */
static constexpr int terminal_checking_interval = 5000;

/** goal and start positions */
/**
* @brief Sets the goal position for the planner.
Expand Down Expand Up @@ -229,18 +235,20 @@ class NavFn
* @brief Run propagation for <cycles> iterations, or until start is reached using
* breadth-first Dijkstra method
* @param cycles The maximum number of iterations to run for
* @param cancelChecker Function to check if the task has been canceled
* @param atStart Whether or not to stop when the start point is reached
* @return true if the start point is reached
*/
bool propNavFnDijkstra(int cycles, bool atStart = false);
bool propNavFnDijkstra(int cycles, std::function<bool()> cancelChecker, bool atStart = false);

/**
* @brief Run propagation for <cycles> iterations, or until start is reached using
* the best-first A* method with Euclidean distance heuristic
* @param cycles The maximum number of iterations to run for
* @param cancelChecker Function to check if the task has been canceled
* @return true if the start point is reached
*/
bool propNavFnAstar(int cycles); /**< returns true if start point found */
bool propNavFnAstar(int cycles, std::function<bool()> cancelChecker);

/** gradient and paths */
float * gradx, * grady; /**< gradient arrays, size of potential array */
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -81,24 +81,28 @@ class NavfnPlanner : public nav2_core::GlobalPlanner
* @brief Creating a plan from start and goal poses
* @param start Start pose
* @param goal Goal pose
* @param cancel_checker Function to check if the task has been canceled
* @return nav_msgs::Path of the generated path
*/
nav_msgs::msg::Path createPlan(
const geometry_msgs::msg::PoseStamped & start,
const geometry_msgs::msg::PoseStamped & goal) override;
const geometry_msgs::msg::PoseStamped & goal,
std::function<bool()> cancel_checker) override;

protected:
/**
* @brief Compute a plan given start and goal poses, provided in global world frame.
* @param start Start pose
* @param goal Goal pose
* @param tolerance Relaxation constraint in x and y
* @param cancel_checker Function to check if the task has been canceled
* @param plan Path to be computed
* @return true if can find the path
*/
bool makePlan(
const geometry_msgs::msg::Pose & start,
const geometry_msgs::msg::Pose & goal, double tolerance,
std::function<bool()> cancel_checker,
nav_msgs::msg::Path & plan);

/**
Expand Down
21 changes: 15 additions & 6 deletions nav2_navfn_planner/src/navfn.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -44,6 +44,7 @@
#include "nav2_navfn_planner/navfn.hpp"

#include <algorithm>
#include "nav2_core/planner_exceptions.hpp"
#include "rclcpp/rclcpp.hpp"

namespace nav2_navfn_planner
Expand Down Expand Up @@ -293,12 +294,12 @@ NavFn::setCostmap(const COSTTYPE * cmap, bool isROS, bool allow_unknown)
}

bool
NavFn::calcNavFnDijkstra(bool atStart)
NavFn::calcNavFnDijkstra(std::function<bool()> cancelChecker, bool atStart)
{
setupNavFn(true);

// calculate the nav fn and path
return propNavFnDijkstra(std::max(nx * ny / 20, nx + ny), atStart);
return propNavFnDijkstra(std::max(nx * ny / 20, nx + ny), cancelChecker, atStart);
}


Expand All @@ -307,12 +308,12 @@ NavFn::calcNavFnDijkstra(bool atStart)
//

bool
NavFn::calcNavFnAstar()
NavFn::calcNavFnAstar(std::function<bool()> cancelChecker)
{
setupNavFn(true);

// calculate the nav fn and path
return propNavFnAstar(std::max(nx * ny / 20, nx + ny));
return propNavFnAstar(std::max(nx * ny / 20, nx + ny), cancelChecker);
}

//
Expand Down Expand Up @@ -571,7 +572,7 @@ NavFn::updateCellAstar(int n)
//

bool
NavFn::propNavFnDijkstra(int cycles, bool atStart)
NavFn::propNavFnDijkstra(int cycles, std::function<bool()> cancelChecker, bool atStart)
{
int nwv = 0; // max priority block size
int nc = 0; // number of cells put into priority blocks
Expand All @@ -581,6 +582,10 @@ NavFn::propNavFnDijkstra(int cycles, bool atStart)
int startCell = start[1] * nx + start[0];

for (; cycle < cycles; cycle++) { // go for this many cycles, unless interrupted
if (cycle % terminal_checking_interval == 0 && cancelChecker()) {
throw nav2_core::PlannerCancelled("Planner was cancelled");
}

if (curPe == 0 && nextPe == 0) { // priority blocks empty
break;
}
Expand Down Expand Up @@ -652,7 +657,7 @@ NavFn::propNavFnDijkstra(int cycles, bool atStart)
//

bool
NavFn::propNavFnAstar(int cycles)
NavFn::propNavFnAstar(int cycles, std::function<bool()> cancelChecker)
{
int nwv = 0; // max priority block size
int nc = 0; // number of cells put into priority blocks
Expand All @@ -667,6 +672,10 @@ NavFn::propNavFnAstar(int cycles)

// do main cycle
for (; cycle < cycles; cycle++) { // go for this many cycles, unless interrupted
if (cycle % terminal_checking_interval == 0 && cancelChecker()) {
throw nav2_core::PlannerCancelled("Planner was cancelled");
SteveMacenski marked this conversation as resolved.
Show resolved Hide resolved
}

if (curPe == 0 && nextPe == 0) { // priority blocks empty
break;
}
Expand Down
10 changes: 6 additions & 4 deletions nav2_navfn_planner/src/navfn_planner.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -129,7 +129,8 @@ NavfnPlanner::cleanup()

nav_msgs::msg::Path NavfnPlanner::createPlan(
const geometry_msgs::msg::PoseStamped & start,
const geometry_msgs::msg::PoseStamped & goal)
const geometry_msgs::msg::PoseStamped & goal,
std::function<bool()> cancel_checker)
{
#ifdef BENCHMARK_TESTING
steady_clock::time_point a = steady_clock::now();
Expand Down Expand Up @@ -183,7 +184,7 @@ nav_msgs::msg::Path NavfnPlanner::createPlan(
return path;
}

if (!makePlan(start.pose, goal.pose, tolerance_, path)) {
if (!makePlan(start.pose, goal.pose, tolerance_, cancel_checker, path)) {
throw nav2_core::NoValidPathCouldBeFound(
"Failed to create plan with tolerance of: " + std::to_string(tolerance_) );
}
Expand Down Expand Up @@ -214,6 +215,7 @@ bool
NavfnPlanner::makePlan(
const geometry_msgs::msg::Pose & start,
const geometry_msgs::msg::Pose & goal, double tolerance,
std::function<bool()> cancel_checker,
nav_msgs::msg::Path & plan)
{
// clear the plan, just in case
Expand Down Expand Up @@ -261,9 +263,9 @@ NavfnPlanner::makePlan(
planner_->setStart(map_goal);
planner_->setGoal(map_start);
if (use_astar_) {
planner_->calcNavFnAstar();
planner_->calcNavFnAstar(cancel_checker);
} else {
planner_->calcNavFnDijkstra(true);
planner_->calcNavFnDijkstra(cancel_checker, true);
}

double resolution = costmap_->getResolution();
Expand Down
5 changes: 4 additions & 1 deletion nav2_planner/include/nav2_planner/planner_server.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -68,12 +68,15 @@ class PlannerServer : public nav2_util::LifecycleNode
* @brief Method to get plan from the desired plugin
* @param start starting pose
* @param goal goal request
* @param planner_id The planner to plan with
* @param cancel_checker A function to check if the action has been canceled
* @return Path
*/
nav_msgs::msg::Path getPlan(
const geometry_msgs::msg::PoseStamped & start,
const geometry_msgs::msg::PoseStamped & goal,
const std::string & planner_id);
const std::string & planner_id,
std::function<bool()> cancel_checker);

protected:
/**
Expand Down
27 changes: 22 additions & 5 deletions nav2_planner/src/planner_server.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -394,6 +394,10 @@ void PlannerServer::computePlanThroughPoses()
throw nav2_core::PlannerTFError("Unable to get start pose");
}

auto cancel_checker = [this]() {
return action_server_poses_->is_cancel_requested();
};

// Get consecutive paths through these points
for (unsigned int i = 0; i != goal->goals.size(); i++) {
// Get starting point
Expand All @@ -413,7 +417,9 @@ void PlannerServer::computePlanThroughPoses()
}

// Get plan from start -> goal
nav_msgs::msg::Path curr_path = getPlan(curr_start, curr_goal, goal->planner_id);
nav_msgs::msg::Path curr_path = getPlan(
curr_start, curr_goal, goal->planner_id,
cancel_checker);

if (!validatePath<ActionThroughPoses>(curr_goal, curr_path, goal->planner_id)) {
throw nav2_core::NoValidPathCouldBeFound(goal->planner_id + " generated a empty path");
Expand Down Expand Up @@ -476,6 +482,9 @@ void PlannerServer::computePlanThroughPoses()
exceptionWarning(curr_start, curr_goal, goal->planner_id, ex);
result->error_code = ActionThroughPosesResult::NO_VIAPOINTS_GIVEN;
action_server_poses_->terminate_current(result);
} catch (nav2_core::PlannerCancelled &) {
RCLCPP_INFO(get_logger(), "Goal was canceled. Canceling planning action.");
action_server_poses_->terminate_all();
} catch (std::exception & ex) {
exceptionWarning(curr_start, curr_goal, goal->planner_id, ex);
result->error_code = ActionThroughPosesResult::UNKNOWN;
Expand Down Expand Up @@ -516,7 +525,11 @@ PlannerServer::computePlan()
throw nav2_core::PlannerTFError("Unable to transform poses to global frame");
}

result->path = getPlan(start, goal_pose, goal->planner_id);
auto cancel_checker = [this]() {
return action_server_pose_->is_cancel_requested();
};

result->path = getPlan(start, goal_pose, goal->planner_id, cancel_checker);

if (!validatePath<ActionThroughPoses>(goal_pose, result->path, goal->planner_id)) {
throw nav2_core::NoValidPathCouldBeFound(goal->planner_id + " generated a empty path");
Expand Down Expand Up @@ -567,6 +580,9 @@ PlannerServer::computePlan()
exceptionWarning(start, goal->goal, goal->planner_id, ex);
result->error_code = ActionToPoseResult::TF_ERROR;
action_server_pose_->terminate_current(result);
} catch (nav2_core::PlannerCancelled &) {
RCLCPP_INFO(get_logger(), "Goal was canceled. Canceling planning action.");
action_server_pose_->terminate_all();
} catch (std::exception & ex) {
exceptionWarning(start, goal->goal, goal->planner_id, ex);
result->error_code = ActionToPoseResult::UNKNOWN;
Expand All @@ -578,22 +594,23 @@ nav_msgs::msg::Path
PlannerServer::getPlan(
const geometry_msgs::msg::PoseStamped & start,
const geometry_msgs::msg::PoseStamped & goal,
const std::string & planner_id)
const std::string & planner_id,
std::function<bool()> cancel_checker)
{
RCLCPP_DEBUG(
get_logger(), "Attempting to a find path from (%.2f, %.2f) to "
"(%.2f, %.2f).", start.pose.position.x, start.pose.position.y,
goal.pose.position.x, goal.pose.position.y);

if (planners_.find(planner_id) != planners_.end()) {
return planners_[planner_id]->createPlan(start, goal);
return planners_[planner_id]->createPlan(start, goal, cancel_checker);
} else {
if (planners_.size() == 1 && planner_id.empty()) {
RCLCPP_WARN_ONCE(
get_logger(), "No planners specified in action call. "
"Server will use only plugin %s in server."
" This warning will appear once.", planner_ids_concat_.c_str());
return planners_[planners_.begin()->first]->createPlan(start, goal);
return planners_[planners_.begin()->first]->createPlan(start, goal, cancel_checker);
} else {
RCLCPP_ERROR(
get_logger(), "planner %s is not a valid planner. "
Expand Down
1 change: 1 addition & 0 deletions nav2_smac_planner/README.md
Original file line number Diff line number Diff line change
Expand Up @@ -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: 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
7 changes: 6 additions & 1 deletion nav2_smac_planner/include/nav2_smac_planner/a_star.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -88,13 +88,16 @@ 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 or
* or planning time exceeded
* @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);
Expand All @@ -104,11 +107,13 @@ class AStarAlgorithm
* @param path Reference to a vector of indicies of generated path
* @param num_iterations Reference to number of iterations to create plan
* @param tolerance Reference to tolerance in costmap nodes
* @param cancel_checker Function to check if the task has been canceled
* @param expansions_log Optional expansions logged for debug
* @return if plan was successful
*/
bool createPath(
CoordinateVector & path, int & num_iterations, const float & tolerance,
std::function<bool()> cancel_checker,
std::vector<std::tuple<float, float, float>> * expansions_log = nullptr);

/**
Expand Down Expand Up @@ -250,11 +255,11 @@ class AStarAlgorithm
*/
void clearStart();

int _timing_interval = 5000;

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