def test_relative_degrees(
self,
mock_latitude_to_m_per_d_longitude,
mock_m_per_latitude
):
mock_latitude_to_m_per_d_longitude.return_value = 1.0
mock_m_per_latitude.return_value = 1.0
self.assertAlmostEqual(Telemetry.distance_m(2, 0, 0, 0), 2)
self.assertAlmostEqual(Telemetry.distance_m(0, 2, 0, 0), 2)
self.assertAlmostEqual(Telemetry.distance_m(0, 0, 2, 0), 2)
self.assertAlmostEqual(Telemetry.distance_m(0, 0, 0, 2), 2)
self.assertAlmostEqual(Telemetry.distance_m(0, 0, 1, 1), math.sqrt(2))
for mult_1 in (-1, 1):
for mult_2 in (-1, 1):
self.assertAlmostEqual(
Telemetry.distance_m(mult_1 * 3, mult_2 * 4, 0, 0),
5
)
self.assertAlmostEqual(
Telemetry.distance_m(mult_1 * 4, mult_2 * 3, 0, 0),
5
)
self.assertAlmostEqual(
Telemetry.distance_m(0, 0, mult_1 * 3, mult_2 * 4),
5
)
self.assertAlmostEqual(
Telemetry.distance_m(0, 0, mult_1 * 4, mult_2 * 3),
5
)
def get_current_waypoint(self, x_m, y_m):
"""Returns the current waypoint."""
if self._current_waypoint == 0:
return self._waypoints[self._current_waypoint]
elif len(self._waypoints) == 1:
return self._waypoints[self._current_waypoint]
else:
current = self._waypoints[self._current_waypoint]
previous = self._waypoints[self._current_waypoint - 1]
distance_m = math.sqrt((current[0] - x_m)**2 +
(current[1] - y_m)**2)
if distance_m < self._distance_m:
return self._waypoints[self._current_waypoint]
# Find the point on the line segment from the previous waypoint to
# the current waypoint that is self._distance_m away and that's in
# the direction of the next waypoint
intersections = self._circle_intersection(previous, current,
(x_m, y_m),
self._distance_m)
if len(intersections) == 0:
# Well, this is bad. I guess we could go for a tangent?
self._logger.warn(
'No chase waypoint in range: {distance}'
' from {point_1}-{point_2}, using tangent'.format(
distance=round(self._distance_m, 3),
point_1=[round(i, 3) for i in previous],
point_2=[round(i, 3) for i in current],
))
tangent_distance_m = self._tangent_distance_m(
(x_m, y_m), previous, current)
if tangent_distance_m == None:
self._logger.warn(
'Unable to compute tangent, falling back to waypoint')
return current
intersections = self._circle_intersection(
previous,
current,
(x_m, y_m),
tangent_distance_m + 0.1 # Avoid floating point issues
)
waypoint = min(intersections,
key=lambda point: Telemetry.distance_m(
current[0], current[1], point[0], point[1]))
return waypoint
raise ValueError('No waypoints left')
def test_relative_degrees(self, mock_latitude_to_m_per_d_longitude,
mock_m_per_latitude):
mock_latitude_to_m_per_d_longitude.return_value = 1.0
mock_m_per_latitude.return_value = 1.0
self.assertAlmostEqual(Telemetry.distance_m(2, 0, 0, 0), 2)
self.assertAlmostEqual(Telemetry.distance_m(0, 2, 0, 0), 2)
self.assertAlmostEqual(Telemetry.distance_m(0, 0, 2, 0), 2)
self.assertAlmostEqual(Telemetry.distance_m(0, 0, 0, 2), 2)
self.assertAlmostEqual(Telemetry.distance_m(0, 0, 1, 1), math.sqrt(2))
for mult_1 in (-1, 1):
for mult_2 in (-1, 1):
self.assertAlmostEqual(
Telemetry.distance_m(mult_1 * 3, mult_2 * 4, 0, 0), 5)
self.assertAlmostEqual(
Telemetry.distance_m(mult_1 * 4, mult_2 * 3, 0, 0), 5)
self.assertAlmostEqual(
Telemetry.distance_m(0, 0, mult_1 * 3, mult_2 * 4), 5)
self.assertAlmostEqual(
Telemetry.distance_m(0, 0, mult_1 * 4, mult_2 * 3), 5)
def test_central_offset(self, mock_consume):
"""The offset should default to Sparkfun, or be loaded from the KML
course.
"""
telemetry = Telemetry()
self.assertLess(
telemetry.distance_m(
# From Google Earth
40.090764,
-105.184879,
CENTRAL_LATITUDE,
CENTRAL_LONGITUDE),
100)
# Smoke test
course = {'course': ((1, 2), (3, 4)), 'inner': ()}
with mock.patch.object(Telemetry,
'load_kml_from_file_name',
return_value=course):
with mock.patch('builtins.open'):
Telemetry('file.kml')
def test_central_offset(self, mock_consume):
"""The offset should default to Sparkfun, or be loaded from the KML
course.
"""
telemetry = Telemetry()
self.assertLess(
telemetry.distance_m(
# From Google Earth
40.090764,
-105.184879,
CENTRAL_LATITUDE,
CENTRAL_LONGITUDE
),
100
)
# Smoke test
course = {
'course': ((1, 2), (3, 4)),
'inner': ()
}
with mock.patch.object(Telemetry, 'load_kml_from_file_name', return_value=course):
with mock.patch('builtins.open'):
Telemetry('file.kml')
def get_current_waypoint(self, x_m, y_m):
"""Returns the current waypoint."""
if self._current_waypoint == 0:
return self._waypoints[self._current_waypoint]
elif len(self._waypoints) == 1:
return self._waypoints[self._current_waypoint]
else:
current = self._waypoints[self._current_waypoint]
previous = self._waypoints[self._current_waypoint - 1]
distance_m = math.sqrt(
(current[0] - x_m) ** 2
+ (current[1] - y_m) ** 2
)
if distance_m < self._distance_m:
return self._waypoints[self._current_waypoint]
# Find the point on the line segment from the previous waypoint to
# the current waypoint that is self._distance_m away and that's in
# the direction of the next waypoint
intersections = self._circle_intersection(
previous,
current,
(x_m, y_m),
self._distance_m
)
if len(intersections) == 0:
# Well, this is bad. I guess we could go for a tangent?
self._logger.warn(
'No chase waypoint in range: {distance}'
' from {point_1}-{point_2}, using tangent'.format(
distance=round(self._distance_m, 3),
point_1=[round(i, 3) for i in previous],
point_2=[round(i, 3) for i in current],
)
)
tangent_distance_m = self._tangent_distance_m(
(x_m, y_m),
previous,
current
)
if tangent_distance_m == None:
self._logger.warn(
'Unable to compute tangent, falling back to waypoint'
)
return current
intersections = self._circle_intersection(
previous,
current,
(x_m, y_m),
tangent_distance_m + 0.1 # Avoid floating point issues
)
waypoint = min(
intersections,
key=lambda point: Telemetry.distance_m(
current[0],
current[1],
point[0],
point[1]
)
)
return waypoint
raise ValueError('No waypoints left')