def test_vector_init(self): coord_sys = coord.CoordinateSystem(upson_hall[0], upson_hall[1]) # try a lat/long initialization vec = coord.Vector(coord_sys, olin_hall[0], olin_hall[1]) self.assertIsNotNone(vec.coordinate_system) self.assertAlmostEqual(vec.latitude, olin_hall[0]) self.assertAlmostEqual(vec.longitude, olin_hall[1]) self.assertAlmostEqual(vec.x, 228.1, 1) self.assertAlmostEqual(vec.y, 197.37, 2) # make sure that the origin is (0,0) orig = coord.Vector(coord_sys, upson_hall[0], upson_hall[1]) self.assertAlmostEqual(orig.x, 0) self.assertAlmostEqual(orig.y, 0) # make a unit vector with a given angle unit_vec = coord.Vector(angle=60) self.assertAlmostEqual(unit_vec.x, 0.5) self.assertAlmostEqual(unit_vec.y, (3**0.5) / 2) self.assertAlmostEqual(unit_vec.magnitude(), 1) # make a vector with a given x and y component xy_vec = coord.Vector(x=1, y=2) self.assertAlmostEqual(xy_vec.x, 1) self.assertAlmostEqual(xy_vec.y, 2)
def collisionAvoidance(buoy_waypoints, boat=boat): """ Returns waypoints list for ideal path (no obstacles) """ orientation = (buoy_waypoints[0] - boat.getPosition().x, buoy_waypoints[1] - boat.getPosition().y) first_direction = (orientation[1], -1 * orientation[0]) first_direction = unitVector(first_direction) dist = 10 (x1, y1) = (buoy_waypoints[0] + dist * first_direction[0], buoy_waypoints[1] + dist * first_direction[1]) first_waypoint = coord.Vector(x=x1, y=y1) second_direction = unitVector(orientation) (x2, y2) = (buoy_waypoints[0] + dist * second_direction[0], buoy_waypoints[1] + dist * second_direction[1]) second_waypoint = coord.Vector(x=x2, y=y2) third_direction = (-1 * orientation[1], orientation[0]) third_direction = unitVector(third_direction) (x3, y3) = (buoy_waypoints[0] + dist * third_direction[0], buoy_waypoints[1] + dist * third_direction[1]) third_waypoint = coord.Vector(x=x3, y=y3) (x4, y4) = (boat.getPosition().x + dist * second_direction[0], boat.getPosition().y + dist * second_direction[1]) fourth_waypoint = coord.Vector(x=x4, y=y4) return [ first_waypoint, second_waypoint, third_waypoint, fourth_waypoint, boat.getPosition() ]
def test_xyDist(self): # test quadrant 1 p1 = coord.Vector(x=1, y=1) p2 = coord.Vector(x=4, y=5) dist = p1.xyDist(p2) self.assertAlmostEqual(dist, 5.0) # test between quadrants p2 = coord.Vector(x=-2, y=-3) dist = p1.xyDist(p2) self.assertAlmostEqual(dist, 5.0)
def test_toUnitVector(self): # test quadrant 1 v = coord.Vector(x=5, y=5) unit_vec = v.toUnitVector() self.assertAlmostEqual(v.angle(), unit_vec.angle()) self.assertAlmostEqual(unit_vec.magnitude(), 1) # test quadrant 3 v = coord.Vector(x=-5, y=-5) unit_vec = v.toUnitVector() self.assertAlmostEqual(v.angle(), unit_vec.angle()) self.assertAlmostEqual(unit_vec.magnitude(), 1)
def test_inverse(self): # test quadrant 1 v = coord.Vector(x=2, y=3) inv = v.inverse() self.assertAlmostEqual(inv.x, -v.x) self.assertAlmostEqual(inv.y, -v.y) # test quadrant 4 v = coord.Vector(x=2, y=-3) inv = v.inverse() self.assertAlmostEqual(inv.x, -v.x) self.assertAlmostEqual(inv.y, -v.y)
def setUp(self): self.waypoints = [(42.444241, 76.481933), (42.446016, 76.484713)] self.nav_controller = nav.NavigationController(self.waypoints, simulation=True) # mock sensor readings - some of these aren't always used self.nav_controller.boat.sensors.wind_direction = 45.0 self.nav_controller.boat.sensors.fix = True self.nav_controller.boat_position = coord.Vector(x=5.0, y=0.0) self.nav_controller.boat.sensors.velocity = coord.Vector(x=0.3, y=0.4) self.nav_controller.boat_to_target = self.nav_controller.current_waypoint.vectorSubtract( self.nav_controller.boat_position)
def test_vectorSubtract(self): # test quadrant 1 p1 = coord.Vector(x=5, y=5) p2 = coord.Vector(x=2, y=3) sub = p1.vectorSubtract(p2) self.assertAlmostEqual(sub.x, 3) self.assertAlmostEqual(sub.y, 2) # test between quadrants p2 = coord.Vector(x=-2, y=-3) sub = p1.vectorSubtract(p2) self.assertAlmostEqual(sub.x, 7) self.assertAlmostEqual(sub.y, 8)
def test_midpoint(self): # test quadrant 1 p1 = coord.Vector(x=1, y=1) p2 = coord.Vector(x=4, y=5) mid = p1.midpoint(p2) self.assertAlmostEqual(mid.x, 2.5) self.assertAlmostEqual(mid.y, 3) # test between quadrants p2 = coord.Vector(x=-4, y=-5) mid = p1.midpoint(p2) self.assertAlmostEqual(mid.x, -1.5) self.assertAlmostEqual(mid.y, -2)
def test_angle(self): v = coord.Vector(x=5, y=5) self.assertAlmostEqual(v.angle(), 45) v = coord.Vector(x=3, y=4) self.assertAlmostEqual(v.angle(), 53.13, 2) v = coord.Vector(x=-5, y=5) self.assertAlmostEqual(v.angle(), 135) v = coord.Vector(x=-5, y=-5) self.assertAlmostEqual(v.angle(), 225) v = coord.Vector(x=5, y=-5) self.assertAlmostEqual(v.angle(), 315)
def test_optAngle(self): # best angle is directly to target on the left side self.nav_controller.boat_to_target = coord.Vector(x=1.0, y=0.0) angle, vmg = self.nav_controller.optAngle(False) # left side self.assertAlmostEqual(angle, 0.0) self.assertAlmostEqual(vmg, 1.0) angle, vmg = self.nav_controller.optAngle(True) # right side self.assertAlmostEqual(angle, 66.0) self.assertAlmostEqual(vmg, np.cos(coord.degToRad(66.0))) # best angle is directly to target on the right side self.nav_controller.boat_to_target = coord.Vector(x=-1.0, y=0.0) angle, vmg = self.nav_controller.optAngle(True) # right side self.assertAlmostEqual(angle, 180.0) self.assertAlmostEqual(vmg, 1.0) angle, vmg = self.nav_controller.optAngle(False) # left side self.assertAlmostEqual(angle, 246.0) self.assertAlmostEqual(vmg, -1.0 * np.cos(coord.degToRad(246.0))) # target is directly upwind (get as close as possible) self.nav_controller.boat_to_target = coord.Vector(x=1.0, y=1.0) angle, vmg = self.nav_controller.optAngle(True) # right side self.assertAlmostEqual(angle, 66.0) self.assertAlmostEqual( vmg, np.sin(coord.degToRad(66.0)) * np.sin(coord.degToRad(45.0)) + np.cos(coord.degToRad(66.0)) * np.cos(coord.degToRad(45.0))) angle, vmg = self.nav_controller.optAngle(False) # left side self.assertAlmostEqual(angle, 24.0) self.assertAlmostEqual( vmg, np.sin(coord.degToRad(24.0)) * np.sin(coord.degToRad(45.0)) + np.cos(coord.degToRad(24.0)) * np.cos(coord.degToRad(45.0))) # target is directly downwind (get as close as possible) self.nav_controller.boat_to_target = coord.Vector(x=-1.0, y=-1.0) angle, vmg = self.nav_controller.optAngle(True) # right side self.assertAlmostEqual(angle, 204.0) self.assertAlmostEqual( vmg, np.sin(coord.degToRad(204.0)) * np.sin(coord.degToRad(225.0)) + np.cos(coord.degToRad(204.0)) * np.cos(coord.degToRad(225.0))) angle, vmg = self.nav_controller.optAngle(False) # left side self.assertAlmostEqual(angle, 246.0) self.assertAlmostEqual( vmg, np.sin(coord.degToRad(246.0)) * np.sin(coord.degToRad(225.0)) + np.cos(coord.degToRad(246.0)) * np.cos(coord.degToRad(225.0)))
def readGPS(self): # use the NMEA parser self.gps_serial_port.open() self.gps_serial_port.reset_input_buffer() for _ in range(5): try: line = self.gps_serial_port.readline().decode('utf-8') except: line = '' nmea_data = nmea.NMEA(line) if nmea_data.status: self.fix = True self.latitude = nmea_data.latitude self.longitude = nmea_data.longitude print("got lat {}, long {}".format(self.latitude, self.longitude)) new_position = coord.Vector(self.coordinate_system, self.latitude, self.longitude) cur_time = time.time() if self.prev_time is None: self.position = new_position self.prev_time = cur_time else: self.velocity = new_position.vectorSubtract(self.position) self.velocity.scale(1.0 / (cur_time - self.prev_time)) self.position = new_position self.prev_time = cur_time self.gps_serial_port.close()
def navigateDetection(self, event=Events.COLLISION_AVOIDANCE): # TODO: modify to implement collision avoidance while self.current_waypoint is not None: time.sleep(2) self.boat.updateSensors() self.boat_position = self.boat.getPosition() (buoy_coords, obst_coords) = Camera.read(self.boat.sensors.yaw, self.boat_position.x, self.boat_position.y) if (buoy_coords is not None & event == Events.SEARCH): # TODO: get buoy pos (buoy_waypoint) buoy_coords = coord.Vector(x=buoy_coords[0], y=buoy_coords[1]) self.current_waypoint = buoy_coords self.waypoints = [buoy_coords] if (obst_coords is not None): obstacle_pos1 = obst_coords # TODO: get obstacle_pos at time t time.sleep(2) snd_read = Camera.read(self.boat.sensors.yaw, self.boat_position.x, self.boat_position.y) obstacle_pos2 = snd_read[0] avoidance_waypoint = assessCollision(obstacle_pos1, obstacle_pos2, 2) if avoidance_waypoint is not None: avoidance_waypoint = coord.Vector(x=avoidance_waypoint[0], y=avoidance_waypoint[1]) self.current_waypoint = avoidance_waypoint self.waypoints.insert(0, avoidance_waypoint) else: if self.boat_position.xyDist( self.current_waypoint) < self.DETECTION_RADIUS: if len(self.waypoints) > 1: self.current_waypoint = self.waypoints[1] del (self.waypoints[0]) else: self.current_waypoint = None del (self.waypoints[0]) break sailing_angle = newSailingAngle(self.boat, self.current_waypoint) self.boat.setServos(sailing_angle)
def collisionWaypoint(time, boat=boat): """ Returns waypoint 2m away to avoid obstacle detected at time time """ angle_deg = (boat.sensors.yaw + 90 - time) while (angle_deg >= 360.0): angle_deg -= 360.0 angle_rad = math.radians(angle_deg) return coord.Vector(boat.getPosition().x + 2 * math.cos(angle_rad), boat.getPosition().y + 2 * math.sin(angle_rad))
def endurance(waypoints, opt_dist, offset): # waypoints in a clockwise order, starting with top left factor = opt_dist / math.sqrt(2) first = coord.Vector(x=waypoints[0].x - factor, y=waypoints[0].y + factor) second = coord.Vector(x=waypoints[1].x + factor, y=waypoints[1].y + factor) third = coord.Vector(x=waypoints[2].x + factor, y=waypoints[2].y - factor) fourth = coord.Vector(x=waypoints[3].x - factor, y=waypoints[3].y - factor) # waypoints placed between corners midpoint1 = first.midpoint(second) first_second = (midpoint1.x, midpoint1.y + offset) midpoint2 = second.midpoint(third) second_third = (midpoint2.x + offset, midpoint2.y) midpoint3 = third.midpoint(fourth) third_fourth = (midpoint3.x, midpoint3.y - offset) midpoint4 = first.midpoint(fourth) fourth_first = (midpoint4.x - offset, midpoint4.y) return [ first, first_second, second, second_third, third, third_fourth, fourth, fourth_first ]
def polar(angle, boat): """Evaluates the polar diagram for a given angle relative to the wind. Args: angle (float): A potential boat heading relative to the absolute wind direction. boat (BoatController): The BoatController (either sim or real) Returns: Vector: A unit boat velocity vector in the global coordinate system. """ # TODO might want to use a less simplistic polar diagram angle = coord.rangeAngle(angle) if (angle > 20 and angle < 160) or (angle > 200 and angle < 340): # put back into global coords return coord.Vector(angle=angle + boat.sensors.wind_direction) return coord.Vector.zeroVector()
def test_newSailingAngle(self): # should turn back toward origin on the right angle = self.nav_controller.newSailingAngle() self.assertAlmostEqual( angle, self.nav_controller.boat_position.inverse().angle()) self.nav_controller.boat_position = coord.Vector(x=-50.0, y=0.0) self.nav_controller.boat_to_target = self.nav_controller.current_waypoint.vectorSubtract( self.nav_controller.boat_position) angle = self.nav_controller.newSailingAngle() self.assertAlmostEqual(angle, 0.0) # beating self.nav_controller.boat_position = coord.Vector(x=-50.0, y=-50.0) self.nav_controller.boat_to_target = self.nav_controller.current_waypoint.vectorSubtract( self.nav_controller.boat_position) angle = self.nav_controller.newSailingAngle() self.assertAlmostEqual(angle, 66.0) # right side is closer to heading self.nav_controller.boat_position = coord.Vector(x=-50.0, y=-50.0) self.nav_controller.boat.sensors.velocity = coord.Vector(x=0.4, y=0.3) self.nav_controller.boat_to_target = self.nav_controller.current_waypoint.vectorSubtract( self.nav_controller.boat_position) angle = self.nav_controller.newSailingAngle() self.assertAlmostEqual(angle, 24.0) # left side is closer to heading # tacking - heading is closer to right, but target is closer to left self.nav_controller.boat_position = coord.Vector(x=-20.0, y=-5.0) self.nav_controller.boat.sensors.velocity = coord.Vector(x=0.3, y=0.4) self.nav_controller.boat_to_target = self.nav_controller.current_waypoint.vectorSubtract( self.nav_controller.boat_position) angle = self.nav_controller.newSailingAngle() self.assertAlmostEqual(angle, 14.0) # tacking - heading is closer to left, but target is closer to right self.nav_controller.boat_position = coord.Vector(x=-5.0, y=-20.0) self.nav_controller.boat.sensors.velocity = coord.Vector(x=0.4, y=0.3) self.nav_controller.boat_to_target = self.nav_controller.current_waypoint.vectorSubtract( self.nav_controller.boat_position) angle = self.nav_controller.newSailingAngle() self.assertAlmostEqual(angle, 76.0)
def test_endurance(self): # waypoints need to be in a clockwise dir, starting top left waypoints = [(0, 25), (45, 25), (45, 0), (0, 0)] midpoints = [(27.5, 25), (45, 12.5), (27.5, 0), (0, 12.5)] offset = 10 opt_dist = 2 self.nav_controller.boat_position = coord.Vector(x=-5, y=10) new_waypoints = self.nav_controller.endurance(waypoints, opt_dist, offset) opt_side = 2 / math.sqrt(2) expected_waypoints = [(-opt_side, 25 + opt_side), (midpoints[0].x, midpoints[0].y + offset), (45 + opt_side, 25 + opt_side), (midpoints[1].x + offset, midpoints[1].y), (45 + opt_side, -opt_side), (midpoints[2].x, midpoints[2].y - offset), (-opt_side, -opt_side), (midpoints[3].x - offset, midpoints[3].y)] self.assertAlmostEqual(new_waypoints, expected_waypoints)
def search(waypoints, scalar=math.pi, constant=100, boat=boat): search_waypoints = [] center_point = waypoints[0] """ entry_point = coord.Vector(center_point.x+100, center_point.y) entry_points = [ coord.Vector(center_point.x-100, center_point.y), coord.Vector(center_point.x, center_point.y+100), coord.Vector(center_point.x, center_point.y-100) ] boat_position = boat.getPosition() for point in entry_points: if (boat_position.xyDist(entry_point) > boat_position.xyDist(point)): entry_point = point search_waypoints.insert(0, entry_point) theta_offset = 0 if (entry_point.x == center_point.x-100): theta_offset = math.pi elif ((entry_point.y == center_point.y+100)): theta_offset = math.pi/2 elif ((entry_point.y == center_point.y-100)): theta_offset = -math.pi/2 """ boat_position = boat.getPosition() theta_offset = math.atan2(boat_position.y - center_point.y, boat_position.x - center_point.x) if (boat_position.x < center_point.x): theta_offset += math.pi entry_point = (100 * math.cos(theta_offset) + center_point.x, 100 * math.sin(theta_offset) + center_point.y) search_waypoints.insert(0, entry_point) for theta in range(1 + theta_offset, 31 + theta_offset, 1): r = constant - scalar * (theta - theta_offset) x = center_point.x + r * math.cos(theta) y = center_point.y + r * math.sin(theta) new_waypoint = coord.Vector(x=x, y=y) search_waypoints.insert(len(search_waypoints), new_waypoint) return search_waypoints
def test_search(self): center_x = 5 center_y = 5 self.nav_controller.boat_position = coord.Vector(x=-150, y=7) center_waypoint = [(center_x, center_y)] waypoints = self.nav_controller.search(center_waypoint) expected_waypoints = [(center_x - 100, center_y), (center_x - 52.333, center_y - 81.504), (center_x + 39, center_y - 85.216), (center_x + 89.669, center_y - 12.782), (center_x + 57.15, center_y + 66.17), (center_x - 23.91, center_y + 80.83), (center_x - 77.918, center_y + 22.675), (center_x - 58.811, center_y - 51.251), (center_x + 10.893, center_y - 74.071), (center_x + 65.351, center_y - 29.559), (center_x + 57.547, center_y + 37.311), (center_x - 0.28, center_y + 65.442), (center_x - 52.573, center_y + 33.429), (center_x - 53.684, center_y - 24.857), (center_x - 7.66, center_y - 55.492), (center_x + 40.169, center_y - 34.385), (center_x + 47.629, center_y + 14.319), (center_x + 12.821, center_y + 44.794), (center_x - 28.692, center_y + 32.631), (center_x - 39.854, center_y - 6.042), (center_x - 15.168, center_y - 33.932), (center_x + 18.637, center_y - 28.469), (center_x + 30.884, center_y + 0.279), (center_x + 14.783, center_y + 23.477), (center_x - 10.436, center_y + 22.279), (center_x - 21.271, center_y + 2.84), (center_x - 11.85, center_y - 13.969), (center_x + 4.434, center_y - 14.515), (center_x + 11.585, center_y - 3.26), (center_x + 6.653, center_y + 5.902), (center_x - 0.887, center_y + 5.683), (center_x - 2.388, center_y + 1.055)] for i in range(len(waypoints)): self.assertAlmostEqual(waypoints[i], expected_waypoints[i])
def precisionNavigation(waypoints, offset=5.0, side_length=50.0, boat=boat): # waypoints:[topleft_buoy, topright_buoy, botleft_buoy, botright_buoy] topleft_buoy = waypoints[0] topright_buoy = waypoints[1] botleft_buoy = waypoints[2] botright_buoy = waypoints[3] start_pos = topleft_buoy.midpoint(topright_buoy) # find inner and outer waypoints on first side of triangle start_point = start_pos end_point = botleft_buoy dist = (start_point.xyDist(end_point)) / 3 point_line = find_inner_outer_points(start_point, end_point, dist, 0) first_waypoint = find_inner_outer_points(point_line, end_point, dist, 1) dist_out = 2 * dist point_line_2 = find_inner_outer_points(start_point, end_point, dist_out, 0) second_waypoint = find_inner_outer_points(point_line_2, end_point, dist_out, -1) # first offset from buoy dist_buoy = (start_point.xyDist(end_point)) / 10 third_waypoint = buoy_offset(start_point, end_point, dist_buoy) # find inner and outer waypoints on second side of triangle start_point = end_point end_point = botright_buoy dist = (start_point.xyDist(end_point)) / 3 point_line = find_inner_outer_points(start_point, end_point, dist, 0) fourth_waypoint = find_inner_outer_points(point_line, end_point, dist, 1) dist_out = 2 * dist point_line_2 = find_inner_outer_points(start_point, end_point, dist_out, 0) fifth_waypoint = find_inner_outer_points(point_line_2, end_point, dist_out, -1) # second offset from buoy dist_buoy = (start_point.xyDist(end_point)) / 10 sixth_waypoint = buoy_offset(start_point, end_point, dist_buoy) # find inner and outer waypoints on third side of triangle start_point = end_point end_point = start_point dist = (start_point.xyDist(end_point)) / 3 point_line = find_inner_outer_points(start_point, end_point, dist, 0) seventh_waypoint = find_inner_outer_points(point_line, end_point, dist, -1) dist_out = 2 * dist point_line_2 = find_inner_outer_points(start_point, end_point, dist_out, 0) eighth_waypoint = find_inner_outer_points(point_line_2, end_point, dist_out, 1) # final waypoint is start_point ninth_waypoint = start_pos # Convert to coord vectors first_waypoint = coord.Vector(x=first_waypoint[0], y=first_waypoint[1]) second_waypoint = coord.Vector(x=second_waypoint[0], y=second_waypoint[1]) third_waypoint = coord.Vector(x=third_waypoint[0], y=third_waypoint[1]) fourth_waypoint = coord.Vector(x=fourth_waypoint[0], y=fourth_waypoint[1]) fifth_waypoint = coord.Vector(x=fifth_waypoint[0], y=fifth_waypoint[1]) sixth_waypoint = coord.Vector(x=sixth_waypoint[0], y=sixth_waypoint[1]) seventh_waypoint = coord.Vector(x=seventh_waypoint[0], y=seventh_waypoint[1]) eighth_waypoint = coord.Vector(x=eighth_waypoint[0], y=eighth_waypoint[1]) ninth_waypoint = coord.Vector(x=ninth_waypoint[0], y=ninth_waypoint[1]) waypoints = [ first_waypoint, second_waypoint, third_waypoint, fourth_waypoint, fifth_waypoint, sixth_waypoint, seventh_waypoint, eighth_waypoint, ninth_waypoint ] return waypoints
def __init__(self, event=None, waypoints=[]): self.DETECTION_RADIUS = 5.0 self.coordinate_system = coord.CoordinateSystem( waypoints[0][0], waypoints[0][1]) self.waypoints = [ coord.Vector(self.coordinate_system, w[0], w[1]) for w in waypoints ] self.boat = boat.BoatController( coordinate_system=self.coordinate_system) self.radio = radio.Radio(9600) self.radio.transmitString("Waiting for GPS fix...\n") # wait until we know where we are while self.boat.sensors.velocity is None: self.boat.sensors.readGPS() # ok if this is blocking self.radio.transmitString( "Established GPS fix. Beginning navigation...\n") # self.current_waypoint = self.waypoints.pop(0) # self.current_waypoint = self.waypoints[desired_fst_waypoint] # TODO: add modified ^ to event algos before each navigate call if event == Events.ENDURANCE: # 7 hrs = 25200 sec exit_before = 25200 start_time = time.time() loop_waypoints = endurance(self.waypoints, opt_dist=10, offset=10) self.current_waypoint = loop_waypoints[3] while (time.time() - start_time < exit_before): self.waypoints = loop_waypoints self.current_waypoint = self.waypoints.pop(0) self.navigate elif event == Events.STATION_KEEPING: # to find an optimal radius, 10 for now exit_before = 300 circle_radius = 10 self.waypoints = stationKeeping(self.waypoints, circle_radius, "ENTRY") self.current_waypoint = self.waypoints.pop(0) self.navigate() # Set timer start_time = time.time() loop_waypoints = stationKeeping(self.waypoints, circle_radius, "KEEP") while time.time() - start_time < exit_before: self.waypoints = loop_waypoints self.current_waypoint = self.waypoints.pop(0) self.navigate() self.waypoints = stationKeeping(self.waypoints, circle_radius, "EXIT") elif event == Events.PRECISION_NAVIGATION: self.waypoints = precisionNavigation(self.waypoints) elif event == Events.COLLISION_AVOIDANCE: self.waypoints = collisionAvoidance(self.waypoints) self.current_waypoint = self.waypoints[0] self.navigateDetection() elif event == Events.SEARCH: self.waypoints = search(self.waypoints) self.current_waypoint = self.waypoints[0] self.navigateDetection(event=Events.SEARCH) self.current_waypoint = self.waypoints.pop(0) self.navigate()
def test_station_keeping(self): # entry- given 4 square waypoints, return: 1) entry point 2) center # corner waypoint order: NW, NE, SE, SW waypoints = [(5, 45), (45, 45), (5, 45), (5, 5)] # West entry self.nav_controller.boat_position = coord.Vector(x=0, y=25) new_waypoints = self.nav_controller.station_keeping( waypoints, 10, "ENTRY") expected_waypoints = [(5, 25), (25, 25)] self.assertAlmostEqual(new_waypoints, expected_waypoints) # North entry self.nav_controller.boat_position = coord.Vector(x=25, y=55) new_waypoints = self.nav_controller.station_keeping( waypoints, 10, "ENTRY") expected_waypoints = [(25, 45), (25, 25)] self.assertAlmostEqual(new_waypoints, expected_waypoints) # East entry self.nav_controller.boat_position = coord.Vector(x=55, y=25) new_waypoints = self.nav_controller.station_keeping( waypoints, 10, "ENTRY") expected_waypoints = [(45, 25), (25, 25)] self.assertAlmostEqual(new_waypoints, expected_waypoints) # South entry self.nav_controller.boat_position = coord.Vector(x=25, y=0) new_waypoints = self.nav_controller.station_keeping( waypoints, 10, "ENTRY") expected_waypoints = [(25, 5), (25, 25)] self.assertAlmostEqual(new_waypoints, expected_waypoints) # keep- given position, yaw, and wind, return: 1) Four 90 degree waypoints to loop # scenario 1- (facing east, optimal angle is 45) self.nav_controller.boat_position = coord.Vector(x=0.0, y=0.0) self.nav_controller.boat.sensors.yaw = 0.0 # ccw path self.nav_controller.boat.sensors.wind_direction = 45.0 new_waypoints = self.nav_controller.station_keeping([], 10, "KEEP") expected_waypoints = [(5 * math.sqrt(2), 5 * math.sqrt(2)), (-5 * math.sqrt(2), 5 * math.sqrt(2)), (-5 * math.sqrt(2), -5 * math.sqrt(2)), (5 * math.sqrt(2), -5 * math.sqrt(2))] self.assertAlmostEqual(new_waypoints, expected_waypoints) # cw path self.nav_controller.boat.sensors.wind_direction = 315 new_waypoints = self.nav_controller.station_keeping([], 10, "KEEP") first_angle = math.radians(45) expected_waypoints = [(5 * math.sqrt(2), -5 * math.sqrt(2)), (-5 * math.sqrt(2), -5 * math.sqrt(2)), (-5 * math.sqrt(2), 5 * math.sqrt(2)), (5 * math.sqrt(2), 5 * math.sqrt(2))] self.assertAlmostEqual(new_waypoints, expected_waypoints) # scenario 2- not facing east(60), non-45 optimal angle (15) self.nav_controller.boat_position = coord.Vector(x=5.0, y=5.0) self.nav_controller.boat.sensors.yaw = 60.0 # ccw path self.nav_controller.boat.sensors.wind_direction = 45.0 new_waypoints = self.nav_controller.station_keeping([], 10, "KEEP") first_angle = math.radians(60 + 45) expected_waypoints = [ (5 + 10 * math.cos(first_angle), 5 + 10 * math.sin(first_angle)), (5 + 10 * math.cos(first_angle + math.pi / 2), 5 + 10 * math.sin(first_angle + math.pi / 2)), (5 + 10 * math.cos(first_angle + math.pi), 5 + 10 * math.sin(first_angle + math.pi)), (5 + 10 * math.cos(first_angle + 3 * math.pi / 2), 5 + 10 * math.sin(first_angle + 3 * math.pi / 2)) ] self.assertAlmostEqual(new_waypoints, expected_waypoints) # cw path self.nav_controller.boat.sensors.wind_direction = 90.0 new_waypoints = self.nav_controller.station_keeping([], 10, "KEEP") first_angle = math.radians(60 - 15) expected_waypoints = [(10 * math.cos(first_angle), 10 * math.sin(first_angle)), (10 * math.cos(first_angle - math.pi / 2), 10 * math.sin(first_angle - math.pi / 2)), (10 * math.cos(first_angle - math.pi), 10 * math.sin(first_angle - math.pi)), (10 * math.cos(first_angle - 3 * math.pi / 2), 10 * math.sin(first_angle - 3 * math.pi / 2))] self.assertAlmostEqual(new_waypoints, expected_waypoints) # exit- 4 waypoints, get closest exit waypoints = [(5, 45), (45, 45), (5, 45), (5, 5)] # center=(25,25) # West exit expected_exit = (-5, 25) self.nav_controller.boat_position = coord.Vector(x=15, y=25) exit_waypoint = self.nav_controller.station_keeping( waypoints, 10, "EXIT") self.assertAlmostEqual(exit_waypoint, expected_exit) # North exit expected_exit = (25, 55) self.nav_controller.boat_position = coord.Vector(x=25, y=35) exit_waypoint = self.nav_controller.station_keeping( waypoints, 10, "EXIT") self.assertAlmostEqual(exit_waypoint, expected_exit) # East exit expected_exit = (55, 25) self.nav_controller.boat_position = coord.Vector(x=35, y=25) exit_waypoint = self.nav_controller.station_keeping( waypoints, 10, "EXIT") self.assertAlmostEqual(exit_waypoint, expected_exit) # South exit expected_exit = (25, -5) self.nav_controller.boat_position = coord.Vector(x=25, y=15) exit_waypoint = self.nav_controller.station_keeping( waypoints, 10, "EXIT") self.assertAlmostEqual(exit_waypoint, expected_exit)
def test_dot(self): v1 = coord.Vector(x=3, y=2) v2 = coord.Vector(x=-2, y=5) dot_prod = v1.dot(v2) self.assertAlmostEqual(dot_prod, 4)
def test_magnitude(self): v = coord.Vector(x=3, y=4) self.assertAlmostEqual(v.magnitude(), 5) v = coord.Vector(x=-3, y=4) self.assertAlmostEqual(v.magnitude(), 5)