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)
