Straight analytic expansions (#4549)

* Add test to verify analytic expansions are straight

Signed-off-by: James Ward <j.ward@sydney.edu.au>

* Use continuous scaling of angle when performing analytic expansion

Only applies to Hybrid A* - behaviour in lattice planner is unchanged

Signed-off-by: James Ward <j.ward@sydney.edu.au>

---------

Signed-off-by: James Ward <j.ward@sydney.edu.au>

nav2_smac_planner/include/nav2_smac_planner/node_hybrid.hpp

@@ -112,6 +112,13 @@ struct HybridMotionTable
    */
   float getAngleFromBin(const unsigned int & bin_idx);
 
+  /**
+   * @brief Get the angle scaled across bins from a raw orientation
+   * @param theta Angle in radians
+   * @return angle scaled across bins
+   */
+  double getAngle(const double & theta);
+
   MotionModel motion_model = MotionModel::UNKNOWN;
   MotionPoses projections;
   unsigned int size_x;

nav2_smac_planner/include/nav2_smac_planner/node_lattice.hpp

@@ -97,6 +97,13 @@ struct LatticeMotionTable
    */
   float & getAngleFromBin(const unsigned int & bin_idx);
 
+  /**
+   * @brief Get the angular bin to use from a raw orientation
+   * @param theta Angle in radians
+   * @return bin index of closest angle to request
+   */
+  double getAngle(const double & theta);
+
   unsigned int size_x;
   unsigned int num_angle_quantization;
   float change_penalty;

nav2_smac_planner/src/analytic_expansion.cpp

@@ -216,7 +216,7 @@ typename AnalyticExpansion<NodeT>::AnalyticExpansionNodes AnalyticExpansion<Node
     // Make sure in range [0, 2PI)
     theta = (reals[2] < 0.0) ? (reals[2] + 2.0 * M_PI) : reals[2];
     theta = (theta > 2.0 * M_PI) ? (theta - 2.0 * M_PI) : theta;
-    angle = node->motion_table.getClosestAngularBin(theta);
+    angle = node->motion_table.getAngle(theta);
 
     // Turn the pose into a node, and check if it is valid
     index = NodeT::getIndex(

nav2_smac_planner/src/node_hybrid.cpp

@@ -339,6 +339,11 @@ float HybridMotionTable::getAngleFromBin(const unsigned int & bin_idx)
   return bin_idx * bin_size;
 }
 
+double HybridMotionTable::getAngle(const double & theta)
+{
+  return theta / bin_size;
+}
+
 NodeHybrid::NodeHybrid(const uint64_t index)
 : parent(nullptr),
   pose(0.0f, 0.0f, 0.0f),

nav2_smac_planner/src/node_lattice.cpp

@@ -180,6 +180,11 @@ float & LatticeMotionTable::getAngleFromBin(const unsigned int & bin_idx)
   return lattice_metadata.heading_angles[bin_idx];
 }
 
+double LatticeMotionTable::getAngle(const double & theta)
+{
+  return getClosestAngularBin(theta);
+}
+
 NodeLattice::NodeLattice(const uint64_t index)
 : parent(nullptr),
   pose(0.0f, 0.0f, 0.0f),

nav2_smac_planner/test/test_a_star.cpp

@@ -218,6 +218,68 @@ 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);
+
+  // should be a straight path running backwards
+  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()));
+
+  // 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");