diff --git a/src/car_tracking/carla_visualization/CMakeLists.txt b/src/car_tracking/carla_visualization/CMakeLists.txt new file mode 100644 index 0000000000..d3ab40e5d8 --- /dev/null +++ b/src/car_tracking/carla_visualization/CMakeLists.txt @@ -0,0 +1,63 @@ +cmake_minimum_required(VERSION 3.0.2) +project(carla_visualization) + +## Compile as C++11, supported in ROS Kinetic and newer +# add_compile_options(-std=c++11) + +## Find catkin macros and libraries +## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) +## is used, also find other catkin packages +find_package(catkin REQUIRED COMPONENTS + rospy + sensor_msgs + std_msgs + cv_bridge +) + +## Uncomment this if the package has a setup.py +# catkin_python_setup() + +################################### +## catkin specific configuration ## +################################### +## The catkin_package macro generates cmake config files for your package +## Declare things to be passed to dependent projects +## INCLUDE_DIRS: uncomment this if your package contains header files +## LIBRARIES: libraries you create in this project that dependent projects also need +## CATKIN_DEPENDS: catkin_packages dependent projects also need +## DEPENDS: system dependencies of this project that dependent projects also need +catkin_package( +# INCLUDE_DIRS include +# LIBRARIES carla_visualization +# CATKIN_DEPENDS rospy sensor_msgs std_msgs cv_bridge +# DEPENDS system_lib +) + +########### +## Build ## +########### + +## Specify additional locations of header files +## Your package locations should be listed before other locations +include_directories( +# include + ${catkin_INCLUDE_DIRS} +) + +############# +## Install ## +############# + +# Mark executable scripts (Python) for installation +# in contrast to setup.py, you can choose the destination +catkin_install_python(PROGRAMS + scripts/publisher.py + scripts/subscriber.py + scripts/utils.py + DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +# Mark launch files for installation +install(DIRECTORY launch/ + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch +) diff --git a/src/car_tracking/carla_visualization/launch/main.launch b/src/car_tracking/carla_visualization/launch/main.launch new file mode 100644 index 0000000000..e9b641dc34 --- /dev/null +++ b/src/car_tracking/carla_visualization/launch/main.launch @@ -0,0 +1,11 @@ + + + + + + + + + + + diff --git a/src/car_tracking/carla_visualization/package.xml b/src/car_tracking/carla_visualization/package.xml new file mode 100644 index 0000000000..e0bf3686e4 --- /dev/null +++ b/src/car_tracking/carla_visualization/package.xml @@ -0,0 +1,26 @@ + + + carla_visualization + 0.0.0 + Carla visualization with ROS 1 + Your Name + MIT + + catkin + rospy + sensor_msgs + std_msgs + cv_bridge + rospy + sensor_msgs + std_msgs + cv_bridge + rospy + sensor_msgs + std_msgs + cv_bridge + + + catkin + + diff --git a/src/car_tracking/carla_visualization/scripts/publisher.py b/src/car_tracking/carla_visualization/scripts/publisher.py new file mode 100644 index 0000000000..3fa69bb1ca --- /dev/null +++ b/src/car_tracking/carla_visualization/scripts/publisher.py @@ -0,0 +1,189 @@ +# scripts/publisher.py +#!/usr/bin/env python3 +import carla +import queue +import rospy +import time +import numpy as np +from sensor_msgs.msg import Image, CompressedImage +from std_msgs.msg import String, Float32MultiArray, Bool +from cv_bridge import CvBridge +from utils import ( + CAMERA_WIDTH, CAMERA_HEIGHT, WEATHER_SWITCH_INTERVAL, SUPPORTED_WEATHERS, + EDGES, build_projection_matrix, get_image_point, point_in_canvas, + get_2d_box_from_3d_edges, get_vehicle_color, clear_npc, clear_static_vehicle, + set_weather, get_random_weather, SHOW_VEHICLES_3D, SHOW_TRAFFIC_LIGHTS, SHOW_TRAFFIC_LIGHTS_STATE +) + +# 全局变量 +bridge = CvBridge() +image_queue = queue.Queue() + +def camera_callback(image, queue): + """相机回调函数,将图像存入队列""" + queue.put(np.reshape(np.copy(image.raw_data), (image.height, image.width, 4))) + +def carla_publisher_node(): + # 初始化ROS节点 + rospy.init_node('carla_data_publisher', anonymous=True) + + # 创建发布者 + pub_image = rospy.Publisher('/carla/camera/image_raw', Image, queue_size=10) # 原始图像 + pub_compressed_image = rospy.Publisher('/carla/camera/image_compressed', CompressedImage, queue_size=10) # 压缩图像 + pub_vehicle_data = rospy.Publisher('/carla/vehicle/track_data', Float32MultiArray, queue_size=10) # 车辆跟踪数据 + pub_weather = rospy.Publisher('/carla/weather/current', String, queue_size=10) # 天气信息 + pub_3d_switch = rospy.Publisher('/carla/3d/switch', Bool, queue_size=10) # 3D显示开关(示例) + + # 初始化Carla客户端 + client = carla.Client('localhost', 2000) + client.set_timeout(10.0) + world = client.get_world() + + # 设置同步模式 + settings = world.get_settings() + settings.synchronous_mode = True + settings.fixed_delta_seconds = 0.05 + world.apply_settings(settings) + + # 生成主车辆 + bp_lib = world.get_blueprint_library() + vehicle_bp = bp_lib.find('vehicle.lincoln.mkz_2020') + spawn_points = world.get_map().get_spawn_points() + vehicle = world.try_spawn_actor(vehicle_bp, carla.Transform(spawn_points[0])) + if not vehicle: + rospy.logerr("主车辆生成失败!") + return + vehicle.set_autopilot(True) + + # 生成相机 + camera_bp = bp_lib.find('sensor.camera.rgb') + camera_bp.set_attribute('image_size_x', str(CAMERA_WIDTH)) + camera_bp.set_attribute('image_size_y', str(CAMERA_HEIGHT)) + camera_init_trans = carla.Transform(carla.Location(x=1, z=2)) + camera = world.spawn_actor(camera_bp, camera_init_trans, attach_to=vehicle) + camera.listen(lambda image: camera_callback(image, image_queue)) + + # 清理NPC + clear_npc(world) + clear_static_vehicle(world) + + # 生成NPC车辆 + for _ in range(50): + vehicle_bp_list = bp_lib.filter('vehicle') + car_bp = [bp for bp in vehicle_bp_list if int(bp.get_attribute('number_of_wheels')) == 4] + if car_bp: + npc = world.try_spawn_actor(random.choice(car_bp), random.choice(spawn_points)) + if npc: + npc.set_autopilot(True) + + # 初始化参数 + fov = camera_bp.get_attribute("fov").as_float() + K = build_projection_matrix(CAMERA_WIDTH, CAMERA_HEIGHT, fov) + K_b = build_projection_matrix(CAMERA_WIDTH, CAMERA_HEIGHT, fov, is_behind_camera=True) + tracked_vehicles = {} + frame_counter = 0 + current_weather = "ClearNoon" + set_weather(world, current_weather) + last_weather_switch_time = time.time() + + # 频率设置(与Carla同步,20Hz) + rate = rospy.Rate(20) + + try: + while not rospy.is_shutdown(): + world.tick() + frame_counter += 1 + + # 自动切换天气 + auto_switch = False + if WEATHER_SWITCH_INTERVAL > 0: + current_time = time.time() + if current_time - last_weather_switch_time >= WEATHER_SWITCH_INTERVAL: + current_weather = get_random_weather() + set_weather(world, current_weather) + last_weather_switch_time = current_time + auto_switch = True + + # 发布天气信息 + pub_weather.publish(current_weather) + + # 处理键盘控制(ROS参数服务器或话题控制,这里简化为示例) + # 可通过订阅话题来修改3D显示开关 + pub_3d_switch.publish(SHOW_VEHICLES_3D) + + # 获取相机图像 + if not image_queue.empty(): + image = image_queue.get() + # 转换为ROS Image消息并发布 + ros_image = bridge.cv2_to_imgmsg(image, encoding="bgra8") + ros_image.header.stamp = rospy.Time.now() + pub_image.publish(ros_image) + # 发布压缩图像(可选,减少带宽) + compressed_image = bridge.cv2_to_compressed_imgmsg(image[..., :3], "jpg") + pub_compressed_image.publish(compressed_image) + + # 采集车辆跟踪数据 + vehicle_data = Float32MultiArray() + boxes = [] + ids = [] + track_data = {} + current_vehicles = 0 + max_distance = 0.0 + + for npc in world.get_actors().filter('*vehicle*'): + if npc.id != vehicle.id: + dist = npc.get_transform().location.distance(vehicle.get_transform().location) + max_distance = max(max_distance, dist) + if dist < 50: + forward_vec = vehicle.get_transform().get_forward_vector() + ray = npc.get_transform().location - vehicle.get_transform().location + if forward_vec.dot(ray) > 0: + verts = [v for v in npc.bounding_box.get_world_vertices(npc.get_transform())] + points_2d = [] + world_2_camera = np.array(camera.get_transform().get_inverse_matrix()) + for vert in verts: + ray0 = vert - camera.get_transform().location + cam_forward_vec = camera.get_transform().get_forward_vector() + if cam_forward_vec.dot(ray0) > 0: + p = get_image_point(vert, K, world_2_camera) + else: + p = get_image_point(vert, K_b, world_2_camera) + points_2d.append(p) + x_min, x_max, y_min, y_max = get_2d_box_from_3d_edges( + points_2d, EDGES, CAMERA_HEIGHT, CAMERA_WIDTH + ) + if (y_max - y_min) * (x_max - x_min) > 100 and (x_max - x_min) > 20: + if point_in_canvas((x_min, y_min), CAMERA_HEIGHT, CAMERA_WIDTH) and \ + point_in_canvas((x_max, y_max), CAMERA_HEIGHT, CAMERA_WIDTH): + ids.append(npc.id) + boxes.append([x_min, y_min, x_max, y_max]) + if npc.id not in tracked_vehicles: + tracked_vehicles[npc.id] = {'color': get_vehicle_color(npc.id)} + tracked_vehicles[npc.id]['distance'] = dist + track_data[npc.id] = tracked_vehicles[npc.id] + current_vehicles += 1 + + # 封装车辆数据并发布(示例:车辆数量、最大距离、第一个框的坐标) + vehicle_data.data = [current_vehicles, max_distance] + (boxes[0] if boxes else [0,0,0,0]) + pub_vehicle_data.publish(vehicle_data) + + rate.sleep() + + except rospy.ROSInterruptException: + pass + except Exception as e: + rospy.logerr(f"发布节点出错:{e}") + finally: + # 清理资源 + if camera: + camera.stop() + camera.destroy() + if vehicle: + vehicle.destroy() + settings = world.get_settings() + settings.synchronous_mode = False + world.apply_settings(settings) + rospy.loginfo("Carla资源已清理") + +if __name__ == '__main__': + carla_publisher_node() diff --git a/src/car_tracking/carla_visualization/scripts/subscriber.py b/src/car_tracking/carla_visualization/scripts/subscriber.py new file mode 100644 index 0000000000..dc6a3621e1 --- /dev/null +++ b/src/car_tracking/carla_visualization/scripts/subscriber.py @@ -0,0 +1,160 @@ +# scripts/subscriber.py +#!/usr/bin/env python3 +import rospy +import cv2 +import numpy as np +from sensor_msgs.msg import Image, CompressedImage +from std_msgs.msg import String, Float32MultiArray, Bool +from cv_bridge import CvBridge +from utils import ( + CAMERA_WIDTH, CAMERA_HEIGHT, CHART_WIDTH, CHART_HEIGHT, MAX_HISTORY_FRAMES, + TRACK_WINDOW_WIDTH, TRACK_WINDOW_HEIGHT, FONT_SCALE_SMALL, FONT_SCALE_MEDIUM, + LINE_THICKNESS, SUPPORTED_WEATHERS, COCO_CLASS_NAMES, + custom_draw_bounding_boxes, draw_dynamic_chart, convert_image_format, + draw_3d_objects, SHOW_VEHICLES_3D, SHOW_TRAFFIC_LIGHTS, SHOW_TRAFFIC_LIGHTS_STATE +) + +# 全局变量 +bridge = CvBridge() +history_frames = [] +history_vehicles = [] +history_max_dist = [] +current_weather = "ClearNoon" +tracked_vehicles = {} +frame_counter = 0 + +def image_callback(msg): + """原始图像回调函数""" + global frame_counter + frame_counter += 1 + try: + # 将ROS Image消息转换为OpenCV图像 + cv_image = bridge.imgmsg_to_cv2(msg, "bgra8") + process_image(cv_image) + except Exception as e: + rospy.logerr(f"图像处理出错:{e}") + +def compressed_image_callback(msg): + """压缩图像回调函数(可选)""" + global frame_counter + frame_counter += 1 + try: + cv_image = bridge.compressed_imgmsg_to_cv2(msg, "bgr8") + # 补全4通道(如果需要) + cv_image = np.dstack((cv_image, np.ones((CAMERA_HEIGHT, CAMERA_WIDTH), dtype=np.uint8)*255)) + process_image(cv_image) + except Exception as e: + rospy.logerr(f"压缩图像处理出错:{e}") + +def weather_callback(msg): + """天气信息回调函数""" + global current_weather + current_weather = msg.data + +def vehicle_data_callback(msg): + """车辆数据回调函数""" + global history_frames, history_vehicles, history_max_dist + # 解析数据:[current_vehicles, max_distance, x1, y1, x2, y2] + current_vehicles = int(msg.data[0]) + max_distance = msg.data[1] + # 更新历史数据 + history_frames.append(frame_counter) + history_vehicles.append(current_vehicles) + history_max_dist.append(max_distance) + # 截断数据 + if len(history_frames) > MAX_HISTORY_FRAMES: + history_frames.pop(0) + history_vehicles.pop(0) + history_max_dist.pop(0) + +def process_image(cv_image): + """处理图像并可视化""" + global tracked_vehicles + + # 模拟车辆跟踪数据(实际可从话题中解析) + boxes = [] + ids = [] + track_data = {} + # 这里简化处理,实际可根据vehicle_data话题中的数据填充 + if len(history_vehicles) > 0 and history_vehicles[-1] > 0: + boxes = np.array([[100, 100, 200, 200]]) # 示例框 + ids = [1] # 示例ID + tracked_vehicles[1] = {'color': (0, 255, 0), 'distance': 20.0} # 示例数据 + track_data = tracked_vehicles + + # 绘制2D框 + labels = np.array([2] * len(boxes)) # car类别 + output = custom_draw_bounding_boxes( + cv_image, boxes, labels, COCO_CLASS_NAMES, ids, track_data + ) if len(boxes) > 0 else cv_image + + # 注意:3D绘制需要Carla的world/camera/vehicle对象,这里如果需要可通过ROS参数或服务传递, + # 简化版可注释3D绘制,仅保留2D可视化 + # output_3d, _, _ = draw_3d_objects(output, world, camera, vehicle, K, K_b) + + # 转换图像格式 + output_rgb = convert_image_format(output) + # 绘制动态图表 + chart_image = draw_dynamic_chart(history_frames, history_vehicles, history_max_dist) + # 拼接图像 + combined_image = np.hstack((output_rgb, chart_image)) + + # 绘制天气信息 + cv2.putText( + combined_image, f"Weather: {current_weather}", + (10, 30), cv2.FONT_HERSHEY_SIMPLEX, FONT_SCALE_MEDIUM, (255, 255, 255), 2 + ) + # 绘制控制提示 + cv2.putText( + combined_image, "Press Q:Quit", + (10, CAMERA_HEIGHT - 10), cv2.FONT_HERSHEY_SIMPLEX, FONT_SCALE_SMALL, (255, 255, 255), 1 + ) + + # 绘制跟踪窗口 + track_window = np.zeros((TRACK_WINDOW_HEIGHT, TRACK_WINDOW_WIDTH, 3), dtype=np.uint8) + cv2.putText( + track_window, "Vehicle Tracking Monitor", (10, 30), + cv2.FONT_HERSHEY_SIMPLEX, FONT_SCALE_MEDIUM, (255, 255, 255), LINE_THICKNESS + ) + cv2.putText( + track_window, f"Current Weather: {current_weather}", + (10, 60), cv2.FONT_HERSHEY_SIMPLEX, FONT_SCALE_SMALL, (255, 255, 255), 1 + ) + y_offset = 90 + for vid, data in list(tracked_vehicles.items())[:10]: + if y_offset > TRACK_WINDOW_HEIGHT - 20: + break + color = data['color'] + dist = data.get('distance', 0.0) + cv2.putText( + track_window, f"ID: {vid} | Dist: {dist:.1f}m", (10, y_offset), + cv2.FONT_HERSHEY_SIMPLEX, FONT_SCALE_SMALL, color, 1 + ) + y_offset += 30 + + # 显示窗口 + cv2.imshow('2D Ground Truth + Visualization', combined_image) + cv2.imshow('Vehicle Tracking Monitor', track_window) + # 按键退出 + if cv2.waitKey(1) & 0xFF == ord('q'): + rospy.signal_shutdown("用户按下Q键退出") + cv2.destroyAllWindows() + +def carla_subscriber_node(): + # 初始化ROS节点 + rospy.init_node('carla_data_subscriber', anonymous=True) + + # 订阅话题 + rospy.Subscriber('/carla/camera/image_raw', Image, image_callback) + # rospy.Subscriber('/carla/camera/image_compressed', CompressedImage, compressed_image_callback) # 可选 + rospy.Subscriber('/carla/weather/current', String, weather_callback) + rospy.Subscriber('/carla/vehicle/track_data', Float32MultiArray, vehicle_data_callback) + + # 保持节点运行 + rospy.spin() + + # 清理窗口 + cv2.destroyAllWindows() + +if __name__ == '__main__': + carla_subscriber_node() diff --git a/src/car_tracking/carla_visualization/scripts/utils.py b/src/car_tracking/carla_visualization/scripts/utils.py new file mode 100644 index 0000000000..d031aae1ed --- /dev/null +++ b/src/car_tracking/carla_visualization/scripts/utils.py @@ -0,0 +1,357 @@ +# scripts/utils.py +import carla +import cv2 +import numpy as np +import random + +# ===================== 依赖导入(保留原代码的导入逻辑)===================== +try: + from what.models.detection.datasets.coco import COCO_CLASS_NAMES + from utils.box_utils import draw_bounding_boxes + from utils.projection import get_image_point, build_projection_matrix, point_in_canvas, get_2d_box_from_3d_edges + from utils.world import clear_npc, clear_static_vehicle +except ImportError: + # 保留原代码的替代实现 + COCO_CLASS_NAMES = ['person', 'bicycle', 'car', 'motorcycle', 'airplane', 'bus', 'train', 'truck', 'boat', 'traffic light'] + def draw_bounding_boxes(image, boxes, labels, class_names, ids=None): + img = image.copy() + for i, box in enumerate(boxes): + x1, y1, x2, y2 = map(int, box) + label = class_names[labels[i]] if labels[i] < len(class_names) else 'unknown' + cv2.rectangle(img, (x1, y1), (x2, y2), (0, 255, 0), 2) + cv2.putText(img, label, (x1, y1-5), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 255, 0), 1) + return img + def build_projection_matrix(w, h, fov, is_behind_camera=False): + fov_rad = np.deg2rad(fov) + fx = w / (2 * np.tan(fov_rad / 2)) + fy = h / (2 * np.tan(fov_rad / 2)) + cx = w / 2 + cy = h / 2 + matrix = np.array([[fx, 0, cx], [0, fy, cy], [0, 0, 1]]) + if is_behind_camera: + matrix[0, 0] = -fx + return matrix + def get_image_point(loc, K, w2c): + loc_vector = np.array([loc.x, loc.y, loc.z, 1]) + point_in_cam = np.dot(w2c, loc_vector) + point_in_cam = point_in_cam / point_in_cam[3] + point_in_2d = np.dot(K, point_in_cam[:3]) + point_in_2d = point_in_2d / point_in_2d[2] + return (point_in_2d[0], point_in_2d[1]) + def point_in_canvas(point, h, w): + return 0 <= point[0] <= w and 0 <= point[1] <= h + def get_2d_box_from_3d_edges(points, edges, h, w): + xs = [p[0] for p in points if point_in_canvas(p, h, w)] + ys = [p[1] for p in points if point_in_canvas(p, h, w)] + return min(xs) if xs else 0, max(xs) if xs else w, min(ys) if ys else 0, max(ys) if ys else h + def clear_npc(world): + for actor in world.get_actors().filter('*vehicle*'): + actor.destroy() + def clear_static_vehicle(world): + pass + +# ===================== 配置常量(保留原代码的所有配置)===================== +# 相机配置 +CAMERA_WIDTH = 640 +CAMERA_HEIGHT = 640 +# 图表配置 +CHART_WIDTH = 400 +CHART_HEIGHT = CAMERA_HEIGHT # 与相机高度一致 +MAX_HISTORY_FRAMES = 50 # 最近50帧数据 +# 跟踪窗口配置 +TRACK_WINDOW_WIDTH = 300 +TRACK_WINDOW_HEIGHT = 400 +# 绘图配置 +FONT_SCALE_SMALL = 0.4 +FONT_SCALE_MEDIUM = 0.6 +LINE_THICKNESS = 2 +POINT_RADIUS = 2 +# 天气配置 +WEATHER_SWITCH_INTERVAL = 10 # 随机天气切换间隔(秒,0表示不自动切换) +SUPPORTED_WEATHERS = { + 1: "ClearNoon", # 晴天正午 + 2: "CloudyNoon", # 多云正午 + 3: "RainyNoon", # 雨天正午 + 4: "Sunset", # 黄昏 + 5: "Foggy", # 雾天 + 6: "Stormy" # 暴雨 +} + +# 3D可视化配置 +DISTANCE_THRESHOLD = 80 # 车辆3D框显示距离阈值(米) +TRAFFIC_LIGHT_DISTANCE = 60 # 红绿灯显示距离阈值(米) +EDGES = [[0, 1], [1, 3], [3, 2], [2, 0], [0, 4], [4, 5], + [5, 1], [5, 7], [7, 6], [6, 4], [6, 2], [7, 3]] # 3D边界框边 +# 颜色定义(BGR格式) +VEHICLE_3D_COLOR = (0, 255, 0) # 车辆3D框默认颜色 +TRAFFIC_LIGHT_COLORS = { + 0: (0, 255, 0), # 绿色 + 1: (0, 255, 255), # 黄色 + 2: (0, 0, 255), # 红色 + 3: (255, 255, 255) # 白色(未知状态) +} +TRAFFIC_LIGHT_STATE_NAMES = { + 0: "GREEN", + 1: "YELLOW", + 2: "RED", + 3: "UNKNOWN" +} +# 全局开关(用于节点间通信控制) +SHOW_VEHICLES_3D = True +SHOW_TRAFFIC_LIGHTS = True +SHOW_TRAFFIC_LIGHTS_STATE = True + +# ===================== 工具函数(保留原代码的所有工具函数)===================== +def get_vehicle_color(vehicle_id): + """为车辆生成固定唯一的RGB颜色(基于ID种子)""" + np.random.seed(vehicle_id) + return tuple(np.random.randint(0, 255, 3).tolist()) + +def custom_draw_bounding_boxes(image, boxes, labels, class_names, ids=None, track_data=None): + """保留原始边界框绘制逻辑,叠加跟踪数据""" + img = draw_bounding_boxes(image, boxes, labels, class_names, ids) + if ids is not None and track_data is not None and len(boxes) > 0: + for i, box in enumerate(boxes): + vid = ids[i] + if vid in track_data: + x1, y1, x2, y2 = map(int, box) + color = track_data[vid]['color'] + dist = track_data[vid]['distance'] + # 绘制距离文本 + cv2.putText( + img, f"Dist: {dist:.1f}m", (x1, y1 + 10), + cv2.FONT_HERSHEY_SIMPLEX, FONT_SCALE_SMALL, color, 1 + ) + # 绘制跟踪颜色外框 + cv2.rectangle(img, (x1 - 1, y1 - 1), (x2 + 1, y2 + 1), color, 1) + return img + +def init_chart_background(width, height): + """初始化图表背景""" + chart = np.zeros((height, width, 3), dtype=np.uint8) + # 绘制标题 + cv2.putText( + chart, "Real-Time Statistics (Last 50 Frames)", (10, 30), + cv2.FONT_HERSHEY_SIMPLEX, FONT_SCALE_MEDIUM, (255, 255, 255), LINE_THICKNESS + ) + # 绘制网格 + grid_color = (50, 50, 50) + for y in range(50, height - 30, 50): + cv2.line(chart, (50, y), (width - 50, y), grid_color, 1) + for x in range(50, width - 50, 50): + cv2.line(chart, (x, 30), (x, height - 30), grid_color, 1) + # 绘制图例 + cv2.putText( + chart, "Current Vehicles (green)", (10, height - 10), + cv2.FONT_HERSHEY_SIMPLEX, FONT_SCALE_SMALL, (0, 255, 0), 1 + ) + cv2.putText( + chart, "Max Distance (red)", (200, height - 10), + cv2.FONT_HERSHEY_SIMPLEX, FONT_SCALE_SMALL, (0, 0, 255), 1 + ) + return chart + +def draw_dynamic_chart(history_frames, history_vehicles, history_max_dist): + """绘制实时动态折线图""" + chart = init_chart_background(CHART_WIDTH, CHART_HEIGHT) + if len(history_frames) == 0: + return chart + # 数据归一化 + max_veh = max(history_vehicles) if history_vehicles else 1 + max_veh = max_veh if max_veh != 0 else 1 + norm_veh = [(v / max_veh) * (CHART_HEIGHT - 60) for v in history_vehicles] + y_veh = np.array([CHART_HEIGHT - 30 - v for v in norm_veh], dtype=int) + + max_d = max(history_max_dist) if history_max_dist else 1 + max_d = max_d if max_d != 0 else 1 + norm_dist = [(d / max_d) * (CHART_HEIGHT - 60) for d in history_max_dist] + y_dist = np.array([CHART_HEIGHT - 30 - d for d in norm_dist], dtype=int) + + x_coords = np.array([ + 50 + (i * (CHART_WIDTH - 100) / (len(history_frames) - 1 if len(history_frames) > 1 else 1)) + for i in range(len(history_frames)) + ], dtype=int) + + # 绘制折线和数据点 + if len(x_coords) > 1: + cv2.polylines(chart, [np.column_stack((x_coords, y_veh))], isClosed=False, color=(0, 255, 0), + thickness=LINE_THICKNESS) + cv2.polylines(chart, [np.column_stack((x_coords, y_dist))], isClosed=False, color=(0, 0, 255), + thickness=LINE_THICKNESS) + for x, y in zip(x_coords, y_veh): + cv2.circle(chart, (x, y), POINT_RADIUS, (0, 255, 0), -1) + for x, y in zip(x_coords, y_dist): + cv2.circle(chart, (x, y), POINT_RADIUS, (0, 0, 255), -1) + + # 绘制当前数值标注 + if len(history_vehicles) > 0 and len(history_max_dist) > 0: + current_veh = history_vehicles[-1] + current_dist = history_max_dist[-1] + cv2.putText( + chart, f"Now: {current_veh} cars | {current_dist:.1f}m", + (CHART_WIDTH - 200, 30), cv2.FONT_HERSHEY_SIMPLEX, FONT_SCALE_SMALL, + (255, 255, 255), 1 + ) + return chart + +def convert_image_format(image): + """将4通道BGRA图像转换为3通道RGB图像""" + return image[..., :3] if image.shape[-1] == 4 else image.copy() + +def draw_3d_objects(image, world, camera, vehicle, K, K_b): + """在图像上绘制3D车辆边界框和交通信号灯""" + try: + img = image.copy() + height, width = CAMERA_HEIGHT, CAMERA_WIDTH + world_2_camera = np.array(camera.get_transform().get_inverse_matrix()) + + vehicle_count_3d = 0 + traffic_light_count = 0 + + # 绘制车辆3D边界框 + if SHOW_VEHICLES_3D: + vehicles = list(world.get_actors().filter('*vehicle*')) + for npc in vehicles: + if npc.id == vehicle.id: + continue + dist = npc.get_transform().location.distance(vehicle.get_transform().location) + if dist >= DISTANCE_THRESHOLD: + continue + forward_vec = vehicle.get_transform().get_forward_vector() + ray = npc.get_transform().location - vehicle.get_transform().location + if forward_vec.dot(ray) <= 0: + continue + + bb = npc.bounding_box + verts = bb.get_world_vertices(npc.get_transform()) + points_2d = [] + for vert in verts: + ray0 = vert - camera.get_transform().location + cam_forward_vec = camera.get_transform().get_forward_vector() + if cam_forward_vec.dot(ray0) > 0: + p = get_image_point(vert, K, world_2_camera) + else: + p = get_image_point(vert, K_b, world_2_camera) + points_2d.append(p) + + for edge in EDGES: + p1 = points_2d[edge[0]] + p2 = points_2d[edge[1]] + if point_in_canvas(p1, height, width) or point_in_canvas(p2, height, width): + thickness = max(1, int(2 - dist / 50)) + color_intensity = max(50, int(255 - dist)) + color = (0, color_intensity, 0) + cv2.line(img, (int(p1[0]), int(p1[1])), + (int(p2[0]), int(p2[1])), color, thickness) + vehicle_count_3d += 1 + + # 绘制交通信号灯 + if SHOW_TRAFFIC_LIGHTS: + traffic_lights = list(world.get_actors().filter('*traffic_light*')) + for light in traffic_lights: + dist = light.get_transform().location.distance(vehicle.get_transform().location) + if dist >= TRAFFIC_LIGHT_DISTANCE: + continue + forward_vec = vehicle.get_transform().get_forward_vector() + ray = light.get_transform().location - vehicle.get_transform().location + if forward_vec.dot(ray) <= 0: + continue + + location = light.get_transform().location + ray0 = location - camera.get_transform().location + cam_forward_vec = camera.get_transform().get_forward_vector() + if cam_forward_vec.dot(ray0) > 0: + point_2d = get_image_point(location, K, world_2_camera) + else: + point_2d = get_image_point(location, K_b, world_2_camera) + + if not point_in_canvas(point_2d, height, width): + continue + + x, y = int(point_2d[0]), int(point_2d[1]) + light_state = light.get_state() + state_mapping = { + carla.TrafficLightState.Green: 0, + carla.TrafficLightState.Yellow: 1, + carla.TrafficLightState.Red: 2, + } + state_idx = state_mapping.get(light_state, 3) + light_color = TRAFFIC_LIGHT_COLORS[state_idx] + state_name = TRAFFIC_LIGHT_STATE_NAMES[state_idx] + + radius = max(6, int(15 - dist / 20)) + cv2.circle(img, (x, y), radius, light_color, -1) + cv2.circle(img, (x, y), radius, (255, 255, 255), 1) + + if SHOW_TRAFFIC_LIGHTS_STATE and radius > 4: + text_size = cv2.getTextSize(state_name, cv2.FONT_HERSHEY_SIMPLEX, 0.5, 1)[0] + text_x = x - text_size[0] // 2 + text_y = y - radius - 5 + cv2.rectangle(img, (text_x - 3, text_y - text_size[1] - 3), + (text_x + text_size[0] + 3, text_y + 3), + (40, 40, 40), -1) + cv2.putText(img, state_name, (text_x, text_y), + cv2.FONT_HERSHEY_SIMPLEX, 0.5, (255, 255, 255), 1) + + traffic_light_count += 1 + + return img, vehicle_count_3d, traffic_light_count + except Exception as e: + print(f"3D物体绘制错误:{e}") + return image, 0, 0 + +def set_weather(world, weather_type): + """设置Carla世界的天气""" + weather = carla.WeatherParameters() + if weather_type == "ClearNoon": + weather.sun_altitude_angle = 60.0 + weather.cloudiness = 0.0 + weather.precipitation = 0.0 + weather.wetness = 0.0 + weather.fog_density = 0.0 + elif weather_type == "CloudyNoon": + weather.sun_altitude_angle = 60.0 + weather.cloudiness = 80.0 + weather.wind_intensity = 20.0 + weather.precipitation = 0.0 + weather.wetness = 0.0 + weather.fog_density = 0.0 + elif weather_type == "RainyNoon": + weather.sun_altitude_angle = 60.0 + weather.cloudiness = 80.0 + weather.precipitation = 50.0 + weather.precipitation_deposits = 20.0 + weather.wetness = 80.0 + weather.wind_intensity = 30.0 + weather.fog_density = 10.0 + elif weather_type == "Sunset": + weather.sun_altitude_angle = 10.0 + weather.sun_azimuth_angle = 180.0 + weather.cloudiness = 30.0 + weather.wind_intensity = 10.0 + weather.precipitation = 0.0 + weather.fog_density = 5.0 + elif weather_type == "Foggy": + weather.sun_altitude_angle = 30.0 + weather.cloudiness = 20.0 + weather.fog_density = 90.0 + weather.fog_distance = 10.0 + weather.fog_falloff = 1.0 + weather.precipitation = 0.0 + weather.wetness = 0.0 + elif weather_type == "Stormy": + weather.sun_altitude_angle = 30.0 + weather.cloudiness = 100.0 + weather.precipitation = 100.0 + weather.precipitation_deposits = 50.0 + weather.wetness = 100.0 + weather.wind_intensity = 70.0 + weather.fog_density = 30.0 + world.set_weather(weather) + print(f"当前天气已切换为:{weather_type}") + +def get_random_weather(): + """获取随机的天气类型""" + weather_codes = list(SUPPORTED_WEATHERS.keys()) + random_code = random.choice(weather_codes) + return SUPPORTED_WEATHERS[random_code]