1015 lines
33 KiB
Python
1015 lines
33 KiB
Python
"""为新版控制器实验CSV生成包含轨迹、误差、速度和转角的六子图总图。"""
|
||
|
||
from __future__ import annotations
|
||
|
||
import argparse
|
||
from pathlib import Path
|
||
|
||
import matplotlib
|
||
|
||
matplotlib.use("Agg")
|
||
import matplotlib.pyplot as plt
|
||
import numpy as np
|
||
import pandas as pd
|
||
|
||
|
||
SCRIPT_DIR = Path(__file__).resolve().parent
|
||
LATERAL_JUMP_THRESHOLD_METERS = 0.03
|
||
JUMP_INSET_CONTEXT_SAMPLES = 6
|
||
MAXIMUM_PLAUSIBLE_LINEAR_SPEED_METERS_PER_SECOND = 1.20
|
||
POSITION_JUMP_MARGIN_METERS = 0.03
|
||
|
||
|
||
def configure_matplotlib() -> None:
|
||
"""配置可显示中文和负号的Matplotlib字体。"""
|
||
plt.rcParams["font.sans-serif"] = [
|
||
"Microsoft YaHei",
|
||
"SimHei",
|
||
"Noto Sans CJK SC",
|
||
"Arial Unicode MS",
|
||
"DejaVu Sans",
|
||
]
|
||
plt.rcParams["axes.unicode_minus"] = False
|
||
|
||
|
||
def numeric_column(
|
||
frame: pd.DataFrame,
|
||
name: str,
|
||
default: float = np.nan,
|
||
) -> np.ndarray:
|
||
"""将CSV列安全转换为浮点数组，缺失列使用指定默认值。"""
|
||
if name not in frame.columns:
|
||
return np.full(len(frame), default, dtype=float)
|
||
return pd.to_numeric(frame[name], errors="coerce").to_numpy(
|
||
dtype=float,
|
||
copy=True,
|
||
)
|
||
|
||
|
||
def first_finite(values: np.ndarray, default: float) -> float:
|
||
"""读取数组中的第一个有限值。"""
|
||
finite = values[np.isfinite(values)]
|
||
return float(finite[0]) if finite.size else default
|
||
|
||
|
||
def first_text(frame: pd.DataFrame, name: str, default: str) -> str:
|
||
"""读取文本元数据列中的第一个非空值。"""
|
||
if name not in frame.columns:
|
||
return default
|
||
values = frame[name].dropna().astype(str)
|
||
values = values[values.str.strip() != ""]
|
||
return values.iloc[0] if not values.empty else default
|
||
|
||
|
||
def text_column(frame: pd.DataFrame, name: str) -> np.ndarray:
|
||
"""读取用于诊断标注的原始文本列，缺失值转换为空字符串。"""
|
||
if name not in frame.columns:
|
||
return np.full(len(frame), "", dtype=object)
|
||
return frame[name].fillna("").astype(str).to_numpy(
|
||
dtype=object,
|
||
copy=True,
|
||
)
|
||
|
||
|
||
def fill_reference_series(
|
||
values: np.ndarray,
|
||
fallback: np.ndarray,
|
||
) -> np.ndarray:
|
||
"""前后填充后台采样得到的控制参考值，缺失时使用解析速度曲线。"""
|
||
series = pd.Series(values, dtype=float)
|
||
filled = series.ffill().bfill().to_numpy(
|
||
dtype=float,
|
||
copy=True,
|
||
)
|
||
missing = ~np.isfinite(filled)
|
||
filled[missing] = fallback[missing]
|
||
return filled
|
||
|
||
|
||
def planned_motion(
|
||
time_seconds: np.ndarray,
|
||
length_meters: float,
|
||
cruise_speed_mps: float,
|
||
acceleration_mps2: float,
|
||
deceleration_mps2: float,
|
||
) -> tuple[np.ndarray, np.ndarray]:
|
||
"""计算从静止出发并在终点静止的梯形或三角形理想时间速度轨迹。"""
|
||
acceleration_distance = (
|
||
cruise_speed_mps**2 / (2.0 * acceleration_mps2)
|
||
)
|
||
deceleration_distance = (
|
||
cruise_speed_mps**2 / (2.0 * deceleration_mps2)
|
||
)
|
||
|
||
if acceleration_distance + deceleration_distance <= length_meters:
|
||
peak_speed = cruise_speed_mps
|
||
else:
|
||
peak_speed = np.sqrt(
|
||
2.0
|
||
* length_meters
|
||
/ (1.0 / acceleration_mps2 + 1.0 / deceleration_mps2)
|
||
)
|
||
acceleration_distance = (
|
||
peak_speed**2 / (2.0 * acceleration_mps2)
|
||
)
|
||
deceleration_distance = (
|
||
peak_speed**2 / (2.0 * deceleration_mps2)
|
||
)
|
||
|
||
acceleration_time = peak_speed / acceleration_mps2
|
||
deceleration_time = peak_speed / deceleration_mps2
|
||
cruise_distance = max(
|
||
0.0,
|
||
length_meters - acceleration_distance - deceleration_distance,
|
||
)
|
||
cruise_time = cruise_distance / peak_speed
|
||
deceleration_start_time = acceleration_time + cruise_time
|
||
finish_time = deceleration_start_time + deceleration_time
|
||
|
||
progress = np.zeros_like(time_seconds, dtype=float)
|
||
speed = np.zeros_like(time_seconds, dtype=float)
|
||
|
||
accelerating = time_seconds <= acceleration_time
|
||
progress[accelerating] = (
|
||
0.5 * acceleration_mps2 * time_seconds[accelerating] ** 2
|
||
)
|
||
speed[accelerating] = acceleration_mps2 * time_seconds[accelerating]
|
||
|
||
cruising = (
|
||
(time_seconds > acceleration_time)
|
||
& (time_seconds <= deceleration_start_time)
|
||
)
|
||
progress[cruising] = (
|
||
acceleration_distance
|
||
+ peak_speed * (time_seconds[cruising] - acceleration_time)
|
||
)
|
||
speed[cruising] = peak_speed
|
||
|
||
decelerating = (
|
||
(time_seconds > deceleration_start_time)
|
||
& (time_seconds <= finish_time)
|
||
)
|
||
remaining_time = finish_time - time_seconds[decelerating]
|
||
progress[decelerating] = (
|
||
length_meters
|
||
- 0.5 * deceleration_mps2 * remaining_time**2
|
||
)
|
||
speed[decelerating] = deceleration_mps2 * remaining_time
|
||
|
||
finished = time_seconds > finish_time
|
||
progress[finished] = length_meters
|
||
speed[finished] = 0.0
|
||
return progress, speed
|
||
|
||
|
||
def load_experiment(csv_path: Path) -> dict[str, object]:
|
||
"""读取新版CSV并构造绘图所需的统一SI单位数据。"""
|
||
frame = pd.read_csv(csv_path, encoding="utf-8-sig")
|
||
if frame.empty:
|
||
raise ValueError("CSV没有任何采样行。")
|
||
|
||
time_seconds = numeric_column(frame, "ElapsedSeconds")
|
||
valid_time = np.isfinite(time_seconds)
|
||
frame = frame.loc[valid_time].reset_index(drop=True)
|
||
time_seconds = time_seconds[valid_time]
|
||
if time_seconds.size < 2:
|
||
raise ValueError("CSV中的有效时间采样不足2帧。")
|
||
time_seconds = time_seconds - time_seconds[0]
|
||
|
||
state_x = numeric_column(frame, "StateXMeters")
|
||
state_y = numeric_column(frame, "StateYMeters")
|
||
has_processed = numeric_column(frame, "HasProcessedState", 0.0) > 0.5
|
||
processed_valid = has_processed & np.isfinite(state_x) & np.isfinite(state_y)
|
||
|
||
raw_x_meters = numeric_column(frame, "DetourX") / 1000.0
|
||
raw_y_meters = numeric_column(frame, "DetourY") / 1000.0
|
||
valid_raw_position = (
|
||
np.isfinite(raw_x_meters) & np.isfinite(raw_y_meters)
|
||
)
|
||
detour_tick_raw = text_column(frame, "DetourTickRaw")
|
||
detour_l_step = numeric_column(frame, "DetourLStep")
|
||
actual_x = np.where(processed_valid, state_x, raw_x_meters)
|
||
actual_y = np.where(processed_valid, state_y, raw_y_meters)
|
||
valid_position = np.isfinite(actual_x) & np.isfinite(actual_y)
|
||
if np.count_nonzero(valid_position) < 2:
|
||
raise ValueError("CSV中没有足够的有效车辆位置。")
|
||
|
||
start = np.array(
|
||
[
|
||
first_finite(numeric_column(frame, "ReferenceStartX"), np.nan),
|
||
first_finite(numeric_column(frame, "ReferenceStartY"), np.nan),
|
||
],
|
||
dtype=float,
|
||
) / 1000.0
|
||
end = np.array(
|
||
[
|
||
first_finite(numeric_column(frame, "ReferenceEndX"), np.nan),
|
||
first_finite(numeric_column(frame, "ReferenceEndY"), np.nan),
|
||
],
|
||
dtype=float,
|
||
) / 1000.0
|
||
if not np.all(np.isfinite(start)) or not np.all(np.isfinite(end)):
|
||
raise ValueError("CSV缺少有效的参考起点或终点。")
|
||
|
||
line = end - start
|
||
length_meters = float(np.linalg.norm(line))
|
||
if length_meters <= 1e-6:
|
||
raise ValueError("参考直线长度必须大于0。")
|
||
tangent = line / length_meters
|
||
left_normal = np.array([-tangent[1], tangent[0]])
|
||
displacement = np.column_stack([actual_x, actual_y]) - start
|
||
# 与C# TrajectoryProjector保持一致：轨迹位于车辆左侧时为正。
|
||
derived_lateral_error = -(displacement @ left_normal)
|
||
recorded_lateral_error = numeric_column(
|
||
frame,
|
||
"ControlLateralErrorMeters",
|
||
)
|
||
has_control_reference = (
|
||
numeric_column(frame, "HasControlReference", 0.0) > 0.5
|
||
)
|
||
recorded_lateral_valid = (
|
||
has_control_reference & np.isfinite(recorded_lateral_error)
|
||
)
|
||
lateral_error = (
|
||
np.where(recorded_lateral_valid, recorded_lateral_error, np.nan)
|
||
if np.any(recorded_lateral_valid)
|
||
else derived_lateral_error
|
||
)
|
||
|
||
state_yaw = numeric_column(frame, "StateYawRadians")
|
||
raw_yaw = np.deg2rad(numeric_column(frame, "DetourTheta"))
|
||
actual_yaw = np.where(
|
||
has_processed & np.isfinite(state_yaw),
|
||
state_yaw,
|
||
raw_yaw,
|
||
)
|
||
reference_yaw = np.arctan2(tangent[1], tangent[0])
|
||
derived_heading_error = np.arctan2(
|
||
np.sin(reference_yaw - actual_yaw),
|
||
np.cos(reference_yaw - actual_yaw),
|
||
)
|
||
recorded_heading_error = numeric_column(
|
||
frame,
|
||
"ControlHeadingErrorRadians",
|
||
)
|
||
recorded_heading_valid = (
|
||
has_control_reference & np.isfinite(recorded_heading_error)
|
||
)
|
||
heading_error = (
|
||
np.where(recorded_heading_valid, recorded_heading_error, np.nan)
|
||
if np.any(recorded_heading_valid)
|
||
else derived_heading_error
|
||
)
|
||
|
||
# 投影定义满足：参考点 = 车体位置 + 横向误差 × 参考航向左法向。
|
||
# 因此无需假设轨迹类型，即可从有效控制周期还原车辆实际使用的参考轨迹。
|
||
projected_reference_yaw = actual_yaw + heading_error
|
||
reference_x = (
|
||
actual_x - lateral_error * np.sin(projected_reference_yaw)
|
||
)
|
||
reference_y = (
|
||
actual_y + lateral_error * np.cos(projected_reference_yaw)
|
||
)
|
||
valid_reference_position = (
|
||
has_control_reference
|
||
& np.isfinite(reference_x)
|
||
& np.isfinite(reference_y)
|
||
)
|
||
|
||
# 同时检查Detour是否超出车辆物理运动边界，以及控制状态的横向误差
|
||
# 是否发生离散突变。后者能覆盖状态层延迟接受持续定位偏移的情况。
|
||
raw_delta_x = np.full(len(frame), np.nan, dtype=float)
|
||
raw_delta_y = np.full(len(frame), np.nan, dtype=float)
|
||
sample_delta_time = np.full(len(frame), np.nan, dtype=float)
|
||
raw_delta_x[1:] = np.diff(raw_x_meters)
|
||
raw_delta_y[1:] = np.diff(raw_y_meters)
|
||
sample_delta_time[1:] = np.diff(time_seconds)
|
||
detour_position_step = np.hypot(raw_delta_x, raw_delta_y)
|
||
|
||
detour_tick_numeric = numeric_column(frame, "DetourTickRaw")
|
||
detour_tick_delta_seconds = np.full(len(frame), np.nan, dtype=float)
|
||
detour_tick_delta_seconds[1:] = (
|
||
np.diff(detour_tick_numeric) / 10_000_000.0
|
||
)
|
||
source_delta_time = sample_delta_time.copy()
|
||
valid_tick_delta = (
|
||
np.isfinite(detour_tick_delta_seconds)
|
||
& (detour_tick_delta_seconds > 0.0)
|
||
& (detour_tick_delta_seconds <= 0.5)
|
||
)
|
||
source_delta_time[valid_tick_delta] = (
|
||
detour_tick_delta_seconds[valid_tick_delta]
|
||
)
|
||
|
||
consecutive_raw_position_valid = np.zeros(len(frame), dtype=bool)
|
||
consecutive_raw_position_valid[1:] = (
|
||
valid_raw_position[1:] & valid_raw_position[:-1]
|
||
)
|
||
maximum_plausible_position_step = (
|
||
MAXIMUM_PLAUSIBLE_LINEAR_SPEED_METERS_PER_SECOND
|
||
* source_delta_time
|
||
+ POSITION_JUMP_MARGIN_METERS
|
||
)
|
||
raw_detour_jump = (
|
||
consecutive_raw_position_valid
|
||
& np.isfinite(detour_position_step)
|
||
& np.isfinite(source_delta_time)
|
||
& (source_delta_time > 0.0)
|
||
& (source_delta_time <= 0.5)
|
||
& (detour_position_step > maximum_plausible_position_step)
|
||
)
|
||
|
||
state_lateral_step = np.full(len(frame), np.nan, dtype=float)
|
||
state_lateral_step[1:] = np.diff(lateral_error)
|
||
state_lateral_jump = (
|
||
np.isfinite(state_lateral_step)
|
||
& np.isfinite(sample_delta_time)
|
||
& (sample_delta_time > 0.0)
|
||
& (sample_delta_time <= 0.5)
|
||
& (np.abs(state_lateral_step) >= LATERAL_JUMP_THRESHOLD_METERS)
|
||
)
|
||
suspected_jump = raw_detour_jump | state_lateral_jump
|
||
jump_indices = np.flatnonzero(suspected_jump)
|
||
jump_magnitude = np.zeros(len(frame), dtype=float)
|
||
jump_magnitude[raw_detour_jump] = detour_position_step[raw_detour_jump]
|
||
jump_magnitude[state_lateral_jump] = np.maximum(
|
||
jump_magnitude[state_lateral_jump],
|
||
np.abs(state_lateral_step[state_lateral_jump]),
|
||
)
|
||
|
||
cruise_speed = first_finite(
|
||
numeric_column(frame, "ReferenceSpeed"),
|
||
0.30,
|
||
)
|
||
acceleration = first_finite(
|
||
numeric_column(
|
||
frame,
|
||
"ReferenceAccelerationMetersPerSecondSquared",
|
||
),
|
||
0.20,
|
||
)
|
||
deceleration = first_finite(
|
||
numeric_column(
|
||
frame,
|
||
"ReferenceDecelerationMetersPerSecondSquared",
|
||
),
|
||
0.20,
|
||
)
|
||
if acceleration <= 0.0:
|
||
acceleration = 0.20
|
||
if deceleration <= 0.0:
|
||
deceleration = 0.20
|
||
|
||
_, ideal_speed = planned_motion(
|
||
time_seconds,
|
||
length_meters,
|
||
cruise_speed,
|
||
acceleration,
|
||
deceleration,
|
||
)
|
||
reference_speed = fill_reference_series(
|
||
numeric_column(frame, "ControlReferenceSpeedMetersPerSecond"),
|
||
ideal_speed,
|
||
)
|
||
if np.any(has_control_reference):
|
||
reference_speed[~has_control_reference] = np.nan
|
||
motion_frame_yaw_radians = np.deg2rad(
|
||
numeric_column(frame, "ReferenceMotionFrameYawDegrees", 0.0)
|
||
)
|
||
motion_direction_cosine = np.cos(motion_frame_yaw_radians)
|
||
motion_direction_sine = np.sin(motion_frame_yaw_radians)
|
||
state_body_vx = numeric_column(frame, "StateBodyVxMetersPerSecond")
|
||
state_body_vy = numeric_column(frame, "StateBodyVyMetersPerSecond")
|
||
velocity_valid = (
|
||
numeric_column(frame, "StateVelocityEstimateValid", 0.0) > 0.5
|
||
)
|
||
state_body_vx[~velocity_valid] = np.nan
|
||
state_body_vy[~velocity_valid] = np.nan
|
||
state_motion_speed = (
|
||
state_body_vx * motion_direction_cosine
|
||
+ state_body_vy * motion_direction_sine
|
||
)
|
||
has_velocity_diagnostics = (
|
||
numeric_column(frame, "HasVelocityDiagnostics", 0.0) > 0.5
|
||
)
|
||
detour_speed = numeric_column(
|
||
frame,
|
||
"DetourEstimatedBodyVxMetersPerSecond",
|
||
)
|
||
detour_speed_valid = (
|
||
has_velocity_diagnostics
|
||
& (
|
||
numeric_column(
|
||
frame,
|
||
"DetourVelocityEstimateValid",
|
||
0.0,
|
||
)
|
||
> 0.5
|
||
)
|
||
)
|
||
detour_speed[~detour_speed_valid] = np.nan
|
||
wheel_raw_body_vx = numeric_column(
|
||
frame,
|
||
"WheelFeedbackRawBodyVxMetersPerSecond",
|
||
)
|
||
wheel_filtered_body_vx = numeric_column(
|
||
frame,
|
||
"WheelFeedbackFilteredBodyVxMetersPerSecond",
|
||
)
|
||
wheel_raw_body_vy = numeric_column(
|
||
frame,
|
||
"WheelFeedbackRawBodyVyMetersPerSecond",
|
||
)
|
||
wheel_filtered_body_vy = numeric_column(
|
||
frame,
|
||
"WheelFeedbackFilteredBodyVyMetersPerSecond",
|
||
)
|
||
wheel_speed_valid = (
|
||
has_velocity_diagnostics
|
||
& (
|
||
numeric_column(
|
||
frame,
|
||
"WheelFeedbackVelocityEstimateValid",
|
||
0.0,
|
||
)
|
||
> 0.5
|
||
)
|
||
)
|
||
wheel_raw_speed = (
|
||
wheel_raw_body_vx * motion_direction_cosine
|
||
+ wheel_raw_body_vy * motion_direction_sine
|
||
)
|
||
wheel_filtered_speed = (
|
||
wheel_filtered_body_vx * motion_direction_cosine
|
||
+ wheel_filtered_body_vy * motion_direction_sine
|
||
)
|
||
|
||
# 兼容尚未记录轮速Vy的旧版β=0实验；非零β缺少Vy时不能伪造投影速度。
|
||
body_x_motion = np.abs(motion_direction_sine) <= 1e-12
|
||
missing_raw_projection = ~np.isfinite(wheel_raw_speed)
|
||
missing_filtered_projection = ~np.isfinite(wheel_filtered_speed)
|
||
wheel_raw_speed[body_x_motion & missing_raw_projection] = (
|
||
wheel_raw_body_vx[body_x_motion & missing_raw_projection]
|
||
)
|
||
wheel_filtered_speed[
|
||
body_x_motion & missing_filtered_projection
|
||
] = wheel_filtered_body_vx[
|
||
body_x_motion & missing_filtered_projection
|
||
]
|
||
wheel_raw_speed[~wheel_speed_valid] = np.nan
|
||
wheel_filtered_speed[~wheel_speed_valid] = np.nan
|
||
actual_speed = np.where(
|
||
np.isfinite(wheel_filtered_speed),
|
||
wheel_filtered_speed,
|
||
state_motion_speed,
|
||
)
|
||
command_speed = numeric_column(frame, "CommandSpeed")
|
||
|
||
has_steering_diagnostics = (
|
||
numeric_column(frame, "HasSteeringDiagnostics", 0.0) > 0.5
|
||
)
|
||
steering_angles_degrees = {}
|
||
for wheel_name in (
|
||
"LeftFront",
|
||
"LeftRear",
|
||
"RightFront",
|
||
"RightRear",
|
||
):
|
||
values = numeric_column(
|
||
frame,
|
||
f"ActualSteer{wheel_name}Degrees",
|
||
)
|
||
values[~has_steering_diagnostics] = np.nan
|
||
steering_angles_degrees[wheel_name] = values
|
||
|
||
has_gcp_command = (
|
||
numeric_column(frame, "HasGcpCommand", 0.0) > 0.5
|
||
)
|
||
front_gcp_degrees = np.rad2deg(
|
||
numeric_column(frame, "CommandFrontGcpAngleRadians")
|
||
)
|
||
rear_gcp_degrees = np.rad2deg(
|
||
numeric_column(frame, "CommandRearGcpAngleRadians")
|
||
)
|
||
front_gcp_degrees[~has_gcp_command] = np.nan
|
||
rear_gcp_degrees[~has_gcp_command] = np.nan
|
||
|
||
return {
|
||
"frame": frame,
|
||
"time": time_seconds,
|
||
"actual_x": actual_x,
|
||
"actual_y": actual_y,
|
||
"valid_position": valid_position,
|
||
"raw_x": raw_x_meters,
|
||
"raw_y": raw_y_meters,
|
||
"valid_raw_position": valid_raw_position,
|
||
"detour_tick_raw": detour_tick_raw,
|
||
"detour_l_step": detour_l_step,
|
||
"detour_position_step": detour_position_step,
|
||
"state_lateral_step": state_lateral_step,
|
||
"raw_detour_jump": raw_detour_jump,
|
||
"state_lateral_jump": state_lateral_jump,
|
||
"jump_magnitude": jump_magnitude,
|
||
"jump_indices": jump_indices,
|
||
"reference_x": reference_x,
|
||
"reference_y": reference_y,
|
||
"valid_reference_position": valid_reference_position,
|
||
"start": start,
|
||
"end": end,
|
||
"length": length_meters,
|
||
"lateral_error": lateral_error,
|
||
"heading_error": heading_error,
|
||
"reference_speed": reference_speed,
|
||
"actual_speed": actual_speed,
|
||
"detour_speed": detour_speed,
|
||
"wheel_raw_speed": wheel_raw_speed,
|
||
"wheel_filtered_speed": wheel_filtered_speed,
|
||
"command_speed": command_speed,
|
||
"steering_angles_degrees": steering_angles_degrees,
|
||
"front_gcp_degrees": front_gcp_degrees,
|
||
"rear_gcp_degrees": rear_gcp_degrees,
|
||
"controller_name": first_text(
|
||
frame,
|
||
"ControllerName",
|
||
"NewController",
|
||
),
|
||
"trajectory_name": first_text(
|
||
frame,
|
||
"TrajectoryName",
|
||
"Trajectory",
|
||
),
|
||
}
|
||
|
||
|
||
def finite_rmse(values: np.ndarray) -> float:
|
||
"""计算忽略无效样本后的均方根值。"""
|
||
finite = values[np.isfinite(values)]
|
||
return float(np.sqrt(np.mean(finite**2))) if finite.size else np.nan
|
||
|
||
|
||
def save_figure(
|
||
fig: plt.Figure,
|
||
destination: Path,
|
||
show: bool,
|
||
) -> None:
|
||
"""保存并关闭一张实验图。"""
|
||
fig.tight_layout(rect=(0.0, 0.0, 1.0, 0.97))
|
||
fig.savefig(destination, dpi=300, bbox_inches="tight")
|
||
if show:
|
||
plt.show()
|
||
plt.close(fig)
|
||
|
||
|
||
def plot_experiment(
|
||
csv_path: Path,
|
||
output_directory: Path,
|
||
show: bool,
|
||
) -> list[Path]:
|
||
"""为单份新版控制器CSV生成一张包含六个子图的实验总图。"""
|
||
data = load_experiment(csv_path)
|
||
output_directory.mkdir(parents=True, exist_ok=True)
|
||
title = f"{data['controller_name']} - {data['trajectory_name']}"
|
||
fig, axes = plt.subplots(3, 2, figsize=(18.0, 16.0))
|
||
fig.suptitle(title, fontsize=16)
|
||
|
||
# 1. 期望轨迹与实际轨迹。
|
||
axis = axes[0, 0]
|
||
valid_position = data["valid_position"]
|
||
valid_raw_position = data["valid_raw_position"]
|
||
valid_reference_position = data["valid_reference_position"]
|
||
jump_indices = data["jump_indices"]
|
||
if np.count_nonzero(valid_reference_position) >= 2:
|
||
axis.plot(
|
||
data["reference_x"][valid_reference_position],
|
||
data["reference_y"][valid_reference_position],
|
||
"--",
|
||
linewidth=2.0,
|
||
label="控制器实际使用的参考轨迹",
|
||
)
|
||
else:
|
||
axis.plot(
|
||
[data["start"][0], data["end"][0]],
|
||
[data["start"][1], data["end"][1]],
|
||
"--",
|
||
linewidth=2.0,
|
||
label="参考起终点连线",
|
||
)
|
||
axis.plot(
|
||
data["raw_x"][valid_raw_position],
|
||
data["raw_y"][valid_raw_position],
|
||
":",
|
||
color="tab:gray",
|
||
linewidth=1.2,
|
||
alpha=0.85,
|
||
label="Detour原始轨迹",
|
||
)
|
||
axis.plot(
|
||
data["actual_x"][valid_position],
|
||
data["actual_y"][valid_position],
|
||
color="tab:orange",
|
||
linewidth=1.5,
|
||
label="控制使用的状态轨迹",
|
||
)
|
||
if jump_indices.size:
|
||
axis.scatter(
|
||
data["raw_x"][jump_indices],
|
||
data["raw_y"][jump_indices],
|
||
color="red",
|
||
marker="x",
|
||
s=65,
|
||
linewidths=1.8,
|
||
zorder=8,
|
||
label="疑似定位/状态突变",
|
||
)
|
||
axis.scatter(*data["start"], color="green", s=45, label="起点")
|
||
axis.scatter(*data["end"], color="red", s=45, label="终点")
|
||
# 诊断图优先展示厘米级横向变化；横纵轴独立缩放，避免4m行程
|
||
# 将数厘米的定位阶跃压缩成几乎不可见的一条细线。
|
||
axis.set_aspect("auto")
|
||
axis.set_xlabel("世界坐标X / m")
|
||
axis.set_ylabel("世界坐标Y / m")
|
||
axis.set_title("期望轨迹与状态轨迹对比（横纵轴独立缩放）")
|
||
axis.grid(True, alpha=0.3)
|
||
axis.legend(fontsize=7, loc="upper left")
|
||
|
||
if jump_indices.size:
|
||
strongest_jump_index = int(
|
||
jump_indices[
|
||
np.argmax(
|
||
np.abs(
|
||
data["jump_magnitude"][jump_indices]
|
||
)
|
||
)
|
||
]
|
||
)
|
||
context_start = max(
|
||
0,
|
||
strongest_jump_index - JUMP_INSET_CONTEXT_SAMPLES,
|
||
)
|
||
context_end = min(
|
||
len(data["time"]),
|
||
strongest_jump_index + JUMP_INSET_CONTEXT_SAMPLES + 1,
|
||
)
|
||
context = np.arange(context_start, context_end)
|
||
inset = axis.inset_axes([0.54, 0.08, 0.43, 0.43])
|
||
inset.set_zorder(10)
|
||
inset.set_facecolor("white")
|
||
context_reference_valid = (
|
||
data["valid_reference_position"][context]
|
||
)
|
||
if np.count_nonzero(context_reference_valid) >= 2:
|
||
reference_context = context[context_reference_valid]
|
||
inset.plot(
|
||
data["reference_x"][reference_context],
|
||
data["reference_y"][reference_context],
|
||
"--",
|
||
linewidth=1.2,
|
||
color="tab:blue",
|
||
)
|
||
context_raw_valid = data["valid_raw_position"][context]
|
||
raw_context = context[context_raw_valid]
|
||
inset.plot(
|
||
data["raw_x"][raw_context],
|
||
data["raw_y"][raw_context],
|
||
":",
|
||
linewidth=1.0,
|
||
color="tab:gray",
|
||
)
|
||
context_state_valid = data["valid_position"][context]
|
||
state_context = context[context_state_valid]
|
||
inset.plot(
|
||
data["actual_x"][state_context],
|
||
data["actual_y"][state_context],
|
||
linewidth=1.2,
|
||
color="tab:orange",
|
||
)
|
||
inset.scatter(
|
||
data["raw_x"][strongest_jump_index],
|
||
data["raw_y"][strongest_jump_index],
|
||
color="red",
|
||
marker="x",
|
||
s=45,
|
||
linewidths=1.5,
|
||
zorder=8,
|
||
)
|
||
jump_descriptions = []
|
||
if data["raw_detour_jump"][strongest_jump_index]:
|
||
jump_descriptions.append(
|
||
"Detour位移="
|
||
f"{data['detour_position_step'][strongest_jump_index] * 1000.0:.1f}mm"
|
||
)
|
||
if data["state_lateral_jump"][strongest_jump_index]:
|
||
jump_descriptions.append(
|
||
"状态横向Δ="
|
||
f"{data['state_lateral_step'][strongest_jump_index] * 1000.0:+.1f}mm"
|
||
)
|
||
diagnostic_parts = []
|
||
detour_tick = data["detour_tick_raw"][strongest_jump_index]
|
||
if detour_tick:
|
||
diagnostic_parts.append(f"tick={detour_tick}")
|
||
detour_l_step = data["detour_l_step"][strongest_jump_index]
|
||
if np.isfinite(detour_l_step):
|
||
diagnostic_parts.append(f"l_step={detour_l_step:g}")
|
||
diagnostic_suffix = (
|
||
"\n" + " ".join(diagnostic_parts)
|
||
if diagnostic_parts
|
||
else ""
|
||
)
|
||
inset.set_title(
|
||
f"最大疑似突变：t={data['time'][strongest_jump_index]:.3f}s\n"
|
||
f"{'，'.join(jump_descriptions)}"
|
||
f"{diagnostic_suffix}",
|
||
fontsize=7,
|
||
)
|
||
inset.set_aspect("auto")
|
||
inset.tick_params(labelsize=6)
|
||
inset.grid(True, alpha=0.25)
|
||
|
||
# 2. 横向误差。
|
||
lateral_mm = data["lateral_error"] * 1000.0
|
||
lateral_rmse_mm = finite_rmse(lateral_mm)
|
||
axis = axes[0, 1]
|
||
axis.plot(data["time"], lateral_mm, linewidth=1.5)
|
||
if jump_indices.size:
|
||
for jump_index in jump_indices:
|
||
axis.axvline(
|
||
data["time"][jump_index],
|
||
color="red",
|
||
linewidth=0.8,
|
||
alpha=0.35,
|
||
)
|
||
valid_jump_error = (
|
||
data["state_lateral_jump"][jump_indices]
|
||
& np.isfinite(lateral_mm[jump_indices])
|
||
)
|
||
visible_jump_indices = jump_indices[valid_jump_error]
|
||
if visible_jump_indices.size:
|
||
axis.scatter(
|
||
data["time"][visible_jump_indices],
|
||
lateral_mm[visible_jump_indices],
|
||
color="red",
|
||
marker="x",
|
||
s=45,
|
||
linewidths=1.5,
|
||
zorder=7,
|
||
label="控制状态横向突变",
|
||
)
|
||
axis.axhline(0.0, color="black", linewidth=0.8)
|
||
axis.set_xlabel("时间 / s")
|
||
axis.set_ylabel("横向误差 / mm")
|
||
axis.set_title(
|
||
"横向误差（轨迹在车辆左侧为正）\n"
|
||
f"RMSE={lateral_rmse_mm:.2f}mm"
|
||
)
|
||
axis.grid(True, alpha=0.3)
|
||
if jump_indices.size and np.any(
|
||
data["state_lateral_jump"][jump_indices]
|
||
& np.isfinite(lateral_mm[jump_indices])
|
||
):
|
||
axis.legend(fontsize=8)
|
||
|
||
# 3. 航向误差。
|
||
heading_degrees = np.rad2deg(data["heading_error"])
|
||
heading_rmse_degrees = finite_rmse(heading_degrees)
|
||
axis = axes[1, 0]
|
||
axis.plot(data["time"], heading_degrees, linewidth=1.5)
|
||
axis.axhline(0.0, color="black", linewidth=0.8)
|
||
axis.set_xlabel("时间 / s")
|
||
axis.set_ylabel("航向角偏差 / °")
|
||
axis.set_title(
|
||
"航向角偏差：参考轨迹航向－实际车体航向（逆时针为正）\n"
|
||
f"RMSE={heading_rmse_degrees:.3f}°"
|
||
)
|
||
axis.grid(True, alpha=0.3)
|
||
|
||
# 4. 参考、命令、Detour车头分量和沿β投影的轮速解算速度。
|
||
speed_error = data["actual_speed"] - data["reference_speed"]
|
||
speed_rmse = finite_rmse(speed_error)
|
||
axis = axes[1, 1]
|
||
axis.plot(
|
||
data["time"],
|
||
data["reference_speed"],
|
||
linewidth=1.8,
|
||
label="控制器实际参考速度",
|
||
)
|
||
axis.plot(
|
||
data["time"],
|
||
data["command_speed"],
|
||
"--",
|
||
linewidth=1.3,
|
||
label="纵向控制器下发速度",
|
||
)
|
||
if np.any(np.isfinite(data["detour_speed"])):
|
||
axis.plot(
|
||
data["time"],
|
||
data["detour_speed"],
|
||
":",
|
||
linewidth=1.2,
|
||
label="Detour估计Vx（车头分量）",
|
||
)
|
||
if np.any(np.isfinite(data["wheel_filtered_speed"])):
|
||
wheel_filtered_valid = np.isfinite(
|
||
data["wheel_filtered_speed"]
|
||
)
|
||
axis.scatter(
|
||
data["time"][wheel_filtered_valid],
|
||
data["wheel_filtered_speed"][wheel_filtered_valid],
|
||
color="tab:red",
|
||
s=14,
|
||
marker="o",
|
||
zorder=5,
|
||
label="轮速解算β方向速度（控制使用）",
|
||
)
|
||
else:
|
||
axis.plot(
|
||
data["time"],
|
||
data["actual_speed"],
|
||
linewidth=1.5,
|
||
label="控制器实际纵向速度",
|
||
)
|
||
axis.set_xlabel("时间 / s")
|
||
axis.set_ylabel("速度 / (m/s)")
|
||
axis.set_title(
|
||
"参考速度、控制命令与观测速度\n"
|
||
f"轮速β方向速度相对参考速度RMSE={speed_rmse:.4f}m/s"
|
||
)
|
||
axis.grid(True, alpha=0.3)
|
||
axis.legend(fontsize=8)
|
||
|
||
# 5. 四个舵轮的实际机械转角。
|
||
axis = axes[2, 0]
|
||
wheel_labels = {
|
||
"LeftFront": "左前轮",
|
||
"LeftRear": "左后轮",
|
||
"RightFront": "右前轮",
|
||
"RightRear": "右后轮",
|
||
}
|
||
steering_data_available = False
|
||
for wheel_name, wheel_label in wheel_labels.items():
|
||
wheel_angles = data["steering_angles_degrees"][wheel_name]
|
||
if np.any(np.isfinite(wheel_angles)):
|
||
steering_data_available = True
|
||
axis.plot(
|
||
data["time"],
|
||
wheel_angles,
|
||
linewidth=1.2,
|
||
label=wheel_label,
|
||
)
|
||
if steering_data_available:
|
||
axis.axhline(0.0, color="black", linewidth=0.8)
|
||
axis.legend(fontsize=8, ncol=2)
|
||
else:
|
||
axis.text(
|
||
0.5,
|
||
0.5,
|
||
"CSV不含四舵轮转角诊断数据",
|
||
ha="center",
|
||
va="center",
|
||
transform=axis.transAxes,
|
||
)
|
||
axis.set_xlabel("时间 / s")
|
||
axis.set_ylabel("实际舵角 / °")
|
||
axis.set_title("四个舵轮实际反馈转角")
|
||
axis.grid(True, alpha=0.3)
|
||
|
||
# 6. 经过角速度限制后实际发送的前、后虚拟GCP转角。
|
||
axis = axes[2, 1]
|
||
gcp_data_available = (
|
||
np.any(np.isfinite(data["front_gcp_degrees"]))
|
||
or np.any(np.isfinite(data["rear_gcp_degrees"]))
|
||
)
|
||
if gcp_data_available:
|
||
axis.plot(
|
||
data["time"],
|
||
data["front_gcp_degrees"],
|
||
linewidth=1.4,
|
||
label="前GCP",
|
||
)
|
||
axis.plot(
|
||
data["time"],
|
||
data["rear_gcp_degrees"],
|
||
linewidth=1.4,
|
||
label="后GCP",
|
||
)
|
||
axis.axhline(0.0, color="black", linewidth=0.8)
|
||
axis.legend(fontsize=8)
|
||
else:
|
||
axis.text(
|
||
0.5,
|
||
0.5,
|
||
"CSV不含前后GCP转角数据",
|
||
ha="center",
|
||
va="center",
|
||
transform=axis.transAxes,
|
||
)
|
||
axis.set_xlabel("时间 / s")
|
||
axis.set_ylabel("GCP命令角 / °")
|
||
axis.set_title("前后虚拟GCP实际发送转角")
|
||
axis.grid(True, alpha=0.3)
|
||
|
||
destination = output_directory / f"{csv_path.stem}_summary_6plots.png"
|
||
save_figure(fig, destination, show)
|
||
|
||
print(
|
||
f"{csv_path.name}: 横向RMSE={lateral_rmse_mm:.3f}mm, "
|
||
f"航向RMSE={heading_rmse_degrees:.4f}°, "
|
||
f"速度RMSE={speed_rmse:.5f}m/s"
|
||
)
|
||
if jump_indices.size:
|
||
strongest_jump_index = int(
|
||
jump_indices[
|
||
np.argmax(
|
||
np.abs(
|
||
data["jump_magnitude"][jump_indices]
|
||
)
|
||
)
|
||
]
|
||
)
|
||
print(
|
||
f" 检出{jump_indices.size}个疑似定位/状态突变，"
|
||
f"最大幅值={data['jump_magnitude'][strongest_jump_index] * 1000.0:.2f}mm，"
|
||
f"时刻={data['time'][strongest_jump_index]:.3f}s"
|
||
)
|
||
print(f"已生成六子图总图：{destination}")
|
||
return [destination]
|
||
|
||
|
||
def discover_csv_files(arguments: list[str]) -> list[Path]:
|
||
"""读取命令行文件或目录；目录中只选取非计时CSV。"""
|
||
if arguments:
|
||
files = []
|
||
for item in arguments:
|
||
path = Path(item).expanduser().resolve()
|
||
if path.is_dir():
|
||
files.extend(
|
||
sorted(
|
||
candidate
|
||
for candidate in path.glob("*.csv")
|
||
if not candidate.stem.endswith("_timing")
|
||
)
|
||
)
|
||
else:
|
||
files.append(path)
|
||
else:
|
||
files = sorted(
|
||
path
|
||
for path in SCRIPT_DIR.glob("*.csv")
|
||
if not path.stem.endswith("_timing")
|
||
)
|
||
files.extend(
|
||
sorted(
|
||
path
|
||
for path in (SCRIPT_DIR / "data").glob("*.csv")
|
||
if not path.stem.endswith("_timing")
|
||
)
|
||
)
|
||
files = list(dict.fromkeys(path for path in files if path.is_file()))
|
||
if not files:
|
||
raise FileNotFoundError(
|
||
"没有找到轨迹CSV；请传入文件、目录，或将文件放到脚本目录/data中。"
|
||
)
|
||
return files
|
||
|
||
|
||
def main() -> None:
|
||
"""解析命令行并批量处理新版控制器实验CSV。"""
|
||
parser = argparse.ArgumentParser(
|
||
description="绘制新版控制器轨迹实验的六子图总图。"
|
||
)
|
||
parser.add_argument(
|
||
"csv",
|
||
nargs="*",
|
||
help="需要处理的轨迹CSV文件或包含轨迹CSV的目录。",
|
||
)
|
||
parser.add_argument(
|
||
"--output-dir",
|
||
help="图片输出目录；默认使用脚本目录/plots。",
|
||
)
|
||
parser.add_argument(
|
||
"--show",
|
||
action="store_true",
|
||
help="保存图片后同时显示窗口。",
|
||
)
|
||
arguments = parser.parse_args()
|
||
configure_matplotlib()
|
||
|
||
output_directory = (
|
||
Path(arguments.output_dir).expanduser().resolve()
|
||
if arguments.output_dir
|
||
else SCRIPT_DIR / "plots"
|
||
)
|
||
failed = 0
|
||
for csv_path in discover_csv_files(arguments.csv):
|
||
try:
|
||
plot_experiment(csv_path, output_directory, arguments.show)
|
||
except Exception as exception:
|
||
failed += 1
|
||
print(f"处理失败：{csv_path}：{exception}")
|
||
|
||
if failed:
|
||
raise SystemExit(f"共有{failed}个CSV处理失败。")
|
||
|
||
|
||
if __name__ == "__main__":
|
||
main()
|