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: a58d272ae4afa11624220b9392295ad6659db1df

Pipfile

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

Pipfile.lock

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

tools/sim/bridge.py

@@ -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 cereal.messaging as messaging
-from common.params import Params
-from common.numpy_fast import clip
-from common.realtime import Ratekeeper, DT_DMON
+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 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.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,18 +27,18 @@ 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.
 
 pm = messaging.PubMaster(['roadCameraState', 'sensorEvents', 'can', "gpsLocationExternal"])
-sm = messaging.SubMaster(['carControl','controlsState'])
+sm = messaging.SubMaster(['carControl', 'controlsState'])
+
 
 class VehicleState:
   def __init__(self):
@@ -40,8 +46,9 @@ class VehicleState:
     self.angle = 0
     self.bearing_deg = 0.0
     self.vel = carla.Vector3D()
-    self.cruise_button= 0
-    self.is_engaged=False
+    self.cruise_button = 0
+    self.is_engaged = False
+
 
 def steer_rate_limit(old, new):
   # Rate limiting to 0.5 degrees per step
@@ -53,21 +60,56 @@ def steer_rate_limit(old, new):
   else:
     return new
 
-frame_id = 0
-def cam_callback(image):
-  global frame_id
-  img = np.frombuffer(image.raw_data, dtype=np.dtype("uint8"))
-  img = np.reshape(img, (H, W, 4))
-  img = img[:, :, [0, 1, 2]].copy()
 
-  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]
-  }
-  pm.send('roadCameraState', dat)
-  frame_id += 1
+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,
+      "transform": [1.0, 0.0, 0.0,
+                    0.0, 1.0, 0.0,
+                    0.0, 0.0, 1.0]
+    }
+    pm.send('roadCameraState', dat)
+    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,20 +156,21 @@ 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')
 
   # 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.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 = {
     "timestamp": int(time.time() * 1000),
-    "flags": 1, # valid fix
+    "flags": 1,  # valid fix
     "accuracy": 1.0,
     "verticalAccuracy": 1.0,
     "speedAccuracy": 0.1,
@@ -141,8 +186,9 @@ def gps_callback(gps, vehicle_state):
 
   pm.send('gpsLocationExternal', dat)
 
+
 def fake_driver_monitoring(exit_event: threading.Event):
-  pm = messaging.PubMaster(['driverState','driverMonitoringState'])
+  pm = messaging.PubMaster(['driverState', 'driverMonitoringState'])
   while not exit_event.is_set():
     # dmonitoringmodeld output
     dat = messaging.new_message('driverState')
@@ -160,16 +206,16 @@ 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():
     can_function(pm, vs.speed, vs.angle, i, vs.cruise_button, vs.is_engaged)
     time.sleep(0.01)
-    i+=1
+    i += 1
 
 
 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
@@ -253,12 +299,11 @@ def bridge(q):
   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
+  throttle_manual_multiplier = 0.7  # keyboard signal is always 1
+  brake_manual_multiplier = 0.7  # keyboard signal is always 1
+  steer_manual_multiplier = 45 * STEER_RATIO  # keyboard signal is always 1
 
-
-  while 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,19 +346,16 @@ 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)
+      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
@@ -325,24 +366,24 @@ def bridge(q):
       old_steer = steer_out
 
     else:
-      if throttle_out==0 and old_throttle>0:
-        if throttle_ease_out_counter>0:
+      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:
+      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:
+      if steer_out == 0 and old_steer != 0:
+        if steer_ease_out_counter > 0:
           steer_out = old_steer
           steer_ease_out_counter += -1
         else:
@@ -350,28 +391,27 @@ 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)
+    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.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
+    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:
+    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))
 
     rk.keep_time()
@@ -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()

tools/sim/launch_openpilot.sh

@@ -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

tools/sim/lib/can.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))