mirror of
https://github.com/dragonpilot/dragonpilot.git
synced 2026-02-19 07:44:00 +08:00
Simulator Tests (#24274)
* 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>
This commit is contained in:
Gijs Koning
0
tools/sim/__init__.py
Normal file
0
tools/sim/__init__.py
Normal file
@@ -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 numpy as np
|
||||
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.numpy_fast import clip
|
||||
from common.params import Params
|
||||
from common.realtime import DT_DMON, Ratekeeper
|
||||
from selfdrive.car.honda.values import CruiseButtons
|
||||
from selfdrive.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 @@ pm = messaging.PubMaster(['roadCameraState', 'wideRoadCameraState', 'sensorEvent
|
||||
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 @@ class Camerad:
|
||||
# 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 @@ class Camerad:
|
||||
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)
|
||||
client.set_timeout(10)
|
||||
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)
|
||||
class CarlaBridge:
|
||||
|
||||
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)
|
||||
def __init__(self, args):
|
||||
set_params_enabled()
|
||||
|
||||
blueprint_library = world.get_blueprint_library()
|
||||
msg = messaging.new_message('liveCalibration')
|
||||
msg.liveCalibration.validBlocks = 20
|
||||
msg.liveCalibration.rpyCalib = [0.0, 0.0, 0.0]
|
||||
Params().put("CalibrationParams", msg.to_bytes())
|
||||
|
||||
world_map = world.get_map()
|
||||
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)
|
||||
|
||||
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)
|
||||
def _on_shutdown(self, signal, frame):
|
||||
self._shutdown = True
|
||||
|
||||
max_steer_angle = vehicle.get_physics_control().wheels[0].max_steer_angle
|
||||
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} ")
|
||||
|
||||
# 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)
|
||||
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)
|
||||
|
||||
transform = carla.Transform(carla.Location(x=0.8, z=1.13))
|
||||
world.set_weather(carla.WeatherParameters.ClearSunset)
|
||||
|
||||
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
|
||||
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)
|
||||
|
||||
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
|
||||
blueprint_library = world.get_blueprint_library()
|
||||
|
||||
cameras = [road_camera, road_wide_camera]
|
||||
world_map = world.get_map()
|
||||
|
||||
vehicle_state = VehicleState()
|
||||
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
|
||||
|
||||
# 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))
|
||||
# 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)
|
||||
|
||||
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))
|
||||
transform = carla.Transform(carla.Location(x=0.8, z=1.13))
|
||||
|
||||
# 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()
|
||||
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
|
||||
|
||||
# can loop
|
||||
rk = Ratekeeper(100, print_delay_threshold=0.05)
|
||||
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
|
||||
|
||||
# init
|
||||
throttle_ease_out_counter = REPEAT_COUNTER
|
||||
brake_ease_out_counter = REPEAT_COUNTER
|
||||
steer_ease_out_counter = REPEAT_COUNTER
|
||||
self._carla_objects.extend([road_camera, road_wide_camera])
|
||||
|
||||
vc = carla.VehicleControl(throttle=0, steer=0, brake=0, reverse=False)
|
||||
vehicle_state = VehicleState()
|
||||
|
||||
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
|
||||
# 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))
|
||||
|
||||
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
|
||||
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))
|
||||
|
||||
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
|
||||
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()
|
||||
|
||||
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
|
||||
# can loop
|
||||
rk = Ratekeeper(100, print_delay_threshold=0.05)
|
||||
|
||||
# --------------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":
|
||||
# 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] == "ignition":
|
||||
vehicle_state.ignition = not vehicle_state.ignition
|
||||
elif m[0] == "quit":
|
||||
break
|
||||
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
|
||||
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
|
||||
old_steer = steer_out
|
||||
old_throttle = throttle_out
|
||||
old_brake = brake_out
|
||||
|
||||
if is_openpilot_engaged:
|
||||
sm.update(0)
|
||||
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
|
||||
# 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
|
||||
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
|
||||
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
|
||||
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 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
|
||||
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)
|
||||
# --------------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)
|
||||
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)
|
||||
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
|
||||
# --------------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 % 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()
|
||||
if rk.frame % 5 == 0:
|
||||
world.tick()
|
||||
rk.keep_time()
|
||||
self.started = True
|
||||
|
||||
rk.keep_time()
|
||||
# Clean up resources in the opposite order they were created.
|
||||
self.close()
|
||||
|
||||
# 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 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 bridge_keep_alive(q: Any):
|
||||
while 1:
|
||||
try:
|
||||
bridge(q)
|
||||
break
|
||||
except RuntimeError as e:
|
||||
print("Restarting bridge. Error:", e)
|
||||
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__":
|
||||
# make sure params are in a good state
|
||||
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())
|
||||
|
||||
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()
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -18,6 +18,7 @@ fi
|
||||
docker pull carlasim/carla:0.9.12
|
||||
|
||||
docker run \
|
||||
--name carla_sim \
|
||||
--rm \
|
||||
--gpus all \
|
||||
--net=host \
|
||||
|
||||
0
tools/sim/test/__init__.py
Normal file
0
tools/sim/test/__init__.py
Normal file
104
tools/sim/test/test_carla_integration.py
Executable file
104
tools/sim/test/test_carla_integration.py
Executable 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()
|
||||
Reference in New Issue
Block a user