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, )