180 lines
4.4 KiB
Python
180 lines
4.4 KiB
Python
import csv
|
||
import os
|
||
import re
|
||
import matplotlib.pyplot as plt
|
||
import numpy as np
|
||
import glob
|
||
|
||
files = [
|
||
"stanley_*_*_*_*.csv",
|
||
]
|
||
|
||
data = {}
|
||
for pattern in files:
|
||
|
||
matched_files = glob.glob(pattern)
|
||
|
||
if not matched_files:
|
||
raise FileNotFoundError(
|
||
f"找不到文件: {pattern}"
|
||
)
|
||
matched_files.sort(
|
||
key=lambda x: (
|
||
float(x.split("_")[1]), # 速度
|
||
float(x.split("_")[2]), # 参数1
|
||
float(x.split("_")[3]), # 参数2
|
||
float(x.split("_")[4].replace(".csv", "")) # 半径
|
||
)
|
||
)
|
||
|
||
print(f"匹配到 {len(matched_files)} 个文件:")
|
||
for file in matched_files:
|
||
if not os.path.exists(file):
|
||
raise FileNotFoundError(f"找不到文件: {file}")
|
||
|
||
distance = []
|
||
lateral_error = []
|
||
heading_error = []
|
||
yaw_command = []
|
||
|
||
with open(file, "r", encoding="utf-8-sig", newline="") as f:
|
||
reader = csv.DictReader(f)
|
||
|
||
required = {"distance_m", "lateral_error_m", "heading_error_rad","yaw_command"}
|
||
if not required.issubset(reader.fieldnames or []):
|
||
raise ValueError(
|
||
f"{file} 缺少必要列，需要: {required}，实际列: {reader.fieldnames}"
|
||
)
|
||
|
||
for row in reader:
|
||
try:
|
||
distance.append(float(row["distance_m"]))
|
||
lateral_error.append(abs(float(row["lateral_error_m"])))
|
||
heading_error.append(abs(float(row["heading_error_rad"])))
|
||
yaw_command.append(float(row["yaw_command"]))
|
||
except (ValueError, TypeError):
|
||
continue
|
||
|
||
name = os.path.splitext(os.path.basename(file))[0]
|
||
data[name] = {
|
||
"distance": distance,
|
||
"lateral_error": lateral_error,
|
||
"heading_error": heading_error,
|
||
"yaw_command": yaw_command,
|
||
}
|
||
|
||
# 颜色由 matplotlib 自动分配，不手动指定
|
||
names = list(data.keys())
|
||
|
||
# =========================
|
||
# 图1：Lateral Error
|
||
# =========================
|
||
plt.figure(figsize=(12, 6))
|
||
|
||
for name in names:
|
||
plt.plot(
|
||
data[name]["distance"][::15],
|
||
data[name]["lateral_error"][::15],
|
||
label=name,
|
||
linewidth=1.5,
|
||
)
|
||
|
||
plt.xlabel("Distance (m)")
|
||
plt.ylabel("Lateral Error (m)")
|
||
plt.title("Lateral Error Comparison")
|
||
plt.grid(True, alpha=0.3)
|
||
plt.legend()
|
||
plt.tight_layout()
|
||
|
||
lateral_output = "Lateral_error_comparison.png"
|
||
plt.savefig(lateral_output, dpi=200)
|
||
plt.show()
|
||
|
||
# =========================
|
||
# 图2：Heading Error
|
||
# =========================
|
||
plt.figure(figsize=(12, 6))
|
||
|
||
for name in names:
|
||
plt.plot(
|
||
data[name]["distance"],
|
||
data[name]["heading_error"],
|
||
label=name,
|
||
linewidth=1.5,
|
||
)
|
||
|
||
plt.axhline(0, linewidth=2.0)
|
||
plt.xlabel("Distance (m)")
|
||
plt.ylabel("Heading Error (rad)")
|
||
plt.title("Heading Error Comparison")
|
||
plt.grid(True, alpha=0.3)
|
||
plt.legend()
|
||
plt.tight_layout()
|
||
|
||
heading_output = "Heading_error_comparison.png"
|
||
plt.savefig(heading_output, dpi=200)
|
||
plt.show()
|
||
|
||
|
||
# =========================
|
||
# 图3：Yaw Command
|
||
# =========================
|
||
plt.figure(figsize=(12, 6))
|
||
|
||
for name in names:
|
||
plt.plot(
|
||
data[name]["distance"],
|
||
data[name]["yaw_command"],
|
||
label=name,
|
||
linewidth=1.5,
|
||
)
|
||
|
||
plt.axhline(0, linewidth=2.0)
|
||
plt.xlabel("Distance (m)")
|
||
plt.ylabel("Yaw Command (rad)")
|
||
plt.title("Yaw Command Comparison")
|
||
plt.grid(True, alpha=0.3)
|
||
plt.legend()
|
||
plt.tight_layout()
|
||
|
||
yaw_output = "Yaw_command_comparison.png"
|
||
plt.savefig(yaw_output, dpi=200)
|
||
plt.show()
|
||
print("读取完成：")
|
||
for name in names:
|
||
print(f"{name}: {len(data[name]['distance'])} points")
|
||
|
||
print("\n已保存：")
|
||
print(lateral_output)
|
||
print(heading_output)
|
||
print(yaw_output)
|
||
|
||
|
||
print("\n" + "=" * 90)
|
||
print(
|
||
f"{'Controller':<20}"
|
||
f"{'Lat Max(m)':>12}"
|
||
f"{'Lat Mean(m)':>13}"
|
||
f"{'Lat RMSE(m)':>14}"
|
||
f"{'Lat P95(m)':>13}"
|
||
f"{'Head Max(deg)':>15}"
|
||
f"{'Head RMSE(deg)':>16}"
|
||
)
|
||
print("=" * 100)
|
||
|
||
for name in names:
|
||
lat = np.array(data[name]["lateral_error"])
|
||
head = np.array(data[name]["heading_error"])
|
||
|
||
print(
|
||
f"{name:<20}"
|
||
f"{np.max(lat):>12.4f}"
|
||
f"{np.mean(lat):>13.4f}"
|
||
f"{np.sqrt(np.mean(lat**2)):>14.4f}"
|
||
f"{np.percentile(lat, 95):>13.4f}"
|
||
f"{np.degrees(np.max(head)):>15.4f}"
|
||
f"{np.degrees(np.sqrt(np.mean(head**2))):>16.4f}"
|
||
)
|
||
|
||
print("=" * 100)
|