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

Pipfile

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

Pipfile.lock

@@ -1,3 +1,3 @@
 version https://git-lfs.github.com/spec/v1
-oid sha256:480ddcab4c5a1b12e6f9981381addba80ce3c24c01a577d849dfa995b737e66a
+oid sha256:3a3f3fe1935735e8ad7b9f24c39e200e5d462704d97276520227043bab07890e
-size 143544
+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 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 common.params import Params
-from lib.can import can_function
+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',
+parser.add_argument('--spawn_point', dest='num_selected_spawn_point', type=int, default=16)
-        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):
+class Camerad:
-  global frame_id
+  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,
-    "transform": [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.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 True:
-  while 1:
     # 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()

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="camerad,loggerd"
-export BLOCK="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))