diff --git a/tools/sim/__init__.py b/tools/sim/__init__.py new file mode 100644 index 00000000000000..e69de29bb2d1d6 diff --git a/tools/sim/bridge.py b/tools/sim/bridge.py index 71d7a204897198..a6f4672e68df4e 100755 --- a/tools/sim/bridge.py +++ b/tools/sim/bridge.py @@ -1,9 +1,10 @@ #!/usr/bin/env python3 import argparse import math +import os +import signal import threading import time -import os from multiprocessing import Process, Queue from typing import Any @@ -12,8 +13,6 @@ import pyopencl as cl import pyopencl.array as cl_array -from lib.can import can_function - import cereal.messaging as messaging from cereal import log from cereal.visionipc.visionipc_pyx import VisionIpcServer, VisionStreamType # pylint: disable=no-name-in-module, import-error @@ -22,15 +21,9 @@ from common.params import Params from common.realtime import DT_DMON, Ratekeeper from selfdrive.car.honda.values import CruiseButtons +from selfdrive.manager.helpers import unblock_stdout from selfdrive.test.helpers import set_params_enabled - -parser = argparse.ArgumentParser(description='Bridge between CARLA and openpilot.') -parser.add_argument('--joystick', action='store_true') -parser.add_argument('--low_quality', 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) - -args = parser.parse_args() +from tools.sim.lib.can import can_function W, H = 1928, 1208 REPEAT_COUNTER = 5 @@ -41,6 +34,16 @@ 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('--low_quality', 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) + + return parser.parse_args(add_args) + + class VehicleState: def __init__(self): self.speed = 0 @@ -80,11 +83,13 @@ def __init__(self): # 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 " + 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 " # TODO: move rgb_to_yuv.cl to local dir once the frame stream camera is removed kernel_fn = os.path.join(BASEDIR, "selfdrive", "camerad", "transforms", "rgb_to_yuv.cl") - prg = cl.Program(self.ctx, open(kernel_fn).read()).build(cl_arg) + self._kernel_file = open(kernel_fn) + + prg = cl.Program(self.ctx, self._kernel_file.read()).build(cl_arg) self.krnl = prg.rgb_to_yuv 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 @@ -126,6 +131,9 @@ def _cam_callback(self, image, frame_id, pub_type, rgb_type, yuv_type): setattr(dat, pub_type, msg) pm.send(pub_type, dat) + def close(self): + self._kernel_file.close() + def imu_callback(imu, vehicle_state): vehicle_state.bearing_deg = math.degrees(imu.compass) @@ -231,265 +239,300 @@ def can_function_runner(vs: VehicleState, exit_event: threading.Event): i += 1 -def bridge(q): - # setup CARLA +def connect_carla_client(): client = carla.Client("127.0.0.1", 2000) - client.set_timeout(10.0) - world = client.load_world(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 args.low_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] - spawn_points = world_map.get_spawn_points() - assert len(spawn_points) > args.num_selected_spawn_point, \ - f'''No spawn point {args.num_selected_spawn_point}, try a value between 0 and - {len(spawn_points)} for this town.''' - spawn_point = spawn_points[args.num_selected_spawn_point] - vehicle = world.spawn_actor(vehicle_bp, spawn_point) - - 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)) - blueprint.set_attribute('sensor_tick', '0.05') - camera = world.spawn_actor(blueprint, transform, attach_to=vehicle) - camera.listen(callback) - return camera - - camerad = Camerad() - road_camera = create_camera(fov=40, callback=camerad.cam_callback_road) - road_wide_camera = create_camera(fov=163, callback=camerad.cam_callback_wide_road) # fov bigger than 163 shows unwanted artifacts - - cameras = [road_camera, road_wide_camera] - - vehicle_state = VehicleState() - - # reenable IMU - imu_bp = blueprint_library.find('sensor.other.imu') - 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)) - - # launch fake car threads - threads = [] - exit_event = threading.Event() - threads.append(threading.Thread(target=panda_state_function, args=(vehicle_state, exit_event,))) - threads.append(threading.Thread(target=peripheral_state_function, args=(exit_event,))) - threads.append(threading.Thread(target=fake_driver_monitoring, args=(exit_event,))) - threads.append(threading.Thread(target=can_function_runner, args=(vehicle_state, exit_event,))) - for t in threads: - t.start() - - # can loop - rk = Ratekeeper(100, print_delay_threshold=0.05) - - # 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 - - while True: - # 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 - 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 + client.set_timeout(10) + return client - 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() - - # Clean up resources in the opposite order they were created. - exit_event.set() - for t in reversed(threads): - t.join() - gps.destroy() - imu.destroy() - for c in cameras: - c.destroy() - vehicle.destroy() - - -def bridge_keep_alive(q: Any): - while 1: - try: - bridge(q) - break - except RuntimeError as e: - print("Restarting bridge. Error:", e) +class CarlaBridge: -if __name__ == "__main__": - # make sure params are in a good state - set_params_enabled() + def __init__(self, args): + set_params_enabled() - msg = messaging.new_message('liveCalibration') - msg.liveCalibration.validBlocks = 20 - msg.liveCalibration.rpyCalib = [0.0, 0.0, 0.0] - Params().put("CalibrationParams", msg.to_bytes()) + msg = messaging.new_message('liveCalibration') + msg.liveCalibration.validBlocks = 20 + msg.liveCalibration.rpyCalib = [0.0, 0.0, 0.0] + Params().put("CalibrationParams", msg.to_bytes()) + self._args = args + self._carla_objects = [] + self._camerad = None + self._threads_exit_event = threading.Event() + self._threads = [] + self._shutdown = False + self.started = False + signal.signal(signal.SIGTERM, self._on_shutdown) + + def _on_shutdown(self, signal, frame): + self._shutdown = True + + def bridge_keep_alive(self, q: Queue, retries: int): + while not self._shutdown: + try: + self._run(q) + break + except RuntimeError as e: + if retries == 0: + raise + retries -= 1 + print(f"Restarting bridge. Retries left {retries}. Error: {e} ") + + def _run(self, q: Queue): + client = connect_carla_client() + 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 self._args.low_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] + 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 self._args.low_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() + road_camera = create_camera(fov=40, callback=self._camerad.cam_callback_road) + road_wide_camera = create_camera(fov=163, callback=self._camerad.cam_callback_wide_road) # fov bigger than 163 shows unwanted artifacts + + self._carla_objects.extend([road_camera, road_wide_camera]) + + vehicle_state = VehicleState() + + # reenable IMU + imu_bp = blueprint_library.find('sensor.other.imu') + 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._carla_objects.extend([imu, gps]) + # launch fake car threads + self._threads.append(threading.Thread(target=panda_state_function, args=(vehicle_state, self._threads_exit_event,))) + self._threads.append(threading.Thread(target=peripheral_state_function, args=(self._threads_exit_event,))) + self._threads.append(threading.Thread(target=fake_driver_monitoring, args=(self._threads_exit_event,))) + self._threads.append(threading.Thread(target=can_function_runner, args=(vehicle_state, self._threads_exit_event,))) + for t in self._threads: + t.start() + + # can loop + rk = Ratekeeper(100, print_delay_threshold=0.05) + + # 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 + + while not self._shutdown: + # 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 + 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 + + # Clean up resources in the opposite order they were created. + self.close() + + def close(self): + self._threads_exit_event.set() + if self._camerad is not None: + self._camerad.close() + 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): + unblock_stdout() # Fix error when publishing too many lag message + 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() - p = Process(target=bridge_keep_alive, args=(q,), daemon=True) - p.start() + args = parse_args() + + carla_bridge = CarlaBridge(args) + p = carla_bridge.run(q) if args.joystick: # start input poll for joystick - from lib.manual_ctrl import wheel_poll_thread + from tools.sim.lib.manual_ctrl import wheel_poll_thread + wheel_poll_thread(q) else: # start input poll for keyboard - from lib.keyboard_ctrl import keyboard_poll_thread + from tools.sim.lib.keyboard_ctrl import keyboard_poll_thread + keyboard_poll_thread(q) + p.join() diff --git a/tools/sim/launch_openpilot.sh b/tools/sim/launch_openpilot.sh index b94a453b7bb6a8..f5bac8a864f332 100755 --- a/tools/sim/launch_openpilot.sh +++ b/tools/sim/launch_openpilot.sh @@ -8,4 +8,4 @@ export FINGERPRINT="HONDA CIVIC 2016" export BLOCK="camerad,loggerd" DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" >/dev/null && pwd )" -cd ../../selfdrive/manager && ./manager.py +cd ../../selfdrive/manager && exec ./manager.py diff --git a/tools/sim/start_carla.sh b/tools/sim/start_carla.sh index 8b2725950723b1..8c3b140c826ff9 100755 --- a/tools/sim/start_carla.sh +++ b/tools/sim/start_carla.sh @@ -18,6 +18,7 @@ fi docker pull carlasim/carla:0.9.12 docker run \ + --name carla_sim \ --rm \ --gpus all \ --net=host \ diff --git a/tools/sim/test/__init__.py b/tools/sim/test/__init__.py new file mode 100644 index 00000000000000..e69de29bb2d1d6 diff --git a/tools/sim/test/test_carla_integration.py b/tools/sim/test/test_carla_integration.py new file mode 100755 index 00000000000000..8801a432fd3282 --- /dev/null +++ b/tools/sim/test/test_carla_integration.py @@ -0,0 +1,104 @@ +#!/usr/bin/env python3 +import subprocess +import time +import unittest +from multiprocessing import Queue + +from cereal import messaging +from tools.sim import bridge +from tools.sim.bridge import CarlaBridge + + +class TestCarlaIntegration(unittest.TestCase): + """ + Tests need Carla simulator to run + """ + processes = None + + def setUp(self): + self.processes = [] + # We want to make sure that carla_sim docker is still running. Skip output shell + subprocess.run("docker rm -f carla_sim", shell=True, stderr=subprocess.PIPE, check=False) + + self.processes.append(subprocess.Popen(".././start_carla.sh")) + + def test_run_bridge(self): + # Test bridge connect with carla and runs without any errors for 60 seconds + test_duration = 60 + + carla_bridge = CarlaBridge(bridge.parse_args(['--low_quality'])) + p = carla_bridge.run(Queue(), retries=3) + self.processes = [p] + + time.sleep(test_duration) + + self.assertEqual(p.exitcode, None, f"Bridge process should be running, but exited with code {p.exitcode}") + + 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='../') + self.processes.append(p_manager) + + sm = messaging.SubMaster(['controlsState', 'carEvents', 'managerState']) + q = Queue() + carla_bridge = CarlaBridge(bridge.parse_args(['--low_quality'])) + p_bridge = carla_bridge.run(q, retries=3) + self.processes.append(p_bridge) + + max_time_per_step = 20 + + # 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 sm offline, 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_up") # 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) + subprocess.run("docker rm -f carla_sim", shell=True, stderr=subprocess.PIPE, check=False) + + + +if __name__ == "__main__": + unittest.main()