CARLA: send YUV and RGB over visionipc from bridge.py (#23012)

* CARLA: send YUV and RGB over visionipc

* CARLA: send YUV and RGB over visionipc-fix pipfile

* CARLA: send YUV and RGB over visionipc-Camerad class

* relock pipfile

* small bridge cleanup

* use tici camera resolution

* update vof

* HUD_SETTING has no counter

* no loggerd

Co-authored-by: jwolffe <wolffja@gmail.com>
Co-authored-by: Willem Melching <willem.melching@gmail.com>
old-commit-hash: a58d272ae4
commatwo_master
jimw 3 years ago committed by GitHub
parent 5f299f7f3e
commit cf29eacca4
  1. 4
      Pipfile
  2. 4
      Pipfile.lock
  3. 98
      tools/sim/bridge.py
  4. 3
      tools/sim/launch_openpilot.sh
  5. 2
      tools/sim/lib/can.py

@ -1,3 +1,3 @@
version https://git-lfs.github.com/spec/v1
oid sha256:6aefb46314552707e165cf9bde6dbceb54c6176bc50b91036ed685d84cdb3265
size 1196
oid sha256:52b8d2b00d9d1d8b3430c2bc74e4ff51ab460317f931ed35cc74c6b9cc90f4d0
size 1211

4
Pipfile.lock generated

@ -1,3 +1,3 @@
version https://git-lfs.github.com/spec/v1
oid sha256:480ddcab4c5a1b12e6f9981381addba80ce3c24c01a577d849dfa995b737e66a
size 143544
oid sha256:3a3f3fe1935735e8ad7b9f24c39e200e5d462704d97276520227043bab07890e
size 145075

@ -1,19 +1,25 @@
#!/usr/bin/env python3
import argparse
import carla # pylint: disable=import-error
import math
import numpy as np
import time
import threading
from cereal import log
import time
import os
from multiprocessing import Process, Queue
from typing import Any
import carla # pylint: disable=import-error
import numpy as np
import pyopencl as cl
import pyopencl.array as cl_array
from lib.can import can_function
import cereal.messaging as messaging
from common.params import Params
from cereal import log
from cereal.visionipc.visionipc_pyx import VisionIpcServer, VisionStreamType # pylint: disable=no-name-in-module, import-error
from common.basedir import BASEDIR
from common.numpy_fast import clip
from common.realtime import Ratekeeper, DT_DMON
from lib.can import can_function
from common.params import Params
from common.realtime import DT_DMON, Ratekeeper
from selfdrive.car.honda.values import CruiseButtons
from selfdrive.test.helpers import set_params_enabled
@ -21,12 +27,11 @@ parser = argparse.ArgumentParser(description='Bridge between CARLA and openpilot
parser.add_argument('--joystick', action='store_true')
parser.add_argument('--low_quality', action='store_true')
parser.add_argument('--town', type=str, default='Town04_Opt')
parser.add_argument('--spawn_point', dest='num_selected_spawn_point',
type=int, default=16)
parser.add_argument('--spawn_point', dest='num_selected_spawn_point', type=int, default=16)
args = parser.parse_args()
W, H = 1164, 874
W, H = 1928, 1208
REPEAT_COUNTER = 5
PRINT_DECIMATION = 100
STEER_RATIO = 15.
@ -34,6 +39,7 @@ STEER_RATIO = 15.
pm = messaging.PubMaster(['roadCameraState', 'sensorEvents', 'can', "gpsLocationExternal"])
sm = messaging.SubMaster(['carControl', 'controlsState'])
class VehicleState:
def __init__(self):
self.speed = 0
@ -43,6 +49,7 @@ class VehicleState:
self.cruise_button = 0
self.is_engaged = False
def steer_rate_limit(old, new):
# Rate limiting to 0.5 degrees per step
limit = 0.5
@ -53,21 +60,56 @@ def steer_rate_limit(old, new):
else:
return new
frame_id = 0
def cam_callback(image):
global frame_id
class Camerad:
def __init__(self):
self.frame_id = 0
self.vipc_server = VisionIpcServer("camerad")
# TODO: remove RGB buffers once the last RGB vipc subscriber is removed
self.vipc_server.create_buffers(VisionStreamType.VISION_STREAM_RGB_BACK, 4, True, W, H)
self.vipc_server.create_buffers(VisionStreamType.VISION_STREAM_YUV_BACK, 40, 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 "
# TODO: move rgb_to_yuv.cl to local dir once the frame stream camera is removed
kernel_fn = os.path.join(BASEDIR, "selfdrive", "camerad", "transforms", "rgb_to_yuv.cl")
prg = cl.Program(self.ctx, open(kernel_fn).read()).build(cl_arg)
self.krnl = prg.rgb_to_yuv
self.Wdiv4 = W // 4 if (W % 4 == 0) else (W + (4 - W % 4)) // 4
self.Hdiv4 = H // 4 if (H % 4 == 0) else (H + (4 - H % 4)) // 4
def cam_callback(self, image):
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(), np.int32((rgb.size / 2)))
eof = self.frame_id * 0.05
# TODO: remove RGB send once the last RGB vipc subscriber is removed
self.vipc_server.send(VisionStreamType.VISION_STREAM_RGB_BACK, img.tobytes(), self.frame_id, eof, eof)
self.vipc_server.send(VisionStreamType.VISION_STREAM_YUV_BACK, yuv.data.tobytes(), self.frame_id, eof, eof)
dat = messaging.new_message('roadCameraState')
dat.roadCameraState = {
"frameId": image.frame,
"image": img.tobytes(),
"transform": [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]
"transform": [1.0, 0.0, 0.0,
0.0, 1.0, 0.0,
0.0, 0.0, 1.0]
}
pm.send('roadCameraState', dat)
frame_id += 1
self.frame_id += 1
def imu_callback(imu, vehicle_state):
vehicle_state.bearing_deg = math.degrees(imu.compass)
@ -83,6 +125,7 @@ def imu_callback(imu, vehicle_state):
dat.sensorEvents[1].gyroUncalibrated.v = [imu.gyroscope.x, imu.gyroscope.y, imu.gyroscope.z]
pm.send('sensorEvents', dat)
def panda_state_function(exit_event: threading.Event):
pm = messaging.PubMaster(['pandaStates'])
while not exit_event.is_set():
@ -97,6 +140,7 @@ def panda_state_function(exit_event: threading.Event):
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():
@ -112,6 +156,7 @@ def peripheral_state_function(exit_event: threading.Event):
pm.send('peripheralState', dat)
time.sleep(0.5)
def gps_callback(gps, vehicle_state):
dat = messaging.new_message('gpsLocationExternal')
@ -141,6 +186,7 @@ def gps_callback(gps, vehicle_state):
pm.send('gpsLocationExternal', dat)
def fake_driver_monitoring(exit_event: threading.Event):
pm = messaging.PubMaster(['driverState', 'driverMonitoringState'])
while not exit_event.is_set():
@ -160,6 +206,7 @@ def fake_driver_monitoring(exit_event: threading.Event):
time.sleep(DT_DMON)
def can_function_runner(vs: VehicleState, exit_event: threading.Event):
i = 0
while not exit_event.is_set():
@ -169,7 +216,6 @@ def can_function_runner(vs: VehicleState, exit_event: threading.Event):
def bridge(q):
# setup CARLA
client = carla.Client("127.0.0.1", 2000)
client.set_timeout(10.0)
@ -209,11 +255,12 @@ def bridge(q):
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', '70')
blueprint.set_attribute('fov', '40')
blueprint.set_attribute('sensor_tick', '0.05')
transform = carla.Transform(carla.Location(x=0.8, z=1.13))
camera = world.spawn_actor(blueprint, transform, attach_to=vehicle)
camera.listen(cam_callback)
camerad = Camerad()
camera.listen(camerad.cam_callback)
vehicle_state = VehicleState()
@ -244,7 +291,6 @@ def bridge(q):
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
@ -257,8 +303,7 @@ def bridge(q):
brake_manual_multiplier = 0.7 # keyboard signal is always 1
steer_manual_multiplier = 45 * STEER_RATIO # keyboard signal is always 1
while 1:
while True:
# 1. Read the throttle, steer and brake from op or manual controls
# 2. Set instructions in Carla
# 3. Send current carstate to op via can
@ -282,7 +327,6 @@ def bridge(q):
brake_manual = float(m[1])
is_openpilot_engaged = False
elif m[0] == "reverse":
#in_reverse = not in_reverse
cruise_button = CruiseButtons.CANCEL
is_openpilot_engaged = False
elif m[0] == "cruise":
@ -302,16 +346,13 @@ def bridge(q):
steer_out = steer_manual * steer_manual_multiplier
brake_out = brake_manual * brake_manual_multiplier
#steer_out = steer_out
# steer_out = steer_rate_limit(old_steer, steer_out)
old_steer = steer_out
old_throttle = throttle_out
old_brake = brake_out
# print('message',old_throttle, old_steer, old_brake)
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)
@ -350,7 +391,6 @@ def bridge(q):
old_steer = 0
# --------------Step 2-------------------------------
steer_carla = steer_out / (max_steer_angle * STEER_RATIO * -1)
steer_carla = np.clip(steer_carla, -1, 1)
@ -385,6 +425,7 @@ def bridge(q):
camera.destroy()
vehicle.destroy()
def bridge_keep_alive(q: Any):
while 1:
try:
@ -393,6 +434,7 @@ def bridge_keep_alive(q: Any):
except RuntimeError:
print("Restarting bridge...")
if __name__ == "__main__":
# make sure params are in a good state
set_params_enabled()

@ -5,8 +5,7 @@ export NOBOARD="1"
export SIMULATION="1"
export FINGERPRINT="HONDA CIVIC 2016"
# TODO: remove this once the bridge uses visionipc
export BLOCK="loggerd"
export BLOCK="camerad,loggerd"
DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" >/dev/null && pwd )"
cd ../../selfdrive/manager && ./manager.py

@ -62,7 +62,7 @@ def can_function(pm, speed, angle, idx, cruise_button, is_engaged):
msg.append(packer.make_can_msg("CRUISE", 0, {}, idx))
msg.append(packer.make_can_msg("SCM_FEEDBACK", 0, {"MAIN_ON": 1}, idx))
msg.append(packer.make_can_msg("POWERTRAIN_DATA", 0, {"ACC_STATUS": int(is_engaged)}, idx))
msg.append(packer.make_can_msg("HUD_SETTING", 0, {}, idx))
msg.append(packer.make_can_msg("HUD_SETTING", 0, {}))
# *** cam bus ***
msg.append(packer.make_can_msg("STEERING_CONTROL", 2, {}, idx))

Loading…
Cancel
Save