diff --git a/system/manager/manager.py b/system/manager/manager.py index 2d80c78ff50072..f7e8bcdcd769b7 100755 --- a/system/manager/manager.py +++ b/system/manager/manager.py @@ -1,6 +1,7 @@ #!/usr/bin/env python3 import datetime import os +import platform import signal import sys import time @@ -209,7 +210,10 @@ def main() -> None: if __name__ == "__main__": - unblock_stdout() + # Avoid forking before process startup in macOS simulation runs. + should_unblock_stdout = not (platform.system() == "Darwin" and os.getenv("SIMULATION") == "1") + if should_unblock_stdout: + unblock_stdout() try: main() diff --git a/tools/sim/bridge/metadrive/metadrive_process.py b/tools/sim/bridge/metadrive/metadrive_process.py index 2486d87ff997cb..eb2895150d415a 100644 --- a/tools/sim/bridge/metadrive/metadrive_process.py +++ b/tools/sim/bridge/metadrive/metadrive_process.py @@ -3,27 +3,23 @@ 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): + # Import metadrive and panda3d lazily to avoid initializing Cocoa paths in parent processes. + 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,8 +47,13 @@ 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: @@ -86,8 +87,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..28abf3cc94b52b 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,23 @@ class MetaDriveWorld(World): def __init__(self, status_q, config, test_duration, test_run, dual_camera=False): super().__init__(dual_camera) + 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 +39,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 +103,7 @@ 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 + 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..5e4a61d0624df3 100644 --- a/tools/sim/lib/camerad.py +++ b/tools/sim/lib/camerad.py @@ -1,4 +1,5 @@ import numpy as np +import time from msgq.visionipc import VisionIpcServer, VisionStreamType from cereal import messaging @@ -64,7 +65,7 @@ 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) + eof = int(time.monotonic() * 1e9) self.vipc_server.send(yuv_type, yuv, frame_id, eof, eof) dat = messaging.new_message(pub_type, valid=True)