From e6a646bcd42242e747d8b25a61da816606efba03 Mon Sep 17 00:00:00 2001
From: Adam Aposhian <adam.l.aposhian@gmail.com>
Date: Tue, 17 Aug 2021 09:56:51 -0600
Subject: [PATCH] Smac planner bad alloc (#2516)

* test(nav2_smac_planner): show short path bad_alloc

When given a goal that is one or zero costmap
cells away, AStarAlgorithm throws std::bad_alloc

* fix(nav2_smac_planner): fixed bad_alloc
---
 nav2_smac_planner/src/a_star.cpp       |  4 ++-
 nav2_smac_planner/test/test_a_star.cpp | 41 ++++++++++++++++++++++++++
 2 files changed, 44 insertions(+), 1 deletion(-)

diff --git a/nav2_smac_planner/src/a_star.cpp b/nav2_smac_planner/src/a_star.cpp
index 4a483c0c6b..66acd4b005 100644
--- a/nav2_smac_planner/src/a_star.cpp
+++ b/nav2_smac_planner/src/a_star.cpp
@@ -557,7 +557,9 @@ typename AStarAlgorithm<NodeT>::AnalyticExpansionNodes AStarAlgorithm<NodeT>::ge
   unsigned int num_intervals = std::floor(d / sqrt_2);
 
   AnalyticExpansionNodes possible_nodes;
-  possible_nodes.reserve(num_intervals - 1);  // We won't store this node or the goal
+  // When "from" and "to" are zero or one cell away,
+  // num_intervals == 0
+  possible_nodes.reserve(num_intervals);  // We won't store this node or the goal
   std::vector<double> reals;
 
   // Pre-allocate
diff --git a/nav2_smac_planner/test/test_a_star.cpp b/nav2_smac_planner/test/test_a_star.cpp
index 17e10708f2..c485df5f2b 100644
--- a/nav2_smac_planner/test/test_a_star.cpp
+++ b/nav2_smac_planner/test/test_a_star.cpp
@@ -164,6 +164,47 @@ TEST(AStarTest, test_a_star_se2)
   delete costmapA;
 }
 
+TEST(AStarTest, test_se2_single_pose_path)
+{
+  nav2_smac_planner::SearchInfo info;
+  info.change_penalty = 0.1;
+  info.non_straight_penalty = 1.1;
+  info.reverse_penalty = 2.0;
+  info.minimum_turning_radius = 8;  // in grid coordinates
+  unsigned int size_theta = 72;
+  info.cost_penalty = 1.7;
+  nav2_smac_planner::AStarAlgorithm<nav2_smac_planner::NodeHybrid> a_star(
+    nav2_smac_planner::MotionModel::DUBIN, info);
+  int max_iterations = 100;
+  float tolerance = 10.0;
+  int it_on_approach = 10;
+  int num_it = 0;
+
+  a_star.initialize(false, max_iterations, it_on_approach, 401, size_theta);
+
+  nav2_costmap_2d::Costmap2D * costmapA =
+    new nav2_costmap_2d::Costmap2D(100, 100, 0.1, 0.0, 0.0, 0);
+
+  std::unique_ptr<nav2_smac_planner::GridCollisionChecker> checker =
+    std::make_unique<nav2_smac_planner::GridCollisionChecker>(costmapA, size_theta);
+  checker->setFootprint(nav2_costmap_2d::Footprint(), true, 0.0);
+
+  // functional case testing
+  a_star.setCollisionChecker(checker.get());
+  a_star.setStart(10u, 10u, 0u);
+  // Goal is one costmap cell away
+  a_star.setGoal(11u, 10u, 0u);
+  nav2_smac_planner::NodeHybrid::CoordinateVector path;
+  EXPECT_TRUE(a_star.createPath(path, num_it, tolerance));
+
+  // Check that the path is length one
+  // With the current implementation, this produces a longer path
+  // EXPECT_EQ(path.size(), 1u);
+  EXPECT_GE(path.size(), 1u);
+
+  delete costmapA;
+}
+
 TEST(AStarTest, test_constants)
 {
   nav2_smac_planner::MotionModel mm = nav2_smac_planner::MotionModel::UNKNOWN;  // unknown