Add Avoidance Plugin to MAVSDK

[coderkalyan]

These are my first thoughts regarding adding an avoidance plugin to MAVSDK.

Requirements: In my mind, as per the rest of MAVSDK, this plugin should be self-contained and therefore not depend on any other frameworks such as ROS. In fact, I don't think that the core Avoidance plugin should even implement any actual avoidance logic; it should just expose a high level API to the MAVLink Path Planning (Trajectory) protocol. This will allow for a flexible way to implement Mission mode obstacle avoidance - see PX4 Obstacle Avoidance for more details. If any avoidance logic for i.e. point cloud via LiDAR is implemented, it should be kept separate.

The two main architectures that come to mind for me are:

1. A "polling" approach - the user calls avoidance.receive_trajectory() which returns a list of waypoints. These waypoints are then altered as needed to "avoid obstacles". Finally, avoidance.upload_trajectory() is called on the new waypoints. This procedure is repeated in a loop at 5Hz.
2. A "subscribing" approach - the avoidance API allows you to subscribe a function as a callback. The existing trajectory is passed into the callback, processed, then outputted in the function return. In this approach, MAVSDK handles registering and triggering the callback, and the user only implements the callback.

Off the bat, the subscribing approach seems cleaner to me. However, there are a few considerations to make before deciding on and implementing an approach:

• Although I'm not sure if it will be performant enough, I would like to leave room for writing gRPC bindings for other languages.
• If considering other languages bindings, callbacks might be implemented differently, so this will have to be figured out.
• Callbacks require the avoidance logic function to be somewhat contained, and if other variables/code are required, it might be hard to reference other code. I'm not sure to what extent this is a problem, as I haven't done much async/callbacks in C++ before.

I'd love to hear people's thoughts to decide on an architecture, so I can start developing it!

[hamishwillee]

Note also the PX4 path planning docs, which pull out the "generic behaviour" from the obstacle avoidance and safe landing implementation.

Off the top of my head the considerations are that:

• The API does not specify sending rates, but PX4 requires some minimums or it will report that the avoidance system is lost. Given that the minimum sending rates are not defined, this may need to be tunable (but should at least be the minimum PX4 expects)
• Polling makes no sense to me given the rates might vary.
• The avoidance system on PX4 is expecting a heartbeat, and may expect that to have the component ID for an avoidance system. That will need to be checked.
• Not every planner handles every mode, and if it does not handle a mode, it is supposed to mirror back the trajectory it receives - this ensures the vehicle just follows the PX4-planned behaviour.
  • It may be that the API should provide a simple interface for determining mode, and for doing that mirroring.
  • It might make sense to make it easy for multiple systems to plugin to the API to handle different modes (but only one per mode).

FMI, what is the need/justification for this. Most of the companion side stuff is handled by ROS already, so I'm not sure why you'd do this unless you already had a good non-ROS library for the avoidance planning.

[coderkalyan]

@hamishwillee as for justification, my use case calls for different sources of obstacle avoidance than simply a point cloud. Furthermore, I am not using ROS in my flight stack for various other reasons. Also, the majority of my code base is written in Python and MAVSDK-Python, so I would like to see if I can implement avoidance in Python as well, since my algorithm requires significantly less processing than a point-cloud approach as the locations of obstacles are already known in real-time (I just need to plot a path around them).

[jkflying]

IMO the polling approach works best. If you stop or delay sending, the vehicle will just slow down and stop, as a safety precaution. For smooth flight the message sending should be at ~15Hz minimum, but that can just be following a predefined path generated in a slower algorithm.

You also need to be smart in how your planning works, and when. For example when landing, you need to use the velocity setpoint for Z, since the altitude where the drone contacts the ground isn't exactly known to the mm.

The polling approach is basically a quick wrapper around the mavlink message. I forsee changes in both the message semantics and how PX4 handles it in the future to provide smoother flight characteristics and not require 15Hz+ send rates.

So from my perspective a simpler implementation that maps closer to the real message will be easier to update for new behavior in the future.

(Note: the mavlink message is still marked WIP/unstable)

[JonasVautherin]

IMO the polling approach works best. If you stop or delay sending, the vehicle will just slow down and stop, as a safety precaution.

In my understanding:

• PX4 repeatedly (at 5Hz) sends TRAJECTORY_REPRESENTATION_WAYPOINTS messages with its planned trajectory. The companion
• CC repeatedly (at 30Hz for the local planner) sends TRAJECTORY_REPRESENTATION_WAYPOINTS messages with the updated trajectory.

Is that correct?

If yes, I think that "polling" and "subscribing" are independent from the safety precaution mentioned above. It seems to me that with the "polling" interface, one would run something like:

while (true) {
    auto desired_trajectory = avoidance.receive_trajectory();
    auto updated_trajectory = compute_updated_trajectory(desired_trajectory);

    send_updated_trajectory(updated_trajectory);
    sleep(<whatever needed to be at 30Hz>);
}

And MAVSDK would have to subscribe to the TRAJECTORY_REPRESENTATION_WAYPOINTS updates and provide them as a "polling interface".

With the "subscribing" interface, MAVSDK would not save any state but it would delegate that to the user. So the user would subscribe_desired_trajectory(<callback>); to get the latest state, and send updated trajectories at the desired rate.

In both cases, the avoidance code should stop sending updated trajectories if something happens (that's the safety measure), and therefore MAVSDK should never "automatically" send TRAJECTORY_REPRESENTATION_WAYPOINTS messages. This is what you meant, @jkflying, right?

I would be more in favor of the "subscribing" interface, because that's more consistent with the way MAVSDK works right now, but I totally agree on the safety measure (if I understood it correctly).

@hamishwillee: I did not get the modes concern. Isn't it correct to say that an OA module using the offboard mode should use the MAVSDK offboard plugin, and one using the mission mode should use this plugin (not sure of the name)? Also, can't we use the TRAJECTORY_REPRESENTATION_WAYPOINTS in other modes, like in position, when doing a goto_location()?

@hamishwillee: I think that the justification may be that it could be more lightweight than using ROS. Like an OA module could be a C++ library, and it could be wrapped in a module using ROS, or in another module using MAVSDK.

[coderkalyan]

Agreed, the main justification is to abstract the OA API from ROS and the actual algorithm.

@JonasVautherin I am also in favor of the subscribing method, although that adds considerable work to the plugin backend.

@JonasVautherin what are your thoughts on how callbacks will work in gRPC and other language bindings?

I am open to both architectures, so somebody will have to let me know what we decide on :wink:

[JonasVautherin]

although that adds considerable work to the plugin backend.

Why is that? I would think that it just "relays" the TRAJECTORY_REPRESENTATION_WAYPOINTS coming from PX4, and nothing more. To me that's the closest to MAVLink, and the simplest one. And it should work well with the language bindings (we already have streams, e.g. for the telemetry).

[coderkalyan]

@JonasVautherin You're right, I forgot that we already have stream capabilities - so it should be pretty simple. Should I try to implement that then?

[JonasVautherin]

I'd like to have @jkflying's opinion again, as I'm not completely sure of my interpretation above :sweat_smile:. But feel free to go ahead and open a draft PR, yes!

[jkflying]

Yep @JonasVautherin that sounds about what I think the simplest implementation would be. Maybe have an .updated() method, which returns true if there is fresh data that hasn't been polled yet? Of course, having a callback that can be registered for when there is new data is also fine, however it could be annoying because it forces you to think about multiple threads even for very simple cases, which the polling doesn't. Maybe have both? :man_shrugging:

Edit: how about the something like receiveTrajectory(bool waitForNew)? That would allow easy polling, and people who want reactive stuff can just block on the call and do it themselves? I'm not sure what is standard for the MAVSDK though...

[jkflying]

Sorry @JonasVautherin I'm tired and didn't grok the whole thread. If it's idiomatic for MAVSDK to have a callback for the message received from the FCU, then that's probably a better way to go. Client can do the buffering on their side. My only concern is that it forces the client to do multithreading from the first line of code, since the callback comes from a different thread.

For the safety mechanism, yes the client (not MAVSDK) should manually trigger every message, so that there is a timeout on the FCU if the algorithm crashes, or a sensor fails, or whatever, because the message stops being sent.

[hamishwillee]

@JonasVautherin

One other consideration. PX4 doesn't use them, but if you need to be "MAVLink compliant" you may need to think about TRAJECTORY_REPRESENTATION_BEZIER handling too.

I did not get the modes concern.

When you have object avoidance turned on in PX4 (COM_OBS_AVOID=1) then in all automatic modes the trajectory interface is used. Specifically PX4 will emit the desired waypoints and expect back the target waypoints. The planner must always send back the target waypoints even if it can't do planning! So for example, the local planner doesn't know how to plan a safe landing, so when in land mode it just mirrors back the desired waypoints from PX4 (which it will follow with very slight delay).

So my point was that it might be worth:

• Making it easy for a planner to work out the mode with just this plugin
• Making it easy for a planner to be able to ignore modes it doesn't support - e.g. by having the plugin automatically mirror back desired waypoints for unsupported modes.

Just an idea.

Isn't it correct to say that an OA module using the offboard mode should use the MAVSDK offboard plugin, and one using the mission mode should use this plugin (not sure of the name)?

Yes. Offboard mode OA is just offboard mode - PX4 doesn't know OA is happening.

Also, can't we use the TRAJECTORY_REPRESENTATION_WAYPOINTS in other modes, like in position, when doing a goto_location()?

No. It is currently only enabled in automatic modes, and goto is part of the offboard mode.

If you want to do something in position mode then you need to look at the collision prevention API. Essentially that just supplies distance sensor data. The "3rd party opportunity" with that API isn't documented yet. I'm having a separate conversation with @jkflying on that - I'm not completely convinced of it, but then I haven't read all his responses to my queries yet.

I think that the justification may be that it could be more lightweight than using ROS. Like an OA module could be a C++ library, and it could be wrapped in a module using ROS, or in another module using MAVSDK.

Thanks. Are you actually planning such a library? I'm a little concerned when we develop APIs that are highly theoretical.

[jkflying]

Thanks. Are you actually planning such a library? I'm a little concerned when we develop APIs that are highly theoretical.

ROS is great for proof of concept, but I think when people want to harden a system they like to simplify it as much as possible, and ROS pulls in too many dependencies to make that 'nice'. I think especially for something like a safety system having the opportunity to use a small, easily auditable C++ interface will be a big boon to anybody trying to eg. get certification.

If you want to do something in position mode then you need to look at the collision prevention API. Essentially that just supplies distance sensor data. The "3rd party opportunity" with that API isn't documented yet. I'm having a separate conversation with @jkflying on that - I'm not completely convinced of it, but then I haven't read all his responses to my queries yet.

About the manual control collision prevention systems, that is an entirely different kettle of fish from this whole conversation, since you don't want the user to get any surprise responses from the drone that they didn't command, but also don't want it to get stuck on trivial obstacles when a few degrees to one side of the requested setpoints would satisfy the safety checks. We spent a fair amount of time tuning this to make it feel 'nice', but I agree that the potential usecases aren't as widespread as the auto-mode interface.

One other consideration. PX4 doesn't use them, but if you need to be "MAVLink compliant" you may need to think about TRAJECTORY_REPRESENTATION_BEZIER handling too.

This will come to PX4 at some point too. Note again, as I said above, both of these messages are marked as WIP in the mavlink spec still.

Mode handling etc

There could be some benefit here, about setting a blacklist of modes you don't want to handle, so it just automatically reflects the setpoints back as they are received. However I'm worried we are coupling a little too tightly to current PX4 behavior of this message, which might change in the future if it gets smarter. I'd prefer to do something minimal for now, and if we see a need for the complex one and we're happy the API is stable, only then do it.

[coderkalyan]

@jkflying @hamishwillee @JonasVautherin Thanks for all the great ideas and considerations. I've been thinking about various designs to account for all/most of the problems listed above. Here's my new design - it's a bit of a compromise, and pulls in a few ideas from the FollowMe plugin as well:

I suggest that we separate receiving TRAJECTORY_REPRESENTATION_WAYPOINTS from the avoidance algorithm's posting. PX4 sends trajectory at a fixed 5Hz. For this reason, I think the most idiomatic API for receiving the trajectory is a callback, similar to the Telemetry plugin or the FollowMe FakeLocationProvider. The user gets the waypoints, and it is up to them what to do with it. This also allows for the implementation of @jkflying's point about TRAJECTORY_REPRESENTATION_BEZIER - it can be another service to subscribe to, in which the callback is returned in bezier format.

The sending of modified/updated trajectory, however, is completely up to the user in terms of frequency, and definitely should not be limited by the 5Hz posting rate of PX4. Uploading trajectory should be a single function which uploads a given trajectory object. This can be run in a loop that clocks at the user's desired frequency based on algorithmic performance/desired vehicle speed.

I'm thinking that to share data between the callback and the trajectory uploader, a custom helper class can be created by the user - something similar to the FakeLocationProvider in the FollowMe example. The trajectory receiving callback updates the vehicle's desired trajectory to this mediator class. The mediator can then handle computing a new trajectory based on vehicle position updates, updates in obstacle locations, etc. The post loop can then just retrieve an updated trajectory from the mediator and upload it to the drone. This allows for the post loop to work independently of whether or not there are new trajectory updates from the drone, even by sending the same trajectory multiple times in a row until new data is received.

I will open a WIP PR and start writing this; I'm still open to suggestions/changes/improvements!

[jayarjo]

Does this thread mean that MAVSDK as of now still doesn't support neither TRAJECTORY_REPRESENTATION_WAYPOINTS nor TRAJECTORY_REPRESENTATION_BEZIER messages?

[julianoes]

Not that I know of. Pull requests are always welcome.

[DronecodeBot]

This issue has been mentioned on Discussion Forum for PX4, Pixhawk, QGroundControl, MAVSDK, MAVLink. There might be relevant details there:

https://discuss.px4.io/t/set-trajectory-setpoint-using-mavsdk/31696/2