fixed start (#3168)

* fixed start

* return true

* fix tests

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;

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

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

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 true;
+}
+
+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

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);
  }