def main():
"""Main function."""
logger = DummyLogger()
box = [(0, 0), (1, 0), (1, 1), (0, 1), (0, 0)]
waypoint_generator = SimpleWaypointGenerator(
SimpleWaypointGenerator.get_waypoints_from_file_name(
'control/paths/solid-state-depot.kmz'
)
)
waypoint_generator._waypoints.clear()
for x, y in ((x_ * .005 + 10, y_ * .005 + 10) for x_, y_ in box):
waypoint_generator._waypoints.append((x, y))
telemetry = DummyTelemetry(
logger,
waypoint_generator.get_current_waypoint(
waypoint_generator._waypoints[0][0],
waypoint_generator._waypoints[0][1]
)
)
driver = DummyDriver(telemetry, logger)
command = Command(
telemetry,
driver,
waypoint_generator,
logger,
sleep_time_milliseconds=1,
)
command.run_course()
command.start()
while not waypoint_generator.done():
time.sleep(0.1)
command.kill()
def test_zipped_files_smoke():
"""The generator should also support zipped KML files (KMZ)."""
# The generator always prepends 'paths/', so force it to drop to /tmp
archive_file_name = "../../../../../tmp/test.kmz"
with zipfile.ZipFile(archive_file_name, "w") as archive:
archive.write("paths/solid-state-depot.kml", "doc.kml")
SimpleWaypointGenerator.get_waypoints_from_file_name(archive_file_name)
def main():
"""Main function."""
logger = DummyLogger()
box = [(0, 0), (1, 0), (1, 1), (0, 1), (0, 0)]
waypoint_generator = SimpleWaypointGenerator(
SimpleWaypointGenerator.get_waypoints_from_file_name(
'control/paths/solid-state-depot.kmz'))
waypoint_generator._waypoints.clear()
for x, y in ((x_ * .005 + 10, y_ * .005 + 10) for x_, y_ in box):
waypoint_generator._waypoints.append((x, y))
telemetry = DummyTelemetry(
logger,
waypoint_generator.get_current_waypoint(
waypoint_generator._waypoints[0][0],
waypoint_generator._waypoints[0][1]))
driver = DummyDriver(telemetry, logger)
command = Command(
telemetry,
driver,
waypoint_generator,
logger,
sleep_time_milliseconds=1,
)
command.run_course()
command.start()
while not waypoint_generator.done():
time.sleep(0.1)
command.kill()
def test_zipped_files_smoke():
"""The generator should also support zipped KML files (KMZ)."""
# The generator always prepends 'paths/', so force it to drop to /tmp
archive_file_name = '../../../../../tmp/test.kmz'
with zipfile.ZipFile(archive_file_name, 'w') as archive:
archive.write('paths/solid-state-depot.kml', 'doc.kml')
SimpleWaypointGenerator.get_waypoints_from_file_name(archive_file_name)
def benchmark_command_run_course_iterator():
"""Benchmark the logic for driving the car."""
logger = DummyLogger()
telemetry = Telemetry(logger)
waypoint_generator = SimpleWaypointGenerator(
SimpleWaypointGenerator.get_waypoints_from_file_name(
'paths/solid-state-depot.kmz'
)
)
driver = DummyDriver(telemetry, logger)
command = Command(telemetry, driver, waypoint_generator, logger)
iterations = 250
start = time.time()
iterator = command._run_course_iterator()
step = None
for step in zip(range(iterations), iterator):
pass
assert step is not None
assert step[0] == iterations - 1
end = time.time()
print(
'{} iterations of Command._run_course_iterator, each took {:.5}'.format(
iterations,
(end - start) / float(iterations)
)
)
def benchmark_command_run_course_iterator():
"""Benchmark the logic for driving the car."""
logger = DummyLogger()
telemetry = Telemetry(logger)
waypoint_generator = SimpleWaypointGenerator(
SimpleWaypointGenerator.get_waypoints_from_file_name(
'paths/solid-state-depot.kmz'))
driver = DummyDriver(telemetry, logger)
command = Command(telemetry, driver, waypoint_generator, logger)
iterations = 250
start = time.time()
iterator = command._run_course_iterator()
step = None
for step in zip(range(iterations), iterator):
pass
assert step is not None
assert step[0] == iterations - 1
end = time.time()
print('{} iterations of Command._run_course_iterator, each took {:.5}'.
format(iterations, (end - start) / float(iterations)))
def test_load_waypoints(self):
"""Tests loading waypoints from a KML format file."""
coordinates_long_lat = zip(range(10), range(10, 0, -1))
coordinates_str = " ".join(("{},{},50".format(long, lat) for long, lat in coordinates_long_lat))
kml = KML_TEMPLATE.format(coordinates_str).encode("utf-8")
kml_buffer = io.BytesIO(kml)
waypoints = SimpleWaypointGenerator._load_waypoints(kml_buffer)
for m_offset, long_lat in zip(waypoints, coordinates_long_lat):
x_m_1, y_m_1 = m_offset
long_, lat = long_lat
x_m_2 = Telemetry.longitude_to_m_offset(long_)
y_m_2 = Telemetry.latitude_to_m_offset(lat)
self.assertEqual(x_m_1, x_m_2)
self.assertEqual(y_m_1, y_m_2)
def test_load_waypoints(self):
"""Tests loading waypoints from a KML format file."""
coordinates_long_lat = zip(range(10), range(10, 0, -1))
coordinates_str = ' '.join(('{},{},50'.format(long, lat)
for long, lat in coordinates_long_lat))
kml = KML_TEMPLATE.format(coordinates_str).encode('utf-8')
kml_buffer = io.BytesIO(kml)
waypoints = SimpleWaypointGenerator._load_waypoints(kml_buffer)
for m_offset, long_lat in zip(waypoints, coordinates_long_lat):
x_m_1, y_m_1 = m_offset
long_, lat = long_lat
x_m_2 = Telemetry.longitude_to_m_offset(long_)
y_m_2 = Telemetry.latitude_to_m_offset(lat)
self.assertEqual(x_m_1, x_m_2)
self.assertEqual(y_m_1, y_m_2)
def 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,
)
def make_generator(waypoints=None):
"""Returns a KML waypoint generator."""
if waypoints is None:
waypoints = [(1, 1), (5, 10)]
return SimpleWaypointGenerator(waypoints)
