def test_utm_latlon_conversion():
n = Navigation(utm_zone=30)
lat = 50.927482
lon = -1.408787
x, y = n.latlon_to_utm(lat, lon)
ll = n.utm_to_latlon(x, y)
assert_almost_equal(ll.lat.decimal_degree, lat)
assert_almost_equal(ll.lon.decimal_degree, lon)
def test_utm_latlon_conversion():
n = Navigation(utm_zone=30)
lat = 50.927482
lon = -1.408787
x, y = n.latlon_to_utm(lat, lon)
ll = n.utm_to_latlon(x, y)
assert_almost_equal(ll.lat.decimal_degree, lat)
assert_almost_equal(ll.lon.decimal_degree, lon)
def leg_wp(wpA, wpB):
'''
Generate 2 waypoints on the leg between wpA and wpB
'''
nav = Navigation()
wpA_utm = nav.latlon_to_utm(wpA[0], wpA[1])
wpB_utm = nav.latlon_to_utm(wpB[0], wpB[1])
vAB = np.array([wpB_utm[0] - wpA_utm[0], wpB_utm[1] - wpA_utm[1]])
AB = np.linalg.norm(vAB)
if CLOCKWISE:
vAB_orth = np.array([-vAB[1], vAB[0]]) / AB
else:
vAB_orth = np.array([vAB[1], -vAB[0]]) / AB
wpA_p_utm = wpA_utm + vAB_orth * RADIUS
wpB_p_utm = wpB_utm + vAB_orth * RADIUS
wpA_p = nav.utm_to_latlon(wpA_p_utm[0], wpA_p_utm[1])
wpB_p = nav.utm_to_latlon(wpB_p_utm[0], wpB_p_utm[1])
return wpA_p, wpB_p
def leg_wp(wpA, wpB):
'''
Generate 2 waypoints on the leg between wpA and wpB
'''
nav = Navigation()
wpA_utm = nav.latlon_to_utm(wpA[0], wpA[1])
wpB_utm = nav.latlon_to_utm(wpB[0], wpB[1])
vAB = np.array([wpB_utm[0] - wpA_utm[0], wpB_utm[1] - wpA_utm[1]])
AB = np.linalg.norm(vAB)
if CLOCKWISE:
vAB_orth = np.array([-vAB[1], vAB[0]])/AB
else:
vAB_orth = np.array([vAB[1], -vAB[0]])/AB
wpA_p_utm = wpA_utm + vAB_orth*RADIUS
wpB_p_utm = wpB_utm + vAB_orth*RADIUS
wpA_p = nav.utm_to_latlon(wpA_p_utm[0], wpA_p_utm[1])
wpB_p = nav.utm_to_latlon(wpB_p_utm[0], wpB_p_utm[1])
return wpA_p, wpB_p
location = os.path.basename(input_file).split("_")[0]
output_file_name = os.path.join(
image_dir,
str(origin[0]) + '_' + str(origin[1]) + "_" + str(side_dist) + "_" +
location + ".png")
nav = Navigation()
origin_utm = nav.latlon_to_utm(origin[0], origin[1])
minx = -side_dist
maxx = +side_dist
miny = -side_dist
maxy = +side_dist
SO_corner = nav.utm_to_latlon(origin_utm[0] + minx, origin_utm[1] + miny)
NE_corner = nav.utm_to_latlon(origin_utm[0] + maxx, origin_utm[1] + maxy)
image_map = smopy.Map((float(SO_corner.lat), float(
SO_corner.lon), float(NE_corner.lat), float(NE_corner.lon)),
z=18,
maxtiles=302,
margin=0)
mapminx, mapminy = image_map.to_pixels(
(float(SO_corner.lat), float(SO_corner.lon)), )
mapmaxx, mapmaxy = image_map.to_pixels(
(float(NE_corner.lat), float(NE_corner.lon)), )
mapminx = int(mapminx)
wp_list_bot.append(wp_list_bot[-1] + vAB)
for wp_idx_hor in range(2):
wp_list_bot.append(wp_list_bot[-1] + dir*vAB_orth)
dir = -dir
wp_list_bot.pop(0) # to remove the first wp (only there because it was easier to loop like that...)
#### Finish line
wp_finish = [wpA_utm + vAB*5 + vAB_orth/2, wpA_utm + vAB*5 - vAB_orth/2]
wp_finish = [wpD_utm - vAB + vAB_orth/2, wpD_utm - vAB - vAB_orth/2] # uncomment if wpD is correctly set
# wp_finish = [wpC_utm + vAB + vAB_orth/2, wpC_utm + vAB - vAB_orth/2] # uncomment if wpC is correctly set
wp_list = wp_start + wp_list_top + wp_list_bot + wp_finish
wp_latlon_list = [ nav.utm_to_latlon(wp[0], wp[1]) for wp in wp_list]
wp_table = {}
wp_tasks = []
idx = 1
for wp in wp_latlon_list:
wp_table[str(idx)] = [float(wp.lat), float(wp.lon)]
wp_tasks.append({"kind": "to_waypoint", "waypoint": str(idx)})
[float(wp.lat), float(wp.lon)]
idx += 1
#wp_idx_list = [ str(i+1) for i in range(idx-1)]
#yaml_data['wp/list'] = wp_idx_list
yaml_data['wp/tasks'] = wp_tasks
yaml_data['wp/table'] = wp_table
