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)