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