The smac planner max_planning_time parameter can timeout the A* planning stage (#2586)

- Until now the max_planning_time parameter only had an effect on the smoothing and sampling stages of the planner, so the actual A* planning could possibly take a lot longer

Author: zoltan-lengyel

nav2_smac_planner/include/nav2_smac_planner/a_star.hpp

@@ -105,11 +105,14 @@ 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 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 double & max_planning_time,
     const float & lookup_table_size,
     const unsigned int & dim_3_size);
 
@@ -283,9 +286,12 @@ class AStarAlgorithm
    */
   NodePtr setAnalyticPath(const NodePtr & node, const AnalyticExpansionNodes & expanded_nodes);
 
+  int _timing_interval = 5000;
+
   bool _traverse_unknown;
   int _max_iterations;
   int _max_on_approach_iterations;
+  double _max_planning_time;
   float _tolerance;
   unsigned int _x_size;
   unsigned int _y_size;

nav2_smac_planner/src/a_star.cpp

@@ -42,6 +42,7 @@ AStarAlgorithm<NodeT>::AStarAlgorithm(
   const SearchInfo & search_info)
 : _traverse_unknown(true),
   _max_iterations(0),
+  _max_planning_time(0),
   _x_size(0),
   _y_size(0),
   _search_info(search_info),
@@ -63,12 +64,14 @@ void AStarAlgorithm<NodeT>::initialize(
   const bool & allow_unknown,
   int & max_iterations,
   const int & max_on_approach_iterations,
+  const double & max_planning_time,
   const float & lookup_table_size,
   const unsigned int & dim_3_size)
 {
   _traverse_unknown = allow_unknown;
   _max_iterations = max_iterations;
   _max_on_approach_iterations = max_on_approach_iterations;
+  _max_planning_time = max_planning_time;
   NodeT::precomputeDistanceHeuristic(lookup_table_size, _motion_model, dim_3_size, _search_info);
   _dim3_size = dim_3_size;
 }
@@ -78,12 +81,14 @@ void AStarAlgorithm<Node2D>::initialize(
   const bool & allow_unknown,
   int & max_iterations,
   const int & max_on_approach_iterations,
+  const double & max_planning_time,
   const float & /*lookup_table_size*/,
   const unsigned int & dim_3_size)
 {
   _traverse_unknown = allow_unknown;
   _max_iterations = max_iterations;
   _max_on_approach_iterations = max_on_approach_iterations;
+  _max_planning_time = max_planning_time;
 
   if (dim_3_size != 1) {
     throw std::runtime_error("Node type Node2D cannot be given non-1 dim 3 quantization.");
@@ -211,6 +216,7 @@ bool AStarAlgorithm<NodeT>::createPath(
   CoordinateVector & path, int & iterations,
   const float & tolerance)
 {
+  steady_clock::time_point start_time = steady_clock::now();
   _tolerance = tolerance;
   _best_heuristic_node = {std::numeric_limits<float>::max(), 0};
   clearQueue();
@@ -248,6 +254,15 @@ bool AStarAlgorithm<NodeT>::createPath(
     };
 
   while (iterations < getMaxIterations() && !_queue.empty()) {
+    // Check for planning timeout only on every Nth iteration
+    if (iterations % _timing_interval == 0) {
+      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;
+      }
+    }
+
     // 1) Pick Nbest from O s.t. min(f(Nbest)), remove from queue
     current_node = getNextNode();
 

nav2_smac_planner/src/smac_planner_2d.cpp

@@ -128,6 +128,7 @@ void SmacPlanner2D::configure(
     _allow_unknown,
     _max_iterations,
     _max_on_approach_iterations,
+    _max_planning_time,
     0.0 /*unused for 2D*/,
     1.0 /*unused for 2D*/);
 
@@ -355,6 +356,7 @@ SmacPlanner2D::dynamicParametersCallback(std::vector<rclcpp::Parameter> paramete
         reinit_a_star = true;
         _search_info.cost_penalty = parameter.as_double();
       } else if (name == _name + ".max_planning_time") {
+        reinit_a_star = true;
         _max_planning_time = parameter.as_double();
       }
     } else if (type == ParameterType::PARAMETER_BOOL) {
@@ -414,6 +416,7 @@ SmacPlanner2D::dynamicParametersCallback(std::vector<rclcpp::Parameter> paramete
         _allow_unknown,
         _max_iterations,
         _max_on_approach_iterations,
+        _max_planning_time,
         0.0 /*unused for 2D*/,
         1.0 /*unused for 2D*/);
     }

nav2_smac_planner/src/smac_planner_hybrid.cpp

@@ -164,6 +164,7 @@ void SmacPlannerHybrid::configure(
     _allow_unknown,
     _max_iterations,
     std::numeric_limits<int>::max(),
+    _max_planning_time,
     _lookup_table_dim,
     _angle_quantizations);
 
@@ -375,6 +376,7 @@ void SmacPlannerHybrid::on_parameter_event_callback(
 
     if (type == ParameterType::PARAMETER_DOUBLE) {
       if (name == _name + ".max_planning_time") {
+        reinit_a_star = true;
         _max_planning_time = value.double_value;
       } else if (name == _name + ".lookup_table_size") {
         reinit_a_star = true;
@@ -475,6 +477,7 @@ void SmacPlannerHybrid::on_parameter_event_callback(
         _allow_unknown,
         _max_iterations,
         std::numeric_limits<int>::max(),
+        _max_planning_time,
         _lookup_table_dim,
         _angle_quantizations);
     }

nav2_smac_planner/test/test_a_star.cpp

@@ -43,9 +43,10 @@ TEST(AStarTest, test_a_star_2d)
   float tolerance = 0.0;
   float some_tolerance = 20.0;
   int it_on_approach = 10;
+  double max_planning_time = 120.0;
   int num_it = 0;
 
-  a_star.initialize(false, max_iterations, it_on_approach, 0.0, 1);
+  a_star.initialize(false, max_iterations, it_on_approach, max_planning_time, 0.0, 1);
 
   nav2_costmap_2d::Costmap2D * costmapA =
     new nav2_costmap_2d::Costmap2D(100, 100, 0.1, 0.0, 0.0, 0);
@@ -81,7 +82,7 @@ TEST(AStarTest, test_a_star_2d)
   // failure cases with invalid inputs
   nav2_smac_planner::AStarAlgorithm<nav2_smac_planner::Node2D> a_star_2(
     nav2_smac_planner::MotionModel::VON_NEUMANN, info);
-  a_star_2.initialize(false, max_iterations, it_on_approach, 0, 1);
+  a_star_2.initialize(false, max_iterations, it_on_approach, max_planning_time, 0, 1);
   num_it = 0;
   EXPECT_THROW(a_star_2.createPath(path, num_it, tolerance), std::runtime_error);
   a_star_2.setCollisionChecker(checker.get());
@@ -130,9 +131,10 @@ TEST(AStarTest, test_a_star_se2)
   int max_iterations = 10000;
   float tolerance = 10.0;
   int it_on_approach = 10;
+  double max_planning_time = 120.0;
   int num_it = 0;
 
-  a_star.initialize(false, max_iterations, it_on_approach, 401, size_theta);
+  a_star.initialize(false, max_iterations, it_on_approach, max_planning_time, 401, size_theta);
 
   nav2_costmap_2d::Costmap2D * costmapA =
     new nav2_costmap_2d::Costmap2D(100, 100, 0.1, 0.0, 0.0, 0);
@@ -178,9 +180,10 @@ TEST(AStarTest, test_se2_single_pose_path)
   int max_iterations = 100;
   float tolerance = 10.0;
   int it_on_approach = 10;
+  double max_planning_time = 120.0;
   int num_it = 0;
 
-  a_star.initialize(false, max_iterations, it_on_approach, 401, size_theta);
+  a_star.initialize(false, max_iterations, it_on_approach, max_planning_time, 401, size_theta);
 
   nav2_costmap_2d::Costmap2D * costmapA =
     new nav2_costmap_2d::Costmap2D(100, 100, 0.1, 0.0, 0.0, 0);