diff --git a/autopilot.py b/autopilot.py index 8efc298..201de7f 100644 --- a/autopilot.py +++ b/autopilot.py @@ -91,6 +91,8 @@ class AutoPilot(Pilot): self.points = points self.target_idx = 0 + self.target_reached_threshold = 5 + self.direct_step_threshold = 35 self.timer = Timer() self.chunk_points = np.array([[chunk.pos.x, chunk.pos.y] for chunk in self.chunks]) @@ -355,13 +357,13 @@ class AutoPilot(Pilot): return command def make_command(self) -> PilotCommand: - # Расстояние до цели + # Расстояние до цели в пикселях карты. distance_to_target = math.sqrt( (self.points[self.target_idx][0] - self.pos.x) ** 2 + (self.points[self.target_idx][1] - self.pos.y) ** 2 ) * self.pixel_ratio - if distance_to_target < 35: + if distance_to_target < self.target_reached_threshold: self.target_idx += 1 if self.target_idx == len(self.points): @@ -370,10 +372,24 @@ class AutoPilot(Pilot): distance_to_target = math.sqrt( (self.points[self.target_idx][0] - self.pos.x) ** 2 + (self.points[self.target_idx][1] - self.pos.y) ** 2 - ) + ) * self.pixel_ratio angle_trajectory = self.pos.yaw + math.pi / 2 + # Вычисляем угол к цели + target_angle = math.atan2(self.points[self.target_idx][1] - self.pos.y, self.points[self.target_idx][0] - self.pos.x) + + # Вычисляем разность углов (направление поворота) + angle_diff = target_angle - angle_trajectory + + # Нормализуем разность углов в диапазон [-π, π] + angle_diff %= 2 * math.pi + if angle_diff >= math.pi: + angle_diff -= 2 * math.pi + + if distance_to_target < self.direct_step_threshold: + return PilotCommand(angle_diff, distance_to_target, False, self.timer.get_elapsed()) + # Проверка на слепую зону R = 120 blind = np.array([ @@ -394,19 +410,6 @@ class AutoPilot(Pilot): if np.min(blind) < R: return PilotCommand(0, 10, False, self.timer.get_elapsed()) - - - # Вычисляем угол к цели - target_angle = math.atan2(self.points[self.target_idx][1] - self.pos.y, self.points[self.target_idx][0] - self.pos.x) - - # Вычисляем разность углов (направление поворота) - angle_diff = target_angle - angle_trajectory - - # Нормализуем разность углов в диапазон [-π, π] - angle_diff %= 2 * math.pi - if angle_diff >= math.pi: - angle_diff -= 2 * math.pi - d_r = max(5, min(10., distance_to_target / 2)) d_a_limit = np.radians(5) diff --git a/main.py b/main.py index a5e8534..df3286b 100644 --- a/main.py +++ b/main.py @@ -26,6 +26,11 @@ def get_map(map_name: str = 'google', lat=49.103814, lon=55.794258, zoom=18): if map_name == 'yandex': return YandexMap(lat, lon, zoom) return None +def get_pixel_ratio(map_name: str, zoom: int) -> float: + if map_name == 'google': return constants.GOOGLE_PIXEL_RATIO[zoom] + if map_name == 'yandex': return constants.YANDEX_PIXEL_RATIO[zoom] + raise ValueError(f"Unknown map provider: {map_name}") + def make_global_photo(filename, map_name: str = 'google', lat=49.103814, lon=55.794258, zoom=13): online_map: YandexMap | GoogleMap = get_map(map_name, lat, lon, zoom) online_map.save_photo(filename) @@ -53,6 +58,13 @@ def make_test_run_dir(name: str) -> Path: run_dir.mkdir(parents=True, exist_ok=True) return run_dir +def calc_polyline_length(points) -> float: + points = np.array(points) + if len(points) < 2: + return 0.0 + segments = points[1:] - points[:-1] + return float(np.hypot(segments[:, 0], segments[:, 1]).sum()) + def move_map_safely(online_map: YandexMap | GoogleMap, dx: float, dy: float, step: int = 300): """Двигает карту короткими drag-шагами, чтобы Selenium не выходил за viewport.""" steps = max(1, int(np.ceil(max(abs(dx), abs(dy)) / step))) @@ -76,7 +88,10 @@ def build(name: str, map_name: str, lat: float, lon: float): shutil.copyfile('map.jpg', map_path) points = get_trajectory_points(str(map_path)) - online_map: YandexMap | GoogleMap = get_map(map_name, lat, lon, 15) + map_zoom = 15 + flight_zoom = 18 + map_pixel_ratio = get_pixel_ratio(map_name, map_zoom) + online_map: YandexMap | GoogleMap = get_map(map_name, lat, lon, map_zoom) width, height = online_map.get_size() @@ -92,7 +107,7 @@ def build(name: str, map_name: str, lat: float, lon: float): online_map.scroll(0.5, 0.5, 10, True) sleep(2) geo = online_map.get_geolocation() - online_map.open(geo['lat'], geo['lon'], 18) + online_map.open(geo['lat'], geo['lon'], flight_zoom) sleep(5) points_coords_pixel = points_coords.copy() / online_map.pixel_ratio @@ -110,6 +125,9 @@ def build(name: str, map_name: str, lat: float, lon: float): for i in range(5000): if command.stop: + final_pos = Position(points_coords[-1][0], points_coords[-1][1]) + if not positions or np.hypot(positions[-1].x - final_pos.x, positions[-1].y - final_pos.y) > 1: + positions.append(final_pos) break chunk = simulator.get_chunk() @@ -117,6 +135,12 @@ def build(name: str, map_name: str, lat: float, lon: float): command = pilot.make_command() command.velocity /= online_map.pixel_ratio + if command.stop: + final_pos = Position(points_coords[-1][0], points_coords[-1][1]) + if not positions or np.hypot(positions[-1].x - final_pos.x, positions[-1].y - final_pos.y) > 1: + positions.append(final_pos) + break + print("Position:", simulator.pos) # Save Image @@ -137,7 +161,14 @@ def build(name: str, map_name: str, lat: float, lon: float): 'map_image': 'map.jpg', 'map_size': (width, height), 'map_zero_point': points[0], - 'map_extent': make_map_extent(width, height, points[0], online_map.pixel_ratio), + 'map_provider': map_name, + 'map_zoom': map_zoom, + 'flight_zoom': flight_zoom, + 'map_pixel_ratio': map_pixel_ratio, + 'flight_pixel_ratio': online_map.pixel_ratio, + 'map_extent': make_map_extent(width, height, points[0], map_pixel_ratio), + 'route_length': calc_polyline_length(points_coords), + 'build_flight_length': calc_polyline_length([[pos.x, pos.y] for pos in positions]), } print(points_coords) @@ -178,8 +209,11 @@ def run( chunks: list[VisionChunk] = [] for i in range(len(data['chunk_positions'])): pos = data['chunk_positions'][i] + chunk_path = dir_chunks / f"chunk_{i}.png" + if not chunk_path.exists(): + continue if len(chunks) == 0 or np.hypot(chunks[-1].pos.x - pos.x, chunks[-1].pos.y - pos.y) > ref_min_distance: - chunk = VisionChunk.load_image(dir_chunks / f"chunk_{i}.png") + chunk = VisionChunk.load_image(chunk_path) chunk.pos = data['chunk_positions'][i] / online_map.pixel_ratio chunks.append(chunk) @@ -192,9 +226,13 @@ def run( print("R: ", r) forward_points = data['points'] + planned_route_length = float(data.get('route_length', calc_polyline_length(forward_points))) + build_flight_length = float(data.get('build_flight_length', calc_polyline_length([[pos.x, pos.y] for pos in data.get('chunk_positions', [])]))) return_points = forward_points[::-1].copy() points = return_points / online_map.pixel_ratio print("READ RTH POINTS:", points) + print("Saved planned route length:", planned_route_length) + print("Saved build flight length:", build_flight_length) vis_manager = VisualizationManager() map_path = dir / data.get('map_image', 'map.jpg') @@ -203,9 +241,12 @@ def run( build_trajectory = np.array([[pos.x, pos.y] for pos in data.get('chunk_positions', [])]) if len(build_trajectory) == 0: build_trajectory = forward_points + map_extent = data['map_extent'] + if 'map_pixel_ratio' in data and 'map_size' in data and 'map_zero_point' in data: + map_extent = make_map_extent(data['map_size'][0], data['map_size'][1], data['map_zero_point'], data['map_pixel_ratio']) vis_manager.set_route_map( map_path, - data['map_extent'], + map_extent, build_trajectory, turn_point, forward_points[0], @@ -227,6 +268,10 @@ def run( start_position.yaw += np.pi simulator.pos = start_position.copy() pilot.pos = start_position.copy() + if len(points) > 1: + distance_to_first_return_point = np.hypot(points[0][0] - pilot.pos.x, points[0][1] - pilot.pos.y) * online_map.pixel_ratio + if distance_to_first_return_point < pilot.direct_step_threshold: + pilot.target_idx = 1 move_map_safely(online_map, start_position.x, start_position.y) vis_manager.update_rth_trajectory(turn_position.x, turn_position.y) @@ -243,6 +288,9 @@ def run( errors = [] start_point = forward_points[0] final_return_error = None + return_route_length = 0.0 + prev_return_point = np.array([turn_position.x, turn_position.y], dtype=float) + odometry_warmup_frames = 2 sleep(1) @@ -257,6 +305,9 @@ def run( chunk = simulator.get_chunk() command = pilot.handle(chunk) + if i < odometry_warmup_frames: + pilot.pos = simulator.pos.copy() + command = pilot.make_command() command.velocity /= online_map.pixel_ratio proc_time = np.append(proc_time, command.proccessing_time) @@ -268,12 +319,17 @@ def run( vis_manager.pause(0.2) vis_manager.set_target_index(pilot.target_idx) - vis_manager.update_drone_trajectory(pilot.pos.x * online_map.pixel_ratio, pilot.pos.y * online_map.pixel_ratio) - vis_manager.update_global_map(simulator.pos.x * online_map.pixel_ratio, simulator.pos.y * online_map.pixel_ratio) - vis_manager.update_rth_trajectory(simulator.pos.x * online_map.pixel_ratio, simulator.pos.y * online_map.pixel_ratio) - vis_manager.update_error_plot(i, pilot.pos.x * online_map.pixel_ratio, pilot.pos.y * online_map.pixel_ratio, simulator.pos.x * online_map.pixel_ratio, simulator.pos.y * online_map.pixel_ratio) + current_return_point = np.array([simulator.pos.x * online_map.pixel_ratio, simulator.pos.y * online_map.pixel_ratio], dtype=float) + return_route_length += float(np.hypot(*(current_return_point - prev_return_point))) + prev_return_point = current_return_point - errors.append(np.hypot((pilot.pos.x - simulator.pos.x) * online_map.pixel_ratio, (pilot.pos.y - simulator.pos.y) * online_map.pixel_ratio)) + vis_manager.update_drone_trajectory(pilot.pos.x * online_map.pixel_ratio, pilot.pos.y * online_map.pixel_ratio) + vis_manager.update_global_map(current_return_point[0], current_return_point[1]) + vis_manager.update_rth_trajectory(current_return_point[0], current_return_point[1]) + + if i >= odometry_warmup_frames: + vis_manager.update_error_plot(i, pilot.pos.x * online_map.pixel_ratio, pilot.pos.y * online_map.pixel_ratio, current_return_point[0], current_return_point[1]) + errors.append(np.hypot((pilot.pos.x - simulator.pos.x) * online_map.pixel_ratio, (pilot.pos.y - simulator.pos.y) * online_map.pixel_ratio)) vis_manager.update_display() vis_manager.pause(0.2) @@ -283,29 +339,42 @@ def run( print("Average FPS:", 1 / last_proc_times.mean()) print("Pilot coords:", pilot.pos) print("Simulator coords:", simulator.pos) + if command.stop: + break sleep(0.5) simulator.handle(command.dangle, command.velocity) final_point = np.array([simulator.pos.x * online_map.pixel_ratio, simulator.pos.y * online_map.pixel_ratio]) final_return_error = float(np.hypot(final_point[0] - start_point[0], final_point[1] - start_point[1])) + full_route_length = build_flight_length + return_route_length vis_manager.set_final_point(final_point[0], final_point[1], final_return_error) print("Errors:", errors) print("MSE:", (np.array(errors) ** 2).mean()) print("RMSE:", (np.array(errors) ** 2).mean() ** 0.5) - print("Return error:", final_return_error) + print("Return route length:", return_route_length) + print("Full route length:", full_route_length) + print("Final-to-start error:", final_return_error) print("Average FPS:", 1 / proc_time.mean()) test_run_dir = make_test_run_dir(name) vis_manager.save_plots(test_run_dir) + metrics = { + 'position_errors': errors, + 'mse': float((np.array(errors) ** 2).mean()) if errors else None, + 'rmse': float((np.array(errors) ** 2).mean() ** 0.5) if errors else None, + 'planned_route_length': planned_route_length, + 'build_flight_length': build_flight_length, + 'return_route_length': return_route_length, + 'full_route_length': full_route_length, + 'final_to_start_error': final_return_error, + 'start_point': start_point, + 'turn_point': np.array([turn_position.x, turn_position.y]), + 'final_point': final_point, + } with (test_run_dir / 'metrics.pkl').open('wb') as file: - pickle.dump({ - 'position_errors': errors, - 'mse': float((np.array(errors) ** 2).mean()) if errors else None, - 'rmse': float((np.array(errors) ** 2).mean() ** 0.5) if errors else None, - 'return_error': final_return_error, - 'start_point': start_point, - 'turn_point': np.array([turn_position.x, turn_position.y]), - 'final_point': final_point, - }, file) + pickle.dump(metrics, file) + with (test_run_dir / 'metrics.txt').open('w', encoding='utf-8') as file: + for key, value in metrics.items(): + file.write(f"{key}: {value}\n") vis_manager.show_final() def parse_args(): diff --git a/map.jpg b/map.jpg index d06195f..56ac1b5 100644 Binary files a/map.jpg and b/map.jpg differ diff --git a/simulator.py b/simulator.py index 91393b2..acc9fbc 100644 --- a/simulator.py +++ b/simulator.py @@ -66,7 +66,7 @@ class Simulator: # Обновляем yaw в объекте Position self.pos.yaw += dangle - velocity = max(velocity, 10) + velocity = max(velocity, 0) # Вычисляем смещение на основе текущего yaw dx = int(math.cos(math.pi / 2 + self.pos.yaw) * velocity) diff --git a/visualization.py b/visualization.py index a974577..9768d2f 100644 --- a/visualization.py +++ b/visualization.py @@ -225,8 +225,12 @@ class VisualizationManager: if all_points: points = np.vstack(all_points) margin = 50 - self.ax_route_map.set_xlim(points[:, 0].min() - margin, points[:, 0].max() + margin) - self.ax_route_map.set_ylim(points[:, 1].min() - margin, points[:, 1].max() + margin) + x_center = (points[:, 0].min() + points[:, 0].max()) / 2 + y_center = (points[:, 1].min() + points[:, 1].max()) / 2 + half_range = max(points[:, 0].max() - points[:, 0].min(), points[:, 1].max() - points[:, 1].min()) / 2 + margin + self.ax_route_map.set_xlim(x_center - half_range, x_center + half_range) + self.ax_route_map.set_ylim(y_center - half_range, y_center + half_range) + self.ax_route_map.set_aspect('equal', adjustable='box') self.ax_route_map.legend(loc='best') @@ -682,9 +686,10 @@ class VisualizationManager: def show_final(self): """Показывает финальное состояние окна""" - plt.ioff() print("Симуляция завершена. Окно визуализации остается открытым для анализа.") - plt.pause(100000) + self.fig.canvas.draw_idle() + self.fig.canvas.flush_events() + plt.pause(0.5) def pause(self, duration: float): plt.pause(duration) @@ -708,5 +713,8 @@ class VisualizationManager: for filename, axes_item in axes.items(): if axes_item is None: continue - bbox = axes_item.get_tightbbox(renderer).expanded(1.08, 1.15) - self.fig.savefig(output_dir / filename, dpi=150, bbox_inches=bbox) + try: + bbox = axes_item.get_tightbbox(renderer).transformed(self.fig.dpi_scale_trans.inverted()).expanded(1.08, 1.15) + self.fig.savefig(output_dir / filename, dpi=150, bbox_inches=bbox) + except Exception as exc: + print(f"Не удалось сохранить {filename}: {exc}")