Skip to content
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

start is now added to the path #3140

Merged
merged 3 commits into from
Aug 23, 2022
Merged

start is now added to the path #3140

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
4f9c9aa
start is now added to the path
jwallace42 Aug 22, 2022
99a4831
lint fix
jwallace42 Aug 23, 2022
2f72423
Merge branch 'main' into add_start_smac
jwallace42 Aug 23, 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
4 changes: 2 additions & 2 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,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;
}
Expand Down
4 changes: 2 additions & 2 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,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;
}
Expand Down
4 changes: 2 additions & 2 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 @@ -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
Expand Down Expand Up @@ -576,7 +576,7 @@ bool NodeLattice::backtracePath(CoordinateVector & path)
}

current_node = current_node->parent;
}
} while (current_node->parent);

return path.size() > 0;
}
Expand Down