新增核心功能：语义分割摄像头

src/carla_autonomous_driving_perception/carla_model3_spawn_with_spectator.py

@@ -9,38 +9,63 @@
 
 # 自定义线性插值函数（适配同步帧）
 def lerp(a, b, t):
-    """线性插值：t值根据同步帧率调整（30帧下0.15更稳定）"""
     return a + t * (b - a)
 
+# 语义分割调色板（Cityscapes格式，兼容所有CARLA版本）
+CITYSCAPES_PALETTE = [
+    (0, 0, 0),          # 0: 未标注
+    (70, 70, 70),       # 1: 建筑物
+    (100, 40, 40),      # 2: 围栏
+    (55, 90, 80),       # 3: 其他
+    (220, 20, 60),      # 4: 行人
+    (153, 153, 153),    # 5: 杆子
+    (157, 234, 50),     # 6: 道路线
+    (128, 64, 128),     # 7: 道路
+    (244, 35, 232),     # 8: 人行道
+    (107, 142, 35),     # 9: 植被
+    (0, 0, 142),        # 10: 车辆
+    (102, 102, 156),    # 11: 墙壁
+    (220, 220, 0),      # 12: 交通灯
+    (70, 130, 180),     # 13: 交通标志
+    (81, 0, 81),        # 14: 天
+    (150, 100, 100),    # 15: 地形
+    (230, 150, 140),    # 16: 护栏
+    (180, 165, 180),    # 17: 栅栏
+    (250, 170, 30),     # 18: 静态
+    (110, 190, 160),    # 19: 动态
+    (170, 120, 50),     # 20: 其他
+    (45, 60, 150),      # 21: 水
+    (145, 170, 100)     # 22: 路面标记
+]
+
 # 1. 连接CARLA服务器并配置强同步模式
 client = carla.Client('localhost', 2000)
 client.set_timeout(15.0)
 world = client.load_world('Town05')
 
-# 启用严格同步模式（关键：固定帧间隔，禁用异步更新）
+# 启用严格同步模式
 settings = world.get_settings()
-settings.synchronous_mode = True  # 客户端控制帧推进
-settings.fixed_delta_seconds = 1/30  # 30帧/秒（与后续tick频率一致）
-settings.no_rendering_mode = False  # 启用渲染
+settings.synchronous_mode = True
+settings.fixed_delta_seconds = 1/30
+settings.no_rendering_mode = False
 world.apply_settings(settings)
 
-# 2. 初始化同步锁与帧数据缓存（确保线程安全）
+# 2. 初始化同步锁与帧数据缓存
 frame_lock = Lock()
-latest_snapshot = None  # 存储当前帧的Actor快照（含车辆状态）
+latest_snapshot = None
 
-# 绑定帧同步回调：每帧更新车辆状态快照
+# 绑定帧同步回调
 def on_world_tick(snapshot):
     global latest_snapshot
     with frame_lock:
-        latest_snapshot = snapshot  # 缓存当前帧的所有Actor状态
+        latest_snapshot = snapshot
 world.on_tick(on_world_tick)
 
 bp_lib = world.get_blueprint_library()
 spawn_points = world.get_map().get_spawn_points()
 
 # 3. 生成主角车辆（Tesla Model3）
 model3_bp = bp_lib.find('vehicle.tesla.model3')
-# 确保生成点有效（避免初始位置异常导致抖动）
 vehicle = None
 for _ in range(5):
     try:
@@ -52,52 +77,57 @@ def on_world_tick(snapshot):
 if not vehicle:
     raise Exception("主角车辆生成失败，请重启CARLA服务器")
 
-# 4. 初始化实时RGB摄像头（新增模块）
-def init_camera(vehicle):
-    """初始化绑定到主角车的RGB摄像头，返回摄像头actor和图像队列"""
-    # 摄像头蓝图配置
+# 4. 初始化RGB摄像头（保留shutter_speed，该传感器支持）
+def init_rgb_camera(vehicle):
     camera_bp = bp_lib.find('sensor.camera.rgb')
-    camera_bp.set_attribute('image_size_x', '1024')  # 图像宽度
-    camera_bp.set_attribute('image_size_y', '720')   # 图像高度
-    camera_bp.set_attribute('fov', '90')             # 视场角
-    camera_bp.set_attribute('shutter_speed', '100')  # 减少运动模糊
-
-    # 摄像头安装位置：车前方2米，高度1.5米，略微上仰（便于观察前方路况）
+    camera_bp.set_attribute('image_size_x', '1024')
+    camera_bp.set_attribute('image_size_y', '720')
+    camera_bp.set_attribute('fov', '90')
+    camera_bp.set_attribute('shutter_speed', '100')  # RGB摄像头支持此属性
     camera_transform = carla.Transform(
         carla.Location(x=2.0, z=1.5),
         carla.Rotation(pitch=-5)
     )
-
-    # 生成摄像头并绑定到主角车
-    camera = world.spawn_actor(
-        camera_bp,
-        camera_transform,
-        attach_to=vehicle
-    )
-
-    # 创建图像队列（线程安全）
+    camera = world.spawn_actor(camera_bp, camera_transform, attach_to=vehicle)
     image_queue = queue.Queue()
-    camera.listen(image_queue.put)  # 摄像头数据存入队列
-
-    print("RGB摄像头初始化完成，实时画面将在窗口显示（按'q'关闭）")
+    camera.listen(image_queue.put)
+    print("RGB摄像头初始化完成")
     return camera, image_queue
 
-# 初始化摄像头
-camera, image_queue = init_camera(vehicle)
-
-# 5. 生成NPC车辆（减少至100辆，确保同步性能）
-npc_count = 100  # 500辆会导致同步延迟，100辆是性能与效果的平衡
+# 5. 初始化语义分割摄像头（移除shutter_speed，该传感器不支持）
+def init_semantic_camera(vehicle):
+    """初始化语义分割摄像头，返回传感器和数据队列"""
+    sem_bp = bp_lib.find('sensor.camera.semantic_segmentation')
+    # 仅保留语义分割摄像头支持的属性
+    sem_bp.set_attribute('image_size_x', '1024')
+    sem_bp.set_attribute('image_size_y', '720')
+    sem_bp.set_attribute('fov', '90')
+    # 移除shutter_speed设置（语义分割摄像头不支持）
+    sem_transform = carla.Transform(
+        carla.Location(x=2.0, z=1.5),
+        carla.Rotation(pitch=-5)
+    )
+    sem_camera = world.spawn_actor(sem_bp, sem_transform, attach_to=vehicle)
+    sem_queue = queue.Queue()
+    sem_camera.listen(sem_queue.put)  # 语义数据存入队列
+    print("语义分割摄像头初始化完成")
+    return sem_camera, sem_queue
+
+# 初始化摄像头（同时初始化RGB和语义分割）
+rgb_camera, rgb_queue = init_rgb_camera(vehicle)
+sem_camera, sem_queue = init_semantic_camera(vehicle)  # 新增语义摄像头
+
+# 6. 生成NPC车辆（保留原有逻辑）
+npc_count = 100
 print(f"开始生成{npc_count}辆NPC车辆...")
 for i in range(npc_count):
     vehicle_bp = random.choice(bp_lib.filter('vehicle'))
-    if 'tesla' in vehicle_bp.id:  # 避免与主角车混淆
+    if 'tesla' in vehicle_bp.id:
         continue
-    # 尝试生成（避开主角车位置）
     spawn_point = random.choice(spawn_points)
     if spawn_point.location.distance(vehicle.get_location()) < 20:
         continue
     world.try_spawn_actor(vehicle_bp, spawn_point)
-    # 每生成20辆同步一次，确保服务器不卡顿
     if i % 20 == 0:
         world.tick()
         time.sleep(0.1)
@@ -107,40 +137,32 @@ def init_camera(vehicle):
 actual_npc_count = len(all_vehicles) - 1
 print(f"NPC生成完成 | 实际数量: {actual_npc_count}辆（总车辆: {len(all_vehicles)}）")
 
-# 6. 启动所有车辆自动驾驶（绑定交通管理器同步端口）
+# 7. 启动所有车辆自动驾驶
 tm = client.get_trafficmanager(8000)
-tm.set_synchronous_mode(True)  # 交通管理器也启用同步模式
+tm.set_synchronous_mode(True)
 for v in all_vehicles:
-    v.set_autopilot(True, tm.get_port())  # 所有车辆通过TM控制，确保行为同步
+    v.set_autopilot(True, tm.get_port())
 
-# 7. 平滑视角函数（基于当前帧快照数据）
+# 8. 平滑视角函数（保留原有逻辑）
 def set_spectator_smooth(last_transform=None):
-    """
-    基于当前帧快照更新视角，彻底避免异步抖动
-    数据来源：on_world_tick缓存的latest_snapshot（当前帧精确状态）
-    """
     spectator = world.get_spectator()
     with frame_lock:
         if not latest_snapshot:
-            return last_transform  # 等待第一帧数据
-        # 从当前帧快照中获取主角车的精确状态（而非实时查询）
+            return last_transform
         vehicle_snapshot = latest_snapshot.find(vehicle.id)
         if not vehicle_snapshot:
             return last_transform
-        vehicle_tf = vehicle_snapshot.get_transform()  # 这是当前帧的精确位置
+        vehicle_tf = vehicle_snapshot.get_transform()
 
-    # 目标视角：车后8米、上方3米，轻微右偏（便于观察整车和周围环境）
     target_tf = carla.Transform(
         vehicle_tf.transform(carla.Location(x=-8, z=3, y=0.5)),
         vehicle_tf.rotation
     )
 
-    # 首次调用直接设置
     if last_transform is None:
         spectator.set_transform(target_tf)
         return target_tf
 
-    # 插值平滑（t=0.15适配30帧，同步模式下更稳定）
     smooth_loc = carla.Location(
         x=lerp(last_transform.location.x, target_tf.location.x, 0.15),
         y=lerp(last_transform.location.y, target_tf.location.y, 0.15),
@@ -155,46 +177,55 @@ def set_spectator_smooth(last_transform=None):
     spectator.set_transform(smooth_tf)
     return smooth_tf
 
-# 8. 主循环（整合实时摄像头画面与原有逻辑）
-print("\n程序运行中（强同步模式），按Ctrl+C或摄像头窗口按'q'退出...")
-print("功能：实时RGB摄像头画面 + 车辆自动驾驶 + 平滑视角")
+# 9. 主循环（处理图像显示）
+print("\n程序运行中，按Ctrl+C或任一窗口按'q'退出...")
+print("功能：RGB摄像头 + 语义分割摄像头 + 车辆自动驾驶 + 平滑视角")
 last_spectator_tf = None
 clock = pygame.time.Clock()
 
 try:
-    # 先推进一帧获取初始快照
     world.tick()
     last_spectator_tf = set_spectator_smooth()
 
     while True:
-        # 推进一帧（触发世界更新和摄像头数据采集）
         world.tick()
-
-        # 更新 spectator 视角（平滑跟随）
         last_spectator_tf = set_spectator_smooth(last_spectator_tf)
 
-        # 处理实时摄像头画面（新增逻辑）
-        if not image_queue.empty():
-            image = image_queue.get()
-            # 将原始数据转换为RGBA格式并reshape
-            img = np.reshape(np.copy(image.raw_data), (image.height, image.width, 4))
-            # 显示图像（OpenCV窗口）
-            cv2.imshow('CARLA RGB Camera', img)
-            # 按'q'键退出
+        # 处理RGB图像
+        if not rgb_queue.empty():
+            rgb_image = rgb_queue.get()
+            rgb_img = np.reshape(np.copy(rgb_image.raw_data), 
+                                (rgb_image.height, rgb_image.width, 4))
+            cv2.imshow('RGB Camera', rgb_img)
+            if cv2.waitKey(1) == ord('q'):
+                break
+
+        # 处理语义分割图像
+        if not sem_queue.empty():
+            sem_image = sem_queue.get()
+            # 提取语义分割原始数据（单通道类别ID）
+            sem_data = np.reshape(np.copy(sem_image.raw_data), 
+                                (sem_image.height, sem_image.width, 4))[:, :, 2].astype(np.int32)
+            # 映射到Cityscapes调色板（转换为RGB可视化）
+            sem_rgb = np.zeros((sem_image.height, sem_image.width, 3), dtype=np.uint8)
+            for i in range(len(CITYSCAPES_PALETTE)):
+                sem_rgb[sem_data == i] = CITYSCAPES_PALETTE[i]
+            cv2.imshow('Semantic Segmentation', sem_rgb)
             if cv2.waitKey(1) == ord('q'):
                 break
 
-        # 控制客户端帧率与服务器同步
         clock.tick(30)
 
 except KeyboardInterrupt:
     print("\n用户中断，清理资源...")
 finally:
-    # 清理摄像头资源（关键：避免残留传感器）
-    camera.stop()  # 停止摄像头监听
-    camera.destroy()  # 销毁摄像头actor
+    # 清理所有传感器
+    rgb_camera.stop()
+    rgb_camera.destroy()
+    sem_camera.stop()
+    sem_camera.destroy()
 
-    # 恢复CARLA默认设置
+    # 恢复CARLA设置
     settings.synchronous_mode = False
     tm.set_synchronous_mode(False)
     world.apply_settings(settings)
@@ -204,6 +235,5 @@ def set_spectator_smooth(last_transform=None):
         if v.is_alive:
             v.destroy()
 
-    # 关闭所有OpenCV窗口
     cv2.destroyAllWindows()
     print("资源清理完成，同步模式已关闭")