Skip to content

fixed start #3168

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
merged 3 commits into from
Sep 6, 2022
Merged

fixed start #3168

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
869e33b
fixed start
jwallace42 Sep 2, 2022
63cf5e2
return true
jwallace42 Sep 2, 2022
6af5560
fix tests
jwallace42 Sep 2, 2022
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
6 changes: 6 additions & 0 deletions nav2_smac_planner/include/nav2_smac_planner/node_lattice.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down
9 changes: 6 additions & 3 deletions nav2_smac_planner/src/node_2d.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Rever to path.size() > 0

Copy link
Contributor Author

@jwallace42 jwallace42 Sep 3, 2022 •
edited
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

So, the path size will always be greater than 0 because we check
if (!this->parent) { return false; }
We will always return at least two points if the current node has a parent. The first one from the while and second from the start.

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Very good point 😄

Apparently my defensive programming is defensive about defense 😆

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

:)

}

} // namespace nav2_smac_planner
11 changes: 8 additions & 3 deletions nav2_smac_planner/src/node_hybrid.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
73 changes: 41 additions & 32 deletions nav2_smac_planner/src/node_lattice.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
8 changes: 4 additions & 4 deletions nav2_smac_planner/test/test_a_star.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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);
}
Expand Down Expand Up @@ -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);
}
Expand Down Expand Up @@ -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);
}
Expand Down Expand Up @@ -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);
}
Expand Down