def parse_line(line):
'''
:return a GeoCoord object parsed from the line
:rtype GeoCoord
'''
li = line.split(',')
if (len(li) == 4):
return coordinate.GeoCoord(li[1], li[2], li[3])
elif (len(li) == 3):
return coordinate.GeoCoord(li[1], li[2])
else:
return None
def approximate(pt, pt_guess, dist_target):
new_lat = pt_guess.lat
new_lon = pt_guess.lon
while (True):
dist = dist_between_coord(pt, pt_guess)
error = (dist_target - dist) / dist
if (abs(error) < 0.00001):
return coordinate.GeoCoord(new_lat, new_lon, pt_guess.alt)
else:
d_lat = pt_guess.lat - pt.lat
d_lon = pt_guess.lon - pt.lon
new_lat = (1 + error) * d_lat + pt.lat
new_lon = (1 + error) * d_lon + pt.lon
pt_guess = coordinate.GeoCoord(new_lat, new_lon, pt_guess.alt)
return pt_guess
def coord_with_dist_from_pt(pt, dist, angle):
'''
Approximate the coordinate of the point that is some distance
from the original point,
:param pt:
:param dist:
:param angle: use the pt as the center of the coordinate system,
the angle between the line connect the resulting point and
the origin and the postive X axis
'''
def approximate(pt, pt_guess, dist_target):
new_lat = pt_guess.lat
new_lon = pt_guess.lon
while (True):
dist = dist_between_coord(pt, pt_guess)
error = (dist_target - dist) / dist
if (abs(error) < 0.00001):
return coordinate.GeoCoord(new_lat, new_lon, pt_guess.alt)
else:
d_lat = pt_guess.lat - pt.lat
d_lon = pt_guess.lon - pt.lon
new_lat = (1 + error) * d_lat + pt.lat
new_lon = (1 + error) * d_lon + pt.lon
pt_guess = coordinate.GeoCoord(new_lat, new_lon, pt_guess.alt)
return pt_guess
# Assume in the same hemisphere
assert (angle <= m.pi and angle >= -1.0 * m.pi)
one_m = 0.000009
one_m = 0.000011444
one_m = 0.000010222
initial_lat = pt.lat + m.sin(angle) * dist * one_m
initial_lon = pt.lon + m.cos(angle) * dist * one_m
inital_guess_pt = coordinate.GeoCoord(initial_lat, initial_lon, pt.alt)
return approximate(pt, inital_guess_pt, dist)
def parse_line(line):
li = line.split(',')
coord = coordinate.GeoCoord(li[1], li[2])
if (len(li) == 4):
p = Point(li[0][0], li[0][1:], coord, li[3])
else:
p = Point(li[0][0], li[0][1:], coord)
return p
def center_point(pts_list, alt):
lat = 0
lon = 0
for pt in pts_list:
lat += pt.lat
lon += pt.lon
lat /= len(pts_list)
lon /= len(pts_list)
return coordinate.GeoCoord(lat, lon, alt)
def main():
# Assumed order: waypts, search
filename = 'data.csv'
#(op_area, search_area, waypoints_list) = read_file(filename)
# given a takeoff point
# generate path to go over waypts
# generate search path within search_area
pt1 = coordinate.GeoCoord(38.145278, -76.428889)
pi = math.pi
pt2 = search_path.coord_with_dist_from_pt(pt1, 10.0, pi / 4.0)
print('----------')
pt1 = coordinate.GeoCoord(38.145278, -76.428889, 20)
pt2 = coordinate.GeoCoord(38.145278, -76.428850, 20)
pt3 = coordinate.GeoCoord(38.145278, -76.428820, 20)
pt4 = coordinate.GeoCoord(38.145278, -76.428800, 20)
pt5 = coordinate.GeoCoord(38.145278, -76.428780, 20)
test = mission.Mission()
item1 = mission.MissionItem(pt1, 22)
test.add_mission_item(item1)
item2 = mission.MissionItem(pt2, 16)
test.add_mission_item(item2)
item3 = mission.MissionItem(pt3, 16)
test.add_mission_item(item3)
item4 = mission.MissionItem(pt4, 16)
test.add_mission_item(item4)
item5 = mission.MissionItem(pt5, 21)
test.add_mission_item(item5)
f = open('output.txt', 'w')
f.write(str(test))
f.close()
def main():
# Read the File
filename = 'data.csv'
(op_area, search_area, waypoints_list) = file_parser.read_file(filename)
m1 = mission.Mission()
f1 = mission.Fence()
# Given a takeoff point
takeoff_pt = coordinate.GeoCoord(38.145495, -76.428166, 200)
item1 = mission.MissionItem(takeoff_pt, 22)
m1.add_mission_item(item1)
# Generate path to go over waypts
waypt_path = path.generate_waypt_path(path_info, takeoff_pt,
waypoints_list, op_area)
for pt in waypt_path:
item1 = mission.MissionItem(pt, 16)
m1.add_mission_item(item1)
# Generate search path within search_area
search_area_path = []
#search_area_path = path.generate_search_path(path_info, waypt_path[-1], search_area, op_area, 30)
search_area_path = path.generate_search_path_spiral(
path_info, waypt_path[-1], search_area, op_area, 30)
for pt in search_area_path:
item1 = mission.MissionItem(pt, 16)
m1.add_mission_item(item1)
# Add Landing point, assume to be takeoff point as well
item1 = mission.MissionItem(takeoff_pt, 21)
m1.add_mission_item(item1)
# Generate Fence from the operational area
for pt in op_area:
f1.add_boundary_point(pt)
# Output the mission into file
f = open('output.mission', 'w')
f.write(str(m1))
f.close()
# Output the fence into file
f = open('output.fence', 'w')
f.write(str(f1))
f.close()