From 7afd01d202ff41ec3212df3a1722661b664e1096 Mon Sep 17 00:00:00 2001
From: Yuansugu123 <2451009878@qq.com>
Date: Mon, 22 Dec 2025 16:00:01 +0800
Subject: [PATCH] =?UTF-8?q?=E6=8F=90=E4=BA=A4ros=E5=B0=81=E8=A3=85?=
=?UTF-8?q?=E5=8C=85?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit
---
.../carla_visualization/CMakeLists.txt | 63 ++++
.../carla_visualization/launch/main.launch | 11 +
.../carla_visualization/package.xml | 26 ++
.../carla_visualization/scripts/publisher.py | 189 ++++++++++
.../carla_visualization/scripts/subscriber.py | 160 ++++++++
.../carla_visualization/scripts/utils.py | 357 ++++++++++++++++++
6 files changed, 806 insertions(+)
create mode 100644 src/car_tracking/carla_visualization/CMakeLists.txt
create mode 100644 src/car_tracking/carla_visualization/launch/main.launch
create mode 100644 src/car_tracking/carla_visualization/package.xml
create mode 100644 src/car_tracking/carla_visualization/scripts/publisher.py
create mode 100644 src/car_tracking/carla_visualization/scripts/subscriber.py
create mode 100644 src/car_tracking/carla_visualization/scripts/utils.py
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]