add timeout logic && fix tests for new function signature (#55)

smac_planner/include/smac_planner/a_star.hpp

@@ -22,6 +22,7 @@
 #include <memory>
 #include <queue>
 #include <utility>
+#include <chrono>
 #include "Eigen/Core"
 
 #include "nav2_costmap_2d/costmap_2d.hpp"
@@ -106,7 +107,7 @@ class AStarAlgorithm
    * @param tolerance Reference to tolerance in costmap nodes
    * @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::chrono::steady_clock::time_point start_time, double max_planning_time);
 
   /**
    * @brief Create the graph based on the node type. For 2D nodes, a cost grid.

smac_planner/src/a_star.cpp

@@ -224,7 +224,7 @@ bool AStarAlgorithm<NodeT>::areInputsValid()
 template<typename NodeT>
 bool AStarAlgorithm<NodeT>::createPath(
   CoordinateVector & path, int & iterations,
-  const float & tolerance)
+  const float & tolerance, steady_clock::time_point start_time, double max_planning_time)
 {
   _tolerance = tolerance * NodeT::neutral_cost;
   _best_heuristic_node = {std::numeric_limits<float>::max(), 0};
@@ -306,6 +306,14 @@ bool AStarAlgorithm<NodeT>::createPath(
       }
     }
 
+    // check how much time left since planning started
+    // if we used all of it, and step three didn't returned path,
+    // we fail due to timeout
+    steady_clock::time_point n = steady_clock::now();
+    duration<double> time_span = duration_cast<duration<double>>(n - start_time);
+    if (time_span.count() * 1000 >= max_planning_time) {
+        throw std::runtime_error("SMAC planner - TIMEOUT: no path found: max_planning_time reached");
+    }
     // 4) Expand neighbors of Nbest not visited
     neighbors.clear();
     NodeT::getNeighbors(

smac_planner/src/smac_planner.cpp

@@ -282,7 +282,7 @@ nav_msgs::msg::Path SmacPlanner::createPlan(
   std::string error;
   try {
     if (!_a_star->createPath(
-        path, num_iterations, _tolerance / static_cast<float>(costmap->getResolution())))
+        path, num_iterations, _tolerance / static_cast<float>(costmap->getResolution()), a, _max_planning_time))
     {
       if (num_iterations < _a_star->getMaxIterations()) {
           nav_msgs::msg::Path plan;

smac_planner/src/smac_planner_2d.cpp

@@ -228,7 +228,7 @@ nav_msgs::msg::Path SmacPlanner2D::createPlan(
   std::string error;
   try {
     if (!_a_star->createPath(
-        path, num_iterations, _tolerance / static_cast<float>(costmap->getResolution())))
+        path, num_iterations, _tolerance / static_cast<float>(costmap->getResolution()), a, _max_planning_time))
     {
       if (num_iterations < _a_star->getMaxIterations()) {
         error = std::string("no valid path found");

smac_planner/test/test_a_star.cpp

@@ -16,6 +16,7 @@
 #include <memory>
 #include <string>
 #include <vector>
+#include <chrono>
 
 #include "gtest/gtest.h"
 #include "rclcpp/rclcpp.hpp"
@@ -26,6 +27,8 @@
 #include "smac_planner/a_star.hpp"
 #include "smac_planner/collision_checker.hpp"
 
+using namespace std::chrono;  // NOLINT
+
 class RclCppFixture
 {
 public:
@@ -61,7 +64,8 @@ TEST(AStarTest, test_a_star_2d)
   a_star.setStart(20u, 20u, 0);
   a_star.setGoal(80u, 80u, 0);
   smac_planner::Node2D::CoordinateVector path;
-  EXPECT_TRUE(a_star.createPath(path, num_it, tolerance));
+  steady_clock::time_point n = steady_clock::now();
+  EXPECT_TRUE(a_star.createPath(path, num_it, tolerance, n, 20000000));
   EXPECT_EQ(num_it, 556);
 
   // check path is the right size and collision free
@@ -84,23 +88,32 @@ TEST(AStarTest, test_a_star_2d)
   a_star_2.initialize(false, max_iterations, it_on_approach);
   a_star_2.setFootprint(nav2_costmap_2d::Footprint(), true);
   num_it = 0;
-  EXPECT_THROW(a_star_2.createPath(path, num_it, tolerance), std::runtime_error);
+  n = steady_clock::now();
+
+  EXPECT_THROW(a_star_2.createPath(path, num_it, tolerance, n, 200000), std::runtime_error);
   a_star_2.createGraph(costmapA->getSizeInCellsX(), costmapA->getSizeInCellsY(), 1, costmapA);
   num_it = 0;
-  EXPECT_THROW(a_star_2.createPath(path, num_it, tolerance), std::runtime_error);
+  n = steady_clock::now();
+
+  EXPECT_THROW(a_star_2.createPath(path, num_it, tolerance, n, 20000), std::runtime_error);
   a_star_2.setStart(50, 50, 0);  // invalid
   a_star_2.setGoal(0, 0, 0);  // valid
   num_it = 0;
-  EXPECT_THROW(a_star_2.createPath(path, num_it, tolerance), std::runtime_error);
+   n = steady_clock::now();
+
+  EXPECT_THROW(a_star_2.createPath(path, num_it, tolerance, n, 200000), std::runtime_error);
   a_star_2.setStart(0, 0, 0);  // valid
   a_star_2.setGoal(50, 50, 0);  // invalid
   num_it = 0;
-  EXPECT_THROW(a_star_2.createPath(path, num_it, tolerance), std::runtime_error);
+   n = steady_clock::now();
+
+  EXPECT_THROW(a_star_2.createPath(path, num_it, tolerance, n, 20000), std::runtime_error);
   num_it = 0;
   // invalid goal but liberal tolerance
   a_star_2.setStart(20, 20, 0);  // valid
   a_star_2.setGoal(50, 50, 0);  // invalid
-  EXPECT_TRUE(a_star_2.createPath(path, num_it, some_tolerance));
+  n = steady_clock::now();
+  EXPECT_TRUE(a_star_2.createPath(path, num_it, some_tolerance, n , 20000));
   EXPECT_EQ(path.size(), 32u);
   for (unsigned int i = 0; i != path.size(); i++) {
     EXPECT_EQ(costmapA->getCost(path[i].x, path[i].y), 0);
@@ -150,7 +163,9 @@ TEST(AStarTest, test_a_star_se2)
   a_star.setStart(10u, 10u, 0u);
   a_star.setGoal(80u, 80u, 40u);
   smac_planner::NodeSE2::CoordinateVector path;
-  EXPECT_TRUE(a_star.createPath(path, num_it, tolerance));
+  steady_clock::time_point n = steady_clock::now();
+
+  EXPECT_TRUE(a_star.createPath(path, num_it, tolerance, n, 20000));
 
   // check path is the right size and collision free
   EXPECT_EQ(num_it, 61);

smac_planner/test/test_smoother.cpp

@@ -16,6 +16,7 @@
 #include <memory>
 #include <string>
 #include <vector>
+#include <chrono>
 
 #include "gtest/gtest.h"
 #include "rclcpp/rclcpp.hpp"
@@ -24,6 +25,9 @@
 #include "smac_planner/a_star.hpp"
 #include "smac_planner/smoother.hpp"
 
+using namespace std::chrono;  // NOLINT
+
+
 class RclCppFixture
 {
 public:
@@ -65,7 +69,8 @@ TEST(SmootherTest, test_smoother)
   a_star.setStart(10u, 10u, 0u);
   a_star.setGoal(80u, 80u, 40u);
   smac_planner::NodeSE2::CoordinateVector plan;
-  EXPECT_TRUE(a_star.createPath(plan, num_it, tolerance));
+  steady_clock::time_point n = steady_clock::now();
+  EXPECT_TRUE(a_star.createPath(plan, num_it, tolerance, n, 20000000));
 
   // populate our smoothing paths
   std::vector<Eigen::Vector2d> path;