Add information to path planning re: Bezier curves

[jkflying]

This is a first pass. I'm not really happy with the formatting, but I leave that to the expert :-p

Right now there is nothing available to send the Bezier curves, so there it isn't much more to add yet.

[jkflying]

Whoops, the Bezier isn't available in Offboard (I think). That would need to be clarified as well. It is only available in COM_OBS_AVOID=1 mode.

[hamishwillee]

@jkflying I've done a minor restructure and added a few questions, but bigger picture I still don't understand this.

1. For waypoint trajectory PX4 sends desired path and planner sends back setpoints. What gets sent out by PX4 in order to get bezier setpoints?
2. TRAJECTORY_REPRESENTATION_BEZIER.pos_x is an array of 5 values with description "X-coordinate of starting bezier point, set to NaN if not being used". I'm guessing this an array of X starting points for 5 points? So the description should actually be "X-coordinates of bezier points (index 0 is the start point), set to NaN if not being used"
3. I think we need a diagram because from this description I don't understand the message flow, or what the "start of the curve" means in relations to the timestamp. I envisage PX4 might send a couple of waypoints to the companion. The companion works out a nice curve to that/several points, and gets back a curve that does the first .5s of that. Otherwise much like the waypoint approach.
   I also don't get what "start of the curve" means for first point and how the delta thingy works. It might be that the curve defines a whole path and each message says we're just going to do 0.5seconds travel of that.
   Anyway, this is clearly obvious to you, but I hope you can see that this is not enough to understand what gets sent/received.

[jkflying]

1. For waypoint trajectory PX4 sends desired path and planner sends back setpoints. What gets sent out by PX4 in order to get bezier setpoints?

PX4 still sends back the waypoint trajectories. These are just the next N waypoints in the mission, so nothing changed that side. The difference is that you can now specify from the companion the exact vehicle motion, not just overriding what the next waypoint should be.

2. [TRAJECTORY_REPRESENTATION_BEZIER.pos_x](https://mavlink.io/en/messages/common.html#TRAJECTORY_REPRESENTATION_BEZIER) is an array of 5 values with description "X-coordinate of starting bezier point, set to NaN if not being used". I'm guessing this an array of X starting points for 5 points? So the description should actually be "X-coordinates of bezier points (index 0 is the start point), set to NaN if not being used"

Even better, it should be for X-coordinates of Bezier control points. Bezier curves are funny in that the curves don't actually go through the points you specify, the points are just used to calculate parameters of the curve that is followed. So it's important to specify that they are control points, not just points.

3. I think we need a diagram because from this description I don't understand the message flow, or what the "start of the curve" means in relations to the timestamp. I envisage PX4 might send a couple of waypoints to the companion. The companion works out a nice curve to that/several points, and gets back a curve that does the first .5s of that. Otherwise much like the waypoint approach.
   I also don't get what "start of the curve" means for first point and how the delta thingy works. It might be that the curve defines a whole path and each message says we're just going to do 0.5seconds travel of that.
   Anyway, this is clearly obvious to you, but I hope you can see that this is not enough to understand what gets sent/received.

So this is more about understanding how Bezier curves work, which isn't immediately obvious from how they are parameterized. You specify a bunch of control points, and then a 'fraction traveled', often referred to as t, which shows you where on the curve you are. The only guarantees of the Bezier curve in terms of position, is that you start at the first point when t=0, and end at the last point when t=1.

So the delta here is used to say how long (in time, ie. seconds) it takes to get from the first point to the last point, which in t terms is from t=0 to t=1.

What happens on the flight controller is it checks the current time, compares that to the timestamp the message was sent and the duration of the curve from delta to get t. Say the message was sent 0.1 seconds ago, and the duration is 0.3s, then t would be 0.33333 because it is 1/3 through the duration.

It can then use that t, plus the control points, to get the exact position (and velocity, and acceleration) that the vehicle should have right now to follow the curve.

Clear as mud, I know. Please let me know if / what isn't making sense, so I can explain it better somehow 🙈

[hamishwillee]

1. For waypoint trajectory PX4 sends desired path and planner sends back setpoints. What gets sent out by PX4 in order to get bezier setpoints?

PX4 still sends back the waypoint trajectories. These are just the next N waypoints in the mission, so nothing changed that side. The difference is that you can now specify from the companion the exact vehicle motion, not just overriding what the next waypoint should be.

Just what I needed to know. Restructuring because PX4 doesn't know what trajectory messages it will get back so it always sends out the same waypoints.

That means I think two corrections to your comment above (?)

1. "The difference is that you can now specify from the companion the exact vehicle motion, not just overriding what the next waypoint setpoint should be."
2. It is really it isn't "N" waypoints supplied (except in theory). We know that this is actually fixed at 2 (current and next) from the docs.

[hamishwillee]

FYI, updated definitions for the interface in mavlink and removed WIP. Please check them: mavlink/mavlink#1321

[hamishwillee]

So this is more about understanding how Bezier curves work, which isn't immediately obvious from how they are parameterized ...
... Clear as mud, I know. Please let me know if / what isn't making sense, so I can explain it better somehow

Actually pretty good, though hard to document for externals without saying all this.

What this tells me (that wasn't obvious previously) is that we're only supplying a curve for the next planned time fragment (around 0.5s), and we're supplying it from the point at which we sent the message [i.e. we're not supplying 2 minutes of curve, and telling them to just use some fragment of it].
It also tells me that the planner is sending these at a high enough rate that the next point can be calculated while we're still on the old curve - essentially that we're sending a time-window of the same curve most of the time, modified as we get obstacles.

[hamishwillee]

OK. Updated. I think it is probably good enough to publish (if you could please sanity check).

As an aside, we know that a planner that does not support a particular mode should mirror back the setpoints. What about a planner that does not support missions in mission mode? It is receiving waypoints not setpoints, so mirroring the incoming trajectory will send back the setpoint as the waypoint itself. Is that expected?

[jkflying]

That means I think two corrections to your comment above (?)

1. "The difference is that you can now specify from the companion the exact vehicle motion, not just overriding what the next waypoint setpoint should be."

Correct.

2. It is really it isn't "N" waypoints supplied (except in theory). We know that this is actually fixed at 2 (current and next) from the docs.

Yes, this is a limitation from PX4 side due to the triplet architecture. Perhaps one day it will send more 🙂

OK. Updated. I think it is probably good enough to publish (if you could please sanity check).

I saw a few small things, otherwise this looks good.

As an aside, we know that a planner that does not support a particular mode should mirror back the setpoints. What about a planner that does not support missions in mission mode? It is receiving waypoints not setpoints, so mirroring the incoming trajectory will send back the setpoint as the waypoint itself. Is that expected?

Everything in auto mode works on either position waypoints or velocity setpoints. We've been abusing that on the avoidance side by sending waypoints that get updated in location each message to try to fool the FCU into following the path we want, so the Bezier is just a way of doing this 'properly' instead of working around the old interface. However, if you mirror back exactly what is being sent from the FCU, the vehicle should behave exactly as it does with COM_OBS_AVOID=0 ie. turned off. The only difference I can think of is that it might take an extra few milliseconds to 'accept' a waypoint and start going to the next one, due to comms delays. So if you don't support a certain mode in the planner, just mirror back the waypoint trajectory and it will behave 'normally', ie. as it would if the avoidance was off.

[hamishwillee]

Thanks @jkflying - Doubtless we could iterate forever, but I think this is more than good enough for people to understand what they need to do.