Simulator: cleanup in preparation for metadrive (#29903)
* sim bridge cleanup
* fix carla
* remove that exception
* pr cleanup
* update car in a thread
* more cleanup
* import sorting
* handle exits better
old-commit-hash: 7f6718a7cb
test-msgs
Justin Newberry
2 years ago
committed by
GitHub
parent
109515a208
commit
27cacb51d4
12 changed files with 773 additions and 659 deletions
@ -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() |
|
||||||
@ -0,0 +1,163 @@ |
|||||||
|
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.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.image_lock: |
||||||
|
self.road_image = self.carla_image_to_rgb(image) |
||||||
|
|
||||||
|
def cam_callback_wide_road(self, image): |
||||||
|
with self.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 |
||||||
|
client = carla.Client(self.host, self.port) |
||||||
|
client.set_timeout(5) |
||||||
|
|
||||||
|
world = client.load_world(self.town) |
||||||
|
|
||||||
|
settings = world.get_settings() |
||||||
|
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) |
||||||
@ -0,0 +1,163 @@ |
|||||||
|
import signal |
||||||
|
import threading |
||||||
|
import functools |
||||||
|
|
||||||
|
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 |
||||||
|
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 |
||||||
|
|
||||||
|
|
||||||
|
def rk_loop(function, hz, exit_event: threading.Event): |
||||||
|
rk = Ratekeeper(hz) |
||||||
|
while not exit_event.is_set(): |
||||||
|
function() |
||||||
|
rk.keep_time() |
||||||
|
|
||||||
|
|
||||||
|
class SimulatorBridge(ABC): |
||||||
|
TICKS_PER_FRAME = 5 |
||||||
|
|
||||||
|
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] |
||||||
|
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() |
||||||
|
|
||||||
|
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): |
||||||
|
try: |
||||||
|
self._run(q) |
||||||
|
finally: |
||||||
|
self.close() |
||||||
|
|
||||||
|
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() |
||||||
|
return bridge_p |
||||||
|
|
||||||
|
@abstractmethod |
||||||
|
def spawn_world(self) -> World: |
||||||
|
pass |
||||||
|
|
||||||
|
def _run(self, q: Queue): |
||||||
|
self.world = self.spawn_world() |
||||||
|
|
||||||
|
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._exit_event)) |
||||||
|
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): |
||||||
|
self.world.tick() |
||||||
|
|
||||||
|
throttle_manual = steer_manual = brake_manual = 0. |
||||||
|
|
||||||
|
while self._keep_alive: |
||||||
|
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. |
||||||
|
|
||||||
|
# Read manual controls |
||||||
|
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 |
||||||
|
|
||||||
|
# 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) |
||||||
|
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 |
||||||
|
|
||||||
|
self.world.apply_controls(steer_out, throttle_out, brake_out) |
||||||
|
self.world.read_sensors(self.simulator_state) |
||||||
|
|
||||||
|
if self.rk.frame % self.TICKS_PER_FRAME == 0: |
||||||
|
self.world.tick() |
||||||
|
self.world.read_cameras() |
||||||
|
|
||||||
|
self.started = True |
||||||
|
|
||||||
|
rk.keep_time() |
||||||
@ -0,0 +1,70 @@ |
|||||||
|
import numpy as np |
||||||
|
import os |
||||||
|
import pyopencl as cl |
||||||
|
import pyopencl.array as cl_array |
||||||
|
|
||||||
|
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: |
||||||
|
"""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) |
||||||
@ -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 |
|
||||||
@ -0,0 +1,86 @@ |
|||||||
|
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 |
||||||
|
|
||||||
|
@abstractmethod |
||||||
|
def close(self): |
||||||
|
pass |
||||||
@ -0,0 +1,110 @@ |
|||||||
|
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): |
||||||
|
self.send_can_messages(simulator_state) |
||||||
|
self.send_panda_state(simulator_state) |
||||||
|
|
||||||
|
self.idx += 1 |
||||||
@ -0,0 +1,127 @@ |
|||||||
|
import time |
||||||
|
|
||||||
|
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 |
||||||
|
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) |
||||||
@ -0,0 +1,50 @@ |
|||||||
|
#!/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) |
||||||
|
|
||||||
|
simulator_bridge.shutdown() |
||||||
|
|
||||||
|
p.join() |
||||||
Loading…
Reference in new issue