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_safety_zone():
n = Navigation(safety_zone_ll=[
(50.78, 1.00),
(50.78, 1.04),
(50.82, 1.04),
(50.82, 1.00),
])
n.update_position(DummyNSFMsg(50.8, 1.02))
assert_equal(n.check_safety_zone(), 0)
n.update_position(DummyNSFMsg(50.78001, 1.02))
assert_equal(n.check_safety_zone(), 1)
n.update_position(DummyNSFMsg(50.75, 1.02))
assert_equal(n.check_safety_zone(), 2)
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 setUp(self):
nav = Navigation(beating_angle=45, utm_zone=30)
nav.heading = 0
nav.wind_direction = 225
self.hp = HeadingPlan(nav)
self.hp.waypoint_xy = Point(0, 0)
def setUp(self):
nav = Navigation(beating_angle=45, utm_zone=30)
self.hp = HeadingPlan(nav, waypoint=LatLon(50.7, -0.98),
tack_decision_samples=100,
tack_line_offset=0.01)
self.hp.nav.update_position(DummyNSF(50.7, -1.02))
def test_step(self):
tr = TasksRunner(tasks_def_1, Navigation(utm_zone=30))
tr.start_next_task()
self.assertIsInstance(tr.active_task, HeadingPlan)
tr.start_next_task()
self.assertIsInstance(tr.active_task, StationKeeping)
def test_load_bad(self):
with self.assertRaises(ValueError):
tr = TasksRunner(tasks_bad, Navigation(utm_zone=30))
def test_load(self):
tr = TasksRunner(tasks_def_1, Navigation(utm_zone=30))
self.assertIsInstance(tr.tasks[0], HeadingPlan)
self.assertIsInstance(tr.tasks[1], StationKeeping)
# Load yaml file given in argument
input_file = sys.argv[1]
with open(input_file, 'r') as f:
yaml_data = yaml.safe_load(f)
output_file = input_file[:-5] + "_gen_obstacle.yaml"
margin = 10 # [m]
wp1 = yaml_data['wp/table']['wp1']
wp2 = yaml_data['wp/table']['wp2']
#wp3 = yaml_data['wp/table']['wp3']
wp4 = yaml_data['wp/table']['wp4']
nav = Navigation(utm_zone=UTM_ZONE)
wp1_utm = nav.latlon_to_utm(wp1[0], wp1[1])
wp2_utm = nav.latlon_to_utm(wp2[0], wp2[1])
#wp3_utm = nav.latlon_to_utm(wp3[0], wp3[1])
wp4_utm = nav.latlon_to_utm(wp4[0], wp4[1])
# Unit vector 12
v12 = np.array([wp2_utm[0] - wp1_utm[0], wp2_utm[1] - wp1_utm[1]])
d12 = np.linalg.norm(v12)
v12_unit = v12 / d12
# Unit vector 14
v14 = np.array([wp4_utm[0] - wp1_utm[0], wp4_utm[1] - wp1_utm[1]])
# Load yaml file given in argument
input_file = sys.argv[1]
with open(input_file, 'r') as f:
yaml_data = yaml.safe_load(f)
output_file = input_file[:-5] + "_gen_obstacle.yaml"
margin = 10 # [m]
wp1 = yaml_data['wp/table']['1']
wp0 = yaml_data['wp/table']['0']
#wp3 = yaml_data['wp/table']['wp3']
nav = Navigation()
wp1_utm = nav.latlon_to_utm(wp1[0], wp1[1])
wp0_utm = nav.latlon_to_utm(wp0[0], wp0[1])
# Unit vector 10
v10 = np.array([wp0_utm[0] - wp1_utm[0], wp0_utm[1] - wp1_utm[1]])
d10 = np.linalg.norm(v10)
v10_unit = v10 / d10
print(d10)
# Unit vector orth to 10
v10_orth = np.array([-v10_unit[1], v10_unit[0]])
