def vertex_list():
from searchspace.geometry import Point
vertices = list()
vertices.append(Point(1, 5))
vertices.append(Point(4, 5))
vertices.append(Point(4, 1))
vertices.append(Point(1, 1))
return vertices
def test_construct_from_two_points():
"""test_construct_from_two_points
"""
from searchspace.geometry import Point, Line
p1 = Point(0, 0)
p2 = Point(1, 0)
l = Line.construct_from_two_points(p1, p2)
assert l.slope == 0
assert l.y_intercept == 0
assert l.x is None
def test_points_distance():
"""test_points_distance()
"""
from searchspace.geometry import Point
from searchspace.geometry import points_distance
p1 = Point(1, 1)
p2 = Point(1, 15)
p3 = Point(1, 1)
assert points_distance(p1, p1) == 0.0
assert points_distance(p1, p3) == 0.0
assert points_distance(p1, p2) == 14.0
def boundary_vertices():
"""boundary_vertices
Fixture to provide search boundary vertices.
"""
from searchspace.geometry import Point
boundary_vertices = list()
boundary_vertices.append(Point(18.1, -37.1))
boundary_vertices.append(Point(18.1, -37.0))
boundary_vertices.append(Point(18.0, -37.0))
boundary_vertices.append(Point(18.0, -37.1))
return boundary_vertices
def _starting_waypt():
"""_starting_waypt()
Retrieve the starting position UTM coordinates from
the config file and return the position as a Point.
"""
auv_position = config['starting']['auv_position_utm'].split(',')
logging.debug('_starting_waypt(): Setting AUV position {0}'.format(
auv_position))
return Point(float(auv_position[0]),
float(auv_position[1]))
def test_formula_from_points():
"""test_formula_from_points
"""
from searchspace.geometry import formula_from_points, Point
p1 = Point(0, 0)
p2 = Point(1, 0)
slope, y_intercept, x = formula_from_points(p1, p2)
assert slope == 0
assert y_intercept == 0
assert x is None
p2 = Point(0, 1)
slope, y_intercept, x = formula_from_points(p1, p2)
assert slope == float("inf")
assert y_intercept == 0
assert x == 0
p1 = Point(1, 0)
p2 = Point(1, 0)
slope, y_intercept, x = formula_from_points(p1, p2)
assert slope == float("inf")
assert y_intercept is None
assert x == 1
p1 = Point(0, 0)
p2 = Point(1, 1)
slope, y_intercept, x = formula_from_points(p1, p2)
assert slope == 1
assert y_intercept == 0
assert x is None
p1 = Point(0, 0)
p2 = Point(1, -1)
slope, y_intercept, x = formula_from_points(p1, p2)
assert slope == -1
assert y_intercept == 0
assert x is None
def test_point_is_inside(vertex_list):
"""test_point_is_inside
"""
from searchspace.geometry import Point, Polygon
inside_pt = Point(3, 4)
outside_pt = Point(5, 7)
on_the_edge_pt = Point(4, 3)
rectangle = Polygon(vertex_list)
assert rectangle.point_is_inside(inside_pt)
assert not rectangle.point_is_inside(outside_pt)
assert rectangle.point_is_inside(on_the_edge_pt)
polygon = Polygon([Point(1, 5), Point(4, 5), Point(3, 1), Point(2, 1)])
outside_pt = Point(1, 1)
assert polygon.point_is_inside(inside_pt)
assert not polygon.point_is_inside(outside_pt)
def geo_to_cartesian(self, geo_position):
"""geo_to_cartesian()
Convert the lat and lon from position to x and y
in the local area. Cartesian (0, 0) is equivalent
to Geo (_west_lon, _south_lat).
"""
lat = geo_position[0]
lon = geo_position[1]
if (lat < self._south_lat or
lat > self._north_lat or
lon < self._west_lon or
lon > self._east_lon):
raise Exception('Position is outside the defined area.')
x = lon_degrees_to_meters(lon - self._west_lon, self._center_lat)
y = lat_degrees_to_meters(lat - self._south_lat, self._center_lat)
return Point(x, y)
def test_construct_from_heading():
"""test_construct_from_heading
"""
from searchspace.geometry import Point, Line
p1 = Point(0, 1)
l = Line.construct_from_heading(p1, 90)
assert l.slope == 0
assert l.y_intercept == 1
assert l.x is None
l = Line.construct_from_heading(p1, 0)
assert l.slope == float("inf")
assert l.y_intercept == 0
assert l.x is 0
l = Line.construct_from_heading(p1, 45)
assert l.slope == pytest.approx(1.0, rel=0.0000001)
assert l.y_intercept == 1
assert l.x is None
def test_inside_the_boundaries(monkeypatch):
"""test_inside_the_boundaries()
"""
from searchspace.searchspace import SearchSpace
from searchspace.geometry import Point, Polygon
s = SearchSpace()
monkeypatch.setattr(
s, '_boundary_polygon',
Polygon(
[Point(10, 300),
Point(100, 300),
Point(100, 100),
Point(10, 100)]))
inside_pt = Point(80, 150)
outside_pt = Point(80, 310)
on_the_edge = Point(99, 150)
assert s._inside_the_boundaries(inside_pt)
assert not s._inside_the_boundaries(outside_pt)
assert s._inside_the_boundaries(on_the_edge)
def move_toward_waypoint(self, waypoint):
"""move_toward_waypoint()
Compare the current position of the AUV to the given
waypoint. Calculate the distance and the bearing to
the waypoint. If either is greater than the tolerance
parameters, calculate the appropriate amounts to adjust
the heading, depth, and speed of the AUV and effect
those adjustments. Return 'MORE' to indicate the AUV
is not yet close enough.
Otherwise, if the AUV is within the tolerances for
the waypoint, return 'DONE'.
"""
self._current_waypoint['x'] = waypoint[0]
self._current_waypoint['y'] = waypoint[1]
self._current_waypoint['depth'] = waypoint[2]
# Ignore waypoint[3]
if self.distance_to_waypoint() > float(config['auv']['distance_tolerance']):
logging.debug('move_toward_waypoint(): Moving toward the waypoint')
bearing = bearing_to_point(
Point(self._auv_data[config['variables']['easting_x']],
self._auv_data[config['variables']['northing_y']]),
Point(self._current_waypoint['x'],
self._current_waypoint['y']))
self.auv_control.publish_variable(
config['variables']['set_heading'],
bearing,
-1)
self.auv_control.publish_variable(
config['variables']['set_depth'],
self._current_waypoint['depth'] - self.altitude_safety(),
-1)
self.auv_control.publish_variable(
config['variables']['set_speed'],
float(config['auv']['max_speed']),
-1)
return 'MORE'
elif (abs((self._current_waypoint['depth'] - self.altitude_safety()) -
self._auv_data[config['variables']['depth']]) >
float(config['auv']['depth_tolerance'])):
logging.debug('move_toward_waypoint(): Moving toward the depth')
heading = self.turn_toward_heading(
int(self._auv_data[config['variables']['heading']]),
int(config['starting']['set']))
self.auv_control.publish_variable(
config['variables']['set_heading'],
heading,
-1)
self.auv_control.publish_variable(
config['variables']['set_depth'],
self._current_waypoint['depth'] - self.altitude_safety(),
-1)
self.auv_control.publish_variable(
config['variables']['set_speed'],
float(config['auv']['depth_speed']),
-1)
return 'MORE'
else:
logging.debug('move_toward_waypoint(): Reached the waypoint and depth')
return 'DONE'
def test_bearing_to_point():
"""test_bearing_to_point()
"""
from math import tan
from searchspace.geometry import Point
from searchspace.geometry import bearing_to_point
p1 = Point(1, 1)
p2 = Point(1, 1)
assert bearing_to_point(p1, p2) is None
p1 = Point(1, 1)
p2 = Point(10, 10)
assert bearing_to_point(p1, p2) == 45
p1 = Point(1, 1)
p2 = Point(1, 10)
assert bearing_to_point(p1, p2) == 0
assert bearing_to_point(p2, p1) == 180
p1 = Point(1, 1)
p2 = Point(-5, 7)
assert bearing_to_point(p1, p2) == 315
p1 = Point(1, 1)
p2 = Point(10, 1)
assert bearing_to_point(p1, p2) == 90
p1 = Point(1, 1)
p2 = Point(-5, 1)
assert bearing_to_point(p1, p2) == 270
def test_next_track_heading_and_waypt(monkeypatch):
"""test_next_track_heading_and_waypt
"""
from searchspace.searchspace import SearchSpace
from auv_bonus_xprize.settings import config
from searchspace.geometry import Point, Line, Polygon
config['starting']['auv_position_utm'] = '80,150'
config['starting']['northwest_utm'] = '10,300'
config['starting']['northeast_utm'] = '100,300'
config['starting']['southeast_utm'] = '100,100'
config['starting']['southwest_utm'] = '10,100'
s = SearchSpace()
monkeypatch.setattr(
s, '_boundary_polygon',
Polygon(
[Point(10, 300),
Point(100, 300),
Point(100, 100),
Point(10, 100)]))
monkeypatch.setattr(
s, '_northern_boundary',
Line.construct_from_two_points(Point(10, 300), Point(100, 300)))
monkeypatch.setattr(
s, '_eastern_boundary',
Line.construct_from_two_points(Point(100, 300), Point(100, 100)))
monkeypatch.setattr(
s, '_southern_boundary',
Line.construct_from_two_points(Point(100, 100), Point(10, 100)))
monkeypatch.setattr(
s, '_western_boundary',
Line.construct_from_two_points(Point(10, 100), Point(10, 300)))
monkeypatch.setattr(s, '_current_set', 90)
heading, waypt = s._next_track_heading_and_waypt(Point(80, 150))
assert waypt.x == 80 and waypt.y == 300
assert heading == 0
monkeypatch.setattr(s, '_current_set', 270)
heading, waypt = s._next_track_heading_and_waypt(Point(80, 150))
assert waypt.x == 80 and waypt.y == 300
assert heading == 0
monkeypatch.setattr(s, '_current_set', 0)
heading, waypt = s._next_track_heading_and_waypt(Point(80, 150))
assert waypt.x == 10 and waypt.y == 150
assert heading == 270
monkeypatch.setattr(s, '_current_set', 135)
heading, waypt = s._next_track_heading_and_waypt(Point(80, 105))
assert waypt.x == 100 and waypt.y == 125
assert heading == 45
monkeypatch.setattr(s, '_current_set', 225)
heading, waypt = s._next_track_heading_and_waypt(Point(80, 105))
assert waypt.x == 10 and waypt.y == 175
assert heading == 315
def set_search_boundaries(self):
"""set_search_boundaries
Calculate the horizontal search boundaries as lines offset
from the configured contest boundaries by a buffer. The
definition of the contest area vertices, as well as the
buffer at the boundary, come from the configuration file.
The results of this method are saved as instance variables
named like _northern_boundary and are lines defined to
work with UTM positions.
https://www.latlong.net/lat-long-utm.html
https://stackoverflow.com/questions/343865/how-to-convert-from-utm-to-latlng-in-python-or-javascript
"""
boundary_buffer = round(float(config['search']['boundary_buffer_meters']))
vertex_list = list()
northwest_utm = config['starting']['northwest_utm'].split(',')
northwest_vertex = Point(round(float(northwest_utm[0])) + boundary_buffer,
round(float(northwest_utm[1])) - boundary_buffer)
vertex_list.append(northwest_vertex)
northeast_utm = config['starting']['northeast_utm'].split(',')
northeast_vertex = Point(round(float(northeast_utm[0])) - boundary_buffer,
round(float(northeast_utm[1])) - boundary_buffer)
vertex_list.append(northeast_vertex)
southeast_utm = config['starting']['southeast_utm'].split(',')
southeast_vertex = Point(round(float(southeast_utm[0])) - boundary_buffer,
round(float(southeast_utm[1])) + boundary_buffer)
vertex_list.append(southeast_vertex)
southwest_utm = config['starting']['southwest_utm'].split(',')
southwest_vertex = Point(round(float(southwest_utm[0])) + boundary_buffer,
round(float(southwest_utm[1])) + boundary_buffer)
vertex_list.append(southwest_vertex)
self._boundary_polygon = Polygon(vertex_list)
self._northern_boundary = Line.construct_from_two_points(
northwest_vertex,
northeast_vertex)
self._perimeter_length = points_distance(
northwest_vertex,
northeast_vertex)
self._eastern_boundary = Line.construct_from_two_points(
northeast_vertex,
southeast_vertex)
self._perimeter_length += points_distance(
northeast_vertex,
southeast_vertex)
self._southern_boundary = Line.construct_from_two_points(
southeast_vertex,
southwest_vertex)
self._perimeter_length += points_distance(
southeast_vertex,
southwest_vertex)
self._western_boundary = Line.construct_from_two_points(
southwest_vertex,
northwest_vertex)
self._perimeter_length += points_distance(
southwest_vertex,
northwest_vertex)