From b84632f87aa08473aa5c9596692279a72d989254 Mon Sep 17 00:00:00 2001 From: Justin Newberry Date: Thu, 14 Sep 2023 14:33:59 -0700 Subject: [PATCH 1/8] sim bridge cleanup --- tools/sim/bridge.py | 561 ---------------------- tools/sim/bridge/__init__.py | 0 tools/sim/bridge/carla.py | 171 +++++++ tools/sim/bridge/common.py | 140 ++++++ tools/sim/lib/camerad.py | 69 +++ tools/sim/lib/can.py | 94 ---- tools/sim/lib/common.py | 81 ++++ tools/sim/lib/simulated_car.py | 112 +++++ tools/sim/lib/simulated_sensors.py | 128 +++++ tools/sim/run_bridge.py | 47 ++ tools/sim/tests/test_carla_integration.py | 6 +- 11 files changed, 751 insertions(+), 658 deletions(-) delete mode 100755 tools/sim/bridge.py 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/lib/camerad.py delete mode 100755 tools/sim/lib/can.py create mode 100644 tools/sim/lib/common.py create mode 100644 tools/sim/lib/simulated_car.py create mode 100644 tools/sim/lib/simulated_sensors.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 33fb2c020e29d2..00000000000000 --- a/tools/sim/bridge.py +++ /dev/null @@ -1,561 +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 - -import carla -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 openpilot.common.basedir import BASEDIR -from openpilot.common.numpy_fast import clip -from openpilot.common.params import Params -from openpilot.common.realtime import DT_DMON, Ratekeeper -from openpilot.selfdrive.car.honda.values import CruiseButtons -from openpilot.selfdrive.test.helpers import set_params_enabled -from openpilot.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) - - 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 = carla.Vector3D() - 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, dual_camera): - 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) - if dual_camera: - 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): - self._cam_callback(image, 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) - 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() - - # convert RGB frame to YUV - 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) - eof = int(frame_id * 0.05 * 1e9) - - self.vipc_server.send(yuv_type, yuv.data.tobytes(), 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 - } - 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 - - -def connect_carla_client(host: str, port: int): - client = carla.Client(host, port) - client.set_timeout(5) - return client - - -class CarlaBridge: - - def __init__(self, arguments): - 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()) - self.params.put_bool("DisengageOnAccelerator", True) - - self._args = arguments - self._carla_objects = [] - self._camerad = None - 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._carla_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): - 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(self._args.dual_camera) - - if self._args.dual_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) - road_camera = create_camera(fov=40, callback=self._camerad.cam_callback_road) - self._carla_objects.append(road_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._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=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 - - 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. - 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 - - # 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) - - 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_carla = steer_out / (max_steer_angle * STEER_RATIO * -1) - - 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) - - vc.throttle = throttle_out / 0.6 - vc.steer = steer_carla - vc.brake = brake_out - vehicle.apply_control(vc) - - # --------------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 - - 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)) - - if rk.frame % 5 == 0: - world.tick() - rk.keep_time() - self.started = True - - def close(self): - self.started = False - self._exit_event.set() - - for s in self._carla_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 - - -if __name__ == "__main__": - q: Any = Queue() - args = parse_args() - - carla_bridge = CarlaBridge(args) - p = carla_bridge.run(q) - - if args.joystick: - # start input poll for joystick - from openpilot.tools.sim.lib.manual_ctrl import wheel_poll_thread - - wheel_poll_thread(q) - else: - # start input poll for keyboard - from openpilot.tools.sim.lib.keyboard_ctrl import keyboard_poll_thread - - keyboard_poll_thread(q) - p.join() 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..ceb3c4652e2caa --- /dev/null +++ b/tools/sim/bridge/carla.py @@ -0,0 +1,171 @@ +import numpy as np +from openpilot.common.params import Params +from openpilot.tools.sim.lib.common import SimulatorState, vec3 +from openpilot.tools.sim.bridge.common import World, SimulatorBridge +from openpilot.tools.sim.lib.camerad import W, H + + +class CarlaWorld(World): + def __init__(self, world, vehicle, high_quality=False, dual_camera=False): + super().__init__(dual_camera) + import carla + self.world = world + self.vc: carla.VehicleControl = 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.params = Params() + + self.road_image = np.zeros((H, W, 3), dtype=np.uint8) + + self.steer_ratio = 15 + + self.carla_objects = [] + + blueprint_library = self.world.get_blueprint_library() + 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)) + blueprint.set_attribute('sensor_tick', str(1/20)) + if not high_quality: + blueprint.set_attribute('enable_postprocess_effects', 'False') + camera = world.spawn_actor(blueprint, transform, attach_to=vehicle) + camera.listen(callback) + return camera + + self.road_camera = create_camera(fov=40, callback=self.cam_callback_road) + if dual_camera: + self.road_wide_camera = create_camera(fov=120, callback=self.cam_callback_wide_road) # fov bigger than 120 shows unwanted artifacts + else: + self.road_wide_camera = None + + # re-enable IMU + imu_bp = blueprint_library.find('sensor.other.imu') + imu_bp.set_attribute('sensor_tick', '0.01') + self.imu = world.spawn_actor(imu_bp, transform, attach_to=vehicle) + + gps_bp = blueprint_library.find('sensor.other.gnss') + self.gps = world.spawn_actor(gps_bp, transform, attach_to=vehicle) + self.params.put_bool("UbloxAvailable", True) + + self.carla_objects = [self.imu, self.gps, self.road_camera, self.road_wide_camera] + + def close(self): + for s in self.carla_objects: + if s is not None: + try: + s.destroy() + except Exception as e: + print("Failed to destroy carla object", e) + + def carla_image_to_rgb(self, image): + rgb = np.frombuffer(image.raw_data, dtype=np.dtype("uint8")) + rgb = np.reshape(rgb, (H, W, 4)) + return np.ascontiguousarray(rgb[:, :, [0, 1, 2]]) + + def cam_callback_road(self, image): + with self.world.image_lock: + self.road_image = self.carla_image_to_rgb(image) + + def cam_callback_wide_road(self, image): + with self.world.image_lock: + self.wide_road_image = self.carla_image_to_rgb(image) + + def apply_controls(self, steer_angle, throttle_out, brake_out): + self.vc.throttle = throttle_out + + steer_carla = steer_angle * -1 / (self.max_steer_angle * self.steer_ratio) + steer_carla = np.clip(steer_carla, -1, 1) + + self.vc.steer = steer_carla + self.vc.brake = brake_out + self.vehicle.apply_control(self.vc) + + def read_sensors(self, simulator_state: SimulatorState): + simulator_state.imu.bearing = self.imu.get_transform().rotation.yaw + + simulator_state.imu.accelerometer = vec3( + self.imu.get_acceleration().x, + self.imu.get_acceleration().y, + self.imu.get_acceleration().z + ) + + simulator_state.imu.gyroscope = vec3( + self.imu.get_angular_velocity().x, + self.imu.get_angular_velocity().y, + self.imu.get_angular_velocity().z + ) + + simulator_state.gps.from_xy([self.vehicle.get_location().x, self.vehicle.get_location().y]) + + simulator_state.velocity = self.vehicle.get_velocity() + simulator_state.valid = True + simulator_state.steering_angle = self.vc.steer * self.max_steer_angle + + def read_cameras(self): + pass # cameras are read within a callback for carla + + def tick(self): + self.world.tick() + + +class CarlaBridge(SimulatorBridge): + TICKS_PER_FRAME = 5 + + def __init__(self, arguments): + super().__init__(arguments) + self.host = arguments.host + self.port = arguments.port + self.town = arguments.town + self.num_selected_spawn_point = arguments.num_selected_spawn_point + + def spawn_world(self): + 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.host, self.port) + world = client.load_world(self.town) + + settings = world.get_settings() + #settings.synchronous_mode = True # Enables synchronous mode + settings.fixed_delta_seconds = 0.01 + world.apply_settings(settings) + + world.set_weather(carla.WeatherParameters.ClearSunset) + + if not self.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.num_selected_spawn_point, \ + f'''No spawn point {self.num_selected_spawn_point}, try a value between 0 and {len(spawn_points)} for this town.''' + spawn_point = spawn_points[self.num_selected_spawn_point] + vehicle = world.spawn_actor(vehicle_bp, spawn_point) + + physics_control = vehicle.get_physics_control() + physics_control.mass = 2326 + 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) + + return CarlaWorld(world, vehicle, dual_camera=self.dual_camera) + + def close(self): + super().close() \ 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..6949be29f0e252 --- /dev/null +++ b/tools/sim/bridge/common.py @@ -0,0 +1,140 @@ +import signal +import threading + +from multiprocessing import Process, Queue +from abc import ABC, abstractmethod + +import cereal.messaging as messaging +from openpilot.common.params import Params + +from openpilot.common.numpy_fast import clip +from openpilot.common.realtime import Ratekeeper +from openpilot.selfdrive.test.helpers import set_params_enabled +from openpilot.selfdrive.car.honda.values import CruiseButtons +from openpilot.tools.sim.lib.common import SimulatorState, World +from openpilot.tools.sim.lib.simulated_car import SimulatedCar +from openpilot.tools.sim.lib.simulated_sensors import SimulatedSensors + + +class SimulatorBridge(ABC): + TICKS_PER_FRAME = 5 + + def __init__(self, arguments): + 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()) + + self.dual_camera = arguments.dual_camera + self.high_quality = arguments.high_quality + + 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() + self.simulator_state = SimulatorState() + + def _on_shutdown(self, signal, frame): + self._keep_alive = False + + def bridge_keep_alive(self, q: Queue, retries: int): + try: + self._run(q) + finally: + self.close() + + def close(self): + self.started = False + self._exit_event.set() + + 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 + + @abstractmethod + def spawn_world(self) -> World: + pass + + def _run(self, q: Queue): + self.world = world = self.spawn_world() + self.simulated_car = SimulatedCar() + self.simulated_sensors = SimulatedSensors(self.dual_camera) + + rk = Ratekeeper(100, print_delay_threshold=None) + + # Simulation tends to be slow in the initial steps. This prevents lagging later + for _ in range(20): + world.tick() + + throttle_manual = steer_manual = brake_manual = 0. + + # loop + 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 + + throttle_out = steer_out = brake_out = 0.0 + throttle_op = steer_op = brake_op = 0.0 + + self.simulator_state.cruise_button = 0 + + throttle_manual = steer_manual = brake_manual = 0. + + # --------------Step 1------------------------------- + if not q.empty(): + message = q.get() + m = message.split('_') + if m[0] == "steer": + steer_manual = float(m[1]) + elif m[0] == "throttle": + throttle_manual = float(m[1]) + elif m[0] == "brake": + brake_manual = float(m[1]) + elif m[0] == "cruise": + if m[1] == "down": + self.simulator_state.cruise_button = CruiseButtons.DECEL_SET + elif m[1] == "up": + self.simulator_state.cruise_button = CruiseButtons.RES_ACCEL + elif m[1] == "cancel": + self.simulator_state.cruise_button = CruiseButtons.CANCEL + elif m[1] == "main": + self.simulator_state.cruise_button = CruiseButtons.MAIN + elif m[0] == "ignition": + self.simulator_state.ignition = not self.simulator_state.ignition + elif m[0] == "quit": + break + + self.simulator_state.user_brake = brake_manual + self.simulator_state.user_gas = throttle_manual + + steer_manual = steer_manual * -40 + + self.simulated_car.update(self.simulator_state) + self.simulated_sensors.update(self.simulator_state, self.world) + + is_openpilot_engaged = self.simulated_car.sm['controlsState'].active + + if is_openpilot_engaged: + throttle_op = clip(self.simulated_car.sm['carControl'].actuators.accel / 1.6, 0.0, 1.0) + brake_op = clip(-self.simulated_car.sm['carControl'].actuators.accel / 4.0, 0.0, 1.0) + steer_op = self.simulated_car.sm['carControl'].actuators.steeringAngleDeg + + throttle_out = throttle_op if is_openpilot_engaged else throttle_manual + brake_out = brake_op if is_openpilot_engaged else brake_manual + steer_out = steer_op if is_openpilot_engaged else steer_manual + + world.apply_controls(steer_out, throttle_out, brake_out) + world.read_sensors(self.simulator_state) + + if rk.frame % self.TICKS_PER_FRAME == 0: + world.read_cameras() + world.tick() + + self.started = True diff --git a/tools/sim/lib/camerad.py b/tools/sim/lib/camerad.py new file mode 100644 index 00000000000000..75f12d6cb89018 --- /dev/null +++ b/tools/sim/lib/camerad.py @@ -0,0 +1,69 @@ +import numpy as np +import os +import pyopencl as cl +import pyopencl.array as cl_array +from openpilot.common.basedir import BASEDIR + +from cereal.visionipc import VisionIpcServer, VisionStreamType +from cereal import messaging +from openpilot.tools.sim.lib.common import W, H + +class Camerad: + """Simulates the camerad daemon""" + def __init__(self, dual_camera): + self.pm = messaging.PubMaster(['roadCameraState', 'wideRoadCameraState']) + + 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) + if dual_camera: + 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_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 + + # Returns: yuv bytes + def rgb_to_yuv(self, rgb): + assert rgb.shape == (H, W, 3), f"{rgb.shape}" + assert rgb.dtype == np.uint8 + + rgb_cl = cl_array.to_device(self.queue, rgb) + yuv_cl = cl_array.empty_like(rgb_cl) + 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() + + 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) + self.pm.send(pub_type, dat) \ No newline at end of file diff --git a/tools/sim/lib/can.py b/tools/sim/lib/can.py deleted file mode 100755 index db321d31935c78..00000000000000 --- a/tools/sim/lib/can.py +++ /dev/null @@ -1,94 +0,0 @@ -#!/usr/bin/env python3 -import cereal.messaging as messaging -from opendbc.can.packer import CANPacker -from opendbc.can.parser import CANParser -from openpilot.selfdrive.boardd.boardd_api_impl import can_list_to_can_capnp -from openpilot.selfdrive.car import crc8_pedal - -packer = CANPacker("honda_civic_touring_2016_can_generated") -rpacker = CANPacker("acura_ilx_2016_nidec") - - -def get_car_can_parser(): - dbc_f = 'honda_civic_touring_2016_can_generated' - checks = [ - (0xe4, 100), - (0x1fa, 50), - (0x200, 50), - ] - return CANParser(dbc_f, checks, 0) -cp = get_car_can_parser() - -def can_function(pm, speed, angle, idx, cruise_button, is_engaged): - - msg = [] - - # *** powertrain bus *** - - speed = speed * 3.6 # convert m/s to kph - msg.append(packer.make_can_msg("ENGINE_DATA", 0, {"XMISSION_SPEED": speed})) - msg.append(packer.make_can_msg("WHEEL_SPEEDS", 0, { - "WHEEL_SPEED_FL": speed, - "WHEEL_SPEED_FR": speed, - "WHEEL_SPEED_RL": speed, - "WHEEL_SPEED_RR": speed - })) - - msg.append(packer.make_can_msg("SCM_BUTTONS", 0, {"CRUISE_BUTTONS": cruise_button})) - - values = {"COUNTER_PEDAL": idx & 0xF} - checksum = crc8_pedal(packer.make_can_msg("GAS_SENSOR", 0, {"COUNTER_PEDAL": idx & 0xF})[2][:-1]) - values["CHECKSUM_PEDAL"] = checksum - msg.append(packer.make_can_msg("GAS_SENSOR", 0, values)) - - msg.append(packer.make_can_msg("GEARBOX", 0, {"GEAR": 4, "GEAR_SHIFTER": 8})) - msg.append(packer.make_can_msg("GAS_PEDAL_2", 0, {})) - msg.append(packer.make_can_msg("SEATBELT_STATUS", 0, {"SEATBELT_DRIVER_LATCHED": 1})) - msg.append(packer.make_can_msg("STEER_STATUS", 0, {})) - msg.append(packer.make_can_msg("STEERING_SENSORS", 0, {"STEER_ANGLE": angle})) - msg.append(packer.make_can_msg("VSA_STATUS", 0, {})) - msg.append(packer.make_can_msg("STANDSTILL", 0, {"WHEELS_MOVING": 1 if speed >= 1.0 else 0})) - msg.append(packer.make_can_msg("STEER_MOTOR_TORQUE", 0, {})) - msg.append(packer.make_can_msg("EPB_STATUS", 0, {})) - msg.append(packer.make_can_msg("DOORS_STATUS", 0, {})) - msg.append(packer.make_can_msg("CRUISE_PARAMS", 0, {})) - msg.append(packer.make_can_msg("CRUISE", 0, {})) - msg.append(packer.make_can_msg("SCM_FEEDBACK", 0, {"MAIN_ON": 1})) - msg.append(packer.make_can_msg("POWERTRAIN_DATA", 0, {"ACC_STATUS": int(is_engaged)})) - msg.append(packer.make_can_msg("HUD_SETTING", 0, {})) - msg.append(packer.make_can_msg("CAR_SPEED", 0, {})) - - # *** cam bus *** - msg.append(packer.make_can_msg("STEERING_CONTROL", 2, {})) - msg.append(packer.make_can_msg("ACC_HUD", 2, {})) - msg.append(packer.make_can_msg("LKAS_HUD", 2, {})) - msg.append(packer.make_can_msg("BRAKE_COMMAND", 2, {})) - - # *** radar bus *** - if idx % 5 == 0: - msg.append(rpacker.make_can_msg("RADAR_DIAGNOSTIC", 1, {"RADAR_STATE": 0x79})) - for i in range(16): - msg.append(rpacker.make_can_msg("TRACK_%d" % i, 1, {"LONG_DIST": 255.5})) - - pm.send('can', can_list_to_can_capnp(msg)) - -def sendcan_function(sendcan): - sc = messaging.drain_sock_raw(sendcan) - cp.update_strings(sc, sendcan=True) - - if cp.vl[0x1fa]['COMPUTER_BRAKE_REQUEST']: - brake = cp.vl[0x1fa]['COMPUTER_BRAKE'] / 1024. - else: - brake = 0.0 - - if cp.vl[0x200]['GAS_COMMAND'] > 0: - gas = ( cp.vl[0x200]['GAS_COMMAND'] + 83.3 ) / (0.253984064 * 2**16) - else: - gas = 0.0 - - if cp.vl[0xe4]['STEER_TORQUE_REQUEST']: - steer_torque = cp.vl[0xe4]['STEER_TORQUE']/3840 - else: - steer_torque = 0.0 - - return gas, brake, steer_torque diff --git a/tools/sim/lib/common.py b/tools/sim/lib/common.py new file mode 100644 index 00000000000000..7f36f495106a72 --- /dev/null +++ b/tools/sim/lib/common.py @@ -0,0 +1,81 @@ +import math +import threading +import numpy as np + +from abc import ABC, abstractmethod +from collections import namedtuple + +W, H = 1928, 1208 + + +vec3 = namedtuple("vec3", ["x", "y", "z"]) + +class GPSState: + def __init__(self): + self.latitude = 0 + self.longitude = 0 + self.altitude = 0 + + def from_xy(self, xy): + """Simulates a lat/lon from an xy coordinate on a plane, for simple simlation. TODO: proper global projection?""" + BASE_LAT = 32.75308505188913 + BASE_LON = -117.2095393365393 + DEG_TO_METERS = 100000 + + self.latitude = float(BASE_LAT + xy[0] / DEG_TO_METERS) + self.longitude = float(BASE_LON + xy[1] / DEG_TO_METERS) + self.altitude = 0 + + +class IMUState: + def __init__(self): + self.accelerometer: vec3 = vec3(0,0,0) + self.gyroscope: vec3 = vec3(0,0,0) + self.bearing: float = 0 + + +class SimulatorState: + def __init__(self): + self.valid = False + self.is_engaged = False + self.ignition = True + + self.velocity: vec3 = None + self.bearing: float = 0 + self.gps = GPSState() + self.imu = IMUState() + + self.steering_angle: float = 0 + + self.user_gas: float = 0 + self.user_brake: float = 0 + + self.cruise_button = 0 + + @property + def speed(self): + return math.sqrt(self.velocity.x ** 2 + self.velocity.y ** 2 + self.velocity.z ** 2) + +class World(ABC): + def __init__(self, dual_camera): + self.dual_camera = dual_camera + + self.image_lock = threading.Lock() + self.road_image = np.zeros((H, W, 3), dtype=np.uint8) + self.wide_road_image = np.zeros((H, W, 3), dtype=np.uint8) + + @abstractmethod + def apply_controls(self, steer_sim, throttle_out, brake_out): + pass + + @abstractmethod + def tick(self): + pass + + @abstractmethod + def read_sensors(self, simulator_state: SimulatorState): + pass + + @abstractmethod + def read_cameras(self): + pass \ No newline at end of file diff --git a/tools/sim/lib/simulated_car.py b/tools/sim/lib/simulated_car.py new file mode 100644 index 00000000000000..54002aba9fc781 --- /dev/null +++ b/tools/sim/lib/simulated_car.py @@ -0,0 +1,112 @@ +import cereal.messaging as messaging +from opendbc.can.packer import CANPacker +from opendbc.can.parser import CANParser +from openpilot.selfdrive.boardd.boardd_api_impl import can_list_to_can_capnp +from openpilot.selfdrive.car import crc8_pedal +from openpilot.tools.sim.lib.common import SimulatorState + + +class SimulatedCar: + """Simulates a honda civic 2016 (panda state + can messages) to OpenPilot""" + packer = CANPacker("honda_civic_touring_2016_can_generated") + rpacker = CANPacker("acura_ilx_2016_nidec") + + def __init__(self): + self.pm = messaging.PubMaster(['can', 'pandaStates']) + self.sm = messaging.SubMaster(['carControl', 'controlsState']) + self.cp = self.get_car_can_parser() + self.idx = 0 + + @staticmethod + def get_car_can_parser(): + dbc_f = 'honda_civic_touring_2016_can_generated' + checks = [ + (0xe4, 100), + (0x1fa, 50), + (0x200, 50), + ] + return CANParser(dbc_f, checks, 0) + + def send_can_messages(self, simulator_state: SimulatorState): + if not simulator_state.valid: + return + + msg = [] + + # *** powertrain bus *** + + speed = simulator_state.speed * 3.6 # convert m/s to kph + msg.append(self.packer.make_can_msg("ENGINE_DATA", 0, {"XMISSION_SPEED": speed})) + msg.append(self.packer.make_can_msg("WHEEL_SPEEDS", 0, { + "WHEEL_SPEED_FL": speed, + "WHEEL_SPEED_FR": speed, + "WHEEL_SPEED_RL": speed, + "WHEEL_SPEED_RR": speed + })) + + msg.append(self.packer.make_can_msg("SCM_BUTTONS", 0, {"CRUISE_BUTTONS": simulator_state.cruise_button})) + + values = { + "COUNTER_PEDAL": self.idx & 0xF, + "INTERCEPTOR_GAS": simulator_state.user_gas * 2**12, + "INTERCEPTOR_GAS2": simulator_state.user_gas * 2**12, + } + checksum = crc8_pedal(self.packer.make_can_msg("GAS_SENSOR", 0, values)[2][:-1]) + values["CHECKSUM_PEDAL"] = checksum + msg.append(self.packer.make_can_msg("GAS_SENSOR", 0, values)) + + msg.append(self.packer.make_can_msg("GEARBOX", 0, {"GEAR": 4, "GEAR_SHIFTER": 8})) + msg.append(self.packer.make_can_msg("GAS_PEDAL_2", 0, {})) + msg.append(self.packer.make_can_msg("SEATBELT_STATUS", 0, {"SEATBELT_DRIVER_LATCHED": 1})) + msg.append(self.packer.make_can_msg("STEER_STATUS", 0, {})) + msg.append(self.packer.make_can_msg("STEERING_SENSORS", 0, {"STEER_ANGLE": simulator_state.steering_angle})) + msg.append(self.packer.make_can_msg("VSA_STATUS", 0, {})) + msg.append(self.packer.make_can_msg("STANDSTILL", 0, {"WHEELS_MOVING": 1 if simulator_state.speed >= 1.0 else 0})) + msg.append(self.packer.make_can_msg("STEER_MOTOR_TORQUE", 0, {})) + msg.append(self.packer.make_can_msg("EPB_STATUS", 0, {})) + msg.append(self.packer.make_can_msg("DOORS_STATUS", 0, {})) + msg.append(self.packer.make_can_msg("CRUISE_PARAMS", 0, {})) + msg.append(self.packer.make_can_msg("CRUISE", 0, {})) + msg.append(self.packer.make_can_msg("SCM_FEEDBACK", 0, {"MAIN_ON": 1})) + msg.append(self.packer.make_can_msg("POWERTRAIN_DATA", 0, + { + "ACC_STATUS": int(simulator_state.is_engaged), + "PEDAL_GAS": simulator_state.user_gas, + "BRAKE_PRESSED": simulator_state.user_brake > 0 + })) + msg.append(self.packer.make_can_msg("HUD_SETTING", 0, {})) + msg.append(self.packer.make_can_msg("CAR_SPEED", 0, {})) + + # *** cam bus *** + msg.append(self.packer.make_can_msg("STEERING_CONTROL", 2, {})) + msg.append(self.packer.make_can_msg("ACC_HUD", 2, {})) + msg.append(self.packer.make_can_msg("LKAS_HUD", 2, {})) + msg.append(self.packer.make_can_msg("BRAKE_COMMAND", 2, {})) + + # *** radar bus *** + if self.idx % 5 == 0: + msg.append(self.rpacker.make_can_msg("RADAR_DIAGNOSTIC", 1, {"RADAR_STATE": 0x79})) + for i in range(16): + msg.append(self.rpacker.make_can_msg("TRACK_%d" % i, 1, {"LONG_DIST": 255.5})) + + self.pm.send('can', can_list_to_can_capnp(msg)) + + def send_panda_state(self, simulator_state): + dat = messaging.new_message('pandaStates', 1) + dat.valid = True + dat.pandaStates[0] = { + 'ignitionLine': simulator_state.ignition, + 'pandaType': "blackPanda", + 'controlsAllowed': True, + 'safetyModel': 'hondaNidec', + } + self.pm.send('pandaStates', dat) + + def update(self, simulator_state: SimulatorState): + for _ in range(2): + self.send_can_messages(simulator_state) + self.send_panda_state(simulator_state) + + self.idx += 1 + + self.sm.update(0) diff --git a/tools/sim/lib/simulated_sensors.py b/tools/sim/lib/simulated_sensors.py new file mode 100644 index 00000000000000..c0bdf260359372 --- /dev/null +++ b/tools/sim/lib/simulated_sensors.py @@ -0,0 +1,128 @@ +import time +import cereal.messaging as messaging + +from openpilot.common.params import Params +from cereal import log + +from openpilot.common.realtime import DT_DMON + +from openpilot.tools.sim.lib.camerad import Camerad + +from typing import TYPE_CHECKING +if TYPE_CHECKING: + from openpilot.tools.sim.lib.common import World, SimulatorState + + +class SimulatedSensors: + """Simulates the C3 sensors (acc, gyro, gps, peripherals, dm state, cameras) to OpenPilot""" + + def __init__(self, dual_camera=False): + self.pm = messaging.PubMaster(['accelerometer', 'gyroscope', 'gpsLocationExternal', 'driverStateV2', 'driverMonitoringState', 'peripheralState']) + self.camerad = Camerad(dual_camera=dual_camera) + self.last_perp_update = 0 + self.last_dmon_update = 0 + + def send_imu_message(self, simulator_state: 'SimulatorState'): + for _ in range(5): + 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 = [simulator_state.imu.accelerometer.x, simulator_state.imu.accelerometer.y, simulator_state.imu.accelerometer.z] + self.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 = [simulator_state.imu.gyroscope.x, simulator_state.imu.gyroscope.y, simulator_state.imu.gyroscope.z] + self.pm.send('gyroscope', dat) + + def send_gps_message(self, simulator_state: 'SimulatorState'): + if not simulator_state.valid: + return + + # transform vel from carla to NED + # north is -Y in CARLA + velNED = [ + -simulator_state.velocity.y, # north/south component of NED is negative when moving south + simulator_state.velocity.x, # positive when moving east, which is x in carla + simulator_state.velocity.z, + ] + + for _ in range(10): + dat = messaging.new_message('gpsLocationExternal') + 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": simulator_state.imu.bearing, + "latitude": simulator_state.gps.latitude, + "longitude": simulator_state.gps.longitude, + "altitude": simulator_state.gps.altitude, + "speed": simulator_state.speed, + "source": log.GpsLocationData.SensorSource.ublox, + } + + self.pm.send('gpsLocationExternal', dat) + + def send_peripheral_state(self): + dat = messaging.new_message('peripheralState') + dat.valid = True + dat.peripheralState = { + 'pandaType': log.PandaState.PandaType.blackPanda, + 'voltage': 12000, + 'current': 5678, + 'fanSpeedRpm': 1000 + } + Params().put_bool("ObdMultiplexingEnabled", False) + self.pm.send('peripheralState', dat) + + def send_fake_driver_monitoring(self): + # 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 + self.pm.send('driverStateV2', dat) + + # dmonitoringd output + dat = messaging.new_message('driverMonitoringState') + dat.driverMonitoringState = { + "faceDetected": True, + "isDistracted": False, + "awarenessStatus": 1., + } + self.pm.send('driverMonitoringState', dat) + + def send_camera_images(self, world: 'World'): + with world.image_lock: + yuv = self.camerad.rgb_to_yuv(world.road_image) + self.camerad.cam_send_yuv_road(yuv) + + if world.dual_camera: + yuv = self.camerad.rgb_to_yuv(world.wide_road_image) + self.camerad.cam_send_yuv_wide_road(yuv) + + def update(self, simulator_state: 'SimulatorState', world: 'World'): + now = time.time() + self.send_imu_message(simulator_state) + self.send_gps_message(simulator_state) + + if (now - self.last_dmon_update) > DT_DMON/2: + self.send_fake_driver_monitoring() + self.last_dmon_update = now + + if (now - self.last_perp_update) > 0.25: + self.send_peripheral_state() + self.last_perp_update = now + + self.send_camera_images(world) \ No newline at end of file diff --git a/tools/sim/run_bridge.py b/tools/sim/run_bridge.py new file mode 100755 index 00000000000000..a79eea5a073f64 --- /dev/null +++ b/tools/sim/run_bridge.py @@ -0,0 +1,47 @@ +#!/usr/bin/env python +import argparse + +from typing import Any +from multiprocessing import Queue + +from openpilot.tools.sim.bridge.common import SimulatorBridge +from openpilot.tools.sim.bridge.carla import CarlaBridge + + +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('--simulator', dest='simulator', type=str, default='carla') + + # Carla specific + 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) + + return parser.parse_args(add_args) + +if __name__ == "__main__": + q: Any = Queue() + args = parse_args() + + simulator_bridge: SimulatorBridge + if 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 openpilot.tools.sim.lib.manual_ctrl import wheel_poll_thread + + wheel_poll_thread(q) + else: + # start input poll for keyboard + from openpilot.tools.sim.lib.keyboard_ctrl import keyboard_poll_thread + + keyboard_poll_thread(q) + p.join() diff --git a/tools/sim/tests/test_carla_integration.py b/tools/sim/tests/test_carla_integration.py index 65f5f4a30f0621..b5a60ee0e36798 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 openpilot.common.basedir import BASEDIR from openpilot.selfdrive.manager.helpers import unblock_stdout -from openpilot.tools.sim import bridge -from openpilot.tools.sim.bridge import CarlaBridge +from openpilot.tools.sim.run_bridge import parse_args +from openpilot.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) From 60138e7e198914a6c0cc39be95a0d18218f62d8b Mon Sep 17 00:00:00 2001 From: Justin Newberry Date: Thu, 14 Sep 2023 14:46:44 -0700 Subject: [PATCH 2/8] fix carla --- selfdrive/controls/controlsd.py | 2 +- tools/sim/bridge/carla.py | 4 ++-- tools/sim/lib/common.py | 1 + 3 files changed, 4 insertions(+), 3 deletions(-) diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 1dc9dd39f788cb..8c2c5180dd0ec8 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -339,7 +339,7 @@ def update_events(self, CS): self.events.add(EventName.cameraMalfunction) elif not self.sm.all_freq_ok(self.camera_packets): self.events.add(EventName.cameraFrameRate) - if not REPLAY and self.rk.lagging: + if not REPLAY and not SIMULATION and self.rk.lagging: self.events.add(EventName.controlsdLagging) if len(self.sm['radarState'].radarErrors) or (not self.rk.lagging and not self.sm.all_checks(['radarState'])): self.events.add(EventName.radarFault) diff --git a/tools/sim/bridge/carla.py b/tools/sim/bridge/carla.py index ceb3c4652e2caa..56c5300837fe76 100644 --- a/tools/sim/bridge/carla.py +++ b/tools/sim/bridge/carla.py @@ -67,11 +67,11 @@ def carla_image_to_rgb(self, image): return np.ascontiguousarray(rgb[:, :, [0, 1, 2]]) def cam_callback_road(self, image): - with self.world.image_lock: + with self.image_lock: self.road_image = self.carla_image_to_rgb(image) def cam_callback_wide_road(self, image): - with self.world.image_lock: + with self.image_lock: self.wide_road_image = self.carla_image_to_rgb(image) def apply_controls(self, steer_angle, throttle_out, brake_out): diff --git a/tools/sim/lib/common.py b/tools/sim/lib/common.py index 7f36f495106a72..1a2f7070d68e08 100644 --- a/tools/sim/lib/common.py +++ b/tools/sim/lib/common.py @@ -56,6 +56,7 @@ def __init__(self): def speed(self): return math.sqrt(self.velocity.x ** 2 + self.velocity.y ** 2 + self.velocity.z ** 2) + class World(ABC): def __init__(self, dual_camera): self.dual_camera = dual_camera From ba3d92d0da4027bbf70f930a194945b76b2b3dc6 Mon Sep 17 00:00:00 2001 From: Justin Newberry Date: Thu, 14 Sep 2023 15:05:55 -0700 Subject: [PATCH 3/8] remove that exception --- selfdrive/controls/controlsd.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 8c2c5180dd0ec8..1dc9dd39f788cb 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -339,7 +339,7 @@ def update_events(self, CS): self.events.add(EventName.cameraMalfunction) elif not self.sm.all_freq_ok(self.camera_packets): self.events.add(EventName.cameraFrameRate) - if not REPLAY and not SIMULATION and self.rk.lagging: + if not REPLAY and self.rk.lagging: self.events.add(EventName.controlsdLagging) if len(self.sm['radarState'].radarErrors) or (not self.rk.lagging and not self.sm.all_checks(['radarState'])): self.events.add(EventName.radarFault) From 782705c75147bd1020b4867e666ec434dac148bd Mon Sep 17 00:00:00 2001 From: Justin Newberry Date: Thu, 14 Sep 2023 15:28:44 -0700 Subject: [PATCH 4/8] pr cleanup --- tools/sim/bridge/carla.py | 16 +++++----------- tools/sim/bridge/common.py | 7 ++++++- tools/sim/lib/common.py | 4 ++++ 3 files changed, 15 insertions(+), 12 deletions(-) diff --git a/tools/sim/bridge/carla.py b/tools/sim/bridge/carla.py index 56c5300837fe76..b96a6b268f5fc3 100644 --- a/tools/sim/bridge/carla.py +++ b/tools/sim/bridge/carla.py @@ -1,4 +1,5 @@ import numpy as np + from openpilot.common.params import Params from openpilot.tools.sim.lib.common import SimulatorState, vec3 from openpilot.tools.sim.bridge.common import World, SimulatorBridge @@ -123,17 +124,13 @@ def __init__(self, arguments): self.num_selected_spawn_point = arguments.num_selected_spawn_point def spawn_world(self): - 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 + import carla + client = carla.Client(self.host, self.port) + client.set_timeout(5) - client = connect_carla_client(self.host, self.port) world = client.load_world(self.town) settings = world.get_settings() - #settings.synchronous_mode = True # Enables synchronous mode settings.fixed_delta_seconds = 0.01 world.apply_settings(settings) @@ -165,7 +162,4 @@ def connect_carla_client(host: str, port: int): physics_control.gear_switch_time = 0.0 vehicle.apply_physics_control(physics_control) - return CarlaWorld(world, vehicle, dual_camera=self.dual_camera) - - def close(self): - super().close() \ No newline at end of file + return CarlaWorld(world, vehicle, dual_camera=self.dual_camera) \ No newline at end of file diff --git a/tools/sim/bridge/common.py b/tools/sim/bridge/common.py index 6949be29f0e252..e2ffc54a26b848 100644 --- a/tools/sim/bridge/common.py +++ b/tools/sim/bridge/common.py @@ -3,10 +3,10 @@ from multiprocessing import Process, Queue from abc import ABC, abstractmethod +from typing import Optional import cereal.messaging as messaging from openpilot.common.params import Params - from openpilot.common.numpy_fast import clip from openpilot.common.realtime import Ratekeeper from openpilot.selfdrive.test.helpers import set_params_enabled @@ -39,6 +39,8 @@ def __init__(self, arguments): self._exit = threading.Event() self.simulator_state = SimulatorState() + self.world: Optional[World] = None + def _on_shutdown(self, signal, frame): self._keep_alive = False @@ -52,6 +54,9 @@ def close(self): self.started = False self._exit_event.set() + if self.world is not None: + self.world.close() + def run(self, queue, retries=-1): bridge_p = Process(target=self.bridge_keep_alive, args=(queue, retries), daemon=True) bridge_p.start() diff --git a/tools/sim/lib/common.py b/tools/sim/lib/common.py index 1a2f7070d68e08..3de89f39fca8b3 100644 --- a/tools/sim/lib/common.py +++ b/tools/sim/lib/common.py @@ -79,4 +79,8 @@ def read_sensors(self, simulator_state: SimulatorState): @abstractmethod def read_cameras(self): + pass + + @abstractmethod + def close(self): pass \ No newline at end of file From ac5fd71cf0462893308a4f1a7b7521f6b4e73f8f Mon Sep 17 00:00:00 2001 From: Justin Newberry Date: Thu, 14 Sep 2023 16:00:45 -0700 Subject: [PATCH 5/8] update car in a thread --- tools/sim/bridge/common.py | 25 ++++++++++++++++++------- tools/sim/lib/simulated_car.py | 7 +++---- 2 files changed, 21 insertions(+), 11 deletions(-) diff --git a/tools/sim/bridge/common.py b/tools/sim/bridge/common.py index e2ffc54a26b848..cd74267ff475b6 100644 --- a/tools/sim/bridge/common.py +++ b/tools/sim/bridge/common.py @@ -1,5 +1,7 @@ import signal import threading +import time +import functools from multiprocessing import Process, Queue from abc import ABC, abstractmethod @@ -16,6 +18,12 @@ from openpilot.tools.sim.lib.simulated_sensors import SimulatedSensors +def loop(function, dt): + while True: + function() + time.sleep(dt) + + class SimulatorBridge(ABC): TICKS_PER_FRAME = 5 @@ -67,15 +75,19 @@ def spawn_world(self) -> World: pass def _run(self, q: Queue): - self.world = world = self.spawn_world() + self.world = self.spawn_world() + self.simulated_car = SimulatedCar() self.simulated_sensors = SimulatedSensors(self.dual_camera) + self.simulated_car_thread = threading.Thread(target=loop, args=(functools.partial(self.simulated_car.update, self.simulator_state), 0.01)) + self.simulated_car_thread.start() + rk = Ratekeeper(100, print_delay_threshold=None) # Simulation tends to be slow in the initial steps. This prevents lagging later for _ in range(20): - world.tick() + self.world.tick() throttle_manual = steer_manual = brake_manual = 0. @@ -121,7 +133,6 @@ def _run(self, q: Queue): steer_manual = steer_manual * -40 - self.simulated_car.update(self.simulator_state) self.simulated_sensors.update(self.simulator_state, self.world) is_openpilot_engaged = self.simulated_car.sm['controlsState'].active @@ -135,11 +146,11 @@ def _run(self, q: Queue): brake_out = brake_op if is_openpilot_engaged else brake_manual steer_out = steer_op if is_openpilot_engaged else steer_manual - world.apply_controls(steer_out, throttle_out, brake_out) - world.read_sensors(self.simulator_state) + self.world.apply_controls(steer_out, throttle_out, brake_out) + self.world.read_sensors(self.simulator_state) if rk.frame % self.TICKS_PER_FRAME == 0: - world.read_cameras() - world.tick() + self.world.read_cameras() + self.world.tick() self.started = True diff --git a/tools/sim/lib/simulated_car.py b/tools/sim/lib/simulated_car.py index 54002aba9fc781..bbfae237319df7 100644 --- a/tools/sim/lib/simulated_car.py +++ b/tools/sim/lib/simulated_car.py @@ -103,10 +103,9 @@ def send_panda_state(self, simulator_state): self.pm.send('pandaStates', dat) def update(self, simulator_state: SimulatorState): - for _ in range(2): - self.send_can_messages(simulator_state) - self.send_panda_state(simulator_state) + self.send_can_messages(simulator_state) + self.send_panda_state(simulator_state) - self.idx += 1 + self.idx += 1 self.sm.update(0) From 75e1735649e20eca97d1ee5be2a0e44c4eca001f Mon Sep 17 00:00:00 2001 From: Justin Newberry Date: Fri, 15 Sep 2023 11:33:36 -0700 Subject: [PATCH 6/8] more cleanup --- tools/sim/bridge/carla.py | 2 -- tools/sim/bridge/common.py | 26 ++++++++++++++------------ tools/sim/lib/keyboard_ctrl.py | 1 - tools/sim/lib/simulated_car.py | 4 +--- 4 files changed, 15 insertions(+), 18 deletions(-) diff --git a/tools/sim/bridge/carla.py b/tools/sim/bridge/carla.py index b96a6b268f5fc3..df25734e19cb46 100644 --- a/tools/sim/bridge/carla.py +++ b/tools/sim/bridge/carla.py @@ -16,8 +16,6 @@ def __init__(self, world, vehicle, high_quality=False, dual_camera=False): self.max_steer_angle: float = vehicle.get_physics_control().wheels[0].max_steer_angle self.params = Params() - self.road_image = np.zeros((H, W, 3), dtype=np.uint8) - self.steer_ratio = 15 self.carla_objects = [] diff --git a/tools/sim/bridge/common.py b/tools/sim/bridge/common.py index cd74267ff475b6..9e1802ebcefc60 100644 --- a/tools/sim/bridge/common.py +++ b/tools/sim/bridge/common.py @@ -1,6 +1,5 @@ import signal import threading -import time import functools from multiprocessing import Process, Queue @@ -18,10 +17,11 @@ from openpilot.tools.sim.lib.simulated_sensors import SimulatedSensors -def loop(function, dt): +def rk_loop(function, hz): + rk = Ratekeeper(hz) while True: function() - time.sleep(dt) + rk.keep_time() class SimulatorBridge(ABC): @@ -31,6 +31,8 @@ def __init__(self, arguments): set_params_enabled() self.params = Params() + self.rk = Ratekeeper(100) + msg = messaging.new_message('liveCalibration') msg.liveCalibration.validBlocks = 20 msg.liveCalibration.rpyCalib = [0.0, 0.0, 0.0] @@ -80,7 +82,7 @@ def _run(self, q: Queue): self.simulated_car = SimulatedCar() self.simulated_sensors = SimulatedSensors(self.dual_camera) - self.simulated_car_thread = threading.Thread(target=loop, args=(functools.partial(self.simulated_car.update, self.simulator_state), 0.01)) + self.simulated_car_thread = threading.Thread(target=rk_loop, args=(functools.partial(self.simulated_car.update, self.simulator_state), 100)) self.simulated_car_thread.start() rk = Ratekeeper(100, print_delay_threshold=None) @@ -91,12 +93,7 @@ def _run(self, q: Queue): throttle_manual = steer_manual = brake_manual = 0. - # loop 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 - throttle_out = steer_out = brake_out = 0.0 throttle_op = steer_op = brake_op = 0.0 @@ -104,7 +101,7 @@ def _run(self, q: Queue): throttle_manual = steer_manual = brake_manual = 0. - # --------------Step 1------------------------------- + # Read manual controls if not q.empty(): message = q.get() m = message.split('_') @@ -133,10 +130,13 @@ def _run(self, q: Queue): steer_manual = steer_manual * -40 + # Update openpilot on current sensor state self.simulated_sensors.update(self.simulator_state, self.world) is_openpilot_engaged = self.simulated_car.sm['controlsState'].active + self.simulated_car.sm.update(0) + if is_openpilot_engaged: throttle_op = clip(self.simulated_car.sm['carControl'].actuators.accel / 1.6, 0.0, 1.0) brake_op = clip(-self.simulated_car.sm['carControl'].actuators.accel / 4.0, 0.0, 1.0) @@ -149,8 +149,10 @@ def _run(self, q: Queue): self.world.apply_controls(steer_out, throttle_out, brake_out) self.world.read_sensors(self.simulator_state) - if rk.frame % self.TICKS_PER_FRAME == 0: - self.world.read_cameras() + if self.rk.frame % self.TICKS_PER_FRAME == 0: self.world.tick() + self.world.read_cameras() self.started = True + + rk.keep_time() diff --git a/tools/sim/lib/keyboard_ctrl.py b/tools/sim/lib/keyboard_ctrl.py index 803aa091a32b9b..cd69d9e229d9da 100644 --- a/tools/sim/lib/keyboard_ctrl.py +++ b/tools/sim/lib/keyboard_ctrl.py @@ -38,7 +38,6 @@ def getch() -> str: def keyboard_poll_thread(q: 'Queue[str]'): while True: c = getch() - print("got %s" % c) if c == '1': q.put("cruise_up") elif c == '2': diff --git a/tools/sim/lib/simulated_car.py b/tools/sim/lib/simulated_car.py index bbfae237319df7..951db6952436b2 100644 --- a/tools/sim/lib/simulated_car.py +++ b/tools/sim/lib/simulated_car.py @@ -106,6 +106,4 @@ def update(self, simulator_state: SimulatorState): self.send_can_messages(simulator_state) self.send_panda_state(simulator_state) - self.idx += 1 - - self.sm.update(0) + self.idx += 1 \ No newline at end of file From 6aae3739260a19447f8a62b305ef88d9537db26a Mon Sep 17 00:00:00 2001 From: Justin Newberry Date: Fri, 15 Sep 2023 11:35:42 -0700 Subject: [PATCH 7/8] import sorting --- tools/sim/bridge/common.py | 1 + tools/sim/lib/camerad.py | 3 ++- tools/sim/lib/keyboard_ctrl.py | 1 + tools/sim/lib/simulated_car.py | 1 + tools/sim/lib/simulated_sensors.py | 5 ++--- 5 files changed, 7 insertions(+), 4 deletions(-) diff --git a/tools/sim/bridge/common.py b/tools/sim/bridge/common.py index 9e1802ebcefc60..ee5efff082a12c 100644 --- a/tools/sim/bridge/common.py +++ b/tools/sim/bridge/common.py @@ -7,6 +7,7 @@ from typing import Optional import cereal.messaging as messaging + from openpilot.common.params import Params from openpilot.common.numpy_fast import clip from openpilot.common.realtime import Ratekeeper diff --git a/tools/sim/lib/camerad.py b/tools/sim/lib/camerad.py index 75f12d6cb89018..4621347787ca79 100644 --- a/tools/sim/lib/camerad.py +++ b/tools/sim/lib/camerad.py @@ -2,10 +2,11 @@ import os import pyopencl as cl import pyopencl.array as cl_array -from openpilot.common.basedir import BASEDIR from cereal.visionipc import VisionIpcServer, VisionStreamType from cereal import messaging + +from openpilot.common.basedir import BASEDIR from openpilot.tools.sim.lib.common import W, H class Camerad: diff --git a/tools/sim/lib/keyboard_ctrl.py b/tools/sim/lib/keyboard_ctrl.py index cd69d9e229d9da..57d5834026036c 100644 --- a/tools/sim/lib/keyboard_ctrl.py +++ b/tools/sim/lib/keyboard_ctrl.py @@ -1,6 +1,7 @@ import sys import termios import time + from termios import (BRKINT, CS8, CSIZE, ECHO, ICANON, ICRNL, IEXTEN, INPCK, ISTRIP, IXON, PARENB, VMIN, VTIME) from typing import NoReturn diff --git a/tools/sim/lib/simulated_car.py b/tools/sim/lib/simulated_car.py index 951db6952436b2..bbdbb9fcc48b71 100644 --- a/tools/sim/lib/simulated_car.py +++ b/tools/sim/lib/simulated_car.py @@ -1,4 +1,5 @@ import cereal.messaging as messaging + from opendbc.can.packer import CANPacker from opendbc.can.parser import CANParser from openpilot.selfdrive.boardd.boardd_api_impl import can_list_to_can_capnp diff --git a/tools/sim/lib/simulated_sensors.py b/tools/sim/lib/simulated_sensors.py index c0bdf260359372..dd55c02f02d3ef 100644 --- a/tools/sim/lib/simulated_sensors.py +++ b/tools/sim/lib/simulated_sensors.py @@ -1,11 +1,10 @@ import time -import cereal.messaging as messaging -from openpilot.common.params import Params from cereal import log +import cereal.messaging as messaging +from openpilot.common.params import Params from openpilot.common.realtime import DT_DMON - from openpilot.tools.sim.lib.camerad import Camerad from typing import TYPE_CHECKING From 5bc911672b3cb8c34147df500b38b65488225d23 Mon Sep 17 00:00:00 2001 From: Justin Newberry Date: Fri, 15 Sep 2023 12:10:16 -0700 Subject: [PATCH 8/8] handle exits better --- tools/sim/bridge/common.py | 10 +++++++--- tools/sim/run_bridge.py | 3 +++ 2 files changed, 10 insertions(+), 3 deletions(-) diff --git a/tools/sim/bridge/common.py b/tools/sim/bridge/common.py index ee5efff082a12c..464ea088232538 100644 --- a/tools/sim/bridge/common.py +++ b/tools/sim/bridge/common.py @@ -18,9 +18,9 @@ from openpilot.tools.sim.lib.simulated_sensors import SimulatedSensors -def rk_loop(function, hz): +def rk_loop(function, hz, exit_event: threading.Event): rk = Ratekeeper(hz) - while True: + while not exit_event.is_set(): function() rk.keep_time() @@ -53,6 +53,9 @@ def __init__(self, arguments): self.world: Optional[World] = None def _on_shutdown(self, signal, frame): + self.shutdown() + + def shutdown(self): self._keep_alive = False def bridge_keep_alive(self, q: Queue, retries: int): @@ -83,7 +86,8 @@ def _run(self, q: Queue): self.simulated_car = SimulatedCar() self.simulated_sensors = SimulatedSensors(self.dual_camera) - self.simulated_car_thread = threading.Thread(target=rk_loop, args=(functools.partial(self.simulated_car.update, self.simulator_state), 100)) + self.simulated_car_thread = threading.Thread(target=rk_loop, args=(functools.partial(self.simulated_car.update, self.simulator_state), + 100, self._exit_event)) self.simulated_car_thread.start() rk = Ratekeeper(100, print_delay_threshold=None) diff --git a/tools/sim/run_bridge.py b/tools/sim/run_bridge.py index a79eea5a073f64..8e6dc8f2ef28db 100755 --- a/tools/sim/run_bridge.py +++ b/tools/sim/run_bridge.py @@ -44,4 +44,7 @@ def parse_args(add_args=None): from openpilot.tools.sim.lib.keyboard_ctrl import keyboard_poll_thread keyboard_poll_thread(q) + + simulator_bridge.shutdown() + p.join()