def geofence_draw_callback(self, points):
'''callback from drawing waypoints'''
if len(points) != self.Geofence0["numV"]:
print("Insufficient points in polygon, try drawing polygon again")
print("(Expected %d points, Received %d points)" %
(self.Geofence0["numV"], len(points)))
return
for pts in points:
self.Geofence0['Vertices'].append(pts)
# Adjust fence id to not leave empty spaces
if self.Geofence0['id'] > len(self.fenceList) + len(
self.drawnFenceList):
self.Geofence0['id'] = len(self.fenceList) + len(
self.drawnFenceList)
name = "DraftFence" + str(self.Geofence0['id'])
# Check that the geofence polygon is nice
add_geofence = True
if geofence_check_niceness:
points = self.fence_to_polygon(self.Geofence0["Vertices"])
# Reverse points if not counterclockwise
if not counterclockwise_edges(points):
points.reverse()
self.Geofence0["Vertices"].reverse()
add_geofence = nice_polygon_2D(points, 0.01)
# Add the geofence to the draft list if it is a nice polygon
if add_geofence:
if self.Geofence0['id'] not in self.drawnFenceIds:
self.drawnFenceList.append(self.Geofence0)
else:
self.drawnFenceList[self.drawnFenceIds.index(
self.Geofence0['id'])] = self.Geofence0
self.mpstate.map.remove_object(name)
print("Geofence %d added (draft, confirm with 'geofence upload')" %
(self.Geofence0['id']))
else:
print(
"Geofence not added - Polygon does not meet niceness criteria (must be counterclockwise)"
)
return
# Draw the draft geofence (but do not upload it yet)
points = self.Geofence0['Vertices'][:]
points.append(points[0])
if self.Geofence0['type'] == 0:
gcf = (255, 255, 150)
else:
gcf = (255, 200, 200)
self.mpstate.map.add_object(
mp_slipmap.SlipPolygon(name,
points,
layer=2,
linewidth=2,
colour=gcf))
def GetGeofence(self, filename):
tree = ET.parse(filename)
root = tree.getroot()
for child in root:
id = int(child.get('id'))
type = int(child.find('type').text)
numV = int(child.find('num_vertices').text)
floor = float(child.find('floor').text)
roof = float(child.find('roof').text)
Vertices = []
if (len(child.findall('vertex')) >= numV):
for vertex in child.findall('vertex'):
coord = (float(vertex.find('lat').text),
float(vertex.find('lon').text))
Vertices.append(coord)
points = []
origin = Vertices[0]
for vertex in Vertices[0:numV]:
# Convert from geodetic coordinates to NED coordinates
pt = self.LLA2NED(origin, vertex)
# Reverse North,East coords to match x,y coords used by nice_polygon_2D
pt = (pt[1], pt[0])
points.append(Vector(*pt))
if not counterclockwise_edges(points):
points.reverse()
Vertices.reverse()
if self.nice_geofence(points):
Geofence = {
'id': id,
'type': type,
'numV': numV,
'floor': floor,
'roof': roof,
'Vertices': Vertices[:numV]
}
self.fenceList.append(Geofence)
else:
print(
"Geofence %d not added - Polygon does not meet niceness criteria."
% id)
def GetGeofence(self, filename):
'''add geofences from a file'''
tree = ET.parse(filename)
root = tree.getroot()
for child in root:
id = int(child.get('id'))
type = int(child.find('type').text)
numV = int(child.find('num_vertices').text)
floor = float(child.find('floor').text)
roof = float(child.find('roof').text)
Vertices = []
if (len(child.findall('vertex')) >= numV):
for vertex in child.findall('vertex'):
coord = (float(vertex.find('lat').text),
float(vertex.find('lon').text))
Vertices.append(coord)
# Check geofence niceness
add_geofence = True
if geofence_check_niceness:
points = self.fence_to_polygon(Vertices[0:numV])
if not counterclockwise_edges(points):
points.reverse()
Vertices.reverse()
# Add geofence if polygon is nice
add_geofence = nice_polygon_2D(points, 0.01)
if add_geofence:
Geofence = {
'id': id,
'type': type,
'numV': numV,
'floor': floor,
'roof': roof,
'Vertices': Vertices[:numV]
}
self.fenceList.append(Geofence)
else:
print(
"Geofence %d not added - Polygon does not meet niceness criteria."
% id)