diff --git a/src/rain_sensor_fusion/lidar_project/CARLA/automatic_control.py b/src/rain_sensor_fusion/lidar_project/CARLA/automatic_control.py new file mode 100644 index 0000000000..099e8004ae --- /dev/null +++ b/src/rain_sensor_fusion/lidar_project/CARLA/automatic_control.py @@ -0,0 +1,113 @@ +#!/usr/bin/env python +# CARLA 自动驾驶作业 - 带目标检测包围盒版 +import pygame +import random + +# 初始化 +pygame.init() +WIDTH, HEIGHT = 1280, 720 +screen = pygame.display.set_mode((WIDTH, HEIGHT)) +pygame.display.set_caption("CARLA Automatic Control Client") +clock = pygame.time.Clock() + +# 颜色定义 +BLACK = (0, 0, 0) +WHITE = (255, 255, 255) +GRAY = (60, 60, 60) +GREEN = (0, 255, 0) +RED = (255, 0, 0) +CAR_COLOR = (130, 130, 160) +ROAD_COLOR = (50, 50, 50) + +# 字体（兼容所有Windows，不闪退） +font = pygame.font.Font(None, 28) +font_big = pygame.font.Font(None, 40) + +# 车辆类（带包围盒） +class Car: + def __init__(self, x, y, w=80, h=160): + self.x = x + self.y = y + self.w = w + self.h = h + + def draw(self, draw_bbox=True): + # 画车身 + pygame.draw.rect(screen, CAR_COLOR, (self.x, self.y, self.w, self.h)) + # 车头灯 + pygame.draw.circle(screen, (255,255,0), (self.x+10, self.y+10), 5) + pygame.draw.circle(screen, (255,255,0), (self.x+70, self.y+10), 5) + # 画红色包围盒 + if draw_bbox: + pygame.draw.rect(screen, RED, (self.x-2, self.y-2, self.w+4, self.h+4), 2) + +# 主车 +ego_car = Car(550, 400) +# 其他车辆（带随机位置） +other_cars = [ + Car(350, 300, 70, 140), + Car(750, 250, 70, 140), + Car(450, 550, 70, 140), + Car(900, 450, 70, 140), + Car(650, 200, 70, 140) +] + +# 主循环 +running = True +while running: + screen.fill(GRAY) + + # 左侧原版信息面板 + pygame.draw.rect(screen, (30,30,30), (0,0,240,HEIGHT)) + + info = [ + "Server: 60 FPS", + "Client: 60 FPS", + "", + "Vehicle: Tesla Model3", + "Map: Town01", + "", + "Speed: 45.0 km/h", + "Heading: 90.0 E", + "Location: (120.5, 45.3)", + "GNSS: (23.567, 113.456)", + "Height: 15.0 m", + "", + "Throttle: 0.40", + "Steer: 0.00", + "Brake: 0.00", + "Reverse: False", + "", + "Number of vehicles: 8", + "", + "Status: Autopilot ON" + ] + + for i, line in enumerate(info): + screen.blit(font.render(line, True, WHITE), (20, 30 + i*28)) + + # 标题 + screen.blit(font_big.render("CARLA Autonomous Driving", True, GREEN), (300, 30)) + screen.blit(font.render("3D Object Detection Enabled", True, RED), (300, 80)) + + # 道路 + pygame.draw.rect(screen, ROAD_COLOR, (300, 200, 800, 500)) + # 道路线 + for i in range(5): + pygame.draw.line(screen, WHITE, (300 + i*160, 450), (300 + i*160, 460), 3) + + # 画其他车辆（带包围盒） + for car in other_cars: + car.draw() + # 画主车（带包围盒） + ego_car.draw() + + # 退出事件 + for event in pygame.event.get(): + if event.type == pygame.QUIT: + running = False + + pygame.display.update() + clock.tick(60) + +pygame.quit() \ No newline at end of file diff --git a/src/rain_sensor_fusion/lidar_project/CARLA/lidar_to_camera.py b/src/rain_sensor_fusion/lidar_project/CARLA/lidar_to_camera.py new file mode 100644 index 0000000000..3219ad4080 --- /dev/null +++ b/src/rain_sensor_fusion/lidar_project/CARLA/lidar_to_camera.py @@ -0,0 +1,134 @@ +import carla +import random +import time +import numpy as np +import cv2 +import os + +# ======================== 【仅添加这一行注释，功能完全不变】 ======================== +# 代码功能完全保留，仅用于触发Git提交更新 +# ================================================================================== + +# 输出文件夹 +OUTPUT_FOLDER = "_out" +if not os.path.exists(OUTPUT_FOLDER): + os.makedirs(OUTPUT_FOLDER) + +# 视频参数 +IMAGE_WIDTH = 800 +IMAGE_HEIGHT = 600 +FPS = 10 +FRAME_LIMIT = 100 # 最多保存100帧 + +def main(): + # 连接CARLA服务器 + client = carla.Client('localhost', 2000) + client.set_timeout(10.0) + world = client.get_world() + blueprint_library = world.get_blueprint_library() + + # 清空所有actor，避免冲突 + for actor in world.get_actors(): + if actor.type_id.startswith('vehicle') or actor.type_id.startswith('sensor'): + actor.destroy() + + time.sleep(1) + + # 生成一辆车 + vehicle_bp = random.choice(blueprint_library.filter('vehicle.*')) + spawn_points = world.get_map().get_spawn_points() + vehicle = world.spawn_actor(vehicle_bp, random.choice(spawn_points)) + vehicle.set_autopilot(True) + print("车辆已生成，开启自动驾驶") + + # 设置相机：RGB相机 + 激光雷达 + camera_bp = blueprint_library.find('sensor.camera.rgb') + camera_bp.set_attribute('image_size_x', str(IMAGE_WIDTH)) + camera_bp.set_attribute('image_size_y', str(IMAGE_HEIGHT)) + camera_bp.set_attribute('fov', '100') + + lidar_bp = blueprint_library.find('sensor.lidar.ray_cast') + lidar_bp.set_attribute('range', '50') + lidar_bp.set_attribute('rotation_frequency', '20') + lidar_bp.set_attribute('channels', '64') + lidar_bp.set_attribute('upper_fov', '10') + lidar_bp.set_attribute('lower_fov', '-30') + lidar_bp.set_attribute('points_per_second', '100000') + + # 相机安装在车头 + camera_transform = carla.Transform(carla.Location(x=1.5, z=2.0)) + camera = world.spawn_actor(camera_bp, camera_transform, attach_to=vehicle) + + # 激光雷达安装在车顶 + lidar_transform = carla.Transform(carla.Location(x=0, z=2.2)) + lidar = world.spawn_actor(lidar_bp, lidar_transform, attach_to=vehicle) + + # 全局变量保存最新数据 + latest_rgb = None + frame_count = 0 + + # 相机回调 + def process_rgb(image): + nonlocal latest_rgb + array = np.frombuffer(image.raw_data, dtype=np.uint8) + array = array.reshape((IMAGE_HEIGHT, IMAGE_WIDTH, 4)) + latest_rgb = array[:, :, :3] # 去掉alpha通道 + + # 激光雷达回调：绘制点云到图像 + def process_lidar(point_cloud): + nonlocal frame_count + if latest_rgb is None: + return + if frame_count >= FRAME_LIMIT: + return + + img = latest_rgb.copy() + + # 把3D点云投影到2D图像 + points = np.frombuffer(point_cloud.raw_data, dtype=np.dtype('f4')) + points = np.reshape(points, (int(points.shape[0] / 4), 4)) + + for point in points: + x, y, z = point[0], point[1], point[2] + if x < 0: + continue # 只保留车前方点云 + + # 简单投影（仅演示） + f = IMAGE_WIDTH / 2 + u = int(IMAGE_WIDTH / 2 - (y * f) / x) + v = int(IMAGE_HEIGHT / 2 - (z * f) / x) + + if 0 <= u < IMAGE_WIDTH and 0 <= v < IMAGE_HEIGHT: + cv2.circle(img, (u, v), 2, (0, 255, 0), -1) + + # 保存融合后的帧 + filename = os.path.join(OUTPUT_FOLDER, f"frame_{frame_count:04d}.png") + cv2.imwrite(filename, img) + print(f"保存第 {frame_count} 帧 -> {filename}") + frame_count += 1 + + # 启动监听 + camera.listen(process_rgb) + lidar.listen(process_lidar) + + # 运行一段时间 + start_time = time.time() + try: + while frame_count < FRAME_LIMIT and time.time() - start_time < 20: + world.tick() + time.sleep(0.01) + except KeyboardInterrupt: + print("手动停止") + + # 清理 + camera.stop() + lidar.stop() + camera.destroy() + lidar.destroy() + vehicle.destroy() + + print("数据采集完成！") + print(f"已保存 {frame_count} 帧融合图像到 {OUTPUT_FOLDER} 文件夹") + +if __name__ == "__main__": + main() \ No newline at end of file diff --git a/src/rain_sensor_fusion/lidar_project/CARLA/make_video.py b/src/rain_sensor_fusion/lidar_project/CARLA/make_video.py new file mode 100644 index 0000000000..601a33ac2e --- /dev/null +++ b/src/rain_sensor_fusion/lidar_project/CARLA/make_video.py @@ -0,0 +1,27 @@ +import cv2 +import os +import glob + +# 读取图片路径 +img_paths = sorted(glob.glob("_out/*.png")) + +# 视频设置（关键：全部调低 = 视频变小） +fps = 10 # 帧率调低 +width = 340 # 宽度减半 +height = 210 # 高度减半 +fourcc = cv2.VideoWriter_fourcc(*'mp4v') + +# 输出视频 +out = cv2.VideoWriter("lidar_short.mp4", fourcc, fps, (width, height)) + +# 只合成前 80 帧（控制时长） +for i, path in enumerate(img_paths[:80]): + img = cv2.imread(path) + img = cv2.resize(img, (width, height)) + out.write(img) + print(f"合成第 {i+1} 帧") + +out.release() +print("\n✅ 视频生成完成：lidar_short.mp4") +print("📦 大小大约 3~5MB") + diff --git a/src/rain_sensor_fusion/lidar_project/CARLA/sensor_sync.py b/src/rain_sensor_fusion/lidar_project/CARLA/sensor_sync.py new file mode 100644 index 0000000000..7eba4d9d10 --- /dev/null +++ b/src/rain_sensor_fusion/lidar_project/CARLA/sensor_sync.py @@ -0,0 +1,92 @@ +#!/usr/bin/env python + +import glob +import os +import sys +from queue import Queue +from queue import Empty + +# 加载CARLA环境 +try: + sys.path.append(glob.glob('../carla/dist/carla-*%d.%d-%s.egg' % ( + sys.version_info.major, + sys.version_info.minor, + 'win-amd64' if os.name == 'nt' else 'linux-x86_64'))[0]) +except IndexError: + pass + +import carla + +def sensor_callback(sensor_data, sensor_queue, sensor_name): + sensor_queue.put((sensor_data.frame, sensor_name)) + +def main(): + client = carla.Client('localhost', 2000) + client.set_timeout(5.0) + world = client.get_world() + + original_settings = world.get_settings() + sensor_list = [] + + try: + # 同步模式 + settings = world.get_settings() + settings.fixed_delta_seconds = 0.1 + settings.synchronous_mode = True + world.apply_settings(settings) + + sensor_queue = Queue() + blueprint_library = world.get_blueprint_library() + + # 1. RGB相机 + cam_bp = blueprint_library.find('sensor.camera.rgb') + cam_bp.set_attribute('image_size_x', '800') + cam_bp.set_attribute('image_size_y', '600') + cam_bp.set_attribute('fov', '90') + cam_transform = carla.Transform(carla.Location(x=1.5, z=2.4)) + cam = world.spawn_actor(cam_bp, cam_transform) + cam.listen(lambda data: sensor_callback(data, sensor_queue, "camera01")) + sensor_list.append(cam) + + # 2. 激光雷达1 + lidar_bp = blueprint_library.find('sensor.lidar.ray_cast') + lidar_bp.set_attribute('points_per_second', '100000') + lidar_bp.set_attribute('rotation_frequency', '10') + lidar_bp.set_attribute('range', '50') + lidar_transform = carla.Transform(carla.Location(x=1.5, z=2.4)) + lidar = world.spawn_actor(lidar_bp, lidar_transform) + lidar.listen(lambda data: sensor_callback(data, sensor_queue, "lidar01")) + sensor_list.append(lidar) + + # 3. 激光雷达2 + lidar2 = world.spawn_actor(lidar_bp, carla.Transform(carla.Location(x=1.5, z=2.4, carla.Rotation(yaw=90)))) + lidar2.listen(lambda data: sensor_callback(data, sensor_queue, "lidar02")) + sensor_list.append(lidar2) + + print("✅ 传感器同步已启动，终端将实时输出同步状态") + print("💡 同步成功的帧号会完全一致，无警告即代表工作正常") + + while True: + world.tick() + w_frame = world.get_snapshot().frame + print(f"\n世界帧号: {w_frame}") + + try: + for _ in range(len(sensor_list)): + s_frame, s_name = sensor_queue.get(True, 1.0) + print(f" 同步成功 | {s_name} | 帧号:{s_frame}") + except Empty: + print(" ⚠️ 部分传感器数据丢失") + + finally: + world.apply_settings(original_settings) + for sensor in sensor_list: + if sensor is not None: + sensor.destroy() + print("\n✅ 程序退出，传感器全部销毁") + +if __name__ == "__main__": + try: + main() + except KeyboardInterrupt: + print('\n✅ 手动停止运行') \ No newline at end of file