How to create a complex geofence with exclusion zones in Python?

For some time now, I’ve been trying to send complex geofences programmatically via Python to a CubeOrange FC. My program was tested with versions 4.4.4 and 4.7.0-dev.
By ‘complex’, I mean geofences that consist of one inclusion zone and one or more smaller exclusion zones inside the inclusion zone.

I’m routing the traffic over mavproxy to see MAVLink responses in a console, using the command
mavproxy.py --out 127.0.0.1:14550 --master /dev/ttyACM0 --map --console

Using examples from Mavproxy’s fence module and for pymavlink, I’ve come up with the following code:

from pymavlink import mavutil, mavwp
import pymavlink.dialects.v20.all as dialect

vehicle = mavutil.mavlink_connection("localhost:14550")

vehicle.wait_heartbeat()
print("Heartbeat from system (system %u component %u)" % (vehicle.target_system, vehicle.target_component))

fence_list = ([51.119594, 6.484470, dialect.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_EXCLUSION],
              [51.080141, 7.678193, dialect.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_EXCLUSION],
              [50.472869, 7.687169, dialect.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_EXCLUSION],
              [50.358485, 6.556273, dialect.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_EXCLUSION]
              )

class Fence():
    
    def add_polygon(self, polygon, command):
        fence_count = 0
        message = dialect.MAVLink_param_request_read_message(target_system=vehicle.target_system,
                                                         target_component=vehicle.target_component,
                                                         param_id=FENCE_TOTAL,
                                                         param_index=PARAM_INDEX)
        vehicle.mav.send(message)
        while True:
            message = vehicle.recv_match(type=dialect.MAVLink_param_value_message.msgname,
                                             blocking=True)
            # convert the message to dictionary
            message = message.to_dict()
            if message["param_id"] == "FENCE_TOTAL":
                print("FENCE_TOTAL parameter original:", message)
                fence_count = int(message["param_value"])
                break
            
        message = dialect.MAVLink_mission_count_message(target_system=vehicle.target_system,
                                                target_component=vehicle.target_component,
                                                count=len(polygon),
                                                mission_type=dialect.MAV_MISSION_TYPE_MISSION)
        vehicle.mav.send(message)
        message = vehicle.recv_match(blocking=True)
        message = message.to_dict()
        print(message)
            
        count = len(polygon)
        for i in range(0, count):
            coord = polygon[i]
            lat = coord[0]
            lon = coord[1]
            
            m = mavutil.mavlink.MAVLink_mission_item_int_message(
                    vehicle.target_system,
                    vehicle.target_component,
                    0,    # seq
                    mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT,    # frame
                    command,    # command
                    0,    # current
                    0,    # autocontinue
                    count, # param1,
                    0.0,  # param2,
                    0.0,  # param3
                    0.0,  # param4
                    int(lat*1e7),  # x (latitude)
                    int(lon*1e7),  # y (longitude)
                    0,                     # z (altitude)
                    mavutil.mavlink.MAV_MISSION_TYPE_FENCE,
                )
            vehicle.mav.send(m)
        vehicle.param_set_send('FENCE_TOTAL', count, 3)
        
    def add_polygon_mission(self, target_locations):
        message = dialect.MAVLink_mission_count_message(target_system=vehicle.target_system,
                                                target_component=vehicle.target_component,
                                                count=len(target_locations),
                                                mission_type=dialect.MAV_MISSION_TYPE_FENCE)

        # send mission count message to the vehicle
        vehicle.mav.send(message)

        message = vehicle.recv_match(blocking=True)
        print(message)
        
        # this loop will run until receive a valid MISSION_ACK message
        while True:
        
            # catch a message
            message = vehicle.recv_match(blocking=True)
        
            # convert this message to dictionary
            message = message.to_dict()
        
            # check this message is MISSION_REQUEST
            if message["mavpackettype"] == dialect.MAVLink_mission_request_message.msgname:
        
                # check this request is for mission items
                if message["mission_type"] == dialect.MAV_MISSION_TYPE_FENCE:
        
                    # get the sequence number of requested mission item
                    seq = message["seq"]
                    
                    print(message)

                    # create mission item int message that contains the home location (0th mission item)
                    coord = target_locations[seq]
                    lat = coord[0]
                    lon = coord[1]
                    message = dialect.MAVLink_mission_item_int_message(target_system=vehicle.target_system,
                                                                       target_component=vehicle.target_component,
                                                                       seq=seq,
                                                                       frame=dialect.MAV_FRAME_GLOBAL,
                                                                       command=coord[2],
                                                                       current=0,
                                                                       autocontinue=0,
                                                                       param1=len(target_locations),
                                                                       param2=0,
                                                                       param3=0,
                                                                       param4=0,
                                                                       x=int(lat*1e7),  # x (latitude)
                                                                       y=int(lon*1e7),  # y (longitude)
                                                                       z=0,
                                                                       mission_type=dialect.MAV_MISSION_TYPE_FENCE)
        
                    # send the mission item int message to the vehicle
                    vehicle.mav.send(message)
        
            # check this message is MISSION_ACK
            elif message["mavpackettype"] == dialect.MAVLink_mission_ack_message.msgname:
        
                # check this acknowledgement is for mission and it is accepted
                if message["mission_type"] == dialect.MAV_MISSION_TYPE_FENCE and \
                        message["type"] == dialect.MAV_MISSION_ACCEPTED:
                    # break the loop since the upload is successful
                    print("Mission upload is successful")
                    break
                elif message["mission_type"] == dialect.MAV_MISSION_TYPE_FENCE:
                    print (message)
        
fence = Fence()
fence.add_polygon_mission(fence_list)

The method add_polygon successfully creates a geofence out of a polygon of arbitrary designation (inclusion or exclusion). However, I have no idea how to pass it multiple polygons or add a new polygon to an existing geofence. Putting inclusion and exclusions polygons into a single list results in the following response:

Got MISSION_ACK: TYPE_FENCE: ERROR
AP: Received incorrect vertex type (want=98 got=97)
AP: Fence validation failed

Does anybody have an idea how this is supposed to work? Am I even on the right track or is there a better way to upload multiple fence polygons programmatically?

I managed to find it out by analyzing what what QGroundControl sends to the vehicle when creating a fence (using Wireshark)

I was essentially right with my previous script, the only difference is parameter 1 in the MAV_CMD_NAV_FENCE_POLYGON_VERTEX_xxx messages. I always set it to the number of all fence items but it has to be set to the number of items in the current fence polygon. So if you have one inclusion polygon with 4 points and one exclusion one with 5, the values for seq and param 1 when sending the points would be:

9/4
9/4
9/4
9/4
9/5
9/5
9/5
9/5
9/5

The following adjusted script takes a list of polygon lists and constructs a fence:

from pymavlink import mavutil, mavwp
import pymavlink.dialects.v20.all as dialect

vehicle = mavutil.mavlink_connection("localhost:14550")

vehicle.wait_heartbeat()
print("Heartbeat from system (system %u component %u)" % (vehicle.target_system, vehicle.target_component))

fence_items = [[[51.080141, 7.678193, dialect.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_INCLUSION, 0],[50.472869, 7.687169, dialect.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_INCLUSION, 0],[50.358485, 6.556273, dialect.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_INCLUSION, 0],[51.119594, 6.484470, dialect.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_INCLUSION, 0]], [[50.8561802, 7.333573, dialect.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_EXCLUSION, 0],[50.8564579, 7.3358493, dialect.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_EXCLUSION, 0],[50.8550949, 7.3360746, dialect.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_EXCLUSION, 0],[50.8554064, 7.3345064, dialect.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_EXCLUSION, 0]]]

class Fence():
    
    def set_fence(self, polygons):
        
        seq = 0
        poly_index = []
        # polygons = polygons[0]
        for polygon in polygons:
            count = len(polygon)
            for point in polygon:
                poly_index.append([count, point])
                seq+=1
        
        message = dialect.MAVLink_mission_count_message(target_system=vehicle.target_system,
                                                target_component=vehicle.target_component,
                                                count=len(poly_index),
                                                mission_type=dialect.MAV_MISSION_TYPE_FENCE)

        # send mission count message to the vehicle
        vehicle.mav.send(message)

        message = vehicle.recv_match(blocking=True)
        print(message)
                
        while True:
        
            # catch a message
            message = vehicle.recv_match(blocking=True)
        
            # convert this message to dictionary
            message = message.to_dict()
        
            # check this message is MISSION_REQUEST
            if message["mavpackettype"] == dialect.MAVLink_mission_request_message.msgname:
        
                # check this request is for mission items
                if message["mission_type"] == dialect.MAV_MISSION_TYPE_FENCE:
        
                    # get the sequence number of requested mission item
                    seq = message["seq"]
                    
                    print(message)

                    # create mission item int message that contains the home location (0th mission item)
                    coord = poly_index[seq][1]
                    vertex_count = poly_index[seq][0]
                    lat = coord[0]
                    lon = coord[1]
                    message = dialect.MAVLink_mission_item_int_message(target_system=vehicle.target_system,
                                                                       target_component=vehicle.target_component,
                                                                       seq=seq,
                                                                       frame=dialect.MAV_FRAME_GLOBAL,
                                                                       command=coord[2],
                                                                       current=0,
                                                                       autocontinue=0,
                                                                       param1=vertex_count,
                                                                       param2=0,
                                                                       param3=0,
                                                                       param4=0,
                                                                       x=int(lat*1e7),  # x (latitude)
                                                                       y=int(lon*1e7),  # y (longitude)
                                                                       z=0,
                                                                       mission_type=dialect.MAV_MISSION_TYPE_FENCE)
        
                    # send the mission item int message to the vehicle
                    vehicle.mav.send(message)
        
            # check this message is MISSION_ACK
            elif message["mavpackettype"] == dialect.MAVLink_mission_ack_message.msgname:
        
                # check this acknowledgement is for mission and it is accepted
                if message["mission_type"] == dialect.MAV_MISSION_TYPE_FENCE and \
                        message["type"] == dialect.MAV_MISSION_ACCEPTED:
                    # break the loop since the upload is successful
                    print("Mission upload is successful")
                    break
                elif message["mission_type"] == dialect.MAV_MISSION_TYPE_FENCE:
                    print (message)

fence = Fence()
fence.set_fence(fence_items)