class DummyDriver(object):
"""Dummy class that implements the Driver interface."""
def __init__(self, telemetry):
self._telemetry = telemetry
self._logger = AsyncLogger()
self._throttle = 0.0
self._steering= 0.0
def drive(self, throttle_percentage, steering_percentage):
"""Sends a command to the RC car. Throttle should be a float between
-1.0 for reverse and 1.0 for forward. Steering should be a float
between -1.0 for left and 1.0 for right.
"""
assert -1.0 <= throttle_percentage <= 1.0
assert -1.0 <= steering_percentage <= 1.0
if (
self._throttle == throttle_percentage
and self._steering == steering_percentage
):
return
self._telemetry.process_drive_command(
throttle_percentage,
steering_percentage
)
self._logger.debug(
'throttle = {throttle}, steering = {steering}'.format(
throttle=throttle_percentage,
steering=steering_percentage,
)
)
self._throttle = throttle_percentage
self._steering = steering_percentage
def get_throttle(self):
"""Returns the current throttle."""
return self._throttle
def get_turn(self):
"""Returns the current turn."""
return self._steering
class Sup800fTelemetry(threading.Thread):
"""Reader of GPS module that implements the TelemetryData interface."""
def __init__(self, serial):
"""Create the TelemetryData thread."""
super(Sup800fTelemetry, self).__init__()
self.name = self.__class__.__name__
self._telemetry = TelemetryProducer()
self._serial = serial
self._logger = AsyncLogger()
self._run = True
self._iterations = 0
# These initial measurements are from a calibration observation
self._compass_offsets = (-11.87, -5.97)
self._magnitude_mean = 353.310
self._magnitude_std_dev = 117.918
self._calibrate_compass_end_time = None
self._nmea_mode = True
self._last_compass_heading_d = 0.0
self._dropped_compass_messages = 0
self._dropped_threshold = 10
self._hdop = 5.0
def handle_message(message):
"""Handles command messages. Only cares about calibrate compass;
other messages are ignored.
"""
if message == 'calibrate-compass':
self.calibrate_compass(10)
consume = lambda: consume_messages(config.COMMAND_FORWARDED_EXCHANGE,
handle_message)
thread = threading.Thread(target=consume)
thread.name = '{}:consume_messages:{}'.format(
self.__class__.__name__, config.COMMAND_FORWARDED_EXCHANGE)
thread.start()
def run(self):
"""Run in a thread, hands raw telemetry readings to telemetry
instance.
"""
failed_to_switch_mode = False
while self._run:
try:
self._run_inner()
except EnvironmentError as env:
self._logger.debug('Failed to switch mode: {}'.format(env))
if not failed_to_switch_mode:
failed_to_switch_mode = True
import traceback
self._logger.debug(traceback.format_exc())
# Maybe resetting the module mode will help
try:
if self._nmea_mode:
switch_to_nmea_mode(self._serial)
else:
switch_to_binary_mode(self._serial)
except:
pass
except Exception as exc: # pylint: disable=broad-except
self._logger.warn(
'Telemetry data caught exception: {}'.format(exc))
def _run_inner(self):
"""Inner part of run."""
binary_count = 0
while self._run:
if self._calibrate_compass_end_time is not None:
self._calibrate_compass()
# NMEA mode
if self._nmea_mode:
if self._get_gprc():
switch_to_binary_mode(self._serial)
self._nmea_mode = False
else:
if self._get_binary():
binary_count += 1
if binary_count >= 3:
switch_to_nmea_mode(self._serial)
self._nmea_mode = True
binary_count = 0
def _get_gprc(self):
"""Gets and processes a single GPRC message."""
# This blocks until a new message is received
line = self._serial.readline()
try:
line = line.decode('utf-8')
except:
raise EnvironmentError('Not a UTF-8 message')
if line.startswith('$GPRMC'):
self._handle_gprmc(line)
return True
elif line.startswith('$GPGSA'):
self._handle_gpgsa(line)
return True
return False
def _get_binary(self):
"""Gets and processes a single binary message."""
try:
message = get_message(self._serial, 1000)
except ValueError:
self._logger.error('No binary message received')
return False
parsed = parse_binary(message)
if parsed is None:
return False
self._handle_binary(parsed)
return True
@staticmethod
def _timestamp(dt):
"""Computes the Unix timestamp from a datetime object. This is needed
because Python < 3.2 doesn't have .timestamp built in.
"""
return (dt - datetime.datetime(1970, 1, 1, tzinfo=pytz.utc)) \
/ datetime.timedelta(seconds=1)
def _handle_gprmc(self, gprmc_message):
"""Handles GPRMC (recommended minimum specific GNSS data) messages."""
parts = gprmc_message.split(',')
latitude_str = parts[3]
longitude_str = parts[5]
decimal_index = latitude_str.find('.')
latitude_degrees = float(latitude_str[:decimal_index - 2])
latitude_minutes = float(latitude_str[decimal_index - 2:])
decimal_index = longitude_str.find('.')
longitude_degrees = float(longitude_str[:decimal_index - 2])
longitude_minutes = float(longitude_str[decimal_index - 2:])
latitude = latitude_degrees + latitude_minutes / 60.0
longitude = longitude_degrees + longitude_minutes / 60.0
if parts[4] == 'S':
latitude = -latitude
if parts[6] == 'W':
longitude = -longitude
speed_knots = float(parts[7])
speed_m_s = speed_knots * 0.514444444
course = float(parts[8])
# Below a certain speed, the module uses the compass to determine
# course, which is not calibrated, so we need to use our own value.
if speed_m_s < COMPASS_SPEED_CUTOFF_M_S:
course = self._last_compass_heading_d
time_ = parts[1]
hours = int(time_[0:2])
minutes = int(time_[2:4])
seconds = float(time_[4:])
date = parts[9]
day = int(date[0:2])
month = int(date[2:4])
year = int(date[4:]) + 2000
# datetime doesn't do float seconds, so we need to fudge it later
datetime_ = datetime.datetime(year,
month,
day,
hours,
minutes,
0,
tzinfo=pytz.utc)
timestamp_s = self._timestamp(datetime_) + seconds
self._telemetry.gps_reading(
latitude,
longitude,
self._hdop * 5.0, # This is a guess. Smaller HDOP is more precise.
course,
speed_m_s,
timestamp_s,
'sup800f')
def _handle_gpgsa(self, gpgsa_message):
"""Handles GSA (GNSS DOP and active satellites) messages."""
# $GPGSA,A,3,23,03,26,09,27,16,22,31,,,,,1.9,1.1,1.5*31\r\n
# type, mode M=manual or A=automatic, fix type 1=N/A 2=2D 3=3D,
# satellites used 1-12, PDOP, HDOP, VDOP + checksum
parts = gpgsa_message.split(',')
#pdop = float(parts[-3])
hdop = float(parts[-2])
#vdop = float(parts[-1].split('*')[0])
self._hdop = hdop
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 kill(self):
"""Stops any data collection."""
self._run = False
def calibrate_compass(self, seconds):
"""Requests that the car calibrate the compasss."""
if self._calibrate_compass_end_time is None:
self._calibrate_compass_end_time = time.time() + seconds
else:
self._logger.warn('Compass is already being calibrated')
def _calibrate_compass(self):
"""Calibrates the compass."""
self._logger.info('Calibrating compass; setting to binary mode')
switch_to_binary_mode(self._serial)
self._nmea_mode = False
for _ in range(10):
self._serial.readline()
maxes = [-float('inf')] * 2
mins = [float('inf')] * 2
flux_readings = []
# We should be driving for this long
while time.time() < self._calibrate_compass_end_time:
data = get_message(self._serial)
try:
binary = parse_binary(data)
except ValueError as ve:
self._logger.info(
'Unable to parse binary message {}: {}'.format(data, ve))
continue
# TODO: This should never be None, see comment in sup800f.py
if binary is None:
continue
flux_values = (
binary.magnetic_flux_ut_x,
binary.magnetic_flux_ut_y,
)
maxes = [max(a, b) for a, b in zip(maxes, flux_values)]
mins = [min(a, b) for a, b in zip(mins, flux_values)]
flux_readings.append(flux_values)
self._compass_offsets = [(max_ + min_) * 0.5
for max_, min_ in zip(maxes, mins)]
self._logger.info('Compass calibrated, offsets are {}'.format(
[round(i, 2) for i in self._compass_offsets]))
total_magnitudes = numpy.array([(x - self._compass_offsets[0])**2 +
(y - self._compass_offsets[1])**2
for x, y in flux_readings])
self._magnitude_mean = total_magnitudes.mean()
self._magnitude_std_dev = total_magnitudes.std()
self._logger.info('Magnitudes mean: {}, standard deviation: {}'.format(
round(self._magnitude_mean, 3), round(self._magnitude_std_dev, 3)))
self._calibrate_compass_end_time = None
switch_to_nmea_mode(self._serial)
self._nmea_mode = True
class Telemetry(object):
"""Provides up to date telemetry data to other modules. This class will use
the current command direction, anomalous value filtering and interpolation
to provide more accurate readings than just raw data dumps.
"""
EQUATORIAL_RADIUS_M = 6378.1370 * 1000
M_PER_D_LATITUDE = EQUATORIAL_RADIUS_M * 2.0 * math.pi / 360.0
HISTORICAL_SPEED_READINGS_COUNT = 10
HISTORICAL_ACCELEROMETER_READINGS_COUNT = 5
def __init__(self, kml_file_name=None):
self._data = {}
self._logger = AsyncLogger()
self._speed_history = collections.deque()
self._z_acceleration_g = collections.deque()
self._lock = threading.Lock()
# TODO: For the competition, just hard code the compass. For now, the
# Kalman filter should start reading in values and correct quickly.
self._location_filter = LocationFilter(0.0, 0.0, 0.0)
self._estimated_steering = 0.0
self._estimated_throttle = 0.0
self._target_steering = 0.0
self._target_throttle = 0.0
self._ignored_points = collections.defaultdict(lambda: 0)
self._ignored_points_thresholds = collections.defaultdict(lambda: 10)
consume = lambda: consume_messages(config.TELEMETRY_EXCHANGE, self.
_handle_message)
thread = threading.Thread(target=consume)
thread.name = '{}:consume_messages:{}'.format(
self.__class__.__name__, config.TELEMETRY_EXCHANGE)
thread.start()
self._course_m = None
try:
if kml_file_name is not None:
self.load_kml_from_file_name(kml_file_name)
self._logger.info(
'Loaded {} course points and {} inner objects'.format(
len(self._course_m['course']),
len(self._course_m['inner'])))
else:
self._course_m = None
if self._course_m is not None:
if len(self._course_m['course']) == 0:
self._logger.warn(
'No course defined for {}'.format(kml_file_name))
if len(self._course_m['inner']) == 0:
self._logger.warn(
'No inner obstacles defined for {}'.format(
kml_file_name))
except Exception as e:
self._logger.error('Unable to load course file: {}'.format(e))
@synchronized
def get_raw_data(self):
"""Returns the raw most recent telemetry readings."""
return self._data
@synchronized
def get_data(self, update=None):
"""Returns the approximated telemetry data."""
if update is None:
update = True
if update:
self._location_filter.update_dead_reckoning()
values = {}
for key in ('accelerometer_m_s_s',
): # Left as a loop if we want more later
if key in self._data:
values[key] = self._data[key]
values['speed_m_s'] = self._location_filter.estimated_speed()
values['heading_d'] = self._location_filter.estimated_heading()
x_m, y_m = self._location_filter.estimated_location()
values['x_m'], values['y_m'] = x_m, y_m
latitude = self.offset_y_m_to_latitude(y_m)
self._logger.debug('Estimates: {}'.format(
json.dumps({
'latitude_d':
self.offset_y_m_to_latitude(y_m),
'longitude_d':
self.offset_x_m_to_longitude(x_m, latitude),
'heading_d':
values['heading_d'],
'device_id':
'estimate'
})))
values['throttle'] = self._estimated_throttle
values['steering'] = self._estimated_steering
return values
@synchronized
def process_drive_command(self, throttle, steering):
"""Process a drive command. When the command module tells the car to do
something (e.g. drive forward and left), that data should be integrated
into the telemetry immediately, because GPS sensors and what not
normally have a slight delay.
"""
assert -1.0 <= throttle <= 1.0, 'Bad throttle in telemetry'
assert -1.0 <= steering <= 1.0, 'Bad steering in telemetry'
self._target_steering = steering
self._target_throttle = throttle
def _handle_message(self, message):
"""Stores telemetry data from messages received from some source."""
original_message = message
message = json.loads(original_message)
if 'speed_m_s' in message and message['speed_m_s'] <= MAX_SPEED_M_S:
self._speed_history.append(message['speed_m_s'])
while len(self._speed_history
) > self.HISTORICAL_SPEED_READINGS_COUNT:
self._speed_history.popleft()
if 'compass_d' in message:
self._update_estimated_drive()
self._location_filter.update_compass(message['compass_d'],
message['confidence'])
self._logger.debug(original_message)
elif 'acceleration_g_z' in message:
# TODO(skari): Detect if we've run into something
self._z_acceleration_g.append(message['acceleration_g_z'])
while len(self._z_acceleration_g
) > self.HISTORICAL_ACCELEROMETER_READINGS_COUNT:
self._z_acceleration_g.popleft()
self._logger.debug(original_message)
elif 'latitude_d' in message:
if message['speed_m_s'] < MAX_SPEED_M_S:
self._handle_gps_message(message)
self._data = message
self._logger.debug(original_message)
elif 'load_waypoints' in message:
self.load_kml_from_file_name(message['load_waypoints'])
else:
self._logger.debug(
'Unexpected message: {}'.format(original_message))
def _handle_gps_message(self, message):
"""Handles a GPS telemetry message."""
device = message['device_id']
point_m = (Telemetry.longitude_to_m_offset(message['longitude_d'],
message['latitude_d']),
Telemetry.latitude_to_m_offset(message['latitude_d']))
if self._m_point_in_course(point_m):
self._ignored_points[device] = 0
self._ignored_points_thresholds[device] = 10
message['x_m'] = point_m[0]
message['y_m'] = point_m[1]
self._update_estimated_drive()
self._location_filter.update_gps(
message['x_m'],
message['y_m'],
# The location filter supports accuracy in both directions,
# but TelemetryProducer only reports one right now. I don't
# think any of my sources report both right now.
message['accuracy_m'],
message['accuracy_m'],
message['heading_d'],
message['speed_m_s'])
else:
self._ignored_points[device] += 1
if self._ignored_points[device] > self._ignored_points_thresholds[
device]:
self._logger.info(
'Dropped {} out of bounds points from {} in a row'.format(
self._ignored_points[device], device))
self._ignored_points[device] = 0
self._ignored_points_thresholds[device] += 10
else:
self._logger.debug(
'Ignoring out of bounds point: {}'.format(point_m))
# In general, I've found that speed and heading readings tend
# to be fairly accurate, even if the actual coordinates are
# off. i.e., GPS readings are usually consistently off by N
# meters in the short term and not random all over the place.
if 'heading_d' in message and 'speed_m_s' in message:
heading_d = message['heading_d']
speed_m_s = message['speed_m_s']
# iPhone sometimes produces null if there is no speed fix yet
if heading_d is not None and speed_m_s is not None:
if speed_m_s <= MAX_SPEED_M_S:
self._location_filter.update_heading_and_speed(
heading_d, speed_m_s)
else:
self._logger.debug(
'Ignoring too high of speed value: {}'.format(
speed_m_s))
@synchronized
def is_stopped(self):
"""Determines if the RC car is moving."""
if len(self._speed_history) < self.HISTORICAL_SPEED_READINGS_COUNT:
return False
if all((speed == 0.0 for speed in self._speed_history)):
self._speed_history.clear()
return True
return False
@synchronized
def is_inverted(self):
"""Determines if the RC car is inverted."""
if len(self._z_acceleration_g
) < self.HISTORICAL_ACCELEROMETER_READINGS_COUNT:
return False
if all((z_acceleration_g < 0.0
for z_acceleration_g in self._z_acceleration_g)):
self._z_acceleration_g.clear()
return True
return False
@synchronized
def load_kml_from_file_name(self, kml_file_name):
"""Loads KML from a file name."""
directory = 'paths' + os.sep
if not kml_file_name.startswith(directory):
kml_file_name = directory + kml_file_name
if kml_file_name.endswith('.kmz'):
import zipfile
with zipfile.ZipFile(kml_file_name) as archive:
self._course_m = self._load_kml_from_stream(
archive.open('doc.kml'))
else:
with open(kml_file_name) as stream:
self._course_m = self._load_kml_from_stream(stream)
def _update_estimated_drive(self):
"""Updates the estimations of the drive state, e.g. the current
throttle and steering.
"""
self._estimated_throttle = self._target_throttle
self._estimated_steering = self._target_steering
# Also tell the location filter that we've changed
if self._estimated_throttle != self._target_throttle:
self._location_filter.manual_throttle(self._estimated_throttle *
MAX_SPEED_M_S)
# Values for Tamiya Grasshopper, from observation. This is at .5
# throttle, but we turn faster at higher speeds.
BASE_MAX_TURN_RATE_D_S = 150.0
# We always update the steering change, because we don't have sensors
# to get estimates for it from other sources for our Kalman filter
if self._estimated_throttle > 0:
self._location_filter.manual_steering(self._estimated_steering *
BASE_MAX_TURN_RATE_D_S)
elif self._estimated_throttle < 0:
self._location_filter.manual_steering(self._estimated_steering *
BASE_MAX_TURN_RATE_D_S)
else:
self._location_filter.manual_steering(0)
def _load_kml_from_stream(self, kml_stream):
"""Loads the course boundaries from a KML file."""
course = collections.defaultdict(lambda: [])
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:
self._logger.warn('Not a KML file')
return None
document = get_child(root, 'Document')
for placemark in document.iterchildren():
if not placemark.tag.endswith('Placemark'):
continue
try:
polygon = get_child(placemark, 'Polygon')
except ValueError:
# The KML also includes Path elements; those are fine
continue
bound = get_child(polygon, 'outerBoundaryIs')
ring = get_child(bound, 'LinearRing')
coordinates = get_child(ring, '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)),
))
if str(placemark.name).startswith('course'):
course['course'] = waypoints
elif str(placemark.name).startswith('inner'):
course['inner'].append(waypoints)
return course
def _m_point_in_course(self, point_m):
if self._course_m is None:
return True
if not self.point_in_polygon(point_m, self._course_m['course']):
return False
for inner in self._course_m['inner']:
if self.point_in_polygon(point_m, inner):
return False
return True
@staticmethod
def rotate_radians_clockwise(point, radians):
"""Rotates the point by radians."""
pt_x, pt_y = point
cosine = math.cos(-radians)
sine = math.sin(-radians)
return (pt_x * cosine - pt_y * sine, pt_x * sine + pt_y * cosine)
@staticmethod
def rotate_degrees_clockwise(point, degrees):
"""Rotates the point by degrees."""
pt_x, pt_y = point
cosine = math.cos(math.radians(-degrees))
sine = math.sin(math.radians(-degrees))
return (pt_x * cosine - pt_y * sine, pt_x * sine + pt_y * cosine)
@classmethod
def m_per_d_latitude(cls):
"""Returns the numbers of meters per degree of latitude."""
return cls.M_PER_D_LATITUDE
@classmethod
def latitude_to_m_per_d_longitude(cls, latitude_d, cache=None):
"""Returns the number of meters per degree longitude at a given
latitude.
"""
def calculate(latitude_d):
"""Calculates the number of meters per degree longitude at a
given latitude.
"""
radius_m = \
math.cos(math.radians(latitude_d)) * cls.EQUATORIAL_RADIUS_M
circumference_m = 2.0 * math.pi * radius_m
return circumference_m / 360.0
if cache is not None and cache:
return calculate(latitude_d)
if 'cache' not in Telemetry.latitude_to_m_per_d_longitude.__dict__:
Telemetry.latitude_to_m_per_d_longitude.__dict__['cache'] = [
latitude_d, calculate(latitude_d)
]
cache = Telemetry.latitude_to_m_per_d_longitude.cache
if cache is not None and latitude_d - 0.1 < cache[0] < latitude_d + 0.1:
return cache[1]
cache[0] = latitude_d
cache[1] = calculate(latitude_d)
return cache[1]
@classmethod
def distance_m(cls, latitude_d_1, longitude_d_1, latitude_d_2,
longitude_d_2):
"""Returns the distance in meters between two waypoints in degrees."""
diff_latitude_d = latitude_d_1 - latitude_d_2
diff_longitude_d = longitude_d_1 - longitude_d_2
diff_1_m = diff_latitude_d * cls.m_per_d_latitude()
diff_2_m = (diff_longitude_d *
Telemetry.latitude_to_m_per_d_longitude(latitude_d_1))
return math.sqrt(diff_1_m**2.0 + diff_2_m**2.0)
@staticmethod
def is_turn_left(heading_d, goal_heading_d):
"""Determines if the vehicle facing a heading in degrees needs to turn
left to reach a goal heading in degrees.
"""
pt_1 = Telemetry.rotate_degrees_clockwise((1, 0), heading_d)
pt_2 = Telemetry.rotate_degrees_clockwise((1, 0), goal_heading_d)
pt_1 = list(pt_1) + [0]
pt_2 = list(pt_2) + [0]
cross_product = \
pt_1[1] * pt_2[2] - pt_1[2] * pt_2[1] \
+ pt_1[2] * pt_2[0] - pt_1[0] * pt_2[2] \
+ pt_1[0] * pt_2[1] - pt_1[1] * pt_2[0]
if cross_product > 0:
return True
return False
@staticmethod
def relative_degrees(x_m_1, y_m_1, x_m_2, y_m_2):
"""Computes the relative degrees from the first waypoint to the second,
where north is 0.
"""
relative_y_m = float(y_m_2) - y_m_1
relative_x_m = float(x_m_2) - x_m_1
if relative_x_m == 0.0:
if relative_y_m > 0.0:
return 0.0
return 180.0
degrees = math.degrees(math.atan(relative_y_m / relative_x_m))
if relative_x_m > 0.0:
return 90.0 - degrees
else:
return 270.0 - degrees
@staticmethod
def acceleration_mss_velocity_ms_to_radius_m(acceleration_m_s_s,
velocity_m_s):
"""Converts the lateral acceleration force (accessible from the Android
phone) and the car's velocity to the car's turn radius in meters.
"""
# centripetal acceleration = velocity ^ 2 / radius
return velocity_m_s**2 / acceleration_m_s_s
@staticmethod
def acceleration_mss_velocity_ms_to_ds(acceleration_m_s_s, velocity_m_s):
"""Converts the lateral acceleration force (accessible from the Android
phone) and the car's velocity to the car's turn rate in degrees per
second.
"""
radius_m = Telemetry.acceleration_mss_velocity_ms_to_radius_m(
acceleration_m_s_s, velocity_m_s)
circumference_m = 2 * math.pi * radius_m
return circumference_m / float(velocity_m_s) * 360.0
@staticmethod
def wrap_degrees(degrees):
"""Wraps a degree value that's too high or too low."""
dividend = int(degrees) // 360
return (degrees + (dividend + 1) * 360.0) % 360.0
@staticmethod
def difference_d(heading_1_d, heading_2_d):
"""Calculates the absolute difference in degrees between two
headings.
"""
wrap_1_d = Telemetry.wrap_degrees(heading_1_d)
wrap_2_d = Telemetry.wrap_degrees(heading_2_d)
diff_d = abs(wrap_1_d - wrap_2_d)
if diff_d > 180.0:
diff_d = 360.0 - diff_d
return diff_d
@classmethod
def latitude_to_m_offset(cls, latitude_d):
"""Returns the offset in meters for a given coordinate."""
y_m = cls.m_per_d_latitude() * (latitude_d - CENTRAL_LATITUDE)
return y_m
@classmethod
def longitude_to_m_offset(cls, longitude_d, latitude_d):
"""Returns the offset in meters for a given coordinate."""
x_m = cls.latitude_to_m_per_d_longitude(latitude_d) * (
longitude_d - CENTRAL_LONGITUDE)
return x_m
@classmethod
def offset_y_m_to_latitude(cls, y_m):
"""Returns the inverse of latitude_to_m_offset."""
return y_m / cls.m_per_d_latitude() + CENTRAL_LATITUDE
@classmethod
def offset_x_m_to_longitude(cls, x_m, latitude_d):
"""Returns the inverse of longitude_to_m_offset."""
distance = cls.latitude_to_m_per_d_longitude(latitude_d)
return x_m / distance + CENTRAL_LONGITUDE
@staticmethod
def distance_to_waypoint(heading_d_1, heading_d_2, distance_travelled):
"""Calculates the distance to a waypoint, given two observed headings
to the waypoint and distance travelled in a straight line between the
two observations.
"""
m_1 = math.tan(math.radians(90.0 - heading_d_1))
m_2 = math.tan(math.radians(90.0 - heading_d_2))
x = distance_travelled / (m_1 - m_2)
hypotenuse = x / math.cos(math.radians(90.0 - heading_d_1))
return hypotenuse
@staticmethod
def offset_from_waypoint(heading_d, offset_to_waypoint_d, distance):
"""Calculates the offset (x, y) from a waypoint, given the heading of
the vehicle, the angle from the vehicle's heading to the waypoint, and
the distance to the waypoint.
"""
angle = Telemetry.wrap_degrees(180.0 + heading_d +
offset_to_waypoint_d)
return Telemetry.rotate_degrees_clockwise((0.0, distance), angle)
@staticmethod
def intersects(a, b, c, d):
"""Returns True if two line segments intersect."""
def ccw(a, b, c):
return (c[1] - a[1]) * (b[0] - a[0]) > (b[1] - a[1]) * (c[0] -
a[0])
return ccw(a, c, d) != ccw(b, c, d) and ccw(a, b, c) != ccw(a, b, d)
@staticmethod
def point_in_polygon(point, polygon):
"""Returns true if a point is strictly inside of a simple polygon."""
min_x = min(p[0] for p in polygon)
min_y = min(p[1] for p in polygon)
# To avoid degenerate parallel cases, put some arbitrary numbers here
outside = (min_x - .029238029833, min_y - .0132323872)
inside = False
next_point = iter(polygon)
next(next_point)
for p1, p2 in zip(polygon, next_point):
if Telemetry.intersects(outside, point, p1, p2):
inside = not inside
if Telemetry.intersects(outside, point, polygon[-1], polygon[0]):
inside = not inside
return inside
class Driver(object):
"""Class that implements the Driver interface."""
def __init__(self, telemetry):
self._telemetry = telemetry
self._logger = AsyncLogger()
self._throttle = 0.0
self._steering = 0.0
self._max_throttle = 1.0
with open('/dev/pi-blaster', 'w') as blaster:
blaster.write('{pin}={throttle}\n'.format(
pin=THROTTLE_GPIO_PIN, throttle=self._get_throttle(0.0)))
blaster.write('{pin}={steering}\n'.format(
pin=STEERING_GPIO_PIN, steering=self._get_steering(0.0)))
def drive(self, throttle_percentage, steering_percentage):
"""Sends a command to the RC car. Throttle should be a float between
-1.0 for reverse and 1.0 for forward. Turn should be a float between
-1.0 for left and 1.0 for right.
"""
assert -1.0 <= throttle_percentage <= 1.0
assert -1.0 <= steering_percentage <= 1.0
self._telemetry.process_drive_command(throttle_percentage,
steering_percentage)
self._logger.debug('throttle = {throttle}, turn = {turn}'.format(
throttle=throttle_percentage,
turn=steering_percentage,
))
self._throttle = throttle_percentage
self._steering = steering_percentage
self._logger.debug('Throttle: {}, steering: {}'.format(
throttle_percentage, steering_percentage))
if throttle_percentage > 0.0:
throttle = min(self._max_throttle, throttle_percentage)
else:
# Reverse is slower than forward, so allow 2x
throttle = max(-2 * self._max_throttle, throttle_percentage, -1.0)
with open('/dev/pi-blaster', 'w') as blaster:
blaster.write('{pin}={throttle}\n'.format(
pin=THROTTLE_GPIO_PIN, throttle=self._get_throttle(throttle)))
blaster.write('{pin}={steering}\n'.format(
pin=STEERING_GPIO_PIN,
steering=self._get_steering(steering_percentage)))
def get_throttle(self):
"""Returns the current throttle."""
return self._throttle
def get_turn(self):
"""Returns the current turn."""
return self._steering
@staticmethod
def _get_throttle(percentage):
"""Returns the throttle value."""
# Purposely limit the reverse in case we try to go back while still
# rolling - prevent damage to the gear box
if not (-0.5 <= percentage <= 1.0):
raise ValueError('Bad throttle: {}'.format(percentage))
return round(
(THROTTLE_NEUTRAL_US + THROTTLE_DIFF * percentage) * 0.0001, 3)
@staticmethod
def _get_steering(percentage):
"""Returns the steering value."""
if not (-1.0 <= percentage <= 1.0):
raise ValueError('Bad steering')
return round(
(STEERING_NEUTRAL_US + STEERING_DIFF * percentage) * 0.0001, 3)
def set_max_throttle(self, max_throttle):
"""Sets the maximum throttle."""
self._max_throttle = min(1.0, max_throttle)
class Command(threading.Thread): # pylint: disable=too-many-instance-attributes
"""Processes telemetry data and controls the RC car."""
VALID_COMMANDS = {'start', 'stop', 'reset', 'calibrate-compass'}
NEUTRAL_TIME_1_S = 1.0
REVERSE_TIME_S = 0.25
NEUTRAL_TIME_2_S = 0.25
NEUTRAL_TIME_3_S = 1.0
STRAIGHT_TIME_S = 8.0
def __init__(
self,
telemetry,
driver,
waypoint_generator,
sleep_time_milliseconds=None,
):
"""Create the Command thread."""
super(Command, self).__init__()
self.name = self.__class__.__name__
self._telemetry = telemetry
if sleep_time_milliseconds is None:
self._sleep_time_seconds = .02
else:
self._sleep_time_seconds = sleep_time_milliseconds / 1000.0
self._driver = driver
self._logger = AsyncLogger()
self._forwarder = CommandForwardProducer()
self._run = True
self._run_course = False
self._waypoint_generator = waypoint_generator
self._sleep_time = None
self._wake_time = None
self._start_time = None
self._camera = picamera.PiCamera()
# If the car is on the starting line (not started yet)
self._on_starting_line = True
self._commands = queue.Queue()
def callback(message):
self._commands.put(message)
# I never could figure out how to do multi consumer exchanges, and
# I only need it for two consumers... so, just forward the message
# on to the one place it needs to go
self._forwarder.forward(message)
consume = lambda: consume_messages(config.COMMAND_EXCHANGE, callback)
self._thread = threading.Thread(target=consume)
self._thread.name = '{}:consume_messages'.format(
self.__class__.__name__
)
self._thread.start()
def _handle_message(self, command):
"""Handles command messages, e.g. 'start' or 'stop'."""
if command not in self.VALID_COMMANDS:
if command.startswith('set-max-throttle'):
try:
max_throttle = float(command.split('=')[1])
self.set_max_throttle(max_throttle)
return
except Exception as exc: # pylint: disable=broad-except
self._logger.error(
'Bad throttle command: "{command}": {exc}'.format(
command=command,
exc=exc,
)
)
return
self._logger.error(
'Unknown command: "{command}"'.format(
command=command
)
)
return
if command == 'start':
try:
now = datetime.datetime.now()
file_name = '/data/{}-{}-{}_{}:{}:{}.h264'.format(
now.year,
now.month,
now.day,
now.hour,
now.minute,
now.second
)
self._camera.start_recording(file_name)
except Exception as exc: # pylint: disable=broad-except
self._logger.warning('Unable to save video: {}'.format(exc))
self.run_course()
elif command == 'stop':
threading.Thread(target=self._stop_recording).start()
self.stop()
elif command == 'reset':
self._on_starting_line = True
self.reset()
elif command == 'calibrate-compass':
self.calibrate_compass(10)
def _wait(self):
"""We just define this function separately so that it's easy to patch
when testing.
"""
self._sleep_time = time.time()
if self._wake_time is not None:
time_awake = self._sleep_time - self._wake_time
else:
time_awake = 0.0
time.sleep(max(self._sleep_time_seconds - time_awake, 0.0))
self._wake_time = time.time()
def run(self):
"""Run in a thread, controls the RC car."""
error_count = 0
if self._waypoint_generator.done():
threading.Thread(target=self._stop_recording).start()
self._logger.info('All waypoints reached')
return
while self._run:
try:
while self._run and not self._run_course:
while not self._commands.empty():
self._handle_message(self._commands.get())
if self._telemetry.is_inverted():
self._logger.info('Car is inverted, starting in 3')
def inverted_start():
while self._telemetry.is_inverted():
time.sleep(0.25)
self._logger.info('Starting in 3 seconds')
time.sleep(3)
if not self._run_course:
self._handle_message('start')
else:
self._logger.info('Inverted start aborted')
threading.Thread(target=inverted_start).start()
self._wait()
if not self._run:
return
# If we are on the starting line
if self._on_starting_line:
self._on_starting_line = False
self._logger.info(
'Driving straight for {} seconds'.format(
self.STRAIGHT_TIME_S
)
)
straight_iterator = self._straight_iterator()
while (
self._run
and self._run_course
and next(straight_iterator)
):
telemetry = self._telemetry.get_data()
if self._waypoint_generator.reached(
telemetry['x_m'],
telemetry['y_m']
):
self._logger.info('Reached waypoint')
self._waypoint_generator.next()
self._driver.drive(1.0, 0.0)
self._wait()
self._logger.info('Running course iteration')
run_iterator = self._run_iterator()
while self._run and self._run_course and next(run_iterator):
while not self._commands.empty():
self._handle_message(self._commands.get())
self._wait()
self._logger.info('Stopping course')
self._driver.drive(0.0, 0.0)
except Exception as exception: # pylint: disable=broad-except
trace_back = sys.exc_info()[2]
traces = traceback.extract_tb(trace_back)
# Find the last local file
for index in range(len(traces) - 1, -1, -1):
file_name, line_number, function_name, _ = traces[index]
if file_name.endswith('.py'):
break
trace = '{file_}:{line} {function}'.format(
file_=file_name,
line=line_number,
function=function_name
)
self._logger.warning(
'Command thread had exception from {trace}, ignoring:'
' {type_}:{message}'.format(
trace=trace,
type_=str(type(exception)),
message=str(exception),
)
)
error_count += 1
if error_count > 10:
self._logger.warning('Too many exceptions, pausing')
self.stop()
for _ in range(10):
# If we want to kill the thread or continue running the
# course again, then stop the pause
if not self._run or self._run_course:
break
time.sleep(0.5)
self.run_course()
self._logger.warning('Restarting after pause')
error_count = 0
def _straight_iterator(self, seconds=None):
"""Runs straight for a little bit."""
if seconds == None:
seconds = self.STRAIGHT_TIME_S
start_s = time.time()
while time.time() - start_s < seconds:
yield True
yield False
def _run_iterator(self):
"""Returns an iterator that drives everything."""
course_iterator = self._run_course_iterator()
while True:
if (
self._telemetry.is_stopped()
and self._start_time is not None
and time.time() - self._start_time > 2.0
):
self._logger.info(
'RC car is not moving according to speed history, reversing'
)
unstuck_iterator = self._unstuck_yourself_iterator(1.0)
while next(unstuck_iterator):
yield True
# Force the car to drive for a little while
start = time.time()
self._start_time = start
while (
self._run
and self._run_course
and time.time() < start + 2.0
and next(course_iterator)
):
yield True
yield next(course_iterator)
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 calibrate_compass(self, seconds):
"""Calibrates the compass."""
# Don't calibrate while driving
if self._run_course:
self._logger.warn("Can't configure compass while running")
return
start = time.time()
self._driver.drive(0.5, 1.0)
try:
while (
self._run
and not self._run_course
and time.time() < start + seconds
):
time.sleep(0.1)
except: # pylint: disable=bare-except
pass
self._driver.drive(0.0, 0.0)
def set_max_throttle(self, throttle):
"""Sets the maximum throttle speed."""
self._driver.set_max_throttle(throttle)
def run_course(self):
"""Starts the RC car running the course."""
self._run_course = True
self._start_time = time.time()
def stop(self):
"""Stops the RC car from running the course."""
self._driver.drive(0.0, 0.0)
self._run_course = False
def reset(self):
"""Resets the waypoints for the RC car."""
if self.is_running_course():
self._logger.warn('Tried to reset the course while running')
return
self._waypoint_generator.reset()
def kill(self):
"""Kills the thread."""
self._run = False
def is_running_course(self):
"""Returns True if we're currently navigating the course."""
return self._run_course
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 _stop_recording(self):
"""Stops recording after a little while."""
time.sleep(5)
self._camera.stop_recording()
class Sup800fTelemetry(threading.Thread):
"""Reader of GPS module that implements the TelemetryData interface."""
def __init__(self, serial):
"""Create the TelemetryData thread."""
super(Sup800fTelemetry, self).__init__()
self.name = self.__class__.__name__
self._telemetry = TelemetryProducer()
self._serial = serial
self._logger = AsyncLogger()
self._run = True
self._iterations = 0
# These initial measurements are from a calibration observation
self._compass_offsets = (-11.87, -5.97)
self._magnitude_mean = 353.310
self._magnitude_std_dev = 117.918
self._calibrate_compass_end_time = None
self._nmea_mode = True
self._last_compass_heading_d = 0.0
self._dropped_compass_messages = 0
self._dropped_threshold = 10
self._hdop = 5.0
def handle_message(message):
"""Handles command messages. Only cares about calibrate compass;
other messages are ignored.
"""
if message == 'calibrate-compass':
self.calibrate_compass(10)
consume = lambda: consume_messages(
config.COMMAND_FORWARDED_EXCHANGE,
handle_message
)
thread = threading.Thread(target=consume)
thread.name = '{}:consume_messages:{}'.format(
self.__class__.__name__,
config.COMMAND_FORWARDED_EXCHANGE
)
thread.start()
def run(self):
"""Run in a thread, hands raw telemetry readings to telemetry
instance.
"""
failed_to_switch_mode = False
while self._run:
try:
self._run_inner()
except EnvironmentError as env:
self._logger.debug('Failed to switch mode: {}'.format(env))
if not failed_to_switch_mode:
failed_to_switch_mode = True
import traceback
self._logger.debug(traceback.format_exc())
# Maybe resetting the module mode will help
try:
if self._nmea_mode:
switch_to_nmea_mode(self._serial)
else:
switch_to_binary_mode(self._serial)
except:
pass
except Exception as exc: # pylint: disable=broad-except
self._logger.warn(
'Telemetry data caught exception: {}'.format(
exc
)
)
def _run_inner(self):
"""Inner part of run."""
binary_count = 0
while self._run:
if self._calibrate_compass_end_time is not None:
self._calibrate_compass()
# NMEA mode
if self._nmea_mode:
if self._get_gprc():
switch_to_binary_mode(self._serial)
self._nmea_mode = False
else:
if self._get_binary():
binary_count += 1
if binary_count >= 3:
switch_to_nmea_mode(self._serial)
self._nmea_mode = True
binary_count = 0
def _get_gprc(self):
"""Gets and processes a single GPRC message."""
# This blocks until a new message is received
line = self._serial.readline()
try:
line = line.decode('utf-8')
except:
raise EnvironmentError('Not a UTF-8 message')
if line.startswith('$GPRMC'):
self._handle_gprmc(line)
return True
elif line.startswith('$GPGSA'):
self._handle_gpgsa(line)
return True
return False
def _get_binary(self):
"""Gets and processes a single binary message."""
try:
message = get_message(self._serial, 1000)
except ValueError:
self._logger.error('No binary message received')
return False
parsed = parse_binary(message)
if parsed is None:
return False
self._handle_binary(parsed)
return True
@staticmethod
def _timestamp(dt):
"""Computes the Unix timestamp from a datetime object. This is needed
because Python < 3.2 doesn't have .timestamp built in.
"""
return (dt - datetime.datetime(1970, 1, 1, tzinfo=pytz.utc)) \
/ datetime.timedelta(seconds=1)
def _handle_gprmc(self, gprmc_message):
"""Handles GPRMC (recommended minimum specific GNSS data) messages."""
parts = gprmc_message.split(',')
latitude_str = parts[3]
longitude_str = parts[5]
decimal_index = latitude_str.find('.')
latitude_degrees = float(latitude_str[:decimal_index - 2])
latitude_minutes = float(latitude_str[decimal_index - 2:])
decimal_index = longitude_str.find('.')
longitude_degrees = float(longitude_str[:decimal_index - 2])
longitude_minutes = float(longitude_str[decimal_index - 2:])
latitude = latitude_degrees + latitude_minutes / 60.0
longitude = longitude_degrees + longitude_minutes / 60.0
if parts[4] == 'S':
latitude = -latitude
if parts[6] == 'W':
longitude = -longitude
speed_knots = float(parts[7])
speed_m_s = speed_knots * 0.514444444
course = float(parts[8])
# Below a certain speed, the module uses the compass to determine
# course, which is not calibrated, so we need to use our own value.
if speed_m_s < COMPASS_SPEED_CUTOFF_M_S:
course = self._last_compass_heading_d
time_ = parts[1]
hours = int(time_[0:2])
minutes = int(time_[2:4])
seconds = float(time_[4:])
date = parts[9]
day = int(date[0:2])
month = int(date[2:4])
year = int(date[4:]) + 2000
# datetime doesn't do float seconds, so we need to fudge it later
datetime_ = datetime.datetime(
year,
month,
day,
hours,
minutes,
0,
tzinfo=pytz.utc
)
timestamp_s = self._timestamp(datetime_) + seconds
self._telemetry.gps_reading(
latitude,
longitude,
self._hdop * 5.0, # This is a guess. Smaller HDOP is more precise.
course,
speed_m_s,
timestamp_s,
'sup800f'
)
def _handle_gpgsa(self, gpgsa_message):
"""Handles GSA (GNSS DOP and active satellites) messages."""
# $GPGSA,A,3,23,03,26,09,27,16,22,31,,,,,1.9,1.1,1.5*31\r\n
# type, mode M=manual or A=automatic, fix type 1=N/A 2=2D 3=3D,
# satellites used 1-12, PDOP, HDOP, VDOP + checksum
parts = gpgsa_message.split(',')
#pdop = float(parts[-3])
hdop = float(parts[-2])
#vdop = float(parts[-1].split('*')[0])
self._hdop = hdop
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 kill(self):
"""Stops any data collection."""
self._run = False
def calibrate_compass(self, seconds):
"""Requests that the car calibrate the compasss."""
if self._calibrate_compass_end_time is None:
self._calibrate_compass_end_time = time.time() + seconds
else:
self._logger.warn('Compass is already being calibrated')
def _calibrate_compass(self):
"""Calibrates the compass."""
self._logger.info('Calibrating compass; setting to binary mode')
switch_to_binary_mode(self._serial)
self._nmea_mode = False
for _ in range(10):
self._serial.readline()
maxes = [-float('inf')] * 2
mins = [float('inf')] * 2
flux_readings = []
# We should be driving for this long
while time.time() < self._calibrate_compass_end_time:
data = get_message(self._serial)
try:
binary = parse_binary(data)
except ValueError as ve:
self._logger.info(
'Unable to parse binary message {}: {}'.format(
data,
ve
)
)
continue
# TODO: This should never be None, see comment in sup800f.py
if binary is None:
continue
flux_values = (
binary.magnetic_flux_ut_x,
binary.magnetic_flux_ut_y,
)
maxes = [max(a, b) for a, b in zip(maxes, flux_values)]
mins = [min(a, b) for a, b in zip(mins, flux_values)]
flux_readings.append(flux_values)
self._compass_offsets = [
(max_ + min_) * 0.5 for max_, min_ in zip(maxes, mins)
]
self._logger.info(
'Compass calibrated, offsets are {}'.format(
[round(i, 2) for i in self._compass_offsets]
)
)
total_magnitudes = numpy.array([
(x - self._compass_offsets[0]) ** 2 +
(y - self._compass_offsets[1]) ** 2
for x, y in flux_readings
])
self._magnitude_mean = total_magnitudes.mean()
self._magnitude_std_dev = total_magnitudes.std()
self._logger.info(
'Magnitudes mean: {}, standard deviation: {}'.format(
round(self._magnitude_mean, 3),
round(self._magnitude_std_dev, 3)
)
)
self._calibrate_compass_end_time = None
switch_to_nmea_mode(self._serial)
self._nmea_mode = True
def main():
"""Sets up logging, signal handling, etc. and starts the threads."""
signal.signal(signal.SIGINT, terminate)
parser = make_parser()
args = parser.parse_args()
#try:
# global POPEN
# POPEN = subprocess.Popen((
# 'raspivid', '-o', args.video, '-w', '1024', '-h', '576', '-b', '6000000', '-t', '300000'
# ))
#except Exception:
# logging.warning('Unable to save video')
concrete_logger = logging.Logger('sparkfun')
concrete_logger.setLevel(logging.DEBUG)
formatter = logging.Formatter(
'%(asctime)s:%(levelname)s %(message)s'
)
file_handler = None
try:
if os.path.exists(args.log):
os.remove(args.log)
file_handler = logging.FileHandler(args.log)
file_handler.setFormatter(formatter)
file_handler.setLevel(logging.DEBUG)
concrete_logger.addHandler(file_handler)
try:
last_log = os.path.dirname(args.log) + os.sep + 'last-log.txt'
with open(last_log, 'a') as last_log:
last_log.write(args.log + '\n')
except Exception as exc:
print('Unable to save last log information: {}'.format(exc))
except Exception as exception:
logging.warning('Could not create file log: ' + str(exception))
stdout_handler = logging.StreamHandler(sys.stdout)
if args.verbose:
stdout_handler.setLevel(logging.DEBUG)
else:
stdout_handler.setLevel(logging.INFO)
stdout_handler.setFormatter(formatter)
concrete_logger.addHandler(stdout_handler)
async_logger = AsyncLoggerReceiver(concrete_logger)
# We need to start async_logger now so that other people can log to it
async_logger.start()
time.sleep(0.1)
THREADS.append(async_logger)
web_socket_handler = WebSocketLoggingHandler()
web_socket_handler.setLevel(logging.INFO)
web_socket_handler.setFormatter(formatter)
concrete_logger.addHandler(web_socket_handler)
logger = AsyncLogger()
if sys.version_info.major < 3:
logger.warn(
'Python 2 is not officially supported, use at your own risk'
)
kml_file = args.kml_file
if kml_file is None:
logger.info(
'Setting waypoints to Solid State Depot for testing'
)
kml_file = 'solid-state-depot.kml'
if args.chase:
waypoint_generator = ChaseWaypointGenerator(
SimpleWaypointGenerator.get_waypoints_from_file_name(
kml_file
)
)
else:
waypoint_generator = ExtensionWaypointGenerator(
SimpleWaypointGenerator.get_waypoints_from_file_name(
kml_file
)
)
logger.debug('Calling start_threads')
start_threads(
waypoint_generator,
logger,
web_socket_handler,
args.max_throttle,
kml_file,
)
class Command(threading.Thread): # pylint: disable=too-many-instance-attributes
"""Processes telemetry data and controls the RC car."""
VALID_COMMANDS = {'start', 'stop', 'reset', 'calibrate-compass'}
NEUTRAL_TIME_1_S = 1.0
REVERSE_TIME_S = 0.25
NEUTRAL_TIME_2_S = 0.25
NEUTRAL_TIME_3_S = 1.0
STRAIGHT_TIME_S = 8.0
def __init__(
self,
telemetry,
driver,
waypoint_generator,
sleep_time_milliseconds=None,
):
"""Create the Command thread."""
super(Command, self).__init__()
self.name = self.__class__.__name__
self._telemetry = telemetry
if sleep_time_milliseconds is None:
self._sleep_time_seconds = .02
else:
self._sleep_time_seconds = sleep_time_milliseconds / 1000.0
self._driver = driver
self._logger = AsyncLogger()
self._forwarder = CommandForwardProducer()
self._run = True
self._run_course = False
self._waypoint_generator = waypoint_generator
self._sleep_time = None
self._wake_time = None
self._start_time = None
self._camera = picamera.PiCamera()
# If the car is on the starting line (not started yet)
self._on_starting_line = True
self._commands = queue.Queue()
def callback(message):
self._commands.put(message)
# I never could figure out how to do multi consumer exchanges, and
# I only need it for two consumers... so, just forward the message
# on to the one place it needs to go
self._forwarder.forward(message)
consume = lambda: consume_messages(config.COMMAND_EXCHANGE, callback)
self._thread = threading.Thread(target=consume)
self._thread.name = '{}:consume_messages'.format(
self.__class__.__name__)
self._thread.start()
def _handle_message(self, command):
"""Handles command messages, e.g. 'start' or 'stop'."""
if command not in self.VALID_COMMANDS:
if command.startswith('set-max-throttle'):
try:
max_throttle = float(command.split('=')[1])
self.set_max_throttle(max_throttle)
return
except Exception as exc: # pylint: disable=broad-except
self._logger.error(
'Bad throttle command: "{command}": {exc}'.format(
command=command,
exc=exc,
))
return
self._logger.error(
'Unknown command: "{command}"'.format(command=command))
return
if command == 'start':
try:
now = datetime.datetime.now()
file_name = '/data/{}-{}-{}_{}:{}:{}.h264'.format(
now.year, now.month, now.day, now.hour, now.minute,
now.second)
self._camera.start_recording(file_name)
except Exception as exc: # pylint: disable=broad-except
self._logger.warning('Unable to save video: {}'.format(exc))
self.run_course()
elif command == 'stop':
threading.Thread(target=self._stop_recording).start()
self.stop()
elif command == 'reset':
self._on_starting_line = True
self.reset()
elif command == 'calibrate-compass':
self.calibrate_compass(10)
def _wait(self):
"""We just define this function separately so that it's easy to patch
when testing.
"""
self._sleep_time = time.time()
if self._wake_time is not None:
time_awake = self._sleep_time - self._wake_time
else:
time_awake = 0.0
time.sleep(max(self._sleep_time_seconds - time_awake, 0.0))
self._wake_time = time.time()
def run(self):
"""Run in a thread, controls the RC car."""
error_count = 0
if self._waypoint_generator.done():
threading.Thread(target=self._stop_recording).start()
self._logger.info('All waypoints reached')
return
while self._run:
try:
while self._run and not self._run_course:
while not self._commands.empty():
self._handle_message(self._commands.get())
if self._telemetry.is_inverted():
self._logger.info('Car is inverted, starting in 3')
def inverted_start():
while self._telemetry.is_inverted():
time.sleep(0.25)
self._logger.info('Starting in 3 seconds')
time.sleep(3)
if not self._run_course:
self._handle_message('start')
else:
self._logger.info('Inverted start aborted')
threading.Thread(target=inverted_start).start()
self._wait()
if not self._run:
return
# If we are on the starting line
if self._on_starting_line:
self._on_starting_line = False
self._logger.info('Driving straight for {} seconds'.format(
self.STRAIGHT_TIME_S))
straight_iterator = self._straight_iterator()
while (self._run and self._run_course
and next(straight_iterator)):
telemetry = self._telemetry.get_data()
if self._waypoint_generator.reached(
telemetry['x_m'], telemetry['y_m']):
self._logger.info('Reached waypoint')
self._waypoint_generator.next()
self._driver.drive(1.0, 0.0)
self._wait()
self._logger.info('Running course iteration')
run_iterator = self._run_iterator()
while self._run and self._run_course and next(run_iterator):
while not self._commands.empty():
self._handle_message(self._commands.get())
self._wait()
self._logger.info('Stopping course')
self._driver.drive(0.0, 0.0)
except Exception as exception: # pylint: disable=broad-except
trace_back = sys.exc_info()[2]
traces = traceback.extract_tb(trace_back)
# Find the last local file
for index in range(len(traces) - 1, -1, -1):
file_name, line_number, function_name, _ = traces[index]
if file_name.endswith('.py'):
break
trace = '{file_}:{line} {function}'.format(
file_=file_name, line=line_number, function=function_name)
self._logger.warning(
'Command thread had exception from {trace}, ignoring:'
' {type_}:{message}'.format(
trace=trace,
type_=str(type(exception)),
message=str(exception),
))
error_count += 1
if error_count > 10:
self._logger.warning('Too many exceptions, pausing')
self.stop()
for _ in range(10):
# If we want to kill the thread or continue running the
# course again, then stop the pause
if not self._run or self._run_course:
break
time.sleep(0.5)
self.run_course()
self._logger.warning('Restarting after pause')
error_count = 0
def _straight_iterator(self, seconds=None):
"""Runs straight for a little bit."""
if seconds == None:
seconds = self.STRAIGHT_TIME_S
start_s = time.time()
while time.time() - start_s < seconds:
yield True
yield False
def _run_iterator(self):
"""Returns an iterator that drives everything."""
course_iterator = self._run_course_iterator()
while True:
if (self._telemetry.is_stopped() and self._start_time is not None
and time.time() - self._start_time > 2.0):
self._logger.info(
'RC car is not moving according to speed history, reversing'
)
unstuck_iterator = self._unstuck_yourself_iterator(1.0)
while next(unstuck_iterator):
yield True
# Force the car to drive for a little while
start = time.time()
self._start_time = start
while (self._run and self._run_course
and time.time() < start + 2.0
and next(course_iterator)):
yield True
yield next(course_iterator)
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 calibrate_compass(self, seconds):
"""Calibrates the compass."""
# Don't calibrate while driving
if self._run_course:
self._logger.warn("Can't configure compass while running")
return
start = time.time()
self._driver.drive(0.5, 1.0)
try:
while (self._run and not self._run_course
and time.time() < start + seconds):
time.sleep(0.1)
except: # pylint: disable=bare-except
pass
self._driver.drive(0.0, 0.0)
def set_max_throttle(self, throttle):
"""Sets the maximum throttle speed."""
self._driver.set_max_throttle(throttle)
def run_course(self):
"""Starts the RC car running the course."""
self._run_course = True
self._start_time = time.time()
def stop(self):
"""Stops the RC car from running the course."""
self._driver.drive(0.0, 0.0)
self._run_course = False
def reset(self):
"""Resets the waypoints for the RC car."""
if self.is_running_course():
self._logger.warn('Tried to reset the course while running')
return
self._waypoint_generator.reset()
def kill(self):
"""Kills the thread."""
self._run = False
def is_running_course(self):
"""Returns True if we're currently navigating the course."""
return self._run_course
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 _stop_recording(self):
"""Stops recording after a little while."""
time.sleep(5)
self._camera.stop_recording()
class Telemetry(object):
"""Provides up to date telemetry data to other modules. This class will use
the current command direction, anomalous value filtering and interpolation
to provide more accurate readings than just raw data dumps.
"""
EQUATORIAL_RADIUS_M = 6378.1370 * 1000
M_PER_D_LATITUDE = EQUATORIAL_RADIUS_M * 2.0 * math.pi / 360.0
HISTORICAL_SPEED_READINGS_COUNT = 10
HISTORICAL_ACCELEROMETER_READINGS_COUNT = 5
def __init__(self, kml_file_name=None):
self._data = {}
self._logger = AsyncLogger()
self._speed_history = collections.deque()
self._z_acceleration_g = collections.deque()
self._lock = threading.Lock()
# TODO: For the competition, just hard code the compass. For now, the
# Kalman filter should start reading in values and correct quickly.
self._location_filter = LocationFilter(0.0, 0.0, 0.0)
self._estimated_steering = 0.0
self._estimated_throttle = 0.0
self._target_steering = 0.0
self._target_throttle = 0.0
self._ignored_points = collections.defaultdict(lambda: 0)
self._ignored_points_thresholds = collections.defaultdict(lambda: 10)
consume = lambda: consume_messages(
config.TELEMETRY_EXCHANGE,
self._handle_message
)
thread = threading.Thread(target=consume)
thread.name = '{}:consume_messages:{}'.format(
self.__class__.__name__,
config.TELEMETRY_EXCHANGE
)
thread.start()
self._course_m = None
try:
if kml_file_name is not None:
self.load_kml_from_file_name(kml_file_name)
self._logger.info(
'Loaded {} course points and {} inner objects'.format(
len(self._course_m['course']),
len(self._course_m['inner'])
)
)
else:
self._course_m = None
if self._course_m is not None:
if len(self._course_m['course']) == 0:
self._logger.warn(
'No course defined for {}'.format(kml_file_name)
)
if len(self._course_m['inner']) == 0:
self._logger.warn(
'No inner obstacles defined for {}'.format(kml_file_name)
)
except Exception as e:
self._logger.error('Unable to load course file: {}'.format(e))
@synchronized
def get_raw_data(self):
"""Returns the raw most recent telemetry readings."""
return self._data
@synchronized
def get_data(self, update=None):
"""Returns the approximated telemetry data."""
if update is None:
update = True
if update:
self._location_filter.update_dead_reckoning()
values = {}
for key in ('accelerometer_m_s_s',): # Left as a loop if we want more later
if key in self._data:
values[key] = self._data[key]
values['speed_m_s'] = self._location_filter.estimated_speed()
values['heading_d'] = self._location_filter.estimated_heading()
x_m, y_m = self._location_filter.estimated_location()
values['x_m'], values['y_m'] = x_m, y_m
latitude = self.offset_y_m_to_latitude(y_m)
self._logger.debug(
'Estimates: {}'.format(
json.dumps({
'latitude_d': self.offset_y_m_to_latitude(y_m),
'longitude_d': self.offset_x_m_to_longitude(x_m, latitude),
'heading_d': values['heading_d'],
'device_id': 'estimate'
})
)
)
values['throttle'] = self._estimated_throttle
values['steering'] = self._estimated_steering
return values
@synchronized
def process_drive_command(self, throttle, steering):
"""Process a drive command. When the command module tells the car to do
something (e.g. drive forward and left), that data should be integrated
into the telemetry immediately, because GPS sensors and what not
normally have a slight delay.
"""
assert -1.0 <= throttle <= 1.0, 'Bad throttle in telemetry'
assert -1.0 <= steering <= 1.0, 'Bad steering in telemetry'
self._target_steering = steering
self._target_throttle = throttle
def _handle_message(self, message):
"""Stores telemetry data from messages received from some source."""
original_message = message
message = json.loads(original_message)
if 'speed_m_s' in message and message['speed_m_s'] <= MAX_SPEED_M_S:
self._speed_history.append(message['speed_m_s'])
while len(self._speed_history) > self.HISTORICAL_SPEED_READINGS_COUNT:
self._speed_history.popleft()
if 'compass_d' in message:
self._update_estimated_drive()
self._location_filter.update_compass(
message['compass_d'],
message['confidence']
)
self._logger.debug(original_message)
elif 'acceleration_g_z' in message:
# TODO(skari): Detect if we've run into something
self._z_acceleration_g.append(message['acceleration_g_z'])
while len(self._z_acceleration_g) > self.HISTORICAL_ACCELEROMETER_READINGS_COUNT:
self._z_acceleration_g.popleft()
self._logger.debug(original_message)
elif 'latitude_d' in message:
if message['speed_m_s'] < MAX_SPEED_M_S:
self._handle_gps_message(message)
self._data = message
self._logger.debug(original_message)
elif 'load_waypoints' in message:
self.load_kml_from_file_name(message['load_waypoints'])
else:
self._logger.debug(
'Unexpected message: {}'.format(original_message)
)
def _handle_gps_message(self, message):
"""Handles a GPS telemetry message."""
device = message['device_id']
point_m = (
Telemetry.longitude_to_m_offset(
message['longitude_d'],
message['latitude_d']
),
Telemetry.latitude_to_m_offset(message['latitude_d'])
)
if self._m_point_in_course(point_m):
self._ignored_points[device] = 0
self._ignored_points_thresholds[device] = 10
message['x_m'] = point_m[0]
message['y_m'] = point_m[1]
self._update_estimated_drive()
self._location_filter.update_gps(
message['x_m'],
message['y_m'],
# The location filter supports accuracy in both directions,
# but TelemetryProducer only reports one right now. I don't
# think any of my sources report both right now.
message['accuracy_m'],
message['accuracy_m'],
message['heading_d'],
message['speed_m_s']
)
else:
self._ignored_points[device] += 1
if self._ignored_points[device] > self._ignored_points_thresholds[device]:
self._logger.info(
'Dropped {} out of bounds points from {} in a row'.format(
self._ignored_points[device],
device
)
)
self._ignored_points[device] = 0
self._ignored_points_thresholds[device] += 10
else:
self._logger.debug(
'Ignoring out of bounds point: {}'.format(point_m)
)
# In general, I've found that speed and heading readings tend
# to be fairly accurate, even if the actual coordinates are
# off. i.e., GPS readings are usually consistently off by N
# meters in the short term and not random all over the place.
if 'heading_d' in message and 'speed_m_s' in message:
heading_d = message['heading_d']
speed_m_s = message['speed_m_s']
# iPhone sometimes produces null if there is no speed fix yet
if heading_d is not None and speed_m_s is not None:
if speed_m_s <= MAX_SPEED_M_S:
self._location_filter.update_heading_and_speed(
heading_d,
speed_m_s
)
else:
self._logger.debug(
'Ignoring too high of speed value: {}'.format(
speed_m_s
)
)
@synchronized
def is_stopped(self):
"""Determines if the RC car is moving."""
if len(self._speed_history) < self.HISTORICAL_SPEED_READINGS_COUNT:
return False
if all((speed == 0.0 for speed in self._speed_history)):
self._speed_history.clear()
return True
return False
@synchronized
def is_inverted(self):
"""Determines if the RC car is inverted."""
if len(self._z_acceleration_g) < self.HISTORICAL_ACCELEROMETER_READINGS_COUNT:
return False
if all((z_acceleration_g < 0.0 for z_acceleration_g in self._z_acceleration_g)):
self._z_acceleration_g.clear()
return True
return False
@synchronized
def load_kml_from_file_name(self, kml_file_name):
"""Loads KML from a file name."""
directory = 'paths' + os.sep
if not kml_file_name.startswith(directory):
kml_file_name = directory + kml_file_name
if kml_file_name.endswith('.kmz'):
import zipfile
with zipfile.ZipFile(kml_file_name) as archive:
self._course_m = self._load_kml_from_stream(archive.open('doc.kml'))
else:
with open(kml_file_name) as stream:
self._course_m = self._load_kml_from_stream(stream)
def _update_estimated_drive(self):
"""Updates the estimations of the drive state, e.g. the current
throttle and steering.
"""
self._estimated_throttle = self._target_throttle
self._estimated_steering = self._target_steering
# Also tell the location filter that we've changed
if self._estimated_throttle != self._target_throttle:
self._location_filter.manual_throttle(
self._estimated_throttle * MAX_SPEED_M_S
)
# Values for Tamiya Grasshopper, from observation. This is at .5
# throttle, but we turn faster at higher speeds.
BASE_MAX_TURN_RATE_D_S = 150.0
# We always update the steering change, because we don't have sensors
# to get estimates for it from other sources for our Kalman filter
if self._estimated_throttle > 0:
self._location_filter.manual_steering(
self._estimated_steering * BASE_MAX_TURN_RATE_D_S
)
elif self._estimated_throttle < 0:
self._location_filter.manual_steering(
self._estimated_steering * BASE_MAX_TURN_RATE_D_S
)
else:
self._location_filter.manual_steering(0)
def _load_kml_from_stream(self, kml_stream):
"""Loads the course boundaries from a KML file."""
course = collections.defaultdict(lambda: [])
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:
self._logger.warn('Not a KML file')
return None
document = get_child(root, 'Document')
for placemark in document.iterchildren():
if not placemark.tag.endswith('Placemark'):
continue
try:
polygon = get_child(placemark, 'Polygon')
except ValueError:
# The KML also includes Path elements; those are fine
continue
bound = get_child(polygon, 'outerBoundaryIs')
ring = get_child(bound, 'LinearRing')
coordinates = get_child(ring, '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)),
))
if str(placemark.name).startswith('course'):
course['course'] = waypoints
elif str(placemark.name).startswith('inner'):
course['inner'].append(waypoints)
return course
def _m_point_in_course(self, point_m):
if self._course_m is None:
return True
if not self.point_in_polygon(point_m, self._course_m['course']):
return False
for inner in self._course_m['inner']:
if self.point_in_polygon(point_m, inner):
return False
return True
@staticmethod
def rotate_radians_clockwise(point, radians):
"""Rotates the point by radians."""
pt_x, pt_y = point
cosine = math.cos(-radians)
sine = math.sin(-radians)
return (
pt_x * cosine - pt_y * sine,
pt_x * sine + pt_y * cosine
)
@staticmethod
def rotate_degrees_clockwise(point, degrees):
"""Rotates the point by degrees."""
pt_x, pt_y = point
cosine = math.cos(math.radians(-degrees))
sine = math.sin(math.radians(-degrees))
return (
pt_x * cosine - pt_y * sine,
pt_x * sine + pt_y * cosine
)
@classmethod
def m_per_d_latitude(cls):
"""Returns the numbers of meters per degree of latitude."""
return cls.M_PER_D_LATITUDE
@classmethod
def latitude_to_m_per_d_longitude(cls, latitude_d, cache=None):
"""Returns the number of meters per degree longitude at a given
latitude.
"""
def calculate(latitude_d):
"""Calculates the number of meters per degree longitude at a
given latitude.
"""
radius_m = \
math.cos(math.radians(latitude_d)) * cls.EQUATORIAL_RADIUS_M
circumference_m = 2.0 * math.pi * radius_m
return circumference_m / 360.0
if cache is not None and cache:
return calculate(latitude_d)
if 'cache' not in Telemetry.latitude_to_m_per_d_longitude.__dict__:
Telemetry.latitude_to_m_per_d_longitude.__dict__['cache'] = [
latitude_d,
calculate(latitude_d)
]
cache = Telemetry.latitude_to_m_per_d_longitude.cache
if cache is not None and latitude_d - 0.1 < cache[0] < latitude_d + 0.1:
return cache[1]
cache[0] = latitude_d
cache[1] = calculate(latitude_d)
return cache[1]
@classmethod
def distance_m(
cls,
latitude_d_1,
longitude_d_1,
latitude_d_2,
longitude_d_2
):
"""Returns the distance in meters between two waypoints in degrees."""
diff_latitude_d = latitude_d_1 - latitude_d_2
diff_longitude_d = longitude_d_1 - longitude_d_2
diff_1_m = diff_latitude_d * cls.m_per_d_latitude()
diff_2_m = (
diff_longitude_d
* Telemetry.latitude_to_m_per_d_longitude(latitude_d_1)
)
return math.sqrt(diff_1_m ** 2.0 + diff_2_m ** 2.0)
@staticmethod
def is_turn_left(heading_d, goal_heading_d):
"""Determines if the vehicle facing a heading in degrees needs to turn
left to reach a goal heading in degrees.
"""
pt_1 = Telemetry.rotate_degrees_clockwise(
(1, 0),
heading_d
)
pt_2 = Telemetry.rotate_degrees_clockwise(
(1, 0),
goal_heading_d
)
pt_1 = list(pt_1) + [0]
pt_2 = list(pt_2) + [0]
cross_product = \
pt_1[1] * pt_2[2] - pt_1[2] * pt_2[1] \
+ pt_1[2] * pt_2[0] - pt_1[0] * pt_2[2] \
+ pt_1[0] * pt_2[1] - pt_1[1] * pt_2[0]
if cross_product > 0:
return True
return False
@staticmethod
def relative_degrees(x_m_1, y_m_1, x_m_2, y_m_2):
"""Computes the relative degrees from the first waypoint to the second,
where north is 0.
"""
relative_y_m = float(y_m_2) - y_m_1
relative_x_m = float(x_m_2) - x_m_1
if relative_x_m == 0.0:
if relative_y_m > 0.0:
return 0.0
return 180.0
degrees = math.degrees(math.atan(relative_y_m / relative_x_m))
if relative_x_m > 0.0:
return 90.0 - degrees
else:
return 270.0 - degrees
@staticmethod
def acceleration_mss_velocity_ms_to_radius_m(
acceleration_m_s_s,
velocity_m_s
):
"""Converts the lateral acceleration force (accessible from the Android
phone) and the car's velocity to the car's turn radius in meters.
"""
# centripetal acceleration = velocity ^ 2 / radius
return velocity_m_s ** 2 / acceleration_m_s_s
@staticmethod
def acceleration_mss_velocity_ms_to_ds(
acceleration_m_s_s,
velocity_m_s
):
"""Converts the lateral acceleration force (accessible from the Android
phone) and the car's velocity to the car's turn rate in degrees per
second.
"""
radius_m = Telemetry.acceleration_mss_velocity_ms_to_radius_m(
acceleration_m_s_s,
velocity_m_s
)
circumference_m = 2 * math.pi * radius_m
return circumference_m / float(velocity_m_s) * 360.0
@staticmethod
def wrap_degrees(degrees):
"""Wraps a degree value that's too high or too low."""
dividend = int(degrees) // 360
return (degrees + (dividend + 1) * 360.0) % 360.0
@staticmethod
def difference_d(heading_1_d, heading_2_d):
"""Calculates the absolute difference in degrees between two
headings.
"""
wrap_1_d = Telemetry.wrap_degrees(heading_1_d)
wrap_2_d = Telemetry.wrap_degrees(heading_2_d)
diff_d = abs(wrap_1_d - wrap_2_d)
if diff_d > 180.0:
diff_d = 360.0 - diff_d
return diff_d
@classmethod
def latitude_to_m_offset(cls, latitude_d):
"""Returns the offset in meters for a given coordinate."""
y_m = cls.m_per_d_latitude() * (latitude_d - CENTRAL_LATITUDE)
return y_m
@classmethod
def longitude_to_m_offset(cls, longitude_d, latitude_d):
"""Returns the offset in meters for a given coordinate."""
x_m = cls.latitude_to_m_per_d_longitude(latitude_d) * (longitude_d - CENTRAL_LONGITUDE)
return x_m
@classmethod
def offset_y_m_to_latitude(cls, y_m):
"""Returns the inverse of latitude_to_m_offset."""
return y_m / cls.m_per_d_latitude() + CENTRAL_LATITUDE
@classmethod
def offset_x_m_to_longitude(cls, x_m, latitude_d):
"""Returns the inverse of longitude_to_m_offset."""
distance = cls.latitude_to_m_per_d_longitude(latitude_d)
return x_m / distance + CENTRAL_LONGITUDE
@staticmethod
def distance_to_waypoint(heading_d_1, heading_d_2, distance_travelled):
"""Calculates the distance to a waypoint, given two observed headings
to the waypoint and distance travelled in a straight line between the
two observations.
"""
m_1 = math.tan(math.radians(90.0 - heading_d_1))
m_2 = math.tan(math.radians(90.0 - heading_d_2))
x = distance_travelled / (m_1 - m_2)
hypotenuse = x / math.cos(math.radians(90.0 - heading_d_1))
return hypotenuse
@staticmethod
def offset_from_waypoint(heading_d, offset_to_waypoint_d, distance):
"""Calculates the offset (x, y) from a waypoint, given the heading of
the vehicle, the angle from the vehicle's heading to the waypoint, and
the distance to the waypoint.
"""
angle = Telemetry.wrap_degrees(180.0 + heading_d + offset_to_waypoint_d)
return Telemetry.rotate_degrees_clockwise(
(0.0, distance),
angle
)
@staticmethod
def intersects(a, b, c, d):
"""Returns True if two line segments intersect."""
def ccw(a, b, c):
return (c[1] - a[1]) * (b[0] - a[0]) > (b[1] - a[1]) * (c[0] - a[0])
return ccw(a, c, d) != ccw(b, c, d) and ccw(a, b, c) != ccw(a, b, d)
@staticmethod
def point_in_polygon(point, polygon):
"""Returns true if a point is strictly inside of a simple polygon."""
min_x = min(p[0] for p in polygon)
min_y = min(p[1] for p in polygon)
# To avoid degenerate parallel cases, put some arbitrary numbers here
outside = (min_x - .029238029833, min_y - .0132323872)
inside = False
next_point = iter(polygon)
next(next_point)
for p1, p2 in zip(polygon, next_point):
if Telemetry.intersects(outside, point, p1, p2):
inside = not inside
if Telemetry.intersects(outside, point, polygon[-1], polygon[0]):
inside = not inside
return inside
class Driver(object):
"""Class that implements the Driver interface."""
def __init__(self, telemetry):
self._telemetry = telemetry
self._logger = AsyncLogger()
self._throttle = 0.0
self._steering = 0.0
self._max_throttle = 1.0
with open('/dev/pi-blaster', 'w') as blaster:
blaster.write(
'{pin}={throttle}\n'.format(
pin=THROTTLE_GPIO_PIN,
throttle=self._get_throttle(0.0)
)
)
blaster.write(
'{pin}={steering}\n'.format(
pin=STEERING_GPIO_PIN,
steering=self._get_steering(0.0)
)
)
def drive(self, throttle_percentage, steering_percentage):
"""Sends a command to the RC car. Throttle should be a float between
-1.0 for reverse and 1.0 for forward. Turn should be a float between
-1.0 for left and 1.0 for right.
"""
assert -1.0 <= throttle_percentage <= 1.0
assert -1.0 <= steering_percentage <= 1.0
self._telemetry.process_drive_command(
throttle_percentage,
steering_percentage
)
self._logger.debug(
'throttle = {throttle}, turn = {turn}'.format(
throttle=throttle_percentage,
turn=steering_percentage,
)
)
self._throttle = throttle_percentage
self._steering = steering_percentage
self._logger.debug(
'Throttle: {}, steering: {}'.format(
throttle_percentage,
steering_percentage
)
)
if throttle_percentage > 0.0:
throttle = min(self._max_throttle, throttle_percentage)
else:
# Reverse is slower than forward, so allow 2x
throttle = max(-2 * self._max_throttle, throttle_percentage, -1.0)
with open('/dev/pi-blaster', 'w') as blaster:
blaster.write(
'{pin}={throttle}\n'.format(
pin=THROTTLE_GPIO_PIN,
throttle=self._get_throttle(throttle)
)
)
blaster.write(
'{pin}={steering}\n'.format(
pin=STEERING_GPIO_PIN,
steering=self._get_steering(steering_percentage)
)
)
def get_throttle(self):
"""Returns the current throttle."""
return self._throttle
def get_turn(self):
"""Returns the current turn."""
return self._steering
@staticmethod
def _get_throttle(percentage):
"""Returns the throttle value."""
# Purposely limit the reverse in case we try to go back while still
# rolling - prevent damage to the gear box
if not (-0.5 <= percentage <= 1.0):
raise ValueError('Bad throttle: {}'.format(percentage))
return round((THROTTLE_NEUTRAL_US + THROTTLE_DIFF * percentage) * 0.0001, 3)
@staticmethod
def _get_steering(percentage):
"""Returns the steering value."""
if not (-1.0 <= percentage <= 1.0):
raise ValueError('Bad steering')
return round((STEERING_NEUTRAL_US + STEERING_DIFF * percentage) * 0.0001, 3)
def set_max_throttle(self, max_throttle):
"""Sets the maximum throttle."""
self._max_throttle = min(1.0, max_throttle)