def test_estimate_gps(self):
"""Tests that the estimating of the locations via GPS is sane."""
# I'm not sure how to independently validate these tests for accuracy.
# The best I can think of is to do some sanity tests.
heading_d = 0.0
for coordinates in ((100, 200), (200, 100), (300, 300), (50, 50)):
location_filter = LocationFilter(
coordinates[0],
coordinates[1],
heading_d
)
self.assertEqual(
location_filter.estimated_location(),
coordinates
)
new_coordinates = (150, 150)
for _ in range(6):
location_filter.update_gps(
new_coordinates[0],
new_coordinates[1],
x_accuracy_m=0.1,
y_accuracy_m=0.1,
heading_d=heading_d,
speed_m_s=0
)
for estimated, expected in zip(
location_filter.estimated_location(),
new_coordinates
):
self.assertAlmostEqual(estimated, expected, 2)
def benchmark_location_filter_update_compass():
"""Benchmark the location filter compass update."""
location_filter = LocationFilter(0.0, 0.0, 0.0)
iterations = 100
start = time.time()
for _ in range(iterations):
location_filter.update_compass(20.0)
end = time.time()
print('{} iterations of LocationFilter.update_compass, each took {:.5}'.
format(iterations, (end - start) / float(iterations)))
def benchmark_location_filter_update_dead_reckoning():
"""Benchmark the location filter with dead reckoning and no other input."""
location_filter = LocationFilter(0.0, 0.0, 0.0)
iterations = 1000
start = time.time()
for _ in range(iterations):
location_filter.update_dead_reckoning()
end = time.time()
print(
'{} iterations of LocationFilter.update_dead_reckoning, each took {:.5}'
.format(iterations, (end - start) / float(iterations)))
def benchmark_location_filter_update_dead_reckoning():
"""Benchmark the location filter with dead reckoning and no other input."""
location_filter = LocationFilter(0.0, 0.0, 0.0)
iterations = 1000
start = time.time()
for _ in range(iterations):
location_filter.update_dead_reckoning()
end = time.time()
print(
'{} iterations of LocationFilter.update_dead_reckoning, each took {:.5}'.format(
iterations,
(end - start) / float(iterations)
)
)
def benchmark_location_filter_update_compass():
"""Benchmark the location filter compass update."""
location_filter = LocationFilter(0.0, 0.0, 0.0)
iterations = 100
start = time.time()
for _ in range(iterations):
location_filter.update_compass(20.0)
end = time.time()
print(
'{} iterations of LocationFilter.update_compass, each took {:.5}'.format(
iterations,
(end - start) / float(iterations)
)
)
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, 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 test_estimate(self):
"""Tests that the estimating of the locations via both is sane."""
start_coordinates_m = (100.0, 150.0)
heading_d = 40.0
location_filter = LocationFilter(start_coordinates_m[0],
start_coordinates_m[1], heading_d)
speed_m_s = 5.0
# This would normally naturally get set by running the Kalman filter;
# we'll just manually set it now
location_filter._estimates[3].itemset(0, speed_m_s)
self.assertEqual(location_filter.estimated_location(),
start_coordinates_m)
tick_s = 0.5
step_m = speed_m_s * tick_s
step_x_m, step_y_m = Telemetry.rotate_radians_clockwise(
(0.0, step_m), math.radians(heading_d))
actual_x_m, actual_y_m = start_coordinates_m
def check_estimates():
self.assertLess(
abs(location_filter.estimated_location()[0] - actual_x_m), 0.5)
self.assertLess(
abs(location_filter.estimated_location()[1] - actual_y_m), 0.5)
for update in range(1, 21):
actual_x_m += step_x_m
actual_y_m += step_y_m
# First update by compass
measurements = numpy.matrix([0.0, 0.0, heading_d, 0.0]).transpose()
location_filter._update(measurements,
location_filter.COMPASS_OBSERVER_MATRIX,
location_filter.COMPASS_MEASUREMENT_NOISE,
tick_s)
check_estimates()
# Add some approximated GPS readings
# We'll use very tight standard deviations to try to avoid random
# test failures
measurements = numpy.matrix([
random.normalvariate(actual_x_m, 0.01),
random.normalvariate(actual_y_m, 0.01),
random.normalvariate(heading_d, 0.5),
random.normalvariate(speed_m_s, speed_m_s * 0.1)
]).transpose()
location_filter._update(
measurements,
location_filter.GPS_OBSERVER_MATRIX,
location_filter.GPS_MEASUREMENT_NOISE,
0.0 # Tick isn't used for GPS
)
check_estimates()
def test_estimate_constant_speed(self):
"""Tests that the estimating of the locations via dead reckoning at a
constant speed is sane.
"""
# I'm not sure how to independently validate these tests for accuracy.
# The best I can think of is to do some sanity tests.
start_coordinates_m = (100.0, 200.0)
heading_d = 32.0
location_filter = LocationFilter(
start_coordinates_m[0],
start_coordinates_m[1],
heading_d
)
self.assertEqual(
location_filter.estimated_location(),
start_coordinates_m
)
speed_m_s = 1.8
# This would normally naturally get set by running the Kalman filter;
# we'll just manually set it now
location_filter._estimates[3].itemset(0, speed_m_s)
self.assertEqual(
location_filter.estimated_location(),
start_coordinates_m
)
measurements = numpy.matrix( # pylint: disable=no-member
[0.0, 0.0, heading_d, 0.0]
).transpose() # z
seconds = 5
for _ in range(seconds):
location_filter._update(
measurements,
location_filter.COMPASS_OBSERVER_MATRIX,
location_filter.COMPASS_MEASUREMENT_NOISE,
time_diff_s=1.0
)
offset = Telemetry.rotate_radians_clockwise(
(0.0, speed_m_s * seconds),
math.radians(heading_d)
)
new_coordinates = [s + o for s, o in zip(start_coordinates_m, offset)]
for estimated, expected in zip(
location_filter.estimated_location(),
new_coordinates
):
self.assertAlmostEqual(estimated, expected, 2)
def test_estimate_constant_speed(self):
"""Tests that the estimating of the locations via dead reckoning at a
constant speed is sane.
"""
# I'm not sure how to independently validate these tests for accuracy.
# The best I can think of is to do some sanity tests.
start_coordinates_m = (100.0, 200.0)
heading_d = 32.0
location_filter = LocationFilter(start_coordinates_m[0],
start_coordinates_m[1], heading_d)
self.assertEqual(location_filter.estimated_location(),
start_coordinates_m)
speed_m_s = 1.8
# This would normally naturally get set by running the Kalman filter;
# we'll just manually set it now
location_filter._estimates[3].itemset(0, speed_m_s)
self.assertEqual(location_filter.estimated_location(),
start_coordinates_m)
measurements = numpy.matrix( # pylint: disable=no-member
[0.0, 0.0, heading_d, 0.0]).transpose() # z
seconds = 5
for _ in range(seconds):
location_filter._update(measurements,
location_filter.COMPASS_OBSERVER_MATRIX,
location_filter.COMPASS_MEASUREMENT_NOISE,
time_diff_s=1.0)
offset = Telemetry.rotate_radians_clockwise((0.0, speed_m_s * seconds),
math.radians(heading_d))
new_coordinates = [s + o for s, o in zip(start_coordinates_m, offset)]
for estimated, expected in zip(location_filter.estimated_location(),
new_coordinates):
self.assertAlmostEqual(estimated, expected, 2)
def test_estimate_gps(self):
"""Tests that the estimating of the locations via GPS is sane."""
# I'm not sure how to independently validate these tests for accuracy.
# The best I can think of is to do some sanity tests.
heading_d = 0.0
for coordinates in ((100, 200), (200, 100), (300, 300), (50, 50)):
location_filter = LocationFilter(coordinates[0], coordinates[1],
heading_d)
self.assertEqual(location_filter.estimated_location(), coordinates)
new_coordinates = (150, 150)
for _ in range(6):
location_filter.update_gps(new_coordinates[0],
new_coordinates[1],
x_accuracy_m=0.1,
y_accuracy_m=0.1,
heading_d=heading_d,
speed_m_s=0)
for estimated, expected in zip(
location_filter.estimated_location(), new_coordinates):
self.assertAlmostEqual(estimated, expected, 2)
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
def test_estimate(self):
"""Tests that the estimating of the locations via both is sane."""
start_coordinates_m = (100.0, 150.0)
heading_d = 40.0
location_filter = LocationFilter(
start_coordinates_m[0],
start_coordinates_m[1],
heading_d
)
speed_m_s = 5.0
# This would normally naturally get set by running the Kalman filter;
# we'll just manually set it now
location_filter._estimates[3].itemset(0, speed_m_s)
self.assertEqual(
location_filter.estimated_location(),
start_coordinates_m
)
tick_s = 0.5
step_m = speed_m_s * tick_s
step_x_m, step_y_m = Telemetry.rotate_radians_clockwise(
(0.0, step_m),
math.radians(heading_d)
)
actual_x_m, actual_y_m = start_coordinates_m
def check_estimates():
self.assertLess(
abs(location_filter.estimated_location()[0] - actual_x_m),
0.5
)
self.assertLess(
abs(location_filter.estimated_location()[1] - actual_y_m),
0.5
)
for update in range(1, 21):
actual_x_m += step_x_m
actual_y_m += step_y_m
# First update by compass
measurements = numpy.matrix([0.0, 0.0, heading_d, 0.0]).transpose()
location_filter._update(
measurements,
location_filter.COMPASS_OBSERVER_MATRIX,
location_filter.COMPASS_MEASUREMENT_NOISE,
tick_s
)
check_estimates()
# Add some approximated GPS readings
# We'll use very tight standard deviations to try to avoid random
# test failures
measurements = numpy.matrix([
random.normalvariate(actual_x_m, 0.01),
random.normalvariate(actual_y_m, 0.01),
random.normalvariate(heading_d, 0.5),
random.normalvariate(speed_m_s, speed_m_s * 0.1)
]).transpose()
location_filter._update(
measurements,
location_filter.GPS_OBSERVER_MATRIX,
location_filter.GPS_MEASUREMENT_NOISE,
0.0 # Tick isn't used for GPS
)
check_estimates()
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