diff --git a/tools/sim/bridge/common.py b/tools/sim/bridge/common.py index 464ea08823..360fdc0e68 100644 --- a/tools/sim/bridge/common.py +++ b/tools/sim/bridge/common.py @@ -19,7 +19,7 @@ from openpilot.tools.sim.lib.simulated_sensors import SimulatedSensors def rk_loop(function, hz, exit_event: threading.Event): - rk = Ratekeeper(hz) + rk = Ratekeeper(hz, None) while not exit_event.is_set(): function() rk.keep_time() diff --git a/tools/sim/bridge/metadrive.py b/tools/sim/bridge/metadrive.py new file mode 100644 index 0000000000..f7b937359e --- /dev/null +++ b/tools/sim/bridge/metadrive.py @@ -0,0 +1,156 @@ +import math +import numpy as np +import time + +from openpilot.tools.sim.bridge.common import World, SimulatorBridge +from openpilot.tools.sim.lib.common import vec3, SimulatorState +from openpilot.tools.sim.lib.camerad import W, H + + +def apply_metadrive_patches(): + from metadrive.engine.core.engine_core import EngineCore + from metadrive.engine.core.image_buffer import ImageBuffer + 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"]: + sensor = cls(*args, self, cuda=True) + else: + sensor = cls(*args, self, cuda=False) + assert isinstance(sensor, ImageBuffer), "This API is for adding image sensor" + self.sensors[name] = sensor + + EngineCore.add_image_sensor = add_image_sensor_patched + + # we aren't going to use the built-in observation stack, so disable it to save time + def observe_patched(self, vehicle): + return self.state + + ImageObservation.observe = observe_patched + + +class MetaDriveWorld(World): + def __init__(self, env, ticks_per_frame: float, dual_camera = False): + super().__init__(dual_camera) + self.env = env + self.ticks_per_frame = ticks_per_frame + self.dual_camera = dual_camera + + self.steer_ratio = 15 + + self.vc = [0.0,0.0] + + self.reset_time = 0 + + def get_cam_as_rgb(self, cam): + cam = self.env.engine.sensors[cam] + img = cam.perceive(self.env.vehicle, clip=False) + if type(img) != np.ndarray: + img = img.get() # convert cupy array to numpy + return img + + def apply_controls(self, steer_angle, throttle_out, brake_out): + steer_metadrive = steer_angle * 1 / (self.env.vehicle.MAX_STEERING * self.steer_ratio) + steer_metadrive = np.clip(steer_metadrive, -1, 1) + + if (time.monotonic() - self.reset_time) > 5: + self.vc[0] = steer_metadrive + + if throttle_out: + self.vc[1] = throttle_out/10 + else: + self.vc[1] = -brake_out + else: + self.vc[0] = 0 + self.vc[1] = 0 + + def read_sensors(self, state: SimulatorState): + state.velocity = vec3(x=float(self.env.vehicle.velocity[0]), y=float(self.env.vehicle.velocity[1]), z=0) + state.gps.from_xy(self.env.vehicle.position) + state.bearing = float(math.degrees(self.env.vehicle.heading_theta)) + state.steering_angle = self.env.vehicle.steering * self.env.vehicle.MAX_STEERING + state.valid = True + + def read_cameras(self): + if self.dual_camera: + self.wide_road_image = self.get_cam_as_rgb("rgb_wide") + self.road_image = self.get_cam_as_rgb("rgb_road") + + def tick(self): + obs, _, terminated, _, info = self.env.step(self.vc) + + if terminated: + self.env.reset() + self.reset_time = time.monotonic() + + def close(self): + pass + + +class MetaDriveBridge(SimulatorBridge): + TICKS_PER_FRAME = 2 + + def __init__(self, args): + self.should_render = False + + super(MetaDriveBridge, self).__init__(args) + + def spawn_world(self): + print("----------------------------------------------------------") + print("---- Spawning Metadrive world, this might take awhile ----") + print("----------------------------------------------------------") + from metadrive.component.sensors.rgb_camera import RGBCamera + from metadrive.component.sensors.base_camera import _cuda_enable + from metadrive.envs.metadrive_env import MetaDriveEnv + from panda3d.core import Vec3 + + apply_metadrive_patches() + + C3_POSITION = Vec3(0, 0, 1) + + class RGBCameraWide(RGBCamera): + def __init__(self, *args, **kwargs): + super(RGBCameraWide, self).__init__(*args, **kwargs) + cam = self.get_cam() + cam.setPos(C3_POSITION) + lens = self.get_lens() + lens.setFov(160) + + class RGBCameraRoad(RGBCamera): + def __init__(self, *args, **kwargs): + super(RGBCameraRoad, self).__init__(*args, **kwargs) + cam = self.get_cam() + cam.setPos(C3_POSITION) + lens = self.get_lens() + lens.setFov(40) + + sensors = { + "rgb_road": (RGBCameraRoad, W, H, ) + } + + if self.dual_camera: + sensors["rgb_wide"] = (RGBCameraWide, W, H) + + env = MetaDriveEnv( + dict( + use_render=self.should_render, + vehicle_config=dict( + enable_reverse=False, + image_source="rgb_road", + spawn_longitude=15 + ), + sensors=sensors, + image_on_cuda=_cuda_enable, + image_observation=True, + interface_panel=[], + out_of_route_done=False, + on_continuous_line_done=False, + crash_vehicle_done=False, + crash_object_done=False, + ) + ) + + env.reset() + + return MetaDriveWorld(env, self.TICKS_PER_FRAME) \ No newline at end of file diff --git a/tools/sim/lib/simulated_car.py b/tools/sim/lib/simulated_car.py index 7cd01e1610..41b58063b9 100644 --- a/tools/sim/lib/simulated_car.py +++ b/tools/sim/lib/simulated_car.py @@ -107,6 +107,8 @@ class SimulatedCar: def update(self, simulator_state: SimulatorState): self.send_can_messages(simulator_state) - self.send_panda_state(simulator_state) + + if self.idx % 50 == 0: # only send panda states at 2hz + self.send_panda_state(simulator_state) self.idx += 1 \ No newline at end of file diff --git a/tools/sim/run_bridge.py b/tools/sim/run_bridge.py index 8e6dc8f2ef..b49c8be2aa 100755 --- a/tools/sim/run_bridge.py +++ b/tools/sim/run_bridge.py @@ -6,6 +6,7 @@ from multiprocessing import Queue from openpilot.tools.sim.bridge.common import SimulatorBridge from openpilot.tools.sim.bridge.carla import CarlaBridge +from openpilot.tools.sim.bridge.metadrive import MetaDriveBridge def parse_args(add_args=None): @@ -30,6 +31,8 @@ if __name__ == "__main__": simulator_bridge: SimulatorBridge if args.simulator == "carla": simulator_bridge = CarlaBridge(args) + elif args.simulator == "metadrive": + simulator_bridge = MetaDriveBridge(args) else: raise AssertionError("simulator type not supported") p = simulator_bridge.run(q) diff --git a/tools/sim/tests/test_carla_bridge.py b/tools/sim/tests/test_carla_bridge.py new file mode 100755 index 0000000000..a2b38d11fe --- /dev/null +++ b/tools/sim/tests/test_carla_bridge.py @@ -0,0 +1,43 @@ +#!/usr/bin/env python3 +import subprocess +import time +import unittest + +from openpilot.selfdrive.manager.helpers import unblock_stdout +from openpilot.tools.sim.run_bridge import parse_args +from openpilot.tools.sim.bridge.carla import CarlaBridge +from openpilot.tools.sim.tests.test_sim_bridge import SIM_DIR, TestSimBridgeBase + + +class TestCarlaBridge(TestSimBridgeBase): + """ + Tests need Carla simulator to run + """ + carla_process = None + + def setUp(self): + super().setUp() + + # We want to make sure that carla_sim docker isn't still running. + subprocess.run("docker rm -f carla_sim", shell=True, stderr=subprocess.PIPE, check=False) + self.carla_process = subprocess.Popen("./start_carla.sh", cwd=SIM_DIR) + + # Too many lagging messages in bridge.py can cause a crash. This prevents it. + unblock_stdout() + # Wait 10 seconds to startup carla + time.sleep(10) + + def create_bridge(self): + return CarlaBridge(parse_args([])) + + def tearDown(self): + super().tearDown() + + # Stop carla simulator by removing docker container + subprocess.run("docker rm -f carla_sim", shell=True, stderr=subprocess.PIPE, check=False) + if self.carla_process is not None: + self.carla_process.wait() + + +if __name__ == "__main__": + unittest.main() diff --git a/tools/sim/tests/test_metadrive_bridge.py b/tools/sim/tests/test_metadrive_bridge.py new file mode 100755 index 0000000000..175d3758ff --- /dev/null +++ b/tools/sim/tests/test_metadrive_bridge.py @@ -0,0 +1,15 @@ +#!/usr/bin/env python3 +import unittest + +from openpilot.tools.sim.run_bridge import parse_args +from openpilot.tools.sim.bridge.metadrive import MetaDriveBridge +from openpilot.tools.sim.tests.test_sim_bridge import TestSimBridgeBase + + +class TestMetaDriveBridge(TestSimBridgeBase): + def create_bridge(self): + return MetaDriveBridge(parse_args([])) + + +if __name__ == "__main__": + unittest.main() diff --git a/tools/sim/tests/test_carla_integration.py b/tools/sim/tests/test_sim_bridge.py old mode 100755 new mode 100644 similarity index 71% rename from tools/sim/tests/test_carla_integration.py rename to tools/sim/tests/test_sim_bridge.py index b5a60ee0e3..b00527b322 --- a/tools/sim/tests/test_carla_integration.py +++ b/tools/sim/tests/test_sim_bridge.py @@ -1,40 +1,24 @@ -#!/usr/bin/env python3 +import os import subprocess import time import unittest -import os + from multiprocessing import Queue from cereal import messaging from openpilot.common.basedir import BASEDIR -from openpilot.selfdrive.manager.helpers import unblock_stdout -from openpilot.tools.sim.run_bridge import parse_args -from openpilot.tools.sim.bridge.carla import CarlaBridge - -CI = "CI" in os.environ SIM_DIR = os.path.join(BASEDIR, "tools/sim") -class TestCarlaIntegration(unittest.TestCase): - """ - Tests need Carla simulator to run - """ - processes = None - carla_process = None +class TestSimBridgeBase(unittest.TestCase): + @classmethod + def setUpClass(cls): + if cls is TestSimBridgeBase: + raise unittest.SkipTest("Don't run this base class, run test_carla_bridge.py instead") def setUp(self): self.processes = [] - if not CI: - # We want to make sure that carla_sim docker isn't still running. - subprocess.run("docker rm -f carla_sim", shell=True, stderr=subprocess.PIPE, check=False) - self.carla_process = subprocess.Popen("./start_carla.sh", cwd=SIM_DIR) - - # Too many lagging messages in bridge.py can cause a crash. This prevents it. - unblock_stdout() - # Wait 10 seconds to startup carla - time.sleep(10) - def test_engage(self): # Startup manager and bridge.py. Check processes are running, then engage and verify. p_manager = subprocess.Popen("./launch_openpilot.sh", cwd=SIM_DIR) @@ -42,7 +26,7 @@ class TestCarlaIntegration(unittest.TestCase): sm = messaging.SubMaster(['controlsState', 'carEvents', 'managerState']) q = Queue() - carla_bridge = CarlaBridge(parse_args([])) + carla_bridge = self.create_bridge() p_bridge = carla_bridge.run(q, retries=10) self.processes.append(p_bridge) @@ -99,11 +83,6 @@ class TestCarlaIntegration(unittest.TestCase): else: p.join(15) - # Stop carla simulator by removing docker container - subprocess.run("docker rm -f carla_sim", shell=True, stderr=subprocess.PIPE, check=False) - if self.carla_process is not None: - self.carla_process.wait() - if __name__ == "__main__": unittest.main()