打通新版Stanley轨迹跟踪闭环并补充实验测试与数据分析脚本

Co-authored-by: Cursor <cursoragent@cursor.com>
2026-08-06 15:14:12 +08:00
parent 47973cc94b
commit 19b1e49189
27 changed files with 3482 additions and 22 deletions
@@ -0,0 +1,191 @@
+"""一次运行四个轨迹实验绘图脚本。"""
+
+from __future__ import annotations
+
+import argparse
+import os
+import subprocess
+import sys
+from concurrent.futures import ThreadPoolExecutor, as_completed
+from pathlib import Path
+
+
+SCRIPT_NAMES = (
+    "plot_trajectory_comparison.py",
+    "plot_tracking_errors.py",
+    "plot_speed_response.py",
+    "plot_angular_command.py",
+)
+
+
+def build_command(
+    script_path: Path,
+    files: list[str],
+    frequency_hz: float,
+    filter_window_seconds: float,
+    output_directory: str | None,
+    show: bool,
+) -> list[str]:
+    """为一个绘图脚本构造与统一入口一致的命令行参数。"""
+    command = [
+        sys.executable,
+        str(script_path),
+        *files,
+        "--frequency",
+        str(frequency_hz),
+        "--window",
+        str(filter_window_seconds),
+    ]
+
+    if output_directory:
+        command.extend(["--output-dir", output_directory])
+
+    if show:
+        command.append("--show")
+
+    return command
+
+
+def run_script(
+    script_path: Path,
+    files: list[str],
+    frequency_hz: float,
+    filter_window_seconds: float,
+    output_directory: str | None,
+    show: bool,
+) -> tuple[str, int, str, str]:
+    """运行一个绘图脚本并返回名称、退出码及标准输出和错误。"""
+    result = subprocess.run(
+        build_command(
+            script_path,
+            files,
+            frequency_hz,
+            filter_window_seconds,
+            output_directory,
+            show,
+        ),
+        cwd=script_path.parent,
+        capture_output=True,
+        text=True,
+        encoding="utf-8",
+        errors="replace",
+        env={
+            **os.environ,
+            "PYTHONIOENCODING": "utf-8",
+        },
+        check=False,
+    )
+
+    return (
+        script_path.name,
+        result.returncode,
+        result.stdout.strip(),
+        result.stderr.strip(),
+    )
+
+
+def main() -> None:
+    """并行执行四类实验图的生成任务。"""
+    parser = argparse.ArgumentParser(
+        description="一次生成轨迹、误差、速度响应和角速度指令四类图。"
+    )
+    parser.add_argument(
+        "files",
+        nargs="*",
+        help="一个或多个CSV文件；省略时处理data_process目录下全部CSV。",
+    )
+    parser.add_argument(
+        "--frequency",
+        type=float,
+        default=20.0,
+        help="固定重采样频率，默认20 Hz。",
+    )
+    parser.add_argument(
+        "--window",
+        type=float,
+        default=0.55,
+        help="Savitzky-Golay滤波窗口，默认0.55 s。",
+    )
+    parser.add_argument(
+        "--output-dir",
+        help="图片输出目录；省略时由各绘图脚本使用默认目录。",
+    )
+    parser.add_argument(
+        "--show",
+        action="store_true",
+        help="生成后请求显示图片。",
+    )
+    args = parser.parse_args()
+
+    if args.frequency <= 0.0:
+        parser.error("--frequency必须大于0。")
+
+    if args.window <= 0.0:
+        parser.error("--window必须大于0。")
+
+    script_directory = Path(__file__).resolve().parent
+    script_paths = [
+        script_directory / name
+        for name in SCRIPT_NAMES
+    ]
+    missing_scripts = [
+        str(path)
+        for path in script_paths
+        if not path.is_file()
+    ]
+    if missing_scripts:
+        parser.error(
+            "缺少绘图脚本：" + "，".join(missing_scripts)
+        )
+
+    print("开始并行生成四类实验图……")
+    failures: list[str] = []
+
+    with ThreadPoolExecutor(
+        max_workers=len(script_paths)
+    ) as executor:
+        futures = [
+            executor.submit(
+                run_script,
+                script_path,
+                args.files,
+                args.frequency,
+                args.window,
+                args.output_dir,
+                args.show,
+            )
+            for script_path in script_paths
+        ]
+
+        for future in as_completed(futures):
+            script_name, return_code, stdout, stderr = (
+                future.result()
+            )
+            print(f"\n[{script_name}]")
+            if stdout:
+                print(stdout)
+            if stderr:
+                print(stderr, file=sys.stderr)
+
+            if return_code == 0:
+                print("执行成功。")
+            else:
+                failures.append(script_name)
+                print(
+                    f"执行失败，退出码={return_code}。",
+                    file=sys.stderr,
+                )
+
+    if failures:
+        print(
+            "\n以下脚本执行失败：" +
+            "，".join(failures),
+            file=sys.stderr,
+        )
+        raise SystemExit(1)
+
+    print("\n四类实验图均已生成。")
+
+
+if __name__ == "__main__":
+    main()