[Nav2] New Smoother Plugin! Savitzky-Golay Smoother

Hi all,

Your friendly neighborhood navigator here!

I wanted announce a brand new hot-hot-hot drop of an algorithm I developed on the plane on the way to ROSCon 2022 :airplane: (and stayed up way too late last night writing the unit tests for it). This was actually motivated by another project I was working on but as a toy example I prototyped it as a Path Smoother and was surprised that it solved an oddity in NavFn (more on that below) so I decided to harden, test, and document it for all to enjoy! It compliments the current set of path smoothers quite well, filling a niche the others have not yet.

I’m happy to announce the Savitzky-Golay Smoother! And this time its based on someone else’s named algorithm, so you can’t blame my bad naming practices :wink: This plugin uses the Savitzky-Golay filter from digital signal processing to remove noise from the reference path.

It is useful for all types of planners, but particularly in NavFn to remove tiny artifacts that can occur near the end of paths or Theta* to slightly soften the transition between Line of Sight line segments without modifying the primary path. It is very fast (<< 0.1ms) so is a recommended default for planners that may result in slight discontinuities that can be modeled as noise. However, it will not smooth out larger scale discontinuities, oscillations, or improve smoothness. For those, use one of the other provided smoother plugins. It also provides estimated orientation vectors of the path points after smoothing, because why not, I have this great utility in nav2_utils I wrote for the Simple Smoother plugin.

This algorithm is deterministic and low-parameter. In the below image, some odd points from NavFn’s (pink) gradient descent are smoothed out by the smoother (purple) in the middle and end of a given path, while otherwise retaining the exact character of the path. Thanks for these great examples @amerzlyakov !


Per usual, I’ve uploaded documentation and has 100% unit test coverage (cuz that’s how ya 'navigator rolls :sunglasses:).


Happy digital-signal-processing,