fix simple smoother failing during final approach

[rayferric]

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

[SteveMacenski]

Seems very sensible / a good idea

[SteveMacenski]

@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!

[mergify]

@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).

[mergify]

@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).

[mergify]

@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]

CI failed due to unrelated errors in TF2@rolling:

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

[SteveMacenski]

#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]

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.

[mergify]

@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]

Rebase with #4823 for CI

[rayferric]

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

[SteveMacenski]

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

[mergify]

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

[codecov]

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

[SteveMacenski]

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.

[rayferric]

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]

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]

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.

[SteveMacenski]

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