def test_circle_intersection_none(self):
"""Tests circle line intersection none case."""
intersections = ChaseWaypointGenerator._circle_intersection(
(-5, 0),
(5, 0),
(0, -10),
9
)
self.assertEqual(len(intersections), 0)
def test_tangent_distance_m(self):
"""Tests the tangent distance calculation."""
for point, line_point_1, line_point_2, expected in (
((0, 0), (0, 1), (1, 0), math.sqrt(2) * 0.5),
((0, 0), (0, 2), (2, 0), math.sqrt(8) * 0.5),
((0, 0), (-2, 0), (0, 2), math.sqrt(8) * 0.5),
((0, 0), (-2, 0), (-1, 1), math.sqrt(8) * 0.5),
((0, 0), (-2, 0), (0, 1), 0.8944271909),
):
for offset in self.OFFSETS:
distance_m = ChaseWaypointGenerator._tangent_distance_m(
self._add(point, offset),
self._add(line_point_1, offset),
self._add(line_point_2, offset)
)
self.assertAlmostEqual(distance_m, expected)
distance_m = ChaseWaypointGenerator._tangent_distance_m(
self._add(point, offset),
self._add(line_point_2, offset),
self._add(line_point_1, offset)
)
self.assertAlmostEqual(distance_m, expected)
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 test_circle_intersection_dual(self):
"""Tests circle line intersection two points case."""
def _almost_equal_set_of_points(list_1, list_2, offset):
"""Tests a set of points for approximate equality."""
for point_1 in list_1:
matched = False
for point_2 in list_2:
point_2 = self._add(point_2, offset)
if (
abs(point_1[0] - point_2[0]) < 0.00001
and abs(point_1[1] - point_2[1]) < 0.00001
):
matched = True
break
if not matched:
return False
return True
for p_1, p_2, circle, radius, expected in (
# Vertical
((0, -10), (0, 10), (0, 0), 1, ((0, -1), (0, 1))),
# Horizontal
((-3, 0), (300, 0), (0, 0), 2, ((2, 0), (-2, 0))),
):
for offset in self.OFFSETS:
intersections = ChaseWaypointGenerator._circle_intersection(
self._add(p_1, offset),
self._add(p_2, offset),
self._add(circle, offset),
radius
)
self.assertEqual(len(intersections), 2)
self.assertTrue(
_almost_equal_set_of_points(
intersections,
expected,
offset
),
'Calculated {}, expected {}'.format(intersections, expected)
)
intersections = ChaseWaypointGenerator._circle_intersection(
self._add(p_1, offset),
self._add(p_2, offset),
self._add(circle, offset),
radius
)
self.assertEqual(len(intersections), 2)
self.assertTrue(
_almost_equal_set_of_points(
intersections,
expected,
offset
),
'Calculated {}, expected {}'.format(intersections, expected)
)
# Diagonal
# This case is degenerate itself, because it relies on floating point
p_1, p_2, circle, radius, expected = (
(-2, 0), (0, 2), (0, 0), math.sqrt(2), (-1, 1)
)
intersections = ChaseWaypointGenerator._circle_intersection(
p_1,
p_2,
circle,
radius
)
self.assertGreaterEqual(len(intersections), 1)
for intersection in intersections:
for value, expected_value in zip(intersection, expected):
self.assertAlmostEqual(value, expected_value)
def test_get_current_waypoint(self):
"""Tests the chase waypoint generation."""
points = ((20, 20),)
generator = ChaseWaypointGenerator(points)
self.assertEqual(points[0], generator.get_current_waypoint(19, 19))
points = ((20, 20), (21, 21))
generator = ChaseWaypointGenerator(points)
self.assertEqual(points[0], generator.get_current_waypoint(19, 19))
points = ((0, 0), (0, 1), (0, 2), (0, 3))
generator = ChaseWaypointGenerator(points)
generator._current_waypoint = len(points) // 2
waypoint = generator.get_current_waypoint(-1, 0.5)
self.assertEqual(waypoint[0], 0)
waypoint = generator.get_current_waypoint(0.00000001, 0.5)
self.assertAlmostEqual(waypoint[0], 0)
self.assertGreater(waypoint[1], 0.5)
