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.

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

fix simple smoother failing during final approach #4817

Merged

Conversation

rayferric
Copy link
Contributor

Basic Info

Info Please fill out this column
Ticket(s) this addresses #4710
Primary OS tested on Ubuntu (osrf/ros:jazzy-desktop-full)
Robotic platform tested on my own custom simulated robot
Does this PR contain AI generated software? No

Description of contribution in a few bullet points

  • modified smoother server's simple_smoother plugin to not throw FailedToSmoothPath when the robot is approaching the end of path

I can alter this behavior if requested. My only requirement for this patch is that no error will be thrown when the path is ending and the last and only segment is too short to be processed, causing segments_smoothed to be zero.

For Maintainers:

  • Check that any new parameters added are updated in docs.nav2.org
  • Check that any significant change is added to the migration guide
  • Check that any new features OR changes to existing behaviors are reflected in the tuning guide
  • Check that any new functions have Doxygen added
  • Check that any new features have test coverage
  • Check that any new plugins is added to the plugins page
  • If BT Node, Additionally: add to BT's XML index of nodes for groot, BT package's readme table, and BT library lists

Copy link
Member

@SteveMacenski SteveMacenski left a comment

Choose a reason for hiding this comment

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

Seems very sensible / a good idea

@SteveMacenski
Copy link
Member

@rayferric two things:

  1. There's a CI test failing from this change. I think you just need to set the size of the path > 1 as an input for an exception that should be thrown
  2. Please sign your commits with DCO (see the job failure below)

Then I can merge!

Copy link
Contributor

mergify bot commented Jan 3, 2025

@rayferric, your PR has failed to build. Please check CI outputs and resolve issues.
You may need to rebase or pull in main due to API changes (or your contribution genuinely fails).

1 similar comment
Copy link
Contributor

mergify bot commented Jan 3, 2025

@rayferric, your PR has failed to build. Please check CI outputs and resolve issues.
You may need to rebase or pull in main due to API changes (or your contribution genuinely fails).

@rayferric rayferric force-pushed the fix-simple-smoother-approach branch from a98e159 to 4e3c80d Compare January 3, 2025 15:33
Copy link
Contributor

mergify bot commented Jan 3, 2025

@rayferric, your PR has failed to build. Please check CI outputs and resolve issues.
You may need to rebase or pull in main due to API changes (or your contribution genuinely fails).

@rayferric rayferric force-pushed the fix-simple-smoother-approach branch from 6cbef08 to f4799fd Compare January 3, 2025 16:18
@rayferric
Copy link
Contributor Author

CI failed due to unrelated errors in TF2@rolling:

image

I have pushed an unrelated commit, use tf2's .hpp includes, in order to see if the CI will go through.

@SteveMacenski
Copy link
Member

#4732 should resolve, I just re-kicked CI on that PR and will merge once passing. You can remove those commits and rebase/pull in main changes.

You have some CI test failures but ... I'm guessing those are a fluke given nothing about your PR should have changed that behavior (or the rolling release that created the geometry2 API issue also broke something else we depend on in RPP or exposed an RPP internal bug). Lets do the #4732 rebase and re-kick CI and see if it comes back again.

@rayferric rayferric force-pushed the fix-simple-smoother-approach branch from f4799fd to d61eed5 Compare January 3, 2025 21:35
@rayferric
Copy link
Contributor Author

rayferric commented Jan 3, 2025

Okay, I've removed the extra commit and rebase-merged the changes from main. Now let's wait until #4732 is merged. Also please see if changes in d61eed5 are acceptable.

Copy link
Contributor

mergify bot commented Jan 3, 2025

@rayferric, your PR has failed to build. Please check CI outputs and resolve issues.
You may need to rebase or pull in main due to API changes (or your contribution genuinely fails).

@SteveMacenski
Copy link
Member

Rebase with #4823 for CI

@rayferric rayferric force-pushed the fix-simple-smoother-approach branch from d61eed5 to dc74e30 Compare January 4, 2025 16:57
@rayferric
Copy link
Contributor Author

Here, I rebased with depreciate_c_headers2. Now waiting for your decision regarding the test failures

@SteveMacenski
Copy link
Member

SteveMacenski commented Jan 7, 2025

Rebase on main, it was merged in with some other fixes. It also looks like your PR has an outstanding git conflict. The git diff here shouldn't include any of the h to hpp changes, that should be provided by main

Copy link
Contributor

mergify bot commented Jan 7, 2025

This pull request is in conflict. Could you fix it @rayferric?

@rayferric rayferric force-pushed the fix-simple-smoother-approach branch from dc74e30 to 4426740 Compare January 7, 2025 09:23
Copy link

codecov bot commented Jan 7, 2025

Codecov Report

All modified and coverable lines are covered by tests ✅

Files with missing lines Coverage Δ
nav2_smoother/src/simple_smoother.cpp 100.00% <100.00%> (ø)

... and 5 files with indirect coverage changes

@rayferric rayferric force-pushed the fix-simple-smoother-approach branch from 855007d to e1742ad Compare January 7, 2025 11:37
Copy link
Member

@SteveMacenski SteveMacenski left a comment

Choose a reason for hiding this comment

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

I think there are a couple of bugs that need to be addressed, but generally looks good to me once resolved. They're pretty small.

nav2_smoother/src/simple_smoother.cpp Show resolved Hide resolved
nav2_smoother/src/simple_smoother.cpp Show resolved Hide resolved
@rayferric rayferric force-pushed the fix-simple-smoother-approach branch 3 times, most recently from 4778bce to e9cc2d9 Compare January 12, 2025 19:39
@rayferric
Copy link
Contributor Author

Hey, in order for us to easily resolve this PR, I've reverted my intrusive changes and fixed the tests so that they pass. Is that alright?

@SteveMacenski
Copy link
Member

SteveMacenski commented Jan 14, 2025

I'm going back to the original ticket to remind myself what we were trying to accomplish since we've gone down a few directions. This code has clearly degraded a bit over time and various contributions and refactoring over its lifetime and I want to think about this more abstractly to what we want to do: final path approaches working well and knowing that path segments in various directions can be of arbitrary length.

I think we need to make a few changes here. The returns for smoothImpl and when we throw are a bit crazy - as you point out.

Within smoothImpl:

  • I think the collision failure case should set path = last_path, update the orientations, and return true. If we're mid-execution, then the previous is known to be collision free & that's as good as we can do. If its the first iteration and the path is already in collision, then we can't do much here. If the user cares, then they'd have goal->check_for_collisions set in which the finally validated path would then throw the path in collision error in the main Smoother Server
  • For this specific type of smoother, we can return a semi-smoothed path (others if not converged are not going to be valid, like those from the Constrained Smoother). So, for its >= max_its_ I think we can warn log and return true. Its not as smooth as it could have been, but its perfectly valid and a user configuration, not a runtime requirement.
  • For maximum duration, I think we can leave that alone as throwing since that's an actual exit failure

Within smooth():

  • Now, all returns from smoothImpl are throwing or return true, so we don't need to pay much attention to the return bool anymore. I think we can remove the segments_smoothed and success logic. And we can change the smoothImpl to be a void type.
  • That would also imply removing the potential to throw FailedToSmoothPath, but looking at the git blame for that, it seems haphazardly implemented (along with the rest of the issues we're removing in this thread). I can't think of a reason to keep it.

I also reviewed savitzky_golay_smoother.cpp to see what changes I wanted to make to be aligned with this to fix other potential messy locations. That one looks good to me and does what I basically just suggested.

@rayferric rayferric force-pushed the fix-simple-smoother-approach branch from e9cc2d9 to 386a8b8 Compare January 17, 2025 16:35
@rayferric
Copy link
Contributor Author

rayferric commented Jan 17, 2025

Hi again,
Thanks for the in-depth review and your well thought out suggestions! Thanks to them, preparing the new commits was a breeze. I'm happy that you decided to remove the overly restrictive error handling. As a bonus, I have included a test case for the end-of-path approach scenario, so that my original issue doesn't reoccur.

Copy link
Member

@SteveMacenski SteveMacenski left a comment

Choose a reason for hiding this comment

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

Sorry that this was such a painful review and iteration process, I promise it'll be better in the future 😉

@SteveMacenski SteveMacenski merged commit 6e1c85e into ros-navigation:main Jan 21, 2025
9 of 10 checks passed
RBT22 pushed a commit to EnjoyRobotics/navigation2 that referenced this pull request Jan 22, 2025
* new test case for end of path approach

Signed-off-by: rayferric <[email protected]>

* modify tests to match the more permissive smoother policy

Signed-off-by: rayferric <[email protected]>

* implement steve's suggestions

Signed-off-by: rayferric <[email protected]>

---------

Signed-off-by: rayferric <[email protected]>
Signed-off-by: RBT22 <[email protected]>
masf7g pushed a commit to quasi-robotics/navigation2 that referenced this pull request Jan 28, 2025
* new test case for end of path approach

Signed-off-by: rayferric <[email protected]>

* modify tests to match the more permissive smoother policy

Signed-off-by: rayferric <[email protected]>

* implement steve's suggestions

Signed-off-by: rayferric <[email protected]>

---------

Signed-off-by: rayferric <[email protected]>
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
None yet
Projects
None yet
Development

Successfully merging this pull request may close these issues.

Simple Smoother Aborts Navigation with "No Segments Smoothed" Error When Approaching End of Path
2 participants