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 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) 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 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 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]])
from sailing_robot.navigation import Navigation # Load yaml file given in argument input_file = sys.argv[1] yaml_data = yaml.load(file(input_file, 'r'), Loader=yaml.Loader) 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]])
yaml_data = yaml.safe_load(f) waypoints = np.array(yaml_data['wp/table'].values()) origin = [waypoints[:, 0].mean(), waypoints[:, 1].mean()] side_dist = 250 # half of the size of the square in m 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)
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_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)
# Load yaml file given in argument input_file = sys.argv[1] yaml_data = yaml.load(file(input_file, 'r'), Loader=yaml.Loader) output_file = input_file[:-5] + "_gen_area.yaml" wpA = yaml_data['wp/table']['A'] # top left (start line) wpB = yaml_data['wp/table']['B'] # start line (second point) #wpC = yaml_data['wp/table']['C'] #finish line (top) wpD = yaml_data['wp/table']['D'] # finish line (second point) wpE = yaml_data['wp/table']['E'] # top right corner nav = Navigation() wpA_utm = nav.latlon_to_utm(wpA[0], wpA[1]) wpB_utm = nav.latlon_to_utm(wpB[0], wpB[1]) #wpC_utm = nav.latlon_to_utm(wpC[0], wpC[1]) wpD_utm = nav.latlon_to_utm(wpD[0], wpD[1]) wpE_utm = nav.latlon_to_utm(wpE[0], wpE[1]) # Unit vector AB vAB = np.array([wpB_utm[0] - wpA_utm[0], wpB_utm[1] - wpA_utm[1]])/3.0 # Unit vector orth to AB vAB_orth = np.array([-vAB[1], vAB[0]]) vAE = np.array([wpE_utm[0] - wpA_utm[0], wpE_utm[1] - wpA_utm[1]])/6.0 vAB_orth = vAE