Support cancel in Navfn planner

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

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 */
+  int cancel_check_interval;
+
   /** 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 % cancel_check_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 % cancel_check_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();