Skip to content
Closed
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
4 changes: 3 additions & 1 deletion system/manager/manager.py
Original file line number Diff line number Diff line change
Expand Up @@ -209,7 +209,9 @@ def main() -> None:


if __name__ == "__main__":
unblock_stdout()
import sys
if not (sys.platform == "darwin" and os.environ.get("SIMULATION") == "1"):
unblock_stdout()

try:
main()
Expand Down
24 changes: 16 additions & 8 deletions tools/sim/bridge/metadrive/metadrive_process.py
Original file line number Diff line number Diff line change
Expand Up @@ -3,27 +3,31 @@
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)
# C3_POSITION and C3_HPR defined inside metadrive_process (lazy panda3d import)


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/metadrive initialize Cocoa, unsafe before fork on macOS
global C3_POSITION, C3_HPR
from panda3d.core import Vec3
C3_POSITION = Vec3(0.0, 0, 1.22)
C3_HPR = Vec3(0, 0, 0)

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"]:
Expand All @@ -48,9 +52,13 @@ def arrive_destination_patch(self, *args, **kwargs):
if not arrive_dest_done:
MetaDriveEnv._is_arrive_destination = arrive_destination_patch

return {'EngineCore': EngineCore, 'ImageBuffer': ImageBuffer, 'MetaDriveEnv': MetaDriveEnv, 'ImageObservation': ImageObservation}

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):
# Lazy imports inside subprocess only — safe on macOS
from metadrive.envs.metadrive_env import MetaDriveEnv
arrive_dest_done = config.pop("arrive_dest_done", True)
apply_metadrive_patches(arrive_dest_done)

Expand Down
25 changes: 15 additions & 10 deletions tools/sim/bridge/metadrive/metadrive_world.py
Original file line number Diff line number Diff line change
Expand Up @@ -2,9 +2,14 @@
import functools
import multiprocessing
import numpy as np
import sys
import time

from multiprocessing import Pipe, Array
if sys.platform == "darwin":
mp_ctx = multiprocessing.get_context("spawn")
else:
mp_ctx = multiprocessing


from openpilot.tools.sim.bridge.common import QueueMessage, QueueMessageType
from openpilot.tools.sim.bridge.metadrive.metadrive_process import (metadrive_process, metadrive_simulation_state,
Expand All @@ -17,27 +22,27 @@ class MetaDriveWorld(World):
def __init__(self, status_q, config, test_duration, test_run, dual_camera=False):
super().__init__(dual_camera)
self.status_q = status_q
self.camera_array = Array(ctypes.c_uint8, W*H*3)
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

self.first_engage = None
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,
Expand Down Expand Up @@ -101,7 +106,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" 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

Expand Down
3 changes: 2 additions & 1 deletion tools/sim/lib/camerad.py
Original file line number Diff line number Diff line change
@@ -1,4 +1,5 @@
import numpy as np
import time

from msgq.visionipc import VisionIpcServer, VisionStreamType
from cereal import messaging
Expand Down Expand Up @@ -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)
Expand Down
Loading