From 4f9c9aaa35f03f241ffefbfc43909daf3769813d Mon Sep 17 00:00:00 2001 From: Joshua Wallace Date: Mon, 22 Aug 2022 16:25:22 -0400 Subject: [PATCH 1/2] start is now added to the path --- nav2_smac_planner/src/node_2d.cpp | 3 ++- nav2_smac_planner/src/node_hybrid.cpp | 3 ++- nav2_smac_planner/src/node_lattice.cpp | 3 ++- 3 files changed, 6 insertions(+), 3 deletions(-) diff --git a/nav2_smac_planner/src/node_2d.cpp b/nav2_smac_planner/src/node_2d.cpp index 442fb62b687..4175e8567a6 100644 --- a/nav2_smac_planner/src/node_2d.cpp +++ b/nav2_smac_planner/src/node_2d.cpp @@ -154,11 +154,12 @@ bool Node2D::backtracePath(CoordinateVector & path) NodePtr current_node = this; - while (current_node->parent) { + do { path.push_back( Node2D::getCoords(current_node->getIndex())); current_node = current_node->parent; } + while (current_node->parent); return path.size() > 0; } diff --git a/nav2_smac_planner/src/node_hybrid.cpp b/nav2_smac_planner/src/node_hybrid.cpp index f23b769e6b2..584bb42ce99 100644 --- a/nav2_smac_planner/src/node_hybrid.cpp +++ b/nav2_smac_planner/src/node_hybrid.cpp @@ -705,12 +705,13 @@ bool NodeHybrid::backtracePath(CoordinateVector & path) NodePtr current_node = this; - while (current_node->parent) { + do { 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); return path.size() > 0; } diff --git a/nav2_smac_planner/src/node_lattice.cpp b/nav2_smac_planner/src/node_lattice.cpp index 7e0a917f48c..7d22106ce12 100644 --- a/nav2_smac_planner/src/node_lattice.cpp +++ b/nav2_smac_planner/src/node_lattice.cpp @@ -547,7 +547,7 @@ bool NodeLattice::backtracePath(CoordinateVector & path) const float & grid_resolution = NodeLattice::motion_table.lattice_metadata.grid_resolution; const float pi_2 = 2.0 * M_PI; - while (current_node->parent) { + 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 @@ -577,6 +577,7 @@ bool NodeLattice::backtracePath(CoordinateVector & path) current_node = current_node->parent; } + while (current_node->parent); return path.size() > 0; } From 99a4831979b60a33824cee44c7ecad2bc1994615 Mon Sep 17 00:00:00 2001 From: Joshua Wallace Date: Tue, 23 Aug 2022 10:22:33 -0400 Subject: [PATCH 2/2] lint fix --- nav2_smac_planner/src/node_2d.cpp | 3 +-- nav2_smac_planner/src/node_hybrid.cpp | 3 +-- nav2_smac_planner/src/node_lattice.cpp | 3 +-- 3 files changed, 3 insertions(+), 6 deletions(-) diff --git a/nav2_smac_planner/src/node_2d.cpp b/nav2_smac_planner/src/node_2d.cpp index 4175e8567a6..862b1feca68 100644 --- a/nav2_smac_planner/src/node_2d.cpp +++ b/nav2_smac_planner/src/node_2d.cpp @@ -158,8 +158,7 @@ bool Node2D::backtracePath(CoordinateVector & path) path.push_back( Node2D::getCoords(current_node->getIndex())); current_node = current_node->parent; - } - while (current_node->parent); + } while (current_node->parent); return path.size() > 0; } diff --git a/nav2_smac_planner/src/node_hybrid.cpp b/nav2_smac_planner/src/node_hybrid.cpp index 584bb42ce99..3ccc17ad0bc 100644 --- a/nav2_smac_planner/src/node_hybrid.cpp +++ b/nav2_smac_planner/src/node_hybrid.cpp @@ -710,8 +710,7 @@ bool NodeHybrid::backtracePath(CoordinateVector & path) // Convert angle to radians path.back().theta = NodeHybrid::motion_table.getAngleFromBin(path.back().theta); current_node = current_node->parent; - } - while (current_node->parent); + } while (current_node->parent); return path.size() > 0; } diff --git a/nav2_smac_planner/src/node_lattice.cpp b/nav2_smac_planner/src/node_lattice.cpp index 7d22106ce12..ee61a6df2c8 100644 --- a/nav2_smac_planner/src/node_lattice.cpp +++ b/nav2_smac_planner/src/node_lattice.cpp @@ -576,8 +576,7 @@ bool NodeLattice::backtracePath(CoordinateVector & path) } current_node = current_node->parent; - } - while (current_node->parent); + } while (current_node->parent); return path.size() > 0; }