Adding planner server timeout for costmap waiting (ros-navigation#4673)

* Adding planner server timeout for costmap waiting

Signed-off-by: Steve Macenski <[email protected]>

* Adding controller server's costmap timeout as well

Signed-off-by: Steve Macenski <[email protected]>

---------

Signed-off-by: Steve Macenski <[email protected]>

nav2_core/include/nav2_core/exceptions.hpp

@@ -51,6 +51,13 @@ class PlannerException : public std::runtime_error
   using Ptr = std::shared_ptr<PlannerException>;
 };
 
+class PlannerCancelled : public PlannerException
+{
+public:
+  explicit PlannerCancelled(const std::string & description)
+  : PlannerException(description) {}
+};
+
 }  // namespace nav2_core
 
 #endif  // NAV2_CORE__EXCEPTIONS_HPP_

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

@@ -80,24 +80,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);
 }
 
 //
@@ -573,7 +574,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
@@ -583,6 +584,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;
     }
@@ -654,7 +659,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
@@ -669,6 +674,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

@@ -133,7 +133,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();
@@ -175,10 +176,9 @@ nav_msgs::msg::Path NavfnPlanner::createPlan(
     return path;
   }
 
-  if (!makePlan(start.pose, goal.pose, tolerance_, path)) {
-    RCLCPP_WARN(
-      logger_, "%s: failed to create plan with "
-      "tolerance %.2f.", name_.c_str(), tolerance_);
+  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_) );
   }
 
 
@@ -207,6 +207,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
@@ -268,9 +269,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_smac_planner/README.md

@@ -110,6 +110,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

@@ -86,13 +86,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);
@@ -102,9 +105,14 @@ 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);
+  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);
 
   /**
    * @brief Sets the collision checker to use
@@ -232,12 +240,25 @@ class AStarAlgorithm
    */
   inline void clearGraph();
 
-  int _timing_interval = 5000;
+  /**
+   * @brief Populate a debug log of expansions for Hybrid-A* for visualization
+   * @param node Node expanded
+   * @param expansions_log Log to add not expanded to
+   */
+  inline void populateExpansionsLog(
+    const NodePtr & node, std::vector<std::tuple<float, float, float>> * expansions_log);
+
+  /**
+   * @brief Clear Start
+   */
+  void clearStart();
+
 
   bool _traverse_unknown;
   bool _is_initialized;
   int _max_iterations;
   int _max_on_approach_iterations;
+  int _terminal_checking_interval;
   double _max_planning_time;
   float _tolerance;
   unsigned int _x_size;

nav2_smac_planner/include/nav2_smac_planner/smac_planner_2d.hpp

@@ -82,11 +82,13 @@ class SmacPlanner2D : 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 action has been canceled
    * @return nav2_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:
   /**
@@ -112,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;

nav2_smac_planner/include/nav2_smac_planner/smac_planner_hybrid.hpp

@@ -80,11 +80,13 @@ class SmacPlannerHybrid : 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 action has been canceled
    * @return nav2_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:
   /**
@@ -112,6 +114,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;

nav2_smac_planner/include/nav2_smac_planner/smac_planner_lattice.hpp

@@ -79,11 +79,13 @@ class SmacPlannerLattice : 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 action has been canceled
    * @return nav2_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:
   /**
@@ -107,6 +109,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<nav_msgs::msg::Path>::SharedPtr _raw_plan_publisher;
   double _max_planning_time;