diff --git a/Pipfile b/Pipfile index 50d136e08c..c402520dc9 100644 --- a/Pipfile +++ b/Pipfile @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:6aefb46314552707e165cf9bde6dbceb54c6176bc50b91036ed685d84cdb3265 -size 1196 +oid sha256:52b8d2b00d9d1d8b3430c2bc74e4ff51ab460317f931ed35cc74c6b9cc90f4d0 +size 1211 diff --git a/Pipfile.lock b/Pipfile.lock index e1323863f3..fb248bf830 100644 --- a/Pipfile.lock +++ b/Pipfile.lock @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:480ddcab4c5a1b12e6f9981381addba80ce3c24c01a577d849dfa995b737e66a -size 143544 +oid sha256:3a3f3fe1935735e8ad7b9f24c39e200e5d462704d97276520227043bab07890e +size 145075 diff --git a/tools/sim/bridge.py b/tools/sim/bridge.py index 2873aa7223..ed129db6ee 100755 --- a/tools/sim/bridge.py +++ b/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 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,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() diff --git a/tools/sim/launch_openpilot.sh b/tools/sim/launch_openpilot.sh index e10d688c2f..b94a453b7b 100755 --- a/tools/sim/launch_openpilot.sh +++ b/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 diff --git a/tools/sim/lib/can.py b/tools/sim/lib/can.py index 97754ab0b8..4873947d4b 100755 --- a/tools/sim/lib/can.py +++ b/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))