nav2_smac_planner: make A* return closest path found in case of timeout

nav2_smac_planner/include/nav2_smac_planner/a_star.hpp

@@ -238,6 +238,13 @@ class AStarAlgorithm
    */
   inline bool areInputsValid();
 
+  /**
+   * @brief Get the closest path within tolerance if available
+   * @param path Vector of coordinates to fill with path
+   * @return if a valid path was found within tolerance
+   */
+  inline bool getClosestPathWithinTolerance(CoordinateVector & path);
+
   /**
    * @brief Clear heuristic queue of nodes to search
    */

nav2_smac_planner/src/a_star.cpp

@@ -322,6 +322,17 @@ bool AStarAlgorithm<NodeT>::areInputsValid()
   return true;
 }
 
+template<typename NodeT>
+bool AStarAlgorithm<NodeT>::getClosestPathWithinTolerance(CoordinateVector & path)
+{
+  if (_best_heuristic_node.first < getToleranceHeuristic()) {
+    _graph.at(_best_heuristic_node.second).backtracePath(path);
+    return true;
+  }
+
+  return false;
+}
+
 template<typename NodeT>
 bool AStarAlgorithm<NodeT>::createPath(
   CoordinateVector & path, int & iterations,
@@ -381,7 +392,8 @@ bool AStarAlgorithm<NodeT>::createPath(
       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;
+        // In case of timeout, return the path that is closest, if within tolerance.
+        return getClosestPathWithinTolerance(path);
       }
     }
 
@@ -448,12 +460,8 @@ bool AStarAlgorithm<NodeT>::createPath(
     }
   }
 
-  if (_best_heuristic_node.first < getToleranceHeuristic()) {
-    // If we run out of search options, return the path that is closest, if within tolerance.
-    return _graph.at(_best_heuristic_node.second).backtracePath(path);
-  }
-
-  return false;
+  // If we run out of search options, return the path that is closest, if within tolerance.
+  return getClosestPathWithinTolerance(path);
 }
 
 template<typename NodeT>