edited includes

map_builder.py

@@ -19,6 +19,9 @@ import csv
 import os
 import glob
 
+from pyproj import Transformer, Proj, transform
+
+
 # import matlab.engine
 
 def trim_extent(x_pts, y_pts, scaled_extent):

waypointGenerator.py

+import math
+from pyproj import Transformer, Proj, transform
+import numpy as np
+import json
+
+
+def point_rotation(origin, pt, ang):
+    # returns the pt rotated about the origin by ang (ang in degrees)
+    c = math.cos(math.radians(ang))
+    s = math.sin(math.radians(ang))
+    # translate to origin
+    pt_temp = [pt[0] - origin[0], pt[1] - origin[1]]
+    pt_spun = [ pt_temp[0]*c - pt_temp[1]*s, pt_temp[0]*s + pt_temp[1]*c ]
+    # translate back to frame
+    pt_spun = [pt_spun[0] + origin[0], pt_spun[1] + origin[1]]
+    return pt_spun
+
+def meters2lat_lon(xs, ys):
+    transformer = Transformer.from_crs(3857, 4326) # meters -> lat lon
+    ll_pts = transformer.transform(xs, ys)
+    return ll_pts
+
+def lat_lon2meters(lats, lons):
+    transformer = Transformer.from_crs(4326, 3857) # lat lon -> meters
+    xy_pts = transformer.transform(lats, lons)
+    return xy_pts
+
+if __name__ == "__main__":
+
+    anchor_point = (37.223125, -80.432731)
+    rotation = 45
+    altitude = 2
+    filename = "musical_drones.json"
+    ap_meters = lat_lon2meters(anchor_point[0], anchor_point[1])
+
+    # xy offsets, in meters, from center point...
+    center_offsets = [
+        [-12, -12],
+        [12, -12],
+        [12, 12],
+        [6, 6],
+        [-6, 6],
+        [-6, -6],
+        [-1, -1],
+        [1, -1],
+        [1, 1],
+        [12, -12],
+    ]
+
+    waypoint_dict = {}
+    waypoint_dict_flip = {}
+    for i, pt in enumerate(center_offsets):
+        rotated_pt = point_rotation([0,0], pt, rotation)
+        rotated_pt_flip = point_rotation([0,0], pt, 180 - rotation)
+        wps = meters2lat_lon(rotated_pt[0] + ap_meters[0], rotated_pt[1] + ap_meters[1])
+        wps_flip = meters2lat_lon(rotated_pt_flip[0] + ap_meters[0], rotated_pt_flip[1] + ap_meters[1])
+
+        waypoint_dict.update({"waypoint_{}".format(i) : [wps[0], wps[1], altitude]})
+        waypoint_dict_flip.update({"waypoint_{}".format(i) : [wps_flip[0], wps_flip[1], altitude]})
+
+    # print(waypoint_dict)
+    robot_dict = {"robot_list" : {"robot_0" : waypoint_dict, "robot_1" : waypoint_dict_flip}}
+
+
+    with open(filename, "w") as f:
+        json.dump(robot_dict, f)
+
+    print(robot_dict)
+
+