Direction change index computation during initialization (ros-navigat…

…ion#4026) * modified direction change index computation point prior the direction change index was being computed in ever getNeighbor call, now its done at parse time. Signed-off-by: Anil Kumar Chavali <[email protected]> * changed computation point of direction_change_index * moved computation to getMotionPrimitives function * updated getMotionPrimitives function to return index * updated node lattice tests with modified function * removed obsolete code Signed-off-by: Anil Kumar Chavali <[email protected]> * updated docstring for getMotionPrimitives func. Signed-off-by: Anil Kumar Chavali <[email protected]> * linting fixes using ament_uncrustify Signed-off-by: Anil Kumar Chavali <[email protected]> --------- Signed-off-by: Anil Kumar Chavali <[email protected]>
Marc-Morcos · Jul 4, 2024 · 1eb7957 · 1eb7957
1 parent cf3e718
commit 1eb7957
Show file tree

Hide file tree

Showing 3 changed files with 26 additions and 16 deletions.
diff --git a/nav2_smac_planner/include/nav2_smac_planner/node_lattice.hpp b/nav2_smac_planner/include/nav2_smac_planner/node_lattice.hpp
@@ -67,9 +67,12 @@ struct LatticeMotionTable
   /**
    * @brief Get projections of motion models
    * @param node Ptr to NodeLattice
+   * @param Reference direction change index
    * @return A set of motion poses
    */
-  MotionPrimitivePtrs getMotionPrimitives(const NodeLattice * node);
+  MotionPrimitivePtrs getMotionPrimitives(
+    const NodeLattice * node,
+    unsigned int & direction_change_index);
 
   /**
    * @brief Get file metadata needed

diff --git a/nav2_smac_planner/src/node_lattice.cpp b/nav2_smac_planner/src/node_lattice.cpp
@@ -111,14 +111,19 @@ void LatticeMotionTable::initMotionModel(
   }
 }
 
-MotionPrimitivePtrs LatticeMotionTable::getMotionPrimitives(const NodeLattice * node)
+MotionPrimitivePtrs LatticeMotionTable::getMotionPrimitives(
+  const NodeLattice * node,
+  unsigned int & direction_change_index)
 {
   MotionPrimitives & prims_at_heading = motion_primitives[node->pose.theta];
   MotionPrimitivePtrs primitive_projection_list;
   for (unsigned int i = 0; i != prims_at_heading.size(); i++) {
     primitive_projection_list.push_back(&prims_at_heading[i]);
   }
 
+  // direction change index
+  direction_change_index = static_cast<unsigned int>(primitive_projection_list.size());
+
   if (allow_reverse_expansion) {
     // Find normalized heading bin of the reverse expansion
     double reserve_heading = node->pose.theta - (num_angle_quantization / 2);
@@ -470,17 +475,12 @@ void NodeLattice::getNeighbors(
   bool backwards = false;
   NodePtr neighbor = nullptr;
   Coordinates initial_node_coords, motion_projection;
-  MotionPrimitivePtrs motion_primitives = motion_table.getMotionPrimitives(this);
+  unsigned int direction_change_index = 0;
+  MotionPrimitivePtrs motion_primitives = motion_table.getMotionPrimitives(
+    this,
+    direction_change_index);
   const float & grid_resolution = motion_table.lattice_metadata.grid_resolution;
 
-  unsigned int direction_change_idx = 1e9;
-  for (unsigned int i = 0; i != motion_primitives.size(); i++) {
-    if (motion_primitives[0]->start_angle != motion_primitives[i]->start_angle) {
-      direction_change_idx = i;
-      break;
-    }
-  }
-
   for (unsigned int i = 0; i != motion_primitives.size(); i++) {
     const MotionPose & end_pose = motion_primitives[i]->poses.back();
     motion_projection.x = this->pose.x + (end_pose._x / grid_resolution);
@@ -502,7 +502,7 @@ void NodeLattice::getNeighbors(
       // account in case the robot base footprint is asymmetric.
       backwards = false;
       angle = motion_projection.theta;
-      if (i >= direction_change_idx) {
+      if (i >= direction_change_index) {
         backwards = true;
         angle = motion_projection.theta - (motion_table.num_angle_quantization / 2);
         if (angle < 0) {

diff --git a/nav2_smac_planner/test/test_nodelattice.cpp b/nav2_smac_planner/test/test_nodelattice.cpp
@@ -120,17 +120,22 @@ TEST(NodeLatticeTest, test_node_lattice_neighbors_and_parsing)
     nav2_smac_planner::MotionModel::STATE_LATTICE, x, y, angle_quantization, info);
 
   nav2_smac_planner::NodeLattice aNode(0);
+  unsigned int direction_change_index = 0;
   aNode.setPose(nav2_smac_planner::NodeHybrid::Coordinates(0, 0, 0));
   nav2_smac_planner::MotionPrimitivePtrs projections =
-    nav2_smac_planner::NodeLattice::motion_table.getMotionPrimitives(&aNode);
+    nav2_smac_planner::NodeLattice::motion_table.getMotionPrimitives(
+    &aNode,
+    direction_change_index);
 
   EXPECT_NEAR(projections[0]->poses.back()._x, 0.5, 0.01);
   EXPECT_NEAR(projections[0]->poses.back()._y, -0.35, 0.01);
   EXPECT_NEAR(projections[0]->poses.back()._theta, 5.176, 0.01);
 
   EXPECT_NEAR(
     nav2_smac_planner::NodeLattice::motion_table.getLatticeMetadata(
-      filePath).grid_resolution, 0.05, 0.005);
+      filePath)
+    .grid_resolution,
+    0.05, 0.005);
 }
 
 TEST(NodeLatticeTest, test_node_lattice_conversions)
@@ -248,7 +253,6 @@ TEST(NodeLatticeTest, test_node_lattice)
   delete costmapA;
 }
 
-
 TEST(NodeLatticeTest, test_get_neighbors)
 {
   auto lnode = std::make_shared<rclcpp_lifecycle::LifecycleNode>("test");
@@ -356,9 +360,12 @@ TEST(NodeLatticeTest, test_node_lattice_custom_footprint)
   node.pose.x = 20;
   node.pose.y = 20;
   node.pose.theta = 0;
+
+  // initialize direction change index
+  unsigned int direction_change_index = 0;
   // Test that the node is valid though all motion primitives poses for custom footprint
   nav2_smac_planner::MotionPrimitivePtrs motion_primitives =
-    nav2_smac_planner::NodeLattice::motion_table.getMotionPrimitives(&node);
+    nav2_smac_planner::NodeLattice::motion_table.getMotionPrimitives(&node, direction_change_index);
   EXPECT_GT(motion_primitives.size(), 0u);
   for (unsigned int i = 0; i < motion_primitives.size(); i++) {
     EXPECT_EQ(node.isNodeValid(true, checker.get(), motion_primitives[i], false), true);