Rpp interpolation

[Aposhian]

---

Basic Info

Info	Please fill out this column
Ticket(s) this addresses	#2850
Primary OS tested on	Ubuntu
Robotic platform tested on	Custom robot in Gazebo

---

Description of contribution in a few bullet points

Adds parameter use_lookahead_point_interpolation that interpolates between poses on path to ensure the lookahead point is exactly the desired lookahead distance away from the robot. This leads to smoother commanded velocities for sparse paths.

Description of documentation updates required from your changes

Add parameter docs.

---

Future work that may be required in bullet points

For Maintainers:

• Check that any new parameters added are updated in navigation.ros.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]

LGTM, took me a hot minute and a diagram to get what you were going for, but I understand. Might be worth adding a few comments inline to make it more clear to other users about how this is being modeled. Particularly w.r.t. the 1.0 and -1.0 solutions and how this is setup. Might even be worth adding a link to the comment I left with the diagram so that users looking at the code later have a geometric look at it available

[SteveMacenski]

A diagram would be helpful here as to how this interpolation is happening, but I think I understand the thinking in the diagram I attached below if that's the same as yours.

You have pt i and pt i-1 that you're trying to find the exact intersection point between on the circle of the lookahead radius (pt E). You set Pt i and Pt i-1 as 2 points on a line segment and use the math associated with circle-to-line intersection points to find Pt E.

The couple of assumptions you make is that the line segment roughly approximates the curve of the path. For small distances, that's a good assumption. For further ones in sparse paths, I do wonder if we shouldn't do something more. But if you find that this is good enough, that's fine enough for me. Its basically linear interpolation.

I think there's probably an easier way to do this with just linear interpolation on the line segment, but this works.

[SteveMacenski]

Comment is mostly here for posterity

[Aposhian]

Yes, I can add more documentation and clarity to the code. I just wanted to get this up here before I invested into that 😄 . Yes, you are understanding how it works correctly. As far as whether linear interpolation is good enough vs something more complex, it is good enough to solve my use-case, and it is an incremental improvement for the controller, so I think it is good enough. Beyond linear interpolation I think starts to get into the realm of path planning, but I think we should hold off trying to implement it until it is determined that there is a need.

• Add more documentation/comments about interpolation method

[Aposhian]

I've added some more unit tests to the interpolation method, and it looks like I caught some edge cases that I need to fix.

• Get all unit tests passing for circleSegmentIntersection

[SteveMacenski]

I see you removed the negative solution space, is it true that its always the positive solution, even if the path does a U-turn and the "real" solution is behind the robot pose?

[Aposhian]

I see you removed the negative solution space, is it true that its always the positive solution, even if the path does a U-turn and the "real" solution is behind the robot pose?

While I will verify this with making sure the unit tests pass, I believe it is possible to make the closed form only and always return the intersecting point that is in between the two points. For reference, here is the graph I have been experimenting with for these different formulas, which includes one formula I derived on my own, and one formula used in the page that I linked to in my comment (which is what is implemented in the code now). https://www.desmos.com/calculator/23wsuzzfwo

[Aposhian]

Ok, I think I captured the correct solution now, here: https://www.desmos.com/calculator/xhzoq4wq0r

[SteveMacenski]

That min/max check you had before seemed rational and doesn't take any significant compute time. Even if you don't find the opposite solution, you could at least reject the update to use the raw lookahead point vs. returning a totally wrong-direction-ed point.

Though, that geometry tool you showed is illustrative (you have a bunch of these cool web tools in your pocket!) I think you're probably correct that that solution is generally true. Maybe I'm being overly cautious / concerned if there's a bug later down the line creating a subtle issue. Maybe I'm just being paranoid 😆

I leave it up to you and how confident you are that this will not be a problem in the future.

[Aposhian]

I believe I have addressed all comments. All unit tests are now passing, and I have added an interactive jupyter notebook to the docs folder to illustrate the interpolation algorithm, as well as additional comments. I also have opened the corresponding docs PR to add the parameter use_interpolation.

[SteveMacenski]

Some reason CI jobs have been a little flaky to trigger, just toggled to get CI to run

[SteveMacenski]

As always, I appreciate that you update / add tests for your PRs! It helps quite alot to keep the quality level high. This looks much better (and simpler!) after a round :-)

Waiting on CI