You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
134 lines
4.1 KiB
134 lines
4.1 KiB
import sys
|
|
import os
|
|
|
|
# Add project root to Python path
|
|
sys.path.append(os.path.dirname(os.path.dirname(__file__)))
|
|
|
|
import carla
|
|
import queue
|
|
import config
|
|
|
|
|
|
class SensorManager:
|
|
def __init__(self, world, blueprint_library, vehicle):
|
|
self.world = world
|
|
self.bp_lib = blueprint_library
|
|
self.vehicle = vehicle
|
|
|
|
self.camera = None
|
|
self.radar = None
|
|
self.lidar = None
|
|
|
|
# Queues for synchronization
|
|
self.camera_queue = queue.Queue()
|
|
self.camera_tpp_queue = queue.Queue()
|
|
self.radar_queue = queue.Queue()
|
|
self.lidar_queue = queue.Queue()
|
|
|
|
def spawn_sensors(self):
|
|
self._spawn_camera()
|
|
self._spawn_camera_tpp()
|
|
self._spawn_radar()
|
|
self._spawn_lidar()
|
|
|
|
# ---------------- CAMERA ----------------
|
|
def _spawn_camera(self):
|
|
cam_bp = self.bp_lib.find("sensor.camera.rgb")
|
|
|
|
cam_bp.set_attribute("image_size_x", str(config.CAM_WIDTH))
|
|
cam_bp.set_attribute("image_size_y", str(config.CAM_HEIGHT))
|
|
cam_bp.set_attribute("fov", str(config.CAM_FOV))
|
|
|
|
transform = carla.Transform(
|
|
carla.Location(x=1.5, z=2.4)
|
|
)
|
|
|
|
self.camera = self.world.spawn_actor(
|
|
cam_bp, transform, attach_to=self.vehicle
|
|
)
|
|
|
|
self.camera.listen(lambda data: self.camera_queue.put(data))
|
|
|
|
def _spawn_camera_tpp(self):
|
|
cam_bp = self.bp_lib.find("sensor.camera.rgb")
|
|
cam_bp.set_attribute("image_size_x", str(config.CAM_WIDTH))
|
|
cam_bp.set_attribute("image_size_y", str(config.CAM_HEIGHT))
|
|
cam_bp.set_attribute("fov", str(config.CAM_FOV))
|
|
|
|
transform = carla.Transform(
|
|
carla.Location(x=-5.5, z=2.8),
|
|
carla.Rotation(pitch=-15)
|
|
)
|
|
|
|
self.camera_tpp = self.world.spawn_actor(
|
|
cam_bp, transform, attach_to=self.vehicle
|
|
)
|
|
|
|
self.camera_tpp.listen(lambda data: self.camera_tpp_queue.put(data))
|
|
|
|
# ---------------- RADAR ----------------
|
|
def _spawn_radar(self):
|
|
radar_bp = self.bp_lib.find("sensor.other.radar")
|
|
|
|
radar_bp.set_attribute("horizontal_fov", str(config.RADAR_HORIZONTAL_FOV))
|
|
radar_bp.set_attribute("vertical_fov", str(config.RADAR_VERTICAL_FOV))
|
|
radar_bp.set_attribute("range", str(config.RADAR_RANGE))
|
|
|
|
transform = carla.Transform(
|
|
carla.Location(x=2.0, z=1.0)
|
|
)
|
|
|
|
self.radar = self.world.spawn_actor(
|
|
radar_bp, transform, attach_to=self.vehicle
|
|
)
|
|
|
|
self.radar.listen(lambda data: self.radar_queue.put(data))
|
|
|
|
# ---------------- LIDAR ----------------
|
|
def _spawn_lidar(self):
|
|
lidar_bp = self.bp_lib.find("sensor.lidar.ray_cast_semantic")
|
|
|
|
lidar_bp.set_attribute("range", str(config.LIDAR_RANGE))
|
|
lidar_bp.set_attribute("channels", str(config.LIDAR_CHANNELS))
|
|
lidar_bp.set_attribute("points_per_second", str(config.LIDAR_POINTS_PER_SECOND))
|
|
lidar_bp.set_attribute("rotation_frequency", str(config.LIDAR_ROTATION_FREQUENCY))
|
|
|
|
transform = carla.Transform(
|
|
carla.Location(x=0.0, z=2.5)
|
|
)
|
|
|
|
self.lidar = self.world.spawn_actor(
|
|
lidar_bp, transform, attach_to=self.vehicle
|
|
)
|
|
|
|
self.lidar.listen(lambda data: self.lidar_queue.put(data))
|
|
|
|
# ---------------- FETCH SYNC DATA ----------------
|
|
def get_data(self):
|
|
cam = self.camera_queue.get()
|
|
cam_tpp = self.camera_tpp_queue.get()
|
|
radar = self.radar_queue.get()
|
|
lidar = self.lidar_queue.get()
|
|
|
|
# Strong sync check
|
|
assert cam.frame == cam_tpp.frame == radar.frame == lidar.frame, "Frame mismatch!"
|
|
|
|
return cam, cam_tpp, radar, lidar
|
|
|
|
# ---------------- CLEANUP ----------------
|
|
def destroy(self):
|
|
if self.camera:
|
|
self.camera.stop()
|
|
self.camera.destroy()
|
|
|
|
if self.camera_tpp:
|
|
self.camera_tpp.stop()
|
|
self.camera_tpp.destroy()
|
|
|
|
if self.radar:
|
|
self.radar.stop()
|
|
self.radar.destroy()
|
|
|
|
if self.lidar:
|
|
self.lidar.stop()
|
|
self.lidar.destroy()
|