#!/usr/bin/env python3
"""
🚀 工业级自动驾驶多源数据融合可视化系统(已修复所有问题)
✅ 安全清空图形 | ✅ 固定视图 | ✅ 车道/目标/信号灯完整显示
"""
import pandas as pd
import numpy as np
import matplotlib.pyplot as plt
from matplotlib.patches import Rectangle
from datetime import datetime
import os
import warnings
warnings.filterwarnings("ignore", category=FutureWarning)
plt.rcParams['agg.path.chunksize'] = 10000
# ========================
# 配置参数
# ========================
FILES = {
'ad_state': '/home/wyl/zxd/ceshi/csvdata/1983436010726993921/ad_state.csv',
'basic_func': '/home/wyl/zxd/ceshi/csvdata/1983436010726993921/sf_for_basic_func.csv',
'vehicle_data': '/home/wyl/zxd/ceshi/csvdata/1983436010726993921/ft_vehicle_data_v3.csv',
'mcu_hpa': '/home/wyl/zxd/ceshi/csvdata/1983436010726993921/rtemsg_mcu_hpa_to_soc_hpa.csv'
}
SAVE_GIF = False
OUTPUT_GIF = "autonomous_driving_simulation.gif"
FPS = 10
PLOT_WIDTH, PLOT_HEIGHT = 16, 9
XLIM = (-12, 12) # 固定横向范围
YLIM = (-10, 100) # 固定纵向范围
# ========================
# 工具函数:智能时间戳标准化
# ========================
def detect_and_standardize_timestamp(df):
ts_candidates = [col for col in df.columns if 'time' in col.lower() or 'stamp' in col.lower()]
if not ts_candidates:
raise ValueError(f"❌ 无时间戳列: {df.columns[:5]}...")
priority = ['timestamp', 'Timestamp', 'TIME_STAMP']
ts_col = next((c for c in priority if c in df.columns), ts_candidates[0])
print(f"🔍 使用时间戳列: '{ts_col}'")
df['Timestamp'] = pd.to_numeric(df[ts_col], errors='coerce')
avg_val = df['Timestamp'].dropna().mean()
if avg_val > 1e15:
df['Timestamp'] /= 1e12
elif avg_val > 1e12:
df['Timestamp'] /= 1e9
elif avg_val > 1e10:
df['Timestamp'] /= 1e6
elif avg_val > 1e7:
df['Timestamp'] /= 1e3
df.dropna(subset=['Timestamp'], inplace=True)
df.sort_values('Timestamp', inplace=True)
df.reset_index(drop=True, inplace=True)
return df
# ========================
# 加载并预处理所有 CSV
# ========================
print("🔄 正在加载数据...")
dfs = {}
for key, path in FILES.items():
if not os.path.exists(path):
raise FileNotFoundError(f"❌ 找不到文件: {path}")
try:
raw = pd.read_csv(path)
proc = detect_and_standardize_timestamp(raw)
dfs[key] = proc
print(f"✅ {path}: {len(proc)} 行 × {len(proc.columns)} 列")
except Exception as e:
print(f"⚠️ 跳过 {path}: {e}")
dfs[key] = pd.DataFrame()
# ========================
# 多源对齐(基于 ad_state 时间主轴)
# ========================
if 'ad_state' not in dfs or dfs['ad_state'].empty:
raise ValueError("❌ 必须包含 ad_state.csv 作为主时间轴!")
base_df = dfs['ad_state'][['Timestamp']].drop_duplicates()
for key in ['basic_func', 'vehicle_data', 'mcu_hpa']:
if key in dfs and not dfs[key].empty:
clean = dfs[key].drop_duplicates(subset=['Timestamp'])
base_df = pd.merge_asof(
base_df, clean,
on='Timestamp',
direction='nearest',
tolerance=0.05
)
print(f"🔗 合并 {key}")
merged = base_df.sort_values('Timestamp').reset_index(drop=True)
print(f"📊 对齐后总行数: {len(merged)}, 字段数: {len(merged.columns)}")
# ========================
# 安全取值函数
# ========================
def safe_get(row, field, default=np.nan):
try:
val = row[field]
return val if pd.notna(val) else default
except:
return default
def safe_format_time(ts):
try:
dt = datetime.fromtimestamp(float(ts))
return dt.strftime('%H:%M:%S.%f')[:-3]
except:
return f"{ts:.3f}"
# ========================
# 主可视化函数(安全清除 + 稳定渲染)
# ========================
def simulate_scene():
fig, ax = plt.subplots(figsize=(PLOT_WIDTH, PLOT_HEIGHT), dpi=100)
fig.subplots_adjust(left=0.08, right=0.92, top=0.92, bottom=0.08)
ax.set_facecolor('#1a1a1a')
ax.set_xlim(*XLIM)
ax.set_ylim(*YLIM)
ax.set_aspect('equal', adjustable='box')
ax.grid(True, alpha=0.2, color='gray')
ax.tick_params(colors='white', labelsize=10)
title_obj = ax.set_title("", color='white', fontsize=14, pad=20)
ax.set_xlabel("Lateral (m)", color='white', fontsize=12)
ax.set_ylabel("Longitudinal (m)", color='white', fontsize=12)
frames = []
legend_set = False # 控制图例只添加一次
print("\n🎥 开始播放模拟场景...")
for frame_idx, row in merged.iterrows():
# ✅ 安全清除旧图形(正确方式)
while len(ax.patches) > 0:
ax.patches[-1].remove()
while len(ax.collections) > 0:
ax.collections[0].remove()
for txt in ax.texts.copy():
txt.remove()
# === 更新标题 ===
ts_str = safe_format_time(row['Timestamp'])
title_obj.set_text(f"Frame {frame_idx} | Time: {ts_str}")
# === 自车信息 ===
speed = safe_get(row, 'VehSelf.VLgt', 0.0)
acc = safe_get(row, 'VehSelf.AccLgt', 0.0)
yaw = safe_get(row, 'VehSelf.AgDirDelta', 0.0)
gear = safe_get(row, 'VehSelf.Gear', -1)
brake = safe_get(row, 'VehicleData.BrakePedalPos', 0)
throttle = safe_get(row, 'VehicleData.AccPedalPos', 0)
turn_signal = safe_get(row, 'VehicleData.TurnSignalSts', 0) # 1=左, 2=右
# === 绘制车道线 ===
lane_prefixes = [
('FusionLaneMkrList.ClsLe.Estimn.', 'yellow', 'Left Lane', '-'),
('FusionLaneMkrList.SecClsLe.Estimn.', 'white', 'Sec Left', '--'),
('FusionLaneMkrList.ClsRi.Estimn.', 'yellow', 'Right Lane', '-'),
('FusionLaneMkrList.SecClsRi.Estimn.', 'white', 'Sec Right', '--'),
]
for prefix, color, label, ls in lane_prefixes:
required = [
f"{prefix}ConstCoeff",
f"{prefix}FirstOrderCoeff",
f"{prefix}SecondOrderCoeff",
f"{prefix}ThirdOrderCoeff"
]
if not all(col in row.index for col in required):
continue
coeffs = [safe_get(row, f"{prefix}{name}") for name in required]
if any(pd.isna(c) for c in coeffs):
continue
y = np.linspace(0, YLIM[1], 200)
x = sum(c * y**i for i, c in enumerate(coeffs))
valid = np.isfinite(x) & np.isfinite(y) & (x >= XLIM[0]) & (x <= XLIM[1])
if valid.any():
ax.plot(x[valid], y[valid], color=color, linewidth=3, linestyle=ls, alpha=0.9, zorder=2)
# === 绘制自车 ===
car_l, car_w = 4.5, 2.0
ego_rect = Rectangle((-car_w/2, -car_l/2), car_w, car_l,
angle=np.degrees(yaw), rotation_point='center',
color='blue', alpha=0.8, zorder=10)
ax.add_patch(ego_rect)
ax.arrow(0, 0, 2*np.cos(yaw), 2*np.sin(yaw),
head_width=0.6, fc='cyan', ec='cyan', zorder=11, length_includes_head=True)
# 转向灯
if turn_signal == 1: # 左转
ax.plot([-1.8, -2.5], [-3, -4], 'y-', lw=3, solid_capstyle='round', zorder=12)
elif turn_signal == 2: # 右转
ax.plot([1.8, 2.5], [-3, -4], 'y-', lw=3, solid_capstyle='round', zorder=12)
# === 融合目标 ===
obj_types = {1: 'Car', 3: 'Truck', 4: 'Pedestrian', 5: 'Cyclist', 6: 'Motorcycle', 7: 'Static', 0: 'Unknown'}
colors = {'Car': 'red', 'Truck': 'orange', 'Pedestrian': 'pink', 'Cyclist': 'lime',
'Motorcycle': 'gold', 'Static': 'gray', 'Unknown': 'silver'}
for i in range(64):
prefix = f"FusionObjList.Obj{i}."
pos_lgt = safe_get(row, f"{prefix}Estimn.PosnLgt")
pos_lat = safe_get(row, f"{prefix}Estimn.PosnLat")
if pd.isna(pos_lgt) or pd.isna(pos_lat) or abs(pos_lgt) > 200:
continue
length = max(safe_get(row, f"{prefix}Info.Length", 4.0), 1.0)
width = max(safe_get(row, f"{prefix}Info.Width", 1.8), 1.0)
obj_type_idx = int(safe_get(row, f"{prefix}Info.type", 0))
obj_name = obj_types.get(obj_type_idx, 'Unknown')
color = colors.get(obj_name, 'white')
rect = Rectangle((pos_lat - width/2, pos_lgt - length/2), width, length,
color=color, alpha=0.7, zorder=5, linewidth=1.2, edgecolor='white')
ax.add_patch(rect)
ax.text(pos_lat, pos_lgt, str(i), color='white', fontsize=9,
ha='center', va='center', weight='bold', zorder=6)
# === 交通灯 ===
light_colors = {0: 'red', 1: 'yellow', 2: 'green', 3: 'darkgray'}
for i in range(8):
prefix = f"TrafficLightList.TrafficLight{i}."
lgt = safe_get(row, f"{prefix}PosLgt")
lat = safe_get(row, f"{prefix}PosLat")
col_idx = int(safe_get(row, f"{prefix}Color", 3))
if pd.isna(lgt) or pd.isna(lat):
continue
color = light_colors.get(col_idx, 'white')
ax.plot(lat, lgt, 'o', color=color, markersize=10, zorder=12)
ax.text(lat, lgt + 2, f"TL{i}", color='white', fontsize=7, ha='center')
# === 信息面板 ===
info_text = (
f"Speed: {speed*3.6:.1f} km/h\n"
f"Accel: {acc:+.2f} m/s²\n"
f"Gear: {gear}\n"
f"Brake: {brake}%, Throttle: {throttle}%"
)
ax.text(XLIM[1]-0.5, YLIM[1]-5, info_text,
color='lightgreen', fontsize=10, ha='right', va='top',
bbox=dict(boxstyle="round,pad=0.5", facecolor="black", alpha=0.8),
zorder=20)
# ✅ 强制保持坐标不变(防止缩放抖动)
ax.set_xlim(*XLIM)
ax.set_ylim(*YLIM)
# 渲染
fig.canvas.draw()
plt.pause(1 / FPS)
# 保存帧
if SAVE_GIF:
tmp_file = f"tmp_frame_{frame_idx:05d}.png"
fig.savefig(tmp_file, dpi=100, facecolor=fig.get_facecolor())
frames.append(tmp_file)
# 生成 GIF
if SAVE_GIF and frames:
import imageio
with imageio.get_writer(OUTPUT_GIF, mode='I', duration=1/FPS) as writer:
for f in frames:
writer.append_data(imageio.imread(f))
os.remove(f)
print(f"\n🎉 GIF 已保存: {OUTPUT_GIF}")
plt.close(fig)
# ========================
# 启动程序
# ========================
if __name__ == "__main__":
print(f"\n🚀 启动工业级可视化引擎,共 {len(merged)} 帧...")
simulate_scene()
print("✅ 可视化完成!")
🎥 开始播放模拟场景...
Traceback (most recent call last):
File "/home/wyl/zxd/ceshi/csvdata/1983436010726993921/play.py", line 284, in <module>
simulate_scene()
File "/home/wyl/zxd/ceshi/csvdata/1983436010726993921/play.py", line 149, in simulate_scene
for txt in ax.texts.copy():
AttributeError: 'ArtistList' object has no attribute 'copy'
修复代码报错
最新发布