Skip to content

Issue 1608 segmentation fault planner server node master #1612

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
Mar 26, 2020
Merged

Issue 1608 segmentation fault planner server node master #1612

Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
a481b65
corrected wrong indexing in NavfnPlanner::smoothApproachToGoal function
Mar 24, 2020
bfbe65b
resolved uncrustify errors
Mar 25, 2020
37552d1
corrected the condition inside NavfnPlanner::smoothApproachToGoal to …
Mar 25, 2020
File filter

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
25 changes: 13 additions & 12 deletions nav2_navfn_planner/src/navfn_planner.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -257,19 +257,20 @@ NavfnPlanner::smoothApproachToGoal(
{
// Replace the last pose of the computed path if it's actually further away
// to the second to last pose than the goal pose.

auto second_to_last_pose = plan.poses.end()[-2];
auto last_pose = plan.poses.back();
if (
squared_distance(last_pose.pose, second_to_last_pose.pose) >
squared_distance(goal, second_to_last_pose.pose))
{
plan.poses.back().pose = goal;
} else {
geometry_msgs::msg::PoseStamped goal_copy;
goal_copy.pose = goal;
plan.poses.push_back(goal_copy);
if (plan.poses.size() >= 2) {
auto second_to_last_pose = plan.poses.end()[-2];
auto last_pose = plan.poses.back();
if (
squared_distance(last_pose.pose, second_to_last_pose.pose) >
squared_distance(goal, second_to_last_pose.pose))
{
plan.poses.back().pose = goal;
return;
}
}
geometry_msgs::msg::PoseStamped goal_copy;
goal_copy.pose = goal;
plan.poses.push_back(goal_copy);
}

bool
Expand Down