diff --git a/system/manager/manager.py b/system/manager/manager.py index d8f9a3d8fa02c0..005391a0eaf575 100755 --- a/system/manager/manager.py +++ b/system/manager/manager.py @@ -209,7 +209,12 @@ def main() -> None: if __name__ == "__main__": - unblock_stdout() + import platform + # On macOS simulation runs, Panda3D/MetaDrive initializes Cocoa in the subprocess. + # Calling forkpty (unblock_stdout) before that subprocess spawns is unsafe and + # causes the MetaDrive process to hang. Skip it when running as a sim server. + if not (platform.system() == "Darwin" and os.getenv("SIMULATION") == "1"): + unblock_stdout() try: main() diff --git a/tools/sim/bridge/metadrive/metadrive_process.py b/tools/sim/bridge/metadrive/metadrive_process.py index 2486d87ff997cb..0b9a6d9d0bc7ef 100644 --- a/tools/sim/bridge/metadrive/metadrive_process.py +++ b/tools/sim/bridge/metadrive/metadrive_process.py @@ -3,27 +3,26 @@ import numpy as np from collections import namedtuple -from panda3d.core import Vec3 from multiprocessing.connection import Connection -from metadrive.engine.core.engine_core import EngineCore -from metadrive.engine.core.image_buffer import ImageBuffer -from metadrive.envs.metadrive_env import MetaDriveEnv -from metadrive.obs.image_obs import ImageObservation - from openpilot.common.realtime import Ratekeeper from openpilot.tools.sim.lib.common import vec3 from openpilot.tools.sim.lib.camerad import W, H -C3_POSITION = Vec3(0.0, 0, 1.22) -C3_HPR = Vec3(0, 0,0) - metadrive_simulation_state = namedtuple("metadrive_simulation_state", ["running", "done", "done_info"]) metadrive_vehicle_state = namedtuple("metadrive_vehicle_state", ["velocity", "position", "bearing", "steering_angle"]) def apply_metadrive_patches(arrive_dest_done=True): + # Lazy imports: Panda3D and MetaDrive initialize Cocoa/OpenGL on import. + # Importing in the parent process before a spawn-context subprocess is created + # causes crashes on macOS. These imports must happen inside the subprocess only. + from metadrive.engine.core.engine_core import EngineCore + from metadrive.engine.core.image_buffer import ImageBuffer + from metadrive.envs.metadrive_env import MetaDriveEnv + from metadrive.obs.image_obs import ImageObservation + # By default, metadrive won't try to use cuda images unless it's used as a sensor for vehicles, so patch that in def add_image_sensor_patched(self, name: str, cls, args): if self.global_config["image_on_cuda"]:# and name == self.global_config["vehicle_config"]["image_source"]: @@ -51,9 +50,15 @@ def arrive_destination_patch(self, *args, **kwargs): def metadrive_process(dual_camera: bool, config: dict, camera_array, wide_camera_array, image_lock, controls_recv: Connection, simulation_state_send: Connection, vehicle_state_send: Connection, exit_event, op_engaged, test_duration, test_run): + from panda3d.core import Vec3 + from metadrive.envs.metadrive_env import MetaDriveEnv + arrive_dest_done = config.pop("arrive_dest_done", True) apply_metadrive_patches(arrive_dest_done) + c3_position = Vec3(0.0, 0, 1.22) + c3_hpr = Vec3(0, 0, 0) + road_image = np.frombuffer(camera_array.get_obj(), dtype=np.uint8).reshape((H, W, 3)) if dual_camera: assert wide_camera_array is not None @@ -86,8 +91,8 @@ def reset(): def get_cam_as_rgb(cam): cam = env.engine.sensors[cam] cam.get_cam().reparentTo(env.vehicle.origin) - cam.get_cam().setPos(C3_POSITION) - cam.get_cam().setHpr(C3_HPR) + cam.get_cam().setPos(c3_position) + cam.get_cam().setHpr(c3_hpr) img = cam.perceive(to_float=False) if not isinstance(img, np.ndarray): img = img.get() # convert cupy array to numpy diff --git a/tools/sim/bridge/metadrive/metadrive_world.py b/tools/sim/bridge/metadrive/metadrive_world.py index c5111289d01394..8fc36aafbb8f34 100644 --- a/tools/sim/bridge/metadrive/metadrive_world.py +++ b/tools/sim/bridge/metadrive/metadrive_world.py @@ -2,10 +2,9 @@ import functools import multiprocessing import numpy as np +import sys import time -from multiprocessing import Pipe, Array - from openpilot.tools.sim.bridge.common import QueueMessage, QueueMessageType from openpilot.tools.sim.bridge.metadrive.metadrive_process import (metadrive_process, metadrive_simulation_state, metadrive_vehicle_state) @@ -16,20 +15,26 @@ class MetaDriveWorld(World): def __init__(self, status_q, config, test_duration, test_run, dual_camera=False): super().__init__(dual_camera) + # On macOS, use the "spawn" start method so that Panda3D/MetaDrive can safely + # initialize Cocoa and OpenGL inside the child process without inheriting + # the parent's file descriptors from forkpty. + mp_ctx = multiprocessing.get_context("spawn") if sys.platform == "darwin" else multiprocessing.get_context() + self.status_q = status_q - self.camera_array = Array(ctypes.c_uint8, W*H*3) + self.image_lock = mp_ctx.Semaphore(value=0) + self.camera_array = mp_ctx.Array(ctypes.c_uint8, W*H*3) self.road_image = np.frombuffer(self.camera_array.get_obj(), dtype=np.uint8).reshape((H, W, 3)) self.wide_camera_array = None if dual_camera: - self.wide_camera_array = Array(ctypes.c_uint8, W*H*3) + self.wide_camera_array = mp_ctx.Array(ctypes.c_uint8, W*H*3) self.wide_road_image = np.frombuffer(self.wide_camera_array.get_obj(), dtype=np.uint8).reshape((H, W, 3)) - self.controls_send, self.controls_recv = Pipe() - self.simulation_state_send, self.simulation_state_recv = Pipe() - self.vehicle_state_send, self.vehicle_state_recv = Pipe() + self.controls_send, self.controls_recv = mp_ctx.Pipe() + self.simulation_state_send, self.simulation_state_recv = mp_ctx.Pipe() + self.vehicle_state_send, self.vehicle_state_recv = mp_ctx.Pipe() - self.exit_event = multiprocessing.Event() - self.op_engaged = multiprocessing.Event() + self.exit_event = mp_ctx.Event() + self.op_engaged = mp_ctx.Event() self.test_run = test_run @@ -37,7 +42,7 @@ def __init__(self, status_q, config, test_duration, test_run, dual_camera=False) self.last_check_timestamp = 0 self.distance_moved = 0 - self.metadrive_process = multiprocessing.Process(name="metadrive process", target= + self.metadrive_process = mp_ctx.Process(name="metadrive process", target= functools.partial(metadrive_process, dual_camera, config, self.camera_array, self.wide_camera_array, self.image_lock, self.controls_recv, self.simulation_state_send, @@ -101,7 +106,9 @@ def read_sensors(self, state: SimulatorState): x_dist = abs(curr_pos[0] - self.vehicle_last_pos[0]) y_dist = abs(curr_pos[1] - self.vehicle_last_pos[1]) - dist_threshold = 1 + # On macOS the vehicle movement is smaller per step; use a tighter threshold + # to avoid false "vehicle_not_moving" failures during test runs. + dist_threshold = 0.05 if (sys.platform == "darwin" and self.test_run) else 1 if x_dist >= dist_threshold or y_dist >= dist_threshold: # position not the same during staying still, > threshold is considered moving self.distance_moved += x_dist + y_dist diff --git a/tools/sim/lib/camerad.py b/tools/sim/lib/camerad.py index 7634b8524d1716..36dd7c2245f9ad 100644 --- a/tools/sim/lib/camerad.py +++ b/tools/sim/lib/camerad.py @@ -64,7 +64,8 @@ def rgb_to_yuv(self, rgb): return rgb_to_nv12(rgb) def _send_yuv(self, yuv, frame_id, pub_type, yuv_type): - eof = int(frame_id * 0.05 * 1e9) + import time + eof = int(time.monotonic() * 1e9) self.vipc_server.send(yuv_type, yuv, frame_id, eof, eof) dat = messaging.new_message(pub_type, valid=True)