From fae33d57814ddc568557a9f4265a60acb561070a Mon Sep 17 00:00:00 2001 From: jon-chuang Date: Fri, 17 Feb 2023 19:08:43 +0800 Subject: [PATCH 01/28] finish --- poetry.lock | 2 +- pyproject.toml | 3 +- tools/sim/bridge.py | 450 +++++++++++++++++++++++++++++++------------- 3 files changed, 321 insertions(+), 134 deletions(-) diff --git a/poetry.lock b/poetry.lock index 91e1b5c826506c..be8c4dd310a912 100644 --- a/poetry.lock +++ b/poetry.lock @@ -1604,7 +1604,7 @@ optional = false python-versions = ">=3.7" [package.dependencies] -MarkupSafe = ">=2.0" +MarkupSafe = "==2.0.1" [package.extras] i18n = ["Babel (>=2.7)"] diff --git a/pyproject.toml b/pyproject.toml index 6b4b0bb5fadc46..bc0c13b5108647 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -173,7 +173,8 @@ Werkzeug = "^2.1.2" zerorpc = { git = "https://github.com/commaai/zerorpc-python.git", branch = "master" } omegaconf = "^2.3.0" osmnx = "==1.2.2" - +markupsafe= "==2.0.1" +metadrive-simulator = "0.2.6" [build-system] requires = ["poetry-core"] diff --git a/tools/sim/bridge.py b/tools/sim/bridge.py index f0694078e2bde3..1d746562d77997 100755 --- a/tools/sim/bridge.py +++ b/tools/sim/bridge.py @@ -7,8 +7,9 @@ import time from multiprocessing import Process, Queue from typing import Any +from abc import ABC, abstractmethod +import warnings -import carla # pylint: disable=import-error import numpy as np import pyopencl as cl import pyopencl.array as cl_array @@ -24,9 +25,10 @@ from selfdrive.test.helpers import set_params_enabled from tools.sim.lib.can import can_function -W, H = 1928, 1208 +SCALE = 1 +W, H = 1928 // SCALE, 1208 // SCALE REPEAT_COUNTER = 5 -PRINT_DECIMATION = 100 +PRINT_DECIMATION = 5 STEER_RATIO = 15. pm = messaging.PubMaster(['roadCameraState', 'wideRoadCameraState', 'accelerometer', 'gyroscope', 'can', "gpsLocationExternal"]) @@ -41,6 +43,8 @@ def parse_args(add_args=None): parser.add_argument('--spawn_point', dest='num_selected_spawn_point', type=int, default=16) parser.add_argument('--host', dest='host', type=str, default='127.0.0.1') parser.add_argument('--port', dest='port', type=int, default=2000) + parser.add_argument('--simulator', dest='simulator', type=str, default='metadrive') + parser.add_argument('--frames_per_tick', dest='frames_per_tick', type=int, default=None) return parser.parse_args(add_args) @@ -50,7 +54,7 @@ def __init__(self): self.speed = 0.0 self.angle = 0.0 self.bearing_deg = 0.0 - self.vel = carla.Vector3D() + self.vel = None self.cruise_button = 0 self.is_engaged = False self.ignition = True @@ -67,6 +71,7 @@ def steer_rate_limit(old, new): return new + class Camerad: def __init__(self): self.frame_road_id = 0 @@ -90,27 +95,32 @@ def __init__(self): self.Hdiv4 = H // 4 if (H % 4 == 0) else (H + (4 - H % 4)) // 4 def cam_callback_road(self, image): - self._cam_callback(image, self.frame_road_id, 'roadCameraState', VisionStreamType.VISION_STREAM_ROAD) + yuv = self.img_to_yuv(image) + self._send_yuv(yuv, self.frame_road_id, 'roadCameraState', VisionStreamType.VISION_STREAM_ROAD) self.frame_road_id += 1 def cam_callback_wide_road(self, image): - self._cam_callback(image, self.frame_wide_id, 'wideRoadCameraState', VisionStreamType.VISION_STREAM_WIDE_ROAD) + yuv = self.img_to_yuv(image) + self._send_yuv(yuv, self.frame_wide_id, 'wideRoadCameraState', VisionStreamType.VISION_STREAM_WIDE_ROAD) self.frame_wide_id += 1 - def _cam_callback(self, image, frame_id, pub_type, yuv_type): - img = np.frombuffer(image.raw_data, dtype=np.dtype("uint8")) - img = np.reshape(img, (H, W, 4)) - img = img[:, :, [0, 1, 2]].copy() + def cam_send_yuv_wide_road(self, yuv): + self._send_yuv(yuv, self.frame_wide_id, 'wideRoadCameraState', VisionStreamType.VISION_STREAM_WIDE_ROAD) + self.frame_wide_id += 1 - # convert RGB frame to YUV + # Returns: yuv bytes + def img_to_yuv(self, img): + assert img.shape == (H, W, 3), f"{img.shape}" rgb = np.reshape(img, (H, W * 3)) rgb_cl = cl_array.to_device(self.queue, rgb) yuv_cl = cl_array.empty_like(rgb_cl) self.krnl(self.queue, (np.int32(self.Wdiv4), np.int32(self.Hdiv4)), None, rgb_cl.data, yuv_cl.data).wait() yuv = np.resize(yuv_cl.get(), rgb.size // 2) + return yuv.data.tobytes() + + def _send_yuv(self, yuv, frame_id, pub_type, yuv_type): eof = int(frame_id * 0.05 * 1e9) - - self.vipc_server.send(yuv_type, yuv.data.tobytes(), frame_id, eof, eof) + self.vipc_server.send(yuv_type, yuv, frame_id, eof, eof) dat = messaging.new_message(pub_type) msg = { @@ -154,7 +164,7 @@ def panda_state_function(vs: VehicleState, exit_event: threading.Event): 'ignitionLine': vs.ignition, 'pandaType': "blackPanda", 'controlsAllowed': True, - 'safetyModel': 'hondaNidec' + 'safetyModel': 'hondaNidec', } pm.send('pandaStates', dat) time.sleep(0.5) @@ -200,7 +210,7 @@ def gps_callback(gps, vehicle_state): "longitude": gps.longitude, "altitude": gps.altitude, "speed": vehicle_state.speed, - "source": log.GpsLocationData.SensorSource.ublox, + "source": log.GpsLocationData.SensorSource.wublox, } pm.send('gpsLocationExternal', dat) @@ -211,6 +221,7 @@ def fake_driver_monitoring(exit_event: threading.Event): while not exit_event.is_set(): # dmonitoringmodeld output dat = messaging.new_message('driverStateV2') + dat.driverStateV2.leftDriverData.faceOrientation = [0.0, 1.0, 0.0] dat.driverStateV2.leftDriverData.faceProb = 1.0 pm.send('driverStateV2', dat) @@ -233,36 +244,119 @@ def can_function_runner(vs: VehicleState, exit_event: threading.Event): time.sleep(0.01) i += 1 +class World(ABC): + @abstractmethod + def apply_controls(self, steer_sim, throttle_out, brake_out, frame: int): + pass + @abstractmethod + def get_velocity(self): + pass + @abstractmethod + def get_speed(self) -> float: + pass + @abstractmethod + def get_steer_correction(self) -> float: + pass + @abstractmethod + def tick(self): + pass + +class CarlaWorld(World): + def __init__(self, world, vc, vehicle, camerad): + self.world = world + self.vc = vc + self.vehicle = vehicle + self.max_steer_angle: float = vehicle.get_physics_control().wheels[0].max_steer_angle + self.camerad = camerad + + def apply_controls(self, steer_sim, throttle_out, brake_out, _frame): + self.vc.throttle = throttle_out / 0.6 + self.vc.steer = steer_sim + self.vc.brake = brake_out + self.vehicle.apply_control(self.vc) + + def get_velocity(self): + self.vel = self.vehicle.get_velocity() + return self.vel + + def get_speed(self) -> float: + return math.sqrt(self.vel.x ** 2 + self.vel.y ** 2 + self.vel.z ** 2) # in m/s + + def get_steer_correction(self) -> float: + return self.max_steer_angle * STEER_RATIO * -1 + + def tick(self): + self.world.tick() + +class MetaDriveWorld(World): + def __init__(self, env, frames_per_tick: float): + self.env = env + self.speed = 0.0 + self.yuv = None + self.camerad = Camerad() + self.frames_per_tick = frames_per_tick + + def apply_controls(self, steer_sim, throttle_out, brake_out, frame: int): + vc = [0.0, 0.0] + vc[0] = steer_sim * -1 + if throttle_out: + vc[1] = throttle_out * 10 + else: + vc[1] = -brake_out + + if frame % self.frames_per_tick == 0 or self.yuv is None: + if frame % (self.frames_per_tick * 2) == 0: + o, _, _, _ = self.env.step(vc) + self.speed = o["state"][3] * self.frames_per_tick * 2 # empirically derived + img = self.env.vehicle.image_sensors["rgb_wide"].get_pixels_array(self.env.vehicle, False) + self.yuv = self.camerad.img_to_yuv(img) + + if frame % (self.frames_per_tick * 2) == 0: + self.camerad.cam_send_yuv_wide_road(self.yuv) + + def get_velocity(self): + return None -def connect_carla_client(host: str, port: int): - client = carla.Client(host, port) - client.set_timeout(5) - return client + def get_speed(self) -> float: + return self.speed + def get_steer_correction(self) -> float: + max_steer_angle = 75 / self.frames_per_tick + return max_steer_angle * STEER_RATIO * -1 + + def tick(self): + pass -class CarlaBridge: +class SimulatorBridge(ABC): + FRAMES_PER_TICK = 5 def __init__(self, arguments): + if arguments.frames_per_tick: + self.frames_per_tick = arguments.frames_per_tick + else: + self.frames_per_tick = self.FRAMES_PER_TICK + print(f"Running at {self.frames_per_tick} frames per tick") set_params_enabled() - self.params = Params() msg = messaging.new_message('liveCalibration') msg.liveCalibration.validBlocks = 20 msg.liveCalibration.rpyCalib = [0.0, 0.0, 0.0] self.params.put("CalibrationParams", msg.to_bytes()) + if not arguments.dual_camera: + print("Dual Camera disabled") self.params.put_bool("WideCameraOnly", not arguments.dual_camera) + self.params.put_bool("DisengageOnAccelerator", True) self._args = arguments - self._carla_objects = [] - self._camerad = None + self._simulation_objects = [] self._exit_event = threading.Event() self._threads = [] self._keep_alive = True self.started = False signal.signal(signal.SIGTERM, self._on_shutdown) self._exit = threading.Event() - + def _on_shutdown(self, signal, frame): self._keep_alive = False @@ -278,7 +372,7 @@ def bridge_keep_alive(self, q: Queue, retries: int): raise # Reset for another try - self._carla_objects = [] + self._simulation_objects = [] self._threads = [] self._exit_event = threading.Event() @@ -290,90 +384,16 @@ def bridge_keep_alive(self, q: Queue, retries: int): finally: # Clean up resources in the opposite order they were created. self.close() - + def _run(self, q: Queue): - client = connect_carla_client(self._args.host, self._args.port) - world = client.load_world(self._args.town) - - settings = world.get_settings() - settings.synchronous_mode = True # Enables synchronous mode - settings.fixed_delta_seconds = 0.05 - world.apply_settings(settings) - - world.set_weather(carla.WeatherParameters.ClearSunset) - - if not self._args.high_quality: - world.unload_map_layer(carla.MapLayer.Foliage) - world.unload_map_layer(carla.MapLayer.Buildings) - world.unload_map_layer(carla.MapLayer.ParkedVehicles) - world.unload_map_layer(carla.MapLayer.Props) - world.unload_map_layer(carla.MapLayer.StreetLights) - world.unload_map_layer(carla.MapLayer.Particles) - - blueprint_library = world.get_blueprint_library() - - world_map = world.get_map() - - vehicle_bp = blueprint_library.filter('vehicle.tesla.*')[1] - vehicle_bp.set_attribute('role_name', 'hero') - spawn_points = world_map.get_spawn_points() - assert len(spawn_points) > self._args.num_selected_spawn_point, f'''No spawn point {self._args.num_selected_spawn_point}, try a value between 0 and - {len(spawn_points)} for this town.''' - spawn_point = spawn_points[self._args.num_selected_spawn_point] - vehicle = world.spawn_actor(vehicle_bp, spawn_point) - self._carla_objects.append(vehicle) - max_steer_angle = vehicle.get_physics_control().wheels[0].max_steer_angle - - # make tires less slippery - # wheel_control = carla.WheelPhysicsControl(tire_friction=5) - physics_control = vehicle.get_physics_control() - physics_control.mass = 2326 - # physics_control.wheels = [wheel_control]*4 - physics_control.torque_curve = [[20.0, 500.0], [5000.0, 500.0]] - physics_control.gear_switch_time = 0.0 - vehicle.apply_physics_control(physics_control) - - transform = carla.Transform(carla.Location(x=0.8, z=1.13)) - - def create_camera(fov, callback): - blueprint = blueprint_library.find('sensor.camera.rgb') - blueprint.set_attribute('image_size_x', str(W)) - blueprint.set_attribute('image_size_y', str(H)) - blueprint.set_attribute('fov', str(fov)) - if not self._args.high_quality: - blueprint.set_attribute('enable_postprocess_effects', 'False') - camera = world.spawn_actor(blueprint, transform, attach_to=vehicle) - camera.listen(callback) - return camera - - self._camerad = Camerad() - - if self._args.dual_camera: - road_camera = create_camera(fov=40, callback=self._camerad.cam_callback_road) - self._carla_objects.append(road_camera) - - road_wide_camera = create_camera(fov=120, callback=self._camerad.cam_callback_wide_road) # fov bigger than 120 shows unwanted artifacts - self._carla_objects.append(road_wide_camera) - - vehicle_state = VehicleState() - - # re-enable IMU - imu_bp = blueprint_library.find('sensor.other.imu') - imu_bp.set_attribute('sensor_tick', '0.01') - imu = world.spawn_actor(imu_bp, transform, attach_to=vehicle) - imu.listen(lambda imu: imu_callback(imu, vehicle_state)) - - gps_bp = blueprint_library.find('sensor.other.gnss') - gps = world.spawn_actor(gps_bp, transform, attach_to=vehicle) - gps.listen(lambda gps: gps_callback(gps, vehicle_state)) - self.params.put_bool("UbloxAvailable", True) + self._vehicle_state = VehicleState() + world = self.spawn_objects() - self._carla_objects.extend([imu, gps]) # launch fake car threads - self._threads.append(threading.Thread(target=panda_state_function, args=(vehicle_state, self._exit_event,))) + self._threads.append(threading.Thread(target=panda_state_function, args=(self._vehicle_state, self._exit_event,))) self._threads.append(threading.Thread(target=peripheral_state_function, args=(self._exit_event,))) self._threads.append(threading.Thread(target=fake_driver_monitoring, args=(self._exit_event,))) - self._threads.append(threading.Thread(target=can_function_runner, args=(vehicle_state, self._exit_event,))) + self._threads.append(threading.Thread(target=can_function_runner, args=(self._vehicle_state, self._exit_event,))) for t in self._threads: t.start() @@ -382,8 +402,6 @@ def create_camera(fov, callback): brake_ease_out_counter = REPEAT_COUNTER steer_ease_out_counter = REPEAT_COUNTER - vc = carla.VehicleControl(throttle=0, steer=0, brake=0, reverse=False) - is_openpilot_engaged = False throttle_out = steer_out = brake_out = 0. throttle_op = steer_op = brake_op = 0. @@ -394,10 +412,6 @@ def create_camera(fov, callback): brake_manual_multiplier = 0.7 # keyboard signal is always 1 steer_manual_multiplier = 45 * STEER_RATIO # keyboard signal is always 1 - # Simulation tends to be slow in the initial steps. This prevents lagging later - for _ in range(20): - world.tick() - # loop rk = Ratekeeper(100, print_delay_threshold=0.05) @@ -438,7 +452,7 @@ def create_camera(fov, callback): cruise_button = CruiseButtons.CANCEL is_openpilot_engaged = False elif m[0] == "ignition": - vehicle_state.ignition = not vehicle_state.ignition + self._vehicle_state.ignition = not self._vehicle_state.ignition elif m[0] == "quit": break @@ -491,31 +505,27 @@ def create_camera(fov, callback): old_steer = 0 # --------------Step 2------------------------------- - steer_carla = steer_out / (max_steer_angle * STEER_RATIO * -1) + steer_correction = world.get_steer_correction() + steer_sim = steer_out / steer_correction - steer_carla = np.clip(steer_carla, -1, 1) - steer_out = steer_carla * (max_steer_angle * STEER_RATIO * -1) - old_steer = steer_carla * (max_steer_angle * STEER_RATIO * -1) + steer_sim = np.clip(steer_sim, -1, 1) + steer_out = steer_sim * steer_correction + old_steer = steer_sim * steer_correction - vc.throttle = throttle_out / 0.6 - vc.steer = steer_carla - vc.brake = brake_out - vehicle.apply_control(vc) + world.apply_controls(steer_sim, throttle_out, brake_out, rk.frame) # --------------Step 3------------------------------- - vel = vehicle.get_velocity() - speed = math.sqrt(vel.x ** 2 + vel.y ** 2 + vel.z ** 2) # in m/s - vehicle_state.speed = speed - vehicle_state.vel = vel - vehicle_state.angle = steer_out - vehicle_state.cruise_button = cruise_button - vehicle_state.is_engaged = is_openpilot_engaged + self._vehicle_state.speed = world.get_speed() + self._vehicle_state.vel = world.get_velocity() + self._vehicle_state.angle = steer_out + self._vehicle_state.cruise_button = cruise_button + self._vehicle_state.is_engaged = is_openpilot_engaged if rk.frame % PRINT_DECIMATION == 0: - print("frame: ", "engaged:", is_openpilot_engaged, "; throttle: ", round(vc.throttle, 3), "; steer(c/deg): ", - round(vc.steer, 3), round(steer_out, 3), "; brake: ", round(vc.brake, 3)) + print("frame: ", "engaged:", is_openpilot_engaged, "; throttle: ", round(throttle_out, 3), "; steer(deg): ", + round(steer_out, 3), "; brake: ", round(brake_out, 3)) - if rk.frame % 5 == 0: + if rk.frame % self.FRAMES_PER_TICK == 0: world.tick() rk.keep_time() self.started = True @@ -524,7 +534,7 @@ def close(self): self.started = False self._exit_event.set() - for s in self._carla_objects: + for s in self._simulation_objects: try: s.destroy() except Exception as e: @@ -536,15 +546,191 @@ def run(self, queue, retries=-1): bridge_p = Process(target=self.bridge_keep_alive, args=(queue, retries), daemon=True) bridge_p.start() return bridge_p + + # Must return object of class `World`, after spawning objects into that world + @abstractmethod + def spawn_objects(self): + pass + +class CarlaBridge(SimulatorBridge): + FRAMES_PER_TICK = 5 + + def spawn_objects(self): + camerad = Camerad() + # Simulator specific imports go here + import carla # pylint: disable=import-error + def connect_carla_client(host: str, port: int): + client = carla.Client(host, port) + client.set_timeout(5) + return client + + client = connect_carla_client(self._args.host, self._args.port) + world = client.load_world(self._args.town) + + settings = world.get_settings() + settings.synchronous_mode = True # Enables synchronous mode + settings.fixed_delta_seconds = 0.05 + world.apply_settings(settings) + + world.set_weather(carla.WeatherParameters.ClearSunset) + + if not self._args.high_quality: + world.unload_map_layer(carla.MapLayer.Foliage) + world.unload_map_layer(carla.MapLayer.Buildings) + world.unload_map_layer(carla.MapLayer.ParkedVehicles) + world.unload_map_layer(carla.MapLayer.Props) + world.unload_map_layer(carla.MapLayer.StreetLights) + world.unload_map_layer(carla.MapLayer.Particles) + + blueprint_library = world.get_blueprint_library() + + world_map = world.get_map() + + vehicle_bp = blueprint_library.filter('vehicle.tesla.*')[1] + vehicle_bp.set_attribute('role_name', 'hero') + spawn_points = world_map.get_spawn_points() + assert len(spawn_points) > self._args.num_selected_spawn_point, f'''No spawn point {self._args.num_selected_spawn_point}, try a value between 0 and + {len(spawn_points)} for this town.''' + spawn_point = spawn_points[self._args.num_selected_spawn_point] + vehicle = world.spawn_actor(vehicle_bp, spawn_point) + self._simulation_objects.append(vehicle) + + # make tires less slippery + # wheel_control = carla.WheelPhysicsControl(tire_friction=5) + physics_control = vehicle.get_physics_control() + physics_control.mass = 2326 + # physics_control.wheels = [wheel_control]*4 + physics_control.torque_curve = [[20.0, 500.0], [5000.0, 500.0]] + physics_control.gear_switch_time = 0.0 + vehicle.apply_physics_control(physics_control) + + transform = carla.Transform(carla.Location(x=0.8, z=1.13)) + + def create_camera(fov, callback): + blueprint = blueprint_library.find('sensor.camera.rgb') + blueprint.set_attribute('image_size_x', str(W)) + blueprint.set_attribute('image_size_y', str(H)) + blueprint.set_attribute('fov', str(fov)) + if not self._args.high_quality: + blueprint.set_attribute('enable_postprocess_effects', 'False') + camera = world.spawn_actor(blueprint, transform, attach_to=vehicle) + camera.listen(callback) + return camera + if self._args.dual_camera: + road_camera = create_camera(fov=40, callback=camerad.cam_callback_road) + self._simulation_objects.append(road_camera) + + road_wide_camera = create_camera(fov=120, callback=camerad.cam_callback_wide_road) # fov bigger than 120 shows unwanted artifacts + self._simulation_objects.append(road_wide_camera) + + # re-enable IMU + imu_bp = blueprint_library.find('sensor.other.imu') + imu_bp.set_attribute('sensor_tick', '0.01') + imu = world.spawn_actor(imu_bp, transform, attach_to=vehicle) + imu.listen(lambda imu: imu_callback(imu, self._vehicle_state)) + + gps_bp = blueprint_library.find('sensor.other.gnss') + gps = world.spawn_actor(gps_bp, transform, attach_to=vehicle) + gps.listen(lambda gps: gps_callback(gps, self._vehicle_state)) + self.params.put_bool("UbloxAvailable", True) + self._simulation_objects.extend([imu, gps]) + + vc = carla.VehicleControl(throttle=0, steer=0, brake=0, reverse=False) + + return CarlaWorld(world, vc, vehicle, camerad) + +class MetaDriveBridge(SimulatorBridge): + FRAMES_PER_TICK = 15 + + def __init__(self, args): + if args.dual_camera: + warnings.warn("Dual camera not supported in MetaDrive simulator for performance reasons") + args.dual_camera = False + super(MetaDriveBridge, self).__init__(args) + + def spawn_objects(self): + import metadrive # noqa: F401 pylint: disable=W0611 + import gym + env = gym.make('MetaDrive-10env-v0', config=dict(offscreen_render=True)) + + # from metadrive.utils.space import VehicleParameterSpace + # from metadrive.component.vehicle.vehicle_type import DefaultVehicle + # max_engine_force = VehicleParameterSpace.DEFAULT_VEHICLE["max_engine_force"] + # max_engine_force._replace(max=max_engine_force.max * 10) + # max_engine_force._replace(min=max_engine_force.min * 10) + + # config = dict( + # # camera_dist=3.0, + # # camera_height=1.0, + # # use_render=True, + # vehicle_config=dict( + # # enable_reverse=True, + # # image_source="rgb_camera", + # # rgb_camera=(0,0) + # ), + # offscreen_render=True, + # ) + + env.reset() + from metadrive.constants import CamMask + from metadrive.component.vehicle_module.base_camera import BaseCamera + from metadrive.engine.engine_utils import engine_initialized + from metadrive.engine.core.image_buffer import ImageBuffer + class RGBCameraWide(BaseCamera): + # shape(dim_1, dim_2) + BUFFER_W = W # dim 1 + BUFFER_H = H # dim 2 + CAM_MASK = CamMask.RgbCam + + def __init__(self): + assert engine_initialized(), "You should initialize engine before adding camera to vehicle" + self.BUFFER_W, self.BUFFER_H = W, H + super(RGBCameraWide, self).__init__() + print("SELF", self.BUFFER_W, self.BUFFER_H) + cam = self.get_cam() + lens = self.get_lens() + cam.lookAt(0, 2.4, 1.3) + cam.setHpr(0, 0.8, 0) + lens.setFov(160) + lens.setAspectRatio(1.15) + + env.vehicle.add_image_sensor("rgb_wide", RGBCameraWide()) + + # Monkey patch `get_rgb_array` since it involves unnecessary copies + def patch_get_rgb_array(self): + if self.engine.episode_step <= 1: + self.engine.graphicsEngine.renderFrame() + origin_img = self.cam.node().getDisplayRegion(0).getScreenshot() + img = np.frombuffer(origin_img.getRamImage().getData(), dtype=np.uint8) + img = img.reshape((origin_img.getYSize(), origin_img.getXSize(), 4)) + img = img[::-1] + img = img[..., :-1] + return img + + setattr(ImageBuffer, "get_rgb_array", patch_get_rgb_array) + + # Simulation tends to be slow in the initial steps. This prevents lagging later + for _ in range(20): + env.step([1.0, 0.0]) + + return MetaDriveWorld(env, self.FRAMES_PER_TICK) if __name__ == "__main__": q: Any = Queue() args = parse_args() + print("HELLO WORLD") try: - carla_bridge = CarlaBridge(args) - p = carla_bridge.run(q) + simulator_bridge: SimulatorBridge + if args.simulator == "metadrive": + print("STARTING METADRIVE BRIDGE") + simulator_bridge = MetaDriveBridge(args) + elif args.simulator == "carla": + simulator_bridge = CarlaBridge(args) + else: + raise AssertionError("simulator type not supported") + p = simulator_bridge.run(q) if args.joystick: # start input poll for joystick From f69eb533b8cff0377605e569d606485259772ac9 Mon Sep 17 00:00:00 2001 From: jon-chuang Date: Fri, 17 Feb 2023 19:15:56 +0800 Subject: [PATCH 02/28] minor --- tools/sim/bridge.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/tools/sim/bridge.py b/tools/sim/bridge.py index 1d746562d77997..8204a424561601 100755 --- a/tools/sim/bridge.py +++ b/tools/sim/bridge.py @@ -28,7 +28,7 @@ SCALE = 1 W, H = 1928 // SCALE, 1208 // SCALE REPEAT_COUNTER = 5 -PRINT_DECIMATION = 5 +PRINT_DECIMATION = 100 STEER_RATIO = 15. pm = messaging.PubMaster(['roadCameraState', 'wideRoadCameraState', 'accelerometer', 'gyroscope', 'can', "gpsLocationExternal"]) @@ -210,7 +210,7 @@ def gps_callback(gps, vehicle_state): "longitude": gps.longitude, "altitude": gps.altitude, "speed": vehicle_state.speed, - "source": log.GpsLocationData.SensorSource.wublox, + "source": log.GpsLocationData.SensorSource.ublox, } pm.send('gpsLocationExternal', dat) From c42bd6434cb9641242eb94c6f688044b68e14596 Mon Sep 17 00:00:00 2001 From: jon-chuang Date: Fri, 17 Feb 2023 19:33:02 +0800 Subject: [PATCH 03/28] minor --- tools/sim/bridge.py | 21 +++++++++++---------- 1 file changed, 11 insertions(+), 10 deletions(-) diff --git a/tools/sim/bridge.py b/tools/sim/bridge.py index f4651c530d73b3..4ac3e9aa20b351 100755 --- a/tools/sim/bridge.py +++ b/tools/sim/bridge.py @@ -388,14 +388,15 @@ def bridge_keep_alive(self, q: Queue, retries: int): self.close() def _run(self, q: Queue): - self._vehicle_state = VehicleState() + vehicle_state = VehicleState() + self._vehicle_state = vehicle_state world = self.spawn_objects() # launch fake car threads - self._threads.append(threading.Thread(target=panda_state_function, args=(self._vehicle_state, self._exit_event,))) + self._threads.append(threading.Thread(target=panda_state_function, args=(vehicle_state, self._exit_event,))) self._threads.append(threading.Thread(target=peripheral_state_function, args=(self._exit_event,))) self._threads.append(threading.Thread(target=fake_driver_monitoring, args=(self._exit_event,))) - self._threads.append(threading.Thread(target=can_function_runner, args=(self._vehicle_state, self._exit_event,))) + self._threads.append(threading.Thread(target=can_function_runner, args=(vehicle_state, self._exit_event,))) for t in self._threads: t.start() @@ -454,7 +455,7 @@ def _run(self, q: Queue): cruise_button = CruiseButtons.CANCEL is_openpilot_engaged = False elif m[0] == "ignition": - self._vehicle_state.ignition = not self._vehicle_state.ignition + vehicle_state.ignition = not vehicle_state.ignition elif m[0] == "quit": break @@ -517,11 +518,11 @@ def _run(self, q: Queue): world.apply_controls(steer_sim, throttle_out, brake_out, rk.frame) # --------------Step 3------------------------------- - self._vehicle_state.speed = world.get_speed() - self._vehicle_state.vel = world.get_velocity() - self._vehicle_state.angle = steer_out - self._vehicle_state.cruise_button = cruise_button - self._vehicle_state.is_engaged = is_openpilot_engaged + vehicle_state.speed = world.get_speed() + vehicle_state.vel = world.get_velocity() + vehicle_state.angle = steer_out + vehicle_state.cruise_button = cruise_button + vehicle_state.is_engaged = is_openpilot_engaged if rk.frame % PRINT_DECIMATION == 0: print("frame: ", "engaged:", is_openpilot_engaged, "; throttle: ", round(throttle_out, 3), "; steer(deg): ", @@ -551,7 +552,7 @@ def run(self, queue, retries=-1): # Must return object of class `World`, after spawning objects into that world @abstractmethod - def spawn_objects(self): + def spawn_objects(self) -> World: pass class CarlaBridge(SimulatorBridge): From fc3f6425db27f28c48002046b097d637361b4a94 Mon Sep 17 00:00:00 2001 From: jon-chuang Date: Fri, 17 Feb 2023 19:54:36 +0800 Subject: [PATCH 04/28] minor --- tools/sim/bridge.py | 10 ++++------ 1 file changed, 4 insertions(+), 6 deletions(-) diff --git a/tools/sim/bridge.py b/tools/sim/bridge.py index 4ac3e9aa20b351..75094830828d3d 100755 --- a/tools/sim/bridge.py +++ b/tools/sim/bridge.py @@ -264,9 +264,10 @@ def tick(self): pass class CarlaWorld(World): - def __init__(self, world, vc, vehicle, camerad): + def __init__(self, world, vehicle, camerad): + import carla # pylint: disable=import-error self.world = world - self.vc = vc + self.vc = carla.VehicleControl(throttle=0, steer=0, brake=0, reverse=False) self.vehicle = vehicle self.max_steer_angle: float = vehicle.get_physics_control().wheels[0].max_steer_angle self.camerad = camerad @@ -560,7 +561,6 @@ class CarlaBridge(SimulatorBridge): def spawn_objects(self): camerad = Camerad() - # Simulator specific imports go here import carla # pylint: disable=import-error def connect_carla_client(host: str, port: int): client = carla.Client(host, port) @@ -639,9 +639,7 @@ def create_camera(fov, callback): self.params.put_bool("UbloxAvailable", True) self._simulation_objects.extend([imu, gps]) - vc = carla.VehicleControl(throttle=0, steer=0, brake=0, reverse=False) - - return CarlaWorld(world, vc, vehicle, camerad) + return CarlaWorld(world, vehicle, camerad) class MetaDriveBridge(SimulatorBridge): FRAMES_PER_TICK = 15 From fdfebed860a3844b4fa87134c57e26e69f78ec08 Mon Sep 17 00:00:00 2001 From: jon-chuang Date: Fri, 17 Feb 2023 22:25:20 +0800 Subject: [PATCH 05/28] poetry --- poetry.lock | 2 +- pyproject.toml | 3 +-- 2 files changed, 2 insertions(+), 3 deletions(-) diff --git a/poetry.lock b/poetry.lock index be8c4dd310a912..91e1b5c826506c 100644 --- a/poetry.lock +++ b/poetry.lock @@ -1604,7 +1604,7 @@ optional = false python-versions = ">=3.7" [package.dependencies] -MarkupSafe = "==2.0.1" +MarkupSafe = ">=2.0" [package.extras] i18n = ["Babel (>=2.7)"] diff --git a/pyproject.toml b/pyproject.toml index bc0c13b5108647..9aed704e8bdee4 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -59,6 +59,7 @@ urllib3 = "^1.26.10" utm = "^0.7.0" websocket_client = "^1.3.3" polyline = "^1.4.0" +metadrive-simulator = "^0.2.6.0" [tool.poetry.group.dev.dependencies] @@ -173,8 +174,6 @@ Werkzeug = "^2.1.2" zerorpc = { git = "https://github.com/commaai/zerorpc-python.git", branch = "master" } omegaconf = "^2.3.0" osmnx = "==1.2.2" -markupsafe= "==2.0.1" -metadrive-simulator = "0.2.6" [build-system] requires = ["poetry-core"] From 30079a7a2c991dae7151956585d6d392aaf707c0 Mon Sep 17 00:00:00 2001 From: jon-chuang Date: Sat, 18 Feb 2023 02:34:38 +0800 Subject: [PATCH 06/28] integration test and fix ObdMultiplexing issue --- tools/sim/bridge.py | 8 +- tools/sim/tests/test_metadrive_integration.py | 86 +++++++++++++++++++ 2 files changed, 90 insertions(+), 4 deletions(-) create mode 100755 tools/sim/tests/test_metadrive_integration.py diff --git a/tools/sim/bridge.py b/tools/sim/bridge.py index 75094830828d3d..500825082cc649 100755 --- a/tools/sim/bridge.py +++ b/tools/sim/bridge.py @@ -182,6 +182,7 @@ def peripheral_state_function(exit_event: threading.Event): 'current': 5678, 'fanSpeedRpm': 1000 } + Params().put_bool("ObdMultiplexingDisabled", True) pm.send('peripheralState', dat) time.sleep(0.5) @@ -350,6 +351,7 @@ def __init__(self, arguments): print("Dual Camera disabled") self.params.put_bool("WideCameraOnly", not arguments.dual_camera) self.params.put_bool("DisengageOnAccelerator", True) + # self.params.put_bool("ExperimentalMode", True) self._args = arguments self._simulation_objects = [] @@ -712,20 +714,18 @@ def patch_get_rgb_array(self): setattr(ImageBuffer, "get_rgb_array", patch_get_rgb_array) # Simulation tends to be slow in the initial steps. This prevents lagging later - for _ in range(20): - env.step([1.0, 0.0]) + for _ in range(30): + env.step([0.0, 1.0]) return MetaDriveWorld(env, self.FRAMES_PER_TICK) if __name__ == "__main__": q: Any = Queue() args = parse_args() - print("HELLO WORLD") try: simulator_bridge: SimulatorBridge if args.simulator == "metadrive": - print("STARTING METADRIVE BRIDGE") simulator_bridge = MetaDriveBridge(args) elif args.simulator == "carla": simulator_bridge = CarlaBridge(args) diff --git a/tools/sim/tests/test_metadrive_integration.py b/tools/sim/tests/test_metadrive_integration.py new file mode 100755 index 00000000000000..d6ed5a6e1b8a25 --- /dev/null +++ b/tools/sim/tests/test_metadrive_integration.py @@ -0,0 +1,86 @@ +#!/usr/bin/env python3 +import subprocess +import time +from typing import List +import unittest +import os +from multiprocessing import Queue + +from cereal import messaging +from common.basedir import BASEDIR +from tools.sim import bridge +from tools.sim.bridge import MetaDriveBridge + +CI = "CI" in os.environ + +SIM_DIR = os.path.join(BASEDIR, "tools/sim") + +class TestMetaDriveIntegration(unittest.TestCase): + processes: List[subprocess.Popen] = [] + + 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) + self.processes.append(p_manager) + time.sleep(10.0) + + sm = messaging.SubMaster(['controlsState', 'carEvents', 'managerState']) + q = Queue() + sim_bridge = MetaDriveBridge(bridge.parse_args(["--frames_per_tick", "25"])) + p_bridge = sim_bridge.run(q, retries=10) + self.processes.append(p_bridge) + + max_time_per_step = 60 + + # Wait for bridge to startup + start_waiting = time.monotonic() + while not sim_bridge.started and time.monotonic() < start_waiting + max_time_per_step: + time.sleep(0.1) + self.assertEqual(p_bridge.exitcode, None, f"Bridge process should be running, but exited with code {p_bridge.exitcode}") + + start_time = time.monotonic() + no_car_events_issues_once = False + car_event_issues = [] + not_running = [] + while time.monotonic() < start_time + max_time_per_step: + sm.update() + + not_running = [p.name for p in sm['managerState'].processes if not p.running and p.shouldBeRunning] + car_event_issues = [event.name for event in sm['carEvents'] if any([event.noEntry, event.softDisable, event.immediateDisable])] + + if sm.all_alive() and len(car_event_issues) == 0 and len(not_running) == 0: + no_car_events_issues_once = True + break + + self.assertTrue(no_car_events_issues_once, f"Failed because no messages received, or CarEvents '{car_event_issues}' or processes not running '{not_running}'") + + start_time = time.monotonic() + min_counts_control_active = 100 + control_active = 0 + + while time.monotonic() < start_time + max_time_per_step: + sm.update() + + q.put("cruise_down") # Try engaging + + if sm.all_alive() and sm['controlsState'].active: + control_active += 1 + + if control_active == min_counts_control_active: + break + + self.assertEqual(min_counts_control_active, control_active, f"Simulator did not engage a minimal of {min_counts_control_active} steps was {control_active}") + + def tearDown(self): + print("Test shutting down. CommIssues are acceptable") + for p in reversed(self.processes): + p.terminate() + + for p in reversed(self.processes): + if isinstance(p, subprocess.Popen): + p.wait(15) + else: + p.join(15) + +if __name__ == "__main__": + unittest.main() From e95c2bfbfd12b1b9bb7c4f5bbbc947cb8970c9f3 Mon Sep 17 00:00:00 2001 From: jon-chuang Date: Sat, 18 Feb 2023 15:54:39 +0800 Subject: [PATCH 07/28] refactor --- tools/sim/bridge.py | 751 ---------------------------------------- tools/sim/run_bridge.py | 38 ++ 2 files changed, 38 insertions(+), 751 deletions(-) delete mode 100755 tools/sim/bridge.py create mode 100755 tools/sim/run_bridge.py diff --git a/tools/sim/bridge.py b/tools/sim/bridge.py deleted file mode 100755 index 500825082cc649..00000000000000 --- a/tools/sim/bridge.py +++ /dev/null @@ -1,751 +0,0 @@ -#!/usr/bin/env python3 -import argparse -import math -import os -import signal -import threading -import time -from multiprocessing import Process, Queue -from typing import Any -from abc import ABC, abstractmethod -import warnings - -import numpy as np -import pyopencl as cl -import pyopencl.array as cl_array - -import cereal.messaging as messaging -from cereal import log -from cereal.visionipc import VisionIpcServer, VisionStreamType -from common.basedir import BASEDIR -from common.numpy_fast import clip -from common.params import Params -from common.realtime import DT_DMON, Ratekeeper -from selfdrive.car.honda.values import CruiseButtons -from selfdrive.test.helpers import set_params_enabled -from tools.sim.lib.can import can_function - -SCALE = 1 -W, H = 1928 // SCALE, 1208 // SCALE -REPEAT_COUNTER = 5 -PRINT_DECIMATION = 100 -STEER_RATIO = 15. - -pm = messaging.PubMaster(['roadCameraState', 'wideRoadCameraState', 'accelerometer', 'gyroscope', 'can', "gpsLocationExternal"]) -sm = messaging.SubMaster(['carControl', 'controlsState']) - -def parse_args(add_args=None): - parser = argparse.ArgumentParser(description='Bridge between CARLA and openpilot.') - parser.add_argument('--joystick', action='store_true') - parser.add_argument('--high_quality', action='store_true') - parser.add_argument('--dual_camera', action='store_true') - parser.add_argument('--town', type=str, default='Town04_Opt') - parser.add_argument('--spawn_point', dest='num_selected_spawn_point', type=int, default=16) - parser.add_argument('--host', dest='host', type=str, default='127.0.0.1') - parser.add_argument('--port', dest='port', type=int, default=2000) - parser.add_argument('--simulator', dest='simulator', type=str, default='metadrive') - parser.add_argument('--frames_per_tick', dest='frames_per_tick', type=int, default=None) - - return parser.parse_args(add_args) - - -class VehicleState: - def __init__(self): - self.speed = 0.0 - self.angle = 0.0 - self.bearing_deg = 0.0 - self.vel = None - self.cruise_button = 0 - self.is_engaged = False - self.ignition = True - - -def steer_rate_limit(old, new): - # Rate limiting to 0.5 degrees per step - limit = 0.5 - if new > old + limit: - return old + limit - elif new < old - limit: - return old - limit - else: - return new - - - -class Camerad: - def __init__(self): - self.frame_road_id = 0 - self.frame_wide_id = 0 - self.vipc_server = VisionIpcServer("camerad") - - self.vipc_server.create_buffers(VisionStreamType.VISION_STREAM_ROAD, 5, False, W, H) - self.vipc_server.create_buffers(VisionStreamType.VISION_STREAM_WIDE_ROAD, 5, False, W, H) - self.vipc_server.start_listener() - - # set up for pyopencl rgb to yuv conversion - self.ctx = cl.create_some_context() - self.queue = cl.CommandQueue(self.ctx) - cl_arg = f" -DHEIGHT={H} -DWIDTH={W} -DRGB_STRIDE={W * 3} -DUV_WIDTH={W // 2} -DUV_HEIGHT={H // 2} -DRGB_SIZE={W * H} -DCL_DEBUG " - - kernel_fn = os.path.join(BASEDIR, "tools/sim/rgb_to_nv12.cl") - with open(kernel_fn) as f: - prg = cl.Program(self.ctx, f.read()).build(cl_arg) - self.krnl = prg.rgb_to_nv12 - self.Wdiv4 = W // 4 if (W % 4 == 0) else (W + (4 - W % 4)) // 4 - self.Hdiv4 = H // 4 if (H % 4 == 0) else (H + (4 - H % 4)) // 4 - - def cam_callback_road(self, image): - yuv = self.img_to_yuv(image) - self._send_yuv(yuv, self.frame_road_id, 'roadCameraState', VisionStreamType.VISION_STREAM_ROAD) - self.frame_road_id += 1 - - def cam_callback_wide_road(self, image): - yuv = self.img_to_yuv(image) - self._send_yuv(yuv, self.frame_wide_id, 'wideRoadCameraState', VisionStreamType.VISION_STREAM_WIDE_ROAD) - self.frame_wide_id += 1 - - def cam_send_yuv_wide_road(self, yuv): - self._send_yuv(yuv, self.frame_wide_id, 'wideRoadCameraState', VisionStreamType.VISION_STREAM_WIDE_ROAD) - self.frame_wide_id += 1 - - # Returns: yuv bytes - def img_to_yuv(self, img): - assert img.shape == (H, W, 3), f"{img.shape}" - rgb = np.reshape(img, (H, W * 3)) - rgb_cl = cl_array.to_device(self.queue, rgb) - yuv_cl = cl_array.empty_like(rgb_cl) - self.krnl(self.queue, (np.int32(self.Wdiv4), np.int32(self.Hdiv4)), None, rgb_cl.data, yuv_cl.data).wait() - yuv = np.resize(yuv_cl.get(), rgb.size // 2) - return yuv.data.tobytes() - - def _send_yuv(self, yuv, frame_id, pub_type, yuv_type): - eof = int(frame_id * 0.05 * 1e9) - self.vipc_server.send(yuv_type, yuv, frame_id, eof, eof) - - dat = messaging.new_message(pub_type) - msg = { - "frameId": frame_id, - "transform": [1.0, 0.0, 0.0, - 0.0, 1.0, 0.0, - 0.0, 0.0, 1.0] - } - setattr(dat, pub_type, msg) - pm.send(pub_type, dat) - -def imu_callback(imu, vehicle_state): - # send 5x since 'sensor_tick' doesn't seem to work. limited by the world tick? - for _ in range(5): - vehicle_state.bearing_deg = math.degrees(imu.compass) - dat = messaging.new_message('accelerometer') - dat.accelerometer.sensor = 4 - dat.accelerometer.type = 0x10 - dat.accelerometer.timestamp = dat.logMonoTime # TODO: use the IMU timestamp - dat.accelerometer.init('acceleration') - dat.accelerometer.acceleration.v = [imu.accelerometer.x, imu.accelerometer.y, imu.accelerometer.z] - pm.send('accelerometer', dat) - - # copied these numbers from locationd - dat = messaging.new_message('gyroscope') - dat.gyroscope.sensor = 5 - dat.gyroscope.type = 0x10 - dat.gyroscope.timestamp = dat.logMonoTime # TODO: use the IMU timestamp - dat.gyroscope.init('gyroUncalibrated') - dat.gyroscope.gyroUncalibrated.v = [imu.gyroscope.x, imu.gyroscope.y, imu.gyroscope.z] - pm.send('gyroscope', dat) - time.sleep(0.01) - - -def panda_state_function(vs: VehicleState, exit_event: threading.Event): - pm = messaging.PubMaster(['pandaStates']) - while not exit_event.is_set(): - dat = messaging.new_message('pandaStates', 1) - dat.valid = True - dat.pandaStates[0] = { - 'ignitionLine': vs.ignition, - 'pandaType': "blackPanda", - 'controlsAllowed': True, - 'safetyModel': 'hondaNidec', - } - pm.send('pandaStates', dat) - time.sleep(0.5) - - -def peripheral_state_function(exit_event: threading.Event): - pm = messaging.PubMaster(['peripheralState']) - while not exit_event.is_set(): - dat = messaging.new_message('peripheralState') - dat.valid = True - # fake peripheral state data - dat.peripheralState = { - 'pandaType': log.PandaState.PandaType.blackPanda, - 'voltage': 12000, - 'current': 5678, - 'fanSpeedRpm': 1000 - } - Params().put_bool("ObdMultiplexingDisabled", True) - pm.send('peripheralState', dat) - time.sleep(0.5) - - -def gps_callback(gps, vehicle_state): - dat = messaging.new_message('gpsLocationExternal') - - # transform vel from carla to NED - # north is -Y in CARLA - velNED = [ - -vehicle_state.vel.y, # north/south component of NED is negative when moving south - vehicle_state.vel.x, # positive when moving east, which is x in carla - vehicle_state.vel.z, - ] - - dat.gpsLocationExternal = { - "unixTimestampMillis": int(time.time() * 1000), - "flags": 1, # valid fix - "accuracy": 1.0, - "verticalAccuracy": 1.0, - "speedAccuracy": 0.1, - "bearingAccuracyDeg": 0.1, - "vNED": velNED, - "bearingDeg": vehicle_state.bearing_deg, - "latitude": gps.latitude, - "longitude": gps.longitude, - "altitude": gps.altitude, - "speed": vehicle_state.speed, - "source": log.GpsLocationData.SensorSource.ublox, - } - - pm.send('gpsLocationExternal', dat) - - -def fake_driver_monitoring(exit_event: threading.Event): - pm = messaging.PubMaster(['driverStateV2', 'driverMonitoringState']) - while not exit_event.is_set(): - # dmonitoringmodeld output - dat = messaging.new_message('driverStateV2') - dat.driverStateV2.leftDriverData.faceOrientation = [0., 0., 0.] - dat.driverStateV2.leftDriverData.faceProb = 1.0 - dat.driverStateV2.rightDriverData.faceOrientation = [0., 0., 0.] - dat.driverStateV2.rightDriverData.faceProb = 1.0 - pm.send('driverStateV2', dat) - - # dmonitoringd output - dat = messaging.new_message('driverMonitoringState') - dat.driverMonitoringState = { - "faceDetected": True, - "isDistracted": False, - "awarenessStatus": 1., - } - pm.send('driverMonitoringState', dat) - - time.sleep(DT_DMON) - - -def can_function_runner(vs: VehicleState, exit_event: threading.Event): - i = 0 - while not exit_event.is_set(): - can_function(pm, vs.speed, vs.angle, i, vs.cruise_button, vs.is_engaged) - time.sleep(0.01) - i += 1 - -class World(ABC): - @abstractmethod - def apply_controls(self, steer_sim, throttle_out, brake_out, frame: int): - pass - @abstractmethod - def get_velocity(self): - pass - @abstractmethod - def get_speed(self) -> float: - pass - @abstractmethod - def get_steer_correction(self) -> float: - pass - @abstractmethod - def tick(self): - pass - -class CarlaWorld(World): - def __init__(self, world, vehicle, camerad): - import carla # pylint: disable=import-error - self.world = world - self.vc = carla.VehicleControl(throttle=0, steer=0, brake=0, reverse=False) - self.vehicle = vehicle - self.max_steer_angle: float = vehicle.get_physics_control().wheels[0].max_steer_angle - self.camerad = camerad - - def apply_controls(self, steer_sim, throttle_out, brake_out, _frame): - self.vc.throttle = throttle_out / 0.6 - self.vc.steer = steer_sim - self.vc.brake = brake_out - self.vehicle.apply_control(self.vc) - - def get_velocity(self): - self.vel = self.vehicle.get_velocity() - return self.vel - - def get_speed(self) -> float: - return math.sqrt(self.vel.x ** 2 + self.vel.y ** 2 + self.vel.z ** 2) # in m/s - - def get_steer_correction(self) -> float: - return self.max_steer_angle * STEER_RATIO * -1 - - def tick(self): - self.world.tick() - -class MetaDriveWorld(World): - def __init__(self, env, frames_per_tick: float): - self.env = env - self.speed = 0.0 - self.yuv = None - self.camerad = Camerad() - self.frames_per_tick = frames_per_tick - - def apply_controls(self, steer_sim, throttle_out, brake_out, frame: int): - vc = [0.0, 0.0] - vc[0] = steer_sim * -1 - if throttle_out: - vc[1] = throttle_out * 10 - else: - vc[1] = -brake_out - - if frame % self.frames_per_tick == 0 or self.yuv is None: - if frame % (self.frames_per_tick * 2) == 0: - o, _, _, _ = self.env.step(vc) - self.speed = o["state"][3] * self.frames_per_tick * 2 # empirically derived - img = self.env.vehicle.image_sensors["rgb_wide"].get_pixels_array(self.env.vehicle, False) - self.yuv = self.camerad.img_to_yuv(img) - - if frame % (self.frames_per_tick * 2) == 0: - self.camerad.cam_send_yuv_wide_road(self.yuv) - - def get_velocity(self): - return None - - def get_speed(self) -> float: - return self.speed - - def get_steer_correction(self) -> float: - max_steer_angle = 75 / self.frames_per_tick - return max_steer_angle * STEER_RATIO * -1 - - def tick(self): - pass - - -class SimulatorBridge(ABC): - FRAMES_PER_TICK = 5 - def __init__(self, arguments): - if arguments.frames_per_tick: - self.frames_per_tick = arguments.frames_per_tick - else: - self.frames_per_tick = self.FRAMES_PER_TICK - print(f"Running at {self.frames_per_tick} frames per tick") - set_params_enabled() - self.params = Params() - - msg = messaging.new_message('liveCalibration') - msg.liveCalibration.validBlocks = 20 - msg.liveCalibration.rpyCalib = [0.0, 0.0, 0.0] - self.params.put("CalibrationParams", msg.to_bytes()) - if not arguments.dual_camera: - print("Dual Camera disabled") - self.params.put_bool("WideCameraOnly", not arguments.dual_camera) - self.params.put_bool("DisengageOnAccelerator", True) - # self.params.put_bool("ExperimentalMode", True) - - self._args = arguments - self._simulation_objects = [] - self._exit_event = threading.Event() - self._threads = [] - self._keep_alive = True - self.started = False - signal.signal(signal.SIGTERM, self._on_shutdown) - self._exit = threading.Event() - - def _on_shutdown(self, signal, frame): - self._keep_alive = False - - def bridge_keep_alive(self, q: Queue, retries: int): - try: - while self._keep_alive: - try: - self._run(q) - break - except RuntimeError as e: - self.close() - if retries == 0: - raise - - # Reset for another try - self._simulation_objects = [] - self._threads = [] - self._exit_event = threading.Event() - - retries -= 1 - if retries <= -1: - print(f"Restarting bridge. Error: {e} ") - else: - print(f"Restarting bridge. Retries left {retries}. Error: {e} ") - finally: - # Clean up resources in the opposite order they were created. - self.close() - - def _run(self, q: Queue): - vehicle_state = VehicleState() - self._vehicle_state = vehicle_state - world = self.spawn_objects() - - # launch fake car threads - self._threads.append(threading.Thread(target=panda_state_function, args=(vehicle_state, self._exit_event,))) - self._threads.append(threading.Thread(target=peripheral_state_function, args=(self._exit_event,))) - self._threads.append(threading.Thread(target=fake_driver_monitoring, args=(self._exit_event,))) - self._threads.append(threading.Thread(target=can_function_runner, args=(vehicle_state, self._exit_event,))) - for t in self._threads: - t.start() - - # init - throttle_ease_out_counter = REPEAT_COUNTER - brake_ease_out_counter = REPEAT_COUNTER - steer_ease_out_counter = REPEAT_COUNTER - - is_openpilot_engaged = False - throttle_out = steer_out = brake_out = 0. - throttle_op = steer_op = brake_op = 0. - throttle_manual = steer_manual = brake_manual = 0. - - old_steer = old_brake = old_throttle = 0. - throttle_manual_multiplier = 0.7 # keyboard signal is always 1 - brake_manual_multiplier = 0.7 # keyboard signal is always 1 - steer_manual_multiplier = 45 * STEER_RATIO # keyboard signal is always 1 - - # loop - rk = Ratekeeper(100, print_delay_threshold=0.05) - - while self._keep_alive: - # 1. Read the throttle, steer and brake from op or manual controls - # 2. Set instructions in Carla - # 3. Send current carstate to op via can - - cruise_button = 0 - throttle_out = steer_out = brake_out = 0.0 - throttle_op = steer_op = brake_op = 0.0 - throttle_manual = steer_manual = brake_manual = 0.0 - - # --------------Step 1------------------------------- - if not q.empty(): - message = q.get() - m = message.split('_') - if m[0] == "steer": - steer_manual = float(m[1]) - is_openpilot_engaged = False - elif m[0] == "throttle": - throttle_manual = float(m[1]) - is_openpilot_engaged = False - elif m[0] == "brake": - brake_manual = float(m[1]) - is_openpilot_engaged = False - elif m[0] == "reverse": - cruise_button = CruiseButtons.CANCEL - is_openpilot_engaged = False - elif m[0] == "cruise": - if m[1] == "down": - cruise_button = CruiseButtons.DECEL_SET - is_openpilot_engaged = True - elif m[1] == "up": - cruise_button = CruiseButtons.RES_ACCEL - is_openpilot_engaged = True - elif m[1] == "cancel": - cruise_button = CruiseButtons.CANCEL - is_openpilot_engaged = False - elif m[0] == "ignition": - vehicle_state.ignition = not vehicle_state.ignition - elif m[0] == "quit": - break - - throttle_out = throttle_manual * throttle_manual_multiplier - steer_out = steer_manual * steer_manual_multiplier - brake_out = brake_manual * brake_manual_multiplier - - old_steer = steer_out - old_throttle = throttle_out - old_brake = brake_out - - if is_openpilot_engaged: - sm.update(0) - - # TODO gas and brake is deprecated - throttle_op = clip(sm['carControl'].actuators.accel / 1.6, 0.0, 1.0) - brake_op = clip(-sm['carControl'].actuators.accel / 4.0, 0.0, 1.0) - steer_op = sm['carControl'].actuators.steeringAngleDeg - - throttle_out = throttle_op - steer_out = steer_op - brake_out = brake_op - - steer_out = steer_rate_limit(old_steer, steer_out) - old_steer = steer_out - - else: - if throttle_out == 0 and old_throttle > 0: - if throttle_ease_out_counter > 0: - throttle_out = old_throttle - throttle_ease_out_counter += -1 - else: - throttle_ease_out_counter = REPEAT_COUNTER - old_throttle = 0 - - if brake_out == 0 and old_brake > 0: - if brake_ease_out_counter > 0: - brake_out = old_brake - brake_ease_out_counter += -1 - else: - brake_ease_out_counter = REPEAT_COUNTER - old_brake = 0 - - if steer_out == 0 and old_steer != 0: - if steer_ease_out_counter > 0: - steer_out = old_steer - steer_ease_out_counter += -1 - else: - steer_ease_out_counter = REPEAT_COUNTER - old_steer = 0 - - # --------------Step 2------------------------------- - steer_correction = world.get_steer_correction() - steer_sim = steer_out / steer_correction - - steer_sim = np.clip(steer_sim, -1, 1) - steer_out = steer_sim * steer_correction - old_steer = steer_sim * steer_correction - - world.apply_controls(steer_sim, throttle_out, brake_out, rk.frame) - - # --------------Step 3------------------------------- - vehicle_state.speed = world.get_speed() - vehicle_state.vel = world.get_velocity() - vehicle_state.angle = steer_out - vehicle_state.cruise_button = cruise_button - vehicle_state.is_engaged = is_openpilot_engaged - - if rk.frame % PRINT_DECIMATION == 0: - print("frame: ", "engaged:", is_openpilot_engaged, "; throttle: ", round(throttle_out, 3), "; steer(deg): ", - round(steer_out, 3), "; brake: ", round(brake_out, 3)) - - if rk.frame % self.FRAMES_PER_TICK == 0: - world.tick() - rk.keep_time() - self.started = True - - def close(self): - self.started = False - self._exit_event.set() - - for s in self._simulation_objects: - try: - s.destroy() - except Exception as e: - print("Failed to destroy carla object", e) - for t in reversed(self._threads): - t.join() - - def run(self, queue, retries=-1): - bridge_p = Process(target=self.bridge_keep_alive, args=(queue, retries), daemon=True) - bridge_p.start() - return bridge_p - - # Must return object of class `World`, after spawning objects into that world - @abstractmethod - def spawn_objects(self) -> World: - pass - -class CarlaBridge(SimulatorBridge): - FRAMES_PER_TICK = 5 - - def spawn_objects(self): - camerad = Camerad() - import carla # pylint: disable=import-error - def connect_carla_client(host: str, port: int): - client = carla.Client(host, port) - client.set_timeout(5) - return client - - client = connect_carla_client(self._args.host, self._args.port) - world = client.load_world(self._args.town) - - settings = world.get_settings() - settings.synchronous_mode = True # Enables synchronous mode - settings.fixed_delta_seconds = 0.05 - world.apply_settings(settings) - - world.set_weather(carla.WeatherParameters.ClearSunset) - - if not self._args.high_quality: - world.unload_map_layer(carla.MapLayer.Foliage) - world.unload_map_layer(carla.MapLayer.Buildings) - world.unload_map_layer(carla.MapLayer.ParkedVehicles) - world.unload_map_layer(carla.MapLayer.Props) - world.unload_map_layer(carla.MapLayer.StreetLights) - world.unload_map_layer(carla.MapLayer.Particles) - - blueprint_library = world.get_blueprint_library() - - world_map = world.get_map() - - vehicle_bp = blueprint_library.filter('vehicle.tesla.*')[1] - vehicle_bp.set_attribute('role_name', 'hero') - spawn_points = world_map.get_spawn_points() - assert len(spawn_points) > self._args.num_selected_spawn_point, f'''No spawn point {self._args.num_selected_spawn_point}, try a value between 0 and - {len(spawn_points)} for this town.''' - spawn_point = spawn_points[self._args.num_selected_spawn_point] - vehicle = world.spawn_actor(vehicle_bp, spawn_point) - self._simulation_objects.append(vehicle) - - # make tires less slippery - # wheel_control = carla.WheelPhysicsControl(tire_friction=5) - physics_control = vehicle.get_physics_control() - physics_control.mass = 2326 - # physics_control.wheels = [wheel_control]*4 - physics_control.torque_curve = [[20.0, 500.0], [5000.0, 500.0]] - physics_control.gear_switch_time = 0.0 - vehicle.apply_physics_control(physics_control) - - transform = carla.Transform(carla.Location(x=0.8, z=1.13)) - - def create_camera(fov, callback): - blueprint = blueprint_library.find('sensor.camera.rgb') - blueprint.set_attribute('image_size_x', str(W)) - blueprint.set_attribute('image_size_y', str(H)) - blueprint.set_attribute('fov', str(fov)) - if not self._args.high_quality: - blueprint.set_attribute('enable_postprocess_effects', 'False') - camera = world.spawn_actor(blueprint, transform, attach_to=vehicle) - camera.listen(callback) - return camera - - if self._args.dual_camera: - road_camera = create_camera(fov=40, callback=camerad.cam_callback_road) - self._simulation_objects.append(road_camera) - - road_wide_camera = create_camera(fov=120, callback=camerad.cam_callback_wide_road) # fov bigger than 120 shows unwanted artifacts - self._simulation_objects.append(road_wide_camera) - - # re-enable IMU - imu_bp = blueprint_library.find('sensor.other.imu') - imu_bp.set_attribute('sensor_tick', '0.01') - imu = world.spawn_actor(imu_bp, transform, attach_to=vehicle) - imu.listen(lambda imu: imu_callback(imu, self._vehicle_state)) - - gps_bp = blueprint_library.find('sensor.other.gnss') - gps = world.spawn_actor(gps_bp, transform, attach_to=vehicle) - gps.listen(lambda gps: gps_callback(gps, self._vehicle_state)) - self.params.put_bool("UbloxAvailable", True) - self._simulation_objects.extend([imu, gps]) - - return CarlaWorld(world, vehicle, camerad) - -class MetaDriveBridge(SimulatorBridge): - FRAMES_PER_TICK = 15 - - def __init__(self, args): - if args.dual_camera: - warnings.warn("Dual camera not supported in MetaDrive simulator for performance reasons") - args.dual_camera = False - super(MetaDriveBridge, self).__init__(args) - - def spawn_objects(self): - import metadrive # noqa: F401 pylint: disable=W0611 - import gym - env = gym.make('MetaDrive-10env-v0', config=dict(offscreen_render=True)) - - # from metadrive.utils.space import VehicleParameterSpace - # from metadrive.component.vehicle.vehicle_type import DefaultVehicle - # max_engine_force = VehicleParameterSpace.DEFAULT_VEHICLE["max_engine_force"] - # max_engine_force._replace(max=max_engine_force.max * 10) - # max_engine_force._replace(min=max_engine_force.min * 10) - - # config = dict( - # # camera_dist=3.0, - # # camera_height=1.0, - # # use_render=True, - # vehicle_config=dict( - # # enable_reverse=True, - # # image_source="rgb_camera", - # # rgb_camera=(0,0) - # ), - # offscreen_render=True, - # ) - - env.reset() - from metadrive.constants import CamMask - from metadrive.component.vehicle_module.base_camera import BaseCamera - from metadrive.engine.engine_utils import engine_initialized - from metadrive.engine.core.image_buffer import ImageBuffer - class RGBCameraWide(BaseCamera): - # shape(dim_1, dim_2) - BUFFER_W = W # dim 1 - BUFFER_H = H # dim 2 - CAM_MASK = CamMask.RgbCam - - def __init__(self): - assert engine_initialized(), "You should initialize engine before adding camera to vehicle" - self.BUFFER_W, self.BUFFER_H = W, H - super(RGBCameraWide, self).__init__() - print("SELF", self.BUFFER_W, self.BUFFER_H) - cam = self.get_cam() - lens = self.get_lens() - cam.lookAt(0, 2.4, 1.3) - cam.setHpr(0, 0.8, 0) - lens.setFov(160) - lens.setAspectRatio(1.15) - - env.vehicle.add_image_sensor("rgb_wide", RGBCameraWide()) - - # Monkey patch `get_rgb_array` since it involves unnecessary copies - def patch_get_rgb_array(self): - if self.engine.episode_step <= 1: - self.engine.graphicsEngine.renderFrame() - origin_img = self.cam.node().getDisplayRegion(0).getScreenshot() - img = np.frombuffer(origin_img.getRamImage().getData(), dtype=np.uint8) - img = img.reshape((origin_img.getYSize(), origin_img.getXSize(), 4)) - img = img[::-1] - img = img[..., :-1] - return img - - setattr(ImageBuffer, "get_rgb_array", patch_get_rgb_array) - - # Simulation tends to be slow in the initial steps. This prevents lagging later - for _ in range(30): - env.step([0.0, 1.0]) - - return MetaDriveWorld(env, self.FRAMES_PER_TICK) - -if __name__ == "__main__": - q: Any = Queue() - args = parse_args() - - try: - simulator_bridge: SimulatorBridge - if args.simulator == "metadrive": - simulator_bridge = MetaDriveBridge(args) - elif args.simulator == "carla": - simulator_bridge = CarlaBridge(args) - else: - raise AssertionError("simulator type not supported") - p = simulator_bridge.run(q) - - if args.joystick: - # start input poll for joystick - from tools.sim.lib.manual_ctrl import wheel_poll_thread - - wheel_poll_thread(q) - else: - # start input poll for keyboard - from tools.sim.lib.keyboard_ctrl import keyboard_poll_thread - - keyboard_poll_thread(q) - p.join() - - finally: - # Try cleaning up the wide camera param - # in case users want to use replay after - Params().remove("WideCameraOnly") diff --git a/tools/sim/run_bridge.py b/tools/sim/run_bridge.py new file mode 100755 index 00000000000000..573f64e0c05047 --- /dev/null +++ b/tools/sim/run_bridge.py @@ -0,0 +1,38 @@ +from multiprocessing import Queue +from tools.sim.bridge.carla import CarlaBridge +from tools.sim.bridge.metadrive import MetaDriveBridge +from tools.sim.bridge.common import parse_args, SimulatorBridge +from common.params import Params + +from typing import Any + +if __name__ == "__main__": + q: Any = Queue() + args = parse_args() + + try: + simulator_bridge: SimulatorBridge + if args.simulator == "metadrive": + simulator_bridge = MetaDriveBridge(args) + elif args.simulator == "carla": + simulator_bridge = CarlaBridge(args) + else: + raise AssertionError("simulator type not supported") + p = simulator_bridge.run(q) + + if args.joystick: + # start input poll for joystick + from tools.sim.lib.manual_ctrl import wheel_poll_thread + + wheel_poll_thread(q) + else: + # start input poll for keyboard + from tools.sim.lib.keyboard_ctrl import keyboard_poll_thread + + keyboard_poll_thread(q) + p.join() + + finally: + # Try cleaning up the wide camera param + # in case users want to use replay after + Params().remove("WideCameraOnly") From 142a07e2ed46317cc9b9e4bd5aa2ad608f61e016 Mon Sep 17 00:00:00 2001 From: jon-chuang Date: Sat, 18 Feb 2023 15:55:31 +0800 Subject: [PATCH 08/28] refactor --- tools/sim/bridge/__init__.py | 0 tools/sim/bridge/carla.py | 115 +++++ tools/sim/bridge/common.py | 488 ++++++++++++++++++ tools/sim/bridge/metadrive.py | 125 +++++ tools/sim/tests/test_metadrive_integration.py | 6 +- 5 files changed, 731 insertions(+), 3 deletions(-) create mode 100644 tools/sim/bridge/__init__.py create mode 100644 tools/sim/bridge/carla.py create mode 100644 tools/sim/bridge/common.py create mode 100644 tools/sim/bridge/metadrive.py diff --git a/tools/sim/bridge/__init__.py b/tools/sim/bridge/__init__.py new file mode 100644 index 00000000000000..e69de29bb2d1d6 diff --git a/tools/sim/bridge/carla.py b/tools/sim/bridge/carla.py new file mode 100644 index 00000000000000..54375129912a43 --- /dev/null +++ b/tools/sim/bridge/carla.py @@ -0,0 +1,115 @@ +import math +from tools.sim.bridge.common import World, SimulatorBridge, Camerad, STEER_RATIO, W, H, gps_callback, imu_callback + +class CarlaWorld(World): + def __init__(self, world, vehicle, camerad): + import carla # pylint: disable=import-error + self.world = world + self.vc = carla.VehicleControl(throttle=0, steer=0, brake=0, reverse=False) + self.vehicle = vehicle + self.max_steer_angle: float = vehicle.get_physics_control().wheels[0].max_steer_angle + self.camerad = camerad + + def apply_controls(self, steer_sim, throttle_out, brake_out, _rk): + self.vc.throttle = throttle_out / 0.6 + self.vc.steer = steer_sim + self.vc.brake = brake_out + self.vehicle.apply_control(self.vc) + + def get_velocity(self): + self.vel = self.vehicle.get_velocity() + return self.vel + + def get_speed(self) -> float: + return math.sqrt(self.vel.x ** 2 + self.vel.y ** 2 + self.vel.z ** 2) # in m/s + + def get_steer_correction(self) -> float: + return self.max_steer_angle * STEER_RATIO * -1 + + def tick(self): + self.world.tick() + +class CarlaBridge(SimulatorBridge): + TICKS_PER_FRAME = 5 + + def spawn_objects(self): + camerad = Camerad() + import carla # pylint: disable=import-error + def connect_carla_client(host: str, port: int): + client = carla.Client(host, port) + client.set_timeout(5) + return client + + client = connect_carla_client(self._args.host, self._args.port) + world = client.load_world(self._args.town) + + settings = world.get_settings() + settings.synchronous_mode = True # Enables synchronous mode + settings.fixed_delta_seconds = 0.05 + world.apply_settings(settings) + + world.set_weather(carla.WeatherParameters.ClearSunset) + + if not self._args.high_quality: + world.unload_map_layer(carla.MapLayer.Foliage) + world.unload_map_layer(carla.MapLayer.Buildings) + world.unload_map_layer(carla.MapLayer.ParkedVehicles) + world.unload_map_layer(carla.MapLayer.Props) + world.unload_map_layer(carla.MapLayer.StreetLights) + world.unload_map_layer(carla.MapLayer.Particles) + + blueprint_library = world.get_blueprint_library() + + world_map = world.get_map() + + vehicle_bp = blueprint_library.filter('vehicle.tesla.*')[1] + vehicle_bp.set_attribute('role_name', 'hero') + spawn_points = world_map.get_spawn_points() + assert len(spawn_points) > self._args.num_selected_spawn_point, f'''No spawn point {self._args.num_selected_spawn_point}, try a value between 0 and + {len(spawn_points)} for this town.''' + spawn_point = spawn_points[self._args.num_selected_spawn_point] + vehicle = world.spawn_actor(vehicle_bp, spawn_point) + self._simulation_objects.append(vehicle) + + # make tires less slippery + # wheel_control = carla.WheelPhysicsControl(tire_friction=5) + physics_control = vehicle.get_physics_control() + physics_control.mass = 2326 + # physics_control.wheels = [wheel_control]*4 + physics_control.torque_curve = [[20.0, 500.0], [5000.0, 500.0]] + physics_control.gear_switch_time = 0.0 + vehicle.apply_physics_control(physics_control) + + transform = carla.Transform(carla.Location(x=0.8, z=1.13)) + + def create_camera(fov, callback): + blueprint = blueprint_library.find('sensor.camera.rgb') + blueprint.set_attribute('image_size_x', str(W)) + blueprint.set_attribute('image_size_y', str(H)) + blueprint.set_attribute('fov', str(fov)) + if not self._args.high_quality: + blueprint.set_attribute('enable_postprocess_effects', 'False') + camera = world.spawn_actor(blueprint, transform, attach_to=vehicle) + camera.listen(callback) + return camera + + if self._args.dual_camera: + road_camera = create_camera(fov=40, callback=camerad.cam_callback_road) + self._simulation_objects.append(road_camera) + + road_wide_camera = create_camera(fov=120, callback=camerad.cam_callback_wide_road) # fov bigger than 120 shows unwanted artifacts + self._simulation_objects.append(road_wide_camera) + + # re-enable IMU + imu_bp = blueprint_library.find('sensor.other.imu') + imu_bp.set_attribute('sensor_tick', '0.01') + imu = world.spawn_actor(imu_bp, transform, attach_to=vehicle) + imu.listen(lambda imu: imu_callback(imu, self._vehicle_state)) + + gps_bp = blueprint_library.find('sensor.other.gnss') + gps = world.spawn_actor(gps_bp, transform, attach_to=vehicle) + gps.listen(lambda gps: gps_callback(gps, self._vehicle_state)) + self.params.put_bool("UbloxAvailable", True) + self._simulation_objects.extend([imu, gps]) + + return CarlaWorld(world, vehicle, camerad) \ No newline at end of file diff --git a/tools/sim/bridge/common.py b/tools/sim/bridge/common.py new file mode 100644 index 00000000000000..6f3a4c546c1d23 --- /dev/null +++ b/tools/sim/bridge/common.py @@ -0,0 +1,488 @@ +#!/usr/bin/env python3 +import argparse +import math +import os +import signal +import threading +import time +from multiprocessing import Process, Queue +from abc import ABC, abstractmethod + +import numpy as np +import pyopencl as cl +import pyopencl.array as cl_array + +import cereal.messaging as messaging +from cereal import log +from cereal.visionipc import VisionIpcServer, VisionStreamType +from common.basedir import BASEDIR +from common.numpy_fast import clip +from common.params import Params +from common.realtime import DT_DMON, Ratekeeper +from selfdrive.car.honda.values import CruiseButtons +from selfdrive.test.helpers import set_params_enabled +from tools.sim.lib.can import can_function + +W, H = 1928, 1208 +REPEAT_COUNTER = 5 +PRINT_DECIMATION = 100 +STEER_RATIO = 15. + +pm = messaging.PubMaster(['roadCameraState', 'wideRoadCameraState', 'accelerometer', 'gyroscope', 'can', "gpsLocationExternal"]) +sm = messaging.SubMaster(['carControl', 'controlsState']) + +def parse_args(add_args=None): + parser = argparse.ArgumentParser(description='Bridge between CARLA and openpilot.') + parser.add_argument('--joystick', action='store_true') + parser.add_argument('--high_quality', action='store_true') + parser.add_argument('--dual_camera', action='store_true') + parser.add_argument('--town', type=str, default='Town04_Opt') + parser.add_argument('--spawn_point', dest='num_selected_spawn_point', type=int, default=16) + parser.add_argument('--host', dest='host', type=str, default='127.0.0.1') + parser.add_argument('--port', dest='port', type=int, default=2000) + parser.add_argument('--simulator', dest='simulator', type=str, default='metadrive') + parser.add_argument('--ticks_per_frame', dest='ticks_per_frame', type=int, default=None) + + return parser.parse_args(add_args) + +class VehicleState: + def __init__(self): + self.speed = 0.0 + self.angle = 0.0 + self.bearing_deg = 0.0 + self.vel = None + self.cruise_button = 0 + self.is_engaged = False + self.ignition = True + +def steer_rate_limit(old, new): + # Rate limiting to 0.5 degrees per step + limit = 0.5 + if new > old + limit: + return old + limit + elif new < old - limit: + return old - limit + else: + return new + + +class Camerad: + def __init__(self): + self.frame_road_id = 0 + self.frame_wide_id = 0 + self.vipc_server = VisionIpcServer("camerad") + + self.vipc_server.create_buffers(VisionStreamType.VISION_STREAM_ROAD, 5, False, W, H) + self.vipc_server.create_buffers(VisionStreamType.VISION_STREAM_WIDE_ROAD, 5, False, W, H) + self.vipc_server.start_listener() + + # set up for pyopencl rgb to yuv conversion + self.ctx = cl.create_some_context() + self.queue = cl.CommandQueue(self.ctx) + cl_arg = f" -DHEIGHT={H} -DWIDTH={W} -DRGB_STRIDE={W * 3} -DUV_WIDTH={W // 2} -DUV_HEIGHT={H // 2} -DRGB_SIZE={W * H} -DCL_DEBUG " + + kernel_fn = os.path.join(BASEDIR, "tools/sim/rgb_to_nv12.cl") + with open(kernel_fn) as f: + prg = cl.Program(self.ctx, f.read()).build(cl_arg) + self.krnl = prg.rgb_to_nv12 + self.Wdiv4 = W // 4 if (W % 4 == 0) else (W + (4 - W % 4)) // 4 + self.Hdiv4 = H // 4 if (H % 4 == 0) else (H + (4 - H % 4)) // 4 + + def cam_callback_road(self, image): + yuv = self.img_to_yuv(image) + self._send_yuv(yuv, self.frame_road_id, 'roadCameraState', VisionStreamType.VISION_STREAM_ROAD) + self.frame_road_id += 1 + + def cam_callback_wide_road(self, image): + yuv = self.img_to_yuv(image) + self._send_yuv(yuv, self.frame_wide_id, 'wideRoadCameraState', VisionStreamType.VISION_STREAM_WIDE_ROAD) + self.frame_wide_id += 1 + + def cam_send_yuv_wide_road(self, yuv): + self._send_yuv(yuv, self.frame_wide_id, 'wideRoadCameraState', VisionStreamType.VISION_STREAM_WIDE_ROAD) + self.frame_wide_id += 1 + + # Returns: yuv bytes + def img_to_yuv(self, img): + assert img.shape == (H, W, 3), f"{img.shape}" + rgb = np.reshape(img, (H, W * 3)) + rgb_cl = cl_array.to_device(self.queue, rgb) + yuv_cl = cl_array.empty_like(rgb_cl) + self.krnl(self.queue, (np.int32(self.Wdiv4), np.int32(self.Hdiv4)), None, rgb_cl.data, yuv_cl.data).wait() + yuv = np.resize(yuv_cl.get(), rgb.size // 2) + return yuv.data.tobytes() + + def _send_yuv(self, yuv, frame_id, pub_type, yuv_type): + eof = int(frame_id * 0.05 * 1e9) + self.vipc_server.send(yuv_type, yuv, frame_id, eof, eof) + + dat = messaging.new_message(pub_type) + msg = { + "frameId": frame_id, + "transform": [1.0, 0.0, 0.0, + 0.0, 1.0, 0.0, + 0.0, 0.0, 1.0] + } + setattr(dat, pub_type, msg) + pm.send(pub_type, dat) + +def imu_callback(imu, vehicle_state): + # send 5x since 'sensor_tick' doesn't seem to work. limited by the world tick? + for _ in range(5): + vehicle_state.bearing_deg = math.degrees(imu.compass) + dat = messaging.new_message('accelerometer') + dat.accelerometer.sensor = 4 + dat.accelerometer.type = 0x10 + dat.accelerometer.timestamp = dat.logMonoTime # TODO: use the IMU timestamp + dat.accelerometer.init('acceleration') + dat.accelerometer.acceleration.v = [imu.accelerometer.x, imu.accelerometer.y, imu.accelerometer.z] + pm.send('accelerometer', dat) + + # copied these numbers from locationd + dat = messaging.new_message('gyroscope') + dat.gyroscope.sensor = 5 + dat.gyroscope.type = 0x10 + dat.gyroscope.timestamp = dat.logMonoTime # TODO: use the IMU timestamp + dat.gyroscope.init('gyroUncalibrated') + dat.gyroscope.gyroUncalibrated.v = [imu.gyroscope.x, imu.gyroscope.y, imu.gyroscope.z] + pm.send('gyroscope', dat) + time.sleep(0.01) + + +def panda_state_function(vs: VehicleState, exit_event: threading.Event): + pm = messaging.PubMaster(['pandaStates']) + while not exit_event.is_set(): + dat = messaging.new_message('pandaStates', 1) + dat.valid = True + dat.pandaStates[0] = { + 'ignitionLine': vs.ignition, + 'pandaType': "blackPanda", + 'controlsAllowed': True, + 'safetyModel': 'hondaNidec', + } + pm.send('pandaStates', dat) + time.sleep(0.5) + + +def peripheral_state_function(exit_event: threading.Event): + pm = messaging.PubMaster(['peripheralState']) + while not exit_event.is_set(): + dat = messaging.new_message('peripheralState') + dat.valid = True + # fake peripheral state data + dat.peripheralState = { + 'pandaType': log.PandaState.PandaType.blackPanda, + 'voltage': 12000, + 'current': 5678, + 'fanSpeedRpm': 1000 + } + Params().put_bool("ObdMultiplexingDisabled", True) + pm.send('peripheralState', dat) + time.sleep(0.5) + + +def gps_callback(gps, vehicle_state): + dat = messaging.new_message('gpsLocationExternal') + + # transform vel from carla to NED + # north is -Y in CARLA + velNED = [ + -vehicle_state.vel.y, # north/south component of NED is negative when moving south + vehicle_state.vel.x, # positive when moving east, which is x in carla + vehicle_state.vel.z, + ] + + dat.gpsLocationExternal = { + "unixTimestampMillis": int(time.time() * 1000), + "flags": 1, # valid fix + "accuracy": 1.0, + "verticalAccuracy": 1.0, + "speedAccuracy": 0.1, + "bearingAccuracyDeg": 0.1, + "vNED": velNED, + "bearingDeg": vehicle_state.bearing_deg, + "latitude": gps.latitude, + "longitude": gps.longitude, + "altitude": gps.altitude, + "speed": vehicle_state.speed, + "source": log.GpsLocationData.SensorSource.ublox, + } + + pm.send('gpsLocationExternal', dat) + + +def fake_driver_monitoring(exit_event: threading.Event): + pm = messaging.PubMaster(['driverStateV2', 'driverMonitoringState']) + while not exit_event.is_set(): + # dmonitoringmodeld output + dat = messaging.new_message('driverStateV2') + dat.driverStateV2.leftDriverData.faceOrientation = [0., 0., 0.] + dat.driverStateV2.leftDriverData.faceProb = 1.0 + dat.driverStateV2.rightDriverData.faceOrientation = [0., 0., 0.] + dat.driverStateV2.rightDriverData.faceProb = 1.0 + pm.send('driverStateV2', dat) + + # dmonitoringd output + dat = messaging.new_message('driverMonitoringState') + dat.driverMonitoringState = { + "faceDetected": True, + "isDistracted": False, + "awarenessStatus": 1., + } + pm.send('driverMonitoringState', dat) + + time.sleep(DT_DMON) + + +def can_function_runner(vs: VehicleState, exit_event: threading.Event): + i = 0 + while not exit_event.is_set(): + can_function(pm, vs.speed, vs.angle, i, vs.cruise_button, vs.is_engaged) + time.sleep(0.01) + i += 1 + +class World(ABC): + @abstractmethod + def apply_controls(self, steer_sim, throttle_out, brake_out, rk): + pass + @abstractmethod + def get_velocity(self): + pass + @abstractmethod + def get_speed(self) -> float: + pass + @abstractmethod + def get_steer_correction(self) -> float: + pass + @abstractmethod + def tick(self): + pass + + +class SimulatorBridge(ABC): + TICKS_PER_FRAME = 5 + def __init__(self, arguments): + if arguments.ticks_per_frame: + self.ticks_per_frame = arguments.ticks_per_frame + else: + self.ticks_per_frame = self.TICKS_PER_FRAME + print(f"Running at {self.ticks_per_frame} tick(s) per frame") + set_params_enabled() + self.params = Params() + + msg = messaging.new_message('liveCalibration') + msg.liveCalibration.validBlocks = 20 + msg.liveCalibration.rpyCalib = [0.0, 0.0, 0.0] + self.params.put("CalibrationParams", msg.to_bytes()) + if not arguments.dual_camera: + print("Dual Camera disabled") + self.params.put_bool("WideCameraOnly", not arguments.dual_camera) + self.params.put_bool("DisengageOnAccelerator", True) + self.params.put_bool("ExperimentalMode", True) + self.params.put_bool("ExperimentalLongitudinalEnabled", True) + + self._args = arguments + self._simulation_objects = [] + self._exit_event = threading.Event() + self._threads = [] + self._keep_alive = True + self.started = False + signal.signal(signal.SIGTERM, self._on_shutdown) + self._exit = threading.Event() + + def _on_shutdown(self, signal, frame): + self._keep_alive = False + + def bridge_keep_alive(self, q: Queue, retries: int): + try: + while self._keep_alive: + try: + self._run(q) + break + except RuntimeError as e: + self.close() + if retries == 0: + raise + + # Reset for another try + self._simulation_objects = [] + self._threads = [] + self._exit_event = threading.Event() + + retries -= 1 + if retries <= -1: + print(f"Restarting bridge. Error: {e} ") + else: + print(f"Restarting bridge. Retries left {retries}. Error: {e} ") + finally: + # Clean up resources in the opposite order they were created. + self.close() + + def _run(self, q: Queue): + vehicle_state = VehicleState() + self._vehicle_state = vehicle_state + world = self.spawn_objects() + + # launch fake car threads + self._threads.append(threading.Thread(target=panda_state_function, args=(vehicle_state, self._exit_event,))) + self._threads.append(threading.Thread(target=peripheral_state_function, args=(self._exit_event,))) + self._threads.append(threading.Thread(target=fake_driver_monitoring, args=(self._exit_event,))) + self._threads.append(threading.Thread(target=can_function_runner, args=(vehicle_state, self._exit_event,))) + for t in self._threads: + t.start() + + # init + throttle_ease_out_counter = REPEAT_COUNTER + brake_ease_out_counter = REPEAT_COUNTER + steer_ease_out_counter = REPEAT_COUNTER + + is_openpilot_engaged = False + throttle_out = steer_out = brake_out = 0. + throttle_op = steer_op = brake_op = 0. + throttle_manual = steer_manual = brake_manual = 0. + + old_steer = old_brake = old_throttle = 0. + throttle_manual_multiplier = 0.7 # keyboard signal is always 1 + brake_manual_multiplier = 0.7 # keyboard signal is always 1 + steer_manual_multiplier = 45 * STEER_RATIO # keyboard signal is always 1 + + # loop + rk = Ratekeeper(100, print_delay_threshold=0.05) + + while self._keep_alive: + # 1. Read the throttle, steer and brake from op or manual controls + # 2. Set instructions in Carla + # 3. Send current carstate to op via can + + cruise_button = 0 + throttle_out = steer_out = brake_out = 0.0 + throttle_op = steer_op = brake_op = 0.0 + throttle_manual = steer_manual = brake_manual = 0.0 + + # --------------Step 1------------------------------- + if not q.empty(): + message = q.get() + m = message.split('_') + if m[0] == "steer": + steer_manual = float(m[1]) + is_openpilot_engaged = False + elif m[0] == "throttle": + throttle_manual = float(m[1]) + is_openpilot_engaged = False + elif m[0] == "brake": + brake_manual = float(m[1]) + is_openpilot_engaged = False + elif m[0] == "reverse": + cruise_button = CruiseButtons.CANCEL + is_openpilot_engaged = False + elif m[0] == "cruise": + if m[1] == "down": + cruise_button = CruiseButtons.DECEL_SET + is_openpilot_engaged = True + elif m[1] == "up": + cruise_button = CruiseButtons.RES_ACCEL + is_openpilot_engaged = True + elif m[1] == "cancel": + cruise_button = CruiseButtons.CANCEL + is_openpilot_engaged = False + elif m[0] == "ignition": + vehicle_state.ignition = not vehicle_state.ignition + elif m[0] == "quit": + break + + throttle_out = throttle_manual * throttle_manual_multiplier + steer_out = steer_manual * steer_manual_multiplier + brake_out = brake_manual * brake_manual_multiplier + + old_steer = steer_out + old_throttle = throttle_out + old_brake = brake_out + + if is_openpilot_engaged: + sm.update(0) + + # TODO gas and brake is deprecated + throttle_op = clip(sm['carControl'].actuators.accel / 1.6, 0.0, 1.0) + brake_op = clip(-sm['carControl'].actuators.accel / 4.0, 0.0, 1.0) + steer_op = sm['carControl'].actuators.steeringAngleDeg + + throttle_out = throttle_op + steer_out = steer_op + brake_out = brake_op + + steer_out = steer_rate_limit(old_steer, steer_out) + old_steer = steer_out + + else: + if throttle_out == 0 and old_throttle > 0: + if throttle_ease_out_counter > 0: + throttle_out = old_throttle + throttle_ease_out_counter += -1 + else: + throttle_ease_out_counter = REPEAT_COUNTER + old_throttle = 0 + + if brake_out == 0 and old_brake > 0: + if brake_ease_out_counter > 0: + brake_out = old_brake + brake_ease_out_counter += -1 + else: + brake_ease_out_counter = REPEAT_COUNTER + old_brake = 0 + + if steer_out == 0 and old_steer != 0: + if steer_ease_out_counter > 0: + steer_out = old_steer + steer_ease_out_counter += -1 + else: + steer_ease_out_counter = REPEAT_COUNTER + old_steer = 0 + + # --------------Step 2------------------------------- + steer_correction = world.get_steer_correction() + steer_sim = steer_out / steer_correction + + steer_sim = np.clip(steer_sim, -1, 1) + steer_out = steer_sim * steer_correction + old_steer = steer_sim * steer_correction + + world.apply_controls(steer_sim, throttle_out, brake_out, rk) + + # --------------Step 3------------------------------- + vehicle_state.speed = world.get_speed() + vehicle_state.vel = world.get_velocity() + vehicle_state.angle = steer_out + vehicle_state.cruise_button = cruise_button + vehicle_state.is_engaged = is_openpilot_engaged + + if rk.frame % PRINT_DECIMATION == 0: + print("frame: ", "engaged:", is_openpilot_engaged, "; throttle: ", round(throttle_out, 3), "; steer(deg): ", + round(steer_out, 3), "; brake: ", round(brake_out, 3)) + + if rk.frame % self.TICKS_PER_FRAME == 0: + world.tick() + rk.keep_time() + self.started = True + + def close(self): + self.started = False + self._exit_event.set() + + for s in self._simulation_objects: + try: + s.destroy() + except Exception as e: + print("Failed to destroy carla object", e) + for t in reversed(self._threads): + t.join() + + def run(self, queue, retries=-1): + bridge_p = Process(target=self.bridge_keep_alive, args=(queue, retries), daemon=True) + bridge_p.start() + return bridge_p + + # Must return object of class `World`, after spawning objects into that world + @abstractmethod + def spawn_objects(self) -> World: + pass + diff --git a/tools/sim/bridge/metadrive.py b/tools/sim/bridge/metadrive.py new file mode 100644 index 00000000000000..ef5dba5eb1e9b3 --- /dev/null +++ b/tools/sim/bridge/metadrive.py @@ -0,0 +1,125 @@ +import warnings +import numpy as np +import metadrive # noqa: F401 pylint: disable=W0611 +import gym + +from tools.sim.bridge.common import World, SimulatorBridge, Camerad, STEER_RATIO, W, H + +class MetaDriveWorld(World): + def __init__(self, env, ticks_per_frame: float): + self.env = env + self.speed = 0.0 + self.yuv = None + self.camerad = Camerad() + self.ticks_per_frame = ticks_per_frame + + def apply_controls(self, steer_sim, throttle_out, brake_out, rk): + vc = [0.0, 0.0] + vc[0] = steer_sim * -1 + if throttle_out: + vc[1] = throttle_out * 10 + else: + vc[1] = -brake_out + if rk.frame % self.ticks_per_frame == 0: + if (rk.frame % self.ticks_per_frame * 2) == 0: + o, _, d, _ = self.env.step(vc) + if d: + print("!!!Episode terminated due to violation of safety!!!") + self.env.reset() + self.env.step([0.0, 0.0]) + for _ in range(300): + rk.keep_time() + + self.speed = o["state"][3] * self.ticks_per_frame * 2 # empirically derived + + img = self.env.vehicle.image_sensors["rgb_wide"].get_pixels_array(self.env.vehicle, False) + self.yuv = self.camerad.img_to_yuv(img) + self.camerad.cam_send_yuv_wide_road(self.yuv) + + def get_velocity(self): + return None + + def get_speed(self) -> float: + return self.speed + + def get_steer_correction(self) -> float: + max_steer_angle = 75 / self.ticks_per_frame + return max_steer_angle * STEER_RATIO * -1 + + def tick(self): + pass + + +class MetaDriveBridge(SimulatorBridge): + TICKS_PER_FRAME = 10 + + def __init__(self, args): + if args.dual_camera: + warnings.warn("Dual camera not supported in MetaDrive simulator for performance reasons") + args.dual_camera = False + if args.ticks_per_frame: + self.TICKS_PER_FRAME = args.ticks_per_frame + super(MetaDriveBridge, self).__init__(args) + + def spawn_objects(self): + env = gym.make('MetaDrive-10env-v0', config=dict(offscreen_render=True)) + + # config = dict( + # # camera_dist=3.0, + # # camera_height=1.0, + # # use_render=True, + # vehicle_config=dict( + # # enable_reverse=True, + # # image_source="rgb_camera", + # # rgb_camera=(0,0) + # ), + # offscreen_render=True, + # ) + # from metadrive.utils.space import VehicleParameterSpace + # from metadrive.component.vehicle.vehicle_type import DefaultVehicle + # max_engine_force = VehicleParameterSpace.DEFAULT_VEHICLE["max_engine_force"] + # max_engine_force._replace(max=max_engine_force.max * 10) + # max_engine_force._replace(min=max_engine_force.min * 10) + + env.reset() + from metadrive.constants import CamMask + from metadrive.component.vehicle_module.base_camera import BaseCamera + from metadrive.engine.engine_utils import engine_initialized + from metadrive.engine.core.image_buffer import ImageBuffer + class RGBCameraWide(BaseCamera): + # shape(dim_1, dim_2) + BUFFER_W = W # dim 1 + BUFFER_H = H # dim 2 + CAM_MASK = CamMask.RgbCam + + def __init__(self): + assert engine_initialized(), "You should initialize engine before adding camera to vehicle" + self.BUFFER_W, self.BUFFER_H = W, H + super(RGBCameraWide, self).__init__() + cam = self.get_cam() + lens = self.get_lens() + cam.lookAt(0, 2.4, 1.3) + cam.setHpr(0, 0.8, 0) + lens.setFov(160) + lens.setAspectRatio(1.15) + + env.vehicle.add_image_sensor("rgb_wide", RGBCameraWide()) + + # Monkey patch `get_rgb_array` since it involves unnecessary copies + def patch_get_rgb_array(self): + if self.engine.episode_step <= 1: + self.engine.graphicsEngine.renderFrame() + origin_img = self.cam.node().getDisplayRegion(0).getScreenshot() + img = np.frombuffer(origin_img.getRamImage().getData(), dtype=np.uint8) + img = img.reshape((origin_img.getYSize(), origin_img.getXSize(), 4)) + img = img[::-1] + img = img[..., :-1] + return img + + setattr(ImageBuffer, "get_rgb_array", patch_get_rgb_array) + + # Simulation tends to be slow in the initial steps. This prevents lagging later + for _ in range(30): + env.step([0.0, 1.0]) + + return MetaDriveWorld(env, self.TICKS_PER_FRAME) \ No newline at end of file diff --git a/tools/sim/tests/test_metadrive_integration.py b/tools/sim/tests/test_metadrive_integration.py index d6ed5a6e1b8a25..84fce2d54e7a3b 100755 --- a/tools/sim/tests/test_metadrive_integration.py +++ b/tools/sim/tests/test_metadrive_integration.py @@ -8,8 +8,8 @@ from cereal import messaging from common.basedir import BASEDIR -from tools.sim import bridge -from tools.sim.bridge import MetaDriveBridge +from tools.sim import run_bridge +from tools.sim.run_bridge import MetaDriveBridge CI = "CI" in os.environ @@ -26,7 +26,7 @@ def test_engage(self): sm = messaging.SubMaster(['controlsState', 'carEvents', 'managerState']) q = Queue() - sim_bridge = MetaDriveBridge(bridge.parse_args(["--frames_per_tick", "25"])) + sim_bridge = MetaDriveBridge(run_bridge.parse_args(["--ticks_per_frame", "10"])) p_bridge = sim_bridge.run(q, retries=10) self.processes.append(p_bridge) From 1d07e5d83c7b85d60be33b327d6b6b5ebf92dc96 Mon Sep 17 00:00:00 2001 From: jon-chuang Date: Sat, 18 Feb 2023 16:20:43 +0800 Subject: [PATCH 09/28] minor --- tools/sim/tests/test_carla_integration.py | 6 +++--- tools/sim/tests/test_metadrive_integration.py | 6 +++--- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/tools/sim/tests/test_carla_integration.py b/tools/sim/tests/test_carla_integration.py index f4673919674d29..79072da287b7a8 100755 --- a/tools/sim/tests/test_carla_integration.py +++ b/tools/sim/tests/test_carla_integration.py @@ -8,8 +8,8 @@ from cereal import messaging from common.basedir import BASEDIR from selfdrive.manager.helpers import unblock_stdout -from tools.sim import bridge -from tools.sim.bridge import CarlaBridge +from tools.sim.bridge.common import parse_args +from tools.sim.bridge.carla import CarlaBridge CI = "CI" in os.environ @@ -42,7 +42,7 @@ def test_engage(self): sm = messaging.SubMaster(['controlsState', 'carEvents', 'managerState']) q = Queue() - carla_bridge = CarlaBridge(bridge.parse_args([])) + carla_bridge = CarlaBridge(parse_args([])) p_bridge = carla_bridge.run(q, retries=10) self.processes.append(p_bridge) diff --git a/tools/sim/tests/test_metadrive_integration.py b/tools/sim/tests/test_metadrive_integration.py index 84fce2d54e7a3b..fbd9c5e92ccdb4 100755 --- a/tools/sim/tests/test_metadrive_integration.py +++ b/tools/sim/tests/test_metadrive_integration.py @@ -8,8 +8,8 @@ from cereal import messaging from common.basedir import BASEDIR -from tools.sim import run_bridge -from tools.sim.run_bridge import MetaDriveBridge +from tools.sim.bridge.common import parse_args +from tools.sim.bridge.metadrive import MetaDriveBridge CI = "CI" in os.environ @@ -26,7 +26,7 @@ def test_engage(self): sm = messaging.SubMaster(['controlsState', 'carEvents', 'managerState']) q = Queue() - sim_bridge = MetaDriveBridge(run_bridge.parse_args(["--ticks_per_frame", "10"])) + sim_bridge = MetaDriveBridge(parse_args(["--ticks_per_frame", "10"])) p_bridge = sim_bridge.run(q, retries=10) self.processes.append(p_bridge) From 9ce927c9259232c0e4664946fb3b14cde6d873a4 Mon Sep 17 00:00:00 2001 From: jon-chuang Date: Sat, 18 Feb 2023 16:40:50 +0800 Subject: [PATCH 10/28] minor --- tools/sim/{run_bridge.py => bridge.py} | 6 +++--- tools/sim/tests/test_metadrive_integration.py | 2 +- 2 files changed, 4 insertions(+), 4 deletions(-) rename tools/sim/{run_bridge.py => bridge.py} (76%) diff --git a/tools/sim/run_bridge.py b/tools/sim/bridge.py similarity index 76% rename from tools/sim/run_bridge.py rename to tools/sim/bridge.py index 573f64e0c05047..39ed3a12c83809 100755 --- a/tools/sim/run_bridge.py +++ b/tools/sim/bridge.py @@ -1,7 +1,7 @@ from multiprocessing import Queue -from tools.sim.bridge.carla import CarlaBridge -from tools.sim.bridge.metadrive import MetaDriveBridge -from tools.sim.bridge.common import parse_args, SimulatorBridge +from tools.sim.bridge.carla import CarlaBridge # pylint: disable = no-name-in-module +from tools.sim.bridge.metadrive import MetaDriveBridge # pylint: disable = no-name-in-module +from tools.sim.bridge.common import parse_args, SimulatorBridge # pylint: disable = no-name-in-module from common.params import Params from typing import Any diff --git a/tools/sim/tests/test_metadrive_integration.py b/tools/sim/tests/test_metadrive_integration.py index fbd9c5e92ccdb4..be2e62e30f48a6 100755 --- a/tools/sim/tests/test_metadrive_integration.py +++ b/tools/sim/tests/test_metadrive_integration.py @@ -26,7 +26,7 @@ def test_engage(self): sm = messaging.SubMaster(['controlsState', 'carEvents', 'managerState']) q = Queue() - sim_bridge = MetaDriveBridge(parse_args(["--ticks_per_frame", "10"])) + sim_bridge = MetaDriveBridge(parse_args(["--ticks_per_frame", "20"])) p_bridge = sim_bridge.run(q, retries=10) self.processes.append(p_bridge) From 1899b4171bd426e9b96a36f7a23b0b2d912380bf Mon Sep 17 00:00:00 2001 From: jon-chuang Date: Sat, 18 Feb 2023 16:41:14 +0800 Subject: [PATCH 11/28] dependencies --- poetry.lock | 187 ++++++++++++++++++++++++++++++++++++++++++++++++- pyproject.toml | 4 +- 2 files changed, 189 insertions(+), 2 deletions(-) diff --git a/poetry.lock b/poetry.lock index 91e1b5c826506c..96ac8bab38945b 100644 --- a/poetry.lock +++ b/poetry.lock @@ -1208,6 +1208,41 @@ gevent = ["gevent (>=1.4.0)"] setproctitle = ["setproctitle"] tornado = ["tornado (>=0.2)"] +[[package]] +name = "gym" +version = "0.24.0" +description = "Gym: A universal API for reinforcement learning environments" +category = "dev" +optional = false +python-versions = ">=3.6" + +[package.dependencies] +cloudpickle = ">=1.2.0" +gym_notices = ">=0.0.4" +importlib_metadata = {version = ">=4.8.0", markers = "python_version < \"3.10\""} +numpy = ">=1.18.0" + +[package.extras] +accept-rom-license = ["autorom[accept-rom-license] (>=0.4.2,<0.5.0)"] +all = ["ale-py (>=0.7.5,<0.8.0)", "box2d-py (==2.3.5)", "box2d-py (==2.3.5)", "box2d-py (==2.3.5)", "box2d-py (==2.3.5)", "imageio (>=2.14.1)", "imageio (>=2.14.1)", "lz4 (>=3.1.0)", "lz4 (>=3.1.0)", "lz4 (>=3.1.0)", "lz4 (>=3.1.0)", "matplotlib (>=3.0)", "matplotlib (>=3.0)", "matplotlib (>=3.0)", "matplotlib (>=3.0)", "mujoco (==2.2.0)", "mujoco (==2.2.0)", "mujoco_py (>=2.1,<2.2)", "mujoco_py (>=2.1,<2.2)", "opencv-python (>=3.0)", "opencv-python (>=3.0)", "opencv-python (>=3.0)", "opencv-python (>=3.0)", "pygame (==2.1.0)", "pygame (==2.1.0)", "pygame (==2.1.0)", "pygame (==2.1.0)", "pygame (==2.1.0)", "pygame (==2.1.0)", "pygame (==2.1.0)", "pygame (==2.1.0)", "pygame (==2.1.0)", "pygame (==2.1.0)", "pygame (==2.1.0)", "pygame (==2.1.0)", "scipy (>=1.4.1)", "scipy (>=1.4.1)", "scipy (>=1.4.1)", "scipy (>=1.4.1)"] +atari = ["ale-py (>=0.7.5,<0.8.0)"] +box2d = ["box2d-py (==2.3.5)", "pygame (==2.1.0)"] +classic-control = ["pygame (==2.1.0)"] +mujoco = ["imageio (>=2.14.1)", "mujoco (==2.2.0)"] +mujoco-py = ["mujoco_py (>=2.1,<2.2)"] +noatari = ["box2d-py (==2.3.5)", "box2d-py (==2.3.5)", "imageio (>=2.14.1)", "lz4 (>=3.1.0)", "lz4 (>=3.1.0)", "matplotlib (>=3.0)", "matplotlib (>=3.0)", "mujoco (==2.2.0)", "mujoco_py (>=2.1,<2.2)", "opencv-python (>=3.0)", "opencv-python (>=3.0)", "pygame (==2.1.0)", "pygame (==2.1.0)", "pygame (==2.1.0)", "pygame (==2.1.0)", "pygame (==2.1.0)", "pygame (==2.1.0)", "scipy (>=1.4.1)", "scipy (>=1.4.1)"] +nomujoco = ["box2d-py (==2.3.5)", "lz4 (>=3.1.0)", "matplotlib (>=3.0)", "opencv-python (>=3.0)", "pygame (==2.1.0)", "pygame (==2.1.0)", "pygame (==2.1.0)", "scipy (>=1.4.1)"] +other = ["lz4 (>=3.1.0)", "matplotlib (>=3.0)", "opencv-python (>=3.0)"] +toy-text = ["pygame (==2.1.0)", "scipy (>=1.4.1)"] + +[[package]] +name = "gym-notices" +version = "0.0.8" +description = "Notices for gym" +category = "dev" +optional = false +python-versions = "*" + [[package]] name = "h3" version = "3.7.4" @@ -2053,6 +2088,41 @@ category = "dev" optional = false python-versions = ">=3.7" +[[package]] +name = "metadrive-simulator" +version = "0.2.6.0" +description = "" +category = "dev" +optional = false +python-versions = ">=3.7, <3.10" +develop = false + +[package.dependencies] +gym = ">=0.20.0,<=0.24.0" +lxml = "*" +matplotlib = "*" +numpy = ">=1.21.6,<=1.24.2" +opencv-python = "*" +panda3d = ">=1.10.8,<=1.10.13" +panda3d-gltf = "*" +panda3d-simplepbr = "*" +pandas = "*" +pillow = "*" +protobuf = "3.20.1" +psutil = "*" +pygame = "*" +pytest = "*" +scipy = "*" +seaborn = "*" +tqdm = "*" +yapf = "*" + +[package.source] +type = "git" +url = "https://github.com/jon-chuang/metadrive.git" +reference = "jon/versioning-hack" +resolved_reference = "2e6f00b53fab2e2d4806c1cbd0ba2b0610f19a04" + [[package]] name = "mistune" version = "2.0.4" @@ -2568,6 +2638,22 @@ packaging = "*" protobuf = "*" sympy = "*" +[[package]] +name = "opencv-python" +version = "4.7.0.68" +description = "Wrapper package for OpenCV python bindings." +category = "dev" +optional = false +python-versions = ">=3.6" + +[package.dependencies] +numpy = [ + {version = ">=1.21.0", markers = "python_version <= \"3.9\" and platform_system == \"Darwin\" and platform_machine == \"arm64\""}, + {version = ">=1.17.0", markers = "python_version >= \"3.7\""}, + {version = ">=1.17.3", markers = "python_version >= \"3.8\""}, + {version = ">=1.19.3", markers = "python_version >= \"3.6\" and platform_system == \"Linux\" and platform_machine == \"aarch64\""}, +] + [[package]] name = "opencv-python-headless" version = "4.5.5.64" @@ -2635,6 +2721,40 @@ python-versions = ">=3.6" [package.dependencies] pyparsing = ">=2.0.2,<3.0.5 || >3.0.5" +[[package]] +name = "panda3d" +version = "1.10.13.post1" +description = "Panda3D is a framework for 3D rendering and game development for Python and C++ programs." +category = "dev" +optional = false +python-versions = "*" + +[[package]] +name = "panda3d-gltf" +version = "0.13" +description = "glTF utilities for Panda3D" +category = "dev" +optional = false +python-versions = ">=3.6" + +[package.dependencies] +panda3d = ">=1.10.8" +panda3d-simplepbr = ">=0.6" + +[[package]] +name = "panda3d-simplepbr" +version = "0.10" +description = "A simple, lightweight PBR render pipeline for Panda3D" +category = "dev" +optional = false +python-versions = ">=3.6" + +[package.dependencies] +panda3d = ">=1.10.8" + +[package.extras] +test = ["pylint (>=2.4.0,<2.5.0)", "pytest", "pytest-pylint"] + [[package]] name = "pandas" version = "1.5.1" @@ -3662,6 +3782,22 @@ python-versions = ">=3.6" [package.dependencies] setuptools = "*" +[[package]] +name = "seaborn" +version = "0.12.0" +description = "Statistical data visualization" +category = "dev" +optional = false +python-versions = ">=3.7" + +[package.dependencies] +matplotlib = ">=3.1" +numpy = ">=1.17" +pandas = ">=0.25" + +[package.extras] +stats = ["scipy (>=1.3)", "statsmodels (>=0.10)"] + [[package]] name = "secretstorage" version = "3.3.3" @@ -4487,6 +4623,14 @@ python-versions = "*" [package.dependencies] cffi = ">=1.0" +[[package]] +name = "yapf" +version = "0.32.0" +description = "A formatter for Python code." +category = "dev" +optional = false +python-versions = "*" + [[package]] name = "yarl" version = "1.8.1" @@ -4567,7 +4711,7 @@ testing = ["coverage (>=5.0.3)", "zope.event", "zope.testing"] [metadata] lock-version = "1.1" python-versions = "~3.8" -content-hash = "9e9495c896e6fd0855803aeaf46513c6c22424b86be820759a8baf27d44e73ee" +content-hash = "649cfa913690f1c1518274d25b25f479afe260aa2d0a7b1998ff43eee360d11c" [metadata.files] adal = [ @@ -5712,6 +5856,13 @@ gunicorn = [ {file = "gunicorn-20.1.0-py3-none-any.whl", hash = "sha256:9dcc4547dbb1cb284accfb15ab5667a0e5d1881cc443e0677b4882a4067a807e"}, {file = "gunicorn-20.1.0.tar.gz", hash = "sha256:e0a968b5ba15f8a328fdfd7ab1fcb5af4470c28aaf7e55df02a99bc13138e6e8"}, ] +gym = [ + {file = "gym-0.24.0.tar.gz", hash = "sha256:69f96424be40d23088be978b61f45c23911e2ccddafad08cf2fac608e8bd86e4"}, +] +gym-notices = [ + {file = "gym-notices-0.0.8.tar.gz", hash = "sha256:ad25e200487cafa369728625fe064e88ada1346618526102659b4640f2b4b911"}, + {file = "gym_notices-0.0.8-py3-none-any.whl", hash = "sha256:e5f82e00823a166747b4c2a07de63b6560b1acb880638547e0cabf825a01e463"}, +] h3 = [ {file = "h3-3.7.4-cp27-cp27m-macosx_10_9_x86_64.whl", hash = "sha256:e61d3c6b1b66072f5b74d46dbee7df29daac6ce9738b9a6223a67dc577114515"}, {file = "h3-3.7.4-cp27-cp27m-manylinux2010_x86_64.whl", hash = "sha256:960cd005b8817314d95fbaff3e848a72385df4e3c6c9703ff99b08581c8def69"}, @@ -6276,6 +6427,7 @@ mdurl = [ {file = "mdurl-0.1.2-py3-none-any.whl", hash = "sha256:84008a41e51615a49fc9966191ff91509e3c40b939176e643fd50a5c2196b8f8"}, {file = "mdurl-0.1.2.tar.gz", hash = "sha256:bb413d29f5eea38f31dd4754dd7377d4465116fb207585f97bf925588687c1ba"}, ] +metadrive-simulator = [] mistune = [ {file = "mistune-2.0.4-py2.py3-none-any.whl", hash = "sha256:182cc5ee6f8ed1b807de6b7bb50155df7b66495412836b9a74c8fbdfc75fe36d"}, {file = "mistune-2.0.4.tar.gz", hash = "sha256:9ee0a66053e2267aba772c71e06891fa8f1af6d4b01d5e84e267b4570d4d9808"}, @@ -6640,6 +6792,15 @@ onnxruntime-gpu = [ {file = "onnxruntime_gpu-1.12.1-cp39-cp39-manylinux_2_17_x86_64.manylinux2014_x86_64.whl", hash = "sha256:8e46d0724ce54c5908c5760037b78de741fbd48962b370c29ebc20e608b30eda"}, {file = "onnxruntime_gpu-1.12.1-cp39-cp39-win_amd64.whl", hash = "sha256:fd919373be35b9ba54210688265df38ad5e19a530449385c40dab51da407345d"}, ] +opencv-python = [ + {file = "opencv-python-4.7.0.68.tar.gz", hash = "sha256:9829e6efedde1d1b8419c5bd4d62d289ecbf44ae35b843c6da9e3cbcba1a9a8a"}, + {file = "opencv_python-4.7.0.68-cp37-abi3-macosx_10_13_x86_64.whl", hash = "sha256:abc6adfa8694f71a4caffa922b279bd9d96954a37eee40b147f613c64310b411"}, + {file = "opencv_python-4.7.0.68-cp37-abi3-macosx_11_0_arm64.whl", hash = "sha256:86f4b60b9536948f16d2170ba3a9b22d3955a957dc61a9bc56e53692c6db2c7e"}, + {file = "opencv_python-4.7.0.68-cp37-abi3-manylinux_2_17_aarch64.manylinux2014_aarch64.whl", hash = "sha256:6d1c993811f92ddd7919314ada7b9be1f23db1c73f1384915c834dee8549c0b9"}, + {file = "opencv_python-4.7.0.68-cp37-abi3-manylinux_2_17_x86_64.manylinux2014_x86_64.whl", hash = "sha256:3a00e12546e5578f6bb7ed408c37fcfea533d74e9691cfaf40926f6b43295577"}, + {file = "opencv_python-4.7.0.68-cp37-abi3-win32.whl", hash = "sha256:e770e9f653a0e5e72b973adb8213fae2df4642730ba1faf31e73a54287a4d5d4"}, + {file = "opencv_python-4.7.0.68-cp37-abi3-win_amd64.whl", hash = "sha256:7a08f9d1f9dd52de63a7bb448ab7d6d4a1a85b767c2358501d968d1e4d95098d"}, +] opencv-python-headless = [] osmium = [ {file = "osmium-3.4.1-cp310-cp310-macosx_10_9_x86_64.whl", hash = "sha256:7bf648744dadd3396040ed57332780272356fb4b87dbd17521ba9b8b91c6e665"}, @@ -6667,6 +6828,22 @@ packaging = [ {file = "packaging-21.3-py3-none-any.whl", hash = "sha256:ef103e05f519cdc783ae24ea4e2e0f508a9c99b2d4969652eed6a2e1ea5bd522"}, {file = "packaging-21.3.tar.gz", hash = "sha256:dd47c42927d89ab911e606518907cc2d3a1f38bbd026385970643f9c5b8ecfeb"}, ] +panda3d = [ + {file = "panda3d-1.10.13.post1-cp311-cp311-macosx_10_9_x86_64.whl", hash = "sha256:447ae57766a44d43cd95edebf5ac54a84ae6f17bec6793346113ecdc5801f292"}, + {file = "panda3d-1.10.13.post1-cp311-cp311-macosx_11_0_universal2.whl", hash = "sha256:cb1417a6604eda53bd3b22ea56e11ae83737522b12c56faa83542d593d1e4513"}, + {file = "panda3d-1.10.13.post1-cp311-cp311-manylinux2014_aarch64.whl", hash = "sha256:d049fd28bd858c9bd146e1ee91ba3d9cb61341577d4bf73f79ea46b18fa2236c"}, + {file = "panda3d-1.10.13.post1-cp311-cp311-manylinux2014_x86_64.whl", hash = "sha256:0564592ffe62a37e953b9e07c35ab8b3724dfaa340d0ed2a06c64a7324d854bb"}, + {file = "panda3d-1.10.13.post1-cp311-cp311-win32.whl", hash = "sha256:e9d335c53241d65c6ab7af11520dca337e809d743f05b9325186b6677cffa507"}, + {file = "panda3d-1.10.13.post1-cp311-cp311-win_amd64.whl", hash = "sha256:4ffb25f61e76046f1c0029bc8b74f7a41a0cba01f5be8672c8ba4371294936f1"}, +] +panda3d-gltf = [ + {file = "panda3d-gltf-0.13.tar.gz", hash = "sha256:d06d373bdd91cf530909b669f43080e599463bbf6d3ef00c3558bad6c6b19675"}, + {file = "panda3d_gltf-0.13-py3-none-any.whl", hash = "sha256:02d1a980f447bb1895ff4b48c667f289ba78f07a28ef308d8839b665a621efe2"}, +] +panda3d-simplepbr = [ + {file = "panda3d-simplepbr-0.10.tar.gz", hash = "sha256:b6bd902671b02c8794eb60d8039f256a4115faef067c1368cc70147f8ed83a31"}, + {file = "panda3d_simplepbr-0.10-py3-none-any.whl", hash = "sha256:c526a0fa5b2af8a2f16ee2837dcedbf5987ca917ee95699830675204bf765fb4"}, +] pandas = [ {file = "pandas-1.5.1-cp310-cp310-macosx_10_9_universal2.whl", hash = "sha256:0a78e05ec09731c5b3bd7a9805927ea631fe6f6cb06f0e7c63191a9a778d52b4"}, {file = "pandas-1.5.1-cp310-cp310-macosx_10_9_x86_64.whl", hash = "sha256:5b0c970e2215572197b42f1cff58a908d734503ea54b326412c70d4692256391"}, @@ -7630,6 +7807,10 @@ scons = [ {file = "SCons-4.4.0-py3-none-any.whl", hash = "sha256:cbbd73b83cf85f1aaaf986370359de1bbfd3af97a765cb3554734f6dcec734e1"}, {file = "SCons-4.4.0.tar.gz", hash = "sha256:7703c4e9d2200b4854a31800c1dbd4587e1fa86e75f58795c740bcfa7eca7eaa"}, ] +seaborn = [ + {file = "seaborn-0.12.0-py3-none-any.whl", hash = "sha256:cbeff3deef7c2515aa0af99b2c7e02dc5bf8b42c936a74d8e4b416905b549db0"}, + {file = "seaborn-0.12.0.tar.gz", hash = "sha256:893f17292d8baca616c1578ddb58eb25c72d622f54fc5ee329c8207dc9b57b23"}, +] secretstorage = [ {file = "SecretStorage-3.3.3-py3-none-any.whl", hash = "sha256:f356e6628222568e3af06f2eba8df495efa13b3b63081dafd4f7d9a7b7bc9f99"}, {file = "SecretStorage-3.3.3.tar.gz", hash = "sha256:2403533ef369eca6d2ba81718576c5e0f564d5cca1b58f73a8b23e7d4eeebd77"}, @@ -8232,6 +8413,10 @@ xattr = [ {file = "xattr-0.9.9-pp37-pypy37_pp73-manylinux_2_5_x86_64.manylinux1_x86_64.manylinux_2_17_x86_64.manylinux2014_x86_64.whl", hash = "sha256:baeff3e5dda8ea7e9424cfaee51829f46afe3836c30d02f343f9049c685681ca"}, {file = "xattr-0.9.9.tar.gz", hash = "sha256:09cb7e1efb3aa1b4991d6be4eb25b73dc518b4fe894f0915f5b0dcede972f346"}, ] +yapf = [ + {file = "yapf-0.32.0-py2.py3-none-any.whl", hash = "sha256:8fea849025584e486fd06d6ba2bed717f396080fd3cc236ba10cb97c4c51cf32"}, + {file = "yapf-0.32.0.tar.gz", hash = "sha256:a3f5085d37ef7e3e004c4ba9f9b3e40c54ff1901cd111f05145ae313a7c67d1b"}, +] yarl = [ {file = "yarl-1.8.1-cp310-cp310-macosx_10_9_universal2.whl", hash = "sha256:abc06b97407868ef38f3d172762f4069323de52f2b70d133d096a48d72215d28"}, {file = "yarl-1.8.1-cp310-cp310-macosx_10_9_x86_64.whl", hash = "sha256:07b21e274de4c637f3e3b7104694e53260b5fc10d51fb3ec5fed1da8e0f754e3"}, diff --git a/pyproject.toml b/pyproject.toml index 9aed704e8bdee4..95740332db1de8 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -59,7 +59,6 @@ urllib3 = "^1.26.10" utm = "^0.7.0" websocket_client = "^1.3.3" polyline = "^1.4.0" -metadrive-simulator = "^0.2.6.0" [tool.poetry.group.dev.dependencies] @@ -174,6 +173,9 @@ Werkzeug = "^2.1.2" zerorpc = { git = "https://github.com/commaai/zerorpc-python.git", branch = "master" } omegaconf = "^2.3.0" osmnx = "==1.2.2" +panda3d = ">1.10.8, <=1.10.13" +gym = ">=0.20.0, <=0.24.0" +metadrive-simulator = { git = "https://github.com/jon-chuang/metadrive.git", branch = "jon/versioning-hack" } [build-system] requires = ["poetry-core"] From eaff9b36f03aefc8a765c2158d16e199d409f2ba Mon Sep 17 00:00:00 2001 From: jon-chuang Date: Sat, 18 Feb 2023 17:10:33 +0800 Subject: [PATCH 12/28] default simlator is carla --- tools/sim/bridge/common.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/tools/sim/bridge/common.py b/tools/sim/bridge/common.py index 6f3a4c546c1d23..17177337e3ba22 100644 --- a/tools/sim/bridge/common.py +++ b/tools/sim/bridge/common.py @@ -40,7 +40,7 @@ def parse_args(add_args=None): parser.add_argument('--spawn_point', dest='num_selected_spawn_point', type=int, default=16) parser.add_argument('--host', dest='host', type=str, default='127.0.0.1') parser.add_argument('--port', dest='port', type=int, default=2000) - parser.add_argument('--simulator', dest='simulator', type=str, default='metadrive') + parser.add_argument('--simulator', dest='simulator', type=str, default='carla') parser.add_argument('--ticks_per_frame', dest='ticks_per_frame', type=int, default=None) return parser.parse_args(add_args) @@ -289,7 +289,7 @@ def __init__(self, arguments): self.started = False signal.signal(signal.SIGTERM, self._on_shutdown) self._exit = threading.Event() - + def _on_shutdown(self, signal, frame): self._keep_alive = False @@ -317,7 +317,7 @@ def bridge_keep_alive(self, q: Queue, retries: int): finally: # Clean up resources in the opposite order they were created. self.close() - + def _run(self, q: Queue): vehicle_state = VehicleState() self._vehicle_state = vehicle_state From 1d7de6fb0bf997dd486831d4f056fb2225653805 Mon Sep 17 00:00:00 2001 From: jon-chuang Date: Sat, 18 Feb 2023 18:19:39 +0800 Subject: [PATCH 13/28] minor --- tools/sim/bridge/metadrive.py | 5 +++-- tools/sim/tests/test_metadrive_integration.py | 11 ++++++----- 2 files changed, 9 insertions(+), 7 deletions(-) diff --git a/tools/sim/bridge/metadrive.py b/tools/sim/bridge/metadrive.py index ef5dba5eb1e9b3..aacb86495ae7b2 100644 --- a/tools/sim/bridge/metadrive.py +++ b/tools/sim/bridge/metadrive.py @@ -1,5 +1,6 @@ import warnings import numpy as np +import math import metadrive # noqa: F401 pylint: disable=W0611 import gym @@ -21,7 +22,7 @@ def apply_controls(self, steer_sim, throttle_out, brake_out, rk): else: vc[1] = -brake_out if rk.frame % self.ticks_per_frame == 0: - if (rk.frame % self.ticks_per_frame * 2) == 0: + if (rk.frame % self.ticks_per_frame * 3) == 0: o, _, d, _ = self.env.step(vc) if d: print("!!!Episode terminated due to violation of safety!!!") @@ -30,7 +31,7 @@ def apply_controls(self, steer_sim, throttle_out, brake_out, rk): for _ in range(300): rk.keep_time() - self.speed = o["state"][3] * self.ticks_per_frame * 2 # empirically derived + self.speed = o["state"][3] * math.sqrt(self.ticks_per_frame) * 10 # empirically derived img = self.env.vehicle.image_sensors["rgb_wide"].get_pixels_array(self.env.vehicle, False) self.yuv = self.camerad.img_to_yuv(img) diff --git a/tools/sim/tests/test_metadrive_integration.py b/tools/sim/tests/test_metadrive_integration.py index be2e62e30f48a6..40002cff13dfaa 100755 --- a/tools/sim/tests/test_metadrive_integration.py +++ b/tools/sim/tests/test_metadrive_integration.py @@ -20,15 +20,16 @@ class TestMetaDriveIntegration(unittest.TestCase): 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) - self.processes.append(p_manager) - time.sleep(10.0) - - sm = messaging.SubMaster(['controlsState', 'carEvents', 'managerState']) q = Queue() sim_bridge = MetaDriveBridge(parse_args(["--ticks_per_frame", "20"])) p_bridge = sim_bridge.run(q, retries=10) self.processes.append(p_bridge) + time.sleep(10.0) + + p_manager = subprocess.Popen("./launch_openpilot.sh", cwd=SIM_DIR) + self.processes.append(p_manager) + + sm = messaging.SubMaster(['controlsState', 'carEvents', 'managerState']) max_time_per_step = 60 From 73c08b4a8ff01c361f23e99586756fcac0f106a9 Mon Sep 17 00:00:00 2001 From: jon-chuang Date: Sat, 18 Feb 2023 18:38:10 +0800 Subject: [PATCH 14/28] rename-for-pr --- tools/sim/{bridge.py => run_bridge.py} | 0 1 file changed, 0 insertions(+), 0 deletions(-) rename tools/sim/{bridge.py => run_bridge.py} (100%) diff --git a/tools/sim/bridge.py b/tools/sim/run_bridge.py similarity index 100% rename from tools/sim/bridge.py rename to tools/sim/run_bridge.py From f8f9f213a137b286ed87aaaabdcf98b573723292 Mon Sep 17 00:00:00 2001 From: jon-chuang Date: Sun, 19 Feb 2023 18:35:10 +0800 Subject: [PATCH 15/28] minor --- poetry.lock | 24 +++++++++++------------- 1 file changed, 11 insertions(+), 13 deletions(-) diff --git a/poetry.lock b/poetry.lock index 96ac8bab38945b..8a0daf9a2fbbf8 100644 --- a/poetry.lock +++ b/poetry.lock @@ -1210,29 +1210,27 @@ tornado = ["tornado (>=0.2)"] [[package]] name = "gym" -version = "0.24.0" -description = "Gym: A universal API for reinforcement learning environments" +version = "0.22.0" +description = "Gym: A universal API for reinforcement learning environments." category = "dev" optional = false -python-versions = ">=3.6" +python-versions = ">=3.7" [package.dependencies] cloudpickle = ">=1.2.0" gym_notices = ">=0.0.4" -importlib_metadata = {version = ">=4.8.0", markers = "python_version < \"3.10\""} +importlib_metadata = {version = ">=4.10.0", markers = "python_version < \"3.10\""} numpy = ">=1.18.0" [package.extras] accept-rom-license = ["autorom[accept-rom-license] (>=0.4.2,<0.5.0)"] -all = ["ale-py (>=0.7.5,<0.8.0)", "box2d-py (==2.3.5)", "box2d-py (==2.3.5)", "box2d-py (==2.3.5)", "box2d-py (==2.3.5)", "imageio (>=2.14.1)", "imageio (>=2.14.1)", "lz4 (>=3.1.0)", "lz4 (>=3.1.0)", "lz4 (>=3.1.0)", "lz4 (>=3.1.0)", "matplotlib (>=3.0)", "matplotlib (>=3.0)", "matplotlib (>=3.0)", "matplotlib (>=3.0)", "mujoco (==2.2.0)", "mujoco (==2.2.0)", "mujoco_py (>=2.1,<2.2)", "mujoco_py (>=2.1,<2.2)", "opencv-python (>=3.0)", "opencv-python (>=3.0)", "opencv-python (>=3.0)", "opencv-python (>=3.0)", "pygame (==2.1.0)", "pygame (==2.1.0)", "pygame (==2.1.0)", "pygame (==2.1.0)", "pygame (==2.1.0)", "pygame (==2.1.0)", "pygame (==2.1.0)", "pygame (==2.1.0)", "pygame (==2.1.0)", "pygame (==2.1.0)", "pygame (==2.1.0)", "pygame (==2.1.0)", "scipy (>=1.4.1)", "scipy (>=1.4.1)", "scipy (>=1.4.1)", "scipy (>=1.4.1)"] -atari = ["ale-py (>=0.7.5,<0.8.0)"] +all = ["ale-py (>=0.7.4,<0.8.0)", "box2d-py (==2.3.5)", "box2d-py (==2.3.5)", "lz4 (>=3.1.0)", "lz4 (>=3.1.0)", "mujoco_py (>=1.50,<2.0)", "opencv-python (>=3.0)", "opencv-python (>=3.0)", "pygame (==2.1.0)", "pygame (==2.1.0)", "pygame (==2.1.0)", "pygame (==2.1.0)", "pygame (==2.1.0)", "pygame (==2.1.0)", "scipy (>=1.4.1)", "scipy (>=1.4.1)"] +atari = ["ale-py (>=0.7.4,<0.8.0)"] box2d = ["box2d-py (==2.3.5)", "pygame (==2.1.0)"] classic-control = ["pygame (==2.1.0)"] -mujoco = ["imageio (>=2.14.1)", "mujoco (==2.2.0)"] -mujoco-py = ["mujoco_py (>=2.1,<2.2)"] -noatari = ["box2d-py (==2.3.5)", "box2d-py (==2.3.5)", "imageio (>=2.14.1)", "lz4 (>=3.1.0)", "lz4 (>=3.1.0)", "matplotlib (>=3.0)", "matplotlib (>=3.0)", "mujoco (==2.2.0)", "mujoco_py (>=2.1,<2.2)", "opencv-python (>=3.0)", "opencv-python (>=3.0)", "pygame (==2.1.0)", "pygame (==2.1.0)", "pygame (==2.1.0)", "pygame (==2.1.0)", "pygame (==2.1.0)", "pygame (==2.1.0)", "scipy (>=1.4.1)", "scipy (>=1.4.1)"] -nomujoco = ["box2d-py (==2.3.5)", "lz4 (>=3.1.0)", "matplotlib (>=3.0)", "opencv-python (>=3.0)", "pygame (==2.1.0)", "pygame (==2.1.0)", "pygame (==2.1.0)", "scipy (>=1.4.1)"] -other = ["lz4 (>=3.1.0)", "matplotlib (>=3.0)", "opencv-python (>=3.0)"] +mujoco = ["mujoco_py (>=1.50,<2.0)"] +nomujoco = ["box2d-py (==2.3.5)", "lz4 (>=3.1.0)", "opencv-python (>=3.0)", "pygame (==2.1.0)", "pygame (==2.1.0)", "pygame (==2.1.0)", "scipy (>=1.4.1)"] +other = ["lz4 (>=3.1.0)", "opencv-python (>=3.0)"] toy-text = ["pygame (==2.1.0)", "scipy (>=1.4.1)"] [[package]] @@ -4711,7 +4709,7 @@ testing = ["coverage (>=5.0.3)", "zope.event", "zope.testing"] [metadata] lock-version = "1.1" python-versions = "~3.8" -content-hash = "649cfa913690f1c1518274d25b25f479afe260aa2d0a7b1998ff43eee360d11c" +content-hash = "f9b173ceb766b5e4a53b2075d5a3ee303918f57c77c68b040c9cd68cd16d285d" [metadata.files] adal = [ @@ -5857,7 +5855,7 @@ gunicorn = [ {file = "gunicorn-20.1.0.tar.gz", hash = "sha256:e0a968b5ba15f8a328fdfd7ab1fcb5af4470c28aaf7e55df02a99bc13138e6e8"}, ] gym = [ - {file = "gym-0.24.0.tar.gz", hash = "sha256:69f96424be40d23088be978b61f45c23911e2ccddafad08cf2fac608e8bd86e4"}, + {file = "gym-0.22.0.tar.gz", hash = "sha256:339144c89951758187c378111919bc0e2f1695f9e9d9699e3f19279a6398148d"}, ] gym-notices = [ {file = "gym-notices-0.0.8.tar.gz", hash = "sha256:ad25e200487cafa369728625fe064e88ada1346618526102659b4640f2b4b911"}, From 36370c7290d7ad2586bd955dc4f696a6959dc53a Mon Sep 17 00:00:00 2001 From: jon-chuang Date: Mon, 20 Feb 2023 12:43:06 +0800 Subject: [PATCH 16/28] minor --- tools/cabana/dbcmanager.h | 1 + tools/sim/bridge/common.py | 47 +++++++++++++++++------------------ tools/sim/bridge/metadrive.py | 44 +++++++++++++++++--------------- tools/sim/bridge/utils.py | 14 +++++++++++ 4 files changed, 62 insertions(+), 44 deletions(-) create mode 100644 tools/sim/bridge/utils.py diff --git a/tools/cabana/dbcmanager.h b/tools/cabana/dbcmanager.h index b438b3e1c23629..6fd3f050fab1a6 100644 --- a/tools/cabana/dbcmanager.h +++ b/tools/cabana/dbcmanager.h @@ -3,6 +3,7 @@ #include #include #include +#include struct MessageId { uint8_t source; diff --git a/tools/sim/bridge/common.py b/tools/sim/bridge/common.py index 17177337e3ba22..73dcdcd100bd76 100644 --- a/tools/sim/bridge/common.py +++ b/tools/sim/bridge/common.py @@ -277,7 +277,7 @@ def __init__(self, arguments): if not arguments.dual_camera: print("Dual Camera disabled") self.params.put_bool("WideCameraOnly", not arguments.dual_camera) - self.params.put_bool("DisengageOnAccelerator", True) + # self.params.put_bool("DisengageOnAccelerator", True) self.params.put_bool("ExperimentalMode", True) self.params.put_bool("ExperimentalLongitudinalEnabled", True) @@ -318,6 +318,28 @@ def bridge_keep_alive(self, q: Queue, retries: int): # Clean up resources in the opposite order they were created. self.close() + def close(self): + self.started = False + self._exit_event.set() + + for s in self._simulation_objects: + try: + s.destroy() + except Exception as e: + print("Failed to destroy carla object", e) + for t in reversed(self._threads): + t.join() + + def run(self, queue, retries=-1): + bridge_p = Process(target=self.bridge_keep_alive, args=(queue, retries), daemon=True) + bridge_p.start() + return bridge_p + + # Must return object of class `World`, after spawning objects into that world + @abstractmethod + def spawn_objects(self) -> World: + pass + def _run(self, q: Queue): vehicle_state = VehicleState() self._vehicle_state = vehicle_state @@ -463,26 +485,3 @@ def _run(self, q: Queue): world.tick() rk.keep_time() self.started = True - - def close(self): - self.started = False - self._exit_event.set() - - for s in self._simulation_objects: - try: - s.destroy() - except Exception as e: - print("Failed to destroy carla object", e) - for t in reversed(self._threads): - t.join() - - def run(self, queue, retries=-1): - bridge_p = Process(target=self.bridge_keep_alive, args=(queue, retries), daemon=True) - bridge_p.start() - return bridge_p - - # Must return object of class `World`, after spawning objects into that world - @abstractmethod - def spawn_objects(self) -> World: - pass - diff --git a/tools/sim/bridge/metadrive.py b/tools/sim/bridge/metadrive.py index aacb86495ae7b2..7390e9046a38a0 100644 --- a/tools/sim/bridge/metadrive.py +++ b/tools/sim/bridge/metadrive.py @@ -1,10 +1,9 @@ import warnings import numpy as np import math -import metadrive # noqa: F401 pylint: disable=W0611 -import gym from tools.sim.bridge.common import World, SimulatorBridge, Camerad, STEER_RATIO, W, H +from tools.sim.bridge.utils import timer class MetaDriveWorld(World): def __init__(self, env, ticks_per_frame: float): @@ -22,20 +21,21 @@ def apply_controls(self, steer_sim, throttle_out, brake_out, rk): else: vc[1] = -brake_out if rk.frame % self.ticks_per_frame == 0: - if (rk.frame % self.ticks_per_frame * 3) == 0: - o, _, d, _ = self.env.step(vc) - if d: - print("!!!Episode terminated due to violation of safety!!!") - self.env.reset() - self.env.step([0.0, 0.0]) - for _ in range(300): - rk.keep_time() - - self.speed = o["state"][3] * math.sqrt(self.ticks_per_frame) * 10 # empirically derived - - img = self.env.vehicle.image_sensors["rgb_wide"].get_pixels_array(self.env.vehicle, False) - self.yuv = self.camerad.img_to_yuv(img) - self.camerad.cam_send_yuv_wide_road(self.yuv) + o, _, d, _ = timer("Step", lambda: self.env.step(vc)) + if d: + print("!!!Episode terminated due to violation of safety!!!") + self.env.reset() + self.env.step([0.0, 0.0]) + for _ in range(300): + rk.keep_time() + + self.speed = o["state"][3] * math.sqrt(self.ticks_per_frame) * 10 # empirically derived + + # if (rk.frame % (self.ticks_per_frame * 2)) == 0: + img = timer("pixel array", lambda: self.env.vehicle.image_sensors["rgb_wide"].get_pixels_array(self.env.vehicle, False)) + self.yuv = timer("yuv kernel", lambda: self.camerad.img_to_yuv(img)) + timer("send yuv", lambda: self.camerad.cam_send_yuv_wide_road(self.yuv)) + # timer("send yuv", lambda: self.camerad.cam_send_yuv_wide_road(self.yuv)) def get_velocity(self): return None @@ -63,6 +63,14 @@ def __init__(self, args): super(MetaDriveBridge, self).__init__(args) def spawn_objects(self): + # Lazily import as `metadrive-simulator`` is an optional install + import metadrive # noqa: F401 pylint: disable=W0611, disable=import-error + import gym + from metadrive.constants import CamMask # pylint: disable=import-error + from metadrive.component.vehicle_module.base_camera import BaseCamera # pylint: disable=import-error + from metadrive.engine.engine_utils import engine_initialized # pylint: disable=import-error + from metadrive.engine.core.image_buffer import ImageBuffer # pylint: disable=import-error + env = gym.make('MetaDrive-10env-v0', config=dict(offscreen_render=True)) # config = dict( @@ -83,10 +91,6 @@ def spawn_objects(self): # max_engine_force._replace(min=max_engine_force.min * 10) env.reset() - from metadrive.constants import CamMask - from metadrive.component.vehicle_module.base_camera import BaseCamera - from metadrive.engine.engine_utils import engine_initialized - from metadrive.engine.core.image_buffer import ImageBuffer class RGBCameraWide(BaseCamera): # shape(dim_1, dim_2) BUFFER_W = W # dim 1 diff --git a/tools/sim/bridge/utils.py b/tools/sim/bridge/utils.py new file mode 100644 index 00000000000000..37d0be39f6f3aa --- /dev/null +++ b/tools/sim/bridge/utils.py @@ -0,0 +1,14 @@ +from typing import Any +import timeit + +TIMER_ENABLED = True + +if TIMER_ENABLED: + def timer(name: str, f) -> Any: + start = timeit.default_timer() + ret = f() + print(f"{name} took {round((timeit.default_timer() - start) * 10 ** 3, 3)}ms") + return ret +else: + def timer(name: str, f) -> Any: + return f() \ No newline at end of file From c5490e395d58e9836a62e534fdec5e1da708cbb0 Mon Sep 17 00:00:00 2001 From: jon-chuang Date: Mon, 20 Feb 2023 12:44:37 +0800 Subject: [PATCH 17/28] minor --- tools/sim/bridge/metadrive.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tools/sim/bridge/metadrive.py b/tools/sim/bridge/metadrive.py index 7390e9046a38a0..8d9d5ddc90771f 100644 --- a/tools/sim/bridge/metadrive.py +++ b/tools/sim/bridge/metadrive.py @@ -65,7 +65,7 @@ def __init__(self, args): def spawn_objects(self): # Lazily import as `metadrive-simulator`` is an optional install import metadrive # noqa: F401 pylint: disable=W0611, disable=import-error - import gym + import gym # pylint: disable=import-error from metadrive.constants import CamMask # pylint: disable=import-error from metadrive.component.vehicle_module.base_camera import BaseCamera # pylint: disable=import-error from metadrive.engine.engine_utils import engine_initialized # pylint: disable=import-error From d780e223539935595b1d5df77c5801a52b2c5dcb Mon Sep 17 00:00:00 2001 From: jon-chuang Date: Mon, 6 Mar 2023 09:45:06 +0800 Subject: [PATCH 18/28] minor --- tools/sim/bridge/metadrive.py | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/tools/sim/bridge/metadrive.py b/tools/sim/bridge/metadrive.py index 8d9d5ddc90771f..0e226982ddc8c3 100644 --- a/tools/sim/bridge/metadrive.py +++ b/tools/sim/bridge/metadrive.py @@ -44,7 +44,9 @@ def get_speed(self) -> float: return self.speed def get_steer_correction(self) -> float: - max_steer_angle = 75 / self.ticks_per_frame + # max_steer_angle = 75 / self.ticks_per_frame + # max_steer_angle = 200 / self.ticks_per_frame + max_steer_angle = 5 return max_steer_angle * STEER_RATIO * -1 def tick(self): @@ -63,7 +65,7 @@ def __init__(self, args): super(MetaDriveBridge, self).__init__(args) def spawn_objects(self): - # Lazily import as `metadrive-simulator`` is an optional install + # Lazily import as `metadrive-simulator` is an optional install import metadrive # noqa: F401 pylint: disable=W0611, disable=import-error import gym # pylint: disable=import-error from metadrive.constants import CamMask # pylint: disable=import-error From 681ae2c9a9ea3583878cd9ddd8816eccd9bbd8f2 Mon Sep 17 00:00:00 2001 From: jon-chuang Date: Mon, 6 Mar 2023 09:45:36 +0800 Subject: [PATCH 19/28] undo --- tools/cabana/dbcmanager.h | 1 - 1 file changed, 1 deletion(-) diff --git a/tools/cabana/dbcmanager.h b/tools/cabana/dbcmanager.h index 6fd3f050fab1a6..b438b3e1c23629 100644 --- a/tools/cabana/dbcmanager.h +++ b/tools/cabana/dbcmanager.h @@ -3,7 +3,6 @@ #include #include #include -#include struct MessageId { uint8_t source; From b9123bd62cb3823092daf55425531d86dcc8ea3b Mon Sep 17 00:00:00 2001 From: jon-chuang Date: Mon, 6 Mar 2023 21:34:37 +0800 Subject: [PATCH 20/28] done --- pyproject.toml | 2 +- tools/sim/bridge/carla.py | 2 +- tools/sim/bridge/common.py | 22 +++++-- tools/sim/bridge/metadrive.py | 109 +++++++++++++++++++++------------- tools/sim/bridge/utils.py | 2 +- tools/sim/run_bridge.py | 2 + 6 files changed, 89 insertions(+), 50 deletions(-) diff --git a/pyproject.toml b/pyproject.toml index 95740332db1de8..edd63972941bf3 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -175,7 +175,7 @@ omegaconf = "^2.3.0" osmnx = "==1.2.2" panda3d = ">1.10.8, <=1.10.13" gym = ">=0.20.0, <=0.24.0" -metadrive-simulator = { git = "https://github.com/jon-chuang/metadrive.git", branch = "jon/versioning-hack" } +metadrive-simulator = { git = "https://github.com/metadriverse/metadrive.git", rev ="bfb5d771e2424a687eb07ba5069612d5c8988cf5" } [build-system] requires = ["poetry-core"] diff --git a/tools/sim/bridge/carla.py b/tools/sim/bridge/carla.py index 54375129912a43..817c10adabfc33 100644 --- a/tools/sim/bridge/carla.py +++ b/tools/sim/bridge/carla.py @@ -32,7 +32,7 @@ def tick(self): class CarlaBridge(SimulatorBridge): TICKS_PER_FRAME = 5 - def spawn_objects(self): + def spawn_world(self): camerad = Camerad() import carla # pylint: disable=import-error def connect_carla_client(host: str, port: int): diff --git a/tools/sim/bridge/common.py b/tools/sim/bridge/common.py index 73dcdcd100bd76..9fc2493afc0d12 100644 --- a/tools/sim/bridge/common.py +++ b/tools/sim/bridge/common.py @@ -34,7 +34,9 @@ def parse_args(add_args=None): parser = argparse.ArgumentParser(description='Bridge between CARLA and openpilot.') parser.add_argument('--joystick', action='store_true') + # TODO(jon-chuang): Multiplex the default based on detection of `CUDA` or `ROCm` parser.add_argument('--high_quality', action='store_true') + # TODO(jon-chuang): Multiplex the default based on detection of `CUDA` or `ROCm` parser.add_argument('--dual_camera', action='store_true') parser.add_argument('--town', type=str, default='Town04_Opt') parser.add_argument('--spawn_point', dest='num_selected_spawn_point', type=int, default=16) @@ -98,6 +100,10 @@ def cam_callback_wide_road(self, image): self._send_yuv(yuv, self.frame_wide_id, 'wideRoadCameraState', VisionStreamType.VISION_STREAM_WIDE_ROAD) self.frame_wide_id += 1 + def cam_send_yuv_road(self, yuv): + self._send_yuv(yuv, self.frame_road_id, 'roadCameraState', VisionStreamType.VISION_STREAM_ROAD) + self.frame_road_id += 1 + def cam_send_yuv_wide_road(self, yuv): self._send_yuv(yuv, self.frame_wide_id, 'wideRoadCameraState', VisionStreamType.VISION_STREAM_WIDE_ROAD) self.frame_wide_id += 1 @@ -108,7 +114,7 @@ def img_to_yuv(self, img): rgb = np.reshape(img, (H, W * 3)) rgb_cl = cl_array.to_device(self.queue, rgb) yuv_cl = cl_array.empty_like(rgb_cl) - self.krnl(self.queue, (np.int32(self.Wdiv4), np.int32(self.Hdiv4)), None, rgb_cl.data, yuv_cl.data).wait() + self.krnl(self.queue, (self.Wdiv4, self.Hdiv4), None, rgb_cl.data, yuv_cl.data).wait() yuv = np.resize(yuv_cl.get(), rgb.size // 2) return yuv.data.tobytes() @@ -277,9 +283,9 @@ def __init__(self, arguments): if not arguments.dual_camera: print("Dual Camera disabled") self.params.put_bool("WideCameraOnly", not arguments.dual_camera) - # self.params.put_bool("DisengageOnAccelerator", True) + self.params.put_bool("DisengageOnAccelerator", True) self.params.put_bool("ExperimentalMode", True) - self.params.put_bool("ExperimentalLongitudinalEnabled", True) + # self.params.put_bool("ExperimentalLongitudinalEnabled", True) self._args = arguments self._simulation_objects = [] @@ -337,13 +343,13 @@ def run(self, queue, retries=-1): # Must return object of class `World`, after spawning objects into that world @abstractmethod - def spawn_objects(self) -> World: + def spawn_world(self) -> World: pass def _run(self, q: Queue): vehicle_state = VehicleState() self._vehicle_state = vehicle_state - world = self.spawn_objects() + world = self.spawn_world() # launch fake car threads self._threads.append(threading.Thread(target=panda_state_function, args=(vehicle_state, self._exit_event,))) @@ -368,8 +374,12 @@ def _run(self, q: Queue): brake_manual_multiplier = 0.7 # keyboard signal is always 1 steer_manual_multiplier = 45 * STEER_RATIO # keyboard signal is always 1 + # Simulation tends to be slow in the initial steps. This prevents lagging later + for _ in range(20): + world.tick() + # loop - rk = Ratekeeper(100, print_delay_threshold=0.05) + rk = Ratekeeper(100, None) while self._keep_alive: # 1. Read the throttle, steer and brake from op or manual controls diff --git a/tools/sim/bridge/metadrive.py b/tools/sim/bridge/metadrive.py index 0e226982ddc8c3..c5ce136e416bed 100644 --- a/tools/sim/bridge/metadrive.py +++ b/tools/sim/bridge/metadrive.py @@ -1,6 +1,4 @@ -import warnings import numpy as np -import math from tools.sim.bridge.common import World, SimulatorBridge, Camerad, STEER_RATIO, W, H from tools.sim.bridge.utils import timer @@ -9,7 +7,6 @@ class MetaDriveWorld(World): def __init__(self, env, ticks_per_frame: float): self.env = env self.speed = 0.0 - self.yuv = None self.camerad = Camerad() self.ticks_per_frame = ticks_per_frame @@ -23,19 +20,24 @@ def apply_controls(self, steer_sim, throttle_out, brake_out, rk): if rk.frame % self.ticks_per_frame == 0: o, _, d, _ = timer("Step", lambda: self.env.step(vc)) if d: - print("!!!Episode terminated due to violation of safety!!!") - self.env.reset() - self.env.step([0.0, 0.0]) - for _ in range(300): - rk.keep_time() - - self.speed = o["state"][3] * math.sqrt(self.ticks_per_frame) * 10 # empirically derived - - # if (rk.frame % (self.ticks_per_frame * 2)) == 0: - img = timer("pixel array", lambda: self.env.vehicle.image_sensors["rgb_wide"].get_pixels_array(self.env.vehicle, False)) - self.yuv = timer("yuv kernel", lambda: self.camerad.img_to_yuv(img)) - timer("send yuv", lambda: self.camerad.cam_send_yuv_wide_road(self.yuv)) - # timer("send yuv", lambda: self.camerad.cam_send_yuv_wide_road(self.yuv)) + # Pass for now as the early termination can be a little disorienting + pass + # print("!!!Episode terminated due to violation of safety!!!") + # self.env.reset() + # self.env.step([0.0, 0.0]) + # for _ in range(300): + # rk.keep_time() + + self.speed = o["state"][3] * 75 # empirically derived + + img = timer("pixel array: wide", lambda: self.env.vehicle.image_sensors["rgb_wide"].get_pixels_array(self.env.vehicle, False)) + yuv = timer("yuv kernel: wide", lambda: self.camerad.img_to_yuv(img)) + timer("send yuv: wide", lambda: self.camerad.cam_send_yuv_wide_road(yuv)) + + # TODO(jon-chuang): enable and calibrate road camera + # img = timer("pixel array: road", lambda: self.env.vehicle.image_sensors["rgb_road"].get_pixels_array(self.env.vehicle, False)) + # yuv = timer("yuv kernel: road", lambda: self.camerad.img_to_yuv(img)) + # timer("send yuv: road", lambda: self.camerad.cam_send_yuv_road(yuv)) def get_velocity(self): return None @@ -45,8 +47,7 @@ def get_speed(self) -> float: def get_steer_correction(self) -> float: # max_steer_angle = 75 / self.ticks_per_frame - # max_steer_angle = 200 / self.ticks_per_frame - max_steer_angle = 5 + max_steer_angle = 8 return max_steer_angle * STEER_RATIO * -1 def tick(self): @@ -57,14 +58,11 @@ class MetaDriveBridge(SimulatorBridge): TICKS_PER_FRAME = 10 def __init__(self, args): - if args.dual_camera: - warnings.warn("Dual camera not supported in MetaDrive simulator for performance reasons") - args.dual_camera = False if args.ticks_per_frame: self.TICKS_PER_FRAME = args.ticks_per_frame super(MetaDriveBridge, self).__init__(args) - def spawn_objects(self): + def spawn_world(self): # Lazily import as `metadrive-simulator` is an optional install import metadrive # noqa: F401 pylint: disable=W0611, disable=import-error import gym # pylint: disable=import-error @@ -73,25 +71,32 @@ def spawn_objects(self): from metadrive.engine.engine_utils import engine_initialized # pylint: disable=import-error from metadrive.engine.core.image_buffer import ImageBuffer # pylint: disable=import-error - env = gym.make('MetaDrive-10env-v0', config=dict(offscreen_render=True)) - - # config = dict( - # # camera_dist=3.0, - # # camera_height=1.0, - # # use_render=True, - # vehicle_config=dict( - # # enable_reverse=True, - # # image_source="rgb_camera", - # # rgb_camera=(0,0) - # ), - # offscreen_render=True, - # ) + # remove use of `to(dtype=uint8)` + def get_pixels_array_patched(self, base_object, clip=True): + self.track(base_object) + return type(self)._singleton.get_rgb_array() + setattr(BaseCamera, "get_pixels_array", get_pixels_array_patched) + + config = dict( + # camera_dist=3.0, + # camera_height=1.0, + # use_render=True, + vehicle_config=dict( + # enable_reverse=True, + # image_source="rgb_wide", + rgb_camera=(1,1) + ), + offscreen_render=True, + ) + # from metadrive.utils.space import VehicleParameterSpace # from metadrive.component.vehicle.vehicle_type import DefaultVehicle # max_engine_force = VehicleParameterSpace.DEFAULT_VEHICLE["max_engine_force"] # max_engine_force._replace(max=max_engine_force.max * 10) # max_engine_force._replace(min=max_engine_force.min * 10) + env = gym.make('MetaDrive-10env-v0', config=config) + env.reset() class RGBCameraWide(BaseCamera): # shape(dim_1, dim_2) @@ -112,19 +117,41 @@ def __init__(self): env.vehicle.add_image_sensor("rgb_wide", RGBCameraWide()) - # Monkey patch `get_rgb_array` since it involves unnecessary copies - def patch_get_rgb_array(self): - if self.engine.episode_step <= 1: - self.engine.graphicsEngine.renderFrame() - origin_img = self.cam.node().getDisplayRegion(0).getScreenshot() + # TODO(jon-chuang): enable road camera + # class RGBCameraRoad(BaseCamera): + # # shape(dim_1, dim_2) + # BUFFER_W = W # dim 1 + # BUFFER_H = H # dim 2 + # CAM_MASK = CamMask.RgbCam + + # def __init__(self): + # assert engine_initialized(), "You should initialize engine before adding camera to vehicle" + # self.BUFFER_W, self.BUFFER_H = W, H + # super(RGBCameraRoad, self).__init__() + # cam = self.get_cam() + # lens = self.get_lens() + # cam.lookAt(0, 2.4, 1.3) + # cam.setHpr(0, 0.8, 0) + # lens.setFov(40) + # lens.setAspectRatio(0.5) + + # env.vehicle.add_image_sensor("rgb_road", RGBCameraRoad()) + def transform_img(origin_img): img = np.frombuffer(origin_img.getRamImage().getData(), dtype=np.uint8) img = img.reshape((origin_img.getYSize(), origin_img.getXSize(), 4)) img = img[::-1] img = img[..., :-1] return img - setattr(ImageBuffer, "get_rgb_array", patch_get_rgb_array) + # Monkey patch `get_rgb_array` since it involves unnecessary copies + def patch_get_rgb_array(self): + if self.engine.episode_step <= 1: + self.engine.graphicsEngine.renderFrame() + origin_img = timer("getScreenshot", self.cam.node().getDisplayRegion(0).getScreenshot) + img = timer("transform_img", lambda: transform_img(origin_img)) + return img + setattr(ImageBuffer, "get_rgb_array", patch_get_rgb_array) # Simulation tends to be slow in the initial steps. This prevents lagging later for _ in range(30): env.step([0.0, 1.0]) diff --git a/tools/sim/bridge/utils.py b/tools/sim/bridge/utils.py index 37d0be39f6f3aa..25fee129f45e10 100644 --- a/tools/sim/bridge/utils.py +++ b/tools/sim/bridge/utils.py @@ -1,7 +1,7 @@ from typing import Any import timeit -TIMER_ENABLED = True +TIMER_ENABLED = False if TIMER_ENABLED: def timer(name: str, f) -> Any: diff --git a/tools/sim/run_bridge.py b/tools/sim/run_bridge.py index 39ed3a12c83809..d78fcb1a578529 100755 --- a/tools/sim/run_bridge.py +++ b/tools/sim/run_bridge.py @@ -13,6 +13,8 @@ try: simulator_bridge: SimulatorBridge if args.simulator == "metadrive": + if args.dual_camera: + raise AssertionError("Dual camera not supported in MetaDrive simulator for now") simulator_bridge = MetaDriveBridge(args) elif args.simulator == "carla": simulator_bridge = CarlaBridge(args) From cc44ad3ff3ee7a8e9ac641f1ddf67db09d26c7f8 Mon Sep 17 00:00:00 2001 From: jon-chuang Date: Mon, 6 Mar 2023 21:35:58 +0800 Subject: [PATCH 21/28] gym dep --- pyproject.toml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/pyproject.toml b/pyproject.toml index edd63972941bf3..5978aa4f6b5d89 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -21,6 +21,7 @@ flake8 = "^4.0.1" Flask = "^2.1.2" future-fstrings = "^1.2.0" # for acados gunicorn = "^20.1.0" +gym = ">=0.20.0, <=0.24.0" hatanaka = "==2.4" hexdump = "^3.3" Jinja2 = "^3.1.2" @@ -174,7 +175,6 @@ zerorpc = { git = "https://github.com/commaai/zerorpc-python.git", branch = "mas omegaconf = "^2.3.0" osmnx = "==1.2.2" panda3d = ">1.10.8, <=1.10.13" -gym = ">=0.20.0, <=0.24.0" metadrive-simulator = { git = "https://github.com/metadriverse/metadrive.git", rev ="bfb5d771e2424a687eb07ba5069612d5c8988cf5" } [build-system] From 995c1f592d7ce25c7e8894afcec6372cfc7be264 Mon Sep 17 00:00:00 2001 From: jon-chuang Date: Mon, 6 Mar 2023 21:54:11 +0800 Subject: [PATCH 22/28] lock --- poetry.lock | 24 ++++++++++++++---------- 1 file changed, 14 insertions(+), 10 deletions(-) diff --git a/poetry.lock b/poetry.lock index 8a0daf9a2fbbf8..6c1e3bffa26737 100644 --- a/poetry.lock +++ b/poetry.lock @@ -596,7 +596,7 @@ test = ["pytest-cov"] name = "cloudpickle" version = "2.2.0" description = "Extended pickling support for Python objects" -category = "dev" +category = "main" optional = false python-versions = ">=3.6" @@ -1212,7 +1212,7 @@ tornado = ["tornado (>=0.2)"] name = "gym" version = "0.22.0" description = "Gym: A universal API for reinforcement learning environments." -category = "dev" +category = "main" optional = false python-versions = ">=3.7" @@ -1237,7 +1237,7 @@ toy-text = ["pygame (==2.1.0)", "scipy (>=1.4.1)"] name = "gym-notices" version = "0.0.8" description = "Notices for gym" -category = "dev" +category = "main" optional = false python-versions = "*" @@ -2088,15 +2088,15 @@ python-versions = ">=3.7" [[package]] name = "metadrive-simulator" -version = "0.2.6.0" +version = "0.2.7.0" description = "" category = "dev" optional = false -python-versions = ">=3.7, <3.10" +python-versions = ">=3.6, <3.12" develop = false [package.dependencies] -gym = ">=0.20.0,<=0.24.0" +gym = ">=0.20.0,<0.23.0 || >=0.25.0,<0.26.0" lxml = "*" matplotlib = "*" numpy = ">=1.21.6,<=1.24.2" @@ -2115,11 +2115,15 @@ seaborn = "*" tqdm = "*" yapf = "*" +[package.extras] +cuda = ["PyOpenGL (==3.1.6)", "PyOpenGL-accelerate (==3.1.6)", "cuda-python (==12.0.0)", "glfw", "pyrr (==0.10.3)"] +nuplan = ["bokeh (==2.4)", "hydra", "nuplan-devkit (==1.0.0)"] + [package.source] type = "git" -url = "https://github.com/jon-chuang/metadrive.git" -reference = "jon/versioning-hack" -resolved_reference = "2e6f00b53fab2e2d4806c1cbd0ba2b0610f19a04" +url = "https://github.com/metadriverse/metadrive.git" +reference = "bfb5d771e2424a687eb07ba5069612d5c8988cf5" +resolved_reference = "bfb5d771e2424a687eb07ba5069612d5c8988cf5" [[package]] name = "mistune" @@ -4709,7 +4713,7 @@ testing = ["coverage (>=5.0.3)", "zope.event", "zope.testing"] [metadata] lock-version = "1.1" python-versions = "~3.8" -content-hash = "f9b173ceb766b5e4a53b2075d5a3ee303918f57c77c68b040c9cd68cd16d285d" +content-hash = "ac1370cec835f855882acafd004c381bdcaccbba8a47f9613b95a97c8c551f77" [metadata.files] adal = [ From a3d10568d909f54f6a04baa5c61d18de270a88e0 Mon Sep 17 00:00:00 2001 From: jon-chuang Date: Mon, 6 Mar 2023 22:42:42 +0800 Subject: [PATCH 23/28] sim bridge unified integration test --- tools/sim/tests/test_carla_integration.py | 108 --------------- tools/sim/tests/test_metadrive_integration.py | 87 ------------ .../sim/tests/test_sim_bridge_integration.py | 127 ++++++++++++++++++ 3 files changed, 127 insertions(+), 195 deletions(-) delete mode 100755 tools/sim/tests/test_carla_integration.py delete mode 100755 tools/sim/tests/test_metadrive_integration.py create mode 100755 tools/sim/tests/test_sim_bridge_integration.py diff --git a/tools/sim/tests/test_carla_integration.py b/tools/sim/tests/test_carla_integration.py deleted file mode 100755 index 79072da287b7a8..00000000000000 --- a/tools/sim/tests/test_carla_integration.py +++ /dev/null @@ -1,108 +0,0 @@ -#!/usr/bin/env python3 -import subprocess -import time -import unittest -import os -from multiprocessing import Queue - -from cereal import messaging -from common.basedir import BASEDIR -from selfdrive.manager.helpers import unblock_stdout -from tools.sim.bridge.common import parse_args -from 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 - - 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) - self.processes.append(p_manager) - - sm = messaging.SubMaster(['controlsState', 'carEvents', 'managerState']) - q = Queue() - carla_bridge = CarlaBridge(parse_args([])) - p_bridge = carla_bridge.run(q, retries=10) - self.processes.append(p_bridge) - - max_time_per_step = 60 - - # Wait for bridge to startup - start_waiting = time.monotonic() - while not carla_bridge.started and time.monotonic() < start_waiting + max_time_per_step: - time.sleep(0.1) - self.assertEqual(p_bridge.exitcode, None, f"Bridge process should be running, but exited with code {p_bridge.exitcode}") - - start_time = time.monotonic() - no_car_events_issues_once = False - car_event_issues = [] - not_running = [] - while time.monotonic() < start_time + max_time_per_step: - sm.update() - - not_running = [p.name for p in sm['managerState'].processes if not p.running and p.shouldBeRunning] - car_event_issues = [event.name for event in sm['carEvents'] if any([event.noEntry, event.softDisable, event.immediateDisable])] - - if sm.all_alive() and len(car_event_issues) == 0 and len(not_running) == 0: - no_car_events_issues_once = True - break - - self.assertTrue(no_car_events_issues_once, f"Failed because no messages received, or CarEvents '{car_event_issues}' or processes not running '{not_running}'") - - start_time = time.monotonic() - min_counts_control_active = 100 - control_active = 0 - - while time.monotonic() < start_time + max_time_per_step: - sm.update() - - q.put("cruise_down") # Try engaging - - if sm.all_alive() and sm['controlsState'].active: - control_active += 1 - - if control_active == min_counts_control_active: - break - - self.assertEqual(min_counts_control_active, control_active, f"Simulator did not engage a minimal of {min_counts_control_active} steps was {control_active}") - - def tearDown(self): - print("Test shutting down. CommIssues are acceptable") - for p in reversed(self.processes): - p.terminate() - - for p in reversed(self.processes): - if isinstance(p, subprocess.Popen): - p.wait(15) - 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() diff --git a/tools/sim/tests/test_metadrive_integration.py b/tools/sim/tests/test_metadrive_integration.py deleted file mode 100755 index 40002cff13dfaa..00000000000000 --- a/tools/sim/tests/test_metadrive_integration.py +++ /dev/null @@ -1,87 +0,0 @@ -#!/usr/bin/env python3 -import subprocess -import time -from typing import List -import unittest -import os -from multiprocessing import Queue - -from cereal import messaging -from common.basedir import BASEDIR -from tools.sim.bridge.common import parse_args -from tools.sim.bridge.metadrive import MetaDriveBridge - -CI = "CI" in os.environ - -SIM_DIR = os.path.join(BASEDIR, "tools/sim") - -class TestMetaDriveIntegration(unittest.TestCase): - processes: List[subprocess.Popen] = [] - - def test_engage(self): - # Startup manager and bridge.py. Check processes are running, then engage and verify. - q = Queue() - sim_bridge = MetaDriveBridge(parse_args(["--ticks_per_frame", "20"])) - p_bridge = sim_bridge.run(q, retries=10) - self.processes.append(p_bridge) - time.sleep(10.0) - - p_manager = subprocess.Popen("./launch_openpilot.sh", cwd=SIM_DIR) - self.processes.append(p_manager) - - sm = messaging.SubMaster(['controlsState', 'carEvents', 'managerState']) - - max_time_per_step = 60 - - # Wait for bridge to startup - start_waiting = time.monotonic() - while not sim_bridge.started and time.monotonic() < start_waiting + max_time_per_step: - time.sleep(0.1) - self.assertEqual(p_bridge.exitcode, None, f"Bridge process should be running, but exited with code {p_bridge.exitcode}") - - start_time = time.monotonic() - no_car_events_issues_once = False - car_event_issues = [] - not_running = [] - while time.monotonic() < start_time + max_time_per_step: - sm.update() - - not_running = [p.name for p in sm['managerState'].processes if not p.running and p.shouldBeRunning] - car_event_issues = [event.name for event in sm['carEvents'] if any([event.noEntry, event.softDisable, event.immediateDisable])] - - if sm.all_alive() and len(car_event_issues) == 0 and len(not_running) == 0: - no_car_events_issues_once = True - break - - self.assertTrue(no_car_events_issues_once, f"Failed because no messages received, or CarEvents '{car_event_issues}' or processes not running '{not_running}'") - - start_time = time.monotonic() - min_counts_control_active = 100 - control_active = 0 - - while time.monotonic() < start_time + max_time_per_step: - sm.update() - - q.put("cruise_down") # Try engaging - - if sm.all_alive() and sm['controlsState'].active: - control_active += 1 - - if control_active == min_counts_control_active: - break - - self.assertEqual(min_counts_control_active, control_active, f"Simulator did not engage a minimal of {min_counts_control_active} steps was {control_active}") - - def tearDown(self): - print("Test shutting down. CommIssues are acceptable") - for p in reversed(self.processes): - p.terminate() - - for p in reversed(self.processes): - if isinstance(p, subprocess.Popen): - p.wait(15) - else: - p.join(15) - -if __name__ == "__main__": - unittest.main() diff --git a/tools/sim/tests/test_sim_bridge_integration.py b/tools/sim/tests/test_sim_bridge_integration.py new file mode 100755 index 00000000000000..f90860a553f994 --- /dev/null +++ b/tools/sim/tests/test_sim_bridge_integration.py @@ -0,0 +1,127 @@ +#!/usr/bin/env python3 +from contextlib import AbstractContextManager +import subprocess +import time +from typing import List +import unittest +import os +from multiprocessing import Queue +from tools.sim.bridge.carla import CarlaBridge + +from cereal import messaging +from common.basedir import BASEDIR +from selfdrive.manager.helpers import unblock_stdout +from tools.sim.bridge.common import parse_args +from tools.sim.bridge.metadrive import MetaDriveBridge + +CI = "CI" in os.environ + +SIM_DIR = os.path.join(BASEDIR, "tools/sim") + +class TestSimBridgeIntegrationSmoke(unittest.TestCase): + processes: List[subprocess.Popen] = [] + + def test_engage(self): + # Startup manager and bridge.py. Check processes are running, then engage and verify. + for simulator in ["carla", "metadrive"]: + with self.subTest(simulator=simulator), ProcessHolder() as procs: + q = Queue() + if simulator == "carla": + try: + subprocess.check_output('nvidia-smi') + except FileNotFoundError as e: + print("Skipping: CARLA requires CUDA to run", e) + continue + procs.setUpCarla() + sim_bridge = CarlaBridge(parse_args([])) + elif simulator == "metadrive": + sim_bridge = MetaDriveBridge(parse_args(["--ticks_per_frame", "10"])) + else: + raise AssertionError("Unsupported simulator") + p_bridge = sim_bridge.run(q, retries=10) + procs.processes.append(p_bridge) + time.sleep(10.0) + + p_manager = subprocess.Popen("./launch_openpilot.sh", cwd=SIM_DIR) + procs.processes.append(p_manager) + + sm = messaging.SubMaster(['controlsState', 'carEvents', 'managerState']) + + max_time_per_step = 60 + + # Wait for bridge to startup + start_waiting = time.monotonic() + while not sim_bridge.started and time.monotonic() < start_waiting + max_time_per_step: + time.sleep(0.1) + self.assertEqual(p_bridge.exitcode, None, f"Bridge process should be running, but exited with code {p_bridge.exitcode}") + + start_time = time.monotonic() + no_car_events_issues_once = False + car_event_issues = [] + not_running = [] + while time.monotonic() < start_time + max_time_per_step: + sm.update() + + not_running = [p.name for p in sm['managerState'].processes if not p.running and p.shouldBeRunning] + car_event_issues = [event.name for event in sm['carEvents'] if any([event.noEntry, event.softDisable, event.immediateDisable])] + + if sm.all_alive() and len(car_event_issues) == 0 and len(not_running) == 0: + no_car_events_issues_once = True + break + + self.assertTrue(no_car_events_issues_once, f"Failed because no messages received, or CarEvents '{car_event_issues}' or processes not running '{not_running}'") + + start_time = time.monotonic() + min_counts_control_active = 100 + control_active = 0 + + while time.monotonic() < start_time + max_time_per_step: + sm.update() + + q.put("cruise_down") # Try engaging + + if sm.all_alive() and sm['controlsState'].active: + control_active += 1 + + if control_active == min_counts_control_active: + break + + self.assertEqual(min_counts_control_active, control_active, f"Simulator did not engage a minimal of {min_counts_control_active} steps was {control_active}") + +class ProcessHolder(AbstractContextManager): + def __init__(self): + self.processes = [] + self.carla_process = None + + def __enter__(self): + return self + + def __exit__(self, exc_type, exc_value, traceback): + for p in reversed(self.processes): + p.terminate() + + for p in reversed(self.processes): + if isinstance(p, subprocess.Popen): + p.wait(15) + else: + p.join(15) + + if self.carla_process is not None: + # Stop carla simulator by removing docker container + subprocess.run("docker rm -f carla_sim", shell=True, stderr=subprocess.PIPE, check=False) + self.carla_process.wait() + + def setUpCarla(self): + 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) + + +if __name__ == "__main__": + unittest.main() From 086465c0d1a9d505670b896c7d81d9800ac9774e Mon Sep 17 00:00:00 2001 From: jon-chuang Date: Tue, 7 Mar 2023 00:23:13 +0800 Subject: [PATCH 24/28] empty From 6217b965238b4692185e974b6cbd2a5d79dde65e Mon Sep 17 00:00:00 2001 From: jon-chuang Date: Tue, 7 Mar 2023 00:51:39 +0800 Subject: [PATCH 25/28] calibrate and complex env --- tools/sim/bridge/common.py | 2 +- tools/sim/bridge/metadrive.py | 36 ++++++++++++++++++++--------------- 2 files changed, 22 insertions(+), 16 deletions(-) diff --git a/tools/sim/bridge/common.py b/tools/sim/bridge/common.py index 9fc2493afc0d12..c599bd508beae1 100644 --- a/tools/sim/bridge/common.py +++ b/tools/sim/bridge/common.py @@ -379,7 +379,7 @@ def _run(self, q: Queue): world.tick() # loop - rk = Ratekeeper(100, None) + rk = Ratekeeper(100, print_delay_threshold=None) while self._keep_alive: # 1. Read the throttle, steer and brake from op or manual controls diff --git a/tools/sim/bridge/metadrive.py b/tools/sim/bridge/metadrive.py index c5ce136e416bed..02b8b002e4e053 100644 --- a/tools/sim/bridge/metadrive.py +++ b/tools/sim/bridge/metadrive.py @@ -1,4 +1,5 @@ import numpy as np +import math from tools.sim.bridge.common import World, SimulatorBridge, Camerad, STEER_RATIO, W, H from tools.sim.bridge.utils import timer @@ -22,13 +23,14 @@ def apply_controls(self, steer_sim, throttle_out, brake_out, rk): if d: # Pass for now as the early termination can be a little disorienting pass + # env.render(text={"!!!Episode terminated due to violation of safety!!!"}) # print("!!!Episode terminated due to violation of safety!!!") # self.env.reset() # self.env.step([0.0, 0.0]) # for _ in range(300): # rk.keep_time() - self.speed = o["state"][3] * 75 # empirically derived + self.speed = o["state"][3] * 35 # empirically derived img = timer("pixel array: wide", lambda: self.env.vehicle.image_sensors["rgb_wide"].get_pixels_array(self.env.vehicle, False)) yuv = timer("yuv kernel: wide", lambda: self.camerad.img_to_yuv(img)) @@ -47,7 +49,7 @@ def get_speed(self) -> float: def get_steer_correction(self) -> float: # max_steer_angle = 75 / self.ticks_per_frame - max_steer_angle = 8 + max_steer_angle = 30 / math.sqrt(self.ticks_per_frame) return max_steer_angle * STEER_RATIO * -1 def tick(self): @@ -60,16 +62,18 @@ class MetaDriveBridge(SimulatorBridge): def __init__(self, args): if args.ticks_per_frame: self.TICKS_PER_FRAME = args.ticks_per_frame + self.should_render = args.high_quality + super(MetaDriveBridge, self).__init__(args) def spawn_world(self): # Lazily import as `metadrive-simulator` is an optional install import metadrive # noqa: F401 pylint: disable=W0611, disable=import-error - import gym # pylint: disable=import-error from metadrive.constants import CamMask # pylint: disable=import-error from metadrive.component.vehicle_module.base_camera import BaseCamera # pylint: disable=import-error from metadrive.engine.engine_utils import engine_initialized # pylint: disable=import-error from metadrive.engine.core.image_buffer import ImageBuffer # pylint: disable=import-error + from metadrive.tests.test_functionality.test_object_collision_detection import ComplexEnv # remove use of `to(dtype=uint8)` def get_pixels_array_patched(self, base_object, clip=True): @@ -77,17 +81,17 @@ def get_pixels_array_patched(self, base_object, clip=True): return type(self)._singleton.get_rgb_array() setattr(BaseCamera, "get_pixels_array", get_pixels_array_patched) - config = dict( - # camera_dist=3.0, - # camera_height=1.0, - # use_render=True, - vehicle_config=dict( - # enable_reverse=True, - # image_source="rgb_wide", - rgb_camera=(1,1) - ), - offscreen_render=True, - ) + # config = dict( + # # camera_dist=3.0, + # # camera_height=1.0, + # # use_render=True, + # vehicle_config=dict( + # # enable_reverse=True, + # # image_source="rgb_wide", + # rgb_camera=(1,1) + # ), + # offscreen_render=True, + # ) # from metadrive.utils.space import VehicleParameterSpace # from metadrive.component.vehicle.vehicle_type import DefaultVehicle @@ -95,7 +99,8 @@ def get_pixels_array_patched(self, base_object, clip=True): # max_engine_force._replace(max=max_engine_force.max * 10) # max_engine_force._replace(min=max_engine_force.min * 10) - env = gym.make('MetaDrive-10env-v0', config=config) + # env = gym.make('MetaDrive-10env-v0', config=config) + env = ComplexEnv(dict(use_render=self.should_render, offscreen_render=True, vehicle_config=dict(enable_reverse=True, rgb_camera=(1, 1)))) env.reset() class RGBCameraWide(BaseCamera): @@ -155,5 +160,6 @@ def patch_get_rgb_array(self): # Simulation tends to be slow in the initial steps. This prevents lagging later for _ in range(30): env.step([0.0, 1.0]) + env.render(text={"Switch perspective": "Q or B", "Reset Episode": "R"}) return MetaDriveWorld(env, self.TICKS_PER_FRAME) \ No newline at end of file From f5a83d25a6c4759d74ed2e5f65d96ab5e6f629ec Mon Sep 17 00:00:00 2001 From: jon-chuang Date: Tue, 7 Mar 2023 05:28:00 +0800 Subject: [PATCH 26/28] rm unnec --- poetry.lock | 8 ++++---- pyproject.toml | 2 -- 2 files changed, 4 insertions(+), 6 deletions(-) diff --git a/poetry.lock b/poetry.lock index 6c1e3bffa26737..08be491a4625ee 100644 --- a/poetry.lock +++ b/poetry.lock @@ -596,7 +596,7 @@ test = ["pytest-cov"] name = "cloudpickle" version = "2.2.0" description = "Extended pickling support for Python objects" -category = "main" +category = "dev" optional = false python-versions = ">=3.6" @@ -1212,7 +1212,7 @@ tornado = ["tornado (>=0.2)"] name = "gym" version = "0.22.0" description = "Gym: A universal API for reinforcement learning environments." -category = "main" +category = "dev" optional = false python-versions = ">=3.7" @@ -1237,7 +1237,7 @@ toy-text = ["pygame (==2.1.0)", "scipy (>=1.4.1)"] name = "gym-notices" version = "0.0.8" description = "Notices for gym" -category = "main" +category = "dev" optional = false python-versions = "*" @@ -4713,7 +4713,7 @@ testing = ["coverage (>=5.0.3)", "zope.event", "zope.testing"] [metadata] lock-version = "1.1" python-versions = "~3.8" -content-hash = "ac1370cec835f855882acafd004c381bdcaccbba8a47f9613b95a97c8c551f77" +content-hash = "f74cc0c8f4be5c8a6ec9c87da11f518fb5fbfb72f2ac24079e89bc706cbafb7f" [metadata.files] adal = [ diff --git a/pyproject.toml b/pyproject.toml index 5978aa4f6b5d89..5251473e9fd317 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -21,7 +21,6 @@ flake8 = "^4.0.1" Flask = "^2.1.2" future-fstrings = "^1.2.0" # for acados gunicorn = "^20.1.0" -gym = ">=0.20.0, <=0.24.0" hatanaka = "==2.4" hexdump = "^3.3" Jinja2 = "^3.1.2" @@ -174,7 +173,6 @@ Werkzeug = "^2.1.2" zerorpc = { git = "https://github.com/commaai/zerorpc-python.git", branch = "master" } omegaconf = "^2.3.0" osmnx = "==1.2.2" -panda3d = ">1.10.8, <=1.10.13" metadrive-simulator = { git = "https://github.com/metadriverse/metadrive.git", rev ="bfb5d771e2424a687eb07ba5069612d5c8988cf5" } [build-system] From a129edb075351da0286d89d761d6fedc99a067f2 Mon Sep 17 00:00:00 2001 From: jon-chuang Date: Tue, 7 Mar 2023 05:28:34 +0800 Subject: [PATCH 27/28] lint --- tools/sim/bridge/metadrive.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tools/sim/bridge/metadrive.py b/tools/sim/bridge/metadrive.py index 02b8b002e4e053..e61cd13077fac3 100644 --- a/tools/sim/bridge/metadrive.py +++ b/tools/sim/bridge/metadrive.py @@ -73,7 +73,7 @@ def spawn_world(self): from metadrive.component.vehicle_module.base_camera import BaseCamera # pylint: disable=import-error from metadrive.engine.engine_utils import engine_initialized # pylint: disable=import-error from metadrive.engine.core.image_buffer import ImageBuffer # pylint: disable=import-error - from metadrive.tests.test_functionality.test_object_collision_detection import ComplexEnv + from metadrive.tests.test_functionality.test_object_collision_detection import ComplexEnv # pylint: disable=import-error # remove use of `to(dtype=uint8)` def get_pixels_array_patched(self, base_object, clip=True): From 52ea1c1c2d3bff2bb803c4eb488b5c2e4b08ef81 Mon Sep 17 00:00:00 2001 From: Justin Newberry Date: Thu, 7 Sep 2023 13:36:57 -0700 Subject: [PATCH 28/28] bump submodules --- body | 2 +- cereal | 2 +- laika_repo | 2 +- opendbc | 2 +- panda | 2 +- rednose_repo | 2 +- tinygrad_repo | 2 +- 7 files changed, 7 insertions(+), 7 deletions(-) diff --git a/body b/body index e1805f65ee75fa..6ff44357a3e416 160000 --- a/body +++ b/body @@ -1 +1 @@ -Subproject commit e1805f65ee75fab4454c21eda8b42b49d4bdc48f +Subproject commit 6ff44357a3e416d29044b1d085a3e9223db9691a diff --git a/cereal b/cereal index 42f84fd85d06c0..d469732b3b4c37 160000 --- a/cereal +++ b/cereal @@ -1 +1 @@ -Subproject commit 42f84fd85d06c0fc85371daa2b4820fca49d763e +Subproject commit d469732b3b4c37aaa21fa37682a20f2aba9ab91c diff --git a/laika_repo b/laika_repo index b740b71c82a748..8861844c9b577f 160000 --- a/laika_repo +++ b/laika_repo @@ -1 +1 @@ -Subproject commit b740b71c82a748e3520b1599487d9a7aaf728670 +Subproject commit 8861844c9b577ff7de7d03fab9f4d7f560415fc9 diff --git a/opendbc b/opendbc index 510bfc06954e31..5ebf73ebed8d16 160000 --- a/opendbc +++ b/opendbc @@ -1 +1 @@ -Subproject commit 510bfc06954e31257f8d8de17adf92f9a68a1b71 +Subproject commit 5ebf73ebed8d1693aadffe97bd2dd012da3b3c1c diff --git a/panda b/panda index e0e754de2c7fdb..cb0cd1bfaabc66 160000 --- a/panda +++ b/panda @@ -1 +1 @@ -Subproject commit e0e754de2c7fdbf943dd02db203b7bb40bf3e9b5 +Subproject commit cb0cd1bfaabc6673b4f21b6c8361778c7b3e0014 diff --git a/rednose_repo b/rednose_repo index 3b6bd703b7a766..8658bed29686b2 160000 --- a/rednose_repo +++ b/rednose_repo @@ -1 +1 @@ -Subproject commit 3b6bd703b7a7667e4f82d0b81ef9a454819b94bd +Subproject commit 8658bed29686b2ddae191fd18302986c85542431 diff --git a/tinygrad_repo b/tinygrad_repo index 2e1d47b16625ff..d8dda2af3afcef 160000 --- a/tinygrad_repo +++ b/tinygrad_repo @@ -1 +1 @@ -Subproject commit 2e1d47b16625ff343516287cdd9e4bcb26f5c4ef +Subproject commit d8dda2af3afcef0bb772fff580cfa8b3eabf7f69