AC_Fence: support inclusion group fences, also avoid and Dijkstra

[IamPete1]

This is a change that allows you to be inside one OR other of your polygon fences. The current behavior is that you must be inside ALL of them.

https://youtu.be/_IkjcgyQbRI

[IamPete1]

Should probably add some capability for checking destination is inside the same fence the vehicle is in rather than inside another fence.

[peterbarker]

Pretty sure this is something that should be in the mavlink standard rather than our own custom interpretations.

[IamPete1]

@peterbarker something like this? mavlink/mavlink#1510

[peterbarker]

@IamPete1 yes, something like that. How are we off for backwards-compatability in this case, 'though? I think it's fail-safe as older firmware will just fail in the case that you try to define multiple inclusion groups and then try to arm.

I don't really find your reason compelling - a user workflow setting rally points, fences and rally points driven by a GCS or script would be preferable IMO. MAVProxy has a primitive module to set up these things based on your flying location (the "fix all" options is great!).

That being said, I think this would be useful regardless - being able to use both circular and polygonal includes to form a composite shape...

[IamPete1]

@peterbarker The longer term goal of this is to be able to define pathways to get from one fence area to the next. Probably with a new fence way-point type. Dijkstra's path planning can then get you anywhere inside one of your fences knowing there is a set route to follow for some of it. Probably more applicable to a Rover on a farm than a copter.

[peterbarker]

On Mon, 12 Oct 2020, Peter Hall wrote: @peterbarker The longer term goal of this is to be able to define pathways to get from one fence area to the next. Probably with a new fence way-point type. Dijkstra's path planning can then get you anywhere inside one of your fences knowing there is a set route to follow for some of it. Probably more applicable to a Rover on a farm than a copter.

Nope, you completely lost be there.... You'd relax the fence restrictions while transitting and turn them back on? And use a fence point as a waypoint to get inside the next fence?!

[rmackay9]

I think we will hit issues with Copter and Rover avoidance handling the fences. So there will be an inconsistency between a failsafe triggering RTL (or whatever) and stopping at the fence. It will probably still not be possible to transition from one fence area to the other.

[IamPete1]

I will try and explain better, with a diagram and everything.

I have two fences. I then define a link between them as a number of waypoints. The waypoints will be of a new 'fence link' type, yet to be added. With a little rework of Dijkstra's it should 'just work' to travel between fences. Of course this will need some work to disable fence avoidance if on a pre-defined linking path.

That is the end goal, of which this PR is the first small step. This is more for Rovers on farms than copters in the air, but no reason it should not work for both. In that context the fences are fields and the linking paths are gates and tracks. You can just say go to here in this field, and Dijkstras will work out the path. If your in a fence you can do bendy ruler ect. On a link path you have to stay on path and stop.

[rmackay9]

@IamPete1,

This change isn't really necessary though right? The GCS could combine the two fences and connecting path into a single large inclusion polygon. In this way the complexity of the fence creation is being transferred down to the autopilot.

[IamPete1]

@rmackay9 Like this?

The problem with that is you have set the margin and fence size so the vehicle can sneak down the middle. You have a know path you want to take. Much easier just to give the path than work out how to abuse the fence to give that. Also much easier to create several simple fences with linking paths than to do the equivalent thing with a combination a complicated inclusion and several exclusion fences.

[rmackay9]

@IamPete1, yes, this is an equivalent fence that should accomplish the same thing.

I don't see how the avoidance margin and width of the corridor are any more or less of an issue with this method unless the plan is to turn off avoidance while traveling down the corridor which is really not a good idea I think.

This solution is definitely not easier from a flight code point of view. The user difficulties could be handled by the ground station and would result in less corner cases and exceptions in the flight code.

TBH, I'm less and less a fan of this idea as we discuss it.

[rmackay9]

I really don't think we should solve this in the flight code. I think the GCSs should have features to allow converting multiple overlapping inclusion fences into a single polygon inclusion fence.

[IamPete1]

I don't see how the avoidance margin and width of the corridor are any more or less of an issue with this method unless the plan is to turn off avoidance while traveling down the corridor which is really not a good idea I think.

The plan it to turn off avoidance that would take the vehicle off the given path. It would move back and forth along the path but not off it.

The end goal here is actually not to generate the fence in the GCS. The fence will be generated onboard with a lua script with a manually driven fence outline.

A combination of smaller fences is also a win for Dijkstras computation, n^2 + m^2 + i^2 + j^2 << (n+m+i+j)^2

If we take the farm example, I want to add a new field. This new method you just add a new simple fence and a link. Using inclusion and exclusion you have to re-calculate the whole thing.

Also I don't think Dijkstras handles exclusion zones currently you would need that for two linking paths between two areas.

IMHO discounting the future stuff I would like to implement 'Or' fences is a win just because you can leave them all loaded all the time, no need to remember to upload the correct for each location, as in the demo that will 'just work' with the existing avoidance.

[rmackay9]

@IamPete1,

I'm pretty sure that Dijkstra's handles all types of fences (circular inclusion/exclusion, polygon inclusion/exclusion).

I'm very concerned that this small change will have a large impact on object avoidance and that impact isn't really understood. This PR adds a small change but doesn't make the required changes in simple avoidance and pathplanning to use it properly. Best case we will end up with users asking why the "OR" functionality doesn't work properly. Worst case we this will create a large amount of extra development effort before 4.1 can go out to handle this properly.

So I'm afraid I don't think we can merge this until the impacts on simple avoidance and path planning are understood.

[peterbarker]

... you could also just use a mission command to disable the fence between inclusion zones and re-enable it when you're in the next. I'm really not sure of the utility of having a fence if you're just going to disable it when you feel like it - what happens if the vehicle starts to misbehave while the fence is disabled (or, in the case proposed above, between inclusion zones while following magic waypoints...)

[rmackay9]

@peterbarker,

Not sure if I'm misunderstanding your suggestion but inclusions zones are all "AND" at the moment though. So if you're outside of one inclusion zone then you're outside the fence.

[peterbarker]

@rmackay9 sorry, premises weren't clear. I was just musing on the fact that transiting without a fence is odd. Assumed premise was that these fences were sometimes "or"

[IamPete1]

I'm pretty sure that Dijkstra's handles all types of fences (circular inclusion/exclusion, polygon inclusion/exclusion).

Your absolutely correct, I'm out of date.

This is a WIP pr there is still some work to be done to check everything works, I suspect we will also go via the MAVLink route rather than param.

@peterbarker @rmackay9 It seems my linking way points are not popular. How about a 'corridor' fence, defined by two or more points with a width. The difference between this a a normal polygon is that Dijkstras would add the center line to is graph rather than a 'inset' polygon.

[IamPete1]

I actually wonder if 'Or' fences for polygons should be the default, The current 'And' fences any number of polygones can always be simplified to a single polygons with a subset of the original edges. We could just do that when a new one is loaded.

[rmackay9]

@IamPete1,

Re the corridor fence, I think a line and width is really just a rectangle which is a polygon so not a huge fan of this idea.

Switching all inclusion fences to be OR could be OK. It came up during the initial development re which one it should be and there isn't necessarily a right answer, it's more about user expectations and development complexity. Remember we have the tin-can circular fence as well. The wiki currently states that fences are AND. AND is simpler I think because each fence can be treated individually.

[joshanne]

The current implementation is the vehicle must be inside all inclusion fences: https://ardupilot.org/copter/docs/common-polygon_fence.html

That results in a case like this, where the highlighted area is the only area you can fly:

With a change in the logic on fence breach detection, you can draw a bridging polygon between the two and let the AP handle the rest.

The logic becomes: Am I inside at least one inclusion fence rather than Am I inside _ALL_ inclusion fences.

The current implementation also means I need to redraw a fence every time I fly at a new location. My new described OR'd logic means I can define a new fence for each location I fly at, and must remain inside the inclusion fence for that site.

[peterbarker]

On Tue, 13 Oct 2020, James O wrote: The logic becomes: Am I inside at least one inclusion fence rather than Am I inside _ALL_ inclusion fences.

There's a discontinuity in that logic when you have no inclusion fences. Setting of rally points, mission items, circular fence radius and height and mission items when moving between flying sites is a workflow issue and should be handled as a higher-level function. As previously mentioned, MAVProxy includes a module to help co-ordinate this, but it could also be done as a script.

[joshanne]

There's a discontinuity in that logic when you have no inclusion fences.

Perhaps if I worded it better: If Inclusion Fences are present, we must be inside at least one of them.

Setting of rally points, mission items, circular fence radius and height and mission items when moving between flying sites is a workflow issue

An issue of workflow may be the case for my multiple flight site example, but I still think there is confusion.

Given the screenshot above, what use case is there that allows multiple inclusion polygons but limits allowable flight space to only the overlapped region of all polygons? My expectation is I should be able to fly in all of that region.

Perhaps another way to ask is, when would you use multiple polygons overlapped that you couldn't/wouldn't just use a simple polygon. Again, with the screenshot, there's 4 polygons defining a flight area, but the current implementation can only fly in the smaller highlighted area - so why not just draw the small polygon, right?

[joshanne]

ArduPilot Inclusion and Exclusion Fences Wiki: https://ardupilot.org/copter/docs/common-polygon_fence.html

You can define many inclusion and exclusion fences. However, multiple inclusions fences, including the cylindrical fences must overlap, since the vehicle can operate only within the complete overlap area of all of the inclusion fences. Exclusion fences may be placed within or outside of inclusion fences.

Perhaps I need to ask, if multiple areas are safe to fly (ie. overlapping inclusion fences), why limit to the complete overlap?

Edit: I just re-read @rmackay9 's last statement, seems to me the answer I'm looking for is it was simply a design decision at the time.

[joshanne]

After re-reading randy's comment...

@IamPete1 seems like your simplest solution is to change the logic in how the breach check is performed. I believe AC_PolyFence_loader::breached is where you would look for that. You could then draw a bridging inclusion polygon that allows you to go between the two (why not just draw a bigger polygon in the first place? Certainly drawing 3x 4-point polys is simple to visualize) or disable/enable the fence as you transition (but then you are without fence for a few moments). Implementing OR within AC_PolyFence_loader means you don't need to be changing MAVLink and handle the checks internally to ArduPilot.

Side note: I haven't looked to deeply into how AC_Avoid integrates or is affected by this. Given AC_Avoid also performs a loop over the num_inclusion_polygons and does the math, I would say you should be okay to implement breach checking as OR.

[peterbarker]

On Tue, 13 Oct 2020, James O wrote: Perhaps another way to ask is, when would you use multiple polygons overlapped that you couldn't/wouldn't just use a simple polygon. Again, with the screenshot, there's 4 polygons defining a flight area, but the current implementation can only fly in the smaller highlighted area - so why not just draw the small polygon, right?

The intent there was to handle the fences coming from multiple sources, each of which is limiting where you might fly. For example, the user specifying some on a GCS and a companion computer adding one afterwards based on some other logic (e.g. flight authorisation from an airspace authority). Also - not possible to composite a pair of circular inclusion fences into a single circlar fence unless one entirely overlaps the other :-)

[joshanne]

The intent there was to handle the fences coming from multiple sources, each of which is limiting where you might fly. For example, the user specifying some on a GCS and a companion computer adding one afterwards based on some other logic

Okay, understood. Seems like a move to the OR approach would require differentiation of GCS defined fences versus externally provided fences (companion computer provided). ie. I can be within any of the GCS defined inclusion fences, but MUST be within the externally defined ones too.

Also - not possible to composite a pair of circular inclusion fences into a single circlar fence unless one entirely overlaps the other :-)

Simple! Draw a polygon with many points!

[IamPete1]

Great to be having so much discussion here, would be great if you could all have a look over the MAVLink PR, mavlink/mavlink#1510 that will enable this to work in a more intelligent way. That allows the current fence multiple source fences to still work.

[rmackay9]

Yes, good discussion. From my point of view I think it would be good if @IamPete1 (or someone) tried to enhance simple avoidance (including the backaway feature) to work with OR fences. It could be quite challenging.

[IamPete1]

I have added a new fence load function and re-enabled some tests that were randomly disabled due to unrelated MAVProxy version thing. All those except the bendy ruler test pass, I have added that to the disabled test list.

[IamPete1]

rebased to try and get past CI

[IamPete1]

The re-enabled bendy ruler test was getting stuck, so I have disabled it again.

Also rebased.

[tridge]

what happens with return point in plane if you have multiple fences. We need to ensure we have sane behaviour

[IamPete1]

Amazingly this has rebased cleanly on S-Curves.

[rmackay9]

I somehow thought this had gone in but it hasn't. So we had a request for this in the rover-4.5 forums. https://discuss.ardupilot.org/t/change-fence-enable-during-mission/116690