From 1eb7957c09202c44bb235228b5a2644a27ab0a29 Mon Sep 17 00:00:00 2001 From: Anil Kumar Chavali <44644339+akchobby@users.noreply.github.com> Date: Fri, 5 Jan 2024 18:44:55 +0100 Subject: [PATCH] Direction change index computation during initialization (#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 * 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 * updated docstring for getMotionPrimitives func. Signed-off-by: Anil Kumar Chavali * linting fixes using ament_uncrustify Signed-off-by: Anil Kumar Chavali --------- Signed-off-by: Anil Kumar Chavali --- .../nav2_smac_planner/node_lattice.hpp | 5 ++++- nav2_smac_planner/src/node_lattice.cpp | 22 +++++++++---------- nav2_smac_planner/test/test_nodelattice.cpp | 15 +++++++++---- 3 files changed, 26 insertions(+), 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 index 4fe6a284cf..577cc28e19 100644 --- 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 index c852a96dd3..dfdf47ff4b 100644 --- a/nav2_smac_planner/src/node_lattice.cpp +++ b/nav2_smac_planner/src/node_lattice.cpp @@ -111,7 +111,9 @@ 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; @@ -119,6 +121,9 @@ MotionPrimitivePtrs LatticeMotionTable::getMotionPrimitives(const NodeLattice * primitive_projection_list.push_back(&prims_at_heading[i]); } + // direction change index + direction_change_index = static_cast(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 index 16674d9dad..f64f022f71 100644 --- a/nav2_smac_planner/test/test_nodelattice.cpp +++ b/nav2_smac_planner/test/test_nodelattice.cpp @@ -120,9 +120,12 @@ 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); @@ -130,7 +133,9 @@ TEST(NodeLatticeTest, test_node_lattice_neighbors_and_parsing) 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("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);