diff --git a/common/transformations/camera.py b/common/transformations/camera.py index 7573877a3..0e26bb8ef 100644 --- a/common/transformations/camera.py +++ b/common/transformations/camera.py @@ -6,8 +6,10 @@ from selfdrive.hardware import TICI ## -- hardcoded hardware params -- eon_f_focal_length = 910.0 eon_d_focal_length = 650.0 +# tici_f: narrow road cam. tici_e: wide road cam tici_f_focal_length = 2648.0 -tici_e_focal_length = tici_d_focal_length = 567.0 # probably wrong? magnification is not consistent across frame +tici_d_and_e_focal_length = 567.0 # probably wrong? magnification is not consistent across frame +tici_e_focal_length = tici_d_focal_length = tici_d_and_e_focal_length eon_f_frame_size = (1164, 874) eon_d_frame_size = (816, 612) diff --git a/tools/sim/bridge.py b/tools/sim/bridge.py index ae6483fd2..834fecb54 100755 --- a/tools/sim/bridge.py +++ b/tools/sim/bridge.py @@ -11,6 +11,8 @@ import carla # pylint: disable=import-error import numpy as np import pyopencl as cl import pyopencl.array as cl_array + +from common.transformations.camera import tici_d_and_e_focal_length from lib.can import can_function import cereal.messaging as messaging @@ -26,6 +28,7 @@ from selfdrive.test.helpers import set_params_enabled 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('--only_c2_cameras', 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) @@ -36,7 +39,7 @@ REPEAT_COUNTER = 5 PRINT_DECIMATION = 100 STEER_RATIO = 15. -pm = messaging.PubMaster(['roadCameraState', 'sensorEvents', 'can', "gpsLocationExternal"]) +pm = messaging.PubMaster(['roadCameraState', "wideRoadCameraState", 'sensorEvents', 'can', "gpsLocationExternal"]) sm = messaging.SubMaster(['carControl', 'controlsState']) @@ -62,13 +65,18 @@ def steer_rate_limit(old, new): class Camerad: - def __init__(self): - self.frame_id = 0 + def __init__(self, wide_camera=False): + self.frame_road_id = 0 + self.frame_wide_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_ROAD, 4, True, W, H) self.vipc_server.create_buffers(VisionStreamType.VISION_STREAM_ROAD, 40, False, W, H) + + if wide_camera: + self.vipc_server.create_buffers(VisionStreamType.VISION_STREAM_RGB_WIDE_ROAD, 4, True, W, H) + self.vipc_server.create_buffers(VisionStreamType.VISION_STREAM_WIDE_ROAD, 40, False, W, H) self.vipc_server.start_listener() # set up for pyopencl rgb to yuv conversion @@ -83,7 +91,17 @@ class Camerad: 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): + def cam_callback_road(self, image): + self._cam_callback(image, self.frame_road_id, 'roadCameraState', + VisionStreamType.VISION_STREAM_RGB_ROAD, VisionStreamType.VISION_STREAM_ROAD) + self.frame_road_id += 1 + + def cam_callback_wide_road(self, image): + self._cam_callback(image, self.frame_wide_id, 'wideRoadCameraState', + VisionStreamType.VISION_STREAM_RGB_WIDE_ROAD, VisionStreamType.VISION_STREAM_WIDE_ROAD) + self.frame_wide_id += 1 + + def _cam_callback(self, image, frame_id, pub_type, rgb_type, yuv_type): img = np.frombuffer(image.raw_data, dtype=np.dtype("uint8")) img = np.reshape(img, (H, W, 4)) img = img[:, :, [0, 1, 2]].copy() @@ -94,21 +112,21 @@ class Camerad: 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 + eof = frame_id * 0.05 # TODO: remove RGB send once the last RGB vipc subscriber is removed - self.vipc_server.send(VisionStreamType.VISION_STREAM_RGB_ROAD, img.tobytes(), self.frame_id, eof, eof) - self.vipc_server.send(VisionStreamType.VISION_STREAM_ROAD, yuv.data.tobytes(), self.frame_id, eof, eof) + self.vipc_server.send(rgb_type, img.tobytes(), frame_id, eof, eof) + self.vipc_server.send(yuv_type, yuv.data.tobytes(), frame_id, eof, eof) - dat = messaging.new_message('roadCameraState') - dat.roadCameraState = { + dat = messaging.new_message(pub_type) + msg = { "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 + setattr(dat, pub_type, msg) + pm.send(pub_type, dat) def imu_callback(imu, vehicle_state): @@ -259,16 +277,26 @@ def bridge(q): physics_control.gear_switch_time = 0.0 vehicle.apply_physics_control(physics_control) - 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', '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) - camerad = Camerad() - camera.listen(camerad.cam_callback) + def create_camera(fov): + 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', str(fov)) + blueprint.set_attribute('focal_distance', str(int(tici_d_and_e_focal_length))) + blueprint.set_attribute('sensor_tick', '0.05') + return world.spawn_actor(blueprint, transform, attach_to=vehicle) + + c3_cameras = not args.only_c2_cameras + camerad = Camerad(wide_camera=c3_cameras) + road_camera = create_camera(fov=40) + road_camera.listen(camerad.cam_callback_road) + cameras = [road_camera] + if c3_cameras: + road_wide_camera = create_camera(fov=165) # fov bigger than 165 shows unwanted artifacts + road_wide_camera.listen(camerad.cam_callback_wide_road) + cameras.append(road_wide_camera) vehicle_state = VehicleState() # reenable IMU @@ -432,7 +460,8 @@ def bridge(q): t.join() gps.destroy() imu.destroy() - camera.destroy() + for c in cameras: + c.destroy() vehicle.destroy() @@ -441,8 +470,8 @@ def bridge_keep_alive(q: Any): try: bridge(q) break - except RuntimeError: - print("Restarting bridge...") + except RuntimeError as e: + print("Restarting bridge. Error:", e) if __name__ == "__main__":