101 lines
2.6 KiB
Python
101 lines
2.6 KiB
Python
|
"""绘制控制器下发角速度命令曲线。"""
|
|||
|
|
|
|||
|
|
from __future__ import annotations
|
|||
|
|
|
|||
|
|
import argparse
|
|||
|
|
from pathlib import Path
|
|||
|
|
|
|||
|
|
import matplotlib
|
|||
|
|
matplotlib.use("Agg")
|
|||
|
|
import matplotlib.pyplot as plt
|
|||
|
|
import numpy as np
|
|||
|
|
|
|||
|
|
from plot_trajectory_comparison import (
|
|||
|
|
configure_matplotlib,
|
|||
|
|
discover_csv_files,
|
|||
|
|
load_and_resample,
|
|||
|
|
output_path,
|
|||
|
|
shade_localization_jump_windows,
|
|||
|
|
)
|
|||
|
|
|
|||
|
|
|
|||
|
|
def plot_angular_command(
|
|||
|
|
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,
|
|||
|
|
)
|
|||
|
|
time = frame["TimeSeconds"].to_numpy(dtype=float)
|
|||
|
|
angular_command = frame[
|
|||
|
|
"CommandAngularSpeedRadPerSec"
|
|||
|
|
].to_numpy(dtype=float)
|
|||
|
|
maximum = float(np.max(angular_command))
|
|||
|
|
minimum = float(np.min(angular_command))
|
|||
|
|
|
|||
|
|
fig, ax = plt.subplots(figsize=(10.0, 5.5))
|
|||
|
|
ax.plot(
|
|||
|
|
time,
|
|||
|
|
angular_command,
|
|||
|
|
color="tab:red",
|
|||
|
|
linewidth=1.6,
|
|||
|
|
label="CommandAngularSpeed",
|
|||
|
|
)
|
|||
|
|
ax.axhline(0.0, color="black", linewidth=0.8)
|
|||
|
|
shade_localization_jump_windows(ax, metadata)
|
|||
|
|
ax.set_xlabel("时间 / s")
|
|||
|
|
ax.set_ylabel("命令角速度 / (rad/s)")
|
|||
|
|
ax.set_title(
|
|||
|
|
f"角速度指令曲线\n"
|
|||
|
|
f"{metadata['controller_name']} - "
|
|||
|
|
f"{metadata['trajectory_name']}，"
|
|||
|
|
f"范围=[{minimum:.3f}, {maximum:.3f}]rad/s"
|
|||
|
|
)
|
|||
|
|
ax.grid(True, alpha=0.3)
|
|||
|
|
ax.legend()
|
|||
|
|
fig.tight_layout()
|
|||
|
|
|
|||
|
|
destination = output_path(
|
|||
|
|
csv_path,
|
|||
|
|
output_directory,
|
|||
|
|
"angular_command",
|
|||
|
|
)
|
|||
|
|
fig.savefig(destination, dpi=300, bbox_inches="tight")
|
|||
|
|
if show:
|
|||
|
|
plt.show()
|
|||
|
|
plt.close(fig)
|
|||
|
|
return destination
|
|||
|
|
|
|||
|
|
|
|||
|
|
def main() -> None:
|
|||
|
|
configure_matplotlib()
|
|||
|
|
parser = argparse.ArgumentParser(
|
|||
|
|
description="绘制控制器下发角速度命令曲线。"
|
|||
|
|
)
|
|||
|
|
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_angular_command(
|
|||
|
|
csv_path,
|
|||
|
|
args.frequency,
|
|||
|
|
args.window,
|
|||
|
|
args.output_dir,
|
|||
|
|
args.show,
|
|||
|
|
)
|
|||
|
|
print(f"已生成：{destination}")
|
|||
|
|
|
|||
|
|
|
|||
|
|
if __name__ == "__main__":
|
|||
|
|
main()
|