diff --git a/main.py b/main.py index 74beab0..7e3d289 100644 --- a/main.py +++ b/main.py @@ -181,7 +181,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]) + final_pos = simulator.pos.copy() * online_map.pixel_ratio + final_pos.x = points_coords[-1][0] + final_pos.y = 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 @@ -192,7 +194,9 @@ def build(name: str, map_name: str, lat: float, lon: float): command.velocity /= online_map.pixel_ratio if command.stop: - final_pos = Position(points_coords[-1][0], points_coords[-1][1]) + final_pos = simulator.pos.copy() * online_map.pixel_ratio + final_pos.x = points_coords[-1][0] + final_pos.y = 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 @@ -213,6 +217,7 @@ def build(name: str, map_name: str, lat: float, lon: float): data.update({ 'points': points_coords, 'chunk_positions': positions, + 'final_build_position': positions[-1].copy() if positions else None, 'initial_geolocation': geo, 'map_provider': map_name, 'map_zoom': map_zoom, @@ -335,7 +340,11 @@ def run( simulator = Simulator(online_map) pilot.target_idx = 0 - turn_position = data['chunk_positions'][-1].copy() if data.get('chunk_positions') else Position(forward_points[-1][0], forward_points[-1][1]) + turn_position = data.get('final_build_position') + if turn_position is None: + turn_position = data['chunk_positions'][-1].copy() if data.get('chunk_positions') else Position(forward_points[-1][0], forward_points[-1][1]) + else: + turn_position = turn_position.copy() start_position = turn_position / online_map.pixel_ratio start_position.yaw += np.pi simulator.pos = start_position.copy()