Skip to content

Smooth path even if goal pose is so much near to the robot #5423

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 4 commits into from
Aug 7, 2025
Merged

Smooth path even if goal pose is so much near to the robot #5423

Changes from all commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
e5fb8be
Smooth path even if goal pose is so much near to the robot
CihatAltiparmak Aug 6, 2025
9389c65
Apply suggestions
CihatAltiparmak Aug 7, 2025
112a166
Merge branch 'ros-navigation:main' into fix/smooth_segment_which_has_…
CihatAltiparmak Aug 7, 2025
20b0a7a
Remove unnecessary diff
CihatAltiparmak Aug 7, 2025
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
2 changes: 1 addition & 1 deletion nav2_smoother/src/simple_smoother.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -81,7 +81,7 @@ bool SimpleSmoother::smooth(
std::lock_guard<nav2_costmap_2d::Costmap2D::mutex_t> lock(*(costmap->getMutex()));

for (unsigned int i = 0; i != path_segments.size(); i++) {
if (path_segments[i].end - path_segments[i].start > 9) {
Copy link
Member

@SteveMacenski SteveMacenski Aug 7, 2025

Choose a reason for hiding this comment

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

The path segments still need to be large enough to smooth, i.e. at least size 3, if not that size, it cannot be smoothed any more than it is. I think you just need to update 9 to 3 for this case. It was 9 due to copy+pasting from the SG Filter Smoother in the other plugin which does require length > 9 due to its formulation

Copy link
Contributor Author

@CihatAltiparmak CihatAltiparmak Aug 7, 2025 •
edited
Loading

Choose a reason for hiding this comment

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

It's reasonable. But we have to be careful about the cases which path has 2 pose and we does not set the orientation. Instead of removing the limitation, we can always set orientations first before smoothing? I've seen that when smoothing goes wrong, we always set orientation and thow an exception. How does it sounds to you? With these, we can fix SG Filter as well. two birds in one stone.

Copy link
Member

@SteveMacenski SteveMacenski Aug 7, 2025

Choose a reason for hiding this comment

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

I think in #5424 we're discussing putting orientations into NavFn / Theta* itself, so that should handle that problem, no?

Copy link
Contributor Author

@CihatAltiparmak CihatAltiparmak Aug 7, 2025

Choose a reason for hiding this comment

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

I've changed this part as 3 but IMO, it should be 1, it's because end - start + 1 gives you the pose number of segment, not end - start. additionaly, we should smooth the segments having 3 poses. end - start + 1 > 2 goes to end - start > 1 What do you think?

Copy link
Member

@SteveMacenski SteveMacenski Aug 7, 2025

Choose a reason for hiding this comment

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

If we have a path of 1, we can't smooth it. If we have a path of 2, there's nothing to smooth, its always a straight line. Anything less than 3 is meaningless to 'smooth' because its a null operation.

if (path_segments[i].end - path_segments[i].start > 3) {
// Populate path segment
curr_path_segment.poses.clear();
std::copy(
Expand Down
Loading