Stop planner if the goal is cancelled (ros-navigation#4148)

* Add support for stopping planners when the action is cancelled

Signed-off-by: Kemal Bektas <kemal.bektas@node-robotics.com>

* Support cancel in Smac planner

Signed-off-by: Kemal Bektas <kemal.bektas@node-robotics.com>

* Support cancel in Theta* planner

Signed-off-by: Kemal Bektas <kemal.bektas@node-robotics.com>

* Support cancel in Navfn planner

Signed-off-by: Kemal Bektas <kemal.bektas@node-robotics.com>

* Update nav2_system_tests to use cancel checker

Signed-off-by: Kemal Bektas <kemal.bektas@node-robotics.com>

* Add terminal_checking_interval parameter

Signed-off-by: Kemal Bektas <kemal.bektas@node-robotics.com>

* Handle timeout and cancel check on the same branch and default to 5000 iterations

Signed-off-by: Kemal Bektas <kemal.bektas@node-robotics.com>

* Add system tests for cancel

Signed-off-by: Kemal Bektas <kemal.bektas@node-robotics.com>

---------

Signed-off-by: Kemal Bektas <kemal.bektas@node-robotics.com>
Co-authored-by: Kemal Bektas <kemal.bektas@node-robotics.com>
Signed-off-by: stevedan <stevedan.o.omodolor@gmail.com>

nav2_core/include/nav2_core/global_planner.hpp

@@ -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

nav2_core/include/nav2_core/planner_exceptions.hpp

@@ -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_

nav2_navfn_planner/include/nav2_navfn_planner/navfn.hpp

@@ -43,6 +43,7 @@
 #include <stdint.h>
 #include <string.h>
 #include <stdio.h>
+#include <functional>
 
 namespace nav2_navfn_planner
 {
@@ -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
@@ -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.
@@ -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 */

nav2_navfn_planner/include/nav2_navfn_planner/navfn_planner.hpp

@@ -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);
 
   /**

nav2_navfn_planner/src/navfn.cpp

@@ -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
@@ -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);
 }
 
 
@@ -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);
 }
 
 //
@@ -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
@@ -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;
     }
@@ -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
@@ -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");
+    }
+
     if (curPe == 0 && nextPe == 0) {  // priority blocks empty
       break;
     }

nav2_navfn_planner/src/navfn_planner.cpp

@@ -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();
@@ -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_) );
   }
@@ -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
@@ -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();

nav2_planner/include/nav2_planner/planner_server.hpp

@@ -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:
   /**

nav2_planner/src/planner_server.cpp

@@ -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
@@ -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");
@@ -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;
@@ -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");
@@ -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;
@@ -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. "

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: 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*.

nav2_smac_planner/include/nav2_smac_planner/a_star.hpp

@@ -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);
@@ -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);
 
   /**
@@ -251,11 +256,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;