start is now added to the path (#3140)

* start is now added to the path * lint fix
ros-navigation · Aug 24, 2022 · 4f12a60 · 4f12a60
1 parent 104f1e1
commit 4f12a60
Show file tree

Hide file tree

Showing 3 changed files with 6 additions and 6 deletions.
diff --git a/nav2_smac_planner/src/node_2d.cpp b/nav2_smac_planner/src/node_2d.cpp
@@ -154,11 +154,11 @@ 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
@@ -705,12 +705,12 @@ 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
@@ -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
@@ -576,7 +576,7 @@ bool NodeLattice::backtracePath(CoordinateVector & path)
     }
 
     current_node = current_node->parent;
-  }
+  } while (current_node->parent);
 
   return path.size() > 0;
 }