def test_difference_d(self):
"""Tests the difference calculation between two headings."""
self.assertAlmostEqual(Telemetry.difference_d(359.0, 0.0), 1.0)
self.assertAlmostEqual(Telemetry.difference_d(0.0, 1.0), 1.0)
self.assertAlmostEqual(Telemetry.difference_d(359.0, 1.0), 2.0)
self.assertAlmostEqual(Telemetry.difference_d(360.0, 365.0), 5.0)
self.assertAlmostEqual(Telemetry.difference_d(-355.0, 365.0), 0.0)
def _load_waypoints(kml_stream):
"""Loads and returns the waypoints from a KML string."""
def get_child(element, tag_name):
"""Returns the child element with the given tag name."""
try:
return getattr(element, tag_name)
except AttributeError:
raise ValueError('No {tag} element found'.format(tag=tag_name))
root = parser.parse(kml_stream).getroot()
if 'kml' not in root.tag:
raise ValueError('Not a KML file')
document = get_child(root, 'Document')
placemark = get_child(document, 'Placemark')
line_string = get_child(placemark, 'LineString')
# Unlike all of the other tag names, "coordinates" is not capitalized
coordinates = get_child(line_string, 'coordinates')
waypoints = []
text = coordinates.text.strip()
for csv in re.split(r'\s', text):
(
longitude,
latitude,
altitude # pylint: disable=unused-variable
) = csv.split(',')
waypoints.append((
Telemetry.longitude_to_m_offset(float(longitude), float(latitude)),
Telemetry.latitude_to_m_offset(float(latitude))
))
return waypoints
def test_offset_calculations(self):
"""Tests the latitude/longitude to y_m/x_m offset calculations."""
for value in (-2.0, -1.0, 0.0, 1.0, 2.0):
self.assertAlmostEqual(
value,
Telemetry.offset_y_m_to_latitude(
Telemetry.latitude_to_m_offset(value)))
self.assertAlmostEqual(
value,
Telemetry.latitude_to_m_offset(
Telemetry.offset_y_m_to_latitude(value)))
for value_2 in (-0.01, 0.0, 0.01):
self.assertAlmostEqual(
value,
Telemetry.offset_x_m_to_longitude(
Telemetry.longitude_to_m_offset(value, 0.0), value_2))
self.assertAlmostEqual(
value,
Telemetry.offset_x_m_to_longitude(
Telemetry.longitude_to_m_offset(value, 0.0), value_2))
rally_parking_lot = (40.020776, -105.248319)
self.assertAlmostEqual(
Telemetry.offset_x_m_to_longitude(
Telemetry.longitude_to_m_offset(rally_parking_lot[1],
rally_parking_lot[0]),
rally_parking_lot[0]), rally_parking_lot[1])
def test_latitude_to_m_per_d_longitude(self):
"""Tests the conversion from latitude to meters per degree longitude."""
# Assume Earth is a sphere
self.assertAlmostEqual(
Telemetry.EQUATORIAL_RADIUS_M * 2.0 * math.pi / 360.0,
Telemetry.latitude_to_m_per_d_longitude(0.0))
# Should be symmetrical
for degree in range(0, 85, 1):
self.assertAlmostEqual(
Telemetry.latitude_to_m_per_d_longitude(degree),
Telemetry.latitude_to_m_per_d_longitude(-degree))
# At the poles, should be 0
self.assertAlmostEqual(Telemetry.latitude_to_m_per_d_longitude(90.0),
0.0)
# Known values, from http://www.csgnetwork.com/degreelenllavcalc.html
self.assertAlmostEqual(Telemetry.M_PER_D_LATITUDE,
111319.458,
places=1)
self.assertAlmostEqual(
# Boulder
Telemetry.latitude_to_m_per_d_longitude(40.08, cache=False),
85294.08886768305,
places=2)
def run(self):
"""Run in a thread, hands raw telemetry readings to telemetry
instance.
"""
from control.telemetry import Telemetry
# Normally, you'd have a loop that periodically checks for new readings
# or that blocks until readings are received
while self._run:
try:
time.sleep(self._sleep_time_s)
except Exception: # pylint: disable=broad-except
pass
speed_m_s = 0.0
if self._driver is not None:
speed_m_s = self._driver.get_throttle() * self.MAX_SPEED_M_S
point_m = (0.0, speed_m_s * self._sleep_time_s)
offset_m = Telemetry.rotate_radians_clockwise(
point_m, math.radians(self._heading_d))
self._x_m += offset_m[0]
self._y_m += offset_m[1]
self._heading_d += \
self._driver.get_turn() * self.TURN_D_S * self._sleep_time_s
self._heading_d = Telemetry.wrap_degrees(self._heading_d)
self._iterations += 1
if self._iterations % 5 == 0:
gps_d = Telemetry.wrap_degrees(
self._random.normalvariate(self._heading_d,
self.SIGMA_GPS_D))
self._telemetry.handle_message({
'x_m':
self._random.normalvariate(self._x_m, self.SIGMA_M),
'y_m':
self._random.normalvariate(self._y_m, self.SIGMA_M),
'x_accuracy_m':
self.SIGMA_M,
'y_accuracy_m':
self.SIGMA_M,
'speed_m_s':
self._random.normalvariate(speed_m_s, self.SIGMA_M_S),
'gps_d':
gps_d,
'accelerometer_m_s_s': (0.0, 0.0, 9.8),
})
else:
compass_d = Telemetry.wrap_degrees(
self._random.normalvariate(self._heading_d,
self.SIGMA_COMPASS_D))
self._telemetry.handle_message({
'compass_d':
compass_d,
'accelerometer_m_s_s': (0.0, 0.0, 9.8),
})
def _unstuck_yourself_iterator(self, seconds):
"""Commands the car to reverse and try to get off an obstacle."""
# The ESC requires us to send neutral throttle for a bit, then send
# reverse, then neutral, then reverse again (which will actually drive
# the car in reverse)
start = time.time()
while time.time() < start + self.NEUTRAL_TIME_1_S:
self._driver.drive(0.0, 0.0)
yield True
start = time.time()
while time.time() < start + self.REVERSE_TIME_S:
self._driver.drive(-0.5, 0.0)
yield True
start = time.time()
while time.time() < start + self.NEUTRAL_TIME_2_S:
self._driver.drive(0.0, 0.0)
yield True
telemetry = self._telemetry.get_data()
heading_d = telemetry['heading_d']
current_waypoint = self._waypoint_generator.get_current_waypoint(
telemetry['x_m'], telemetry['y_m'])
degrees = Telemetry.relative_degrees(telemetry['x_m'],
telemetry['y_m'],
current_waypoint[0],
current_waypoint[1])
is_left = Telemetry.is_turn_left(heading_d, degrees)
if is_left is None:
turn_direction = 1.0 if random.randint(0, 1) == 0 else -1.0
elif is_left:
turn_direction = 1.0 # Reversed because we're driving in reverse
else:
turn_direction = -1.0
start = time.time()
while time.time() < start + seconds:
self._driver.drive(-.5, turn_direction)
yield True
# Pause for a bit; jamming from reverse to drive is a bad idea
start = time.time()
while time.time() < start + self.NEUTRAL_TIME_3_S:
self._driver.drive(0.0, 0.0)
yield True
yield False
def test_load_waypoints(self):
"""Tests loading waypoints from a KML format file."""
coordinates_long_lat = zip(range(10), range(10, 0, -1))
coordinates_str = ' '.join(('{},{},50'.format(long, lat)
for long, lat in coordinates_long_lat))
kml = KML_TEMPLATE.format(coordinates_str).encode('utf-8')
kml_buffer = io.BytesIO(kml)
waypoints = SimpleWaypointGenerator._load_waypoints(kml_buffer)
for m_offset, long_lat in zip(waypoints, coordinates_long_lat):
x_m_1, y_m_1 = m_offset
long_, lat = long_lat
x_m_2 = Telemetry.longitude_to_m_offset(long_)
y_m_2 = Telemetry.latitude_to_m_offset(lat)
self.assertEqual(x_m_1, x_m_2)
self.assertEqual(y_m_1, y_m_2)
def _update_position(self):
"""Updates the position using dead reckoning."""
self.update_count -= 1
diff_time_s = 1.0
if self._throttle > 0.0:
self._heading += self._turn * 30.0
self._heading = Telemetry.wrap_degrees(self._heading)
step_m = diff_time_s * self._throttle * self.MAX_SPEED_M_S
point = (0, step_m)
radians = math.radians(self._heading)
point = Telemetry.rotate_radians_clockwise(point, radians)
self._x_m += point[0]
self._y_m += point[1]
def test_estimate(self):
"""Tests that the estimating of the locations via both is sane."""
start_coordinates_m = (100.0, 150.0)
heading_d = 40.0
location_filter = LocationFilter(start_coordinates_m[0],
start_coordinates_m[1], heading_d)
speed_m_s = 5.0
# This would normally naturally get set by running the Kalman filter;
# we'll just manually set it now
location_filter._estimates[3].itemset(0, speed_m_s)
self.assertEqual(location_filter.estimated_location(),
start_coordinates_m)
tick_s = 0.5
step_m = speed_m_s * tick_s
step_x_m, step_y_m = Telemetry.rotate_radians_clockwise(
(0.0, step_m), math.radians(heading_d))
actual_x_m, actual_y_m = start_coordinates_m
def check_estimates():
self.assertLess(
abs(location_filter.estimated_location()[0] - actual_x_m), 0.5)
self.assertLess(
abs(location_filter.estimated_location()[1] - actual_y_m), 0.5)
for update in range(1, 21):
actual_x_m += step_x_m
actual_y_m += step_y_m
# First update by compass
measurements = numpy.matrix([0.0, 0.0, heading_d, 0.0]).transpose()
location_filter._update(measurements,
location_filter.COMPASS_OBSERVER_MATRIX,
location_filter.COMPASS_MEASUREMENT_NOISE,
tick_s)
check_estimates()
# Add some approximated GPS readings
# We'll use very tight standard deviations to try to avoid random
# test failures
measurements = numpy.matrix([
random.normalvariate(actual_x_m, 0.01),
random.normalvariate(actual_y_m, 0.01),
random.normalvariate(heading_d, 0.5),
random.normalvariate(speed_m_s, speed_m_s * 0.1)
]).transpose()
location_filter._update(
measurements,
location_filter.GPS_OBSERVER_MATRIX,
location_filter.GPS_MEASUREMENT_NOISE,
0.0 # Tick isn't used for GPS
)
check_estimates()
def test_acceleration_mss_velocity_ms_to_ds(self):
"""Tests the conversion from acceleration m/s^s and velocity m/s
to rotational speed d/s."""
# Examples taken from
# www.mattawanschools.org/14662062013835470/lib/14662062013935470/
# Ch_8_Problem_set.pdf
velocity_ms = 2 * math.pi * 4.0 / 2.0
acceleration_mss = velocity_ms**2 / 4.0
self.assertAlmostEqual(
4.0 * 0.5 * 360.0, # 2 seconds per revolution
Telemetry.acceleration_mss_velocity_ms_to_ds(
acceleration_mss, velocity_ms))
velocity_ms = 2 * math.pi * 5e4 / 1.8e3
acceleration_mss = velocity_ms**2 / 5e4
self.assertAlmostEqual(
30 * 60 * 360.0, # 1 revolution per 30 minutes
Telemetry.acceleration_mss_velocity_ms_to_ds(
acceleration_mss, velocity_ms))
def _get_extended_waypoint(self):
"""Returns the extended waypoint."""
if self._current_waypoint_index == 0:
return self._waypoints[0]
if (self._current_waypoint_index >= 1
and self._current_waypoint_index < len(self._waypoints)):
previous_waypoint_m = self._waypoints[self._current_waypoint_index
- 1]
current_waypoint_m = self._waypoints[self._current_waypoint_index]
degrees = Telemetry.relative_degrees(previous_waypoint_m[0],
previous_waypoint_m[1],
current_waypoint_m[0],
current_waypoint_m[1])
offset_m = Telemetry.rotate_degrees_clockwise((0.0, self.BEYOND_M),
degrees)
return (current_waypoint_m[0] + offset_m[0],
current_waypoint_m[1] + offset_m[1])
return self._waypoints[-1]
def _update(self, measurements, observer_matrix, measurement_noise,
time_diff_s):
"""Runs the Kalman update using the provided measurements."""
# Prediction step
transition = self._prediction_step(time_diff_s)
# Update uncertainty
# P = A * P * A' + Q
self._covariance_matrix = \
transition * self._covariance_matrix * transition.transpose() \
+ self._process_noise
# Compute the Kalman gain
# K = P * H' * inv(H * P * H' + R)
hphtr = (observer_matrix * self._covariance_matrix *
observer_matrix.transpose() + measurement_noise)
try:
hphtri = hphtr.getI()
except numpy.linalg.linalg.LinAlgError:
for diagonal_index in range(len(hphtr)):
diagonal_value = hphtr[diagonal_index].item(diagonal_index)
if diagonal_value == 0.0:
hphtr[diagonal_index].itemset(diagonal_index, 0.00001)
hphtri = hphtr.getI()
kalman_gain = \
self._covariance_matrix * observer_matrix.transpose() * hphtri
# Determine innovation or residual and update our estimate
# x = x + K * (z - H * x)
zhx = measurements - observer_matrix * self._estimates
heading_d = zhx[2].item(0)
while heading_d > 180.0:
heading_d -= 360.0
while heading_d <= -180.0:
heading_d += 360.0
zhx[2].itemset(0, heading_d)
self._estimates = self._estimates + kalman_gain * zhx
from control.telemetry import Telemetry
heading_d = Telemetry.wrap_degrees(self._estimates[2].item(0))
self._estimates[2].itemset(0, heading_d)
# Update the covariance
# P = (I - K * H) * P
self._covariance_matrix = (
numpy.identity(len(kalman_gain)) -
kalman_gain * observer_matrix) * self._covariance_matrix
assert len(self._estimates) == 4 and len(self._estimates[0]) == 1, \
'Estimates should be size 4x1 but is {}x{} after update'.format(
len(self._estimates),
len(self._estimates[0])
)
def test_point_in_polygon(self):
"""Tests point in polygon."""
diamond = ((1, 0), (0, -1), (-1, 0), (0, 1))
for point in ((1, 1), (-1, 1), (-1, -1), (1, -1)):
self.assertFalse(Telemetry.point_in_polygon(point, diamond))
self.assertTrue(Telemetry.point_in_polygon((0, 0), diamond))
# -------------
# | * * * |
# |*/-\ * /-\*|
# |/ * \-/ * \|
polygon = ((0, 0), (0, 4), (8, 4), (8, 0), (6, 2), (4, 0), (2, 2))
inside = ((1, 1), (2, 3), (4, 3), (6, 3), (7, 1))
outside = ((2, 1), (6, 1), (8.1, 0.1), (8.1, 3.9), (-1, -1), (100, 0),
(0, 100), (100, 100), (-100, -100), (-100, 100), (100,
-100))
for point in inside:
self.assertTrue(Telemetry.point_in_polygon(point, polygon))
for point in outside:
self.assertFalse(Telemetry.point_in_polygon(point, polygon))
# Test the last segment. The function uses an arbitrary offset, so test
# all directions so that we can change the offset and not break.
tiny = 0.0001
polygon = ((-tiny, -tiny), (tiny, -tiny), (tiny, tiny), (-tiny, tiny))
self.assertTrue(Telemetry.point_in_polygon((0, 0), polygon))
for point in polygon:
point_2 = [i * 2 for i in point]
self.assertFalse(Telemetry.point_in_polygon(point_2, polygon))
for point in (-tiny, tiny):
point_2 = (point * 2, 0)
self.assertFalse(Telemetry.point_in_polygon(point_2, polygon))
for point in (-tiny, tiny):
point_2 = (0, point * 2)
self.assertFalse(Telemetry.point_in_polygon(point_2, polygon))
for point in polygon:
point_2 = (point[0] * 2, point[1])
self.assertFalse(Telemetry.point_in_polygon(point_2, polygon))
for point in polygon:
point_2 = (point[0], point[1] * 2)
self.assertFalse(Telemetry.point_in_polygon(point_2, polygon))
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_rotate_degrees_clockwise(self):
"""Tests rotating a vector degrees clockwise."""
base = (1.0, 0.0)
not_rotated = Telemetry.rotate_degrees_clockwise(base, 0.0)
self.assertAlmostEqual(not_rotated[0], base[0])
self.assertAlmostEqual(not_rotated[1], base[1])
ninety = Telemetry.rotate_degrees_clockwise(base, 90.0)
self.assertAlmostEqual(ninety[0], 0.0)
self.assertAlmostEqual(ninety[1], -1.0)
one_eighty = Telemetry.rotate_degrees_clockwise(base, 180.0)
self.assertAlmostEqual(one_eighty[0], -base[0])
self.assertAlmostEqual(one_eighty[1], base[1])
two_seventy = Telemetry.rotate_degrees_clockwise(base, 270.0)
self.assertAlmostEqual(two_seventy[0], 0.0)
self.assertAlmostEqual(two_seventy[1], 1.0)
three_sixty = Telemetry.rotate_degrees_clockwise(base, 360.0)
self.assertAlmostEqual(three_sixty[0], base[0])
self.assertAlmostEqual(three_sixty[1], base[1])
negative_ninety = Telemetry.rotate_degrees_clockwise(base, -90.0)
self.assertAlmostEqual(negative_ninety[0], two_seventy[0])
self.assertAlmostEqual(negative_ninety[1], two_seventy[1])
def test_rotate_radians_clockwise(self):
"""Tests rotating a vector radians clockwise."""
base = (1.0, 0.0)
not_rotated = Telemetry.rotate_radians_clockwise(base, 0.0)
self.assertAlmostEqual(not_rotated[0], base[0])
self.assertAlmostEqual(not_rotated[1], base[1])
ninety = Telemetry.rotate_radians_clockwise(base, math.radians(90.0))
self.assertAlmostEqual(ninety[0], 0.0)
self.assertAlmostEqual(ninety[1], -1.0)
one_eighty = Telemetry.rotate_radians_clockwise(
base, math.radians(180.0))
self.assertAlmostEqual(one_eighty[0], -base[0])
self.assertAlmostEqual(one_eighty[1], base[1])
two_seventy = Telemetry.rotate_radians_clockwise(
base, math.radians(270.0))
self.assertAlmostEqual(two_seventy[0], 0.0)
self.assertAlmostEqual(two_seventy[1], 1.0)
three_sixty = Telemetry.rotate_radians_clockwise(
base, math.radians(360.0))
self.assertAlmostEqual(three_sixty[0], base[0])
self.assertAlmostEqual(three_sixty[1], base[1])
negative_ninety = Telemetry.rotate_radians_clockwise(
base, math.radians(-90.0))
self.assertAlmostEqual(negative_ninety[0], two_seventy[0])
self.assertAlmostEqual(negative_ninety[1], two_seventy[1])
def test_wrap_degrees(self):
"""Tests wrap degrees."""
for d in range(0, 360):
self.assertAlmostEqual(d, Telemetry.wrap_degrees(d))
self.assertAlmostEqual(0.0, Telemetry.wrap_degrees(0.0))
self.assertAlmostEqual(0.0, Telemetry.wrap_degrees(360.0))
self.assertAlmostEqual(359.0, Telemetry.wrap_degrees(-1.0))
self.assertAlmostEqual(359.0, Telemetry.wrap_degrees(-361.0))
self.assertAlmostEqual(1.0, Telemetry.wrap_degrees(361.0))
self.assertAlmostEqual(1.0, Telemetry.wrap_degrees(721.0))
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 _prediction_step(self, time_diff_s):
"""Runs the prediction step and returns the transition matrix."""
# x = A * x + B
heading_r = math.radians(self.estimated_heading())
from control.telemetry import Telemetry
x_delta, y_delta = Telemetry.rotate_radians_clockwise(
(0.0, time_diff_s), heading_r)
speed_m_s = self.estimated_speed()
transition = numpy.matrix([ # A
[1.0, 0.0, 0.0, x_delta], [0.0, 1.0, 0.0, y_delta],
[0.0, 0.0, 1.0, 0.0], [0.0, 0.0, 0.0, 1.0]
])
# Update heading estimate based on steering
new_heading = Telemetry.wrap_degrees(self.estimated_heading() +
self._estimated_turn_rate_d_s *
time_diff_s)
self._estimates.itemset(2, new_heading)
# TODO: Add acceleration values
self._estimates = transition * self._estimates
return transition
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_handle_telemetry_message(self, mpic):
"""Tests the handling of telemetry messages received from telemetry
sources, such as phones or SUP800F.
"""
telemetry = Telemetry()
self.assertEqual(len(telemetry._speed_history), 0)
telemetry._handle_message(
json.dumps({
'device_id': 'test',
'speed_m_s': 1.0
}))
self.assertEqual(len(telemetry._speed_history), 1)
mpic.return_value = True
self.assertEqual(mpic.call_count, 0)
telemetry._handle_message(
json.dumps({
'device_id': 'test',
'latitude_d': 1.0,
'longitude_d': 1.0,
'accuracy_m': 1.0,
'speed_m_s': 1.0,
'heading_d': 0.0
}))
self.assertEqual(mpic.call_count, 1)
mpic.return_value = False
self.assertEqual(mpic.call_count, 1)
telemetry._handle_message(
json.dumps({
'device_id': 'test',
'latitude_d': 1.0,
'longitude_d': 1.0,
'accuracy_m': 1.0,
'speed_m_s': 1.0,
'heading_d': 0.0
}))
self.assertEqual(mpic.call_count, 2)
self.assertEqual(len(telemetry._ignored_points), 1)
self.assertEqual(telemetry._ignored_points['test'], 1)
def _handle_binary(self, message):
"""Handles properietary SUP800F binary messages."""
if message is None:
return
flux_x = float(message.magnetic_flux_ut_x - self._compass_offsets[0])
flux_y = float(message.magnetic_flux_ut_y - self._compass_offsets[1])
if flux_x == 0.0:
# TODO: Figure out what to do here
return
self._last_compass_heading_d = Telemetry.wrap_degrees(
270.0 - math.degrees(math.atan2(flux_y, flux_x)) +
8.666 # Boulder declination
)
magnitude = flux_x**2 + flux_y**2
std_devs_away = abs(self._magnitude_mean -
magnitude) / self._magnitude_std_dev
# In a normal distribution, 95% of readings should be within 2 std devs
if std_devs_away > 2.0:
self._dropped_compass_messages += 1
if self._dropped_compass_messages > self._dropped_threshold:
self._logger.warn(
'Dropped {} compass messages in a row, std dev = {}'.
format(self._dropped_compass_messages,
round(std_devs_away, 3)))
self._dropped_compass_messages = 0
self._dropped_threshold += 10
return
self._dropped_compass_messages = 0
self._dropped_threshold = 10
if std_devs_away > 1.0:
confidence = 2.0 - std_devs_away
else:
confidence = 1.0
self._telemetry.compass_reading(self._last_compass_heading_d,
confidence, 'sup800f')
self._telemetry.accelerometer_reading(message.acceleration_g_x,
message.acceleration_g_y,
message.acceleration_g_z,
'sup800f')
def test_estimate_constant_speed(self):
"""Tests that the estimating of the locations via dead reckoning at a
constant speed is sane.
"""
# I'm not sure how to independently validate these tests for accuracy.
# The best I can think of is to do some sanity tests.
start_coordinates_m = (100.0, 200.0)
heading_d = 32.0
location_filter = LocationFilter(start_coordinates_m[0],
start_coordinates_m[1], heading_d)
self.assertEqual(location_filter.estimated_location(),
start_coordinates_m)
speed_m_s = 1.8
# This would normally naturally get set by running the Kalman filter;
# we'll just manually set it now
location_filter._estimates[3].itemset(0, speed_m_s)
self.assertEqual(location_filter.estimated_location(),
start_coordinates_m)
measurements = numpy.matrix( # pylint: disable=no-member
[0.0, 0.0, heading_d, 0.0]).transpose() # z
seconds = 5
for _ in range(seconds):
location_filter._update(measurements,
location_filter.COMPASS_OBSERVER_MATRIX,
location_filter.COMPASS_MEASUREMENT_NOISE,
time_diff_s=1.0)
offset = Telemetry.rotate_radians_clockwise((0.0, speed_m_s * seconds),
math.radians(heading_d))
new_coordinates = [s + o for s, o in zip(start_coordinates_m, offset)]
for estimated, expected in zip(location_filter.estimated_location(),
new_coordinates):
self.assertAlmostEqual(estimated, expected, 2)
def benchmark_command_run_course_iterator():
"""Benchmark the logic for driving the car."""
logger = DummyLogger()
telemetry = Telemetry(logger)
waypoint_generator = SimpleWaypointGenerator(
SimpleWaypointGenerator.get_waypoints_from_file_name(
'paths/solid-state-depot.kmz'))
driver = DummyDriver(telemetry, logger)
command = Command(telemetry, driver, waypoint_generator, logger)
iterations = 250
start = time.time()
iterator = command._run_course_iterator()
step = None
for step in zip(range(iterations), iterator):
pass
assert step is not None
assert step[0] == iterations - 1
end = time.time()
print('{} iterations of Command._run_course_iterator, each took {:.5}'.
format(iterations, (end - start) / float(iterations)))
def _run_course_iterator(self):
"""Runs a single iteration of the course navigation loop."""
while not self._waypoint_generator.done():
telemetry = self._telemetry.get_data()
current_waypoint = self._waypoint_generator.get_current_waypoint(
telemetry['x_m'], telemetry['y_m'])
distance_m = math.sqrt((telemetry['x_m'] -
current_waypoint[0])**2 +
(telemetry['y_m'] - current_waypoint[1])**2)
self._logger.debug(
'Distance to goal {waypoint}: {distance}'.format(
waypoint=[round(i, 3) for i in current_waypoint],
distance=round(distance_m, 3),
))
# We let the waypoint generator tell us if a waypoint has been
# reached so that it can do fancy algorithms, like "rabbit chase"
if self._waypoint_generator.reached(telemetry['x_m'],
telemetry['y_m']):
self._logger.info('Reached {}'.format(
[round(i, 3) for i in current_waypoint]))
self._waypoint_generator.next()
continue
if distance_m > 10.0 or distance_m / telemetry['speed_m_s'] > 2.0:
throttle = 1.0
else:
throttle = 0.5
degrees = Telemetry.relative_degrees(telemetry['x_m'],
telemetry['y_m'],
current_waypoint[0],
current_waypoint[1])
heading_d = telemetry['heading_d']
self._logger.debug(
'My heading: {my_heading}, goal heading: {goal_heading}'.
format(
my_heading=round(heading_d, 3),
goal_heading=round(degrees, 3),
))
diff_d = Telemetry.difference_d(degrees, heading_d)
if diff_d < 10.0:
# We want to keep turning until we pass the point
is_left = Telemetry.is_turn_left(heading_d, degrees)
while diff_d < 10.0:
yield True
telemetry = self._telemetry.get_data()
degrees = Telemetry.relative_degrees(
telemetry['x_m'], telemetry['y_m'],
current_waypoint[0], current_waypoint[1])
heading_d = telemetry['heading_d']
diff_d = Telemetry.difference_d(degrees, heading_d)
if self._waypoint_generator.reached(
telemetry['x_m'], telemetry['y_m']):
self._logger.info('Reached {}'.format(
[round(i, 3) for i in current_waypoint]))
self._waypoint_generator.next()
break
if Telemetry.is_turn_left(heading_d, degrees) != is_left:
break
self._driver.drive(throttle, 0.0)
yield True
continue
# No sharp turns when we are close, to avoid hard swerves
elif distance_m < 3.0:
turn = 0.25
elif diff_d > 90.0:
turn = 1.0
elif diff_d > 45.0:
turn = 0.5
else:
turn = 0.25
# Turning while going fast causes the car to roll over
if telemetry['speed_m_s'] > 7.0 or throttle >= 0.75:
turn = max(turn, 0.25)
elif telemetry['speed_m_s'] > 4.0 or throttle >= 0.5:
turn = max(turn, 0.5)
if Telemetry.is_turn_left(heading_d, degrees):
turn = -turn
self._driver.drive(throttle, turn)
yield True
self._logger.info('No waypoints, stopping')
self._driver.drive(0.0, 0.0)
self.stop()
yield False
def test_get_current_waypoint(self):
"""Tests the extension waypoint generation."""
points = ((20, 20),)
generator = ExtensionWaypointGenerator(points)
# If there's only one point, always return it
self.assertEqual(points[0], generator.get_current_waypoint(10, 10))
self.assertEqual(points[0], generator.get_current_waypoint(19, 19))
self.assertEqual(points[0], generator.get_current_waypoint(21, 19))
self.assertEqual(points[0], generator.get_current_waypoint(21, 21))
self.assertEqual(points[0], generator.get_current_waypoint(19, 21))
# Project through tests
position = (-100, 20)
points = ((0, 0), (0, 10), (10, 10), (10, 0), (0, 0), (5, 5), (5, 0), (5, -5), (0, 0))
generator = ExtensionWaypointGenerator(points)
self.assertEqual(
generator.get_current_waypoint(
position[0],
position[1]
),
points[0]
)
generator.next()
self.assertTrue(position not in points)
for index in range(1, len(points)):
point = points[index]
previous_point = points[index - 1]
true_waypoint = point
waypoint = generator.get_current_waypoint(
position[0],
position[1]
)
self.assertAlmostEqual(
Telemetry.relative_degrees(
previous_point[0],
previous_point[1],
true_waypoint[0],
true_waypoint[1]
),
Telemetry.relative_degrees(
previous_point[0],
previous_point[1],
waypoint[0],
waypoint[1]
)
)
def distance(x1, y1, x2, y2):
"""Returns distance between 2 points."""
return math.sqrt(
(x1 - x2) ** 2 + (y1 - y2) ** 2
)
self.assertLess(
distance(
previous_point[0],
previous_point[1],
true_waypoint[0],
true_waypoint[1]
) + 1.0,
distance(
previous_point[0],
previous_point[1],
waypoint[0],
waypoint[1]
)
)
generator.next()
def __init__(self):
self.boulder_latitude_m_per_d_longitude = \
Telemetry.latitude_to_m_per_d_longitude(40.015)
def test_is_turn_left(self):
self.assertTrue(Telemetry.is_turn_left(1, 0))
self.assertFalse(Telemetry.is_turn_left(0, 1))
self.assertTrue(Telemetry.is_turn_left(1, 359))
self.assertFalse(Telemetry.is_turn_left(359, 1))
self.assertTrue(Telemetry.is_turn_left(1, 270))
self.assertTrue(Telemetry.is_turn_left(0, 270))
self.assertTrue(Telemetry.is_turn_left(359, 270))
self.assertFalse(Telemetry.is_turn_left(1, 90))
self.assertFalse(Telemetry.is_turn_left(0, 90))
self.assertFalse(Telemetry.is_turn_left(359, 90))
# These shouldn't throw
Telemetry.is_turn_left(0, 0)
Telemetry.is_turn_left(1, 1)
Telemetry.is_turn_left(180, 180)
Telemetry.is_turn_left(180, 0)
Telemetry.is_turn_left(270, 90)
Telemetry.is_turn_left(360, 0)
Telemetry.is_turn_left(0, 360)
def start_threads(
waypoint_generator,
logger,
web_socket_handler,
max_throttle,
kml_file_name,
):
"""Runs everything."""
logger.info('Creating Telemetry')
telemetry = Telemetry(kml_file_name)
telemetry_dumper = TelemetryDumper(
telemetry,
waypoint_generator,
web_socket_handler
)
logger.info('Done creating Telemetry')
global DRIVER
DRIVER = Driver(telemetry)
DRIVER.set_max_throttle(max_throttle)
logger.info('Setting SUP800F to NMEA mode')
serial_ = serial.Serial('/dev/ttyAMA0', 115200)
serial_.setTimeout(1.0)
for _ in range(10):
serial_.readline()
try:
switch_to_nmea_mode(serial_)
except: # pylint: disable=W0702
logger.error('Unable to set mode')
for _ in range(10):
serial_.readline()
logger.info('Done')
# The following objects must be created in order, because of message
# exchange dependencies:
# sup800f_telemetry: reads from command forwarded
# command: reads from command, writes to command forwarded
# button: writes to command
# cherry_py_server: writes to command
# TODO(2016-08-21) Have something better than sleeps to work around race
# conditions
logger.info('Creating threads')
sup800f_telemetry = Sup800fTelemetry(serial_)
time.sleep(0.5)
command = Command(telemetry, DRIVER, waypoint_generator)
time.sleep(0.5)
button = Button()
port = int(get_configuration('PORT', 8080))
address = get_configuration('ADDRESS', '0.0.0.0')
cherry_py_server = CherryPyServer(
port,
address,
telemetry,
waypoint_generator
)
time.sleep(0.5)
global THREADS
THREADS += (
button,
cherry_py_server,
command,
sup800f_telemetry,
telemetry_dumper,
)
for thread in THREADS:
thread.start()
logger.info('Started all threads')
# Use a fake timeout so that the main thread can still receive signals
sup800f_telemetry.join(100000000000)
# Once we get here, sup800f_telemetry has died and there's no point in
# continuing because we're not receiving telemetry messages any more, so
# stop the command module
command.stop()
command.join(100000000000)
cherry_py_server.kill()
cherry_py_server.join(100000000000)
button.kill()
button.join(100000000000)
