Skip to content

Commit

Permalink
Merge pull request #27 from hotosm/fix-terrain_following_waylines
Browse files Browse the repository at this point in the history 
Fix terrain following waylines
  • Loading branch information
@nrjadkry
nrjadkry authored Dec 24, 2024
2 parents 9a89061 + 105b4c8 commit 708ac23
Showing 1 changed file with 77 additions and 60 deletions.
137 changes: 77 additions & 60 deletions drone_flightplan/terrain_following_waylines.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,19 +2,18 @@
"""
Convert a terrain following drone waypoint mission to a terrain following wayline mission by removing as many waypoints as can be done without deviating beyond a certain threshold from the desired Altitude Above Ground Level.
"""
import logging

log = logging.getLogger(__name__)

import sys, os
import argparse
import sys
import json
import logging
import argparse
from shapely.geometry import shape
from shapely import distance
from shapely.ops import transform
from pyproj import Transformer

import csv
log = logging.getLogger(__name__)


def extract_lines(plan):
"""
Expand All @@ -32,18 +31,18 @@ def extract_lines(plan):
--------
waylines : list
A list of lists containing waypoints (which are dicts)
"""
# Empty list to contain all of the lines in the flight plan, each line
# defined as a series of points proceeding along the same heading
waylines = []

# Empty list to hold the points of an individual unidirectional line
currentline = []
lastheading = None

for point in plan:
currentheading = point['properties']['heading']
currentheading = point["properties"]["heading"]
# If it's not the first line, and the direction has changed
if lastheading and currentheading != lastheading:
# Yep, changed direction. Add the current line to the list of lines
Expand All @@ -52,8 +51,14 @@ def extract_lines(plan):
currentline = []
currentline.append(point)
lastheading = currentheading

# Add the last line to waylines after the loop ends
if currentline:
waylines.append(currentline)

return waylines


def trim(line, threshold):
"""
Return a wayline flight line, with the first and last point intact
Expand All @@ -77,18 +82,18 @@ def trim(line, threshold):
"""
# Work in meters. Assumes input is in EPSG:4326 (will break if not).
transformer = Transformer.from_crs(4326, 3857, always_xy=True).transform

# Create a set of all points, indexed, in EPSG:3857 (tp=transformed_points)
tp = []
for point in line:
indexedpoint = {}
geom = transform(transformer, shape(point['geometry']))
indexedpoint['index'] = point['properties']['index']
indexedpoint['geometry'] = geom
geom = transform(transformer, shape(point["geometry"]))
indexedpoint["index"] = point["properties"]["index"]
indexedpoint["geometry"] = geom
tp.append(indexedpoint)

# Keeper points (indexes only)we know about (initially first and last)
kp = [tp[0]['index'], tp[-1]['index']]
kp = [tp[0]["index"], tp[-1]["index"]]

# New keeper points after injection of the intermediate points needed
# to maintain consistent AGL
Expand All @@ -110,9 +115,10 @@ def trim(line, threshold):
# is more efficient and safer to use as a filter.
# nkpset = new keeper point set
nkpset = set(nkp)
new_line = [p for p in line if p['properties']['index'] in nkpset]
new_line = [p for p in line if p["properties"]["index"] in nkpset]
return new_line


def inject(kp, tp, threshold):
"""
Add the point furthest from consistent AGL (if over threshold)
Expand Down Expand Up @@ -148,47 +154,45 @@ def inject(kp, tp, threshold):
# the entire list of original waypoints) those needed to keep AGL
# deviation below the threshold
for segment in segments:
fp = segment[0]['geometry']
lp = segment[1]['geometry']
fp = segment[0]["geometry"]
lp = segment[1]["geometry"]
run = distance(fp, lp)
rise = lp.z - fp.z
slope = 0
# If run is zero will get divide by zero error, check first
if run:
slope = rise / run
max_agl_difference = 0
max_agl_difference_point = 0
injection_point = None
points_to_traverse = segment[1]['index'] - segment[0]['index']
points_to_traverse = segment[1]["index"] - segment[0]["index"]

#pointsinfo = []
#pointsinfo.append([segment[0]['index'],fp,fp.z,fp.z,0,0,'first'])
# pointsinfo = []
# pointsinfo.append([segment[0]['index'],fp,fp.z,fp.z,0,0,'first'])

for i in range(1, points_to_traverse):
pt = tp[i]['geometry']
pt = tp[i]["geometry"]
z = round(pt.z, 2)
ptrun = round(distance(fp, pt),2)
expected_z = (round(fp.z + (ptrun * slope),2))
agl_difference = abs(round(z - expected_z,2))
if (agl_difference > max_agl_difference and
agl_difference > threshold):
ptrun = round(distance(fp, pt), 2)
expected_z = round(fp.z + (ptrun * slope), 2)
agl_difference = abs(round(z - expected_z, 2))
if agl_difference > max_agl_difference and agl_difference > threshold:
max_agl_difference = agl_difference
max_agl_difference_point = tp[i]['index']
injection_point = i
#pointsinfo.append([tp[i]['index'], tp[i]['geometry'],expected_z,
# pointsinfo.append([tp[i]['index'], tp[i]['geometry'],expected_z,
# z, ptrun, agl_difference])
#pointsinfo.append([segment[1]['index'],lp,lp.z,lp.z,rise,run,'last'])
# pointsinfo.append([segment[1]['index'],lp,lp.z,lp.z,rise,run,'last'])
if injection_point:
new_segment = [segment[0], tp[injection_point], segment[1]]
for new_point in new_segment:
new_keeperpoints.append(new_point['index'])
#pointsinfo[injection_point].append('injection point')
new_keeperpoints.append(new_point["index"])
# pointsinfo[injection_point].append('injection point')
else:
for point in segment:
new_keeperpoints.append(point['index'])
new_keeperpoints.append(point["index"])

return new_keeperpoints


def waypoints2waylines(injson, threshold):
"""
Remove as many as possible of the waypoints in a Drone Tasking Manager
Expand All @@ -207,23 +211,26 @@ def waypoints2waylines(injson, threshold):
outgeojson : dict
A dict suitable for writing to a GeoJSON file using json.dump
representing the flight plan, pretty much idential
"""
"""
# Copy the header from the input file
# TODO: should copy all other key-value pairs in the input JSON,
# 'type' might not be the only one.
inheader = injson['type']
inplan = injson['features']

# inheader = injson["type"]
inplan = injson["features"]

# Skip the first point which is a dummy waypoint in the middle of
# the flightplan area (for safety ascending to working altitude)
# TODO: this should probably be a parameter as it's not necessarily
# the case that all flight plans will have a dummy first point
lines = extract_lines(inplan[1:])

log.info(f"\nThis flight plan consists of {len(lines)} lines "
f"and {len(inplan)} waypoints.")

log.info(
f"\nThis flight plan consists of {len(lines)} lines "
f"and {len(inplan)} waypoints."
)

features = []
features.append(inplan[0])
for line in lines:
Expand All @@ -233,19 +240,18 @@ def waypoints2waylines(injson, threshold):

sequential_features = []
for idx, feature in enumerate(features):
feature['properties']['index'] = idx
feature["properties"]["index"] = idx
sequential_features.append(feature)
outgeojson = {}

outgeojson['type'] = injson['type']
outgeojson['features'] = sequential_features

outgeojson["type"] = injson["type"]
outgeojson["features"] = sequential_features

log.info(f"The output flight plan consists of {len(features)} waypoints.")

return outgeojson


if __name__ == "__main__":
"""
Remove as many as possible of the waypoints in a Drone Tasking Manager
Expand All @@ -271,35 +277,46 @@ def waypoints2waylines(injson, threshold):

p.add_argument("infile", help="input flight plan as GeoJSON")
p.add_argument("outfile", help="output flight plan as GeoJSON")
p.add_argument("-th", "--threshold", type=float,
help='Allowable altitude deviation in meters', default=5)
p.add_argument("-lo", "--line_output", action="store_true",
help="Output a csv file with individual segment data")
p.add_argument(
"-th",
"--threshold",
type=float,
help="Allowable altitude deviation in meters",
default=5,
)
p.add_argument(
"-lo",
"--line_output",
action="store_true",
help="Output a csv file with individual segment data",
)

a = p.parse_args()

verbose = True # set via argparse
verbose = True # set via argparse

logging.basicConfig(
level="DEBUG" if verbose else "INFO",
format=("%(asctime)s.%(msecs)03d [%(levelname)s] "
"%(name)s | %(funcName)s:%(lineno)d | %(message)s"),
format=(
"%(asctime)s.%(msecs)03d [%(levelname)s] "
"%(name)s | %(funcName)s:%(lineno)d | %(message)s"
),
datefmt="%y-%m-%d %H:%M:%S",
stream=sys.stdout,
)
)

log.info(f"Let's fuckin' goooo!")
log.info("Let's get started.")

injson = json.load(open(a.infile))

#writer = None
#if a.line_output:
# writer = None
# if a.line_output:
# outcsvfile = os.path.splitext(a.infile)[0] + '_lines.csv'
# writer = csv.writer(open(outcsvfile, 'w'))
# writer.writerow(['index', 'point', 'expected z', 'z', 'run',
# 'agl difference','keypoint'])

outgeojson = waypoints2waylines(injson, a.threshold)
with open(a.outfile, 'w') as output_file:

with open(a.outfile, "w") as output_file:
json.dump(outgeojson, output_file)

0 comments on commit 708ac23

Please sign in to comment.