sadmen
/
jyoung
mirror of https://github.com/jyoung8607/openpilot.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

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

Browse Source 
* 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
vw-mqb-aeb
jimw 4 years ago committed by GitHub
parent 5f299f7f3e
commit cf29eacca4
5 changed files with 108 additions and 67 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  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

4
Pipfile
Unescape View File

@ -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
Unescape View File

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

98
tools/sim/bridge.py
Unescape View File

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

3
tools/sim/launch_openpilot.sh
Unescape View File

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

2
tools/sim/lib/can.py
Unescape View File

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

Write Preview
Loading…
Cancel
Save