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: 7f6718a7cb87909305fd6f23a614db926d225915
This commit is contained in:
Justin Newberry
@@ -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
tools/sim/bridge/__init__.py
Normal file
0
tools/sim/bridge/__init__.py
Normal file
163
tools/sim/bridge/carla.py
Normal file
163
tools/sim/bridge/carla.py
Normal file
@@ -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)
|
||||
163
tools/sim/bridge/common.py
Normal file
163
tools/sim/bridge/common.py
Normal file
@@ -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()
|
||||
70
tools/sim/lib/camerad.py
Normal file
70
tools/sim/lib/camerad.py
Normal file
@@ -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
|
||||
86
tools/sim/lib/common.py
Normal file
86
tools/sim/lib/common.py
Normal file
@@ -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
|
||||
@@ -1,6 +1,7 @@
|
||||
import sys
|
||||
import termios
|
||||
import time
|
||||
|
||||
from termios import (BRKINT, CS8, CSIZE, ECHO, ICANON, ICRNL, IEXTEN, INPCK,
|
||||
ISTRIP, IXON, PARENB, VMIN, VTIME)
|
||||
from typing import NoReturn
|
||||
@@ -38,7 +39,6 @@ def getch() -> str:
|
||||
def keyboard_poll_thread(q: 'Queue[str]'):
|
||||
while True:
|
||||
c = getch()
|
||||
print("got %s" % c)
|
||||
if c == '1':
|
||||
q.put("cruise_up")
|
||||
elif c == '2':
|
||||
|
||||
110
tools/sim/lib/simulated_car.py
Normal file
110
tools/sim/lib/simulated_car.py
Normal file
@@ -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
|
||||
127
tools/sim/lib/simulated_sensors.py
Normal file
127
tools/sim/lib/simulated_sensors.py
Normal file
@@ -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)
|
||||
50
tools/sim/run_bridge.py
Executable file
50
tools/sim/run_bridge.py
Executable file
@@ -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()
|
||||
@@ -8,8 +8,8 @@ from multiprocessing import Queue
|
||||
from cereal import messaging
|
||||
from openpilot.common.basedir import BASEDIR
|
||||
from openpilot.selfdrive.manager.helpers import unblock_stdout
|
||||
from openpilot.tools.sim import bridge
|
||||
from openpilot.tools.sim.bridge import CarlaBridge
|
||||
from openpilot.tools.sim.run_bridge import parse_args
|
||||
from openpilot.tools.sim.bridge.carla import CarlaBridge
|
||||
|
||||
CI = "CI" in os.environ
|
||||
|
||||
@@ -42,7 +42,7 @@ class TestCarlaIntegration(unittest.TestCase):
|
||||
|
||||
sm = messaging.SubMaster(['controlsState', 'carEvents', 'managerState'])
|
||||
q = Queue()
|
||||
carla_bridge = CarlaBridge(bridge.parse_args([]))
|
||||
carla_bridge = CarlaBridge(parse_args([]))
|
||||
p_bridge = carla_bridge.run(q, retries=10)
|
||||
self.processes.append(p_bridge)
|
||||
|
||||
|
||||
Reference in New Issue
Block a user