From 869e33bb37afdd748889d6a5af1d6c971e862175 Mon Sep 17 00:00:00 2001 From: Joshua Wallace Date: Fri, 2 Sep 2022 10:25:51 -0400 Subject: [PATCH 1/3] fixed start --- .../nav2_smac_planner/node_lattice.hpp | 6 ++ nav2_smac_planner/src/node_2d.cpp | 9 ++- nav2_smac_planner/src/node_hybrid.cpp | 11 ++- nav2_smac_planner/src/node_lattice.cpp | 73 +++++++++++-------- 4 files changed, 61 insertions(+), 38 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 b40624efd6a..7f6a82d31c7 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/node_lattice.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/node_lattice.hpp @@ -408,6 +408,12 @@ class NodeLattice */ bool backtracePath(CoordinateVector & path); + /** + * \brief add node to the path + * \param current_node + */ + void addNodeToPath(NodePtr current_node, CoordinateVector & path); + NodeLattice * parent; Coordinates pose; static LatticeMotionTable motion_table; diff --git a/nav2_smac_planner/src/node_2d.cpp b/nav2_smac_planner/src/node_2d.cpp index 862b1feca68..158a3598700 100644 --- a/nav2_smac_planner/src/node_2d.cpp +++ b/nav2_smac_planner/src/node_2d.cpp @@ -154,13 +154,16 @@ bool Node2D::backtracePath(CoordinateVector & path) NodePtr current_node = this; - do { + while (current_node->parent) { path.push_back( Node2D::getCoords(current_node->getIndex())); current_node = current_node->parent; - } while (current_node->parent); + } + + // add the start pose + path.push_back(Node2D::getCoords(current_node->getIndex())); - return path.size() > 0; + return true; } } // namespace nav2_smac_planner diff --git a/nav2_smac_planner/src/node_hybrid.cpp b/nav2_smac_planner/src/node_hybrid.cpp index 3ccc17ad0bc..ebbe357559e 100644 --- a/nav2_smac_planner/src/node_hybrid.cpp +++ b/nav2_smac_planner/src/node_hybrid.cpp @@ -705,14 +705,19 @@ bool NodeHybrid::backtracePath(CoordinateVector & path) NodePtr current_node = this; - do { + while (current_node->parent) { path.push_back(current_node->pose); // Convert angle to radians path.back().theta = NodeHybrid::motion_table.getAngleFromBin(path.back().theta); current_node = current_node->parent; - } while (current_node->parent); + } + + // add the start pose + path.push_back(current_node->pose); + // Convert angle to radians + path.back().theta = NodeHybrid::motion_table.getAngleFromBin(path.back().theta); - return path.size() > 0; + return true; } } // namespace nav2_smac_planner diff --git a/nav2_smac_planner/src/node_lattice.cpp b/nav2_smac_planner/src/node_lattice.cpp index ee61a6df2c8..f4c8db1922e 100644 --- a/nav2_smac_planner/src/node_lattice.cpp +++ b/nav2_smac_planner/src/node_lattice.cpp @@ -541,44 +541,53 @@ bool NodeLattice::backtracePath(CoordinateVector & path) return false; } - Coordinates initial_pose, prim_pose; NodePtr current_node = this; + + while (current_node->parent) { + addNodeToPath(current_node, path); + current_node = current_node->parent; + } + + // add start to path + addNodeToPath(current_node, path); + + return path.size() > 0; +} + +void NodeLattice::addNodeToPath( + NodeLattice::NodePtr current_node, + NodeLattice::CoordinateVector & path) +{ + Coordinates initial_pose, prim_pose; MotionPrimitive * prim = nullptr; const float & grid_resolution = NodeLattice::motion_table.lattice_metadata.grid_resolution; const float pi_2 = 2.0 * M_PI; - - do { - prim = current_node->getMotionPrimitive(); - // if motion primitive is valid, then was searched (rather than analytically expanded), - // include dense path of subpoints making up the primitive at grid resolution - if (prim) { - initial_pose.x = current_node->pose.x - (prim->poses.back()._x / grid_resolution); - initial_pose.y = current_node->pose.y - (prim->poses.back()._y / grid_resolution); - initial_pose.theta = NodeLattice::motion_table.getAngleFromBin(prim->start_angle); - - for (auto it = prim->poses.crbegin(); it != prim->poses.crend(); ++it) { - // Convert primitive pose into grid space if it should be checked - prim_pose.x = initial_pose.x + (it->_x / grid_resolution); - prim_pose.y = initial_pose.y + (it->_y / grid_resolution); - // If reversing, invert the angle because the robot is backing into the primitive - // not driving forward with it - if (current_node->isBackward()) { - prim_pose.theta = std::fmod(it->_theta + M_PI, pi_2); - } else { - prim_pose.theta = it->_theta; - } - path.push_back(prim_pose); + prim = current_node->getMotionPrimitive(); + // if motion primitive is valid, then was searched (rather than analytically expanded), + // include dense path of subpoints making up the primitive at grid resolution + if (prim) { + initial_pose.x = current_node->pose.x - (prim->poses.back()._x / grid_resolution); + initial_pose.y = current_node->pose.y - (prim->poses.back()._y / grid_resolution); + initial_pose.theta = NodeLattice::motion_table.getAngleFromBin(prim->start_angle); + + for (auto it = prim->poses.crbegin(); it != prim->poses.crend(); ++it) { + // Convert primitive pose into grid space if it should be checked + prim_pose.x = initial_pose.x + (it->_x / grid_resolution); + prim_pose.y = initial_pose.y + (it->_y / grid_resolution); + // If reversing, invert the angle because the robot is backing into the primitive + // not driving forward with it + if (current_node->isBackward()) { + prim_pose.theta = std::fmod(it->_theta + M_PI, pi_2); + } else { + prim_pose.theta = it->_theta; } - } else { - // For analytic expansion nodes where there is no valid motion primitive - path.push_back(current_node->pose); - path.back().theta = NodeLattice::motion_table.getAngleFromBin(path.back().theta); + path.push_back(prim_pose); } - - current_node = current_node->parent; - } while (current_node->parent); - - return path.size() > 0; + } else { + // For analytic expansion nodes where there is no valid motion primitive + path.push_back(current_node->pose); + path.back().theta = NodeLattice::motion_table.getAngleFromBin(path.back().theta); + } } } // namespace nav2_smac_planner From 63cf5e2e2a96391d703d20c6ec5ceeb300a13dd0 Mon Sep 17 00:00:00 2001 From: Joshua Wallace Date: Fri, 2 Sep 2022 10:29:38 -0400 Subject: [PATCH 2/3] return true --- nav2_smac_planner/src/node_lattice.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nav2_smac_planner/src/node_lattice.cpp b/nav2_smac_planner/src/node_lattice.cpp index f4c8db1922e..cf3fc163b5f 100644 --- a/nav2_smac_planner/src/node_lattice.cpp +++ b/nav2_smac_planner/src/node_lattice.cpp @@ -551,7 +551,7 @@ bool NodeLattice::backtracePath(CoordinateVector & path) // add start to path addNodeToPath(current_node, path); - return path.size() > 0; + return true; } void NodeLattice::addNodeToPath( From 6af5560954ec86dbd3548c83038fe8a9bc278141 Mon Sep 17 00:00:00 2001 From: Joshua Wallace Date: Fri, 2 Sep 2022 14:14:23 -0400 Subject: [PATCH 3/3] fix tests --- nav2_smac_planner/test/test_a_star.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/nav2_smac_planner/test/test_a_star.cpp b/nav2_smac_planner/test/test_a_star.cpp index 00852e5298e..72a6ba29683 100644 --- a/nav2_smac_planner/test/test_a_star.cpp +++ b/nav2_smac_planner/test/test_a_star.cpp @@ -72,7 +72,7 @@ TEST(AStarTest, test_a_star_2d) EXPECT_EQ(num_it, 2414); // check path is the right size and collision free - EXPECT_EQ(path.size(), 81u); + EXPECT_EQ(path.size(), 82u); for (unsigned int i = 0; i != path.size(); i++) { EXPECT_EQ(costmapA->getCost(path[i].x, path[i].y), 0); } @@ -104,7 +104,7 @@ TEST(AStarTest, test_a_star_2d) a_star_2.setStart(20, 20, 0); // valid a_star_2.setGoal(50, 50, 0); // invalid EXPECT_TRUE(a_star_2.createPath(path, num_it, some_tolerance)); - EXPECT_EQ(path.size(), 20u); + EXPECT_EQ(path.size(), 21u); for (unsigned int i = 0; i != path.size(); i++) { EXPECT_EQ(costmapA->getCost(path[i].x, path[i].y), 0); } @@ -164,7 +164,7 @@ TEST(AStarTest, test_a_star_se2) // check path is the right size and collision free EXPECT_EQ(num_it, 3222); - EXPECT_EQ(path.size(), 62u); + EXPECT_EQ(path.size(), 63u); for (unsigned int i = 0; i != path.size(); i++) { EXPECT_EQ(costmapA->getCost(path[i].x, path[i].y), 0); } @@ -225,7 +225,7 @@ TEST(AStarTest, test_a_star_lattice) // check path is the right size and collision free EXPECT_EQ(num_it, 21); - EXPECT_EQ(path.size(), 48u); + EXPECT_EQ(path.size(), 49u); for (unsigned int i = 0; i != path.size(); i++) { EXPECT_EQ(costmapA->getCost(path[i].x, path[i].y), 0); }