zhujinzhao/auto_drive_misson_center_webots_simulation
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity
94312c9b3c
auto_drive_misson_center_we.../error_calculate.py
liuhaoyan dc9f171aa9 Local control algorithm test only 
1.stanley test data accomplishment
2026-10-05 17:41:57 +08:00

836 lines
14 KiB
Python
Raw Blame History

This file contains ambiguous Unicode characters

This file contains Unicode characters that might be confused with other characters. If you think that this is intentional, you can safely ignore this warning. Use the Escape button to reveal them.

import re
import csv
from pathlib import Path
import numpy as np
import matplotlib.pyplot as plt
# ============================================================
# 参数
# ============================================================
LOG_PATTERN = "path_visualizer.log"
# 每 5 个完整数据取 1 个
SAMPLE_STEP = 1
CSV_FILE = "tracking_error.csv"
# ============================================================
# 1. 查找日志文件
# ============================================================
files = sorted(
Path(".").glob(LOG_PATTERN)
)
if not files:
raise FileNotFoundError(
f"当前目录没有找到：{LOG_PATTERN}"
)
print("找到日志文件：")
for file in files:
print(" ", file)
# ============================================================
# 2. 正则表达式
# ============================================================
# ------------------------------------------------------------
# current pose
#
# [debug] [path_follow]
# [current_pose_x:11.57,
# current_pose_y:-2.13,
# current_pose_yaw:-3.10]
# ------------------------------------------------------------
current_pose_pattern = re.compile(
r"current_pose_x:([-+0-9.eE]+),\s*"
r"current_pose_y:([-+0-9.eE]+),\s*"
r"current_pose_yaw:([-+0-9.eE]+)"
)
# ------------------------------------------------------------
# current path
#
# [debug] [path_follow]
# [current_path_x:11.59,
# current_path_y:-2.12,
# current_path_yaw:-3.10]
# ------------------------------------------------------------
current_path_pattern = re.compile(
r"current_path_x:([-+0-9.eE]+),\s*"
r"current_path_y:([-+0-9.eE]+),\s*"
r"current_path_yaw:([-+0-9.eE]+)"
)
yaw_command_pattern = re.compile(
r"current_cmd_angular_z:([-+0-9.eE]+)"
)
# ============================================================
# 3. 函数：角度归一化
# ============================================================
def normalize_angle(angle):
return (
angle + np.pi
) % (
2.0 * np.pi
) - np.pi
# ============================================================
# 4. 函数：根据 current pose + path 重新计算
# ============================================================
def calculate_errors(
current_x,
current_y,
current_yaw,
path_x,
path_y,
path_yaw
):
# --------------------------------------------------------
# 世界坐标系下的位置差
# --------------------------------------------------------
dx = path_x - current_x
dy = path_y - current_y
# --------------------------------------------------------
# 转换到车辆坐标系
#
# vehicle frame:
#
# x
# ↑
# |
# |
# ● -----> y
#
# 根据当前 yaw 进行旋转
# --------------------------------------------------------
lon_dis = (
np.cos(current_yaw) * dx
+
np.sin(current_yaw) * dy
)
dis = (
-np.sin(current_yaw) * dx
+
np.cos(current_yaw) * dy
)
# --------------------------------------------------------
# 车辆到路径点的欧氏距离
# --------------------------------------------------------
distance = np.sqrt(
dx * dx +
dy * dy
)
# --------------------------------------------------------
# 航向误差
# --------------------------------------------------------
alpha = normalize_angle(
path_yaw - current_yaw
)
# --------------------------------------------------------
# deg
# --------------------------------------------------------
alpha_deg = np.degrees(alpha)
return (
distance,
lon_dis,
dis,
alpha,
alpha_deg
)
# ============================================================
# 5. 读取日志
# ============================================================
all_data = []
yaw_command_data = []
total_raw_count = 0
for file in files:
print()
print("=" * 70)
print(f"读取：{file}")
print("=" * 70)
# --------------------------------------------------------
# 每个文件单独计数
# --------------------------------------------------------
file_data_count = 0
# --------------------------------------------------------
# 临时保存 current pose
# --------------------------------------------------------
pending_pose = None
pending_yaw = None
with file.open(
"r",
encoding="utf-8",
errors="ignore"
) as f:
for line in f:
# =================================================
# current pose
# =================================================
m_pose = current_pose_pattern.search(
line
)
m_yaw = yaw_command_pattern.search(line)
if m_yaw:
pending_yaw = float(m_yaw.group(1))
if file_data_count % SAMPLE_STEP == 0:
yaw_command_data.append(pending_yaw)
if m_pose:
pending_pose = {
"x": float(
m_pose.group(1)
),
"y": float(
m_pose.group(2)
),
"yaw": float(
m_pose.group(3)
),
}
continue
# =================================================
# current path
# =================================================
m_path = current_path_pattern.search(
line
)
if (
m_path is not None
and
pending_pose is not None
):
path_x = float(
m_path.group(1)
)
path_y = float(
m_path.group(2)
)
path_yaw = float(
m_path.group(3)
)
# =================================================
# 重新计算
# =================================================
(
point_distance,
lon_dis,
dis,
alpha,
alpha_deg
) = calculate_errors(
pending_pose["x"],
pending_pose["y"],
pending_pose["yaw"],
path_x,
path_y,
path_yaw
)
# =================================================
# 每 SAMPLE_STEP 个数据取一个
# =================================================
if file_data_count % SAMPLE_STEP == 0:
all_data.append({
"file": file.name,
"current_x":
pending_pose["x"],
"current_y":
pending_pose["y"],
"current_yaw":
pending_pose["yaw"],
"path_x":
path_x,
"path_y":
path_y,
"path_yaw":
path_yaw,
"point_distance":
point_distance,
"lon_dis":
lon_dis,
"dis":
dis,
"alpha":
alpha,
"alpha_deg":
alpha_deg,
})
file_data_count += 1
total_raw_count += 1
# ------------------------------------------------
# 清空
# ------------------------------------------------
pending_pose = None
print(
f"原始完整数据：{file_data_count}"
)
print(
f"抽样后数据："
f"{(file_data_count + SAMPLE_STEP - 1) // SAMPLE_STEP}"
)
# ============================================================
# 6. 检查
# ============================================================
if not all_data:
raise RuntimeError(
"没有解析到 current_pose + current_path 数据。"
)
print()
print("=" * 70)
print("解析完成")
print("=" * 70)
print(
f"原始数据：{total_raw_count}"
)
print(
f"最终数据：{len(all_data)}"
)
# ============================================================
# 7. 提取数据
# ============================================================
current_x = np.array([
d["current_x"]
for d in all_data
])
current_y = np.array([
d["current_y"]
for d in all_data
])
current_yaw = np.array([
d["current_yaw"]
for d in all_data
])
path_x = np.array([
d["path_x"]
for d in all_data
])
path_y = np.array([
d["path_y"]
for d in all_data
])
path_yaw = np.array([
d["path_yaw"]
for d in all_data
])
point_distance = np.array([
d["point_distance"]
for d in all_data
])
lon_dis = np.array([
d["lon_dis"]
for d in all_data
])
dis = np.array([
d["dis"]
for d in all_data
])
alpha = np.array([
d["alpha"]
for d in all_data
])
alpha_deg = np.array([
d["alpha_deg"]
for d in all_data
])
yaw_command = yaw_command_data# 确保长度一致
print(
f"yaw size: {len(yaw_command)}"
)
# ============================================================
# 8. 计算车辆累计行驶距离
# ============================================================
if len(path_x) > 1:
dx = np.diff(
path_x
)
dy = np.diff(
path_y
)
step_distance = np.sqrt(
dx ** 2 +
dy ** 2
)
else:
step_distance = np.array([])
for i in range(len(step_distance)
):
if step_distance[i] > 0.5:
step_distance[i] = 0.0
print(
f"Warning: step_distance[{i}] = {step_distance[i]:.3f} m, which is too large. Setting it to 0.0.")
distance_m = np.zeros(
len(path_x)
)
if len(path_x) > 1:
distance_m[1:] = np.cumsum(
step_distance
)
# ============================================================
# 9. 保存 CSV
# ============================================================
print()
print("=" * 70)
print("保存 CSV")
print("=" * 70)
with open(
CSV_FILE,
"w",
newline="",
encoding="utf-8-sig"
) as f:
writer = csv.writer(f)
writer.writerow([
"distance_m",
"current_x_m",
"current_y_m",
"lateral_error_m",
"heading_error_rad",
"heading_error_deg",
"yaw_command",
])
for i in range(
len(all_data)
):
if i>= len(yaw_command):
break
writer.writerow([
f"{distance_m[i]:.8f}",
f"{current_x[i]:.8f}",
f"{current_y[i]:.8f}",
f"{dis[i]:.8f}",
f"{alpha[i]:.8f}",
f"{alpha_deg[i]:.8f}",
f"{yaw_command[i]:.8f}"
])
print(
f"CSV 已保存："
f"{Path(CSV_FILE).resolve()}"
)
# ============================================================
# 11. 横向误差
# ============================================================
plt.figure(
figsize=(12, 5)
)
plt.plot(
distance_m,
dis,
linewidth=1.2
)
plt.axhline(
0.0,
linestyle="--",
linewidth=0.8
)
plt.xlabel(
"Distance (m)"
)
plt.ylabel(
"Lateral Error (m)"
)
plt.title(
"Recalculated Lateral Error"
)
plt.grid(True)
plt.tight_layout()
plt.show()
# ============================================================
# 13. 航向误差 rad
# ============================================================
plt.figure(
figsize=(12, 5)
)
plt.plot(
distance_m,
alpha,
linewidth=1.2
)
plt.axhline(
0.0,
linestyle="--",
linewidth=0.8
)
plt.xlabel(
"Distance (m)"
)
plt.ylabel(
"Heading Error (rad)"
)
plt.title(
"Recalculated Heading Error"
)
plt.grid(True)
plt.tight_layout()
plt.show()
# ============================================================
# 14. yaw command rad
# ============================================================
plt.figure(
figsize=(12, 5)
)
plt.plot(
distance_m,
yaw_command,
linewidth=1.2
)
plt.axhline(
0.0,
linestyle="--",
linewidth=0.8
)
plt.xlabel(
"Distance (m)"
)
plt.ylabel(
"Yaw Command (rad)"
)
plt.title(
"Recalculated Yaw Command"
)
plt.grid(True)
plt.tight_layout()
plt.show()
# ============================================================
# 15. 车辆轨迹 + 路径点
# ============================================================
plt.figure(
figsize=(8, 8)
)
plt.plot(
current_x,
current_y,
linewidth=1.2,
label="Current Pose"
)
plt.plot(
path_x,
path_y,
linestyle="--",
linewidth=1.0,
label="Current Path Point"
)
plt.scatter(
current_x[0],
current_y[0],
s=50,
label="Start"
)
plt.scatter(
current_x[-1],
current_y[-1],
s=50,
label="End"
)
plt.xlabel(
"X (m)"
)
plt.ylabel(
"Y (m)"
)
plt.title(
"Current Pose and Path"
)
plt.axis(
"equal"
)
plt.grid(True)
plt.legend()
plt.tight_layout()
plt.show()
# ============================================================
# 16. 误差统计
# ============================================================
print()
print("=" * 70)
print("误差统计")
print("=" * 70)
print(
f"最大横向误差："
f"{np.max(np.abs(dis)):.6f} m"
)
print(
f"平均绝对横向误差："
f"{np.mean(np.abs(dis)):.6f} m"
)
print(
f"横向误差 RMSE："
f"{np.sqrt(np.mean(dis ** 2)):.6f} m"
)
print()
print(
f"最大纵向误差："
f"{np.max(np.abs(lon_dis)):.6f} m"
)
print(
f"平均绝对纵向误差："
f"{np.mean(np.abs(lon_dis)):.6f} m"
)
print(
f"纵向误差 RMSE："
f"{np.sqrt(np.mean(lon_dis ** 2)):.6f} m"
)
print()
print(
f"最大航向误差："
f"{np.max(np.abs(alpha_deg)):.6f} deg"
)
print(
f"平均绝对航向误差："
f"{np.mean(np.abs(alpha_deg)):.6f} deg"
)
print(
f"航向误差 RMSE："
f"{np.sqrt(np.mean(alpha_deg ** 2)):.6f} deg"
)
print()
print(
f"最大车辆-路径点距离："
f"{np.max(point_distance):.6f} m"
)
print(
f"平均车辆-路径点距离："
f"{np.mean(point_distance):.6f} m"
)
print(
f"总行驶距离："
f"{distance_m[-1]:.3f} m"
)
Reference in New Issue View Git Blame Copy Permalink