[RPP] Jerk limited trajectory generation

[angstrem98]

Feature request

First of all, thanks for an attractive controller. I had a lot of fun using Regulated Pure Pursuit Controller due to it's simplicity to configure and robustness.
It used to have acceleration limiting which did not work properly and has been removed. There is some deceleration near goal implemented but it requires large lookahead values in order to prevent harsh deceleration.
RPP controller should have motion profile generation with limited acceleration and jerk that allows it to smoothly accelerate from standstill and decelerate when approaching the goal.

Old acceleration limiting was done on current robot speed. Classic approach is to apply kinematic limits to commanded velocities and the underlying hardware tracks the reference. This is being done in the PR.

I'm already working on this feature and opening this issue as part of the procedure to open a PR as noted in navigation2 documentation.

Issue #2633 is closely related, but it is an external velocity smoother. We need to know when to start braking inside the controller in order to reach the goal accurately.

[angstrem98]

Comparing before and after results.
Sending custom differential drive robot in Webots from point A to point B with original implementation results in commanded linear velocity as shown on image

Before:

New implementation in the linked Pull Request produced following commanded linear velocity:

After:

---

We can notice that the new commanded velocity has "S" shaped curve - jerk limited.

[angstrem98]

Motion profile generation is done using ruckig package (it is used inside MoveIt2).
Imagine a robot with significant mass driving at moderate/higher speeds towards the goal. We want it to smoothly come to a stop at the goal point. Velocity smoother #2633 could help us to smooth acceleration, but deceleration near goal with current implementation would cause overshoots. Maybe it is not noticeable with TurtleBot which moves at relatively low speeds, but tests with custom robot moving at 0.4 m/s and above showed overshoots with original implementation.

In order to know when to start braking, one ruckig instance works Position mode and tracks remaining path length.
While following the path, another instance works in velocity mode and sets the angular velocity command.
When we need to rotate to path heading or rotate to goal angle, proportional controller is used to achieve accurate angle control. Tests showed easy achievement of 1 degree tolerance in goal checker.

[SteveMacenski]

I wanted to give you a note that this is still on my review queue. I've been slammed the last few weeks and I'm now getting my footing back to review these bigger PRs that have been sitting around.

The reason this has taken longer is that this introduces a new dependency on a new library I need to review myself. This also has ramifications across the code-base for places we similarly have code that acts to smooth out information (like the odometry smoother, kinematic constraints in both DWB and RPP, etc). For consistency's sake, I want them all implemented the same way so there's some additional work that I need to do to make sure this could be reasonably applied to those situations as well and that I actually understand how this is working under the hood to make sure this is a good technology fit. I believe it is, I mostly need to get up to speed.