def test_reached(self):
"""Tests the reached waypoint algorithm."""
points = ((19, 7), (20, 21), (-10, 20))
generator = ExtensionWaypointGenerator(points)
# Right on top should count as reached
self.assertTrue(generator.reached(points[0][0], points[0][1]))
self.assertTrue(generator.reached(points[0][0] + 0.5, points[0][1]))
self.assertTrue(generator.reached(points[0][0] - 0.5, points[0][1]))
self.assertTrue(generator.reached(points[0][0] - 0.5, points[0][1] + 0.5))
self.assertTrue(generator.reached(points[0][0] + 0.5, points[0][1] + 0.5))
# And reaching the beyond point should count too
waypoint = generator.get_current_waypoint(points[0][0], points[0][1])
self.assertTrue(generator.reached(waypoint[0], waypoint[1]))
def test_get_current_waypoint(self):
"""Tests the extension waypoint generation."""
points = ((20, 20),)
generator = ExtensionWaypointGenerator(points)
# If there's only one point, always return it
self.assertEqual(points[0], generator.get_current_waypoint(10, 10))
self.assertEqual(points[0], generator.get_current_waypoint(19, 19))
self.assertEqual(points[0], generator.get_current_waypoint(21, 19))
self.assertEqual(points[0], generator.get_current_waypoint(21, 21))
self.assertEqual(points[0], generator.get_current_waypoint(19, 21))
# Project through tests
position = (-100, 20)
points = ((0, 0), (0, 10), (10, 10), (10, 0), (0, 0), (5, 5), (5, 0), (5, -5), (0, 0))
generator = ExtensionWaypointGenerator(points)
self.assertEqual(
generator.get_current_waypoint(
position[0],
position[1]
),
points[0]
)
generator.next()
self.assertTrue(position not in points)
for index in range(1, len(points)):
point = points[index]
previous_point = points[index - 1]
true_waypoint = point
waypoint = generator.get_current_waypoint(
position[0],
position[1]
)
self.assertAlmostEqual(
Telemetry.relative_degrees(
previous_point[0],
previous_point[1],
true_waypoint[0],
true_waypoint[1]
),
Telemetry.relative_degrees(
previous_point[0],
previous_point[1],
waypoint[0],
waypoint[1]
)
)
def distance(x1, y1, x2, y2):
"""Returns distance between 2 points."""
return math.sqrt(
(x1 - x2) ** 2 + (y1 - y2) ** 2
)
self.assertLess(
distance(
previous_point[0],
previous_point[1],
true_waypoint[0],
true_waypoint[1]
) + 1.0,
distance(
previous_point[0],
previous_point[1],
waypoint[0],
waypoint[1]
)
)
generator.next()
def 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,
)
