def received_message(message):
"""Handler for receiving a message on the websocket."""
try:
if message.is_binary:
# TODO(2016-08-13) Log an error
return
message = json.loads(str(message))
if 'latitude_d' in message:
TelemetryProducer().gps_reading(
message['latitude_d'],
message['longitude_d'],
message['accuracy_m'],
message['heading_d'],
message['speed_m_s'],
message['timestamp_s'],
message['device_id'],
)
elif 'compass_d' in message:
TelemetryProducer().compass_reading(
message['compass_d'],
message['confidence'],
message['device_id'],
)
else:
AsyncLogger().error(
'Received unexpected web telemetry message: "{}"'.format(
message))
# We need to catch all exceptions because any that are raised will close
# the websocket
except Exception as exc: # pylint: disable=broad-except
AsyncLogger().error(
'Error processing web telemetry message "{}": {} {}'.format(
message, type(exc), exc))
def post_telemetry(self, message): # pylint: disable=no-self-use
"""End point for receiving telemetry readings. Some phones don't support
websockets, so just use plain old POST.
"""
try:
message = json.loads(str(message))
if 'latitude_d' in message:
TelemetryProducer().gps_reading(
message['latitude_d'],
message['longitude_d'],
message['accuracy_m'],
message['heading_d'],
message['speed_m_s'],
message['timestamp_s'],
message['device_id'],
)
elif 'compass_d' in message:
TelemetryProducer().compass_reading(
message['compass_d'],
message['confidence'],
message['device_id'],
)
else:
AsyncLogger().error(
'Received unexpected web telemetry message: {}'.format(
message))
except Exception as exc: # pylint: disable=broad-except
AsyncLogger().error(
'Invalid POST telemetry message "{}": {} {}'.format(
message, type(exc), exc))
return {'success': False, 'message': str(exc)}
return {'success': True}
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 __init__(self):
super(Button, self).__init__()
self.name = self.__class__.__name__
self._logger = AsyncLogger()
self._button_press_time = None
self._run = True
RPIO.setup(BUTTON_GPIO_PIN, RPIO.IN, RPIO.PUD_DOWN)
RPIO.add_interrupt_callback(BUTTON_GPIO_PIN,
self.gpio_callback,
debounce_timeout_ms=50)
self._command = CommandProducer()
def __init__(self, telemetry, waypoint_generator, port):
self._command = CommandProducer()
self._telemetry = telemetry
self._logger = AsyncLogger()
self._port = port
self._waypoint_generator = waypoint_generator
def get_ip(interface):
"""Returns the IPv4 address of a given interface."""
try:
addresses = netifaces.ifaddresses(interface)
if len(addresses) == 0:
return None
if netifaces.AF_INET not in addresses:
return None
return addresses[netifaces.AF_INET][0]['addr']
except Exception as exc: # pylint: disable=broad-except
self._logger.warn(
'Exception trying to get interface address: {exc}'.format(
exc=str(exc)))
return None
self._host_ip = None
def interface_preference(interface):
"""Ordering function that orders wireless adapters first, then
physical, then loopback.
"""
if interface.startswith('wlan') or interface == 'en1':
return 0
if interface.startswith('eth') or interface == 'en0':
return 1
if interface.startswith('lo'):
return 2
return 3
interfaces = sorted(netifaces.interfaces(), key=interface_preference)
for iface in interfaces:
self._host_ip = get_ip(iface)
if self._host_ip is not None:
self._logger.info(
'Monitor web server listening on {iface}'.format(
iface=iface))
break
if self._host_ip is None:
self._logger.error('No valid host found, listening on loopback')
self._host_ip = get_ip('lo')
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))
class Button(threading.Thread): # pylint: disable=too-few-public-methods
"""Listens for physical button presses and controls the car."""
def __init__(self):
super(Button, self).__init__()
self.name = self.__class__.__name__
self._logger = AsyncLogger()
self._button_press_time = None
self._run = True
RPIO.setup(BUTTON_GPIO_PIN, RPIO.IN, RPIO.PUD_DOWN)
RPIO.add_interrupt_callback(
BUTTON_GPIO_PIN,
self.gpio_callback,
debounce_timeout_ms=50
)
self._command = CommandProducer()
def run(self):
"""Run in a thread, waits for button presses."""
while self._run:
RPIO.wait_for_interrupts()
time.sleep(1)
def kill(self):
"""Stops the thread."""
self._run = False
RPIO.stop_waiting_for_interrupts()
def gpio_callback(self, gpio_id, value):
"""Called when the button is pressed."""
if value == BUTTON_UP:
return
self._logger.info('Button pressed: GPIO pin {pin}'.format(pin=gpio_id))
# One press to start, two within a second to stop
if self._button_press_time is None:
self._command.start()
elif time.time() - self._button_press_time < 1.0:
self._command.stop()
else:
self._command.start()
self._button_press_time = time.time()
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 __init__(self, waypoints, distance_m=None):
raise NotImplementedError(
'ChaseWaypointGenerator needs to be updated to inherit from'
' SimpleWaypointGenerator')
self._logger = AsyncLogger()
if distance_m is None:
self._distance_m = 15.0
else:
self._distance_m = distance_m
self._initial_waypoints = copy.deepcopy(waypoints)
self._waypoints = copy.deepcopy(waypoints)
self._current_waypoint = 0
self._last_x_m = None
self._last_y_m = None
class Button(threading.Thread): # pylint: disable=too-few-public-methods
"""Listens for physical button presses and controls the car."""
def __init__(self):
super(Button, self).__init__()
self.name = self.__class__.__name__
self._logger = AsyncLogger()
self._button_press_time = None
self._run = True
RPIO.setup(BUTTON_GPIO_PIN, RPIO.IN, RPIO.PUD_DOWN)
RPIO.add_interrupt_callback(BUTTON_GPIO_PIN,
self.gpio_callback,
debounce_timeout_ms=50)
self._command = CommandProducer()
def run(self):
"""Run in a thread, waits for button presses."""
while self._run:
RPIO.wait_for_interrupts()
time.sleep(1)
def kill(self):
"""Stops the thread."""
self._run = False
RPIO.stop_waiting_for_interrupts()
def gpio_callback(self, gpio_id, value):
"""Called when the button is pressed."""
if value == BUTTON_UP:
return
self._logger.info('Button pressed: GPIO pin {pin}'.format(pin=gpio_id))
# One press to start, two within a second to stop
if self._button_press_time is None:
self._command.start()
elif time.time() - self._button_press_time < 1.0:
self._command.stop()
else:
self._command.start()
self._button_press_time = time.time()
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
def __init__(self, first_way_point):
self._x_m, self._y_m = first_way_point
self._logger = AsyncLogger()
self._x_m -= 1000
self._heading = 0.0
self._last_command_time = time.time()
self._throttle = 0.0
self._turn = 0.0
self.update_count = 1000
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))
def __init__(self, telemetry, port):
self._telemetry = telemetry
logger = AsyncLogger()
self._port = port
def get_ip(interface):
"""Returns the IPv4 address of a given interface."""
try:
addresses = netifaces.ifaddresses(interface)
if len(addresses) == 0:
return None
if netifaces.AF_INET not in addresses:
return None
return addresses[netifaces.AF_INET][0]["addr"]
except Exception as exc: # pylint: disable=broad-except
logger.warn("Exception trying to get interface address: {exc}".format(exc=str(exc)))
return None
self._host_ip = None
def interface_preference(interface):
"""Ordering function that orders wireless adapters first, then
physical, then loopback.
"""
if interface.startswith("wlan") or interface == "en1":
return 0
if interface.startswith("eth") or interface == "en0":
return 1
if interface.startswith("lo"):
return 2
return 3
interfaces = sorted(netifaces.interfaces(), key=interface_preference)
for iface in interfaces:
self._host_ip = get_ip(iface)
if self._host_ip is not None:
logger.info("Web telemetry server listening on {iface}".format(iface=iface))
break
if self._host_ip is None:
logger.error("No valid host found, listening on loopback")
self._host_ip = get_ip("lo")
def __init__(self, waypoints):
self._logger = AsyncLogger()
self._waypoints = copy.deepcopy(waypoints)
self._current_waypoint_index = 0
self._logger.info(
'Loaded {length} waypoints'.format(
length=len(self._waypoints)
)
)
self._last_distance_m = float('inf')
consume = lambda: consume_messages(
config.WAYPOINT_EXCHANGE,
self._handle_message
)
thread = threading.Thread(target=consume)
thread.name = '{}:consume_messages:{}'.format(
self.__class__.__name__,
config.WAYPOINT_EXCHANGE
)
thread.start()
def __init__(self, telemetry, waypoint_generator, port):
self._command = CommandProducer()
self._telemetry = telemetry
self._logger = AsyncLogger()
self._port = port
self._waypoint_generator = waypoint_generator
def get_ip(interface):
"""Returns the IPv4 address of a given interface."""
try:
addresses = netifaces.ifaddresses(interface)
if len(addresses) == 0:
return None
if netifaces.AF_INET not in addresses:
return None
return addresses[netifaces.AF_INET][0]['addr']
except Exception as exc: # pylint: disable=broad-except
self._logger.warn(
'Exception trying to get interface address: {exc}'.format(
exc=str(exc)
)
)
return None
self._host_ip = None
def interface_preference(interface):
"""Ordering function that orders wireless adapters first, then
physical, then loopback.
"""
if interface.startswith('wlan') or interface == 'en1':
return 0
if interface.startswith('eth') or interface == 'en0':
return 1
if interface.startswith('lo'):
return 2
return 3
interfaces = sorted(netifaces.interfaces(), key=interface_preference)
for iface in interfaces:
self._host_ip = get_ip(iface)
if self._host_ip is not None:
self._logger.info(
'Monitor web server listening on {iface}'.format(
iface=iface
)
)
break
if self._host_ip is None:
self._logger.error('No valid host found, listening on loopback')
self._host_ip = get_ip('lo')
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 __init__(self):
super(Button, self).__init__()
self.name = self.__class__.__name__
self._logger = AsyncLogger()
self._button_press_time = None
self._run = True
RPIO.setup(BUTTON_GPIO_PIN, RPIO.IN, RPIO.PUD_DOWN)
RPIO.add_interrupt_callback(
BUTTON_GPIO_PIN,
self.gpio_callback,
debounce_timeout_ms=50
)
self._command = CommandProducer()
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 __init__(self, waypoints, distance_m=None):
raise NotImplementedError(
'ChaseWaypointGenerator needs to be updated to inherit from'
' SimpleWaypointGenerator'
)
self._logger = AsyncLogger()
if distance_m is None:
self._distance_m = 15.0
else:
self._distance_m = distance_m
self._initial_waypoints = copy.deepcopy(waypoints)
self._waypoints = copy.deepcopy(waypoints)
self._current_waypoint = 0
self._last_x_m = None
self._last_y_m = None
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 __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()
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 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 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 SimpleWaypointGenerator(object):
"""Loads and returns waypoints from a KML file."""
def __init__(self, waypoints):
self._logger = AsyncLogger()
self._waypoints = copy.deepcopy(waypoints)
self._current_waypoint_index = 0
self._logger.info(
'Loaded {length} waypoints'.format(
length=len(self._waypoints)
)
)
self._last_distance_m = float('inf')
consume = lambda: consume_messages(
config.WAYPOINT_EXCHANGE,
self._handle_message
)
thread = threading.Thread(target=consume)
thread.name = '{}:consume_messages:{}'.format(
self.__class__.__name__,
config.WAYPOINT_EXCHANGE
)
thread.start()
def get_current_waypoint(self, x_m, y_m): # pylint: disable=unused-argument
"""Returns the current waypoint."""
if self._current_waypoint_index < len(self._waypoints):
return self._waypoints[self._current_waypoint_index]
raise ValueError('No waypoints left')
def get_raw_waypoint(self):
"""Returns the raw waypoint. Should only be used with monitors."""
if self._current_waypoint_index < len(self._waypoints):
return self._waypoints[self._current_waypoint_index]
# Return dummy data for the monitor
return (0.0, 0.0)
def reached(self, x_m, y_m):
"""Returns True if the current waypoint has been reached."""
# I was having problems with the car driving in circles looking for the
# waypoint, so instead of having a hard cutoff of 1.5 m, count the
# waypoint as reached if the distance is < 3m and either the distance
# starts increasing, or the car gets within 1m
distance_m = math.sqrt(
(x_m - self._waypoints[self._current_waypoint_index][0]) ** 2
+ (y_m - self._waypoints[self._current_waypoint_index][1]) ** 2
)
if distance_m < 1.0:
return True
if self._last_distance_m < 3.0 and distance_m > self._last_distance_m:
# This will get overwritten next time
self._last_distance_m = float('inf')
return True
self._last_distance_m = distance_m
return False
def next(self):
"""Goes to the next waypoint."""
self._current_waypoint_index += 1
def done(self):
"""Returns True if the course is done and there are no remaining
waypoints.
"""
return self._current_waypoint_index >= len(self._waypoints)
def reset(self):
"""Resets the waypoints."""
self._current_waypoint_index = 0
def _handle_message(self, message):
"""Handles a message from the waypoint exchange."""
message = json.loads(str(message))
if 'command' not in message:
self._logger.error('Invalid waypoint message: {}'.format(message))
return
if message['command'] == 'load' and 'file' in message:
try:
self._waypoints = self.get_waypoints_from_file_name(message['file'])
self._current_waypoint_index = 0
except Exception as exc: # pylint: disable=broad-except
self._logger.error(
'Unable to load waypoints from {}: {}'.format(
message['file'],
exc
)
)
else:
self._logger.error(
'Invalid waypoint exchange message: {}'.format(message)
)
@classmethod
def get_waypoints_from_file_name(cls, kml_file_name):
"""Loads the KML waypoints from a file."""
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:
kml_stream = archive.open('doc.kml')
return cls._load_waypoints(kml_stream)
else:
with open(kml_file_name) as file_:
return cls._load_waypoints(file_)
@staticmethod
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
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 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 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 StatusApp(object):
"""Status page for the vehicle."""
def __init__(self, telemetry, waypoint_generator, port):
self._command = CommandProducer()
self._telemetry = telemetry
self._logger = AsyncLogger()
self._port = port
self._waypoint_generator = waypoint_generator
def get_ip(interface):
"""Returns the IPv4 address of a given interface."""
try:
addresses = netifaces.ifaddresses(interface)
if len(addresses) == 0:
return None
if netifaces.AF_INET not in addresses:
return None
return addresses[netifaces.AF_INET][0]['addr']
except Exception as exc: # pylint: disable=broad-except
self._logger.warn(
'Exception trying to get interface address: {exc}'.format(
exc=str(exc)))
return None
self._host_ip = None
def interface_preference(interface):
"""Ordering function that orders wireless adapters first, then
physical, then loopback.
"""
if interface.startswith('wlan') or interface == 'en1':
return 0
if interface.startswith('eth') or interface == 'en0':
return 1
if interface.startswith('lo'):
return 2
return 3
interfaces = sorted(netifaces.interfaces(), key=interface_preference)
for iface in interfaces:
self._host_ip = get_ip(iface)
if self._host_ip is not None:
self._logger.info(
'Monitor web server listening on {iface}'.format(
iface=iface))
break
if self._host_ip is None:
self._logger.error('No valid host found, listening on loopback')
self._host_ip = get_ip('lo')
@staticmethod
def get_config(monitor_root_dir):
"""Returns the required CherryPy configuration for this application."""
return {
'/': {
'tools.sessions.on': True,
'tools.staticdir.root': monitor_root_dir + '/monitor/',
},
'/static': {
'tools.staticdir.on': True,
'tools.staticdir.dir': '..' + os.sep + STATIC_DIR
},
'/ws': {
'tools.websocket.on': True,
'tools.websocket.handler_cls': WebSocketHandler
},
}
@cherrypy.expose
def index(self): # pylint: disable=no-self-use
"""Index page."""
# This is the worst templating ever, but I don't feel like it's worth
# installing a full engine just for this one substitution
index_page = None
index_file_name = MONITOR_DIR + 'index.html'
if sys.version_info.major == 2:
with open(index_file_name) as file_:
index_page = file_.read().decode('utf-8')
else:
with open(index_file_name, encoding='utf-8') as file_:
index_page = file_.read()
return index_page.replace(
'${webSocketAddress}', '{host_ip}:{port}/ws'.format(
host_ip=self._host_ip, port=self._port)).replace(
'${waypointFileOptions}', '\n'.join(
('<option value="{file}">{file}</option>'.format(
file=i) for i in os.listdir('paths')
if i.endswith('kml') or i.endswith('kmz'))))
@cherrypy.expose
@cherrypy.tools.json_out()
def telemetry_json(self):
"""Returns the telemetry data of the car."""
telemetry = self._telemetry.get_data(update=False)
waypoint_x_m, waypoint_y_m = self._waypoint_generator.get_raw_waypoint(
)
telemetry.update({
'waypoint_x_m': waypoint_x_m,
'waypoint_y_m': waypoint_y_m,
})
return telemetry
@cherrypy.expose
@cherrypy.tools.json_out()
def run(self):
"""Runs the car."""
self._check_post()
self._command.start()
self._logger.info('Received run command from web')
return {'success': True}
@cherrypy.expose
@cherrypy.tools.json_out()
def stop(self):
"""Stops the car."""
self._check_post()
self._command.stop()
self._logger.info('Received stop command from web')
return {'success': True}
@cherrypy.expose
@cherrypy.tools.json_out()
def reset(self):
"""Resets the waypoints."""
self._check_post()
self._command.reset()
self._logger.info('Received reset command from web')
return {'success': True}
@cherrypy.expose
@cherrypy.tools.json_out()
def calibrate_compass(self): # pylint: disable=no-self-use
"""Calibrates the compass."""
self._check_post()
self._logger.info('Received calibrate compass command from web')
self._command.calibrate_compass()
return {'success': True}
@cherrypy.expose
@cherrypy.tools.json_out()
def set_max_throttle(self, throttle): # pylint: disable=no-self-use
"""Hands off the maximum throttle to the command exchange."""
self._logger.info('Received throttle command from web')
self._command.set_max_throttle(throttle)
return {'success': True}
@cherrypy.expose
@cherrypy.tools.json_out()
def set_waypoints(self, kml_file_name): # pylint: disable=no-self-use
"""Hands off the file to load waypoints to the waypoint exchange."""
self._logger.info('Received set waypoints: {} command from web'.format(
kml_file_name))
WaypointProducer().load_kml_file(kml_file_name)
self._telemetry.load_kml_from_file_name(kml_file_name)
return {'success': True}
@cherrypy.expose
@cherrypy.tools.json_out()
def shut_down(self):
"""Shuts down the Pi."""
self._command.stop()
subprocess.Popen(('bash', '-c', 'sleep 10 && sudo shutdown -h now'))
os.kill(os.getpid(), signal.SIGINT)
return {'success': True}
@cherrypy.expose
def ws(self): # pylint: disable=invalid-name
"""Dummy method to tell CherryPy to expose the web socket end point."""
pass
@staticmethod
def _check_post():
"""Checks that the request method is POST."""
if cherrypy.request.method != 'POST':
cherrypy.response.headers['Allow'] = 'POST'
raise cherrypy.HTTPError(405)
def __init__(self, telemetry):
self._telemetry = telemetry
self._logger = AsyncLogger()
self._throttle = 0.0
self._steering= 0.0
class ChaseWaypointGenerator(object):
"""Generates waypoints using the 'rabbit chase' algorithm."""
def __init__(self, waypoints, distance_m=None):
raise NotImplementedError(
'ChaseWaypointGenerator needs to be updated to inherit from'
' SimpleWaypointGenerator')
self._logger = AsyncLogger()
if distance_m is None:
self._distance_m = 15.0
else:
self._distance_m = distance_m
self._initial_waypoints = copy.deepcopy(waypoints)
self._waypoints = copy.deepcopy(waypoints)
self._current_waypoint = 0
self._last_x_m = None
self._last_y_m = None
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 get_raw_waypoint(self):
"""Returns the raw waypoint. Should only be used with monitors."""
if self._current_waypoint < len(self._waypoints):
return self._waypoints[self._current_waypoint]
return (0.0, 0.0)
@staticmethod
def _circle_intersection(point_1, point_2, circle_center, circle_radius):
"""Returns an iterable list of the points of intersection between a line
and a circle, if any.
"""
# http://mathworld.wolfram.com/Circle-LineIntersection.html
x_1 = point_1[0] - circle_center[0]
x_2 = point_2[0] - circle_center[0]
y_1 = point_1[1] - circle_center[1]
y_2 = point_2[1] - circle_center[1]
d_x = x_2 - x_1
d_y = y_2 - y_1
d_r = math.sqrt(d_x**2 + d_y**2)
determinant = x_1 * y_2 - x_2 * y_1
discriminant = circle_radius**2 * d_r**2 - determinant**2
if discriminant < 0:
return ()
root = math.sqrt(discriminant)
d_r_2 = d_r**2
x_1 = (determinant * d_y + d_x * root) / d_r_2
x_2 = (determinant * d_y - d_x * root) / d_r_2
y_1 = (-determinant * d_x + abs(d_y) * root) / d_r_2
y_2 = (-determinant * d_x - abs(d_y) * root) / d_r_2
# Degenerate case of a tangent line
if x_1 == x_2 and y_1 == y_2:
return ((x_1 + circle_center[0], y_1 + circle_center[1]), )
return ((x_1 + circle_center[0], y_1 + circle_center[1]),
(x_2 + circle_center[0], y_2 + circle_center[1]))
def reached(self, x_m, y_m):
"""Returns True if the current waypoint has been reached."""
self._last_x_m = x_m
self._last_y_m = y_m
return math.sqrt(
(x_m - self._waypoints[self._current_waypoint][0])**2 +
(y_m - self._waypoints[self._current_waypoint][1])**2) < 1.5
def next(self):
"""Goes to the next waypoint."""
self._current_waypoint += 1
def done(self):
"""Returns True if the course is done and there are no remaining
waypoints.
"""
return self._current_waypoint == len(self._waypoints)
def reset(self):
"""Resets the waypoints."""
self._waypoints = copy.deepcopy(self._initial_waypoints)
self._current_waypoint = 0
@staticmethod
def _tangent_distance_m(point, line_point_1, line_point_2):
"""Calculates the distance from a point to a tangent between two
other points.
"""
# https://en.wikipedia.org/wiki/Distance_from_a_point_to_a_line
y_0, x_0 = point
y_1, x_1 = line_point_1
y_2, x_2 = line_point_2
denominator = math.sqrt((y_2 - y_1)**2 + (x_2 - x_1)**2)
if denominator == 0:
# If this happens, then the points are on top of each other
return None
return abs((y_2 - y_1) * x_0 -
(x_2 - x_1) * y_0 + x_2 * y_1 - y_2 * x_1) / denominator
class ChaseWaypointGenerator(object):
"""Generates waypoints using the 'rabbit chase' algorithm."""
def __init__(self, waypoints, distance_m=None):
raise NotImplementedError(
'ChaseWaypointGenerator needs to be updated to inherit from'
' SimpleWaypointGenerator'
)
self._logger = AsyncLogger()
if distance_m is None:
self._distance_m = 15.0
else:
self._distance_m = distance_m
self._initial_waypoints = copy.deepcopy(waypoints)
self._waypoints = copy.deepcopy(waypoints)
self._current_waypoint = 0
self._last_x_m = None
self._last_y_m = None
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 get_raw_waypoint(self):
"""Returns the raw waypoint. Should only be used with monitors."""
if self._current_waypoint < len(self._waypoints):
return self._waypoints[self._current_waypoint]
return (0.0, 0.0)
@staticmethod
def _circle_intersection(point_1, point_2, circle_center, circle_radius):
"""Returns an iterable list of the points of intersection between a line
and a circle, if any.
"""
# http://mathworld.wolfram.com/Circle-LineIntersection.html
x_1 = point_1[0] - circle_center[0]
x_2 = point_2[0] - circle_center[0]
y_1 = point_1[1] - circle_center[1]
y_2 = point_2[1] - circle_center[1]
d_x = x_2 - x_1
d_y = y_2 - y_1
d_r = math.sqrt(d_x ** 2 + d_y ** 2)
determinant = x_1 * y_2 - x_2 * y_1
discriminant = circle_radius ** 2 * d_r ** 2 - determinant ** 2
if discriminant < 0:
return ()
root = math.sqrt(discriminant)
d_r_2 = d_r ** 2
x_1 = (determinant * d_y + d_x * root) / d_r_2
x_2 = (determinant * d_y - d_x * root) / d_r_2
y_1 = (-determinant * d_x + abs(d_y) * root) / d_r_2
y_2 = (-determinant * d_x - abs(d_y) * root) / d_r_2
# Degenerate case of a tangent line
if x_1 == x_2 and y_1 == y_2:
return ((x_1 + circle_center[0], y_1 + circle_center[1]),)
return (
(x_1 + circle_center[0], y_1 + circle_center[1]),
(x_2 + circle_center[0], y_2 + circle_center[1])
)
def reached(self, x_m, y_m):
"""Returns True if the current waypoint has been reached."""
self._last_x_m = x_m
self._last_y_m = y_m
return math.sqrt(
(x_m - self._waypoints[self._current_waypoint][0]) ** 2 +
(y_m - self._waypoints[self._current_waypoint][1]) ** 2
) < 1.5
def next(self):
"""Goes to the next waypoint."""
self._current_waypoint += 1
def done(self):
"""Returns True if the course is done and there are no remaining
waypoints.
"""
return self._current_waypoint == len(self._waypoints)
def reset(self):
"""Resets the waypoints."""
self._waypoints = copy.deepcopy(self._initial_waypoints)
self._current_waypoint = 0
@staticmethod
def _tangent_distance_m(point, line_point_1, line_point_2):
"""Calculates the distance from a point to a tangent between two
other points.
"""
# https://en.wikipedia.org/wiki/Distance_from_a_point_to_a_line
y_0, x_0 = point
y_1, x_1 = line_point_1
y_2, x_2 = line_point_2
denominator = math.sqrt((y_2 - y_1) ** 2 + (x_2 - x_1) ** 2)
if denominator == 0:
# If this happens, then the points are on top of each other
return None
return abs(
(y_2 - y_1) * x_0 - (x_2 - x_1) * y_0
+ x_2 * y_1 - y_2 * x_1
) / denominator
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 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 StatusApp(object):
"""Status page for the vehicle."""
def __init__(self, telemetry, waypoint_generator, port):
self._command = CommandProducer()
self._telemetry = telemetry
self._logger = AsyncLogger()
self._port = port
self._waypoint_generator = waypoint_generator
def get_ip(interface):
"""Returns the IPv4 address of a given interface."""
try:
addresses = netifaces.ifaddresses(interface)
if len(addresses) == 0:
return None
if netifaces.AF_INET not in addresses:
return None
return addresses[netifaces.AF_INET][0]['addr']
except Exception as exc: # pylint: disable=broad-except
self._logger.warn(
'Exception trying to get interface address: {exc}'.format(
exc=str(exc)
)
)
return None
self._host_ip = None
def interface_preference(interface):
"""Ordering function that orders wireless adapters first, then
physical, then loopback.
"""
if interface.startswith('wlan') or interface == 'en1':
return 0
if interface.startswith('eth') or interface == 'en0':
return 1
if interface.startswith('lo'):
return 2
return 3
interfaces = sorted(netifaces.interfaces(), key=interface_preference)
for iface in interfaces:
self._host_ip = get_ip(iface)
if self._host_ip is not None:
self._logger.info(
'Monitor web server listening on {iface}'.format(
iface=iface
)
)
break
if self._host_ip is None:
self._logger.error('No valid host found, listening on loopback')
self._host_ip = get_ip('lo')
@staticmethod
def get_config(monitor_root_dir):
"""Returns the required CherryPy configuration for this application."""
return {
'/': {
'tools.sessions.on': True,
'tools.staticdir.root': monitor_root_dir + '/monitor/',
},
'/static': {
'tools.staticdir.on': True,
'tools.staticdir.dir': '..' + os.sep + STATIC_DIR
},
'/ws': {
'tools.websocket.on': True,
'tools.websocket.handler_cls': WebSocketHandler
},
}
@cherrypy.expose
def index(self): # pylint: disable=no-self-use
"""Index page."""
# This is the worst templating ever, but I don't feel like it's worth
# installing a full engine just for this one substitution
index_page = None
index_file_name = MONITOR_DIR + 'index.html'
if sys.version_info.major == 2:
with open(index_file_name) as file_:
index_page = file_.read().decode('utf-8')
else:
with open(index_file_name, encoding='utf-8') as file_:
index_page = file_.read()
return index_page.replace(
'${webSocketAddress}',
'{host_ip}:{port}/ws'.format(
host_ip=self._host_ip,
port=self._port
)
).replace(
'${waypointFileOptions}',
'\n'.join((
'<option value="{file}">{file}</option>'.format(file=i)
for i in os.listdir('paths')
if i.endswith('kml') or i.endswith('kmz')
))
)
@cherrypy.expose
@cherrypy.tools.json_out()
def telemetry_json(self):
"""Returns the telemetry data of the car."""
telemetry = self._telemetry.get_data(update=False)
waypoint_x_m, waypoint_y_m = self._waypoint_generator.get_raw_waypoint()
telemetry.update({
'waypoint_x_m': waypoint_x_m,
'waypoint_y_m': waypoint_y_m,
})
return telemetry
@cherrypy.expose
@cherrypy.tools.json_out()
def run(self):
"""Runs the car."""
self._check_post()
self._command.start()
self._logger.info('Received run command from web')
return {'success': True}
@cherrypy.expose
@cherrypy.tools.json_out()
def stop(self):
"""Stops the car."""
self._check_post()
self._command.stop()
self._logger.info('Received stop command from web')
return {'success': True}
@cherrypy.expose
@cherrypy.tools.json_out()
def reset(self):
"""Resets the waypoints."""
self._check_post()
self._command.reset()
self._logger.info('Received reset command from web')
return {'success': True}
@cherrypy.expose
@cherrypy.tools.json_out()
def calibrate_compass(self): # pylint: disable=no-self-use
"""Calibrates the compass."""
self._check_post()
self._logger.info('Received calibrate compass command from web')
self._command.calibrate_compass()
return {'success': True}
@cherrypy.expose
@cherrypy.tools.json_out()
def set_max_throttle(self, throttle): # pylint: disable=no-self-use
"""Hands off the maximum throttle to the command exchange."""
self._logger.info('Received throttle command from web')
self._command.set_max_throttle(throttle)
return {'success': True}
@cherrypy.expose
@cherrypy.tools.json_out()
def set_waypoints(self, kml_file_name): # pylint: disable=no-self-use
"""Hands off the file to load waypoints to the waypoint exchange."""
self._logger.info(
'Received set waypoints: {} command from web'.format(
kml_file_name
)
)
WaypointProducer().load_kml_file(kml_file_name)
self._telemetry.load_kml_from_file_name(kml_file_name)
return {'success': True}
@cherrypy.expose
@cherrypy.tools.json_out()
def shut_down(self):
"""Shuts down the Pi."""
self._command.stop()
subprocess.Popen(('bash', '-c', 'sleep 10 && sudo shutdown -h now'))
os.kill(os.getpid(), signal.SIGINT)
return {'success': True}
@cherrypy.expose
def ws(self): # pylint: disable=invalid-name
"""Dummy method to tell CherryPy to expose the web socket end point."""
pass
@staticmethod
def _check_post():
"""Checks that the request method is POST."""
if cherrypy.request.method != 'POST':
cherrypy.response.headers['Allow'] = 'POST'
raise cherrypy.HTTPError(405)
