Merge 2d7265dad9
into ea74a90ca0
commit
c79655e849
|
@ -6,8 +6,10 @@ from selfdrive.hardware import TICI
|
||||||
## -- hardcoded hardware params --
|
## -- hardcoded hardware params --
|
||||||
eon_f_focal_length = 910.0
|
eon_f_focal_length = 910.0
|
||||||
eon_d_focal_length = 650.0
|
eon_d_focal_length = 650.0
|
||||||
|
# tici_f: narrow road cam. tici_e: wide road cam
|
||||||
tici_f_focal_length = 2648.0
|
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_f_frame_size = (1164, 874)
|
||||||
eon_d_frame_size = (816, 612)
|
eon_d_frame_size = (816, 612)
|
||||||
|
|
|
@ -11,6 +11,8 @@ import carla # pylint: disable=import-error
|
||||||
import numpy as np
|
import numpy as np
|
||||||
import pyopencl as cl
|
import pyopencl as cl
|
||||||
import pyopencl.array as cl_array
|
import pyopencl.array as cl_array
|
||||||
|
|
||||||
|
from common.transformations.camera import tici_d_and_e_focal_length
|
||||||
from lib.can import can_function
|
from lib.can import can_function
|
||||||
|
|
||||||
import cereal.messaging as messaging
|
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 = argparse.ArgumentParser(description='Bridge between CARLA and openpilot.')
|
||||||
parser.add_argument('--joystick', action='store_true')
|
parser.add_argument('--joystick', action='store_true')
|
||||||
parser.add_argument('--low_quality', 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('--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)
|
||||||
|
|
||||||
|
@ -36,7 +39,7 @@ REPEAT_COUNTER = 5
|
||||||
PRINT_DECIMATION = 100
|
PRINT_DECIMATION = 100
|
||||||
STEER_RATIO = 15.
|
STEER_RATIO = 15.
|
||||||
|
|
||||||
pm = messaging.PubMaster(['roadCameraState', 'sensorEvents', 'can', "gpsLocationExternal"])
|
pm = messaging.PubMaster(['roadCameraState', "wideRoadCameraState", 'sensorEvents', 'can', "gpsLocationExternal"])
|
||||||
sm = messaging.SubMaster(['carControl', 'controlsState'])
|
sm = messaging.SubMaster(['carControl', 'controlsState'])
|
||||||
|
|
||||||
|
|
||||||
|
@ -62,13 +65,18 @@ def steer_rate_limit(old, new):
|
||||||
|
|
||||||
|
|
||||||
class Camerad:
|
class Camerad:
|
||||||
def __init__(self):
|
def __init__(self, wide_camera=False):
|
||||||
self.frame_id = 0
|
self.frame_road_id = 0
|
||||||
|
self.frame_wide_id = 0
|
||||||
self.vipc_server = VisionIpcServer("camerad")
|
self.vipc_server = VisionIpcServer("camerad")
|
||||||
|
|
||||||
# TODO: remove RGB buffers once the last RGB vipc subscriber is removed
|
# 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_RGB_ROAD, 4, True, W, H)
|
||||||
self.vipc_server.create_buffers(VisionStreamType.VISION_STREAM_ROAD, 40, False, 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()
|
self.vipc_server.start_listener()
|
||||||
|
|
||||||
# set up for pyopencl rgb to yuv conversion
|
# 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.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
|
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.frombuffer(image.raw_data, dtype=np.dtype("uint8"))
|
||||||
img = np.reshape(img, (H, W, 4))
|
img = np.reshape(img, (H, W, 4))
|
||||||
img = img[:, :, [0, 1, 2]].copy()
|
img = img[:, :, [0, 1, 2]].copy()
|
||||||
|
@ -94,21 +112,21 @@ class Camerad:
|
||||||
yuv_cl = cl_array.empty_like(rgb_cl)
|
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()
|
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))
|
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
|
# 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(rgb_type, img.tobytes(), frame_id, eof, eof)
|
||||||
self.vipc_server.send(VisionStreamType.VISION_STREAM_ROAD, yuv.data.tobytes(), self.frame_id, eof, eof)
|
self.vipc_server.send(yuv_type, yuv.data.tobytes(), frame_id, eof, eof)
|
||||||
|
|
||||||
dat = messaging.new_message('roadCameraState')
|
dat = messaging.new_message(pub_type)
|
||||||
dat.roadCameraState = {
|
msg = {
|
||||||
"frameId": image.frame,
|
"frameId": image.frame,
|
||||||
"transform": [1.0, 0.0, 0.0,
|
"transform": [1.0, 0.0, 0.0,
|
||||||
0.0, 1.0, 0.0,
|
0.0, 1.0, 0.0,
|
||||||
0.0, 0.0, 1.0]
|
0.0, 0.0, 1.0]
|
||||||
}
|
}
|
||||||
pm.send('roadCameraState', dat)
|
setattr(dat, pub_type, msg)
|
||||||
self.frame_id += 1
|
pm.send(pub_type, dat)
|
||||||
|
|
||||||
|
|
||||||
def imu_callback(imu, vehicle_state):
|
def imu_callback(imu, vehicle_state):
|
||||||
|
@ -259,16 +277,26 @@ def bridge(q):
|
||||||
physics_control.gear_switch_time = 0.0
|
physics_control.gear_switch_time = 0.0
|
||||||
vehicle.apply_physics_control(physics_control)
|
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))
|
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()
|
vehicle_state = VehicleState()
|
||||||
|
|
||||||
# reenable IMU
|
# reenable IMU
|
||||||
|
@ -432,7 +460,8 @@ def bridge(q):
|
||||||
t.join()
|
t.join()
|
||||||
gps.destroy()
|
gps.destroy()
|
||||||
imu.destroy()
|
imu.destroy()
|
||||||
camera.destroy()
|
for c in cameras:
|
||||||
|
c.destroy()
|
||||||
vehicle.destroy()
|
vehicle.destroy()
|
||||||
|
|
||||||
|
|
||||||
|
@ -441,8 +470,8 @@ def bridge_keep_alive(q: Any):
|
||||||
try:
|
try:
|
||||||
bridge(q)
|
bridge(q)
|
||||||
break
|
break
|
||||||
except RuntimeError:
|
except RuntimeError as e:
|
||||||
print("Restarting bridge...")
|
print("Restarting bridge. Error:", e)
|
||||||
|
|
||||||
|
|
||||||
if __name__ == "__main__":
|
if __name__ == "__main__":
|
||||||
|
|
Loading…
Reference in New Issue