ParkingRobot/data_process/plot_new_controller_experiment.py

"""为新版控制器实验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


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 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
    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)
    )

    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,
        "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_reference_position = data["valid_reference_position"]
    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["actual_x"][valid_position],
        data["actual_y"][valid_position],
        linewidth=1.5,
        label="状态估计后的实际轨迹",
    )
    axis.scatter(*data["start"], color="green", s=45, label="起点")
    axis.scatter(*data["end"], color="red", s=45, label="终点")
    axis.set_aspect("equal", adjustable="box")
    axis.set_xlabel("世界坐标X / m")
    axis.set_ylabel("世界坐标Y / m")
    axis.set_title("期望轨迹与实际轨迹对比")
    axis.grid(True, alpha=0.3)
    axis.legend(fontsize=8)

    # 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)
    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)

    # 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"
    )
    print(f"已生成六子图总图：{destination}")
    return [destination]


def discover_csv_files(arguments: list[str]) -> list[Path]:
    """读取命令行文件；未指定时扫描脚本目录及data子目录中的CSV。"""
    if arguments:
        files = [Path(item).expanduser().resolve() for item in arguments]
    else:
        files = sorted(SCRIPT_DIR.glob("*.csv"))
        files.extend(sorted((SCRIPT_DIR / "data").glob("*.csv")))
    files = [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文件路径。")
    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()