921 lines
29 KiB
Python
921 lines
29 KiB
Python
"""绘制理想轨迹与Detour实际轨迹对比图。
|
|
|
|
不传CSV路径时，默认处理本脚本目录下的全部CSV文件。
|
|
本文件也提供其余三个绘图脚本共用的数据预处理函数。
|
|
"""
|
|
|
|
from __future__ import annotations
|
|
|
|
import argparse
|
|
import re
|
|
from pathlib import Path
|
|
from typing import Any
|
|
|
|
import matplotlib
|
|
matplotlib.use("Agg")
|
|
import matplotlib.pyplot as plt
|
|
import numpy as np
|
|
import pandas as pd
|
|
from scipy.signal import savgol_filter
|
|
|
|
|
|
SCRIPT_DIR = Path(__file__).resolve().parent
|
|
REQUIRED_COLUMNS = {
|
|
"ElapsedSeconds",
|
|
"TrajectoryName",
|
|
"DetourX",
|
|
"DetourY",
|
|
"DetourTheta",
|
|
"CommandSpeed",
|
|
"CommandAngularSpeed",
|
|
"ReferenceStartX",
|
|
"ReferenceStartY",
|
|
"ReferenceEndX",
|
|
"ReferenceEndY",
|
|
"ReferenceSpeed",
|
|
}
|
|
|
|
|
|
def configure_matplotlib() -> None:
|
|
"""配置中文字体和图片输出风格。"""
|
|
matplotlib.rcParams["font.sans-serif"] = [
|
|
"Microsoft YaHei",
|
|
"SimHei",
|
|
"Arial Unicode MS",
|
|
"DejaVu Sans",
|
|
]
|
|
matplotlib.rcParams["axes.unicode_minus"] = False
|
|
matplotlib.rcParams["figure.dpi"] = 120
|
|
|
|
|
|
def _odd_window_length(
|
|
sample_count: int,
|
|
sample_interval: float,
|
|
window_seconds: float,
|
|
polynomial_order: int = 2,
|
|
) -> int | None:
|
|
"""计算不超过数据长度的Savitzky-Golay奇数窗口。"""
|
|
requested = max(
|
|
polynomial_order + 2,
|
|
int(round(window_seconds / sample_interval)),
|
|
)
|
|
if requested % 2 == 0:
|
|
requested += 1
|
|
|
|
maximum = sample_count if sample_count % 2 == 1 else sample_count - 1
|
|
window = min(requested, maximum)
|
|
minimum = polynomial_order + 2
|
|
if minimum % 2 == 0:
|
|
minimum += 1
|
|
|
|
return window if window >= minimum else None
|
|
|
|
|
|
def wrap_degrees(angle_degrees: np.ndarray) -> np.ndarray:
|
|
"""将角度差归一化到[-180°, 180°)。"""
|
|
return (angle_degrees + 180.0) % 360.0 - 180.0
|
|
|
|
|
|
def build_complete_s_curve(
|
|
start: np.ndarray,
|
|
end: np.ndarray,
|
|
offset_mm: float,
|
|
samples_per_segment: int = 120,
|
|
) -> tuple[np.ndarray, np.ndarray]:
|
|
"""重建测试使用的三段三次贝塞尔完整S曲线及各点切线航向。"""
|
|
line = end - start
|
|
length = float(np.linalg.norm(line))
|
|
if length <= 1e-6:
|
|
raise ValueError("S型曲线的起点和终点不能重合。")
|
|
|
|
forward = line / length
|
|
left = np.array([-forward[1], forward[0]])
|
|
controls = [
|
|
np.array([
|
|
[0.0, 0.0],
|
|
[length / 12.0, 0.0],
|
|
[length / 6.0, offset_mm],
|
|
[length * 0.25, offset_mm],
|
|
]),
|
|
np.array([
|
|
[length * 0.25, offset_mm],
|
|
[length / 3.0, offset_mm],
|
|
[length * 2.0 / 3.0, -offset_mm],
|
|
[length * 0.75, -offset_mm],
|
|
]),
|
|
np.array([
|
|
[length * 0.75, -offset_mm],
|
|
[length * 5.0 / 6.0, -offset_mm],
|
|
[length * 11.0 / 12.0, 0.0],
|
|
[length, 0.0],
|
|
]),
|
|
]
|
|
|
|
local_parts: list[np.ndarray] = []
|
|
derivative_parts: list[np.ndarray] = []
|
|
for index, points in enumerate(controls):
|
|
t = np.linspace(0.0, 1.0, samples_per_segment + 1)
|
|
if index > 0:
|
|
t = t[1:]
|
|
one_minus_t = 1.0 - t
|
|
local = (
|
|
one_minus_t[:, None] ** 3 * points[0]
|
|
+ 3.0
|
|
* one_minus_t[:, None] ** 2
|
|
* t[:, None]
|
|
* points[1]
|
|
+ 3.0
|
|
* one_minus_t[:, None]
|
|
* t[:, None] ** 2
|
|
* points[2]
|
|
+ t[:, None] ** 3 * points[3]
|
|
)
|
|
derivative = (
|
|
3.0
|
|
* one_minus_t[:, None] ** 2
|
|
* (points[1] - points[0])
|
|
+ 6.0
|
|
* one_minus_t[:, None]
|
|
* t[:, None]
|
|
* (points[2] - points[1])
|
|
+ 3.0
|
|
* t[:, None] ** 2
|
|
* (points[3] - points[2])
|
|
)
|
|
local_parts.append(local)
|
|
derivative_parts.append(derivative)
|
|
|
|
local_points = np.vstack(local_parts)
|
|
local_derivatives = np.vstack(derivative_parts)
|
|
world_points = (
|
|
start
|
|
+ local_points[:, 0, None] * forward
|
|
+ local_points[:, 1, None] * left
|
|
)
|
|
world_derivatives = (
|
|
local_derivatives[:, 0, None] * forward
|
|
+ local_derivatives[:, 1, None] * left
|
|
)
|
|
headings = np.rad2deg(
|
|
np.arctan2(world_derivatives[:, 1], world_derivatives[:, 0])
|
|
)
|
|
return world_points, headings
|
|
|
|
|
|
def segmented_savgol(
|
|
values: np.ndarray,
|
|
sample_interval: float,
|
|
window_seconds: float,
|
|
derivative: int = 0,
|
|
polynomial_order: int = 2,
|
|
) -> np.ndarray:
|
|
"""对含NaN断点的数据逐段执行SG滤波或求导。"""
|
|
values = np.asarray(values, dtype=float)
|
|
result = np.full_like(values, np.nan)
|
|
finite_indices = np.flatnonzero(np.isfinite(values))
|
|
if finite_indices.size == 0:
|
|
return result
|
|
|
|
breaks = np.flatnonzero(np.diff(finite_indices) > 1)
|
|
starts = np.r_[0, breaks + 1]
|
|
ends = np.r_[breaks + 1, finite_indices.size]
|
|
|
|
for start_index, end_index in zip(starts, ends):
|
|
indices = finite_indices[start_index:end_index]
|
|
segment = values[indices]
|
|
window = _odd_window_length(
|
|
len(segment),
|
|
sample_interval,
|
|
window_seconds,
|
|
polynomial_order,
|
|
)
|
|
if window is not None:
|
|
result[indices] = savgol_filter(
|
|
segment,
|
|
window,
|
|
polynomial_order,
|
|
deriv=derivative,
|
|
delta=sample_interval,
|
|
mode="interp",
|
|
)
|
|
elif derivative == 0:
|
|
result[indices] = segment
|
|
elif len(segment) >= 2:
|
|
result[indices] = np.gradient(segment, sample_interval)
|
|
|
|
return result
|
|
|
|
|
|
def shade_localization_jump_windows(
|
|
axis,
|
|
metadata: dict[str, Any],
|
|
) -> None:
|
|
"""在时间曲线中标记不应参与车辆动力学评价的定位跳变窗口。"""
|
|
for index, (start, end) in enumerate(
|
|
metadata["jump_exclusion_windows"]
|
|
):
|
|
axis.axvspan(
|
|
start,
|
|
end,
|
|
color="tab:red",
|
|
alpha=0.12,
|
|
label="Detour定位跳变排除窗口" if index == 0 else None,
|
|
)
|
|
|
|
|
|
def load_and_resample(
|
|
csv_path: Path,
|
|
frequency_hz: float = 20.0,
|
|
filter_window_seconds: float = 0.55,
|
|
) -> tuple[pd.DataFrame, dict[str, Any]]:
|
|
"""压缩Detour保持帧，检测定位跳变，再分段重采样和平滑。"""
|
|
if not np.isfinite(frequency_hz) or frequency_hz <= 0.0:
|
|
raise ValueError("重采样频率必须是正有限值。")
|
|
|
|
raw = pd.read_csv(csv_path)
|
|
missing = REQUIRED_COLUMNS.difference(raw.columns)
|
|
if missing:
|
|
raise ValueError(
|
|
f"{csv_path.name}缺少列：{', '.join(sorted(missing))}"
|
|
)
|
|
|
|
numeric_columns = [
|
|
"ElapsedSeconds",
|
|
"DetourX",
|
|
"DetourY",
|
|
"DetourTheta",
|
|
"CommandSpeed",
|
|
"CommandAngularSpeed",
|
|
"ReferenceStartX",
|
|
"ReferenceStartY",
|
|
"ReferenceEndX",
|
|
"ReferenceEndY",
|
|
"ReferenceSpeed",
|
|
]
|
|
optional_numeric_columns = [
|
|
"CommandAngularSpeedRadPerSecond",
|
|
"ReferenceAngularSpeedRadPerSecond",
|
|
"ReferenceMotionFrameYawDegrees",
|
|
]
|
|
numeric_columns.extend(
|
|
column
|
|
for column in optional_numeric_columns
|
|
if column in raw.columns
|
|
)
|
|
for column in numeric_columns:
|
|
raw[column] = pd.to_numeric(raw[column], errors="coerce")
|
|
|
|
raw = (
|
|
raw.dropna(subset=[
|
|
"ElapsedSeconds",
|
|
"DetourX",
|
|
"DetourY",
|
|
"DetourTheta",
|
|
])
|
|
.sort_values("ElapsedSeconds")
|
|
.drop_duplicates("ElapsedSeconds", keep="last")
|
|
.reset_index(drop=True)
|
|
)
|
|
if len(raw) < 5:
|
|
raise ValueError(f"{csv_path.name}有效数据不足5行。")
|
|
|
|
time_raw = raw["ElapsedSeconds"].to_numpy(dtype=float)
|
|
time_raw = time_raw - time_raw[0]
|
|
raw["ElapsedSeconds"] = time_raw
|
|
duration = float(time_raw[-1])
|
|
sample_interval = 1.0 / frequency_hz
|
|
time_uniform = np.arange(
|
|
0.0,
|
|
duration + sample_interval * 0.5,
|
|
sample_interval,
|
|
)
|
|
|
|
def interpolate_command(column: str) -> np.ndarray:
|
|
values = raw[column].to_numpy(dtype=float)
|
|
return np.interp(time_uniform, time_raw, values)
|
|
|
|
# 记录器频率高于Detour更新频率，会得到A,A,B,B形式的保持帧。
|
|
# 速度估计前先保留真正发生位姿更新的样本。
|
|
x_all = raw["DetourX"].to_numpy(dtype=float)
|
|
y_all = raw["DetourY"].to_numpy(dtype=float)
|
|
theta_all = raw["DetourTheta"].to_numpy(dtype=float)
|
|
position_change = np.hypot(np.diff(x_all), np.diff(y_all))
|
|
heading_change = np.abs(wrap_degrees(np.diff(theta_all)))
|
|
update_mask = np.r_[
|
|
True,
|
|
(position_change > 1e-6) | (heading_change > 1e-6),
|
|
]
|
|
updates = raw.loc[update_mask].copy().reset_index(drop=True)
|
|
if len(updates) < 3:
|
|
raise ValueError(f"{csv_path.name}有效Detour更新点不足3个。")
|
|
|
|
update_time = updates["ElapsedSeconds"].to_numpy(dtype=float)
|
|
update_x = updates["DetourX"].to_numpy(dtype=float)
|
|
update_y = updates["DetourY"].to_numpy(dtype=float)
|
|
update_theta = updates["DetourTheta"].to_numpy(dtype=float)
|
|
update_command_speed = np.abs(
|
|
updates["CommandSpeed"].to_numpy(dtype=float)
|
|
)
|
|
if "CommandAngularSpeedRadPerSecond" in updates.columns:
|
|
update_command_angular_rad = np.abs(
|
|
updates[
|
|
"CommandAngularSpeedRadPerSecond"
|
|
].to_numpy(dtype=float)
|
|
)
|
|
else:
|
|
# 旧CSV中的CommandAngularSpeed单位为deg/s。
|
|
update_command_angular_rad = np.deg2rad(
|
|
np.abs(
|
|
updates[
|
|
"CommandAngularSpeed"
|
|
].to_numpy(dtype=float)
|
|
)
|
|
)
|
|
|
|
# 自适应跳变阈值：正常移动允许达到参考位移的3倍并保留15mm余量；
|
|
# 低速阶段仍至少允许30mm，防止把普通定位噪声误判为跳变。
|
|
update_dt = np.diff(update_time)
|
|
update_distance = np.hypot(np.diff(update_x), np.diff(update_y))
|
|
expected_distance = (
|
|
0.5 *
|
|
(update_command_speed[1:] + update_command_speed[:-1]) *
|
|
update_dt *
|
|
1000.0
|
|
)
|
|
distance_threshold = np.maximum(
|
|
30.0,
|
|
expected_distance * 3.0 + 15.0,
|
|
)
|
|
update_heading_delta = np.abs(
|
|
wrap_degrees(np.diff(update_theta))
|
|
)
|
|
expected_heading_delta = (
|
|
0.5 *
|
|
(
|
|
update_command_angular_rad[1:] +
|
|
update_command_angular_rad[:-1]
|
|
) *
|
|
update_dt *
|
|
180.0 / np.pi
|
|
)
|
|
heading_threshold = np.maximum(
|
|
5.0,
|
|
expected_heading_delta * 3.0 + 2.0,
|
|
)
|
|
jump_before_current = (
|
|
(update_distance > distance_threshold) |
|
|
(update_heading_delta > heading_threshold)
|
|
)
|
|
jump_at_update = np.r_[False, jump_before_current]
|
|
segment_ids = np.cumsum(jump_at_update.astype(int))
|
|
|
|
jump_events: list[dict[str, float]] = []
|
|
for current_index in np.flatnonzero(jump_at_update):
|
|
previous_index = current_index - 1
|
|
jump_events.append({
|
|
"time_seconds": float(update_time[current_index]),
|
|
"distance_mm": float(update_distance[previous_index]),
|
|
"heading_change_degrees":
|
|
float(update_heading_delta[previous_index]),
|
|
"before_x_mm": float(update_x[previous_index]),
|
|
"before_y_mm": float(update_y[previous_index]),
|
|
"after_x_mm": float(update_x[current_index]),
|
|
"after_y_mm": float(update_y[current_index]),
|
|
})
|
|
|
|
# 不跨越定位跳变插值。跳变前后之间保留NaN，使轨迹图自然断线，
|
|
# 也防止SG滤波把坐标修正涂抹成车辆高速运动。
|
|
x_resampled = np.full_like(time_uniform, np.nan)
|
|
y_resampled = np.full_like(time_uniform, np.nan)
|
|
theta_resampled = np.full_like(time_uniform, np.nan)
|
|
update_theta_unwrapped = np.rad2deg(
|
|
np.unwrap(np.deg2rad(update_theta))
|
|
)
|
|
maximum_segment_id = int(segment_ids[-1])
|
|
for segment_id in range(maximum_segment_id + 1):
|
|
segment_mask = segment_ids == segment_id
|
|
segment_time = update_time[segment_mask]
|
|
if segment_time.size == 0:
|
|
continue
|
|
|
|
interval_start = (
|
|
0.0 if segment_id == 0 else float(segment_time[0])
|
|
)
|
|
interval_end = (
|
|
duration
|
|
if segment_id == maximum_segment_id
|
|
else float(segment_time[-1])
|
|
)
|
|
uniform_mask = (
|
|
(time_uniform >= interval_start) &
|
|
(time_uniform <= interval_end)
|
|
)
|
|
x_resampled[uniform_mask] = np.interp(
|
|
time_uniform[uniform_mask],
|
|
segment_time,
|
|
update_x[segment_mask],
|
|
)
|
|
y_resampled[uniform_mask] = np.interp(
|
|
time_uniform[uniform_mask],
|
|
segment_time,
|
|
update_y[segment_mask],
|
|
)
|
|
theta_resampled[uniform_mask] = np.interp(
|
|
time_uniform[uniform_mask],
|
|
segment_time,
|
|
update_theta_unwrapped[segment_mask],
|
|
)
|
|
|
|
x_filtered = segmented_savgol(
|
|
x_resampled,
|
|
sample_interval,
|
|
filter_window_seconds,
|
|
)
|
|
y_filtered = segmented_savgol(
|
|
y_resampled,
|
|
sample_interval,
|
|
filter_window_seconds,
|
|
)
|
|
theta_filtered = segmented_savgol(
|
|
theta_resampled,
|
|
sample_interval,
|
|
filter_window_seconds,
|
|
)
|
|
|
|
exclusion_half_width = max(
|
|
0.30,
|
|
filter_window_seconds * 0.5,
|
|
)
|
|
jump_exclusion_windows = [
|
|
(
|
|
max(0.0, event["time_seconds"] - exclusion_half_width),
|
|
min(duration, event["time_seconds"] + exclusion_half_width),
|
|
)
|
|
for event in jump_events
|
|
]
|
|
invalid_near_jump = np.zeros(len(time_uniform), dtype=bool)
|
|
for start, end in jump_exclusion_windows:
|
|
invalid_near_jump |= (
|
|
(time_uniform >= start) & (time_uniform <= end)
|
|
)
|
|
|
|
if "CommandAngularSpeedRadPerSecond" in raw.columns:
|
|
angular_command_rad = interpolate_command(
|
|
"CommandAngularSpeedRadPerSecond"
|
|
)
|
|
else:
|
|
angular_command_rad = np.deg2rad(
|
|
interpolate_command("CommandAngularSpeed")
|
|
)
|
|
|
|
frame = pd.DataFrame({
|
|
"TimeSeconds": time_uniform,
|
|
"DetourXRawMm": x_resampled,
|
|
"DetourYRawMm": y_resampled,
|
|
"DetourXFilteredMm": x_filtered,
|
|
"DetourYFilteredMm": y_filtered,
|
|
"DetourThetaUnwrappedDeg": theta_filtered,
|
|
"DetourThetaDeg": wrap_degrees(theta_filtered),
|
|
"CommandSpeedMps": interpolate_command("CommandSpeed"),
|
|
"CommandAngularSpeedRadPerSec":
|
|
angular_command_rad,
|
|
"InvalidNearLocalizationJump": invalid_near_jump,
|
|
})
|
|
|
|
first = raw.iloc[0]
|
|
metadata: dict[str, Any] = {
|
|
"csv_path": csv_path,
|
|
"trajectory_name": str(first["TrajectoryName"]),
|
|
"controller_name": str(first.get("ControllerName", "")),
|
|
"trial_number": str(first.get("TrialNumber", "")),
|
|
# 蟹行轨迹的运动前向相对车体X轴逆时针偏置90°。
|
|
# DetourTheta始终是车体航向，计算航向误差时必须扣除该偏置。
|
|
"motion_frame_yaw_degrees": float(
|
|
first["ReferenceMotionFrameYawDegrees"]
|
|
if (
|
|
"ReferenceMotionFrameYawDegrees" in raw.columns
|
|
and pd.notna(
|
|
first["ReferenceMotionFrameYawDegrees"]
|
|
)
|
|
)
|
|
else (
|
|
90.0
|
|
if "crab" in (
|
|
str(first["TrajectoryName"]) +
|
|
str(first.get("ControllerName", ""))
|
|
).lower()
|
|
else 0.0
|
|
)
|
|
),
|
|
"reference_start_mm": np.array(
|
|
[first["ReferenceStartX"], first["ReferenceStartY"]],
|
|
dtype=float,
|
|
),
|
|
"reference_end_mm": np.array(
|
|
[first["ReferenceEndX"], first["ReferenceEndY"]],
|
|
dtype=float,
|
|
),
|
|
"reference_speed_mps": float(first["ReferenceSpeed"]),
|
|
"reference_angular_speed_rad_per_second": float(
|
|
first.get(
|
|
"ReferenceAngularSpeedRadPerSecond",
|
|
0.0,
|
|
)
|
|
),
|
|
# 圆弧构造时使用了测试开始处Detour航向，因此这里取首帧航向。
|
|
"start_heading_degrees": float(first["DetourTheta"]),
|
|
"sample_interval_seconds": sample_interval,
|
|
"filter_window_seconds": filter_window_seconds,
|
|
"raw_sample_count": len(raw),
|
|
"detour_update_count": len(updates),
|
|
"held_sample_count": int(len(raw) - len(updates)),
|
|
"localization_jump_events": jump_events,
|
|
"jump_exclusion_windows": jump_exclusion_windows,
|
|
}
|
|
return frame, metadata
|
|
|
|
|
|
def build_reference(
|
|
frame: pd.DataFrame,
|
|
metadata: dict[str, Any],
|
|
) -> dict[str, np.ndarray | float | str]:
|
|
"""根据CSV元数据建立直线、圆弧、完整S曲线或原地自转参考及误差。"""
|
|
trajectory_name = str(metadata["trajectory_name"])
|
|
start = np.asarray(metadata["reference_start_mm"], dtype=float)
|
|
end = np.asarray(metadata["reference_end_mm"], dtype=float)
|
|
motion_frame_yaw_degrees = float(
|
|
metadata.get("motion_frame_yaw_degrees", 0.0)
|
|
)
|
|
actual = frame[
|
|
["DetourXFilteredMm", "DetourYFilteredMm"]
|
|
].to_numpy(dtype=float)
|
|
actual_heading = frame["DetourThetaUnwrappedDeg"].to_numpy(dtype=float)
|
|
|
|
radius_match = re.search(
|
|
r"LeftArc(?P<sweep>[0-9.]+)_R(?P<radius>[0-9.]+)mm",
|
|
trajectory_name,
|
|
flags=re.IGNORECASE,
|
|
)
|
|
if radius_match:
|
|
radius = float(radius_match.group("radius"))
|
|
sweep_degrees = float(radius_match.group("sweep"))
|
|
start_body_heading = float(metadata["start_heading_degrees"])
|
|
start_motion_heading = (
|
|
start_body_heading + motion_frame_yaw_degrees
|
|
)
|
|
heading_radians = np.deg2rad(start_motion_heading)
|
|
center = start + radius * np.array(
|
|
[-np.sin(heading_radians), np.cos(heading_radians)]
|
|
)
|
|
start_radial_degrees = start_motion_heading - 90.0
|
|
|
|
radial = actual - center
|
|
distance_to_center = np.linalg.norm(radial, axis=1)
|
|
radial_angle_degrees = np.rad2deg(
|
|
np.arctan2(radial[:, 1], radial[:, 0])
|
|
)
|
|
radial_angle_radians = np.deg2rad(radial_angle_degrees)
|
|
reference_points = center + radius * np.column_stack([
|
|
np.cos(radial_angle_radians),
|
|
np.sin(radial_angle_radians),
|
|
])
|
|
# 对逆时针圆弧，正横向误差表示车辆位于轨迹左侧（圆内侧）。
|
|
lateral_error = radius - distance_to_center
|
|
reference_motion_heading = radial_angle_degrees + 90.0
|
|
reference_heading = (
|
|
reference_motion_heading - motion_frame_yaw_degrees
|
|
)
|
|
heading_error = wrap_degrees(
|
|
actual_heading - reference_heading
|
|
)
|
|
|
|
plot_angles = np.deg2rad(
|
|
np.linspace(
|
|
start_radial_degrees,
|
|
start_radial_degrees + sweep_degrees,
|
|
361,
|
|
)
|
|
)
|
|
ideal_plot = center + radius * np.column_stack([
|
|
np.cos(plot_angles),
|
|
np.sin(plot_angles),
|
|
])
|
|
return {
|
|
"kind": "left_arc",
|
|
"ideal_plot_mm": ideal_plot,
|
|
"reference_points_mm": reference_points,
|
|
"reference_heading_degrees": reference_heading,
|
|
"reference_motion_heading_degrees":
|
|
reference_motion_heading,
|
|
"lateral_error_mm": lateral_error,
|
|
"heading_error_degrees": heading_error,
|
|
"center_mm": center,
|
|
"radius_mm": radius,
|
|
}
|
|
|
|
s_curve_match = re.search(
|
|
r"SCurve(?P<length>[0-9.]+)m_A(?P<offset>[0-9.]+)mm",
|
|
trajectory_name,
|
|
flags=re.IGNORECASE,
|
|
)
|
|
if s_curve_match:
|
|
offset_mm = float(s_curve_match.group("offset"))
|
|
ideal_plot, ideal_heading = build_complete_s_curve(
|
|
start,
|
|
end,
|
|
offset_mm,
|
|
)
|
|
delta = actual[:, np.newaxis, :] - ideal_plot[np.newaxis, :, :]
|
|
nearest_indices = np.argmin(
|
|
np.sum(delta * delta, axis=2),
|
|
axis=1,
|
|
)
|
|
reference_points = ideal_plot[nearest_indices]
|
|
reference_motion_heading = ideal_heading[nearest_indices]
|
|
reference_heading = (
|
|
reference_motion_heading - motion_frame_yaw_degrees
|
|
)
|
|
heading_radians = np.deg2rad(reference_motion_heading)
|
|
left_normals = np.column_stack([
|
|
-np.sin(heading_radians),
|
|
np.cos(heading_radians),
|
|
])
|
|
lateral_error = np.sum(
|
|
(actual - reference_points) * left_normals,
|
|
axis=1,
|
|
)
|
|
heading_error = wrap_degrees(
|
|
actual_heading - reference_heading
|
|
)
|
|
return {
|
|
"kind": "s_curve",
|
|
"ideal_plot_mm": ideal_plot,
|
|
"reference_points_mm": reference_points,
|
|
"reference_heading_degrees": reference_heading,
|
|
"reference_motion_heading_degrees":
|
|
reference_motion_heading,
|
|
"lateral_error_mm": lateral_error,
|
|
"heading_error_degrees": heading_error,
|
|
"offset_mm": offset_mm,
|
|
}
|
|
|
|
line = end - start
|
|
length = float(np.linalg.norm(line))
|
|
if length <= 1e-6:
|
|
rotation_match = re.search(
|
|
r"Rotate(?P<angle>[+-]?[0-9.]+)",
|
|
trajectory_name,
|
|
flags=re.IGNORECASE,
|
|
)
|
|
if rotation_match:
|
|
relative_angle_degrees = float(
|
|
rotation_match.group("angle")
|
|
)
|
|
target_heading_degrees = (
|
|
float(metadata["start_heading_degrees"]) +
|
|
relative_angle_degrees
|
|
)
|
|
reference_points = np.repeat(
|
|
start[np.newaxis, :],
|
|
len(frame),
|
|
axis=0,
|
|
)
|
|
position_drift = np.linalg.norm(
|
|
actual - start,
|
|
axis=1,
|
|
)
|
|
reference_heading = np.full(
|
|
len(frame),
|
|
target_heading_degrees,
|
|
)
|
|
heading_error = wrap_degrees(
|
|
actual_heading - reference_heading
|
|
)
|
|
ideal_plot = np.repeat(
|
|
start[np.newaxis, :],
|
|
2,
|
|
axis=0,
|
|
)
|
|
return {
|
|
"kind": "in_place_rotation",
|
|
"ideal_plot_mm": ideal_plot,
|
|
"reference_points_mm": reference_points,
|
|
"reference_heading_degrees": reference_heading,
|
|
# 对原地自转，该字段表示偏离初始旋转中心的距离。
|
|
"lateral_error_mm": position_drift,
|
|
"heading_error_degrees": heading_error,
|
|
"rotation_center_mm": start,
|
|
"relative_angle_degrees": relative_angle_degrees,
|
|
"target_heading_degrees": target_heading_degrees,
|
|
}
|
|
|
|
raise ValueError(
|
|
f"{trajectory_name}无法识别为圆弧，且参考直线长度为0。"
|
|
)
|
|
|
|
tangent = line / length
|
|
left_normal = np.array([-tangent[1], tangent[0]])
|
|
displacement = actual - start
|
|
progress = np.clip(displacement @ tangent, 0.0, length)
|
|
reference_points = start + np.outer(progress, tangent)
|
|
lateral_error = (actual - reference_points) @ left_normal
|
|
reference_motion_heading_scalar = np.rad2deg(
|
|
np.arctan2(tangent[1], tangent[0])
|
|
)
|
|
reference_heading_scalar = (
|
|
reference_motion_heading_scalar -
|
|
motion_frame_yaw_degrees
|
|
)
|
|
reference_heading = np.full(
|
|
len(frame),
|
|
reference_heading_scalar,
|
|
)
|
|
heading_error = wrap_degrees(
|
|
actual_heading - reference_heading
|
|
)
|
|
ideal_plot = np.linspace(start, end, 361)
|
|
return {
|
|
"kind": "line",
|
|
"ideal_plot_mm": ideal_plot,
|
|
"reference_points_mm": reference_points,
|
|
"reference_heading_degrees": reference_heading,
|
|
"reference_motion_heading_degrees": np.full(
|
|
len(frame),
|
|
reference_motion_heading_scalar,
|
|
),
|
|
"lateral_error_mm": lateral_error,
|
|
"heading_error_degrees": heading_error,
|
|
}
|
|
|
|
|
|
def discover_csv_files(arguments: list[str]) -> list[Path]:
|
|
"""解析命令行CSV；未指定时使用脚本目录下全部CSV。"""
|
|
if arguments:
|
|
files = [Path(item).expanduser().resolve() for item in arguments]
|
|
else:
|
|
files = sorted(SCRIPT_DIR.glob("*.csv"))
|
|
if not files:
|
|
raise FileNotFoundError("没有找到可处理的CSV文件。")
|
|
return files
|
|
|
|
|
|
def output_path(
|
|
csv_path: Path,
|
|
output_directory: str | None,
|
|
suffix: str,
|
|
) -> Path:
|
|
"""构造图片输出路径并创建目录。"""
|
|
directory = (
|
|
Path(output_directory).expanduser().resolve()
|
|
if output_directory
|
|
else csv_path.parent / "plots"
|
|
)
|
|
directory.mkdir(parents=True, exist_ok=True)
|
|
return directory / f"{csv_path.stem}_{suffix}.png"
|
|
|
|
|
|
def plot_trajectory(
|
|
csv_path: Path,
|
|
frequency_hz: float,
|
|
filter_window_seconds: float,
|
|
output_directory: str | None,
|
|
show: bool,
|
|
) -> Path:
|
|
"""生成单份CSV的理想/实际轨迹对比图。"""
|
|
frame, metadata = load_and_resample(
|
|
csv_path,
|
|
frequency_hz,
|
|
filter_window_seconds,
|
|
)
|
|
reference = build_reference(frame, metadata)
|
|
|
|
actual_x_m = frame["DetourXFilteredMm"].to_numpy() / 1000.0
|
|
actual_y_m = frame["DetourYFilteredMm"].to_numpy() / 1000.0
|
|
ideal_m = np.asarray(reference["ideal_plot_mm"]) / 1000.0
|
|
|
|
fig, ax = plt.subplots(figsize=(8.0, 7.0))
|
|
ax.plot(
|
|
ideal_m[:, 0],
|
|
ideal_m[:, 1],
|
|
"--",
|
|
linewidth=2.2,
|
|
label="理想轨迹",
|
|
)
|
|
ax.plot(
|
|
actual_x_m,
|
|
actual_y_m,
|
|
linewidth=1.8,
|
|
label="Detour实际轨迹（滤波后）",
|
|
)
|
|
if reference["kind"] == "in_place_rotation":
|
|
ax.scatter(
|
|
[ideal_m[0, 0]],
|
|
[ideal_m[0, 1]],
|
|
marker="*",
|
|
s=100,
|
|
label="理想旋转中心",
|
|
zorder=5,
|
|
)
|
|
else:
|
|
ax.scatter(
|
|
[ideal_m[0, 0]],
|
|
[ideal_m[0, 1]],
|
|
marker="o",
|
|
s=55,
|
|
label="起点",
|
|
zorder=5,
|
|
)
|
|
ax.scatter(
|
|
[ideal_m[-1, 0]],
|
|
[ideal_m[-1, 1]],
|
|
marker="x",
|
|
s=65,
|
|
label="终点",
|
|
zorder=5,
|
|
)
|
|
for event_index, event in enumerate(
|
|
metadata["localization_jump_events"]
|
|
):
|
|
before = np.array([
|
|
event["before_x_mm"],
|
|
event["before_y_mm"],
|
|
]) / 1000.0
|
|
after = np.array([
|
|
event["after_x_mm"],
|
|
event["after_y_mm"],
|
|
]) / 1000.0
|
|
ax.scatter(
|
|
[before[0], after[0]],
|
|
[before[1], after[1]],
|
|
marker="x",
|
|
color="tab:red",
|
|
s=55,
|
|
zorder=6,
|
|
label="Detour定位跳变前/后"
|
|
if event_index == 0 else None,
|
|
)
|
|
ax.annotate(
|
|
f"定位跳变 {event['distance_mm']:.1f} mm\n"
|
|
f"t={event['time_seconds']:.2f} s",
|
|
xy=(after[0], after[1]),
|
|
xytext=(8, 8),
|
|
textcoords="offset points",
|
|
color="tab:red",
|
|
fontsize=9,
|
|
)
|
|
ax.set_aspect("equal", adjustable="box")
|
|
ax.set_xlabel("世界坐标 X / m")
|
|
ax.set_ylabel("世界坐标 Y / m")
|
|
ax.set_title(
|
|
f"理想轨迹与实际轨迹对比\n"
|
|
f"{metadata['controller_name']} - "
|
|
f"{metadata['trajectory_name']}"
|
|
)
|
|
ax.grid(True, alpha=0.3)
|
|
ax.legend()
|
|
fig.tight_layout()
|
|
|
|
destination = output_path(
|
|
csv_path,
|
|
output_directory,
|
|
"trajectory_comparison",
|
|
)
|
|
fig.savefig(destination, dpi=300, bbox_inches="tight")
|
|
if show:
|
|
plt.show()
|
|
plt.close(fig)
|
|
print(
|
|
f"{csv_path.name}: 原始采样{metadata['raw_sample_count']}帧，"
|
|
f"有效Detour更新{metadata['detour_update_count']}帧，"
|
|
f"保持重复{metadata['held_sample_count']}帧，"
|
|
f"定位跳变{len(metadata['localization_jump_events'])}次"
|
|
)
|
|
return destination
|
|
|
|
|
|
def main() -> None:
|
|
configure_matplotlib()
|
|
parser = argparse.ArgumentParser(
|
|
description="绘制理想轨迹与Detour实际轨迹对比图。"
|
|
)
|
|
parser.add_argument("files", nargs="*", help="一个或多个CSV文件")
|
|
parser.add_argument("--frequency", type=float, default=20.0)
|
|
parser.add_argument("--window", type=float, default=0.55)
|
|
parser.add_argument("--output-dir")
|
|
parser.add_argument("--show", action="store_true")
|
|
args = parser.parse_args()
|
|
|
|
for csv_path in discover_csv_files(args.files):
|
|
destination = plot_trajectory(
|
|
csv_path,
|
|
args.frequency,
|
|
args.window,
|
|
args.output_dir,
|
|
args.show,
|
|
)
|
|
print(f"已生成：{destination}")
|
|
|
|
|
|
if __name__ == "__main__":
|
|
main()
|