sadmen
/
openpilot_comma
mirror of https://github.com/commaai/openpilot.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Simulator Tests (#24274)

Browse Source 
* squash #24009

* Fix from other pr
Add low-quality arg

* Update tools/sim/test/test_carla_integration.py

Co-authored-by: Adeeb Shihadeh <adeebshihadeh@gmail.com>

* Updates for comments. Not finished yet

* commit

* fix arguments

* Final changes for comments

* Final fixes

* increase carla client timeout to 10

* make test executable

* actually wait for controlsd to send messages

* Error proof test. Starting up carla at each test and closing down using docker (tried many things).

* commit test carla

* Removed some time.sleeps
Add some more retries for bridge.

* Stop while loop on shutdown

* Increase teardown waiting time

Co-authored-by: Willem Melching <willem.melching@gmail.com>
Co-authored-by: Adeeb Shihadeh <adeebshihadeh@gmail.com>
old-commit-hash: 9be23cbdb9
taco
Gijs Koning 3 years ago committed by GitHub
parent 16c40a5f7c
commit 4ed3a158af
6 changed files with 410 additions and 262 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 0
      tools/sim/__init__.py
  2. 565
      tools/sim/bridge.py
  3. 2
      tools/sim/launch_openpilot.sh
  4. 1
      tools/sim/start_carla.sh
  5. 0
      tools/sim/test/__init__.py
  6. 104
      tools/sim/test/test_carla_integration.py

0
tools/sim/__init__.py
Unescape View File

565
tools/sim/bridge.py
Unescape View File

@ -1,9 +1,10 @@
#!/usr/bin/env python3 #!/usr/bin/env python3
import argparse import argparse
import math import math
import os
import signal
import threading import threading
import time import time
import os
from multiprocessing import Process, Queue from multiprocessing import Process, Queue
from typing import Any from typing import Any
@ -12,8 +13,6 @@ import numpy as np
import pyopencl as cl import pyopencl as cl
import pyopencl.array as cl_array import pyopencl.array as cl_array
from lib.can import can_function
import cereal.messaging as messaging import cereal.messaging as messaging
from cereal import log from cereal import log
from cereal.visionipc.visionipc_pyx import VisionIpcServer, VisionStreamType # pylint: disable=no-name-in-module, import-error from cereal.visionipc.visionipc_pyx import VisionIpcServer, VisionStreamType # pylint: disable=no-name-in-module, import-error
@ -22,15 +21,9 @@ from common.numpy_fast import clip
from common.params import Params from common.params import Params
from common.realtime import DT_DMON, Ratekeeper from common.realtime import DT_DMON, Ratekeeper
from selfdrive.car.honda.values import CruiseButtons from selfdrive.car.honda.values import CruiseButtons
from selfdrive.manager.helpers import unblock_stdout
from selfdrive.test.helpers import set_params_enabled from selfdrive.test.helpers import set_params_enabled
from tools.sim.lib.can import can_function
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()
W, H = 1928, 1208 W, H = 1928, 1208
REPEAT_COUNTER = 5 REPEAT_COUNTER = 5
@ -41,6 +34,16 @@ pm = messaging.PubMaster(['roadCameraState', 'wideRoadCameraState', 'sensorEvent
sm = messaging.SubMaster(['carControl', 'controlsState']) 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: class VehicleState:
def __init__(self): def __init__(self):
self.speed = 0 self.speed = 0
@ -80,11 +83,13 @@ class Camerad:
# set up for pyopencl rgb to yuv conversion # set up for pyopencl rgb to yuv conversion
self.ctx = cl.create_some_context() self.ctx = cl.create_some_context()
self.queue = cl.CommandQueue(self.ctx) 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 # 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") 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.krnl = prg.rgb_to_yuv
self.Wdiv4 = W // 4 if (W % 4 == 0) else (W + (4 - W % 4)) // 4 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 self.Hdiv4 = H // 4 if (H % 4 == 0) else (H + (4 - H % 4)) // 4
@ -126,6 +131,9 @@ class Camerad:
setattr(dat, pub_type, msg) setattr(dat, pub_type, msg)
pm.send(pub_type, dat) pm.send(pub_type, dat)
def close(self):
self._kernel_file.close()
def imu_callback(imu, vehicle_state): def imu_callback(imu, vehicle_state):
vehicle_state.bearing_deg = math.degrees(imu.compass) vehicle_state.bearing_deg = math.degrees(imu.compass)
@ -231,265 +239,300 @@ def can_function_runner(vs: VehicleState, exit_event: threading.Event):
i += 1 i += 1
def bridge(q): def connect_carla_client():
# setup CARLA
client = carla.Client("127.0.0.1", 2000) client = carla.Client("127.0.0.1", 2000)
client.set_timeout(10.0) client.set_timeout(10)
world = client.load_world(args.town) return client
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
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__": def __init__(self, args):
# make sure params are in a good state set_params_enabled()
set_params_enabled()
msg = messaging.new_message('liveCalibration') msg = messaging.new_message('liveCalibration')
msg.liveCalibration.validBlocks = 20 msg.liveCalibration.validBlocks = 20
msg.liveCalibration.rpyCalib = [0.0, 0.0, 0.0] msg.liveCalibration.rpyCalib = [0.0, 0.0, 0.0]
Params().put("CalibrationParams", msg.to_bytes()) 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() q: Any = Queue()
p = Process(target=bridge_keep_alive, args=(q,), daemon=True) args = parse_args()
p.start()
carla_bridge = CarlaBridge(args)
p = carla_bridge.run(q)
if args.joystick: if args.joystick:
# start input poll for 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) wheel_poll_thread(q)
else: else:
# start input poll for keyboard # 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) keyboard_poll_thread(q)
p.join()

2
tools/sim/launch_openpilot.sh
Unescape View File

@ -8,4 +8,4 @@ export FINGERPRINT="HONDA CIVIC 2016"
export BLOCK="camerad,loggerd" export BLOCK="camerad,loggerd"
DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" >/dev/null && pwd )" DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" >/dev/null && pwd )"
cd ../../selfdrive/manager && ./manager.py cd ../../selfdrive/manager && exec ./manager.py

1
tools/sim/start_carla.sh
Unescape View File

@ -18,6 +18,7 @@ fi
docker pull carlasim/carla:0.9.12 docker pull carlasim/carla:0.9.12
docker run \ docker run \
--name carla_sim \
--rm \ --rm \
--gpus all \ --gpus all \
--net=host \ --net=host \

0
tools/sim/test/__init__.py
Unescape View File

104
tools/sim/test/test_carla_integration.py
Unescape View File

@ -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()
Write Preview
Loading…
Cancel
Save