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 <[email protected]>
ros-navigation · Jul 22, 2024 · 83a04cc · 83a04cc
1 parent c423120
commit 83a04cc
Show file tree

Hide file tree

Showing 6 changed files with 26 additions and 10 deletions.
diff --git a/nav2_smac_planner/include/nav2_smac_planner/node_hybrid.hpp b/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;

diff --git a/nav2_smac_planner/include/nav2_smac_planner/node_lattice.hpp b/nav2_smac_planner/include/nav2_smac_planner/node_lattice.hpp
@@ -96,6 +96,13 @@ struct LatticeMotionTable
    * @return Raw orientation in radians
    */
   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;

diff --git a/nav2_smac_planner/src/analytic_expansion.cpp b/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(

diff --git a/nav2_smac_planner/src/node_hybrid.cpp b/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),

diff --git a/nav2_smac_planner/src/node_lattice.cpp b/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),

diff --git a/nav2_smac_planner/test/test_a_star.cpp b/nav2_smac_planner/test/test_a_star.cpp
@@ -257,7 +257,7 @@ TEST(AStarTest, test_a_star_analytic_expansion)
     std::make_unique<nav2_smac_planner::GridCollisionChecker>(costmap_ros, size_theta, lnode);
   checker->setFootprint(nav2_costmap_2d::Footprint(), true, 0.0);
 
-  // functional case testing
+  // should be a straight path running backwards
   a_star.setCollisionChecker(checker.get());
   a_star.setStart(80u, 0u, 0u);
   a_star.setGoal(20u, 0u, 0u);
@@ -271,14 +271,6 @@ TEST(AStarTest, test_a_star_analytic_expansion)
 
   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);