From c42312062e7e15754fb8e8d882eb762b6e75a680 Mon Sep 17 00:00:00 2001
From: James Ward <j.ward@sydney.edu.au>
Date: Mon, 22 Jul 2024 15:28:43 +1000
Subject: [PATCH] Add test to verify analytic expansions are straight

Signed-off-by: James Ward <j.ward@sydney.edu.au>
---
 nav2_smac_planner/test/test_a_star.cpp | 70 ++++++++++++++++++++++++++
 1 file changed, 70 insertions(+)

diff --git a/nav2_smac_planner/test/test_a_star.cpp b/nav2_smac_planner/test/test_a_star.cpp
index 3ec5156f23a..ceb7f8102f3 100644
--- a/nav2_smac_planner/test/test_a_star.cpp
+++ b/nav2_smac_planner/test/test_a_star.cpp
@@ -218,6 +218,76 @@ TEST(AStarTest, test_a_star_se2)
   nav2_smac_planner::NodeHybrid::destroyStaticAssets();
 }
 
+TEST(AStarTest, test_a_star_analytic_expansion)
+{
+  auto lnode = std::make_shared<rclcpp_lifecycle::LifecycleNode>("test");
+  nav2_smac_planner::SearchInfo info;
+  info.change_penalty = 0.0;
+  info.non_straight_penalty = 1.1;
+  info.reverse_penalty = 0.0;
+  info.minimum_turning_radius = 8;  // in grid coordinates
+  info.retrospective_penalty = 0.015;
+  info.analytic_expansion_max_length = 2000.0;  // in grid coordinates
+  info.analytic_expansion_ratio = 3.5;
+  unsigned int size_theta = 72;
+  info.cost_penalty = 1.7;
+  nav2_smac_planner::AStarAlgorithm<nav2_smac_planner::NodeHybrid> a_star(
+    nav2_smac_planner::MotionModel::REEDS_SHEPP, info);
+  int max_iterations = 10000;
+  float tolerance = 10.0;
+  int it_on_approach = 10;
+  int terminal_checking_interval = 5000;
+  double max_planning_time = 120.0;
+  int num_it = 0;
+
+  a_star.initialize(
+    false, max_iterations, it_on_approach, terminal_checking_interval,
+    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);
+
+  // Convert raw costmap into a costmap ros object
+  auto costmap_ros = std::make_shared<nav2_costmap_2d::Costmap2DROS>();
+  costmap_ros->on_configure(rclcpp_lifecycle::State());
+  auto costmap = costmap_ros->getCostmap();
+  *costmap = *costmapA;
+
+  std::unique_ptr<nav2_smac_planner::GridCollisionChecker> checker =
+    std::make_unique<nav2_smac_planner::GridCollisionChecker>(costmap_ros, size_theta, lnode);
+  checker->setFootprint(nav2_costmap_2d::Footprint(), true, 0.0);
+
+  // functional case testing
+  a_star.setCollisionChecker(checker.get());
+  a_star.setStart(80u, 0u, 0u);
+  a_star.setGoal(20u, 0u, 0u);
+  nav2_smac_planner::NodeHybrid::CoordinateVector path;
+  std::unique_ptr<std::vector<std::tuple<float, float, float>>> expansions = nullptr;
+  expansions = std::make_unique<std::vector<std::tuple<float, float, float>>>();
+
+  auto dummy_cancel_checker = []() {
+      return false;
+    };
+
+  EXPECT_TRUE(a_star.createPath(path, num_it, tolerance, dummy_cancel_checker, expansions.get()));
+
+  // check path is collision free
+  for (unsigned int i = 0; i != path.size(); i++) {
+    EXPECT_EQ(costmapA->getCost(path[i].x, path[i].y), 0);
+  }
+  // no skipped nodes
+  for (unsigned int i = 1; i != path.size(); i++) {
+    EXPECT_LT(hypotf(path[i].x - path[i - 1].x, path[i].y - path[i - 1].y), 2.1f);
+  }
+  // all straight with no wiggle
+  for (unsigned int i = 0; i != path.size(); i++) {
+    EXPECT_NEAR(path[i].theta, 0.0, 1e-3);
+  }
+
+  delete costmapA;
+  nav2_smac_planner::NodeHybrid::destroyStaticAssets();
+}
+
 TEST(AStarTest, test_a_star_lattice)
 {
   auto lnode = std::make_shared<rclcpp_lifecycle::LifecycleNode>("test");