引入最短舵角行驶的CommonUsage并更新项目引用

Co-authored-by: Cursor <cursoragent@cursor.com>

.codex/skills/commit/SKILL.md

@@ -0,0 +1,77 @@
+---
+name: commit
+description: 自动生成中文 git commit 信息并提交推送。读取当前改动，用简洁的中文一句话概括改动内容，然后自动执行 git add、commit、push。当用户说"提交""commit""提交代码""推送"时使用。
+allowed-tools: Bash(git status:*), Bash(git diff:*), Bash(git add:*), Bash(git commit:*), Bash(git push:*), Bash(git log:*), Bash(git branch:*)
+---
+
+# 自动 commit 并 push
+
+读取当前 git 改动，生成简洁的中文 commit 信息，然后自动提交并推送。
+
+## 执行步骤
+
+### 1. 查看当前状态
+
+先了解仓库当前情况：
+
+```bash
+git status
+git diff --stat        # 看改动了哪些文件、改动量
+git diff               # 看未暂存的具体改动
+git diff --staged      # 看已暂存的具体改动
+git log --oneline -5   # 看最近几次提交风格，保持一致
+```
+
+### 2. 分析改动
+
+基于 diff 内容，理解这次改动**实际做了什么**：
+
+- 新增了什么功能/文件
+- 修改/修复了什么
+- 删除/重构了什么
+- 是文档、配置还是代码改动
+
+**不要凭文件名猜测，要看实际 diff 内容。**
+
+### 3. 生成 commit 信息
+
+要求：
+
+- **中文**，简洁，**一句话**概括这次改动的核心内容
+- **不要前缀**（不用 feat/fix/docs 这种 Conventional Commits 前缀）
+- 直接描述做了什么，动词开头，如"添加 ALNS 自适应大邻域搜索算法"、"修复 POX 交叉中的索引越界问题"、"重构 FJSP 解码逻辑去掉 AGV 部分"
+- 如果一次改动包含多个不相关的事情，提示用户是否要分开提交（但默认仍按一条处理）
+- 长度控制在一行能看完，不写冗长描述
+
+### 4. 自动提交并推送
+
+确认 commit 信息后，依次执行：
+
+```bash
+git add -A                          # 暂存所有改动
+git commit -m "生成的中文commit信息"
+git push                            # 推送到当前分支的远程
+```
+
+### 5. 处理常见情况
+
+- **没有改动**：如果 `git status` 显示没有改动，告知用户无需提交，停止
+- **push 失败**：
+  - 如果是因为远程有新提交（需要先 pull），告知用户，建议先 `git pull` 或 `git pull --rebase`，**不要自动强推**
+  - 如果是没有配置远程或没有 upstream 分支，提示用户，给出 `git push -u origin <分支名>` 的建议命令
+  - 如果是认证问题，告知用户检查凭证
+- **当前在重要分支**（如 main/master）：正常执行，但在输出里提示一下当前分支名，让用户心里有数
+
+### 6. 输出
+
+完成后简要报告：
+
+- 生成的 commit 信息
+- 提交到了哪个分支
+- push 是否成功
+
+## 注意事项
+
+- commit 信息必须如实反映 diff 内容，不编造
+- push 失败时不要用 `--force` 强推，交给用户决定
+- 如果改动很大很杂，主动提示用户考虑拆分提交，但不强制

.codex/skills/readme/SKILL.md

@@ -0,0 +1,92 @@
+---
+name: readme
+description: 为当前项目生成适配 Gitee / 公司内部代码仓库的中英文双语 README。默认生成 README.md（中文，Gitee 默认展示）和 README_en.md（英文）两个文件，顶部互相链接切换语言。适用于公司项目、算法项目、机器人项目、工程代码仓库。当用户说“写个README”“生成项目介绍”“生成Gitee README”“make a readme”时使用。
+---
+
+# Gitee 双语 README 生成
+
+为当前项目生成两个互相链接的 README 文件：
+
+- `README.md`：简体中文，作为 Gitee 默认展示文件
+- `README_en.md`：英文版，供中英文切换使用
+
+如果项目中已经存在 `README_zh.md`、`Readme_zh.md`、`Readme_en.md` 等命名，先读取已有文件，并尽量沿用当前仓库已有命名规范；如果没有明确规范，默认使用 `README.md` + `README_en.md`。
+
+## 执行目标
+
+生成符合公司内部 Gitee 仓库风格的 README，不写成 GitHub 开源宣传页。
+
+README 应该让新同事或项目参与者快速知道：
+
+- 项目是什么
+- 面向什么设备 / 平台 / 场景
+- 软件架构大概是什么
+- 如何安装依赖
+- 如何编译 / 运行 / 启动
+- 代码目录怎么组织
+- 如何按公司流程参与开发
+
+## 执行步骤
+
+### 1. 调研项目
+
+先充分了解项目，不要凭空编造内容。
+
+必须优先读取和分析：
+
+- 项目根目录结构
+- 已有 README / 文档
+- 主入口脚本
+- 启动脚本
+- `CMakeLists.txt`
+- `package.xml`
+- `requirements.txt`
+- `pyproject.toml`
+- `package.json`
+- `docker-compose.yml`
+- `Dockerfile`
+- 配置文件
+- launch 文件
+- ROS / ROS2 相关目录
+- 核心源码目录
+- 设备通信、底盘控制、导航、感知、驱动相关代码
+
+需要识别：
+
+- 项目名称
+- 项目用途
+- 运行平台
+- 技术栈
+- 编程语言
+- ROS / ROS2 版本（如果存在）
+- 构建方式
+- 启动方式
+- 主要模块
+- 依赖项
+- 是否有实际设备、仿真环境、域控一体机、阿克曼底盘、CAN、串口、网络通信等内容
+
+**重要：只写代码和文档中真实存在的内容。**
+
+不要编造：
+
+- 未确认的算法
+- 未确认的性能指标
+- 未确认的硬件型号
+- 未确认的 ROS 版本
+- 未确认的启动命令
+- 未确认的部署流程
+- 未确认的许可证
+
+如果信息不足，用“待补充”明确标注，不要用通用模板假装完整。
+
+---
+
+## 2. 文件命名与语言切换
+
+### 默认文件
+
+生成：
+
+```text
+README.md
+README_en.md

.gitignore

@@ -1,56 +1,92 @@
-# .NET / MSBuild生成目录
-**/bin/
-**/obj/
-**/build/
-**/publish/
-artifacts/
-TestResults/
-*.nupkg
-packages/
+##################################################
+# Visual Studio
+##################################################
 
-# MyParking构建脚本生成的部署文件
-/output/
-/ref/CommonUsage.dll
-
-# Python缓存和本地虚拟环境
-**/__pycache__/
-*.py[cod]
-.pytest_cache/
-.mypy_cache/
-.venv/
-venv/
-
-# 实验生成数据；保留脚本、requirements和README
-/data_process/**/*.csv
-/data_process/**/*.png
-/data_process/**/plots/
-/logs/
-
-# IDE和用户配置
+# Visual Studio 工作区缓存
 .vs/
-.idea/
-.vscode/
+**/.vs/
+
+# 用户配置
 *.user
 *.suo
 *.userosscache
 *.sln.docstates
+
+##################################################
+# Build 输出
+##################################################
+
+# 编译输出目录
+bin/
+obj/
+**/bin/
+**/obj/
+
+##################################################
+# Rider / VS Code
+##################################################
+
+.idea/
+.vscode/
+
+##################################################
+# NuGet
+##################################################
+
+*.nupkg
+packages/
+
+##################################################
+# 日志
+##################################################
+
+*.log
+
+##################################################
+# 临时文件
+##################################################
+
+*.tmp
+*.temp
+
+##################################################
+# 测试结果
+##################################################
+
+TestResults/
+
+##################################################
+# 发布目录
+##################################################
+
+publish/
+
+##################################################
+# Windows
+##################################################
+
+Thumbs.db
+Desktop.ini
+
+##################################################
+# JetBrains
+##################################################
+
 _ReSharper*/
 *.DotSettings.user
 
-# 日志、临时文件和本地缓存
-*.log
-*.tmp
-*.temp
+##################################################
+# 缓存
+##################################################
+
 *.cache
 
-# 本地数据库
+##################################################
+# 数据库（如果有）
+##################################################
+
 *.db
 *.sqlite
 *.sqlite3
 
-# 操作系统生成文件
-Thumbs.db
-Desktop.ini
-.DS_Store
-
-# 不要全局忽略*.dll：MedullaAdapter/ref和MultiWheelC/ref中的宿主依赖需要保留。
+*.csv

AGENTS.md

@@ -1,87 +0,0 @@
-# Karpathy原则
-
-## 先理解再修改
-
-- 检查实际实现、调用链和现有约束，不凭名称猜测行为。
-- 明确必要假设；遇到会显著改变结果的歧义时先说明。
-
-## 保持简单
-
-- 使用满足当前需求的最小方案。
-- 不增加推测性功能、无必要抽象或配置。
-
-## 精确修改
-
-- 只触碰与当前任务直接相关的代码，保留既有风格和无关改动。
-- 清理由本次修改产生的废弃代码，不顺便清理原有无关代码。
-
-## 面向验证
-
-- 修改前明确可观察的成功标准，修改后运行相关检查。
-- 如实报告警告、限制和未验证项。
-
-# MyParking项目规则
-
-## 项目定位与事实来源
-
-- `MyParking`是当前正式开发的停车机器人项目，结论优先依据本目录中的当前代码和实际运行配置。
-- 工作区中的旧版停车机器人、MDCS源码和轨迹规划项目只能作为辅助参考，不能覆盖当前实现所表达的事实。
-- 不能从代码、配置或用户提供资料确认的信息统一标记为“待确认”，不得自行补全或编造。
-- 修改代码后检查实际diff，并运行与改动风险相匹配的最相关编译或测试；不主动修改任务范围之外的代码。
-
-## 代码边界
-
-- `CommonUsage-MultiVehicleSync`是独立的通用底盘库，不反向依赖`Shared`、M层或C层。
-- `Shared`只包含M/C共享的数据模型、数学方法和底盘适配代码。
-- `MedullaAdapter`负责M层硬件通信、IO和底盘命令。
-- `MultiWheelC`负责C层动作、控制、实验和数据记录。
-
-## 代码规范
-
-- 新增或修改的类、结构体和方法使用一句话的`/// <summary>`说明业务用途。
-- 单位、坐标系或正负方向不明确时补充说明，不复述代码字面内容。
-- Shared统一使用SI单位：m、m/s、rad、rad/s。
-- 车体坐标系为X向前、Y向左、逆时针为正；旧接口单位只在边界处转换。
-- 角度归一化、最短角差和度弧度转换统一使用`Shared/Mathematics/AngleMath.cs`，不重复手写。
-- 弧度归一化范围为`[-π, π)`，度归一化范围为`[-180°, 180°)`。
-- 车辆航向可用圆周最短角差；受`[-120°, 120°]`限制的机械舵角误差必须直接使用目标值减实际值。
-
-## 控制安全
-
-- 未经明确要求，不改变速度或舵角符号、CAN ID、遥控器映射、机械限位和模式切换策略。
-- 修改底盘命令、模式切换或四轮解算时，说明对实际运动的影响。
-- 实车测试采用低速、短距离，并确保可以立即停车。
-
-## 构建验证
-
-- 修改`CommonUsage-MultiVehicleSync`、`Shared`、`MedullaAdapter`或`MultiWheelC`后，在`MyParking`目录运行：
-
-```powershell
-powershell -NoProfile -ExecutionPolicy Bypass -File .\build-and-package.ps1
-```
-
-- Release构建在命令末尾追加`-Configuration Release`。
-- 报告各项目的警告和错误，并确认M/C部署包使用同一份`CommonUsage.dll`。
-- 不直接编辑`bin`、`obj`、`build`和`output`中的产物。
-- 不手工覆盖`ref/CommonUsage.dll`，由构建脚本统一更新。
-
-# 项目知识库规则
-
-## 按需读取
-
-- 默认只读取`docs/INDEX.md`，再根据当前任务选择最相关的知识文档。
-- 严禁在每个任务开始时读取整个`docs/`；初始只读取与任务直接相关的1～2个文档，信息不足时再扩大范围。
-- 当前任务不依赖项目背景或长期知识时，可以不读取`INDEX.md`之外的文档。
-- 同一会话中已经读取且没有变化的知识文档不要重复读取。
-- 除非任务确实涉及旧版实现或MDCS底层，不读取工作区中的参考项目。
-- 优先使用关键词、类名、方法名和文件路径定位代码，不进行无目的的全库扫描。
-- 不扫描`.git`、`bin`、`obj`、`build`、`output`、日志、缓存、编译产物和第三方依赖。
-
-## 增量更新
-
-- 只有产生了已经确认、长期有效的新知识时，才更新对应文档。
-- 普通代码修改、临时调试、失败尝试和一般问答不需要更新知识库。
-- 每次只读取和更新与当前任务直接相关的文档，采用局部增量修改，不重写无关内容。
-- 不把大段源码、日志、终端输出或聊天记录复制到知识库；使用路径、类型名、方法名和精炼结论。
-- 单纯进度变化只更新`docs/progress.md`中的对应小段。
-- 没有值得长期保存的信息时，不为了形式要求强行更新文档。

ClumsyPilot/AGV.cs

@@ -0,0 +1,21 @@
+using ClumsyCore;
+using MDCSToolBox.Clumsy.AgvInterfaces;
+using MDCSToolBox.Clumsy.MotionControllers;
+
+namespace MultiWheelC
+{
+    public class AGV : MultiWheelInterface
+    {
+        public override AbstractGeometricController GetController()
+                => new ChassisController().Get();
+        public override MultiWheelMagTracker GetMagController()
+            => new MultiWheelMagTracker();
+        public override NaiveMagnetController GetNaiveMagnetController()
+            => new NaiveMagnetController();
+
+        public void Sleep(float seconds)
+        {
+            new DriveTask(new Sleep { Second = seconds }.Get()).Wait();
+        }
+    }
+}

ClumsyPilot/ChassisController.cs

@@ -0,0 +1,45 @@
+using ClumsyCore;
+using ClumsyCore.Pilot;
+using MDCSToolBox.Clumsy.MotionControllers;
+using MDCSToolBox.Clumsy.Movements;
+using MDCSToolBox.Clumsy.Pilot;
+
+namespace MultiWheelC;
+
+public class ChassisController : MovementDefinition<MultiWheelGeometricController>
+{
+    public float BaseSpeed = Configuration.conf.basicSpeed;
+
+    // 创建单车几何跟踪控制器（直接控本车底盘，不走多车 Auto 通道）
+    public override MultiWheelGeometricController Get()
+    {
+        return new MultiWheelGeometricController
+        {
+            Chassis = BasicPilotBase.Chassis,
+            BaseSpeed = BaseSpeed,
+            SlowDistance = PilotDefinition.Conf.SlowDistance,
+            SlowingPow = PilotDefinition.Conf.SlowingPow,
+            FinishDistance = PilotDefinition.Conf.FinishDistance,
+            FinishSpeed = PilotDefinition.Conf.FinishSpeed,
+            FirstThAccuracy = PilotDefinition.Conf.FirstThAccuracy,
+            FirstRotateSpeedFac = PilotDefinition.Conf.FirstRotateSpeedFac,
+            FirstRotateMaxSpeed = PilotDefinition.Conf.FirstRotateMaxSpeed,
+            NotContinuousAngle = PilotDefinition.Conf.NotContinuousAngle,
+            DebugMode = PilotDefinition.Conf.MotionDebugPrint,
+            DebugCurvature = PilotDefinition.Conf.DebugCurvature,
+            PowerSteeringLookAhead = PilotDefinition.Conf.PowerSteeringLookAhead,
+            SpeedLookAhead = PilotDefinition.Conf.SpeedLookAhead,
+            SpeedLookAheadCurveDiff = PilotDefinition.Conf.SpeedLookAheadCurveDiff,
+            SpeedLookBackCurveDiff = PilotDefinition.Conf.SpeedLookBackCurveDiff,
+            SpeedLimitCurveDiffMin = PilotDefinition.Conf.SpeedLimitCurveDiffMin,
+            SpeedLimitCurveMin = PilotDefinition.Conf.SpeedLimitCurveMin,
+            MaxRotateSpeed = PilotDefinition.Conf.MaxRotateSpeedCurveLimit,
+            MaxRotateAcc = PilotDefinition.Conf.MaxRotateAccCurveLimit,
+            GcpThetaThreshold = PilotDefinition.Conf.GcpThetaThreshold,
+            DthLinearFac = PilotDefinition.Conf.DthLinearFac,
+            DthLinearThreshold = PilotDefinition.Conf.DthLinearThreshold,
+            BiasFac = PilotDefinition.Conf.BiasFac,
+            BiasThreshold = PilotDefinition.Conf.BiasThreshold,
+        };
+    }
+}

ClumsyPilot/ClumsyPilot.csproj

@@ -3,7 +3,8 @@
   <PropertyGroup>
     <TargetFramework>netstandard2.0</TargetFramework>
     <LangVersion>10</LangVersion>
-    <AssemblyName>MultiWheelC</AssemblyName>
+    <AllowUnsafeBlocks>true</AllowUnsafeBlocks>
+    <AssemblyName>ClumsyPilot</AssemblyName>
     <RootNamespace>MultiWheelC</RootNamespace>
     <AppendTargetFrameworkToOutputPath>false</AppendTargetFrameworkToOutputPath>
     <OutputPath>build\Clumsy\</OutputPath>
@@ -15,8 +16,8 @@
   </ItemGroup>
 
   <ItemGroup>
-    <Compile Include="..\Shared\**\*.cs"
-             Link="Shared\%(RecursiveDir)%(Filename)%(Extension)" />
+  <Compile Include="..\Shared\*.cs"
+           Link="Shared\%(Filename)%(Extension)" />
 </ItemGroup>
 
   <ItemGroup>

ClumsyPilot/MovementTests.cs

@@ -0,0 +1,620 @@
+using ClumsyCore;
+using ClumsyCore.DTools;
+using ClumsyCore.Interfaces;
+using ClumsyCore.Pilot;
+using CommonUsage.Chassis;
+using MDCSToolBox.Commons.Controllers;
+using MDCSToolBox.Clumsy.Tracks;
+using MyParking.Shared;
+using System;
+using System.Numerics;
+using System.Threading;
+
+namespace MultiWheelC
+{
+    internal static class MovementTestPreparation
+    {
+        // 在测试正式开始前，将四个舵轮稳定回正到车体前向。
+        public static bool AlignWheelsForward(
+            ref DriveTask activeTask)
+        {
+            var preparation = new PrepareWheelsForward();
+            var task = new DriveTask(preparation.Get());
+            activeTask = task;
+
+            try
+            {
+                task.Wait();
+                return preparation.Completed;
+            }
+            catch (Exception ex)
+            {
+                Console.WriteLine(
+                    $"测试前舵轮回正失败：{ex.Message}");
+                return false;
+            }
+            finally
+            {
+                task.Stop();
+                if (ReferenceEquals(activeTask, task))
+                    activeTask = null;
+            }
+        }
+
+        // 只读取实际舵角，检查四个舵轮是否已与车头方向一致。
+        public static bool AreWheelsForward(
+            float toleranceDegrees = 2f)
+        {
+            var chassis =
+                PilotDefinition.Chassis as MultiWheelChassis;
+            if (chassis == null)
+            {
+                Console.WriteLine(
+                    "当前底盘不是MultiWheelChassis，无法检查舵轮方向。");
+                return false;
+            }
+
+            try
+            {
+                var adapter = new MultiWheelChassisAdapter(
+                    chassis,
+                    PilotDefinition.Self.CarNum);
+                var toleranceRadians =
+                    toleranceDegrees * Math.PI / 180.0;
+
+                if (adapter.AreParallelWheelsAligned(
+                        0.0,
+                        toleranceRadians))
+                {
+                    return true;
+                }
+
+                Console.WriteLine(
+                    "四个舵轮尚未与车头方向一致，请先执行“准备：四个舵轮与车头方向一致”。");
+                return false;
+            }
+            catch (Exception ex)
+            {
+                Console.WriteLine(
+                    $"检查舵轮方向失败：{ex.Message}");
+                return false;
+            }
+        }
+    }
+
+    [MovementTest(name = "准备：四个舵轮与车头方向一致")]
+    public class AlignWheelsForwardTest : MovementTest
+    {
+        private DriveTask _task;
+
+        // 单独将四个舵轮转到车体前向0°并等待实际反馈稳定到位。
+        public override void Test()
+        {
+            MovementTestPreparation.AlignWheelsForward(
+                ref _task);
+        }
+
+        // 停止正在执行的舵轮回正任务并清零底盘运动命令。
+        public override void TestStop()
+        {
+            _task?.Stop();
+            _task = null;
+        }
+    }
+
+    [MovementTest(name = "测试连续前进4m")]
+    public class TestForward4m : MovementTest
+    {
+        public float DistanceMillimeters = 4000f; // 测试距离，单位mm。
+        public float CruiseSpeed = 0.3f;  // 巡航速度上限，单位m/s。
+        public int TrialNumber = 1; // 重复实验编号。
+        private DriveTask _task;
+        private TrackingExperimentRecorder _recorder;
+        // 从当前Detour位置沿车头方向生成4m连续直线并记录测试数据。
+        public override void Test()
+        {
+            if (!MovementTestPreparation.AreWheelsForward())
+            {
+                return;
+            }
+
+            var location = DetourInterface.getCartLocation();
+            if (double.IsNaN(location.x) ||
+                double.IsInfinity(location.x) ||
+                double.IsNaN(location.y) ||
+                double.IsInfinity(location.y) ||
+                double.IsNaN(location.th) ||
+                double.IsInfinity(location.th))
+            {
+                Console.WriteLine(
+                    "Detour当前位姿无效，取消连续前进4m测试。");
+                return;
+            }
+            var source = new Vector2((float)location.x, (float)location.y);
+            // Detour航向单位是度，三角函数需要弧度。
+            var headingRadians = location.th * Math.PI / 180.0;
+            var destination = new Vector2(
+                source.X + DistanceMillimeters * (float)Math.Cos(headingRadians),
+                source.Y + DistanceMillimeters * (float)Math.Sin(headingRadians));
+            _recorder =
+                        new TrackingExperimentRecorder(
+                            controllerName: "Stanley",
+                            trajectoryName: "Straight4m",
+                            trialNumber: TrialNumber,
+                            referenceStart: source,
+                            referenceEnd: destination,
+                            referenceSpeed: CruiseSpeed);
+            _recorder.Start();
+            try
+            {
+                _task = new DriveTask(
+                new DstTracker
+                {
+                    Src = source,
+                    Dst = destination,
+                    CarDirectionBias = 0f,
+                    MaxSpeed = CruiseSpeed
+                }.Get());
+                _task.Wait();
+                // 保留少量停车后数据，便于观察速度是否回到零。
+                Thread.Sleep(300);
+            }
+            finally
+            {
+                _task?.Stop();
+                _recorder?.UpdateCommand(0f, 0f);
+                _recorder?.StopAndSave();
+                _task = null;
+                _recorder = null;
+            }
+        }
+
+        public override void TestStop()
+        {
+            _task?.Stop();
+            _recorder?.UpdateCommand(0f, 0f);
+            _recorder?.StopAndSave();
+        }
+    }
+
+    [MovementTest(name = "测试原地自转90°")]
+    public class TestRotate90 : MovementTest
+    {
+        public float RelativeAngleDegrees = 90f; // 相对当前航向的旋转角度，逆时针为正。
+        public float MaxAngularSpeedDegreesPerSecond = 20f; // PID输出的最大角速度。
+        public int TrialNumber = 1; // 重复实验编号。
+
+        private DriveTask _task;
+        private TrackingExperimentRecorder _recorder;
+
+        // 从当前Detour航向开始，原地相对旋转指定角度并记录实验数据。
+        public override void Test()
+        {
+            if (float.IsNaN(RelativeAngleDegrees) ||
+                float.IsInfinity(RelativeAngleDegrees) ||
+                float.IsNaN(MaxAngularSpeedDegreesPerSecond) ||
+                float.IsInfinity(MaxAngularSpeedDegreesPerSecond) ||
+                MaxAngularSpeedDegreesPerSecond <= 0f)
+            {
+                Console.WriteLine("原地旋转测试参数无效。");
+                return;
+            }
+
+            var location = DetourInterface.getCartLocation();
+            if (double.IsNaN(location.x) ||
+                double.IsInfinity(location.x) ||
+                double.IsNaN(location.y) ||
+                double.IsInfinity(location.y) ||
+                double.IsNaN(location.th) ||
+                double.IsInfinity(location.th))
+            {
+                Console.WriteLine(
+                    "Detour当前位姿无效，取消原地旋转测试。");
+                return;
+            }
+
+            var rotationCenter =
+                new Vector2((float)location.x, (float)location.y);
+            var targetWorldAngle =
+                NormalizeDegrees(
+                    (float)location.th + RelativeAngleDegrees);
+
+            _recorder = new TrackingExperimentRecorder(
+                controllerName: "InPlaceRotatePID",
+                trajectoryName: "Rotate90",
+                trialNumber: TrialNumber,
+                referenceStart: rotationCenter,
+                referenceEnd: rotationCenter,
+                referenceSpeed:
+                    MaxAngularSpeedDegreesPerSecond);
+            _recorder.Start();
+
+            try
+            {
+                _task = new DriveTask(
+                    new MultiWheelRotateInPlace
+                    {
+                        // MultiWheelRotateInPlace接收世界坐标系绝对航向。
+                        AngleTarget = targetWorldAngle,
+                        PidparamsRead = () => new PIDParams
+                        {
+                            Kp =
+                                PilotDefinition.Conf.InPlaceRotateKp,
+                            Ki =
+                                PilotDefinition.Conf.InPlaceRotateKi,
+                            Kd =
+                                PilotDefinition.Conf.InPlaceRotateKd,
+                            DeadZone =
+                                PilotDefinition.Conf
+                                    .InPlaceRotateArriveDeg,
+                            SpeedAccPerSec =
+                                PilotDefinition.Conf.InPlaceRotateAcc,
+                            OutputUpperThreshold =
+                                MaxAngularSpeedDegreesPerSecond,
+                            MaxI =
+                                PilotDefinition.Conf.InPlaceRotateMaxI
+                        },
+                        CommandAngularSpeedObserver =
+                            commandAngularSpeed =>
+                                _recorder?.UpdateCommand(
+                                    0f,
+                                    commandAngularSpeed)
+                    }.Get());
+
+                _task.Wait();
+
+                // 保留少量停止后的样本，用于观察角速度是否回到零。
+                Thread.Sleep(300);
+            }
+            finally
+            {
+                _task?.Stop();
+                _recorder?.UpdateCommand(0f, 0f);
+                _recorder?.StopAndSave();
+                _task = null;
+                _recorder = null;
+            }
+        }
+
+        // 停止原地旋转并保存当前已经采集的实验数据。
+        public override void TestStop()
+        {
+            _task?.Stop();
+            _recorder?.UpdateCommand(0f, 0f);
+            _recorder?.StopAndSave();
+        }
+
+        // 将世界航向归一化到大约[-180°,180°]。
+        private static float NormalizeDegrees(float angleDegrees)
+        {
+            return (float)(
+                angleDegrees -
+                Math.Round(angleDegrees / 360.0) * 360.0);
+        }
+    }
+
+    [MovementTest(name = "测试左转90°圆弧")]
+    public class TestArcMovement : MovementTest
+    {
+        public float RadiusMillimeters = 2000f; // 左转圆的半径，单位mm。
+        public float CruiseSpeed = 0.3f; // 圆周运动速度上限，单位m/s。
+        public int TrialNumber = 1; // 重复实验编号。
+
+        private DriveTask _task;
+        private TrackingExperimentRecorder _recorder;
+
+        // 从当前位姿开始，沿半径2m的圆弧向左转弯90°。
+        public override void Test()
+        {
+            if (float.IsNaN(RadiusMillimeters) ||
+                float.IsInfinity(RadiusMillimeters) ||
+                RadiusMillimeters <= 0f ||
+                float.IsNaN(CruiseSpeed) ||
+                float.IsInfinity(CruiseSpeed) ||
+                CruiseSpeed <= 0f)
+            {
+                Console.WriteLine("圆弧运动测试参数无效。");
+                return;
+            }
+
+            if (!MovementTestPreparation.AreWheelsForward())
+            {
+                return;
+            }
+
+            var location = DetourInterface.getCartLocation();
+            if (double.IsNaN(location.x) ||
+                double.IsInfinity(location.x) ||
+                double.IsNaN(location.y) ||
+                double.IsInfinity(location.y) ||
+                double.IsNaN(location.th) ||
+                double.IsInfinity(location.th))
+            {
+                Console.WriteLine(
+                    "Detour当前位姿无效，取消圆弧运动测试。");
+                return;
+            }
+
+            var source =
+                new Vector2((float)location.x, (float)location.y);
+            var headingRadians =
+                location.th * Math.PI / 180.0;
+
+            // 根据世界航向求车体左法向，左转圆心位于车辆左侧。
+            var center = new Vector2(
+                source.X -
+                    RadiusMillimeters *
+                    (float)Math.Sin(headingRadians),
+                source.Y +
+                    RadiusMillimeters *
+                    (float)Math.Cos(headingRadians));
+
+            // 从圆心指向车辆起点的极角，比车辆切线航向小90°。
+            var startRadialAngleDegrees =
+                (float)location.th - 90f;
+
+            var controller = new ChassisController
+            {
+                BaseSpeed = CruiseSpeed
+            }.Get();
+            controller.FinishSpeed = 0f;
+
+            var arc = new CircularArcTrack(
+                center,
+                RadiusMillimeters,
+                startRadialAngleDegrees,
+                startRadialAngleDegrees + 90f,
+                direction: 1)
+            {
+                Speed = CruiseSpeed,
+                CarDirectionBias = 0f
+            };
+
+            if (!controller.AddTrack(arc, "LeftArc90Degrees"))
+            {
+                Console.WriteLine(
+                    "左转90°圆弧轨迹添加失败，取消测试。");
+                return;
+            }
+
+            _recorder = new TrackingExperimentRecorder(
+                controllerName: "GeometricController",
+                trajectoryName:
+                    $"LeftArc90_R{RadiusMillimeters:0}mm",
+                trialNumber: TrialNumber,
+                referenceStart: source,
+                referenceEnd: source,
+                referenceSpeed: CruiseSpeed);
+            _recorder.Start();
+
+            try
+            {
+                _task = new DriveTask(controller.Track());
+                _task.Wait();
+
+                // 保留少量停车后的样本，用于观察速度是否回到零。
+                Thread.Sleep(300);
+            }
+            finally
+            {
+                _task?.Stop();
+                _recorder?.UpdateCommand(0f, 0f);
+                _recorder?.StopAndSave();
+                _task = null;
+                _recorder = null;
+            }
+        }
+
+        // 停止圆弧运动并保存当前已经采集的实验数据。
+        public override void TestStop()
+        {
+            _task?.Stop();
+            _recorder?.UpdateCommand(0f, 0f);
+            _recorder?.StopAndSave();
+        }
+    }
+
+    public abstract class ClampMovementTestBase : MovementTest
+    {
+        public float TimeoutSeconds = 30f; // 动作超时时间，单位s。
+
+        private DriveTask _task;
+        protected abstract bool Close { get; }
+
+        // 根据派生测试类型驱动左右夹臂同步夹紧或打开。
+        public override void Test()
+        {
+            var leftTarget = Close
+                ? PilotDefinition.Self.LeftArmUpperPos
+                : PilotDefinition.Self.LeftArmLowerPos;
+            var rightTarget = Close
+                ? PilotDefinition.Self.RightArmUpperPos
+                : PilotDefinition.Self.RightArmLowerPos;
+
+            if (float.IsNaN(leftTarget) ||
+                float.IsInfinity(leftTarget) ||
+                float.IsNaN(rightTarget) ||
+                float.IsInfinity(rightTarget))
+            {
+                Console.WriteLine(
+                    "夹臂目标位置无效，取消夹臂运动测试。");
+                return;
+            }
+
+            // 防止重复点击时上一项夹臂任务仍在运行。
+            TestStop();
+            Console.WriteLine(
+                $"开始夹臂{(Close ? "夹紧" : "打开")}测试：" +
+                $"左目标={leftTarget}，右目标={rightTarget}");
+
+            var task = new DriveTask(
+                new ClampToTarget
+                {
+                    LeftClampTarget = leftTarget,
+                    RightClampTarget = rightTarget,
+                    TimeoutSeconds = TimeoutSeconds
+                }.Get());
+            _task = task;
+
+            try
+            {
+                task.Wait();
+            }
+            finally
+            {
+                PilotDefinition.Self.SpeedLeftArm = 0f;
+                PilotDefinition.Self.SpeedRightArm = 0f;
+                if (ReferenceEquals(_task, task))
+                    _task = null;
+            }
+        }
+
+        // 停止夹臂任务并立即清零左右夹臂下发速度。
+        public override void TestStop()
+        {
+            _task?.Stop();
+            _task = null;
+            PilotDefinition.Self.SpeedLeftArm = 0f;
+            PilotDefinition.Self.SpeedRightArm = 0f;
+        }
+    }
+
+    [MovementTest(name = "夹臂关闭测试")]
+    public sealed class TestClampOpenMovement
+        : ClampMovementTestBase
+    {
+        protected override bool Close => false;
+    }
+
+    [MovementTest(name = "夹臂启动测试")]
+    public sealed class TestClampCloseMovement
+        : ClampMovementTestBase
+    {
+        protected override bool Close => true;
+    }
+
+}
+
+
+
+
+
+
+#region 旧版测试
+// public abstract class DstTrackerTestBase : MovementTest
+// {
+//     public bool UseInteractivePick = true;
+//     public float srcX;
+//     public float srcY;
+//     public float dstX;
+//     public float dstY;
+//     public float carDirectionBias;
+
+//     private readonly Painter _painter = UI.GetPainter("DstTrackerTest");
+//     private DriveTask _dt;
+
+//     protected DstTrackerTestBase(float defaultCarDirectionBias)
+//     {
+//         carDirectionBias = defaultCarDirectionBias;
+//     }
+
+//     public override void TestStop()
+//     {
+//         _dt?.Stop();
+//         _painter?.Clear();
+//     }
+
+//     public override void Test()
+//     {
+//         Vector2 p1;
+//         Vector2 p2;
+//         if (UseInteractivePick)
+//         {
+//             p1 = UI.GetPoint("point1");
+//             p2 = UI.GetPoint("point2");
+//         }
+//         else
+//         {
+//             p1 = new Vector2(srcX, srcY);
+//             p2 = new Vector2(dstX, dstY);
+//         }
+
+//         _painter.Clear();
+//         _dt = new DriveTask(new DstTracker
+//         {
+//             Src = p1,
+//             Dst = p2,
+//             CarDirectionBias = carDirectionBias,
+//         }.Get());
+//         _dt.Wait();
+//     }
+// }
+
+// [MovementTest(name = "测试终点跟踪动作-前进")]
+// public sealed class DstTrackerForward : DstTrackerTestBase
+// {
+//     public DstTrackerForward() : base(0f) { }
+// }
+
+// [MovementTest(name = "测试终点跟踪动作-后退")]
+// public sealed class DstTrackerBackward : DstTrackerTestBase
+// {
+//     public DstTrackerBackward() : base(180f) { }
+// }
+
+// [MovementTest(name = "底盘旋转测试")]
+// public class RotateToAngleTest : MovementTest
+// {
+//     private DriveTask _dt;
+
+//     // 停止当前正在执行的底盘原地旋转任务。
+//     public override void TestStop()
+//     {
+//         _dt?.Stop();
+//     }
+
+//     // 交互输入目标角度后执行底盘原地旋转测试。
+//     public override void Test()
+//     {
+//         var input = UI.GetInput("输入旋转角度：");
+//         if (!float.TryParse(input, out var angleTarget))
+//         {
+//             Console.WriteLine(
+//                 $"旋转测试输入无效：{input}");
+//             return;
+//         }
+//         // 防止重复启动测试时，上一项旋转任务仍在运行。
+//         _dt?.Stop();
+//         var task = new DriveTask(
+//             new MultiWheelRotateInPlace
+//             {
+//                 AngleTarget = angleTarget,
+
+//                 PidparamsRead = () => new PIDParams
+//                 {
+//                     Kp = PilotDefinition.Conf.InPlaceRotateKp,
+//                     Ki = PilotDefinition.Conf.InPlaceRotateKi,
+//                     Kd = PilotDefinition.Conf.InPlaceRotateKd,
+//                     DeadZone = PilotDefinition.Conf.InPlaceRotateArriveDeg,
+//                     SpeedAccPerSec = PilotDefinition.Conf.InPlaceRotateAcc,
+//                     OutputUpperThreshold = PilotDefinition.Conf.InPlaceRotateMaxSpeed,
+//                     MaxI = PilotDefinition.Conf.InPlaceRotateMaxI,
+//                 }
+//             }.Get());
+//         _dt = task;
+//         try
+//         {
+//             task.Wait();
+//         }
+//         finally
+//         {
+//             // 防止旧任务结束时，错误清除后来启动的新任务。
+//             if (ReferenceEquals(_dt, task))
+//             {
+//                 _dt = null;
+//             }
+//         }
+//     }
+// }
+
+#endregion

ClumsyPilot/Movements.cs

@@ -0,0 +1,562 @@
+using ClumsyCore;
+using ClumsyCore.DTools;
+using ClumsyCore.Interfaces;
+using ClumsyCore.Pilot;
+using CommonUsage.Chassis;
+using MDCSToolBox.Clumsy.Movements;
+using MDCSToolBox.Clumsy.Tracks;
+using MDCSToolBox.Commons.Controllers;
+using System;
+using System.Collections.Generic;
+using System.Drawing;
+using System.Numerics;
+using System.Threading;
+using FundamentalLib;
+using MyParking.Shared;
+
+namespace MultiWheelC
+{
+    // C层测试准备：停车并等待四个舵轮稳定回到车体前向0°。
+    public class PrepareWheelsForward : MovementDefinition
+    {
+        public float ToleranceDegrees = 2f;
+        public float StableSeconds = 0.3f;
+        public float TimeoutSeconds = 10f;
+        public bool Completed { get; private set; }
+
+        public override IEnumerable<bool> Get()
+        {
+            var chassis =
+                PilotDefinition.Chassis as MultiWheelChassis;
+            if (chassis == null)
+            {
+                throw new InvalidOperationException(
+                    "当前底盘不是MultiWheelChassis，无法执行舵轮回正。");
+            }
+
+            var adapter = new MultiWheelChassisAdapter(
+                chassis,
+                PilotDefinition.Self.CarNum);
+            var toleranceRadians =
+                ToleranceDegrees * Math.PI / 180.0;
+            var startTime = DateTime.UtcNow;
+            DateTime? alignedSince = null;
+
+            Completed = false;
+            if (!adapter.PrepareParallelDirection(0.0))
+            {
+                throw new InvalidOperationException(
+                    "无法将所有舵轮下发到车体前向0°。");
+            }
+
+            try
+            {
+                while (true)
+                {
+                    var aligned =
+                        adapter.AreParallelWheelsAligned(
+                            0.0,
+                            toleranceRadians);
+
+                    if (aligned)
+                    {
+                        if (!alignedSince.HasValue)
+                            alignedSince = DateTime.UtcNow;
+
+                        if ((DateTime.UtcNow -
+                             alignedSince.Value).TotalSeconds >=
+                            StableSeconds)
+                        {
+                            Completed = true;
+                            yield break;
+                        }
+                    }
+                    else
+                    {
+                        alignedSince = null;
+                    }
+
+                    if (TimeoutSeconds > 0f &&
+                        (DateTime.UtcNow - startTime).TotalSeconds >
+                        TimeoutSeconds)
+                    {
+                        throw new TimeoutException(
+                            $"舵轮回正超过{TimeoutSeconds:F1}s，" +
+                            "测试已经取消。");
+                    }
+
+                    yield return true;
+                }
+            }
+            finally
+            {
+                // 只清零驱动速度，保留已经下发的0°舵角。
+                adapter.StopImmediately();
+            }
+        }
+    }
+
+    #region 功能项
+    public class Sleep : MovementDefinition
+    {
+        public float Second = 2f;
+
+        public override IEnumerable<bool> Get()
+        {
+            if (Second <= 0)
+            {
+                yield return false;
+                yield break;
+            }
+
+            var endTime = DateTime.UtcNow.AddSeconds(Second);
+            while (DateTime.UtcNow < endTime)
+            {
+                Thread.Sleep(50);
+                yield return true;
+            }
+
+            yield return false;
+        }
+    }
+    public class DriverAble : MovementDefinition
+    {
+        public int WaitTimeoutMs = 2000;
+        public int PollIntervalMs = 50;
+
+        // C层单车硬件：请求全部驱动轮复位并恢复使能。
+        public override IEnumerable<bool> Get()
+        {
+            PilotDefinition.Self.ResetFromC = true;
+
+            try
+            {
+                var start = DateTime.Now;
+                var timeoutMs = Math.Max(0, WaitTimeoutMs);
+                var pollMs = Math.Max(1, PollIntervalMs);
+
+                // 至少保留一个调度周期，确保M层能收到复位请求。
+                yield return true;
+
+                while (!PilotDefinition.Self.WheelAbleState &&
+                        (DateTime.Now - start).TotalMilliseconds < timeoutMs)
+                {
+                    Thread.Sleep(pollMs);
+                    yield return true;
+                }
+            }
+            finally
+            {
+                PilotDefinition.Self.ResetFromC = false;
+            }
+        }
+    }
+    public class DriverDisable : MovementDefinition
+    {
+        public int WaitTimeoutMs = 3000;
+        public int PollIntervalMs = 20;
+
+        // C层单车硬件：请求驱动轮退出使能，并等待M层状态反馈。
+        public override IEnumerable<bool> Get()
+        {
+            var timeoutMs = Math.Max(0, WaitTimeoutMs);
+            var pollMs = Math.Max(1, PollIntervalMs);
+            var startTime = DateTime.UtcNow;
+            var success = false;
+
+            PilotDefinition.Self.DisableFromC = true;
+
+            try
+            {
+                // 至少保持一个C层调度周期，确保M层能收到下使能请求。
+                yield return true;
+
+                success = !PilotDefinition.Self.WheelAbleState;
+
+                while (!success &&
+                        (DateTime.UtcNow - startTime).TotalMilliseconds <
+                            timeoutMs)
+                {
+                    Thread.Sleep(pollMs);
+
+                    success =
+                        !PilotDefinition.Self.WheelAbleState;
+
+                    if (!success)
+                    {
+                        yield return true;
+                    }
+                }
+            }
+            finally
+            {
+                // 无论正常完成、超时、异常还是任务被停止，都撤销请求。
+                PilotDefinition.Self.DisableFromC = false;
+            }
+            if (success)
+            {
+                Console.WriteLine(
+                    $"驱动器下使能完成，" +
+                    $"WheelAbleState=" +
+                    $"{PilotDefinition.Self.WheelAbleState}");
+            }
+            else
+            {
+                Console.WriteLine(
+                    $"驱动器下使能超时，" +
+                    $"WheelAbleState=" +
+                    $"{PilotDefinition.Self.WheelAbleState}，" +
+                    $"等待{timeoutMs}ms");
+            }
+            yield return false;
+        }
+    }
+    #endregion
+
+    #region 直线运动
+    //在世界坐标系下，从路径起点追踪到终点并停车
+    public class DstTracker : MovementDefinition
+    {
+        public Vector2 Src;
+        public Vector2 Dst;
+        // 本次轨迹的巡航速度上限，单位m/s。
+        public float MaxSpeed = PilotDefinition.Conf.DstTrackerMaxSpeed;
+        public float CarDirectionBias = 0f;
+        public Painter Painter = UI.GetPainter("DstTracker");
+        public override IEnumerable<bool> Get()
+        {
+            var chassis = (MultiWheelChassis)PilotDefinition.Chassis;
+            DriveTask task = null;
+            try
+            {
+                Console.WriteLine($"DstTracker src:({Src.X:F2}, {Src.Y:F2}) dst:({Dst.X:F2}, {Dst.Y:F2})");
+                Painter.DrawLine(Color.Cyan, Src.X, Src.Y, Dst.X, Dst.Y, width: 3);
+
+                var tracker = new ChassisController
+                {
+                    BaseSpeed = MaxSpeed
+                }.Get();
+                // 要求路径末端速度下降到零。
+                tracker.FinishSpeed = 0f;
+                var linePath = new LineTrack(Src, Dst)
+                {
+                    CarDirectionBias = CarDirectionBias,
+                    Speed = MaxSpeed
+                };
+                tracker.AddTrack(linePath);
+                task = new DriveTask(tracker.Track());
+                task.Wait();
+                yield return false;
+            }
+            finally
+            {
+                task?.Stop();
+                chassis.SendXYThSpeed(0f, 0f, 0f);
+            }
+        }
+
+    }
+    //直线行走基于轮里程
+    // C层单车底盘：按照车轮里程行驶指定的相对距离。
+    public class LineTracking : MovementDefinition
+    {
+        // 相对动作启动位置的行驶距离，单位mm。
+        // 正数表示前进，负数表示后退。
+        public float TargetDistance;
+        public float MaxSpeed = PilotDefinition.Conf.LineTrackMaxSpeed;
+        public float Kp = PilotDefinition.Conf.LineTrackKp;
+        public float Ki = PilotDefinition.Conf.LineTrackKi;
+        public float Kd = PilotDefinition.Conf.LineTrackKd;
+        public float DeadZone = PilotDefinition.Conf.LineTrackDeadZone;
+        public int SrcId = -1;
+        public int DstId = -1;
+        public Action<int> LeaveSrcFunction;
+        // 接近目标后是否保留速度，交给下一个动作接管。
+        public bool EnableHandover;
+        // 进入动作衔接的剩余距离，单位mm。
+        public float HandoverDistance = 80f;
+        // HandoverSpeed小于0时，使用MaxSpeed的此比例。
+        public float HandoverSpeedRatio = 0.5f;
+        // 大于等于0时，直接作为衔接速度，单位m/s。
+        public float HandoverSpeed = -1f;
+        public float MinHandoverSpeed = 0.05f;
+        private PIDController _pid;
+        // 读取当前单车直线行驶里程，单位mm。
+        private static float ReadPosition()
+        {
+            return
+                (PilotDefinition.Self.LFLActualPos + PilotDefinition.Self.LFRActualPos) / 2f;
+        }
+
+        // 根据动作启动位置和目标距离执行直线里程闭环。
+        public override IEnumerable<bool> Get()
+        {
+            if (float.IsNaN(TargetDistance) || float.IsInfinity(TargetDistance))
+            {
+                throw new ArgumentOutOfRangeException(
+                    nameof(TargetDistance),
+                    "目标行驶距离必须是有限值。");
+            }
+
+            if (float.IsNaN(MaxSpeed) || float.IsInfinity(MaxSpeed) || MaxSpeed <= 0f)
+            {
+                throw new ArgumentOutOfRangeException(
+                    nameof(MaxSpeed),
+                    "最大速度必须是大于零的有限值。");
+            }
+            var chassis = (MultiWheelChassis)PilotDefinition.Chassis;
+            // 每次启动动作时重新读取起始编码器位置。
+            var startPosition = ReadPosition();
+            // PID仍然控制绝对编码器位置，但绝对目标由动作自动计算。
+            var targetPosition = startPosition + TargetDistance;
+            _pid = new PIDController(ReadPosition, Kp, Ki, Kd, 0, DeadZone, MaxSpeed)
+            {
+                SpeedAccPerSec = Math.Abs(MaxSpeed) / 2f
+            };
+            var handoverRequested = false;
+            var keepHandoverSpeed = false;
+            DLog.Log(
+                $"直线里程动作：" +
+                $"起点={startPosition:F1}mm，" +
+                $"距离={TargetDistance:F1}mm，" +
+                $"目标={targetPosition:F1}mm",
+                "straight_line");
+            try
+            {
+                while (true)
+                {
+                    var currentPosition = ReadPosition();
+                    var remainingDistance = targetPosition - currentPosition;
+                    // 接近目标后，保留一定速度交给后续动作。
+                    if (EnableHandover && Math.Abs(remainingDistance) <= Math.Max(1f, HandoverDistance))
+                    {
+                        var direction = Math.Sign(remainingDistance);
+                        if (direction == 0)
+                        {
+                            direction = Math.Sign(TargetDistance);
+                        }
+                        var requestedSpeed = HandoverSpeed >= 0f ? Math.Abs(HandoverSpeed) : Math.Abs(MaxSpeed) * HandoverSpeedRatio;
+                        var maximumSpeed = Math.Abs(MaxSpeed);
+                        var minimumSpeed = Math.Min(Math.Abs(MinHandoverSpeed), maximumSpeed);
+                        var limitedSpeed = Math.Max(minimumSpeed, Math.Min(requestedSpeed, maximumSpeed));
+                        var handoverSpeed = limitedSpeed * direction;
+                        chassis.SendXYThSpeed(handoverSpeed, 0f, 0f);
+                        handoverRequested = true;
+                        // 保持一个调度周期，让速度命令实际生效。
+                        yield return true;
+                        break;
+                    }
+                    var speed = _pid.GetResponse(targetPosition);
+                    chassis.SendXYThSpeed(speed, 0f, 0f);
+                    if (_pid.IsArrived())
+                    {
+                        break;
+                    }
+                    yield return true;
+                }
+                if (SrcId != -1 &&
+                    LeaveSrcFunction != null)
+                {
+                    LeaveSrcFunction(SrcId);
+                    DLog.Log($"释放放车点{SrcId}", "straight_line");
+                }
+                // 只有正常完成动作衔接时才允许保留非零速度。
+                keepHandoverSpeed = handoverRequested;
+            }
+            finally
+            {
+                // 普通完成、人工停止或异常退出时都必须停车。
+                if (!keepHandoverSpeed)
+                {
+                    chassis.SendXYThSpeed(0f, 0f, 0f);
+                }
+            }
+            yield return false;
+        }
+    }
+    //直线行走基于detour
+    public class LineTracking_based_detour : MovementDefinition
+    {
+        public float LineDistance = 1000f;
+        public int SrcId = -1;
+        public int DstId = -1;
+        public Action<int> LeaveSrcFunction = null;
+        public Painter painter = UI.GetPainter("Line", false);
+        // C层单车轨迹：执行早期版本的两点直线跟踪动作。
+        public override IEnumerable<bool> Get()
+        {
+            var curpose = DetourInterface.getCartLocation();
+            Console.WriteLine($"curpose.th:{curpose.th}");
+            var src = new Vector2((float)curpose.x, (float)curpose.y);
+            var headingRadians = curpose.th * Math.PI / 180.0;
+            var dst = new Vector2(
+                (float)(curpose.x +
+                    LineDistance * Math.Cos(headingRadians)),
+                (float)(curpose.y +
+                    LineDistance * Math.Sin(headingRadians)));
+            // var dst = new Vector2((float)curpose.x + LineDistance * (float)Math.Cos(curpose.th),
+            //                       (float)curpose.y + LineDistance * (float)Math.Sin(curpose.th));
+            Console.WriteLine($"src:{src.X} {src.Y}");
+            Console.WriteLine($"dst:{dst.X} {dst.Y}");
+            painter.DrawLine(Color.Green, src.X, src.Y, dst.X, dst.Y, width: 3);
+
+            var tracker = new ChassisController().Get();
+            var linePath = new LineTrack(src, dst) { CarDirectionBias = LineDistance > 0 ? 0 : 180 };
+            tracker.AddTrack(linePath);
+            var _dt = new DriveTask(tracker.Track());
+            _dt.Wait();
+            if (SrcId != -1 && LeaveSrcFunction != null)
+            {
+                LeaveSrcFunction(SrcId);
+                DLog.Log($"释放放车点{SrcId}", "straight_line");
+            }
+            yield return false;
+        }
+    }
+    #endregion
+
+    #region 旋转运动
+    public class MultiWheelRotateInPlace : MovementDefinition
+    {
+        /// <summary>
+        /// 旋转目标角度
+        /// </summary>
+        public float AngleTarget;
+
+        public float MaxSpeed;
+
+        public Func<float> ThetaReader = () => (float)DetourInterface.getCartLocation().th;
+
+        public MultiWheelChassis Chassis = (MultiWheelChassis)PilotDefinition.Chassis;
+
+        public Func<PIDParams> PidparamsRead = () => new PIDParams() { };
+
+        public PIDController thPid;
+
+        // 将本周期PID角速度输出提供给实验记录器，单位deg/s。
+        public Action<float> CommandAngularSpeedObserver;
+
+        // 归一化到大约 [-180°, 180°]
+        private static float RangeAngle(float theta)
+        {
+            return (float)(theta - Math.Round(theta / 360.0f) * 360);
+        }
+
+        // 使用 PID 控制原地旋转到目标角度。
+        public override IEnumerable<bool> Get()
+        {
+            try
+            {
+                var targetAngle = RangeAngle(AngleTarget);
+                var p = PidparamsRead();
+                thPid = new PIDController(ThetaReader, p.Kp);
+                thPid.ChangeParameters(p.Kp, p.Ki, p.Kd, p.MaxI, p.DeadZone,
+                    p.OutputUpperThreshold, p.SpeedAccPerSec);
+
+                while (true)
+                {
+                    var s = thPid.GetResponse(targetAngle, true);
+                    Console.WriteLine($"s:{s} AngleTarget:{AngleTarget}");
+                    CommandAngularSpeedObserver?.Invoke(s);
+                    Chassis.SendXYThSpeed(0, 0, s);
+                    if (thPid.IsArrived()) break;
+                    yield return true;
+                }
+
+                Console.WriteLine($"final rotate to {targetAngle}");
+            }
+            finally
+            {
+                CommandAngularSpeedObserver?.Invoke(0f);
+                Chassis.SendXYThSpeed(0, 0, 0);
+            }
+        }
+    }
+    #endregion
+
+    #region 夹臂运动
+    public class ClampToTarget : MovementDefinition
+    {
+        public float LeftClampTarget;
+        public float RightClampTarget;
+        public float MaxClampSpeed = PilotDefinition.Conf.MaxClampSpeed;
+        public float ClampKp = PilotDefinition.Conf.ClampControlKp;
+        public float ClampKi = PilotDefinition.Conf.ClampControlKi;
+        public float ClampKd = PilotDefinition.Conf.ClampControlKd;
+        public float ClampMaxI = PilotDefinition.Conf.ClampControlMaxI;
+        public float ClampSpeedAcc = PilotDefinition.Conf.ClampControlSpeedAcc;
+        public float ClampDeadZone = PilotDefinition.Conf.ClampControlDeadZone;
+        public float TimeoutSeconds = 30f;
+        private PIDController leftpid, rightpid;
+
+        // C层单车业务：驱动左右夹臂运动到夹紧或松开目标。
+        public override IEnumerable<bool> Get()
+        {
+            try
+            {
+                leftpid = new PIDController(
+                    () => PilotDefinition.Self.ActualPosLeftArm,
+                    ClampKp, ClampKi, ClampKd, ClampMaxI,
+                    ClampDeadZone, MaxClampSpeed)
+                {
+                    SpeedAccPerSec = ClampSpeedAcc
+                };
+
+                rightpid = new PIDController(
+                    () => PilotDefinition.Self.ActualPosRightArm,
+                    ClampKp, ClampKi, ClampKd, ClampMaxI,
+                    ClampDeadZone, MaxClampSpeed)
+                {
+                    SpeedAccPerSec = ClampSpeedAcc
+                };
+
+                var startTime = DateTime.UtcNow;
+                while (true)
+                {
+                    if (TimeoutSeconds > 0f &&
+                        (DateTime.UtcNow - startTime).TotalSeconds >
+                        TimeoutSeconds)
+                    {
+                        Console.WriteLine(
+                            $"夹臂运动超时({TimeoutSeconds:F1}s)，" +
+                            "停止左右夹臂。");
+                        yield break;
+                    }
+
+                    var leftspeed =
+                        leftpid.GetResponse(LeftClampTarget);
+                    var rightspeed =
+                        rightpid.GetResponse(RightClampTarget);
+                    Console.WriteLine(
+                        $"left arm speed:{leftspeed}  " +
+                        $"right arm speed:{rightspeed}");
+
+                    PilotDefinition.Self.SpeedLeftArm = leftspeed;
+                    PilotDefinition.Self.SpeedRightArm = rightspeed;
+
+                    var leftArrived = leftpid.IsArrived();
+                    var rightArrived = rightpid.IsArrived();
+                    if (leftArrived)
+                        PilotDefinition.Self.SpeedLeftArm = 0f;
+                    if (rightArrived)
+                        PilotDefinition.Self.SpeedRightArm = 0f;
+
+                    if (leftArrived && rightArrived)
+                        break;
+
+                    yield return true;
+                }
+
+                Console.WriteLine(
+                    $"left clamp to target:{LeftClampTarget}  " +
+                    $"right clamp to target:{RightClampTarget}");
+            }
+            finally
+            {
+                PilotDefinition.Self.SpeedLeftArm = 0f;
+                PilotDefinition.Self.SpeedRightArm = 0f;
+            }
+        }
+    }
+    #endregion
+}

ClumsyPilot/PilotConfig.cs

@@ -4,12 +4,9 @@ using Newtonsoft.Json;
 
 namespace MultiWheelC;
 
-/// <summary>
-/// 定义由MDCS显示、持久化并随车辆部署的运行参数。
-/// </summary>
-public partial class PilotConfig : MultiWheelPilotConfig
+public class PilotConfig : MultiWheelPilotConfig
 {
-    #region 单车-轨迹跟踪 LineTracking
+    #region 单车-轨迹跟踪
 
     [FieldMember(desc = "直线行走距离")] public float LineTrackDistance = 1000f;
     [FieldMember(desc = "直线行走最大速度")] public float LineTrackMaxSpeed = 0.3f;
@@ -21,6 +18,50 @@ public partial class PilotConfig : MultiWheelPilotConfig
     [FieldMember(desc = "终点跟踪:速度")] public float DstTrackerMaxSpeed = 0.3f;
     #endregion
 
+    #region 单车-原地旋转
+    [FieldMember(desc = "原地旋转：目标朝向(世界坐标系, deg)")]
+    public float InPlaceRotateTargetWorldDeg = 90f;
+
+    [FieldMember(desc = "原地旋转：旋转角速度(deg/s)")]
+    public float InPlaceRotateSpeed = 30f;
+
+    [FieldMember(desc = "原地旋转：到位角度精度(deg)")]
+    public float InPlaceRotateArriveDeg = 1f;
+
+    [FieldMember(desc = "原地旋转：起转前舵轮对齐精度(deg)")]
+    public float InPlaceRotateWheelAlignDeg = 2f;
+
+    [FieldMember(desc = "原地旋转：旋转过程中舵轮偏差重对齐阈值(deg)")]
+    public float InPlaceRotateActiveWheelAlignDeg = 10f;
+
+    #endregion
+
+    #region 单车-临时
+    [FieldMember(desc = "原地旋转Kp")]
+    public float InPlaceRotateKp = 0.2f;
+
+    [FieldMember(desc = "原地旋转Ki")]
+    public float InPlaceRotateKi = 0.01f;
+
+    [FieldMember(desc = "原地旋转Kd")]
+    public float InPlaceRotateKd = 0f;
+
+    [FieldMember(desc = "原地旋转积分限幅")]
+    public float InPlaceRotateMaxI = 0.01f;
+
+    [FieldMember(desc = "原地旋转最大角速度(deg/s)")]
+    public float InPlaceRotateMaxSpeed = 30f;
+
+    [FieldMember(desc = "原地旋转角加速度(deg/s²)")]
+    public float InPlaceRotateAcc = 30f;
+
+    [FieldMember(desc = "原地旋转超时(s)")]
+    public float InPlaceRotateTimeoutSec = 15f;
+    #endregion
+
+
+
+
     #region 单车-钻车与夹抱
     [FieldMember(desc = "2腿检测：雷达名（逗号分隔可多个）")]
     public string TwoLegLidarName = "rear_left_lidar_1,rear_right_lidar_1";

ClumsyPilot/PilotDefinition.cs

@@ -33,10 +33,10 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
     [AsLowerIO(desc = "右后左轮实际位置")] public float RRLActualPos;
     [AsLowerIO(desc = "右后右轮实际位置")] public float RRRActualPos;
 
-    [AsLowerIO(desc = "左夹臂低限位")] public int LeftArmLowerPos;
-    [AsLowerIO(desc = "左夹臂高限位")] public int LeftArmUpperPos;
-    [AsLowerIO(desc = "右夹臂低限位")] public int RightArmLowerPos;
-    [AsLowerIO(desc = "右夹臂高限位")] public int RightArmUpperPos;
+    [AsLowerIO(desc = "左夹臂低限位")] public float LeftArmLowerPos;
+    [AsLowerIO(desc = "左夹臂高限位")] public float LeftArmUpperPos;
+    [AsLowerIO(desc = "右夹臂低限位")] public float RightArmLowerPos;
+    [AsLowerIO(desc = "右夹臂高限位")] public float RightArmUpperPos;
 
     [AsUpperIO(desc = "从C往驱动器下使能")] public bool DisableFromC = false;
     [AsUpperIO(desc = "从C上复位")] public bool ResetFromC = false;

ClumsyPilot/TrackingExperimentRecorder.cs

@@ -0,0 +1,394 @@
+using ClumsyCore.Interfaces;
+using System;
+using System.Collections.Generic;
+using System.Diagnostics;
+using System.Globalization;
+using System.IO;
+using System.Numerics;
+using System.Text;
+using System.Threading;
+
+namespace MultiWheelC
+{
+    // C层实验数据：保存一个采样时刻的定位与控制命令。
+    public sealed class TrackingSample
+    {
+        public double ElapsedSeconds;
+
+        // Detour位置单位为mm，航向单位为deg。
+        public double DetourX;
+        public double DetourY;
+        public double DetourTheta;
+
+        // 车体速度单位为m/s，角速度单位为deg/s。
+        public float CommandSpeed;
+        public float CommandVx;
+        public float CommandVy;
+        public float CommandAngularSpeed;
+    }
+
+    // C层实验工具：统一采集并保存轨迹跟踪实验数据。
+    public sealed class TrackingExperimentRecorder
+    {
+        private readonly string _controllerName;
+        private readonly string _trajectoryName;
+        private readonly int _trialNumber;
+        private readonly Vector2 _referenceStart;
+        private readonly Vector2 _referenceEnd;
+        private readonly float _referenceSpeed;
+        private readonly int _sampleIntervalMs;
+
+        private readonly List<TrackingSample> _samples =
+            new List<TrackingSample>();
+
+        private readonly object _sampleSyncRoot =
+            new object();
+
+        private readonly object _commandSyncRoot =
+            new object();
+
+        private readonly Stopwatch _stopwatch =
+            new Stopwatch();
+
+        private Thread _worker;
+        private volatile bool _running;
+        private int _started;
+        private int _saved;
+
+        private bool _hasExternalCommand;
+        private float _externalCommandSpeed;
+        private float _externalCommandVx;
+        private float _externalCommandVy;
+        private float _externalCommandAngularSpeed;
+
+        public TrackingExperimentRecorder(
+            string controllerName,
+            string trajectoryName,
+            int trialNumber,
+            Vector2 referenceStart,
+            Vector2 referenceEnd,
+            float referenceSpeed,
+            int sampleIntervalMs = 50)
+        {
+            if (string.IsNullOrWhiteSpace(controllerName))
+                throw new ArgumentException(
+                    "控制器名称不能为空。",
+                    nameof(controllerName));
+
+            if (string.IsNullOrWhiteSpace(trajectoryName))
+                throw new ArgumentException(
+                    "轨迹名称不能为空。",
+                    nameof(trajectoryName));
+
+            if (sampleIntervalMs <= 0)
+                throw new ArgumentOutOfRangeException(
+                    nameof(sampleIntervalMs),
+                    "采样周期必须大于零。");
+
+            _controllerName = controllerName;
+            _trajectoryName = trajectoryName;
+            _trialNumber = trialNumber;
+            _referenceStart = referenceStart;
+            _referenceEnd = referenceEnd;
+            _referenceSpeed = referenceSpeed;
+            _sampleIntervalMs = sampleIntervalMs;
+        }
+
+        // 保存成功后的CSV绝对路径；尚未保存时为空。
+        public string SavedFilePath { get; private set; }
+
+        // 启动后台采样线程。
+        public void Start()
+        {
+            if (Interlocked.Exchange(ref _started, 1) != 0)
+                return;
+
+            _stopwatch.Restart();
+            _running = true;
+
+            // 立即保存起点静止状态，避免第一帧被后台线程延迟。
+            CaptureSample();
+
+            _worker = new Thread(SamplingLoop)
+            {
+                IsBackground = true,
+                Name = "TrackingExperimentRecorder"
+            };
+            _worker.Start();
+        }
+
+        // 供Stanley/LQR控制器主动写入本周期最终速度命令。
+        // 调用后优先记录该命令，不再使用底盘反解值。
+        public void UpdateCommand(
+            float commandSpeed,
+            float commandAngularSpeed)
+        {
+            lock (_commandSyncRoot)
+            {
+                _externalCommandSpeed = commandSpeed;
+                _externalCommandVx = commandSpeed;
+                _externalCommandVy = 0f;
+                _externalCommandAngularSpeed =
+                    commandAngularSpeed;
+                _hasExternalCommand = true;
+            }
+        }
+
+        // 供全向、蟹行和曲线控制器写入完整车体速度命令。
+        public void UpdateBodyCommand(
+            float commandVx,
+            float commandVy,
+            float commandAngularSpeed)
+        {
+            lock (_commandSyncRoot)
+            {
+                _externalCommandVx = commandVx;
+                _externalCommandVy = commandVy;
+                _externalCommandSpeed =
+                    (float)Math.Sqrt(
+                        commandVx * commandVx +
+                        commandVy * commandVy);
+                _externalCommandAngularSpeed =
+                    commandAngularSpeed;
+                _hasExternalCommand = true;
+            }
+        }
+
+        // 停止采样并将本次实验保存为CSV；重复调用只保存一次。
+        public void StopAndSave()
+        {
+            if (Volatile.Read(ref _started) == 0)
+                return;
+
+            if (Interlocked.Exchange(ref _saved, 1) != 0)
+                return;
+
+            try
+            {
+                _running = false;
+
+                if (_worker != null &&
+                    _worker != Thread.CurrentThread)
+                {
+                    _worker.Join(
+                        Math.Max(1000, _sampleIntervalMs * 4));
+                }
+
+                // 保存停止时刻的最后一帧。
+                CaptureSample();
+                _stopwatch.Stop();
+                SaveCsv();
+
+                Console.WriteLine(
+                    $"轨迹实验数据已保存：{SavedFilePath}");
+            }
+            catch
+            {
+                // 保存失败后允许调用者再次尝试。
+                Interlocked.Exchange(ref _saved, 0);
+                throw;
+            }
+        }
+
+        // 按固定周期采集Detour位姿和控制命令。
+        private void SamplingLoop()
+        {
+            while (_running)
+            {
+                Thread.Sleep(_sampleIntervalMs);
+
+                if (!_running)
+                    break;
+
+                CaptureSample();
+            }
+        }
+
+        // 采集一帧Detour位姿和控制命令。
+        private void CaptureSample()
+        {
+            try
+            {
+                var location =
+                    DetourInterface.getCartLocation();
+
+                float commandSpeed;
+                float commandVx;
+                float commandVy;
+                float commandAngularSpeed;
+
+                lock (_commandSyncRoot)
+                {
+                    if (_hasExternalCommand)
+                    {
+                        commandSpeed =
+                            _externalCommandSpeed;
+                        commandVx =
+                            _externalCommandVx;
+                        commandVy =
+                            _externalCommandVy;
+                        commandAngularSpeed =
+                            _externalCommandAngularSpeed;
+                    }
+                    else
+                    {
+                        var command =
+                            PilotDefinition.Chassis
+                                .GetCarSpeed(false);
+
+                        commandVx = command.Vx;
+                        commandVy = command.Vy;
+                        commandAngularSpeed = command.Vw;
+                        commandSpeed = (float)Math.Sqrt(
+                            commandVx * commandVx +
+                            commandVy * commandVy);
+                    }
+                }
+
+                var sample = new TrackingSample
+                {
+                    ElapsedSeconds =
+                        _stopwatch.Elapsed.TotalSeconds,
+                    DetourX = location.x,
+                    DetourY = location.y,
+                    DetourTheta = location.th,
+                    CommandSpeed = commandSpeed,
+                    CommandVx = commandVx,
+                    CommandVy = commandVy,
+                    CommandAngularSpeed =
+                        commandAngularSpeed
+                };
+
+                lock (_sampleSyncRoot)
+                {
+                    _samples.Add(sample);
+                }
+            }
+            catch (Exception ex)
+            {
+                // 单帧读取失败不应终止车辆控制或整个记录线程。
+                Console.WriteLine(
+                    $"轨迹实验采样失败：{ex.Message}");
+            }
+        }
+
+        // 将内存中的采样数据写入CSV。
+        private void SaveCsv()
+        {
+            List<TrackingSample> snapshot;
+
+            lock (_sampleSyncRoot)
+            {
+                snapshot =
+                    new List<TrackingSample>(_samples);
+            }
+
+            var outputDirectory = Path.Combine(
+                AppContext.BaseDirectory,
+                "TrackingExperiments");
+
+            Directory.CreateDirectory(outputDirectory);
+
+            var fileName =
+                $"{DateTime.Now:yyyyMMdd_HHmmss_fff}_" +
+                $"{SanitizeFileName(_controllerName)}_" +
+                $"{SanitizeFileName(_trajectoryName)}_" +
+                $"Trial{_trialNumber}.csv";
+
+            SavedFilePath = Path.Combine(
+                outputDirectory,
+                fileName);
+
+            using (var writer = new StreamWriter(
+                SavedFilePath,
+                false,
+                new UTF8Encoding(true)))
+            {
+                writer.WriteLine(
+                    "ElapsedSeconds," +
+                    "ControllerName," +
+                    "TrajectoryName," +
+                    "TrialNumber," +
+                    "DetourX," +
+                    "DetourY," +
+                    "DetourTheta," +
+                    "CommandSpeed," +
+                    "CommandAngularSpeed," +
+                    "CommandVx," +
+                    "CommandVy," +
+                    "ReferenceStartX," +
+                    "ReferenceStartY," +
+                    "ReferenceEndX," +
+                    "ReferenceEndY," +
+                    "ReferenceSpeed");
+
+                foreach (var sample in snapshot)
+                {
+                    writer.WriteLine(string.Join(
+                        ",",
+                        Format(sample.ElapsedSeconds),
+                        EscapeCsv(_controllerName),
+                        EscapeCsv(_trajectoryName),
+                        _trialNumber.ToString(
+                            CultureInfo.InvariantCulture),
+                        Format(sample.DetourX),
+                        Format(sample.DetourY),
+                        Format(sample.DetourTheta),
+                        Format(sample.CommandSpeed),
+                        Format(sample.CommandAngularSpeed),
+                        Format(sample.CommandVx),
+                        Format(sample.CommandVy),
+                        Format(_referenceStart.X),
+                        Format(_referenceStart.Y),
+                        Format(_referenceEnd.X),
+                        Format(_referenceEnd.Y),
+                        Format(_referenceSpeed)));
+                }
+            }
+        }
+
+        // 将文件名中的非法字符替换为下划线。
+        private static string SanitizeFileName(string value)
+        {
+            var result = value;
+
+            foreach (var invalidCharacter in
+                     Path.GetInvalidFileNameChars())
+            {
+                result = result.Replace(
+                    invalidCharacter,
+                    '_');
+            }
+
+            return result;
+        }
+
+        // 按固定小数格式输出数值，避免系统区域设置改变CSV格式。
+        private static string Format(double value)
+        {
+            return value.ToString(
+                "0.######",
+                CultureInfo.InvariantCulture);
+        }
+
+        // 对CSV文本字段进行引号和逗号转义。
+        private static string EscapeCsv(string value)
+        {
+            if (value == null)
+                return string.Empty;
+
+            if (!value.Contains(",") &&
+                !value.Contains("\"") &&
+                !value.Contains("\r") &&
+                !value.Contains("\n"))
+            {
+                return value;
+            }
+
+            return
+                "\"" +
+                value.Replace("\"", "\"\"") +
+                "\"";
+        }
+    }
+}

ClumsyPilot/build/Clumsy/ClumsyPilot.deps.json

@@ -7,7 +7,7 @@
   "targets": {
     ".NETStandard,Version=v2.0": {},
     ".NETStandard,Version=v2.0/": {
-      "MultiWheelC/1.0.0": {
+      "ClumsyPilot/1.0.0": {
         "dependencies": {
           "NETStandard.Library": "2.0.3",
           "Newtonsoft.Json": "13.0.3",
@@ -20,7 +20,7 @@
           "RefFundamentalLib": "0.0.0.0"
         },
         "runtime": {
-          "MultiWheelC.dll": {}
+          "ClumsyPilot.dll": {}
         }
       },
       "Microsoft.NETCore.Platforms/1.1.0": {},
@@ -96,7 +96,7 @@
     }
   },
   "libraries": {
-    "MultiWheelC/1.0.0": {
+    "ClumsyPilot/1.0.0": {
       "type": "project",
       "serviceable": false,
       "sha512": ""

ClumsyPilot/obj/ClumsyPilot.csproj.nuget.dgspec.json

@@ -0,0 +1,84 @@
+{
+  "format": 1,
+  "restore": {
+    "D:\\Users\\Desktop\\入职培训\\停车机器人\\MyParking\\ClumsyPilot\\ClumsyPilot.csproj": {}
+  },
+  "projects": {
+    "D:\\Users\\Desktop\\入职培训\\停车机器人\\MyParking\\ClumsyPilot\\ClumsyPilot.csproj": {
+      "version": "1.0.0",
+      "restore": {
+        "projectUniqueName": "D:\\Users\\Desktop\\入职培训\\停车机器人\\MyParking\\ClumsyPilot\\ClumsyPilot.csproj",
+        "projectName": "ClumsyPilot",
+        "projectPath": "D:\\Users\\Desktop\\入职培训\\停车机器人\\MyParking\\ClumsyPilot\\ClumsyPilot.csproj",
+        "packagesPath": "C:\\Users\\admin\\.nuget\\packages\\",
+        "outputPath": "D:\\Users\\Desktop\\入职培训\\停车机器人\\MyParking\\ClumsyPilot\\obj\\",
+        "projectStyle": "PackageReference",
+        "fallbackFolders": [
+          "C:\\Program Files (x86)\\Microsoft Visual Studio\\Shared\\NuGetPackages"
+        ],
+        "configFilePaths": [
+          "C:\\Users\\admin\\AppData\\Roaming\\NuGet\\NuGet.Config",
+          "C:\\Program Files (x86)\\NuGet\\Config\\Microsoft.VisualStudio.FallbackLocation.config",
+          "C:\\Program Files (x86)\\NuGet\\Config\\Microsoft.VisualStudio.Offline.config"
+        ],
+        "originalTargetFrameworks": [
+          "netstandard2.0"
+        ],
+        "sources": {
+          "C:\\Program Files (x86)\\Microsoft SDKs\\NuGetPackages\\": {},
+          "https://api.nuget.org/v3/index.json": {}
+        },
+        "frameworks": {
+          "netstandard2.0": {
+            "targetAlias": "netstandard2.0",
+            "projectReferences": {}
+          }
+        },
+        "warningProperties": {
+          "warnAsError": [
+            "NU1605"
+          ]
+        },
+        "restoreAuditProperties": {
+          "enableAudit": "true",
+          "auditLevel": "low",
+          "auditMode": "direct"
+        },
+        "SdkAnalysisLevel": "9.0.300"
+      },
+      "frameworks": {
+        "netstandard2.0": {
+          "targetAlias": "netstandard2.0",
+          "dependencies": {
+            "NETStandard.Library": {
+              "suppressParent": "All",
+              "target": "Package",
+              "version": "[2.0.3, )",
+              "autoReferenced": true
+            },
+            "Newtonsoft.Json": {
+              "target": "Package",
+              "version": "[13.0.3, )"
+            },
+            "System.Numerics.Vectors": {
+              "target": "Package",
+              "version": "[4.6.1, )"
+            }
+          },
+          "imports": [
+            "net461",
+            "net462",
+            "net47",
+            "net471",
+            "net472",
+            "net48",
+            "net481"
+          ],
+          "assetTargetFallback": true,
+          "warn": true,
+          "runtimeIdentifierGraphPath": "C:\\Program Files\\dotnet\\sdk\\9.0.316\\RuntimeIdentifierGraph.json"
+        }
+      }
+    }
+  }
+}

ClumsyPilot/obj/ClumsyPilot.csproj.nuget.g.props

@@ -0,0 +1,16 @@
+<?xml version="1.0" encoding="utf-8" standalone="no"?>
+<Project ToolsVersion="14.0" xmlns="http://schemas.microsoft.com/developer/msbuild/2003">
+  <PropertyGroup Condition=" '$(ExcludeRestorePackageImports)' != 'true' ">
+    <RestoreSuccess Condition=" '$(RestoreSuccess)' == '' ">True</RestoreSuccess>
+    <RestoreTool Condition=" '$(RestoreTool)' == '' ">NuGet</RestoreTool>
+    <ProjectAssetsFile Condition=" '$(ProjectAssetsFile)' == '' ">$(MSBuildThisFileDirectory)project.assets.json</ProjectAssetsFile>
+    <NuGetPackageRoot Condition=" '$(NuGetPackageRoot)' == '' ">$(UserProfile)\.nuget\packages\</NuGetPackageRoot>
+    <NuGetPackageFolders Condition=" '$(NuGetPackageFolders)' == '' ">C:\Users\admin\.nuget\packages\;C:\Program Files (x86)\Microsoft Visual Studio\Shared\NuGetPackages</NuGetPackageFolders>
+    <NuGetProjectStyle Condition=" '$(NuGetProjectStyle)' == '' ">PackageReference</NuGetProjectStyle>
+    <NuGetToolVersion Condition=" '$(NuGetToolVersion)' == '' ">6.14.3</NuGetToolVersion>
+  </PropertyGroup>
+  <ItemGroup Condition=" '$(ExcludeRestorePackageImports)' != 'true' ">
+    <SourceRoot Include="C:\Users\admin\.nuget\packages\" />
+    <SourceRoot Include="C:\Program Files (x86)\Microsoft Visual Studio\Shared\NuGetPackages\" />
+  </ItemGroup>
+</Project>

ClumsyPilot/obj/ClumsyPilot.csproj.nuget.g.targets

@@ -0,0 +1,6 @@
+<?xml version="1.0" encoding="utf-8" standalone="no"?>
+<Project ToolsVersion="14.0" xmlns="http://schemas.microsoft.com/developer/msbuild/2003">
+  <ImportGroup Condition=" '$(ExcludeRestorePackageImports)' != 'true' ">
+    <Import Project="$(NuGetPackageRoot)netstandard.library\2.0.3\build\netstandard2.0\NETStandard.Library.targets" Condition="Exists('$(NuGetPackageRoot)netstandard.library\2.0.3\build\netstandard2.0\NETStandard.Library.targets')" />
+  </ImportGroup>
+</Project>

ClumsyPilot/obj/Debug/.NETStandard,Version=v2.0.AssemblyAttributes.cs

@@ -0,0 +1,4 @@
+// <autogenerated />
+using System;
+using System.Reflection;
+[assembly: global::System.Runtime.Versioning.TargetFrameworkAttribute(".NETStandard,Version=v2.0", FrameworkDisplayName = ".NET Standard 2.0")]

ClumsyPilot/obj/Debug/ClumsyPilot.AssemblyInfo.cs

@@ -0,0 +1,22 @@
+//------------------------------------------------------------------------------
+// <auto-generated>
+//     This code was generated by a tool.
+//
+//     Changes to this file may cause incorrect behavior and will be lost if
+//     the code is regenerated.
+// </auto-generated>
+//------------------------------------------------------------------------------
+
+using System;
+using System.Reflection;
+
+[assembly: System.Reflection.AssemblyCompanyAttribute("ClumsyPilot")]
+[assembly: System.Reflection.AssemblyConfigurationAttribute("Debug")]
+[assembly: System.Reflection.AssemblyFileVersionAttribute("1.0.0.0")]
+[assembly: System.Reflection.AssemblyInformationalVersionAttribute("1.0.0")]
+[assembly: System.Reflection.AssemblyProductAttribute("ClumsyPilot")]
+[assembly: System.Reflection.AssemblyTitleAttribute("ClumsyPilot")]
+[assembly: System.Reflection.AssemblyVersionAttribute("1.0.0.0")]
+
+// 由 MSBuild WriteCodeFragment 类生成。
+

ClumsyPilot/obj/Debug/ClumsyPilot.AssemblyInfoInputs.cache

@@ -0,0 +1 @@
+5d77794fa0720c6591db5b06ac60427413c18989a6f7b64420ccb07d122d85bc

ClumsyPilot/obj/Debug/ClumsyPilot.GeneratedMSBuildEditorConfig.editorconfig

@@ -0,0 +1,8 @@
+is_global = true
+build_property.RootNamespace = MultiWheelC
+build_property.ProjectDir = D:\Users\Desktop\入职培训\停车机器人\MyParking\ClumsyPilot\
+build_property.EnableComHosting = 
+build_property.EnableGeneratedComInterfaceComImportInterop = 
+build_property.CsWinRTUseWindowsUIXamlProjections = false
+build_property.EffectiveAnalysisLevelStyle = 
+build_property.EnableCodeStyleSeverity = 

ClumsyPilot/obj/Debug/ClumsyPilot.csproj.CoreCompileInputs.cache

@@ -0,0 +1 @@
+e972a413d047c4137a8ce86cbff54a8d2e2558806d9d974d3d6312467ee8ba4d

ClumsyPilot/obj/Debug/ClumsyPilot.csproj.FileListAbsolute.txt

@@ -0,0 +1,34 @@
+D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\build\Clumsy\ClumsyPilot.deps.json
+D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\build\Clumsy\ClumsyPilot.dll
+D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\build\Clumsy\ClumsyPilot.pdb
+D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\build\Clumsy\CommonUsage.dll
+D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\build\Clumsy\LessokajiWeaverUtilities.dll
+D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\build\Clumsy\MDCSToolBox.dll
+D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\build\Clumsy\RefClumsyCore.dll
+D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\build\Clumsy\RefClumsyDance.dll
+D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\build\Clumsy\RefFundamentalLib.dll
+D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\obj\Debug\ClumsyPilot.csproj.AssemblyReference.cache
+D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\obj\Debug\ClumsyPilot.GeneratedMSBuildEditorConfig.editorconfig
+D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\obj\Debug\ClumsyPilot.AssemblyInfoInputs.cache
+D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\obj\Debug\ClumsyPilot.AssemblyInfo.cs
+D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\obj\Debug\ClumsyPilot.csproj.CoreCompileInputs.cache
+D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\obj\Debug\ClumsyPi.5EF10E9F.Up2Date
+D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\obj\Debug\ClumsyPilot.dll
+D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\obj\Debug\ClumsyPilot.pdb
+D:\Users\Desktop\入职培训\停车机器人\MyParking\ClumsyPilot\build\Clumsy\ClumsyPilot.deps.json
+D:\Users\Desktop\入职培训\停车机器人\MyParking\ClumsyPilot\build\Clumsy\ClumsyPilot.dll
+D:\Users\Desktop\入职培训\停车机器人\MyParking\ClumsyPilot\build\Clumsy\ClumsyPilot.pdb
+D:\Users\Desktop\入职培训\停车机器人\MyParking\ClumsyPilot\build\Clumsy\CommonUsage.dll
+D:\Users\Desktop\入职培训\停车机器人\MyParking\ClumsyPilot\build\Clumsy\LessokajiWeaverUtilities.dll
+D:\Users\Desktop\入职培训\停车机器人\MyParking\ClumsyPilot\build\Clumsy\MDCSToolBox.dll
+D:\Users\Desktop\入职培训\停车机器人\MyParking\ClumsyPilot\build\Clumsy\RefClumsyCore.dll
+D:\Users\Desktop\入职培训\停车机器人\MyParking\ClumsyPilot\build\Clumsy\RefClumsyDance.dll
+D:\Users\Desktop\入职培训\停车机器人\MyParking\ClumsyPilot\build\Clumsy\RefFundamentalLib.dll
+D:\Users\Desktop\入职培训\停车机器人\MyParking\ClumsyPilot\obj\Debug\ClumsyPilot.csproj.AssemblyReference.cache
+D:\Users\Desktop\入职培训\停车机器人\MyParking\ClumsyPilot\obj\Debug\ClumsyPilot.GeneratedMSBuildEditorConfig.editorconfig
+D:\Users\Desktop\入职培训\停车机器人\MyParking\ClumsyPilot\obj\Debug\ClumsyPilot.AssemblyInfoInputs.cache
+D:\Users\Desktop\入职培训\停车机器人\MyParking\ClumsyPilot\obj\Debug\ClumsyPilot.AssemblyInfo.cs
+D:\Users\Desktop\入职培训\停车机器人\MyParking\ClumsyPilot\obj\Debug\ClumsyPilot.csproj.CoreCompileInputs.cache
+D:\Users\Desktop\入职培训\停车机器人\MyParking\ClumsyPilot\obj\Debug\ClumsyPi.5EF10E9F.Up2Date
+D:\Users\Desktop\入职培训\停车机器人\MyParking\ClumsyPilot\obj\Debug\ClumsyPilot.dll
+D:\Users\Desktop\入职培训\停车机器人\MyParking\ClumsyPilot\obj\Debug\ClumsyPilot.pdb

ClumsyPilot/obj/project.assets.json

@@ -0,0 +1,341 @@
+{
+  "version": 3,
+  "targets": {
+    ".NETStandard,Version=v2.0": {
+      "Microsoft.NETCore.Platforms/1.1.0": {
+        "type": "package",
+        "compile": {
+          "lib/netstandard1.0/_._": {}
+        },
+        "runtime": {
+          "lib/netstandard1.0/_._": {}
+        }
+      },
+      "NETStandard.Library/2.0.3": {
+        "type": "package",
+        "dependencies": {
+          "Microsoft.NETCore.Platforms": "1.1.0"
+        },
+        "compile": {
+          "lib/netstandard1.0/_._": {}
+        },
+        "runtime": {
+          "lib/netstandard1.0/_._": {}
+        },
+        "build": {
+          "build/netstandard2.0/NETStandard.Library.targets": {}
+        }
+      },
+      "Newtonsoft.Json/13.0.3": {
+        "type": "package",
+        "compile": {
+          "lib/netstandard2.0/Newtonsoft.Json.dll": {
+            "related": ".xml"
+          }
+        },
+        "runtime": {
+          "lib/netstandard2.0/Newtonsoft.Json.dll": {
+            "related": ".xml"
+          }
+        }
+      },
+      "System.Numerics.Vectors/4.6.1": {
+        "type": "package",
+        "compile": {
+          "lib/netstandard2.0/System.Numerics.Vectors.dll": {
+            "related": ".xml"
+          }
+        },
+        "runtime": {
+          "lib/netstandard2.0/System.Numerics.Vectors.dll": {
+            "related": ".xml"
+          }
+        }
+      }
+    }
+  },
+  "libraries": {
+    "Microsoft.NETCore.Platforms/1.1.0": {
+      "sha512": "kz0PEW2lhqygehI/d6XsPCQzD7ff7gUJaVGPVETX611eadGsA3A877GdSlU0LRVMCTH/+P3o2iDTak+S08V2+A==",
+      "type": "package",
+      "path": "microsoft.netcore.platforms/1.1.0",
+      "files": [
+        ".nupkg.metadata",
+        ".signature.p7s",
+        "ThirdPartyNotices.txt",
+        "dotnet_library_license.txt",
+        "lib/netstandard1.0/_._",
+        "microsoft.netcore.platforms.1.1.0.nupkg.sha512",
+        "microsoft.netcore.platforms.nuspec",
+        "runtime.json"
+      ]
+    },
+    "NETStandard.Library/2.0.3": {
+      "sha512": "st47PosZSHrjECdjeIzZQbzivYBJFv6P2nv4cj2ypdI204DO+vZ7l5raGMiX4eXMJ53RfOIg+/s4DHVZ54Nu2A==",
+      "type": "package",
+      "path": "netstandard.library/2.0.3",
+      "files": [
+        ".nupkg.metadata",
+        ".signature.p7s",
+        "LICENSE.TXT",
+        "THIRD-PARTY-NOTICES.TXT",
+        "build/netstandard2.0/NETStandard.Library.targets",
+        "build/netstandard2.0/ref/Microsoft.Win32.Primitives.dll",
+        "build/netstandard2.0/ref/System.AppContext.dll",
+        "build/netstandard2.0/ref/System.Collections.Concurrent.dll",
+        "build/netstandard2.0/ref/System.Collections.NonGeneric.dll",
+        "build/netstandard2.0/ref/System.Collections.Specialized.dll",
+        "build/netstandard2.0/ref/System.Collections.dll",
+        "build/netstandard2.0/ref/System.ComponentModel.Composition.dll",
+        "build/netstandard2.0/ref/System.ComponentModel.EventBasedAsync.dll",
+        "build/netstandard2.0/ref/System.ComponentModel.Primitives.dll",
+        "build/netstandard2.0/ref/System.ComponentModel.TypeConverter.dll",
+        "build/netstandard2.0/ref/System.ComponentModel.dll",
+        "build/netstandard2.0/ref/System.Console.dll",
+        "build/netstandard2.0/ref/System.Core.dll",
+        "build/netstandard2.0/ref/System.Data.Common.dll",
+        "build/netstandard2.0/ref/System.Data.dll",
+        "build/netstandard2.0/ref/System.Diagnostics.Contracts.dll",
+        "build/netstandard2.0/ref/System.Diagnostics.Debug.dll",
+        "build/netstandard2.0/ref/System.Diagnostics.FileVersionInfo.dll",
+        "build/netstandard2.0/ref/System.Diagnostics.Process.dll",
+        "build/netstandard2.0/ref/System.Diagnostics.StackTrace.dll",
+        "build/netstandard2.0/ref/System.Diagnostics.TextWriterTraceListener.dll",
+        "build/netstandard2.0/ref/System.Diagnostics.Tools.dll",
+        "build/netstandard2.0/ref/System.Diagnostics.TraceSource.dll",
+        "build/netstandard2.0/ref/System.Diagnostics.Tracing.dll",
+        "build/netstandard2.0/ref/System.Drawing.Primitives.dll",
+        "build/netstandard2.0/ref/System.Drawing.dll",
+        "build/netstandard2.0/ref/System.Dynamic.Runtime.dll",
+        "build/netstandard2.0/ref/System.Globalization.Calendars.dll",
+        "build/netstandard2.0/ref/System.Globalization.Extensions.dll",
+        "build/netstandard2.0/ref/System.Globalization.dll",
+        "build/netstandard2.0/ref/System.IO.Compression.FileSystem.dll",
+        "build/netstandard2.0/ref/System.IO.Compression.ZipFile.dll",
+        "build/netstandard2.0/ref/System.IO.Compression.dll",
+        "build/netstandard2.0/ref/System.IO.FileSystem.DriveInfo.dll",
+        "build/netstandard2.0/ref/System.IO.FileSystem.Primitives.dll",
+        "build/netstandard2.0/ref/System.IO.FileSystem.Watcher.dll",
+        "build/netstandard2.0/ref/System.IO.FileSystem.dll",
+        "build/netstandard2.0/ref/System.IO.IsolatedStorage.dll",
+        "build/netstandard2.0/ref/System.IO.MemoryMappedFiles.dll",
+        "build/netstandard2.0/ref/System.IO.Pipes.dll",
+        "build/netstandard2.0/ref/System.IO.UnmanagedMemoryStream.dll",
+        "build/netstandard2.0/ref/System.IO.dll",
+        "build/netstandard2.0/ref/System.Linq.Expressions.dll",
+        "build/netstandard2.0/ref/System.Linq.Parallel.dll",
+        "build/netstandard2.0/ref/System.Linq.Queryable.dll",
+        "build/netstandard2.0/ref/System.Linq.dll",
+        "build/netstandard2.0/ref/System.Net.Http.dll",
+        "build/netstandard2.0/ref/System.Net.NameResolution.dll",
+        "build/netstandard2.0/ref/System.Net.NetworkInformation.dll",
+        "build/netstandard2.0/ref/System.Net.Ping.dll",
+        "build/netstandard2.0/ref/System.Net.Primitives.dll",
+        "build/netstandard2.0/ref/System.Net.Requests.dll",
+        "build/netstandard2.0/ref/System.Net.Security.dll",
+        "build/netstandard2.0/ref/System.Net.Sockets.dll",
+        "build/netstandard2.0/ref/System.Net.WebHeaderCollection.dll",
+        "build/netstandard2.0/ref/System.Net.WebSockets.Client.dll",
+        "build/netstandard2.0/ref/System.Net.WebSockets.dll",
+        "build/netstandard2.0/ref/System.Net.dll",
+        "build/netstandard2.0/ref/System.Numerics.dll",
+        "build/netstandard2.0/ref/System.ObjectModel.dll",
+        "build/netstandard2.0/ref/System.Reflection.Extensions.dll",
+        "build/netstandard2.0/ref/System.Reflection.Primitives.dll",
+        "build/netstandard2.0/ref/System.Reflection.dll",
+        "build/netstandard2.0/ref/System.Resources.Reader.dll",
+        "build/netstandard2.0/ref/System.Resources.ResourceManager.dll",
+        "build/netstandard2.0/ref/System.Resources.Writer.dll",
+        "build/netstandard2.0/ref/System.Runtime.CompilerServices.VisualC.dll",
+        "build/netstandard2.0/ref/System.Runtime.Extensions.dll",
+        "build/netstandard2.0/ref/System.Runtime.Handles.dll",
+        "build/netstandard2.0/ref/System.Runtime.InteropServices.RuntimeInformation.dll",
+        "build/netstandard2.0/ref/System.Runtime.InteropServices.dll",
+        "build/netstandard2.0/ref/System.Runtime.Numerics.dll",
+        "build/netstandard2.0/ref/System.Runtime.Serialization.Formatters.dll",
+        "build/netstandard2.0/ref/System.Runtime.Serialization.Json.dll",
+        "build/netstandard2.0/ref/System.Runtime.Serialization.Primitives.dll",
+        "build/netstandard2.0/ref/System.Runtime.Serialization.Xml.dll",
+        "build/netstandard2.0/ref/System.Runtime.Serialization.dll",
+        "build/netstandard2.0/ref/System.Runtime.dll",
+        "build/netstandard2.0/ref/System.Security.Claims.dll",
+        "build/netstandard2.0/ref/System.Security.Cryptography.Algorithms.dll",
+        "build/netstandard2.0/ref/System.Security.Cryptography.Csp.dll",
+        "build/netstandard2.0/ref/System.Security.Cryptography.Encoding.dll",
+        "build/netstandard2.0/ref/System.Security.Cryptography.Primitives.dll",
+        "build/netstandard2.0/ref/System.Security.Cryptography.X509Certificates.dll",
+        "build/netstandard2.0/ref/System.Security.Principal.dll",
+        "build/netstandard2.0/ref/System.Security.SecureString.dll",
+        "build/netstandard2.0/ref/System.ServiceModel.Web.dll",
+        "build/netstandard2.0/ref/System.Text.Encoding.Extensions.dll",
+        "build/netstandard2.0/ref/System.Text.Encoding.dll",
+        "build/netstandard2.0/ref/System.Text.RegularExpressions.dll",
+        "build/netstandard2.0/ref/System.Threading.Overlapped.dll",
+        "build/netstandard2.0/ref/System.Threading.Tasks.Parallel.dll",
+        "build/netstandard2.0/ref/System.Threading.Tasks.dll",
+        "build/netstandard2.0/ref/System.Threading.Thread.dll",
+        "build/netstandard2.0/ref/System.Threading.ThreadPool.dll",
+        "build/netstandard2.0/ref/System.Threading.Timer.dll",
+        "build/netstandard2.0/ref/System.Threading.dll",
+        "build/netstandard2.0/ref/System.Transactions.dll",
+        "build/netstandard2.0/ref/System.ValueTuple.dll",
+        "build/netstandard2.0/ref/System.Web.dll",
+        "build/netstandard2.0/ref/System.Windows.dll",
+        "build/netstandard2.0/ref/System.Xml.Linq.dll",
+        "build/netstandard2.0/ref/System.Xml.ReaderWriter.dll",
+        "build/netstandard2.0/ref/System.Xml.Serialization.dll",
+        "build/netstandard2.0/ref/System.Xml.XDocument.dll",
+        "build/netstandard2.0/ref/System.Xml.XPath.XDocument.dll",
+        "build/netstandard2.0/ref/System.Xml.XPath.dll",
+        "build/netstandard2.0/ref/System.Xml.XmlDocument.dll",
+        "build/netstandard2.0/ref/System.Xml.XmlSerializer.dll",
+        "build/netstandard2.0/ref/System.Xml.dll",
+        "build/netstandard2.0/ref/System.dll",
+        "build/netstandard2.0/ref/mscorlib.dll",
+        "build/netstandard2.0/ref/netstandard.dll",
+        "build/netstandard2.0/ref/netstandard.xml",
+        "lib/netstandard1.0/_._",
+        "netstandard.library.2.0.3.nupkg.sha512",
+        "netstandard.library.nuspec"
+      ]
+    },
+    "Newtonsoft.Json/13.0.3": {
+      "sha512": "HrC5BXdl00IP9zeV+0Z848QWPAoCr9P3bDEZguI+gkLcBKAOxix/tLEAAHC+UvDNPv4a2d18lOReHMOagPa+zQ==",
+      "type": "package",
+      "path": "newtonsoft.json/13.0.3",
+      "files": [
+        ".nupkg.metadata",
+        ".signature.p7s",
+        "LICENSE.md",
+        "README.md",
+        "lib/net20/Newtonsoft.Json.dll",
+        "lib/net20/Newtonsoft.Json.xml",
+        "lib/net35/Newtonsoft.Json.dll",
+        "lib/net35/Newtonsoft.Json.xml",
+        "lib/net40/Newtonsoft.Json.dll",
+        "lib/net40/Newtonsoft.Json.xml",
+        "lib/net45/Newtonsoft.Json.dll",
+        "lib/net45/Newtonsoft.Json.xml",
+        "lib/net6.0/Newtonsoft.Json.dll",
+        "lib/net6.0/Newtonsoft.Json.xml",
+        "lib/netstandard1.0/Newtonsoft.Json.dll",
+        "lib/netstandard1.0/Newtonsoft.Json.xml",
+        "lib/netstandard1.3/Newtonsoft.Json.dll",
+        "lib/netstandard1.3/Newtonsoft.Json.xml",
+        "lib/netstandard2.0/Newtonsoft.Json.dll",
+        "lib/netstandard2.0/Newtonsoft.Json.xml",
+        "newtonsoft.json.13.0.3.nupkg.sha512",
+        "newtonsoft.json.nuspec",
+        "packageIcon.png"
+      ]
+    },
+    "System.Numerics.Vectors/4.6.1": {
+      "sha512": "sQxefTnhagrhoq2ReR0D/6K0zJcr9Hrd6kikeXsA1I8kOCboTavcUC4r7TSfpKFeE163uMuxZcyfO1mGO3EN8Q==",
+      "type": "package",
+      "path": "system.numerics.vectors/4.6.1",
+      "files": [
+        ".nupkg.metadata",
+        ".signature.p7s",
+        "Icon.png",
+        "PACKAGE.md",
+        "buildTransitive/net461/System.Numerics.Vectors.targets",
+        "buildTransitive/net462/_._",
+        "lib/net462/System.Numerics.Vectors.dll",
+        "lib/net462/System.Numerics.Vectors.xml",
+        "lib/netcoreapp2.0/_._",
+        "lib/netstandard2.0/System.Numerics.Vectors.dll",
+        "lib/netstandard2.0/System.Numerics.Vectors.xml",
+        "lib/netstandard2.1/_._",
+        "system.numerics.vectors.4.6.1.nupkg.sha512",
+        "system.numerics.vectors.nuspec"
+      ]
+    }
+  },
+  "projectFileDependencyGroups": {
+    ".NETStandard,Version=v2.0": [
+      "NETStandard.Library >= 2.0.3",
+      "Newtonsoft.Json >= 13.0.3",
+      "System.Numerics.Vectors >= 4.6.1"
+    ]
+  },
+  "packageFolders": {
+    "C:\\Users\\admin\\.nuget\\packages\\": {},
+    "C:\\Program Files (x86)\\Microsoft Visual Studio\\Shared\\NuGetPackages": {}
+  },
+  "project": {
+    "version": "1.0.0",
+    "restore": {
+      "projectUniqueName": "D:\\Users\\Desktop\\入职培训\\停车机器人\\MyParking\\ClumsyPilot\\ClumsyPilot.csproj",
+      "projectName": "ClumsyPilot",
+      "projectPath": "D:\\Users\\Desktop\\入职培训\\停车机器人\\MyParking\\ClumsyPilot\\ClumsyPilot.csproj",
+      "packagesPath": "C:\\Users\\admin\\.nuget\\packages\\",
+      "outputPath": "D:\\Users\\Desktop\\入职培训\\停车机器人\\MyParking\\ClumsyPilot\\obj\\",
+      "projectStyle": "PackageReference",
+      "fallbackFolders": [
+        "C:\\Program Files (x86)\\Microsoft Visual Studio\\Shared\\NuGetPackages"
+      ],
+      "configFilePaths": [
+        "C:\\Users\\admin\\AppData\\Roaming\\NuGet\\NuGet.Config",
+        "C:\\Program Files (x86)\\NuGet\\Config\\Microsoft.VisualStudio.FallbackLocation.config",
+        "C:\\Program Files (x86)\\NuGet\\Config\\Microsoft.VisualStudio.Offline.config"
+      ],
+      "originalTargetFrameworks": [
+        "netstandard2.0"
+      ],
+      "sources": {
+        "C:\\Program Files (x86)\\Microsoft SDKs\\NuGetPackages\\": {},
+        "https://api.nuget.org/v3/index.json": {}
+      },
+      "frameworks": {
+        "netstandard2.0": {
+          "targetAlias": "netstandard2.0",
+          "projectReferences": {}
+        }
+      },
+      "warningProperties": {
+        "warnAsError": [
+          "NU1605"
+        ]
+      },
+      "restoreAuditProperties": {
+        "enableAudit": "true",
+        "auditLevel": "low",
+        "auditMode": "direct"
+      },
+      "SdkAnalysisLevel": "9.0.300"
+    },
+    "frameworks": {
+      "netstandard2.0": {
+        "targetAlias": "netstandard2.0",
+        "dependencies": {
+          "NETStandard.Library": {
+            "suppressParent": "All",
+            "target": "Package",
+            "version": "[2.0.3, )",
+            "autoReferenced": true
+          },
+          "Newtonsoft.Json": {
+            "target": "Package",
+            "version": "[13.0.3, )"
+          },
+          "System.Numerics.Vectors": {
+            "target": "Package",
+            "version": "[4.6.1, )"
+          }
+        },
+        "imports": [
+          "net461",
+          "net462",
+          "net47",
+          "net471",
+          "net472",
+          "net48",
+          "net481"
+        ],
+        "assetTargetFallback": true,
+        "warn": true,
+        "runtimeIdentifierGraphPath": "C:\\Program Files\\dotnet\\sdk\\9.0.316\\RuntimeIdentifierGraph.json"
+      }
+    }
+  }
+}

ClumsyPilot/obj/project.nuget.cache

@@ -0,0 +1,13 @@
+{
+  "version": 2,
+  "dgSpecHash": "YBvImiCcgSo=",
+  "success": true,
+  "projectFilePath": "D:\\Users\\Desktop\\入职培训\\停车机器人\\MyParking\\ClumsyPilot\\ClumsyPilot.csproj",
+  "expectedPackageFiles": [
+    "C:\\Users\\admin\\.nuget\\packages\\microsoft.netcore.platforms\\1.1.0\\microsoft.netcore.platforms.1.1.0.nupkg.sha512",
+    "C:\\Users\\admin\\.nuget\\packages\\netstandard.library\\2.0.3\\netstandard.library.2.0.3.nupkg.sha512",
+    "C:\\Users\\admin\\.nuget\\packages\\newtonsoft.json\\13.0.3\\newtonsoft.json.13.0.3.nupkg.sha512",
+    "C:\\Users\\admin\\.nuget\\packages\\system.numerics.vectors\\4.6.1\\system.numerics.vectors.4.6.1.nupkg.sha512"
+  ],
+  "logs": []
+}

CommonUsage-MultiVehicleSync/commonusage/Chassis/MultiWheelChassis.cs

@@ -216,48 +216,6 @@ namespace CommonUsage.Chassis
             LastMoveTime = DateTime.Now;
         }
 
-        /// <summary>
-        /// 立即清零XYTh驱动轮速度，同时保留已经准备好的舵角目标和轮速方向。
-        /// 下一条非零命令仍需重新确认四轮实际舵角到位后才会开放驱动速度。
-        /// </summary>
-        public void StopXYThDrivePreserveSteeringState()
-        {
-            if (!Valid) return;
-
-            // 只有已完成Prepare/Adopt交接的XYTh模式才能保留状态。
-            if (!XYThActive)
-            {
-                PredefinedDriveStop();
-                return;
-            }
-
-            for (var i = 0; i < _steerWheels.Count; i++)
-            {
-                _targetSpeeds[i] = 0;
-                _sendSpeeds[i] = 0;
-                _debugSpeeds[i] = 0;
-
-                if (_steerWheels[i] is DiffSteerWheel diffSteerWheel)
-                {
-                    diffSteerWheel.WriteLeftSpeed(0);
-                    diffSteerWheel.WriteRightSpeed(0);
-                }
-                else
-                {
-                    _steerWheels[i].WriteSpeed(0);
-                }
-            }
-
-            GoingActive = false;
-            RotatingActive = false;
-
-            // 保留XYThActive、_sendAngle和_wheelDirs，避免重新选择等价舵角；
-            // 清除到位标记，使下一次推动摇杆时重新核对实际反馈。
-            _xyThWheelsAligned = false;
-            LastMoveTime = DateTime.Now;
-            LastMotionDecomposeFailureReason = "";
-        }
-
         private struct WheelAngleCandidate
         {
             public bool Valid;
@@ -735,158 +693,6 @@ namespace CommonUsage.Chassis
             return true;
         }
 
-        /// <summary>
-        /// 停车并将四个舵轮转到绕当前坐标原点自转所需的切线方向。
-        /// 只下发舵角，不下发驱动速度。
-        /// </summary>
-        public bool PrepareRotateWheels(float alignmentToleranceDegrees = 2.0f)
-        {
-            if (!Valid)
-                return FailMotionDecomposition(
-                    "PrepareRotateWheels",
-                    "invalid chassis",
-                    null);
-
-            if (float.IsNaN(alignmentToleranceDegrees) ||
-                float.IsInfinity(alignmentToleranceDegrees) ||
-                alignmentToleranceDegrees < 0.0f)
-                throw new ArgumentOutOfRangeException(
-                    nameof(alignmentToleranceDegrees),
-                    "自转舵轮到位容差必须是非负有限值。");
-
-            if (_steerWheels.Count == 0)
-                return FailMotionDecomposition(
-                    "PrepareRotateWheels",
-                    "no steer wheels",
-                    null);
-
-            // 模式切换期间必须保持驱动轮停止。
-            PredefinedDriveStop();
-
-            var targetAngles = new float[_steerWheels.Count];
-            var directions = new int[_steerWheels.Count];
-
-            // 先完成全部舵角解算，再统一下发，避免只转动部分舵轮。
-            for (var i = 0; i < _steerWheels.Count; i++)
-            {
-                var wheel = _steerWheels[i];
-                var px = (double)wheel.Position.X;
-                var py = (double)wheel.Position.Y;
-
-                // 逆时针绕原点旋转时，该舵轮的切向方向为(-py, px)。
-                var tangentDegrees =
-                    (float)(Math.Atan2(px, -py) /
-                            Math.PI * 180.0);
-                tangentDegrees = CommonMath.ThDiff(
-                    tangentDegrees,
-                    wheel.ZeroDirection);
-
-                if (!TryResolveWheelAngle(
-                        i,
-                        tangentDegrees,
-                        "PrepareRotateWheels",
-                        out targetAngles[i],
-                        out directions[i],
-                        out var reason))
-                    return FailMotionDecomposition(
-                        "PrepareRotateWheels",
-                        reason,
-                        null);
-            }
-
-            for (var i = 0; i < _steerWheels.Count; i++)
-            {
-                _wheelDirs[i] = directions[i];
-                SendTh(i, targetAngles[i]);
-            }
-
-            var allAligned = true;
-            for (var i = 0; i < _steerWheels.Count; i++)
-            {
-                var actualAngle = _steerWheels[i].ReadAngle();
-                var angleError = targetAngles[i] - actualAngle;
-
-                // 这里比较受机械限位约束的真实舵角，不能使用圆周最短角度差。
-                if (float.IsNaN(actualAngle) ||
-                    float.IsInfinity(actualAngle) ||
-                    Math.Abs(angleError) > alignmentToleranceDegrees)
-                    allAligned = false;
-            }
-
-            LastRotateAligned = allAligned;
-            LastMotionDecomposeFailureReason = "";
-            return true;
-        }
-
-        /// <summary>
-        /// 将PrepareRotateWheels已经确认到位的舵角和轮速方向，
-        /// 原样交接给SendXYThSpeed，作为一段XYTh运动的初始状态。
-        /// 该方法不会调用ResetMotionState，因此不会重新选择等价舵角。
-        /// </summary>
-        public bool AdoptPreparedRotateWheelsForXYTh(
-            float alignmentToleranceDegrees = 2.0f)
-        {
-            if (!Valid)
-                return FailMotionDecomposition(
-                    "AdoptPreparedRotateWheelsForXYTh",
-                    "invalid chassis",
-                    null);
-
-            if (float.IsNaN(alignmentToleranceDegrees) ||
-                float.IsInfinity(alignmentToleranceDegrees) ||
-                alignmentToleranceDegrees < 0.0f)
-                throw new ArgumentOutOfRangeException(
-                    nameof(alignmentToleranceDegrees),
-                    "自转舵轮交接容差必须是非负有限值。");
-
-            if (!LastRotateAligned)
-                return FailMotionDecomposition(
-                    "AdoptPreparedRotateWheelsForXYTh",
-                    "rotate wheels have not been prepared and aligned",
-                    null);
-
-            for (var i = 0; i < _steerWheels.Count; i++)
-            {
-                var actualAngle =
-                    _steerWheels[i].ReadAngle();
-
-                if (float.IsNaN(actualAngle) ||
-                    float.IsInfinity(actualAngle))
-                {
-                    LastRotateAligned = false;
-                    return FailMotionDecomposition(
-                        "AdoptPreparedRotateWheelsForXYTh",
-                        $"wheel {i} angle feedback is invalid: {actualAngle}",
-                        null);
-                }
-
-                // 比较受机械限位约束的实际舵角，不使用圆周最短角。
-                var angleError =
-                    _sendAngle[i] - actualAngle;
-                if (Math.Abs(angleError) >
-                    alignmentToleranceDegrees)
-                {
-                    LastRotateAligned = false;
-                    return FailMotionDecomposition(
-                        "AdoptPreparedRotateWheelsForXYTh",
-                        $"wheel {i} is no longer aligned: " +
-                        $"target={_sendAngle[i]:F1}, actual={actualAngle:F1}, " +
-                        $"error={angleError:F1}",
-                        null);
-                }
-            }
-
-            // 直接继承PrepareRotateWheels写入的_wheelDirs和_sendAngle。
-            // 下一次SendXYThSpeed调用看到XYThActive=true时不会重置这些状态。
-            XYThActive = true;
-            _xyThWheelsAligned = true;
-            GoingActive = false;
-            RotatingActive = false;
-            LastMoveTime = DateTime.Now;
-            LastMotionDecomposeFailureReason = "";
-            return true;
-        }
-
 		/// <summary>
 		/// 绕"已被 SetOriginBias 偏置到车队中心的原点"做原地旋转，可叠加一个车体系小幅纠偏旋量。
 		/// </summary>
@@ -975,19 +781,14 @@ namespace CommonUsage.Chassis
             var maxDth = 0f;
             for (var i = 0; i < _steerWheels.Count; ++i)
             {
-                var actualAngle = _steerWheels[i].ReadAngle();
-                var dth = Math.Abs(ths[i] - actualAngle);
+                var dth = Math.Abs(CommonMath.ThDiff(ths[i], _steerWheels[i].ReadAngle()));
                 maxDth = Math.Max(maxDth, dth);
-                slowFac = Math.Min(
-                    slowFac,
-                    CommonMath.gaussmf(
-                        dth,
-                        Math.Max(SteeringAlignmentSigmaDegrees, 0.1f),
-                        0));
-                if (float.IsNaN(actualAngle) ||
-                    float.IsInfinity(actualAngle) ||
-                    dth > 2.0f)
-                    allWheelAligned = false;
+                slowFac = Math.Min(slowFac, CommonMath.gaussmf(dth, rotSync, 0));
+     //            if (dth > 5)
+     //            {
+					// allWheelAligned = false;
+					// break;
+     //            }
             }
             LastRotateAligned = allWheelAligned; // 供上层做积分抗饱和
 
@@ -1128,7 +929,7 @@ namespace CommonUsage.Chassis
 		// wheel will swing between 90 and -90.
 		private List<int> _wheelDirs;
 
-        // call this before a new motion sequence happens
+		// call this before a new continuous motion happens
 		private void ResetMotionState()
 		{
 			LastMoveTime = DateTime.Now;
@@ -1152,6 +953,8 @@ namespace CommonUsage.Chassis
         {
             var vRotX = -vth / 180 * (float)Math.PI * pos.Y / 1000;
             var vRotY = vth / 180 * (float)Math.PI * pos.X / 1000;
+            Hedingben.ToastText($"vRot:({vRotX:F3},{vRotY:F3}) pos:({pos.X:F1},{pos.Y:F1})",
+                $"SendXYThSpeed-VectorVelocity{i}");
             return new Vector2(vx + vRotX, vy + vRotY);
         }
 		/// <summary>
@@ -1168,199 +971,53 @@ namespace CommonUsage.Chassis
             return ((float)(Math.Atan2(v.Y, v.X) / Math.PI * 180), v.Length());
         }
 
-        /// <summary>
-        /// 原地旋转时舵角误差对应的速度衰减宽度，单位为度。
-        /// </summary>
-        public float SteeringAlignmentSigmaDegrees { get; set; } = 8f;
-        /// <summary>
-        /// 单轮速度低于此值时认为其运动方向无意义，单位为m/s。
-        /// </summary>
-        public float WheelDirectionDeadbandMetersPerSecond { get; set; } = 0.005f;
+        private float w = 0;
+        private float wAcc = 0.1f;
+        private float rotSync = 3f;
         private bool XYThActive = false;
-        private bool _xyThWheelsAligned = false;
-        private DateTime _xyThDiagnosticsLastTime = DateTime.MinValue;
 
-        /// <summary>
-        /// 下发车体二维速度，并根据舵轮机械角度误差进行高斯降速。
-        /// 一段运动开始时必须先等待全部舵轮到位；运动过程中舵角误差越大，
-        /// 四轮驱动速度的统一缩放比例越小，适合作为默认安全接口。
-        /// vx、vy单位为m/s，vth单位为°/s。
-        /// </summary>
-        public bool SendXYThSpeed(
-            float vx,
-            float vy,
-            float vth,
-            TimeSpan? deltaTime = null,
-            bool enableDifferentialSteerFeedforward = false)
+        public bool SendXYThSpeed(float vx, float vy, float vth, TimeSpan? deltaTime = null)
         {
-            const string operationName = "SendXYThSpeed";
-
-            if (!Valid)
-                return FailMotionDecomposition(
-                    operationName,
-                    "invalid chassis",
-                    deltaTime);
-
-            if (Math.Abs(vx) < 1e-6f && Math.Abs(vy) < 1e-6f && Math.Abs(vth) < 1e-6f)
-            {
-                RampStop(deltaTime);
-                XYThActive = false;
-                _xyThWheelsAligned = false;
-                GoingActive = false;
-                RotatingActive = false;
-                LastMotionDecomposeFailureReason = "";
-                return true;
-            }
-
+            if (!Valid) return FailMotionDecomposition("SendXYThSpeed", "invalid chassis", deltaTime);
 			if (!XYThActive)
             {
                 ResetMotionState();
-                _xyThWheelsAligned = false;
+                w = 0;
             }
 			XYThActive = true;
             GoingActive = false;
             RotatingActive = false;
 
-            float[] sendSpeed = new float[_steerWheels.Count];
-            var allWheelsAligned = true;
-            var maximumAngleError = 0f;
-            var alignmentSpeedScale = 1f;
-            const float initialAlignmentToleranceDegrees = 2f;
-            var writeDiagnostics =
-                Debug &&
-                (DateTime.Now - _xyThDiagnosticsLastTime)
-                    .TotalMilliseconds >= 250.0;
+            if (Math.Abs(vx) < 1e-6f && Math.Abs(vy) < 1e-6f && Math.Abs(vth) < 1e-6f)
+            {
+                RampStop(deltaTime);
+                LastMotionDecomposeFailureReason = "";
+                return true;
+            }
    
+            w = w + wAcc;
+            if (w > 1) w = 1;
+            float[] sendSpeed = new float[_steerWheels.Count];
             for (var i = 0; i < _steerWheels.Count; i++)
             {
                 var sw = _steerWheels[i];
                 var (angle, speed) = AngleAndSpeed(sw.Position, vx, vy, vth, i);
-                // 单轮合成速度接近零时，运动方向没有物理意义。
-                // 此时不重新计算和下发舵角，保持上一目标舵角，轮速降为零。
-                var directionDeadband = Math.Max(
-                    WheelDirectionDeadbandMetersPerSecond,
-                    0f);
-
-                if (speed < directionDeadband)
-                {
-                    sendSpeed[i] = 0f;
-
-                    if (writeDiagnostics)
-                    {
-                        Hedingben.ToastText(
-                            $"hold-angle speed:{speed:F4} deadband:{directionDeadband:F4}",
-                            $"{operationName}-{i}");
-                    }
-
-                    continue;
-                }
                 var actualTh = sw.ReadAngle();
-
-                if (float.IsNaN(actualTh) ||
-                    float.IsInfinity(actualTh))
-                {
-                    return FailMotionDecomposition(
-                        operationName,
-                        $"wheel {i} angle feedback is invalid: {actualTh}",
-                        deltaTime);
-                }
-
-                if (!TryResolveWheelAngle(i, CommonMath.ThDiff(angle, sw.ZeroDirection), operationName,
+                if (!TryResolveWheelAngle(i, CommonMath.ThDiff(angle, sw.ZeroDirection), "SendXYThSpeed",
                         out var useAngle, out var dir, out var resolveReason))
-                    return FailMotionDecomposition(operationName, resolveReason, deltaTime);
+                    return FailMotionDecomposition("SendXYThSpeed", resolveReason, deltaTime);
                 speed *= dir;
                 _wheelDirs[i] = dir;
    
                 sendSpeed[i] = speed;
+                if (speed!=0)
                     SendTh(i, useAngle);
+                w = (float)Math.Min(w, CommonMath.gaussmf(CommonMath.ThDiff(actualTh, _sendAngle[i]), rotSync, 0));
 
-                // 这里比较受机械限位约束的实际舵角，不使用圆周最短角。
-                var angleError =
-                    Math.Abs(_sendAngle[i] - actualTh);
-                maximumAngleError =
-                    Math.Max(maximumAngleError, angleError);
-
-                alignmentSpeedScale = Math.Min(
-                    alignmentSpeedScale,
-                    CommonMath.gaussmf(
-                        angleError,
-                        Math.Max(
-                            SteeringAlignmentSigmaDegrees,
-                            0.1f),
-                        0));
-                if (angleError >
-                    initialAlignmentToleranceDegrees)
-                {
-                    allWheelsAligned = false;
+                Hedingben.ToastText($"w:{w:F2} s:{speed:F3} th:{_sendAngle[i]:F1} actualTh:{actualTh:F1}", $"SendXYThSpeed-{i}");
             }
-
-                if (writeDiagnostics)
-                {
-                    Hedingben.ToastText(
-                        $"ready:{_xyThWheelsAligned} " +
-                        $"err:{angleError:F1} scale:{alignmentSpeedScale:F2} " +
-                        $"s:{speed:F3} th:{_sendAngle[i]:F1} actualTh:{actualTh:F1}",
-                        $"{operationName}-{i}");
-                }
-            }
-
-            // 一段XYTh运动刚开始时必须等待全部舵轮到位。
-            if (!_xyThWheelsAligned &&
-                allWheelsAligned)
-            {
-                _xyThWheelsAligned = true;
-            }
-
-            var driveScale = _xyThWheelsAligned
-                ? alignmentSpeedScale
-                : 0f;
-            #region 新增虚拟旋转中心
-            // vth传入单位为deg/s，这里换算成rad/s。
-            var omegaRadiansPerSecond =
-                vth * (float)Math.PI / 180f;
-
-            // 只有调用者主动开启并且存在旋转运动时，
-            // 才启用差速舵轮左右轮的几何速度前馈。
-            var useDifferentialSteerFeedforward =
-                enableDifferentialSteerFeedforward &&
-                Math.Abs(omegaRadiansPerSecond) > 1e-4f;
-
-            var rotationCenter = Vector2.Zero;
-
-            if (useDifferentialSteerFeedforward)
-            {
-                // 根据车体速度场：
-                // vx(point) = vx - omega * y
-                // vy(point) = vy + omega * x
-                // 计算车体坐标系中的瞬时旋转中心。
-                //
-                // vx、vy单位为m/s，计算结果原本是m；
-                // 舵轮Position使用mm，因此乘以1000。
-                rotationCenter = new Vector2(
-                    -vy / omegaRadiansPerSecond * 1000f,
-                     vx / omegaRadiansPerSecond * 1000f);
-            }
-            #endregion
-
-
             for (var i = 0; i < _steerWheels.Count; i++)
-                AccumulateSpeed(
-                    i,
-                    driveScale *
-                    sendSpeed[i],
-                    useDifferentialSteerFeedforward,
-                    rotationCenter,
-                    deltaTime);
-
-            if (writeDiagnostics)
-            {
-                _xyThDiagnosticsLastTime = DateTime.Now;
-                Hedingben.ToastText(
-                    $"ready:{_xyThWheelsAligned} " +
-                    $"maxErr:{maximumAngleError:F1} scale:{driveScale:F2} " +
-                    $"cmd:({vx:F3},{vy:F3},{vth:F1})",
-                    $"{operationName}-alignment");
-            }
+                AccumulateSpeed(i, w * sendSpeed[i],false,new Vector2(0f,0f), deltaTime);
             //todo 计算rotCenter填入
             LastMoveTime = DateTime.Now;
             LastMotionDecomposeFailureReason = "";

CommonUsage-MultiVehicleSync/commonusage/CommonUsage.csproj

@@ -2,8 +2,6 @@
 
   <PropertyGroup>
     <TargetFramework>netstandard2.0</TargetFramework>
-    <AssemblyName>CommonUsage</AssemblyName>
-    <RootNamespace>CommonUsage</RootNamespace>
   </PropertyGroup>
 
   <PropertyGroup>
@@ -34,11 +32,11 @@
 
   <ItemGroup>
     <Reference Include="FundamentalLib">
-      <HintPath>.\ref\RefFundamentalLib.dll</HintPath>
+      <HintPath>..\..\MedullaAdapter\ref\RefFundamentalLib.dll</HintPath>
+    </Reference>
+    <Reference Include="ClumsyCore">
+      <HintPath>..\..\ClumsyPilot\ref\RefClumsyCore.dll</HintPath>
     </Reference>
-    <!-- <Reference Include="ClumsyCore">
-      <HintPath>..\..\MultiWheelC\ref\RefClumsyCore.dll</HintPath>
-    </Reference> -->
   </ItemGroup>
 
   <Target Name="CopyCommonUsageToMyParkingRef" AfterTargets="Build">

MedullaAdapter/DiverCartDefinition.cs

@@ -16,14 +16,6 @@ namespace MedullaAdapter
     [UseManualController(manualController = typeof(Remote))]
     public class DiverCartDefinition : MultiWheelCartDefinition
     {
-        public DiverCartDefinition()
-        {
-            // 实车配置可覆盖这些回退值；四个舵轮在MotorRoutine中统一读取它们。
-            DiffSteerKp = 0.0042f;
-            DiffSteerKi = 0f;
-            DiffSteerKd = 0f;
-        }
-
         #region 基本成员
         public MCUSerialBridge Bridge;
 
@@ -72,51 +64,10 @@ namespace MedullaAdapter
         #endregion
 
         #region  初始参数
-
-        // 旧参数仅用于兼容已有配置和历史日志，新模型逆前馈不读取它。
-        [AsInitParam(desc = "旧差速转舵目标角速度前馈增益（已停用）")]
-        public float DiffSteerRateFeedforwardGain = 0f;
-
-        [AsInitParam(desc = "旧前馈差速舵轮左右轮间距，单位mm（已停用）")]
-        public float DiffSteerWheelDistanceMillimeters = 85f;
-
-        [AsInitParam(desc = "差速转舵前馈最大速度，单位m/s")]
-        public float DiffSteerRateFeedforwardMaximumSpeed = 0.03f;
-
-        [AsInitParam(desc = "启用差速舵轮到位迟滞")]
-        public bool EnableDiffSteerSettlingHysteresis = false;
-
-        [AsInitParam(desc = "差速舵轮停止调整误差，单位deg")]
-        public float DiffSteerStopErrorDegrees = 0.5f;
-
-        [AsInitParam(desc = "差速舵轮重新启动误差，单位deg")]
-        public float DiffSteerRestartErrorDegrees = 0.8f;
-
-        [AsInitParam(desc = "差速舵轮到位确认周期数")]
-        public int DiffSteerSettlingCycles = 3;
-
-        [AsInitParam(desc = "启用差速转舵模型逆前馈")]
-        public bool EnableDiffSteerInverseFeedforward = false;
-
-        [AsInitParam(desc = "静止舵轮模型增益")]
-        public float DiffSteerPlantGain = 687.06f;
-
-        [AsInitParam(desc = "模型逆前馈滤波时间常数，单位s")]
-        public float DiffSteerInverseFeedforwardTimeConstantSeconds = 0.15f;
-
         [AsInitParam(desc = "MCU端口号")] public string MCUPort = "COM4";
         [AsInitParam(desc = "遥控器速度上限")] public float TransmitterSpeedUpperLimit = 1.0f;
         [AsInitParam(desc = "遥控器速度下限")] public float TransmitterSpeedLowerLimit = 0.0f;
-        [AsInitParam(desc = "实体遥控器SB模式防抖时间，单位ms")]
-        public int TransmitterModeDebounceMilliseconds = 200;
         [AsInitParam(desc = "手动控制夹臂速度系数")] public float ManualArmSpeedFac = 1.0f;
-        [AsInitParam(desc = "遥控转弯舵角同步限速宽度，单位为度")]
-        public float ManualSteeringAlignmentSigmaDegrees = 8.0f;
-        [AsInitParam(desc = "自转最大角速度，单位deg/s")]
-        public float MaxSpinAngularSpeedDegreesPerSecond = 30f;
-        [AsInitParam(desc = "轮速诊断日志相对目录")]
-        public string WheelSpeedDiagnosticDirectory =
-            @"logs\wheel-speed";
         [AsInitParam(desc = "左夹臂低限位")][AsLowerIO] public int LeftArmLowerPos = -10000;
         [AsInitParam(desc = "左夹臂高限位")][AsLowerIO] public int LeftArmUpperPos = 5927610;
         [AsInitParam(desc = "右夹臂低限位")][AsLowerIO] public int RightArmLowerPos = -17295;
@@ -136,85 +87,8 @@ namespace MedullaAdapter
         [IOObjectMonitor(desc = "左后右轮PID修正后速度")] public float SpeedLRR;
         [IOObjectMonitor(desc = "右后左轮PID修正后速度")] public float SpeedRRL;
         [IOObjectMonitor(desc = "右后右轮PID修正后速度")] public float SpeedRRR;
-        [IOObjectMonitor(desc = "左前舵轮转向PID输出")] public float DiffSteerOutputLeftFront;
-        [IOObjectMonitor(desc = "左后舵轮转向PID输出")] public float DiffSteerOutputLeftRear;
-        [IOObjectMonitor(desc = "右前舵轮转向PID输出")] public float DiffSteerOutputRightFront;
-        [IOObjectMonitor(desc = "右后舵轮转向PID输出")] public float DiffSteerOutputRightRear;
-        [IOObjectMonitor(desc = "左前舵轮目标角速度前馈输出")] public float DiffSteerRateFeedforwardLeftFront;
-        [IOObjectMonitor(desc = "左后舵轮目标角速度前馈输出")] public float DiffSteerRateFeedforwardLeftRear;
-        [IOObjectMonitor(desc = "右前舵轮目标角速度前馈输出")] public float DiffSteerRateFeedforwardRightFront;
-        [IOObjectMonitor(desc = "右后舵轮目标角速度前馈输出")] public float DiffSteerRateFeedforwardRightRear;
-        [IOObjectMonitor(desc = "左前舵轮转向合成差速输出")] public float DiffSteerTotalOutputLeftFront;
-        [IOObjectMonitor(desc = "左后舵轮转向合成差速输出")] public float DiffSteerTotalOutputLeftRear;
-        [IOObjectMonitor(desc = "右前舵轮转向合成差速输出")] public float DiffSteerTotalOutputRightFront;
-        [IOObjectMonitor(desc = "右后舵轮转向合成差速输出")] public float DiffSteerTotalOutputRightRear;
-        [IOObjectMonitor(desc = "左前舵轮进入停止调整区")] public bool DiffSteerInStopZoneLeftFront;
-        [IOObjectMonitor(desc = "左后舵轮进入停止调整区")] public bool DiffSteerInStopZoneLeftRear;
-        [IOObjectMonitor(desc = "右前舵轮进入停止调整区")] public bool DiffSteerInStopZoneRightFront;
-        [IOObjectMonitor(desc = "右后舵轮进入停止调整区")] public bool DiffSteerInStopZoneRightRear;
-        [IOObjectMonitor(desc = "左前舵轮连续到位周期数")] public int DiffSteerSettlingCountLeftFront;
-        [IOObjectMonitor(desc = "左后舵轮连续到位周期数")] public int DiffSteerSettlingCountLeftRear;
-        [IOObjectMonitor(desc = "右前舵轮连续到位周期数")] public int DiffSteerSettlingCountRightFront;
-        [IOObjectMonitor(desc = "右后舵轮连续到位周期数")] public int DiffSteerSettlingCountRightRear;
-        [IOObjectMonitor(desc = "左前舵轮已到位")] public bool DiffSteerSettledLeftFront;
-        [IOObjectMonitor(desc = "左后舵轮已到位")] public bool DiffSteerSettledLeftRear;
-        [IOObjectMonitor(desc = "右前舵轮已到位")] public bool DiffSteerSettledRightFront;
-        [IOObjectMonitor(desc = "右后舵轮已到位")] public bool DiffSteerSettledRightRear;
         [IOObjectMonitor(desc = "灯光模式")] public int LightMode = 0;
         [IOObjectMonitor(desc = "实体遥控器当前速度倍率")] public float TransmitterSpeed = 0.3f;
-        [IOObjectMonitor(desc = "轮速诊断记录已启用")]
-        public bool WheelSpeedDiagnosticEnabled;
-        [IOObjectMonitor(desc = "轮速诊断记录状态")]
-        public string WheelSpeedDiagnosticStatus = "未启动";
-
-        // M层辨识日志：以下字段只保存控制和反馈事件的单调时钟快照，
-        // 不参与底盘控制、限幅或模式切换。
-        internal long DiffSteerControlTimestamp;
-        internal long DiffSteerControlSequence;
-        internal long WheelCommandTimestamp;
-        internal long WheelCommandSequence;
-        internal float SentSpeedLFL;
-        internal float SentSpeedLFR;
-        internal float SentSpeedLRL;
-        internal float SentSpeedLRR;
-        internal float SentSpeedRFL;
-        internal float SentSpeedRFR;
-        internal float SentSpeedRRL;
-        internal float SentSpeedRRR;
-        internal bool WheelCommandLimitedLFL;
-        internal bool WheelCommandLimitedLFR;
-        internal bool WheelCommandLimitedLRL;
-        internal bool WheelCommandLimitedLRR;
-        internal bool WheelCommandLimitedRFL;
-        internal bool WheelCommandLimitedRFR;
-        internal bool WheelCommandLimitedRRL;
-        internal bool WheelCommandLimitedRRR;
-        internal bool WheelCommandSuppressed;
-        internal float DiffSteerTargetRateLeftFrontDegreesPerSecond;
-        internal float DiffSteerTargetRateLeftRearDegreesPerSecond;
-        internal float DiffSteerTargetRateRightFrontDegreesPerSecond;
-        internal float DiffSteerTargetRateRightRearDegreesPerSecond;
-        internal float DiffSteerFeedforwardDeltaTimeMilliseconds;
-        internal float DiffSteerInverseFeedforwardRawLeftFront;
-        internal float DiffSteerInverseFeedforwardRawLeftRear;
-        internal float DiffSteerInverseFeedforwardRawRightFront;
-        internal float DiffSteerInverseFeedforwardRawRightRear;
-        internal float DiffSteerInverseFeedforwardFilteredLeftFront;
-        internal float DiffSteerInverseFeedforwardFilteredLeftRear;
-        internal float DiffSteerInverseFeedforwardFilteredRightFront;
-        internal float DiffSteerInverseFeedforwardFilteredRightRear;
-        internal bool DiffSteerFeedforwardLimitedLeftFront;
-        internal bool DiffSteerFeedforwardLimitedLeftRear;
-        internal bool DiffSteerFeedforwardLimitedRightFront;
-        internal bool DiffSteerFeedforwardLimitedRightRear;
-        internal long ActualThLeftFrontTimestamp;
-        internal long ActualThLeftFrontSequence;
-        internal long ActualThLeftRearTimestamp;
-        internal long ActualThLeftRearSequence;
-        internal long ActualThRightFrontTimestamp;
-        internal long ActualThRightFrontSequence;
-        internal long ActualThRightRearTimestamp;
-        internal long ActualThRightRearSequence;
         [IOObjectMonitor(desc = "左前左驱动器远程帧701")] public byte LFLRemoteCode = 0;
         [IOObjectMonitor(desc = "左前右驱动器远程帧702")] public byte LFRRemoteCode = 0;
         [IOObjectMonitor(desc = "右前左驱动器远程帧703")] public byte RFLRemoteCode = 0;
@@ -240,22 +114,6 @@ namespace MedullaAdapter
         {
             DisableFromM = true;
         }
-
-        // M层诊断：请求开始保存CAN轮速事件和底盘周期快照。
-        [IOObjectUtility]
-        public void StartWheelSpeedDiagnostic()
-        {
-            WheelSpeedDiagnosticEnabled = true;
-            WheelSpeedDiagnosticStatus = "等待创建日志文件";
-        }
-
-        // M层诊断：请求停止轮速记录并刷新CSV文件。
-        [IOObjectUtility]
-        public void StopWheelSpeedDiagnostic()
-        {
-            WheelSpeedDiagnosticEnabled = false;
-            WheelSpeedDiagnosticStatus = "等待停止并刷新日志";
-        }
         #endregion
 
         public override void CommunicationInit()
@@ -367,131 +225,27 @@ namespace MedullaAdapter
             }
 
             var speed = speedThreshold * y;
-            var normalizedSteering =
-                (float)Math.Pow(
-                    Math.Abs(x),
-                    ManualThetaPow) *
-                Math.Sign(x);
-            var steeringDegrees =
-                -normalizedSteering * MaxManualTheta;
+            var omega = CalculateManualOmega(speed, x);
             ManualMode = (int)mode;
 
             switch (mode)
             {
                 case ManualControlMode.Normal:
-                    // 普通模式统一使用车体速度命令：
-                    // X向前，行驶中连续改变角速度时舵轮边转、车辆边走。
-                    var normalOmegaRadiansPerSecond =
-                        speed *
-                        Math.Tan(
-                            AngleMath.DegreesToRadians(
-                                steeringDegrees)) /
-                        adapter.ControlPointRadiusMeters;
-                    if (!adapter.SendBodyTwist(
-                            new Twist2D(
-                                speed,
-                                0.0,
-                                normalOmegaRadiansPerSecond),
-                            interval))
-                    {
-                        adapter.StopImmediately();
-                        Console.WriteLine(
-                            "Normal SendMotion decomposition failed: " +
-                            adapter.LastFailureReason);
-                    }
+                    SendBodyCommand(vx: speed, vy: 0.0, omegaRadiansPerSecond: omega, interval);
                     break;
                 case ManualControlMode.Crab:
-                    // 舵轮机械范围为[-120°,120°]。
-                    // 蟹行后虚拟轴距由原车宽度决定，比正常模式轴距短。
-                    // 按几何比例缩小转角，使相同摇杆输入获得接近一致的曲率。
-                    var normalSteeringRadians =
-                        AngleMath.DegreesToRadians(steeringDegrees);
-                    var geometryRatio =
-                        adapter.HalfTrackWidthMeters /
-                        adapter.HalfWheelBaseMeters;
-
-                    // +90°运动坐标系已经把虚拟左侧映射为车体后方，
-                    // 此处保持普通模式的转向符号，避免再次取反导致左右颠倒。
-                    var crabSteeringRadians =
-                        Math.Atan(
-                            geometryRatio *
-                            Math.Tan(
-                                normalSteeringRadians));
-
-                    // 蟹行转角最终限制为±30°，为±120°机械舵角保留余量。
-                    var maximumCrabSteeringRadians =
-                        AngleMath.DegreesToRadians(30.0);
-                    crabSteeringRadians = Math.Max(
-                        -maximumCrabSteeringRadians,
-                        Math.Min(
-                            maximumCrabSteeringRadians,
-                            crabSteeringRadians));
-
-                    // 将车体左侧作为虚拟阿克曼车头，并在该运动坐标系中
-                    // 复用与普通模式相同的SendMotion前后控制点解算。
-                    var crabOmegaRadiansPerSecond =
-                        speed *
-                        Math.Tan(crabSteeringRadians) /
-                        adapter.ControlPointRadiusMeters;
-                    if (!adapter.SendBodyTwist(
-                            new Twist2D(
-                                0.0,
-                                speed,
-                                crabOmegaRadiansPerSecond),
-                            interval))
-                    {
-                        adapter.StopImmediately();
-
-                        Console.WriteLine(
-                            "蟹行SendMotion命令分解失败，车辆已经停车：" +
-                            adapter.LastFailureReason);
-                    }
+                    SendBodyCommand(vx: 0.0, vy: speed, omegaRadiansPerSecond: omega, interval);
                     break;
                 case ManualControlMode.Spin:
-                    // 摇杆处于中位时只清零驱动速度，保持已经准备好的
-                    // 自转舵角；下次推动摇杆时仍会重新检查实际舵角。
-                    if (Math.Abs(speed) < 1e-6f)
-                    {
-                        adapter
-                            .StopXYThDrivePreserveSteeringState();
-                        break;
-                    }
+                    var spinOmega =
+                        speed * MaxAngularSpeed *
+                        Math.PI / 180.0;
 
-                    // 自转时speed表示最外侧舵轮中心的目标切向速度，
-                    // 根据v=omega*r换算为SendXYThSpeed需要的角速度。
-                    var requestedSpinOmegaRadiansPerSecond =
-                        speed /
-                        adapter.MaximumWheelRadiusMeters;
-
-                    // 对半径换算结果做正负对称限幅，防止遥控速度参数误设后自转过快。
-                    var maximumSpinOmegaRadiansPerSecond =
-                        AngleMath.DegreesToRadians(
-                            Math.Max(
-                                0f,
-                                MaxSpinAngularSpeedDegreesPerSecond));
-
-                    var spinOmegaRadiansPerSecond =
-                        Math.Max(
-                            -maximumSpinOmegaRadiansPerSecond,
-                            Math.Min(
-                                maximumSpinOmegaRadiansPerSecond,
-                                requestedSpinOmegaRadiansPerSecond));
-
-                    // 普通安全版SendXYThSpeed只下发角速度，
-                    // 四轮实际舵角未到位时不会开放驱动速度。
-                    if (!adapter.SendBodyTwist(
-                            new Twist2D(
-                                0.0,
-                                0.0,
-                                spinOmegaRadiansPerSecond),
-                            interval))
-                    {
-                        adapter.StopImmediately();
-
-                        Console.WriteLine(
-                            "SendXYThSpeed原地自转命令分解失败，车辆已经停车：" +
-                            adapter.LastFailureReason);
-                    }
+                    SendBodyCommand(
+                        vx: 0.0,
+                        vy: 0.0,
+                        omegaRadiansPerSecond: spinOmega,
+                        interval);
                     break;
                 default:
                     ManualMode = -1;
@@ -520,10 +274,6 @@ namespace MedullaAdapter
             if (_pendingManualMode != mode)
             {
                 adapter.StopImmediately();
-                // 所有模式的准备角度均按真实机械舵角表达；
-                // 先退出上一模式的虚拟运动坐标系，再执行预对齐。
-                adapter.ResetToBodyFrame();
-                _activeManualMode = null;
 
                 var preparationAccepted = mode switch
                 {
@@ -553,8 +303,8 @@ namespace MedullaAdapter
             // 后续控制周期保持停车，并读取实际舵角判断是否到位。
             adapter.StopImmediately();
 
-            var toleranceRadians =
-                AngleMath.DegreesToRadians(2.0);
+            const double toleranceRadians =
+                2.0 * Math.PI / 180.0;
 
             bool aligned;
 
@@ -584,33 +334,56 @@ namespace MedullaAdapter
             if (!aligned)
                 return false;
 
-            if (mode == ManualControlMode.Spin)
-            {
-                // 四轮实际舵角确认到位后只交接一次，保留PrepareSpin
-                // 选定的机械舵角和轮速方向，避免首条XYTh命令重新选角。
-                if (!adapter.AdoptPreparedSpinForXYTh(
-                        toleranceRadians))
-                {
-                    return false;
-                }
-            }
-            // 蟹行轮子在真实车体系中到达机械+90°后，
-            // 再将车体左侧激活为SendMotion的虚拟X正方向。
-            else if (mode == ManualControlMode.Crab)
-            {
-                adapter.ActivateMotionFrame(
-                    Math.PI / 2.0);
-            }
-            else
-            {
-                adapter.ResetToBodyFrame();
-            }
-
             _activeManualMode = mode;
             _pendingManualMode = null;
             return true;
         }
 
+        private double CalculateManualOmega(
+            float speed,
+            float steeringInput)
+        {
+            var normalizedSteering =
+                (float)Math.Pow(
+                    Math.Abs(steeringInput),
+                    ManualThetaPow) *
+                Math.Sign(steeringInput);
+
+            var steeringDegrees =
+                -normalizedSteering * MaxManualTheta;
+
+            var steeringRadians =
+                steeringDegrees * Math.PI / 180.0;
+
+            // CommonUsage中的ControlPointRadius单位为毫米。
+            var halfWheelBaseMeters =
+                Math.Max(
+                    Chassis.ControlPointRadius / 1000.0,
+                    0.01);
+
+            return speed * Math.Tan(steeringRadians) / halfWheelBaseMeters;
+        }
+
+        internal void SendBodyCommand(double vx, double vy, double omegaRadiansPerSecond, TimeSpan? interval = null)
+        {
+            var adapter = GetChassisAdapter();
+
+            if (adapter == null)
+                return;
+
+            var command = new ChassisCommand(
+                CarNum,
+                new Twist2D(vx, vy, omegaRadiansPerSecond));
+
+            if (!adapter.Send(command, interval))
+            {
+                adapter.StopImmediately();
+
+                Console.WriteLine(
+                    "底盘命令分解失败，车辆已经停车：" +
+                    adapter.LastFailureReason);
+            }
+        }
         private MultiWheelChassisAdapter _chassisAdapter;
 
         private MultiWheelChassisAdapter GetChassisAdapter()
@@ -625,11 +398,6 @@ namespace MedullaAdapter
                     new MultiWheelChassisAdapter(Chassis, CarNum);
             }
 
-            _chassisAdapter.SteeringAlignmentSigmaDegrees =
-                Math.Max(
-                    ManualSteeringAlignmentSigmaDegrees,
-                    0.1f);
-
             return _chassisAdapter;
         }
 

MedullaAdapter/MCURoutine.cs

@@ -4,9 +4,6 @@ using FundamentalLib;
 using MCUSerialBridgeCLR;
 using System;
 using System.Collections.Generic;
-using System.Diagnostics;
-using System.IO;
-using System.Threading;
 
 namespace MedullaAdapter
 {
@@ -29,9 +26,6 @@ namespace MedullaAdapter
         private bool io_bit4 = false;//黄灯
         private const byte BatteryPortIndex = 3;
         private static readonly byte[] BatteryRequest = BuildBatteryRequest();
-        private readonly WheelSpeedDiagnosticLogger
-            _wheelSpeedLogger =
-                new WheelSpeedDiagnosticLogger();
 
         // M层单车底盘：将车轮线速度换算为驱动电机转速。
         private static float ConvertMps2Rpm(float mps)
@@ -53,24 +47,9 @@ namespace MedullaAdapter
         {
             return BitConverter.ToInt32(payload, offset) * 1875f / 512f / 10000f;
         }
-
-        /// <summary>
-        /// 保存异步CAN反馈的本机单调接收时刻和递增序号，仅供辨识日志使用。
-        /// </summary>
-        private static void MarkFeedbackReceived(
-            ref long timestamp,
-            ref long sequence)
-        {
-            Interlocked.Exchange(
-                ref timestamp,
-                Stopwatch.GetTimestamp());
-            Interlocked.Increment(ref sequence);
-        }
         // M层硬件主循环：交换IO、发送轮组指令并更新车辆反馈状态。
         public override void Operation(int iteration)
         {
-            UpdateWheelSpeedDiagnosticState();
-
             if (_lastIteration != iteration)
             {
                 _lastIteration = iteration;
@@ -179,7 +158,6 @@ namespace MedullaAdapter
             cart.ActualSpeedLeftRear = (cart.ActualSpeedLeftRearLeft + cart.ActualSpeedLeftRearRight) / 2;
             cart.ActualSpeedRightFront = (cart.ActualSpeedRightFrontLeft + cart.ActualSpeedRightFrontRight) / 2;
             cart.ActualSpeedRightRear = (cart.ActualSpeedRightRearLeft + cart.ActualSpeedRightRearRight) / 2;
-            _wheelSpeedLogger.RecordSnapshot(cart);
             // M层CAN辅助：封装本周期驱动器CAN发送参数。
             MCUSerialBridgeError SendCan(byte port, ushort standardId, byte[] payload, bool RTR = false, uint timeout = 2)
             {
@@ -288,7 +266,6 @@ namespace MedullaAdapter
                     return;
                 }
             }
-
             // 没有错误 没有节点保护 就发06 07 0F使能
             else if (_operationTime == 4)
             {
@@ -409,44 +386,6 @@ namespace MedullaAdapter
             var sendLArm = BitConverter.GetBytes((int)Math.Round(v9 * 512f * 10000f / 1875f));
             var sendRArm = BitConverter.GetBytes((int)Math.Round(v10 * 512f * 10000f / 1875f));
 
-            var wheelCommandSuppressed =
-                cart.AlarmLevel == 2 ||
-                cart.WaitStart ||
-                cart.PreparaStart ||
-                _driversDisabled;
-            var speedLimit = Math.Max(0f, cart.SendThresSpeed);
-
-            cart.SentSpeedLFL = wheelCommandSuppressed ? 0f : lfl;
-            cart.SentSpeedLFR = wheelCommandSuppressed ? 0f : lfr;
-            cart.SentSpeedLRL = wheelCommandSuppressed ? 0f : lrl;
-            cart.SentSpeedLRR = wheelCommandSuppressed ? 0f : lrr;
-            cart.SentSpeedRFL = wheelCommandSuppressed ? 0f : rfl;
-            cart.SentSpeedRFR = wheelCommandSuppressed ? 0f : rfr;
-            cart.SentSpeedRRL = wheelCommandSuppressed ? 0f : rrl;
-            cart.SentSpeedRRR = wheelCommandSuppressed ? 0f : rrr;
-            cart.WheelCommandLimitedLFL =
-                Math.Abs(cart.SpeedLFL) > speedLimit;
-            cart.WheelCommandLimitedLFR =
-                Math.Abs(cart.SpeedLFR) > speedLimit;
-            cart.WheelCommandLimitedLRL =
-                Math.Abs(cart.SpeedLRL) > speedLimit;
-            cart.WheelCommandLimitedLRR =
-                Math.Abs(cart.SpeedLRR) > speedLimit;
-            cart.WheelCommandLimitedRFL =
-                Math.Abs(cart.SpeedRFL) > speedLimit;
-            cart.WheelCommandLimitedRFR =
-                Math.Abs(cart.SpeedRFR) > speedLimit;
-            cart.WheelCommandLimitedRRL =
-                Math.Abs(cart.SpeedRRL) > speedLimit;
-            cart.WheelCommandLimitedRRR =
-                Math.Abs(cart.SpeedRRR) > speedLimit;
-            cart.WheelCommandSuppressed = wheelCommandSuppressed;
-            Interlocked.Exchange(
-                ref cart.WheelCommandTimestamp,
-                Stopwatch.GetTimestamp());
-            Interlocked.Increment(
-                ref cart.WheelCommandSequence);
-
             if (iteration % 2 == 0)
             {
                 //SendNodeGuardRequests(SendCan);
@@ -489,79 +428,6 @@ namespace MedullaAdapter
 
         }
 
-        // M层诊断：根据界面开关创建或关闭本次轮速CSV记录。
-        private void UpdateWheelSpeedDiagnosticState()
-        {
-            if (cart == null)
-                return;
-
-            if (cart.WheelSpeedDiagnosticEnabled)
-            {
-                if (_wheelSpeedLogger.IsRunning)
-                    return;
-
-                try
-                {
-                    var configuredDirectory =
-                        string.IsNullOrWhiteSpace(
-                            cart.WheelSpeedDiagnosticDirectory)
-                            ? @"logs\wheel-speed"
-                            : cart.WheelSpeedDiagnosticDirectory;
-
-                    var logDirectory =
-                        Path.IsPathRooted(configuredDirectory)
-                            ? configuredDirectory
-                            : Path.Combine(
-                                AppContext.BaseDirectory,
-                                configuredDirectory);
-
-                    _wheelSpeedLogger.Start(
-                        logDirectory,
-                        cart.CarNum);
-
-                    cart.WheelSpeedDiagnosticStatus =
-                        "记录中：" +
-                        _wheelSpeedLogger.SnapshotLogPath;
-                }
-                catch (Exception ex)
-                {
-                    cart.WheelSpeedDiagnosticEnabled =
-                        false;
-                    cart.WheelSpeedDiagnosticStatus =
-                        "启动失败：" + ex.Message;
-
-                    Console.WriteLine(
-                        "轮速诊断启动失败：" +
-                        ex.Message);
-                }
-
-                return;
-            }
-
-            if (!_wheelSpeedLogger.IsRunning)
-                return;
-
-            try
-            {
-                var snapshotPath =
-                    _wheelSpeedLogger.SnapshotLogPath;
-
-                _wheelSpeedLogger.Stop();
-
-                cart.WheelSpeedDiagnosticStatus =
-                    "已保存：" + snapshotPath;
-            }
-            catch (Exception ex)
-            {
-                cart.WheelSpeedDiagnosticStatus =
-                    "停止失败：" + ex.Message;
-
-                Console.WriteLine(
-                    "轮速诊断停止失败：" +
-                    ex.Message);
-            }
-        }
-
         // M层CAN安全：判断单个驱动节点是否处于可运行状态。
         private static bool IsNodeOperational(byte remoteCode)
         {
@@ -619,9 +485,7 @@ namespace MedullaAdapter
                     handler(msg);
                 }
             });
-            // _canCallbackRegistered = true;
-            _canCallbackRegistered =
-    err0 == MCUSerialBridgeError.OK;
+            _canCallbackRegistered = true;
         }
 
         // M层串口通信：按需注册电池等串口设备回调。
@@ -737,19 +601,8 @@ namespace MedullaAdapter
                     var payload = msg.Payload;
                     if (payload == null || payload.Length < 8) return;
                     var rpm = DecodeRpmFromPayload(payload);
-                    var speed = ConvertRpm2Mps(rpm);
-                    var positionMillimeters =
-                        ConvertR2MM(
-                            BitConverter.ToInt32(payload, 0) /
-                            10000f);
-                    cart.ActualSpeedLeftFrontLeft = speed;
-                    cart.LFLActualPos = positionMillimeters;
-                    _wheelSpeedLogger.RecordCanFeedback(
-                        0x281,
-                        "LFL",
-                        rpm,
-                        speed,
-                        positionMillimeters);
+                    cart.ActualSpeedLeftFrontLeft = ConvertRpm2Mps(rpm);
+                    cart.LFLActualPos = ConvertR2MM(BitConverter.ToInt32(payload, 0) / 10000f);
                 },
                 [0x282] = (msg) =>
                 {
@@ -757,19 +610,8 @@ namespace MedullaAdapter
                     var payload = msg.Payload;
                     if (payload == null || payload.Length < 8) return;
                     var rpm = DecodeRpmFromPayload(payload);
-                    var speed = -ConvertRpm2Mps(rpm);
-                    var positionMillimeters =
-                        ConvertR2MM(
-                            -BitConverter.ToInt32(payload, 0) /
-                            10000f);
-                    cart.ActualSpeedLeftFrontRight = speed;
-                    cart.LFRActualPos = positionMillimeters;
-                    _wheelSpeedLogger.RecordCanFeedback(
-                        0x282,
-                        "LFR",
-                        rpm,
-                        speed,
-                        positionMillimeters);
+                    cart.ActualSpeedLeftFrontRight = -ConvertRpm2Mps(rpm);
+                    cart.LFRActualPos = ConvertR2MM(-BitConverter.ToInt32(payload, 0) / 10000f);
                 },
                 [0x283] = (msg) =>
                 {
@@ -777,19 +619,8 @@ namespace MedullaAdapter
                     var payload = msg.Payload;
                     if (payload == null || payload.Length < 8) return;
                     var rpm = DecodeRpmFromPayload(payload);
-                    var speed = ConvertRpm2Mps(rpm);
-                    var positionMillimeters =
-                        ConvertR2MM(
-                            BitConverter.ToInt32(payload, 0) /
-                            10000f);
-                    cart.ActualSpeedRightFrontLeft = speed;
-                    cart.RFLActualPos = positionMillimeters;
-                    _wheelSpeedLogger.RecordCanFeedback(
-                        0x283,
-                        "RFL",
-                        rpm,
-                        speed,
-                        positionMillimeters);
+                    cart.ActualSpeedRightFrontLeft = ConvertRpm2Mps(rpm);
+                    cart.RFLActualPos = ConvertR2MM(BitConverter.ToInt32(payload, 0) / 10000f);
                 },
                 [0x284] = (msg) =>
                 {
@@ -797,19 +628,8 @@ namespace MedullaAdapter
                     var payload = msg.Payload;
                     if (payload == null || payload.Length < 8) return;
                     var rpm = DecodeRpmFromPayload(payload);
-                    var speed = -ConvertRpm2Mps(rpm);
-                    var positionMillimeters =
-                        ConvertR2MM(
-                            -BitConverter.ToInt32(payload, 0) /
-                            10000f);
-                    cart.ActualSpeedRightFrontRight = speed;
-                    cart.RFRActualPos = positionMillimeters;
-                    _wheelSpeedLogger.RecordCanFeedback(
-                        0x284,
-                        "RFR",
-                        rpm,
-                        speed,
-                        positionMillimeters);
+                    cart.ActualSpeedRightFrontRight = -ConvertRpm2Mps(rpm);
+                    cart.RFRActualPos = ConvertR2MM(-BitConverter.ToInt32(payload, 0) / 10000f);
                 },
                 [0x285] = (msg) =>
                 {
@@ -817,19 +637,8 @@ namespace MedullaAdapter
                     var payload = msg.Payload;
                     if (payload == null || payload.Length < 8) return;
                     var rpm = DecodeRpmFromPayload(payload);
-                    var speed = ConvertRpm2Mps(rpm);
-                    var positionMillimeters =
-                        ConvertR2MM(
-                            BitConverter.ToInt32(payload, 0) /
-                            10000f);
-                    cart.ActualSpeedLeftRearLeft = speed;
-                    cart.LRLActualPos = positionMillimeters;
-                    _wheelSpeedLogger.RecordCanFeedback(
-                        0x285,
-                        "LRL",
-                        rpm,
-                        speed,
-                        positionMillimeters);
+                    cart.ActualSpeedLeftRearLeft = ConvertRpm2Mps(rpm);
+                    cart.LRLActualPos = ConvertR2MM(BitConverter.ToInt32(payload, 0) / 10000f);
                 },
                 [0x286] = (msg) =>
                 {
@@ -837,19 +646,8 @@ namespace MedullaAdapter
                     var payload = msg.Payload;
                     if (payload == null || payload.Length < 8) return;
                     var rpm = DecodeRpmFromPayload(payload);
-                    var speed = -ConvertRpm2Mps(rpm);
-                    var positionMillimeters =
-                        ConvertR2MM(
-                            -BitConverter.ToInt32(payload, 0) /
-                            10000f);
-                    cart.ActualSpeedLeftRearRight = speed;
-                    cart.LRRActualPos = positionMillimeters;
-                    _wheelSpeedLogger.RecordCanFeedback(
-                        0x286,
-                        "LRR",
-                        rpm,
-                        speed,
-                        positionMillimeters);
+                    cart.ActualSpeedLeftRearRight = -ConvertRpm2Mps(rpm);
+                    cart.LRRActualPos = ConvertR2MM(-BitConverter.ToInt32(payload, 0) / 10000f);
                 },
                 [0x287] = (msg) =>
                 {
@@ -857,19 +655,8 @@ namespace MedullaAdapter
                     var payload = msg.Payload;
                     if (payload == null || payload.Length < 8) return;
                     var rpm = DecodeRpmFromPayload(payload);
-                    var speed = ConvertRpm2Mps(rpm);
-                    var positionMillimeters =
-                        ConvertR2MM(
-                            BitConverter.ToInt32(payload, 0) /
-                            10000f);
-                    cart.ActualSpeedRightRearLeft = speed;
-                    cart.RRLActualPos = positionMillimeters;
-                    _wheelSpeedLogger.RecordCanFeedback(
-                        0x287,
-                        "RRL",
-                        rpm,
-                        speed,
-                        positionMillimeters);
+                    cart.ActualSpeedRightRearLeft = ConvertRpm2Mps(rpm);
+                    cart.RRLActualPos = ConvertR2MM(BitConverter.ToInt32(payload, 0) / 10000f);
                 },
                 [0x288] = (msg) =>
                 {
@@ -877,19 +664,8 @@ namespace MedullaAdapter
                     var payload = msg.Payload;
                     if (payload == null || payload.Length < 8) return;
                     var rpm = DecodeRpmFromPayload(payload);
-                    var speed = -ConvertRpm2Mps(rpm);
-                    var positionMillimeters =
-                        ConvertR2MM(
-                            -BitConverter.ToInt32(payload, 0) /
-                            10000f);
-                    cart.ActualSpeedRightRearRight = speed;
-                    cart.RRRActualPos = positionMillimeters;
-                    _wheelSpeedLogger.RecordCanFeedback(
-                        0x288,
-                        "RRR",
-                        rpm,
-                        speed,
-                        positionMillimeters);
+                    cart.ActualSpeedRightRearRight = -ConvertRpm2Mps(rpm);
+                    cart.RRRActualPos = ConvertR2MM(-BitConverter.ToInt32(payload, 0) / 10000f);
                 },
                 [0x289] = (msg) =>
                 {
@@ -1007,18 +783,10 @@ namespace MedullaAdapter
                     //Console.WriteLine("Received 0x18B CAN Message");
                     var payload = msg.Payload;
                     if (payload == null || payload.Length < 4) return;
-                    var raw = BitConverter.ToInt32(payload, 0);
-                    cart.ActualThLeftFront = raw >= 16384
-                    ? (raw - 98303) / 4096f / 5f * 360 - cart.ThBiasLeftFront
-                    : raw / 4096f / 5f * 360 - cart.ThBiasLeftFront;
-                    MarkFeedbackReceived(
-                        ref cart.ActualThLeftFrontTimestamp,
-                        ref cart.ActualThLeftFrontSequence);
-                    _wheelSpeedLogger.RecordSteeringAngleFeedback(
-                        0x18B,
-                        "LF",
-                        raw,
-                        cart.ActualThLeftFront);
+                    cart.ActualThLeftFront = BitConverter.ToInt32(payload, 0);
+                    cart.ActualThLeftFront = cart.ActualThLeftFront >= 16384
+                    ? (cart.ActualThLeftFront - 98303) / 4096f / 5f * 360 - cart.ThBiasLeftFront
+                    : cart.ActualThLeftFront / 4096f / 5f * 360 - cart.ThBiasLeftFront;
                 },
                 [0x18C] = (msg) =>
                 {
@@ -1028,14 +796,6 @@ namespace MedullaAdapter
                     cart.ActualThRightFront = raw >= 16384
                         ? (raw - 98303) / 4096f / 5f * 360 - cart.ThBiasRightFront
                         : raw / 4096f / 5f * 360 - cart.ThBiasRightFront;
-                    MarkFeedbackReceived(
-                        ref cart.ActualThRightFrontTimestamp,
-                        ref cart.ActualThRightFrontSequence);
-                    _wheelSpeedLogger.RecordSteeringAngleFeedback(
-                        0x18C,
-                        "RF",
-                        raw,
-                        cart.ActualThRightFront);
                     //DLog.Log($"RF raw=0x{raw:X8}({raw}) angle={cart.ActualThRightFront:F2}", "0x18C");
                 },
                 [0x18D] = (msg) =>
@@ -1043,36 +803,20 @@ namespace MedullaAdapter
                     //Console.WriteLine($"Received 0x18D CAN Message {DateTime.Now:yyyy-MM-dd HH:mm:ss.ffffff}"); 
                     var payload = msg.Payload;
                     if (payload == null || payload.Length < 4) return;
-                    var raw = BitConverter.ToInt32(payload, 0);
-                    cart.ActualThLeftRear = raw >= 16384
-                        ? (raw - 98303) / 4096f / 5f * 360 - cart.ThBiasLeftRear
-                        : raw / 4096f / 5f * 360 - cart.ThBiasLeftRear;
-                    MarkFeedbackReceived(
-                        ref cart.ActualThLeftRearTimestamp,
-                        ref cart.ActualThLeftRearSequence);
-                    _wheelSpeedLogger.RecordSteeringAngleFeedback(
-                        0x18D,
-                        "LR",
-                        raw,
-                        cart.ActualThLeftRear);
+                    cart.ActualThLeftRear = BitConverter.ToInt32(payload, 0);
+                    cart.ActualThLeftRear = cart.ActualThLeftRear >= 16384
+                        ? (cart.ActualThLeftRear - 98303) / 4096f / 5f * 360 - cart.ThBiasLeftRear
+                        : cart.ActualThLeftRear / 4096f / 5f * 360 - cart.ThBiasLeftRear;
                 },
                 [0x18E] = (msg) =>
                 {
                     //Console.WriteLine("Received 0x18E CAN Message");
                     var payload = msg.Payload;
                     if (payload == null || payload.Length < 4) return;
-                    var raw = BitConverter.ToInt32(payload, 0);
-                    cart.ActualThRightRear = raw >= 16384
-                        ? (raw - 98303) / 4096f / 5f * 360 - cart.ThBiasRightRear
-                        : raw / 4096f / 5f * 360 - cart.ThBiasRightRear;
-                    MarkFeedbackReceived(
-                        ref cart.ActualThRightRearTimestamp,
-                        ref cart.ActualThRightRearSequence);
-                    _wheelSpeedLogger.RecordSteeringAngleFeedback(
-                        0x18E,
-                        "RR",
-                        raw,
-                        cart.ActualThRightRear);
+                    cart.ActualThRightRear = BitConverter.ToInt32(payload, 0);
+                    cart.ActualThRightRear = cart.ActualThRightRear >= 16384
+                        ? (cart.ActualThRightRear - 98303) / 4096f / 5f * 360 - cart.ThBiasRightRear
+                        : cart.ActualThRightRear / 4096f / 5f * 360 - cart.ThBiasRightRear;
                 },
 
                 // 远程帧

MedullaAdapter/MedullaAdapter.csproj

@@ -32,30 +32,24 @@
       <Private>false</Private>
     </Reference>
 
-    <!-- <Reference Include="CycleGUI">
+    <Reference Include="CycleGUI">
       <HintPath>ref\CycleGUI.dll</HintPath>
       <Private>false</Private>
-    </Reference> -->
+    </Reference>
     <Reference Include="CommonUsage">
       <HintPath>..\ref\CommonUsage.dll</HintPath>
     </Reference>
   </ItemGroup>
 
   <ItemGroup>
-  <Compile Include="..\Shared\Models\MotionModels.cs"
-           Link="Shared\Models\MotionModels.cs" />
+  <Compile Include="..\Shared\ChassisCommand.cs"
+            Link="Shared\ChassisCommand.cs" />
 
-  <Compile Include="..\Shared\Mathematics\FrameTransform2D.cs"
-           Link="Shared\Mathematics\FrameTransform2D.cs" />
+  <Compile Include="..\Shared\FrameTransform2D.cs"
+            Link="Shared\FrameTransform2D.cs" />
 
-  <Compile Include="..\Shared\Mathematics\AngleMath.cs"
-           Link="Shared\Mathematics\AngleMath.cs" />
-
-  <Compile Include="..\Shared\Validation\NumericGuard.cs"
-           Link="Shared\Validation\NumericGuard.cs" />
-
-  <Compile Include="..\Shared\Chassis\MultiWheelChassisAdapter.cs"
-           Link="Shared\Chassis\MultiWheelChassisAdapter.cs" />
+  <Compile Include="..\Shared\MultiWheelChassisAdapter.cs"
+            Link="Shared\MultiWheelChassisAdapter.cs" />
 </ItemGroup>
 
 </Project>

MedullaAdapter/MotorRoutine.cs

@@ -1,43 +1,20 @@
 // 计算8个驱动电机的目标速度和舵角PID
 using CartActivator;
+using CommonUsage.Mathematics;
 using FundamentalLib;
 using MDCSToolBox.Commons;
+using MDCSToolBox.Commons.Controllers;
 using System;
-using System.Diagnostics;
-using System.Threading;
+using System.Collections.Generic;
+using System.Linq;
 using static MDCSToolBox.Medulla.Chassis.BasicCartDefinition;
 
 namespace MedullaAdapter
 {
     public class MotorRoutine : LadderLogic<DiverCartDefinition>
     {
-        private const double MaximumFeedforwardIntervalSeconds = 0.2;
-
-        private sealed class DiffSteerWheelControlState
-        {
-            public float PreviousTargetAngleDegrees;
-            public double FilteredInverseFeedforwardMetersPerSecond;
-            public bool IsInStopZone;
-            public int SettlingCycleCount;
-            public bool IsSettled;
-        }
-
         private bool _wasTransmitterControlling;
         private DateTime _lastMoveTime = DateTime.Now;
-        private bool _diffSteerFeedforwardInitialized;
-        private long _lastDiffSteerFeedforwardTimestamp;
-        private readonly DiffSteerWheelControlState _leftFrontSteerState =
-            new DiffSteerWheelControlState();
-        private readonly DiffSteerWheelControlState _leftRearSteerState =
-            new DiffSteerWheelControlState();
-        private readonly DiffSteerWheelControlState _rightFrontSteerState =
-            new DiffSteerWheelControlState();
-        private readonly DiffSteerWheelControlState _rightRearSteerState =
-            new DiffSteerWheelControlState();
-        private DiverCartDefinition.ManualControlMode?
-            _pendingTransmitterControlMode;
-        private DateTime _pendingTransmitterControlModeSince =
-            DateTime.MinValue;
         public override void Operation(int iteration)
         {
             if (!cart.GhostMode && cart.State == -1) return;
@@ -90,80 +67,30 @@ namespace MedullaAdapter
             cart.ClumsyControl = CartDefinition.currentPriority == 0;
             // 计算四个舵轮PID和8个驱动电机最终速度。
             UpdateDiffSteerWheelSpeeds();
-            Interlocked.Exchange(
-                ref cart.DiffSteerControlTimestamp,
-                Stopwatch.GetTimestamp());
-            Interlocked.Increment(
-                ref cart.DiffSteerControlSequence);
             // 平滑更新硬件速度限制。
             UpdateSendSpeedLimit();
             // 更新红黄绿灯状态。
             UpdateLightMode();
         }
-        // M层物理遥控器：只有SB档位连续稳定指定时间后才确认模式切换。
-        private bool TryGetStableTransmitterControlMode(
-            out DiverCartDefinition.ManualControlMode stableMode)
-        {
-            stableMode = cart.TransmitterControlMode;
-
-            DiverCartDefinition.ManualControlMode? requestedMode;
-
-            switch (cart.Transmitter_SB)
-            {
-                case TransmitterState.Mode0:
-                    requestedMode =
-                        DiverCartDefinition.ManualControlMode.Normal;
-                    break;
-
-                case TransmitterState.Mode1:
-                    requestedMode =
-                        DiverCartDefinition.ManualControlMode.Crab;
-                    break;
-
-                case TransmitterState.Mode2:
-                    requestedMode =
-                        DiverCartDefinition.ManualControlMode.Spin;
-                    break;
-
-                default:
-                    _pendingTransmitterControlMode = null;
-                    _pendingTransmitterControlModeSince =
-                        DateTime.MinValue;
-                    return false;
-            }
-
-            if (_pendingTransmitterControlMode != requestedMode)
-            {
-                _pendingTransmitterControlMode = requestedMode;
-                _pendingTransmitterControlModeSince = DateTime.Now;
-                return false;
-            }
-
-            var debounceMilliseconds = Math.Max(
-                0,
-                cart.TransmitterModeDebounceMilliseconds);
-
-            if ((DateTime.Now -
-                 _pendingTransmitterControlModeSince)
-                .TotalMilliseconds < debounceMilliseconds)
-            {
-                return false;
-            }
-
-            stableMode = requestedMode.Value;
-            return true;
-        }
-
         // 物理遥控器设置
         public void TransmitterChassisControl()
         {
             var interval = DateTime.Now - cart.TransmitterLastTime;
-
-            if (!TryGetStableTransmitterControlMode(
-                    out var stableControlMode))
+            switch (cart.Transmitter_SB)
             {
-                // SB处于中间档或尚未稳定时立即停车，
-                // 保持当前模式，不下发新的模式目标角。
+                case TransmitterState.Mode0:
+                    cart.TransmitterControlMode =
+                    DiverCartDefinition.ManualControlMode.Normal;
+                    break;
+                case TransmitterState.Mode1:
+                    cart.TransmitterControlMode =
+                        DiverCartDefinition.ManualControlMode.Crab;
+                    break;
+                case TransmitterState.Mode2:
+                    cart.TransmitterControlMode =
+                        DiverCartDefinition.ManualControlMode.Spin;
+                    break;
+                default:
                     cart.ManualControl(
                         cart.TransmitterControlMode,
                         0, 0, 0,
@@ -172,9 +99,6 @@ namespace MedullaAdapter
                     StopClampArms();
                     return;
             }
-
-            cart.TransmitterControlMode = stableControlMode;
-
             // SA关闭后立即停车。
             if (!cart.Transmitter_SA)
             {
@@ -241,9 +165,7 @@ namespace MedullaAdapter
             cart.SpeedRightArm = 0;
         }
 
-        /// <summary>
-        /// 根据四个舵轮的目标角速度前馈和实际角度反馈修正八个驱动电机速度。
-        /// </summary>
+        // M层单车底盘：根据四个舵轮的目标角度和实际角度修正8个驱动电机速度。
         private void UpdateDiffSteerWheelSpeeds()
         {
             if (cart.LeftFrontPid == null ||
@@ -259,7 +181,6 @@ namespace MedullaAdapter
                 cart.SpeedLRR = 0;
                 cart.SpeedRRL = 0;
                 cart.SpeedRRR = 0;
-                ResetDiffSteerRateFeedforward();
                 return;
             }
 
@@ -303,54 +224,20 @@ namespace MedullaAdapter
                 cart.DiffSteerThresh,
                 cart.DiffSteerSpeedAcc);
 
-            // 根据实际舵角计算四条腿的PID反馈修正量。
-            var feedbackLf = cart.LeftFrontPid.GetResponse(
+            // 根据实际舵角计算四条腿的差速修正量。
+            var diffLf = cart.LeftFrontPid.GetResponse(
                 cart.ThLeftFront, false, false, "LF");
 
-            var feedbackLr = cart.LeftRearPid.GetResponse(
+            var diffLr = cart.LeftRearPid.GetResponse(
                 cart.ThLeftRear, false, false, "LR");
 
-            var feedbackRf = cart.RightFrontPid.GetResponse(
+            var diffRf = cart.RightFrontPid.GetResponse(
                 cart.ThRightFront, false, false, "RF");
 
-            var feedbackRr = cart.RightRearPid.GetResponse(
+            var diffRr = cart.RightRearPid.GetResponse(
                 cart.ThRightRear, false, false, "RR");
 
-            CalculateDiffSteerRateFeedforward(
-                out var feedforwardLf,
-                out var feedforwardLr,
-                out var feedforwardRf,
-                out var feedforwardRr,
-                out var suppressLf,
-                out var suppressLr,
-                out var suppressRf,
-                out var suppressRr);
-
-            if (suppressLf) feedbackLf = 0f;
-            if (suppressLr) feedbackLr = 0f;
-            if (suppressRf) feedbackRf = 0f;
-            if (suppressRr) feedbackRr = 0f;
-
-            var diffLf = feedbackLf + feedforwardLf;
-            var diffLr = feedbackLr + feedforwardLr;
-            var diffRf = feedbackRf + feedforwardRf;
-            var diffRr = feedbackRr + feedforwardRr;
-
-            // 分别保留PID、前馈和合成差速，便于独立标定与诊断。
-            cart.DiffSteerOutputLeftFront = feedbackLf;
-            cart.DiffSteerOutputLeftRear = feedbackLr;
-            cart.DiffSteerOutputRightFront = feedbackRf;
-            cart.DiffSteerOutputRightRear = feedbackRr;
-            cart.DiffSteerRateFeedforwardLeftFront = feedforwardLf;
-            cart.DiffSteerRateFeedforwardLeftRear = feedforwardLr;
-            cart.DiffSteerRateFeedforwardRightFront = feedforwardRf;
-            cart.DiffSteerRateFeedforwardRightRear = feedforwardRr;
-            cart.DiffSteerTotalOutputLeftFront = diffLf;
-            cart.DiffSteerTotalOutputLeftRear = diffLr;
-            cart.DiffSteerTotalOutputRightFront = diffRf;
-            cart.DiffSteerTotalOutputRightRear = diffRr;
-
-            // 左前腿：左右电机施加方向相反的合成差速修正量。
+            // 左前腿：左右电机施加方向相反的PID修正量。
             cart.SpeedLFL = cart.SpeedLeftFrontLeft - diffLf;
             cart.SpeedLFR = cart.SpeedLeftFrontRight + diffLf;
 
@@ -367,444 +254,6 @@ namespace MedullaAdapter
             cart.SpeedRRR = cart.SpeedRightRearRight + diffRr;
         }
 
-        /// <summary>
-        /// 更新四轮独立的到位迟滞状态，并计算模型逆前馈。
-        /// </summary>
-        private void CalculateDiffSteerRateFeedforward(
-            out float leftFront,
-            out float leftRear,
-            out float rightFront,
-            out float rightRear,
-            out bool suppressLeftFront,
-            out bool suppressLeftRear,
-            out bool suppressRightFront,
-            out bool suppressRightRear)
-        {
-            leftFront = leftRear = rightFront = rightRear = 0f;
-            suppressLeftFront = suppressLeftRear = false;
-            suppressRightFront = suppressRightRear = false;
-            ResetDiffSteerCycleDiagnostics();
-
-            var currentTimestamp = Stopwatch.GetTimestamp();
-            var deltaTimeSeconds = 0.0;
-            var hasValidControlPeriod = false;
-
-            if (_diffSteerFeedforwardInitialized)
-            {
-                deltaTimeSeconds =
-                    (currentTimestamp -
-                     _lastDiffSteerFeedforwardTimestamp) /
-                    (double)Stopwatch.Frequency;
-
-                if (double.IsFinite(deltaTimeSeconds) &&
-                    deltaTimeSeconds > 0.0)
-                {
-                    cart.DiffSteerFeedforwardDeltaTimeMilliseconds =
-                        (float)(deltaTimeSeconds * 1000.0);
-                    hasValidControlPeriod =
-                        deltaTimeSeconds <=
-                        MaximumFeedforwardIntervalSeconds;
-                }
-            }
-
-            suppressLeftFront = UpdateDiffSteerSettlingState(
-                _leftFrontSteerState,
-                cart.ThLeftFront,
-                cart.ActualThLeftFront,
-                hasValidControlPeriod);
-            suppressLeftRear = UpdateDiffSteerSettlingState(
-                _leftRearSteerState,
-                cart.ThLeftRear,
-                cart.ActualThLeftRear,
-                hasValidControlPeriod);
-            suppressRightFront = UpdateDiffSteerSettlingState(
-                _rightFrontSteerState,
-                cart.ThRightFront,
-                cart.ActualThRightFront,
-                hasValidControlPeriod);
-            suppressRightRear = UpdateDiffSteerSettlingState(
-                _rightRearSteerState,
-                cart.ThRightRear,
-                cart.ActualThRightRear,
-                hasValidControlPeriod);
-
-            leftFront = CalculateDiffSteerRateFeedforward(
-                _leftFrontSteerState,
-                cart.ThLeftFront,
-                deltaTimeSeconds,
-                hasValidControlPeriod,
-                suppressLeftFront,
-                out var targetRateLf,
-                out var rawLf,
-                out var limitedLf);
-            leftRear = CalculateDiffSteerRateFeedforward(
-                _leftRearSteerState,
-                cart.ThLeftRear,
-                deltaTimeSeconds,
-                hasValidControlPeriod,
-                suppressLeftRear,
-                out var targetRateLr,
-                out var rawLr,
-                out var limitedLr);
-            rightFront = CalculateDiffSteerRateFeedforward(
-                _rightFrontSteerState,
-                cart.ThRightFront,
-                deltaTimeSeconds,
-                hasValidControlPeriod,
-                suppressRightFront,
-                out var targetRateRf,
-                out var rawRf,
-                out var limitedRf);
-            rightRear = CalculateDiffSteerRateFeedforward(
-                _rightRearSteerState,
-                cart.ThRightRear,
-                deltaTimeSeconds,
-                hasValidControlPeriod,
-                suppressRightRear,
-                out var targetRateRr,
-                out var rawRr,
-                out var limitedRr);
-
-            cart.DiffSteerTargetRateLeftFrontDegreesPerSecond = targetRateLf;
-            cart.DiffSteerTargetRateLeftRearDegreesPerSecond = targetRateLr;
-            cart.DiffSteerTargetRateRightFrontDegreesPerSecond = targetRateRf;
-            cart.DiffSteerTargetRateRightRearDegreesPerSecond = targetRateRr;
-            cart.DiffSteerInverseFeedforwardRawLeftFront = rawLf;
-            cart.DiffSteerInverseFeedforwardRawLeftRear = rawLr;
-            cart.DiffSteerInverseFeedforwardRawRightFront = rawRf;
-            cart.DiffSteerInverseFeedforwardRawRightRear = rawRr;
-            cart.DiffSteerFeedforwardLimitedLeftFront = limitedLf;
-            cart.DiffSteerFeedforwardLimitedLeftRear = limitedLr;
-            cart.DiffSteerFeedforwardLimitedRightFront = limitedRf;
-            cart.DiffSteerFeedforwardLimitedRightRear = limitedRr;
-
-            // 无论周期是否有效，都保存本周期目标，避免补算过期阶跃。
-            _leftFrontSteerState.PreviousTargetAngleDegrees =
-                cart.ThLeftFront;
-            _leftRearSteerState.PreviousTargetAngleDegrees =
-                cart.ThLeftRear;
-            _rightFrontSteerState.PreviousTargetAngleDegrees =
-                cart.ThRightFront;
-            _rightRearSteerState.PreviousTargetAngleDegrees =
-                cart.ThRightRear;
-            _lastDiffSteerFeedforwardTimestamp = currentTimestamp;
-            _diffSteerFeedforwardInitialized = true;
-            UpdateDiffSteerStateDiagnostics();
-        }
-
-        /// <summary>
-        /// 更新单个舵轮的停止区、连续确认和重新启动状态。
-        /// </summary>
-        private bool UpdateDiffSteerSettlingState(
-            DiffSteerWheelControlState state,
-            float targetAngleDegrees,
-            float actualAngleDegrees,
-            bool hasValidControlPeriod)
-        {
-            var stopErrorDegrees = cart.DiffSteerStopErrorDegrees;
-            var restartErrorDegrees = cart.DiffSteerRestartErrorDegrees;
-            var settlingCycles = cart.DiffSteerSettlingCycles;
-            var hasValidAngles =
-                IsFinite(targetAngleDegrees) &&
-                IsFinite(actualAngleDegrees);
-            var hasValidParameters =
-                IsFinite(stopErrorDegrees) &&
-                stopErrorDegrees >= 0f &&
-                IsFinite(restartErrorDegrees) &&
-                restartErrorDegrees > stopErrorDegrees &&
-                settlingCycles > 0;
-
-            if (!hasValidAngles)
-            {
-                state.IsInStopZone = false;
-                state.SettlingCycleCount = 0;
-                state.IsSettled = false;
-                ClearDiffSteerFeedforwardState(state);
-                return true;
-            }
-
-            var absoluteErrorDegrees = Math.Abs(
-                (double)targetAngleDegrees - actualAngleDegrees);
-            state.IsInStopZone =
-                hasValidParameters &&
-                absoluteErrorDegrees <= stopErrorDegrees;
-
-            if (!cart.EnableDiffSteerSettlingHysteresis ||
-                !hasValidParameters)
-            {
-                state.SettlingCycleCount = 0;
-                state.IsSettled = false;
-                return false;
-            }
-
-            if (state.IsSettled)
-            {
-                if (absoluteErrorDegrees <= restartErrorDegrees)
-                {
-                    ClearDiffSteerFeedforwardState(state);
-                    return true;
-                }
-
-                state.IsSettled = false;
-                state.SettlingCycleCount = 0;
-            }
-
-            if (!state.IsInStopZone)
-            {
-                state.SettlingCycleCount = 0;
-                return false;
-            }
-
-            ClearDiffSteerFeedforwardState(state);
-            if (hasValidControlPeriod)
-            {
-                state.SettlingCycleCount = Math.Min(
-                    state.SettlingCycleCount + 1,
-                    settlingCycles);
-                state.IsSettled =
-                    state.SettlingCycleCount >= settlingCycles;
-            }
-            else
-            {
-                state.SettlingCycleCount = 0;
-            }
-
-            return true;
-        }
-
-        /// <summary>
-        /// 将目标机械舵角变化率转换为带一阶滤波的对象逆前馈。
-        /// </summary>
-        private float CalculateDiffSteerRateFeedforward(
-            DiffSteerWheelControlState state,
-            float targetAngleDegrees,
-            double deltaTimeSeconds,
-            bool hasValidControlPeriod,
-            bool suppressOutput,
-            out float targetRateDegreesPerSecond,
-            out float rawInverseFeedforward,
-            out bool limited)
-        {
-            targetRateDegreesPerSecond = 0f;
-            rawInverseFeedforward = 0f;
-            limited = false;
-
-            if (!hasValidControlPeriod ||
-                !IsFinite(targetAngleDegrees) ||
-                !IsFinite(state.PreviousTargetAngleDegrees) ||
-                !double.IsFinite(deltaTimeSeconds) ||
-                deltaTimeSeconds <= 0.0)
-            {
-                ClearDiffSteerFeedforwardState(state);
-                return 0f;
-            }
-
-            // 机械舵角受限，必须使用直接差值而不是圆周最短角差。
-            var targetRate =
-                ((double)targetAngleDegrees -
-                 state.PreviousTargetAngleDegrees) /
-                deltaTimeSeconds;
-            if (!double.IsFinite(targetRate))
-            {
-                ClearDiffSteerFeedforwardState(state);
-                return 0f;
-            }
-
-            targetRateDegreesPerSecond = (float)targetRate;
-            if (!IsFinite(targetRateDegreesPerSecond))
-            {
-                targetRateDegreesPerSecond = 0f;
-                ClearDiffSteerFeedforwardState(state);
-                return 0f;
-            }
-
-            var plantGain = cart.DiffSteerPlantGain;
-            var timeConstantSeconds =
-                cart.DiffSteerInverseFeedforwardTimeConstantSeconds;
-            var maximumSpeed =
-                cart.DiffSteerRateFeedforwardMaximumSpeed;
-
-            if (!IsFinite(plantGain) || plantGain <= 0f ||
-                !IsFinite(timeConstantSeconds) ||
-                timeConstantSeconds <= 0f ||
-                !IsFinite(maximumSpeed) || maximumSpeed <= 0f)
-            {
-                ClearDiffSteerFeedforwardState(state);
-                return 0f;
-            }
-
-            var inverseFeedforward = targetRate / plantGain;
-            if (!double.IsFinite(inverseFeedforward))
-            {
-                ClearDiffSteerFeedforwardState(state);
-                return 0f;
-            }
-
-            rawInverseFeedforward = (float)inverseFeedforward;
-            if (!IsFinite(rawInverseFeedforward))
-            {
-                rawInverseFeedforward = 0f;
-                ClearDiffSteerFeedforwardState(state);
-                return 0f;
-            }
-
-            if (!cart.EnableDiffSteerInverseFeedforward ||
-                suppressOutput)
-            {
-                ClearDiffSteerFeedforwardState(state);
-                return 0f;
-            }
-
-            var alpha = 1.0 - Math.Exp(
-                -deltaTimeSeconds / timeConstantSeconds);
-            var filteredFeedforward =
-                state.FilteredInverseFeedforwardMetersPerSecond +
-                alpha *
-                (inverseFeedforward -
-                 state.FilteredInverseFeedforwardMetersPerSecond);
-            if (!double.IsFinite(alpha) ||
-                !double.IsFinite(filteredFeedforward))
-            {
-                ClearDiffSteerFeedforwardState(state);
-                return 0f;
-            }
-
-            state.FilteredInverseFeedforwardMetersPerSecond =
-                filteredFeedforward;
-
-            var limitedSpeed = (float)Math.Clamp(
-                filteredFeedforward,
-                -maximumSpeed,
-                maximumSpeed);
-            limited = Math.Abs(
-                filteredFeedforward - limitedSpeed) > 1e-9;
-            return limitedSpeed;
-        }
-
-        private void ResetDiffSteerCycleDiagnostics()
-        {
-            cart.DiffSteerTargetRateLeftFrontDegreesPerSecond = 0f;
-            cart.DiffSteerTargetRateLeftRearDegreesPerSecond = 0f;
-            cart.DiffSteerTargetRateRightFrontDegreesPerSecond = 0f;
-            cart.DiffSteerTargetRateRightRearDegreesPerSecond = 0f;
-            cart.DiffSteerFeedforwardDeltaTimeMilliseconds = 0f;
-            cart.DiffSteerInverseFeedforwardRawLeftFront = 0f;
-            cart.DiffSteerInverseFeedforwardRawLeftRear = 0f;
-            cart.DiffSteerInverseFeedforwardRawRightFront = 0f;
-            cart.DiffSteerInverseFeedforwardRawRightRear = 0f;
-            cart.DiffSteerFeedforwardLimitedLeftFront = false;
-            cart.DiffSteerFeedforwardLimitedLeftRear = false;
-            cart.DiffSteerFeedforwardLimitedRightFront = false;
-            cart.DiffSteerFeedforwardLimitedRightRear = false;
-        }
-
-        /// <summary>
-        /// 将四轮独立控制状态复制到M层监控和CSV数据源。
-        /// </summary>
-        private void UpdateDiffSteerStateDiagnostics()
-        {
-            cart.DiffSteerInStopZoneLeftFront =
-                _leftFrontSteerState.IsInStopZone;
-            cart.DiffSteerInStopZoneLeftRear =
-                _leftRearSteerState.IsInStopZone;
-            cart.DiffSteerInStopZoneRightFront =
-                _rightFrontSteerState.IsInStopZone;
-            cart.DiffSteerInStopZoneRightRear =
-                _rightRearSteerState.IsInStopZone;
-            cart.DiffSteerSettlingCountLeftFront =
-                _leftFrontSteerState.SettlingCycleCount;
-            cart.DiffSteerSettlingCountLeftRear =
-                _leftRearSteerState.SettlingCycleCount;
-            cart.DiffSteerSettlingCountRightFront =
-                _rightFrontSteerState.SettlingCycleCount;
-            cart.DiffSteerSettlingCountRightRear =
-                _rightRearSteerState.SettlingCycleCount;
-            cart.DiffSteerSettledLeftFront =
-                _leftFrontSteerState.IsSettled;
-            cart.DiffSteerSettledLeftRear =
-                _leftRearSteerState.IsSettled;
-            cart.DiffSteerSettledRightFront =
-                _rightFrontSteerState.IsSettled;
-            cart.DiffSteerSettledRightRear =
-                _rightRearSteerState.IsSettled;
-            cart.DiffSteerInverseFeedforwardFilteredLeftFront =
-                (float)_leftFrontSteerState
-                    .FilteredInverseFeedforwardMetersPerSecond;
-            cart.DiffSteerInverseFeedforwardFilteredLeftRear =
-                (float)_leftRearSteerState
-                    .FilteredInverseFeedforwardMetersPerSecond;
-            cart.DiffSteerInverseFeedforwardFilteredRightFront =
-                (float)_rightFrontSteerState
-                    .FilteredInverseFeedforwardMetersPerSecond;
-            cart.DiffSteerInverseFeedforwardFilteredRightRear =
-                (float)_rightRearSteerState
-                    .FilteredInverseFeedforwardMetersPerSecond;
-        }
-
-        private static void ClearDiffSteerFeedforwardState(
-            DiffSteerWheelControlState state)
-        {
-            state.FilteredInverseFeedforwardMetersPerSecond = 0.0;
-        }
-
-        /// <summary>
-        /// 清除差速转舵前馈历史和监控输出，避免恢复控制时使用过期目标角。
-        /// </summary>
-        private void ResetDiffSteerRateFeedforward()
-        {
-            _diffSteerFeedforwardInitialized = false;
-            _lastDiffSteerFeedforwardTimestamp = 0;
-            ResetDiffSteerWheelState(_leftFrontSteerState);
-            ResetDiffSteerWheelState(_leftRearSteerState);
-            ResetDiffSteerWheelState(_rightFrontSteerState);
-            ResetDiffSteerWheelState(_rightRearSteerState);
-            cart.DiffSteerOutputLeftFront = 0f;
-            cart.DiffSteerOutputLeftRear = 0f;
-            cart.DiffSteerOutputRightFront = 0f;
-            cart.DiffSteerOutputRightRear = 0f;
-            cart.DiffSteerRateFeedforwardLeftFront = 0f;
-            cart.DiffSteerRateFeedforwardLeftRear = 0f;
-            cart.DiffSteerRateFeedforwardRightFront = 0f;
-            cart.DiffSteerRateFeedforwardRightRear = 0f;
-            cart.DiffSteerTargetRateLeftFrontDegreesPerSecond = 0f;
-            cart.DiffSteerTargetRateLeftRearDegreesPerSecond = 0f;
-            cart.DiffSteerTargetRateRightFrontDegreesPerSecond = 0f;
-            cart.DiffSteerTargetRateRightRearDegreesPerSecond = 0f;
-            cart.DiffSteerFeedforwardDeltaTimeMilliseconds = 0f;
-            cart.DiffSteerInverseFeedforwardRawLeftFront = 0f;
-            cart.DiffSteerInverseFeedforwardRawLeftRear = 0f;
-            cart.DiffSteerInverseFeedforwardRawRightFront = 0f;
-            cart.DiffSteerInverseFeedforwardRawRightRear = 0f;
-            cart.DiffSteerFeedforwardLimitedLeftFront = false;
-            cart.DiffSteerFeedforwardLimitedLeftRear = false;
-            cart.DiffSteerFeedforwardLimitedRightFront = false;
-            cart.DiffSteerFeedforwardLimitedRightRear = false;
-            cart.DiffSteerTotalOutputLeftFront = 0f;
-            cart.DiffSteerTotalOutputLeftRear = 0f;
-            cart.DiffSteerTotalOutputRightFront = 0f;
-            cart.DiffSteerTotalOutputRightRear = 0f;
-            UpdateDiffSteerStateDiagnostics();
-        }
-
-        private static void ResetDiffSteerWheelState(
-            DiffSteerWheelControlState state)
-        {
-            state.PreviousTargetAngleDegrees = 0f;
-            state.FilteredInverseFeedforwardMetersPerSecond = 0.0;
-            state.IsInStopZone = false;
-            state.SettlingCycleCount = 0;
-            state.IsSettled = false;
-        }
-
-        /// <summary>
-        /// 判断单精度参数是否可安全参与底盘控制计算。
-        /// </summary>
-        private static bool IsFinite(float value)
-        {
-            return !float.IsNaN(value) &&
-                   !float.IsInfinity(value);
-        }
-
         // M层单车限速：按照加速度和减速度平滑更新实际下发速度上限。
         private void UpdateSendSpeedLimit()
         {

MedullaAdapter/WheelSpeedDiagnosticLogger.cs

@@ -1,692 +0,0 @@
-using System;
-using System.Collections.Concurrent;
-using System.Diagnostics;
-using System.Globalization;
-using System.IO;
-using System.Text;
-using System.Threading;
-
-namespace MedullaAdapter
-{
-    /// <summary>
-    /// 在后台保存驱动器CAN速度事件和底盘周期快照，避免文件IO阻塞CAN回调。
-    /// </summary>
-    internal sealed class WheelSpeedDiagnosticLogger : IDisposable
-    {
-        private readonly struct LogRecord
-        {
-            public LogRecord(bool isCanEvent, string line)
-            {
-                IsCanEvent = isCanEvent;
-                Line = line;
-            }
-
-            public bool IsCanEvent { get; }
-
-            public string Line { get; }
-        }
-
-        private const int MaximumQueuedRecords = 100000;
-        private const double SnapshotIntervalMilliseconds = 20.0;
-        private readonly ConcurrentQueue<LogRecord> _records = new();
-        private readonly AutoResetEvent _recordsAvailable = new(false);
-        private readonly object _lifecycleLock = new();
-        private Stopwatch _stopwatch;
-        private Thread _writerThread;
-        private StreamWriter _canWriter;
-        private StreamWriter _snapshotWriter;
-        private volatile bool _isRunning;
-        private int _queuedRecordCount;
-        private long _receiveSequence;
-        private long _snapshotSequence;
-        private long _droppedRecordCount;
-        private double _lastSnapshotMilliseconds = double.NegativeInfinity;
-        private long _startTimestamp;
-
-        public bool IsRunning => _isRunning;
-
-        public string CanLogPath { get; private set; } = "";
-
-        public string SnapshotLogPath { get; private set; } = "";
-
-        /// <summary>
-        /// 创建本次诊断的两个CSV文件并启动后台写入线程。
-        /// </summary>
-        public void Start(string directory, int carNumber)
-        {
-            lock (_lifecycleLock)
-            {
-                if (_isRunning)
-                    return;
-
-                if (string.IsNullOrWhiteSpace(directory))
-                    throw new ArgumentException(
-                        "轮速诊断目录不能为空。",
-                        nameof(directory));
-
-                Directory.CreateDirectory(directory);
-
-                var filePrefix =
-                    $"{DateTime.Now:yyyyMMdd_HHmmss_fff}_Car{carNumber}";
-
-                CanLogPath = Path.Combine(
-                    directory,
-                    $"{filePrefix}_can.csv");
-
-                SnapshotLogPath = Path.Combine(
-                    directory,
-                    $"{filePrefix}_snapshot.csv");
-
-                _canWriter = CreateWriter(CanLogPath);
-                _snapshotWriter = CreateWriter(SnapshotLogPath);
-
-                _canWriter.WriteLine(
-                    "ElapsedMs,ReceiveSequence,CanId,EventType,ChannelName," +
-                    "RawRpm,SpeedMps,PositionMm,RawAngle,AngleDegrees");
-
-                _snapshotWriter.WriteLine(
-                    "ElapsedMs,SnapshotSequence," +
-                    "ControlElapsedMs,ControlSequence,ControlAgeMs," +
-                    "WheelCommandElapsedMs,WheelCommandSequence,WheelCommandAgeMs," +
-                    "CarNum,ManualControlMode,ManualMode,SendThresSpeed," +
-                    "VoltageV,AlarmLevel,ChassisMode,WheelAbleState," +
-                    "DiffSteerKp,DiffSteerKi,DiffSteerKd,DiffSteerMaxI,DiffSteerDeadZone,DiffSteerThresh,DiffSteerSpeedAcc," +
-                    "DiffSteerRateFeedforwardGain,DiffSteerWheelDistanceMillimeters,DiffSteerRateFeedforwardMaximumSpeed," +
-                    "EnableDiffSteerSettlingHysteresis,DiffSteerStopErrorDegrees,DiffSteerRestartErrorDegrees,DiffSteerSettlingCycles," +
-                    "EnableDiffSteerInverseFeedforward,DiffSteerPlantGain,DiffSteerInverseFeedforwardTimeConstantSeconds," +
-                    "FeedforwardDeltaTimeMs,DiffSteerDeltaTimeSeconds," +
-                    "TargetRateThLeftFrontDegreesPerSecond,TargetRateThLeftRearDegreesPerSecond," +
-                    "TargetRateThRightFrontDegreesPerSecond,TargetRateThRightRearDegreesPerSecond," +
-                    "InverseFeedforwardRawLeftFront,InverseFeedforwardRawLeftRear,InverseFeedforwardRawRightFront,InverseFeedforwardRawRightRear," +
-                    "InverseFeedforwardFilteredLeftFront,InverseFeedforwardFilteredLeftRear,InverseFeedforwardFilteredRightFront,InverseFeedforwardFilteredRightRear," +
-                    "FeedforwardLimitedLeftFront,FeedforwardLimitedLeftRear,FeedforwardLimitedRightFront,FeedforwardLimitedRightRear," +
-                    "InStopZoneLeftFront,InStopZoneLeftRear,InStopZoneRightFront,InStopZoneRightRear," +
-                    "SettlingCountLeftFront,SettlingCountLeftRear,SettlingCountRightFront,SettlingCountRightRear," +
-                    "SettledLeftFront,SettledLeftRear,SettledRightFront,SettledRightRear," +
-                    "PidOutLeftFront,PidOutLeftRear,PidOutRightFront,PidOutRightRear," +
-                    "RateFeedforwardLeftFront,RateFeedforwardLeftRear,RateFeedforwardRightFront,RateFeedforwardRightRear," +
-                    "TotalDiffLeftFront,TotalDiffLeftRear,TotalDiffRightFront,TotalDiffRightRear," +
-                    "CmdLFL,CmdLFR,CmdLRL,CmdLRR,CmdRFL,CmdRFR,CmdRRL,CmdRRR," +
-                    "PidLFL,PidLFR,PidLRL,PidLRR,PidRFL,PidRFR,PidRRL,PidRRR," +
-                    "SentLFLMps,SentLFRMps,SentLRLMps,SentLRRMps,SentRFLMps,SentRFRMps,SentRRLMps,SentRRRMps," +
-                    "CommandLimitedLFL,CommandLimitedLFR,CommandLimitedLRL,CommandLimitedLRR," +
-                    "CommandLimitedRFL,CommandLimitedRFR,CommandLimitedRRL,CommandLimitedRRR," +
-                    "PairCommandLimitedLeftFront,PairCommandLimitedLeftRear," +
-                    "PairCommandLimitedRightFront,PairCommandLimitedRightRear,WheelCommandSuppressed," +
-                    "ActualLFL,ActualLFR,ActualLRL,ActualLRR,ActualRFL,ActualRFR,ActualRRL,ActualRRR," +
-                    "ActualLeftFront,ActualLeftRear,ActualRightFront,ActualRightRear," +
-                    "PositionLFL,PositionLFR,PositionLRL,PositionLRR,PositionRFL,PositionRFR,PositionRRL,PositionRRR," +
-                    "CurrentLFLAmps,CurrentLFRAmps,CurrentLRLAmps,CurrentLRRAmps," +
-                    "CurrentRFLAmps,CurrentRFRAmps,CurrentRRLAmps,CurrentRRRAmps," +
-                    "TargetThLeftFront,TargetThLeftRear,TargetThRightFront,TargetThRightRear," +
-                    "ActualThLeftFront,ActualThLeftRear,ActualThRightFront,ActualThRightRear," +
-                    "ErrorThLeftFront,ErrorThLeftRear,ErrorThRightFront,ErrorThRightRear," +
-                    "ActualThLeftFrontReceiveElapsedMs,ActualThLeftFrontReceiveSequence,ActualThLeftFrontAgeMs," +
-                    "ActualThLeftRearReceiveElapsedMs,ActualThLeftRearReceiveSequence,ActualThLeftRearAgeMs," +
-                    "ActualThRightFrontReceiveElapsedMs,ActualThRightFrontReceiveSequence,ActualThRightFrontAgeMs," +
-                    "ActualThRightRearReceiveElapsedMs,ActualThRightRearReceiveSequence,ActualThRightRearAgeMs");
-
-                while (_records.TryDequeue(out _))
-                {
-                }
-
-                _queuedRecordCount = 0;
-                _receiveSequence = 0;
-                _snapshotSequence = 0;
-                _droppedRecordCount = 0;
-                _lastSnapshotMilliseconds =
-                    double.NegativeInfinity;
-                _startTimestamp = Stopwatch.GetTimestamp();
-                _stopwatch = Stopwatch.StartNew();
-                _isRunning = true;
-
-                _writerThread = new Thread(WriterLoop)
-                {
-                    IsBackground = true,
-                    Name = "WheelSpeedDiagnosticWriter"
-                };
-                _writerThread.Start();
-            }
-        }
-
-        /// <summary>
-        /// 停止记录并等待队列中的诊断数据写入磁盘。
-        /// </summary>
-        public void Stop()
-        {
-            Thread writerThread;
-
-            lock (_lifecycleLock)
-            {
-                if (!_isRunning &&
-                    _writerThread == null)
-                    return;
-
-                _isRunning = false;
-                writerThread = _writerThread;
-                _recordsAvailable.Set();
-            }
-
-            writerThread?.Join(3000);
-
-            lock (_lifecycleLock)
-            {
-                _canWriter?.Flush();
-                _snapshotWriter?.Flush();
-                _canWriter?.Dispose();
-                _snapshotWriter?.Dispose();
-                _canWriter = null;
-                _snapshotWriter = null;
-                _writerThread = null;
-                _stopwatch?.Stop();
-            }
-        }
-
-        /// <summary>
-        /// 将一帧驱动器速度反馈加入内存队列，不在CAN回调中执行文件写入。
-        /// </summary>
-        public void RecordCanFeedback(
-            ushort canId,
-            string motorName,
-            float rawRpm,
-            float speedMetersPerSecond,
-            float positionMillimeters)
-        {
-            if (!_isRunning)
-                return;
-
-            var elapsedMilliseconds =
-                GetElapsedMilliseconds(
-                    Stopwatch.GetTimestamp());
-            var receiveSequence =
-                Interlocked.Increment(
-                    ref _receiveSequence);
-
-            var line = string.Join(
-                ",",
-                Format(elapsedMilliseconds),
-                receiveSequence.ToString(
-                    CultureInfo.InvariantCulture),
-                $"0x{canId:X3}",
-                "MotorSpeedPosition",
-                motorName,
-                Format(rawRpm),
-                Format(speedMetersPerSecond),
-                Format(positionMillimeters),
-                "",
-                "");
-
-            Enqueue(new LogRecord(
-                isCanEvent: true,
-                line));
-        }
-
-        /// <summary>
-        /// 按CAN回调到达时刻记录一帧舵角原始值和换算后的机械角度。
-        /// </summary>
-        public void RecordSteeringAngleFeedback(
-            ushort canId,
-            string wheelName,
-            int rawAngle,
-            float angleDegrees)
-        {
-            if (!_isRunning)
-                return;
-
-            var elapsedMilliseconds =
-                GetElapsedMilliseconds(
-                    Stopwatch.GetTimestamp());
-            var receiveSequence =
-                Interlocked.Increment(
-                    ref _receiveSequence);
-
-            var line = string.Join(
-                ",",
-                Format(elapsedMilliseconds),
-                receiveSequence.ToString(
-                    CultureInfo.InvariantCulture),
-                $"0x{canId:X3}",
-                "SteeringAngle",
-                wheelName,
-                "",
-                "",
-                "",
-                rawAngle.ToString(
-                    CultureInfo.InvariantCulture),
-                Format(angleDegrees));
-
-            Enqueue(new LogRecord(
-                isCanEvent: true,
-                line));
-        }
-
-        /// <summary>
-        /// 按最多50Hz记录一帧控制命令、PID输出、CAN反馈和舵角快照。
-        /// </summary>
-        public void RecordSnapshot(
-            DiverCartDefinition cart)
-        {
-            if (!_isRunning || cart == null)
-                return;
-
-            var snapshotTimestamp = Stopwatch.GetTimestamp();
-            var elapsedMilliseconds =
-                GetElapsedMilliseconds(snapshotTimestamp);
-
-            if (elapsedMilliseconds -
-                _lastSnapshotMilliseconds <
-                SnapshotIntervalMilliseconds)
-            {
-                return;
-            }
-
-            _lastSnapshotMilliseconds =
-                elapsedMilliseconds;
-
-            var snapshotSequence =
-                Interlocked.Increment(
-                    ref _snapshotSequence);
-            var controlTimestamp =
-                Interlocked.Read(
-                    ref cart.DiffSteerControlTimestamp);
-            var controlSequence =
-                Interlocked.Read(
-                    ref cart.DiffSteerControlSequence);
-            var commandTimestamp =
-                Interlocked.Read(
-                    ref cart.WheelCommandTimestamp);
-            var commandSequence =
-                Interlocked.Read(
-                    ref cart.WheelCommandSequence);
-            var actualThLeftFrontTimestamp =
-                Interlocked.Read(
-                    ref cart.ActualThLeftFrontTimestamp);
-            var actualThLeftFrontSequence =
-                Interlocked.Read(
-                    ref cart.ActualThLeftFrontSequence);
-            var actualThLeftRearTimestamp =
-                Interlocked.Read(
-                    ref cart.ActualThLeftRearTimestamp);
-            var actualThLeftRearSequence =
-                Interlocked.Read(
-                    ref cart.ActualThLeftRearSequence);
-            var actualThRightFrontTimestamp =
-                Interlocked.Read(
-                    ref cart.ActualThRightFrontTimestamp);
-            var actualThRightFrontSequence =
-                Interlocked.Read(
-                    ref cart.ActualThRightFrontSequence);
-            var actualThRightRearTimestamp =
-                Interlocked.Read(
-                    ref cart.ActualThRightRearTimestamp);
-            var actualThRightRearSequence =
-                Interlocked.Read(
-                    ref cart.ActualThRightRearSequence);
-
-            var line = string.Join(
-                ",",
-                Format(elapsedMilliseconds),
-                snapshotSequence.ToString(
-                    CultureInfo.InvariantCulture),
-                FormatEventElapsedMilliseconds(controlTimestamp),
-                controlSequence.ToString(
-                    CultureInfo.InvariantCulture),
-                FormatEventAgeMilliseconds(
-                    snapshotTimestamp,
-                    controlTimestamp),
-                FormatEventElapsedMilliseconds(commandTimestamp),
-                commandSequence.ToString(
-                    CultureInfo.InvariantCulture),
-                FormatEventAgeMilliseconds(
-                    snapshotTimestamp,
-                    commandTimestamp),
-                cart.CarNum.ToString(
-                    CultureInfo.InvariantCulture),
-                Format((int)cart.TransmitterControlMode),
-                Format(cart.ManualMode),
-                Format(cart.SendThresSpeed),
-                Format(cart.Voltage),
-                Format(cart.AlarmLevel),
-                Format(cart.ChassisMode),
-                FormatBoolean(cart.WheelAbleState),
-                Format(cart.DiffSteerKp),
-                Format(cart.DiffSteerKi),
-                Format(cart.DiffSteerKd),
-                Format(cart.DiffSteerMaxI),
-                Format(cart.DiffSteerDeadZone),
-                Format(cart.DiffSteerThresh),
-                Format(cart.DiffSteerSpeedAcc),
-                Format(cart.DiffSteerRateFeedforwardGain),
-                Format(cart.DiffSteerWheelDistanceMillimeters),
-                Format(cart.DiffSteerRateFeedforwardMaximumSpeed),
-                FormatBoolean(cart.EnableDiffSteerSettlingHysteresis),
-                Format(cart.DiffSteerStopErrorDegrees),
-                Format(cart.DiffSteerRestartErrorDegrees),
-                Format(cart.DiffSteerSettlingCycles),
-                FormatBoolean(cart.EnableDiffSteerInverseFeedforward),
-                Format(cart.DiffSteerPlantGain),
-                Format(
-                    cart.DiffSteerInverseFeedforwardTimeConstantSeconds),
-                Format(cart.DiffSteerFeedforwardDeltaTimeMilliseconds),
-                Format(
-                    cart.DiffSteerFeedforwardDeltaTimeMilliseconds /
-                    1000f),
-                Format(cart.DiffSteerTargetRateLeftFrontDegreesPerSecond),
-                Format(cart.DiffSteerTargetRateLeftRearDegreesPerSecond),
-                Format(cart.DiffSteerTargetRateRightFrontDegreesPerSecond),
-                Format(cart.DiffSteerTargetRateRightRearDegreesPerSecond),
-                Format(cart.DiffSteerInverseFeedforwardRawLeftFront),
-                Format(cart.DiffSteerInverseFeedforwardRawLeftRear),
-                Format(cart.DiffSteerInverseFeedforwardRawRightFront),
-                Format(cart.DiffSteerInverseFeedforwardRawRightRear),
-                Format(cart.DiffSteerInverseFeedforwardFilteredLeftFront),
-                Format(cart.DiffSteerInverseFeedforwardFilteredLeftRear),
-                Format(cart.DiffSteerInverseFeedforwardFilteredRightFront),
-                Format(cart.DiffSteerInverseFeedforwardFilteredRightRear),
-                FormatBoolean(cart.DiffSteerFeedforwardLimitedLeftFront),
-                FormatBoolean(cart.DiffSteerFeedforwardLimitedLeftRear),
-                FormatBoolean(cart.DiffSteerFeedforwardLimitedRightFront),
-                FormatBoolean(cart.DiffSteerFeedforwardLimitedRightRear),
-                FormatBoolean(cart.DiffSteerInStopZoneLeftFront),
-                FormatBoolean(cart.DiffSteerInStopZoneLeftRear),
-                FormatBoolean(cart.DiffSteerInStopZoneRightFront),
-                FormatBoolean(cart.DiffSteerInStopZoneRightRear),
-                Format(cart.DiffSteerSettlingCountLeftFront),
-                Format(cart.DiffSteerSettlingCountLeftRear),
-                Format(cart.DiffSteerSettlingCountRightFront),
-                Format(cart.DiffSteerSettlingCountRightRear),
-                FormatBoolean(cart.DiffSteerSettledLeftFront),
-                FormatBoolean(cart.DiffSteerSettledLeftRear),
-                FormatBoolean(cart.DiffSteerSettledRightFront),
-                FormatBoolean(cart.DiffSteerSettledRightRear),
-                Format(cart.DiffSteerOutputLeftFront),
-                Format(cart.DiffSteerOutputLeftRear),
-                Format(cart.DiffSteerOutputRightFront),
-                Format(cart.DiffSteerOutputRightRear),
-                Format(cart.DiffSteerRateFeedforwardLeftFront),
-                Format(cart.DiffSteerRateFeedforwardLeftRear),
-                Format(cart.DiffSteerRateFeedforwardRightFront),
-                Format(cart.DiffSteerRateFeedforwardRightRear),
-                Format(cart.DiffSteerTotalOutputLeftFront),
-                Format(cart.DiffSteerTotalOutputLeftRear),
-                Format(cart.DiffSteerTotalOutputRightFront),
-                Format(cart.DiffSteerTotalOutputRightRear),
-                Format(cart.SpeedLeftFrontLeft),
-                Format(cart.SpeedLeftFrontRight),
-                Format(cart.SpeedLeftRearLeft),
-                Format(cart.SpeedLeftRearRight),
-                Format(cart.SpeedRightFrontLeft),
-                Format(cart.SpeedRightFrontRight),
-                Format(cart.SpeedRightRearLeft),
-                Format(cart.SpeedRightRearRight),
-                Format(cart.SpeedLFL),
-                Format(cart.SpeedLFR),
-                Format(cart.SpeedLRL),
-                Format(cart.SpeedLRR),
-                Format(cart.SpeedRFL),
-                Format(cart.SpeedRFR),
-                Format(cart.SpeedRRL),
-                Format(cart.SpeedRRR),
-                Format(cart.SentSpeedLFL),
-                Format(cart.SentSpeedLFR),
-                Format(cart.SentSpeedLRL),
-                Format(cart.SentSpeedLRR),
-                Format(cart.SentSpeedRFL),
-                Format(cart.SentSpeedRFR),
-                Format(cart.SentSpeedRRL),
-                Format(cart.SentSpeedRRR),
-                FormatBoolean(cart.WheelCommandLimitedLFL),
-                FormatBoolean(cart.WheelCommandLimitedLFR),
-                FormatBoolean(cart.WheelCommandLimitedLRL),
-                FormatBoolean(cart.WheelCommandLimitedLRR),
-                FormatBoolean(cart.WheelCommandLimitedRFL),
-                FormatBoolean(cart.WheelCommandLimitedRFR),
-                FormatBoolean(cart.WheelCommandLimitedRRL),
-                FormatBoolean(cart.WheelCommandLimitedRRR),
-                FormatBoolean(
-                    cart.WheelCommandLimitedLFL ||
-                    cart.WheelCommandLimitedLFR),
-                FormatBoolean(
-                    cart.WheelCommandLimitedLRL ||
-                    cart.WheelCommandLimitedLRR),
-                FormatBoolean(
-                    cart.WheelCommandLimitedRFL ||
-                    cart.WheelCommandLimitedRFR),
-                FormatBoolean(
-                    cart.WheelCommandLimitedRRL ||
-                    cart.WheelCommandLimitedRRR),
-                FormatBoolean(cart.WheelCommandSuppressed),
-                Format(cart.ActualSpeedLeftFrontLeft),
-                Format(cart.ActualSpeedLeftFrontRight),
-                Format(cart.ActualSpeedLeftRearLeft),
-                Format(cart.ActualSpeedLeftRearRight),
-                Format(cart.ActualSpeedRightFrontLeft),
-                Format(cart.ActualSpeedRightFrontRight),
-                Format(cart.ActualSpeedRightRearLeft),
-                Format(cart.ActualSpeedRightRearRight),
-                Format(cart.ActualSpeedLeftFront),
-                Format(cart.ActualSpeedLeftRear),
-                Format(cart.ActualSpeedRightFront),
-                Format(cart.ActualSpeedRightRear),
-                Format(cart.LFLActualPos),
-                Format(cart.LFRActualPos),
-                Format(cart.LRLActualPos),
-                Format(cart.LRRActualPos),
-                Format(cart.RFLActualPos),
-                Format(cart.RFRActualPos),
-                Format(cart.RRLActualPos),
-                Format(cart.RRRActualPos),
-                Format(cart.LeftFrontLeftElectric),
-                Format(cart.LeftFrontRightElectric),
-                Format(cart.LeftRearLeftElectric),
-                Format(cart.LeftRearRightElectric),
-                Format(cart.RightFrontLeftElectric),
-                Format(cart.RightFrontRightElectric),
-                Format(cart.RightRearLeftElectric),
-                Format(cart.RightRearRightElectric),
-                Format(cart.ThLeftFront),
-                Format(cart.ThLeftRear),
-                Format(cart.ThRightFront),
-                Format(cart.ThRightRear),
-                Format(cart.ActualThLeftFront),
-                Format(cart.ActualThLeftRear),
-                Format(cart.ActualThRightFront),
-                Format(cart.ActualThRightRear),
-                Format(cart.ThLeftFront - cart.ActualThLeftFront),
-                Format(cart.ThLeftRear - cart.ActualThLeftRear),
-                Format(cart.ThRightFront - cart.ActualThRightFront),
-                Format(cart.ThRightRear - cart.ActualThRightRear),
-                FormatEventElapsedMilliseconds(
-                    actualThLeftFrontTimestamp),
-                actualThLeftFrontSequence.ToString(
-                    CultureInfo.InvariantCulture),
-                FormatEventAgeMilliseconds(
-                    snapshotTimestamp,
-                    actualThLeftFrontTimestamp),
-                FormatEventElapsedMilliseconds(
-                    actualThLeftRearTimestamp),
-                actualThLeftRearSequence.ToString(
-                    CultureInfo.InvariantCulture),
-                FormatEventAgeMilliseconds(
-                    snapshotTimestamp,
-                    actualThLeftRearTimestamp),
-                FormatEventElapsedMilliseconds(
-                    actualThRightFrontTimestamp),
-                actualThRightFrontSequence.ToString(
-                    CultureInfo.InvariantCulture),
-                FormatEventAgeMilliseconds(
-                    snapshotTimestamp,
-                    actualThRightFrontTimestamp),
-                FormatEventElapsedMilliseconds(
-                    actualThRightRearTimestamp),
-                actualThRightRearSequence.ToString(
-                    CultureInfo.InvariantCulture),
-                FormatEventAgeMilliseconds(
-                    snapshotTimestamp,
-                    actualThRightRearTimestamp));
-
-            Enqueue(new LogRecord(
-                isCanEvent: false,
-                line));
-        }
-
-        private static StreamWriter CreateWriter(
-            string path)
-        {
-            return new StreamWriter(
-                path,
-                append: false,
-                new UTF8Encoding(
-                    encoderShouldEmitUTF8Identifier: true),
-                bufferSize: 64 * 1024);
-        }
-
-        private void Enqueue(LogRecord record)
-        {
-            var queuedCount =
-                Interlocked.Increment(
-                    ref _queuedRecordCount);
-
-            if (queuedCount >
-                MaximumQueuedRecords)
-            {
-                Interlocked.Decrement(
-                    ref _queuedRecordCount);
-                Interlocked.Increment(
-                    ref _droppedRecordCount);
-                return;
-            }
-
-            _records.Enqueue(record);
-            _recordsAvailable.Set();
-        }
-
-        private void WriterLoop()
-        {
-            var lastFlushTime = DateTime.UtcNow;
-
-            try
-            {
-                while (_isRunning ||
-                       !_records.IsEmpty)
-                {
-                    var wroteAnyRecord = false;
-
-                    while (_records.TryDequeue(
-                               out var record))
-                    {
-                        Interlocked.Decrement(
-                            ref _queuedRecordCount);
-
-                        if (record.IsCanEvent)
-                            _canWriter.WriteLine(record.Line);
-                        else
-                            _snapshotWriter.WriteLine(record.Line);
-
-                        wroteAnyRecord = true;
-                    }
-
-                    var shouldFlush =
-                        wroteAnyRecord &&
-                        (DateTime.UtcNow -
-                         lastFlushTime)
-                        .TotalMilliseconds >= 500.0;
-
-                    if (shouldFlush)
-                    {
-                        _canWriter.Flush();
-                        _snapshotWriter.Flush();
-                        lastFlushTime = DateTime.UtcNow;
-                    }
-
-                    if (!wroteAnyRecord)
-                        _recordsAvailable.WaitOne(100);
-                }
-
-                var dropped =
-                    Interlocked.Read(
-                        ref _droppedRecordCount);
-
-                if (dropped > 0)
-                {
-                    _canWriter.WriteLine(
-                        $"# DroppedRecords={dropped}");
-                    _snapshotWriter.WriteLine(
-                        $"# DroppedRecords={dropped}");
-                }
-
-                _canWriter.Flush();
-                _snapshotWriter.Flush();
-            }
-            catch (Exception ex)
-            {
-                // 后台日志失败不能终止车辆控制线程。
-                Console.WriteLine(
-                    "轮速诊断后台写入失败：" +
-                    ex.Message);
-            }
-        }
-
-        private static string Format(
-            double value)
-        {
-            return value.ToString(
-                "0.######",
-                CultureInfo.InvariantCulture);
-        }
-
-        /// <summary>
-        /// 将诊断布尔值写成便于MATLAB直接读取的0或1。
-        /// </summary>
-        private static string FormatBoolean(bool value)
-        {
-            return value ? "1" : "0";
-        }
-
-        /// <summary>
-        /// 将本机单调时钟值换算为相对本次日志开始的毫秒数。
-        /// </summary>
-        private double GetElapsedMilliseconds(long timestamp)
-        {
-            return (timestamp - _startTimestamp) *
-                   1000.0 /
-                   Stopwatch.Frequency;
-        }
-
-        /// <summary>
-        /// 格式化发生在本次记录期间的事件时刻，记录前事件返回空字段。
-        /// </summary>
-        private string FormatEventElapsedMilliseconds(long timestamp)
-        {
-            if (timestamp < _startTimestamp)
-                return "";
-
-            return Format(GetElapsedMilliseconds(timestamp));
-        }
-
-        /// <summary>
-        /// 计算快照时刻相对最近一次控制或反馈事件的数据年龄。
-        /// </summary>
-        private static string FormatEventAgeMilliseconds(
-            long currentTimestamp,
-            long eventTimestamp)
-        {
-            if (eventTimestamp <= 0 ||
-                eventTimestamp > currentTimestamp)
-            {
-                return "";
-            }
-
-            return Format(
-                (currentTimestamp - eventTimestamp) *
-                1000.0 /
-                Stopwatch.Frequency);
-        }
-
-        public void Dispose()
-        {
-            Stop();
-            _recordsAvailable.Dispose();
-        }
-    }
-}

MedullaAdapter/WirelessFleetTransport.cs

@@ -1 +0,0 @@
-// M层负责串口读写、校验、收发状态

MedullaAdapter/obj/MedullaAdapter.csproj.nuget.dgspec.json

@@ -0,0 +1,73 @@
+{
+  "format": 1,
+  "restore": {
+    "D:\\Users\\Desktop\\入职培训\\停车机器人\\MyParking\\MedullaAdapter\\MedullaAdapter.csproj": {}
+  },
+  "projects": {
+    "D:\\Users\\Desktop\\入职培训\\停车机器人\\MyParking\\MedullaAdapter\\MedullaAdapter.csproj": {
+      "version": "1.0.0",
+      "restore": {
+        "projectUniqueName": "D:\\Users\\Desktop\\入职培训\\停车机器人\\MyParking\\MedullaAdapter\\MedullaAdapter.csproj",
+        "projectName": "MedullaAdapter",
+        "projectPath": "D:\\Users\\Desktop\\入职培训\\停车机器人\\MyParking\\MedullaAdapter\\MedullaAdapter.csproj",
+        "packagesPath": "C:\\Users\\CodexSandboxOffline\\.nuget\\packages\\",
+        "outputPath": "D:\\Users\\Desktop\\入职培训\\停车机器人\\MyParking\\MedullaAdapter\\obj\\",
+        "projectStyle": "PackageReference",
+        "fallbackFolders": [
+          "C:\\Program Files (x86)\\Microsoft Visual Studio\\Shared\\NuGetPackages"
+        ],
+        "configFilePaths": [
+          "C:\\Users\\admin\\AppData\\Roaming\\NuGet\\NuGet.Config",
+          "C:\\Program Files (x86)\\NuGet\\Config\\Microsoft.VisualStudio.FallbackLocation.config",
+          "C:\\Program Files (x86)\\NuGet\\Config\\Microsoft.VisualStudio.Offline.config"
+        ],
+        "originalTargetFrameworks": [
+          "net8.0"
+        ],
+        "sources": {
+          "C:\\Program Files (x86)\\Microsoft SDKs\\NuGetPackages\\": {},
+          "https://api.nuget.org/v3/index.json": {}
+        },
+        "frameworks": {
+          "net8.0": {
+            "targetAlias": "net8.0",
+            "projectReferences": {}
+          }
+        },
+        "warningProperties": {
+          "warnAsError": [
+            "NU1605"
+          ]
+        },
+        "restoreAuditProperties": {
+          "enableAudit": "true",
+          "auditLevel": "low",
+          "auditMode": "direct"
+        },
+        "SdkAnalysisLevel": "9.0.300"
+      },
+      "frameworks": {
+        "net8.0": {
+          "targetAlias": "net8.0",
+          "imports": [
+            "net461",
+            "net462",
+            "net47",
+            "net471",
+            "net472",
+            "net48",
+            "net481"
+          ],
+          "assetTargetFallback": true,
+          "warn": true,
+          "frameworkReferences": {
+            "Microsoft.NETCore.App": {
+              "privateAssets": "all"
+            }
+          },
+          "runtimeIdentifierGraphPath": "C:\\Program Files\\dotnet\\sdk\\9.0.316/PortableRuntimeIdentifierGraph.json"
+        }
+      }
+    }
+  }
+}

MedullaAdapter/obj/MedullaAdapter.csproj.nuget.g.props

@@ -0,0 +1,16 @@
+<?xml version="1.0" encoding="utf-8" standalone="no"?>
+<Project ToolsVersion="14.0" xmlns="http://schemas.microsoft.com/developer/msbuild/2003">
+  <PropertyGroup Condition=" '$(ExcludeRestorePackageImports)' != 'true' ">
+    <RestoreSuccess Condition=" '$(RestoreSuccess)' == '' ">True</RestoreSuccess>
+    <RestoreTool Condition=" '$(RestoreTool)' == '' ">NuGet</RestoreTool>
+    <ProjectAssetsFile Condition=" '$(ProjectAssetsFile)' == '' ">$(MSBuildThisFileDirectory)project.assets.json</ProjectAssetsFile>
+    <NuGetPackageRoot Condition=" '$(NuGetPackageRoot)' == '' ">C:\Users\CodexSandboxOffline\.nuget\packages\</NuGetPackageRoot>
+    <NuGetPackageFolders Condition=" '$(NuGetPackageFolders)' == '' ">C:\Users\CodexSandboxOffline\.nuget\packages\;C:\Program Files (x86)\Microsoft Visual Studio\Shared\NuGetPackages</NuGetPackageFolders>
+    <NuGetProjectStyle Condition=" '$(NuGetProjectStyle)' == '' ">PackageReference</NuGetProjectStyle>
+    <NuGetToolVersion Condition=" '$(NuGetToolVersion)' == '' ">6.14.3</NuGetToolVersion>
+  </PropertyGroup>
+  <ItemGroup Condition=" '$(ExcludeRestorePackageImports)' != 'true' ">
+    <SourceRoot Include="C:\Users\CodexSandboxOffline\.nuget\packages\" />
+    <SourceRoot Include="C:\Program Files (x86)\Microsoft Visual Studio\Shared\NuGetPackages\" />
+  </ItemGroup>
+</Project>

MedullaAdapter/obj/MedullaAdapter.csproj.nuget.g.targets

@@ -0,0 +1,2 @@
+<?xml version="1.0" encoding="utf-8" standalone="no"?>
+<Project ToolsVersion="14.0" xmlns="http://schemas.microsoft.com/developer/msbuild/2003" />

MedullaAdapter/obj/project.assets.json

@@ -0,0 +1,79 @@
+{
+  "version": 3,
+  "targets": {
+    "net8.0": {}
+  },
+  "libraries": {},
+  "projectFileDependencyGroups": {
+    "net8.0": []
+  },
+  "packageFolders": {
+    "C:\\Users\\CodexSandboxOffline\\.nuget\\packages\\": {},
+    "C:\\Program Files (x86)\\Microsoft Visual Studio\\Shared\\NuGetPackages": {}
+  },
+  "project": {
+    "version": "1.0.0",
+    "restore": {
+      "projectUniqueName": "D:\\Users\\Desktop\\入职培训\\停车机器人\\MyParking\\MedullaAdapter\\MedullaAdapter.csproj",
+      "projectName": "MedullaAdapter",
+      "projectPath": "D:\\Users\\Desktop\\入职培训\\停车机器人\\MyParking\\MedullaAdapter\\MedullaAdapter.csproj",
+      "packagesPath": "C:\\Users\\CodexSandboxOffline\\.nuget\\packages\\",
+      "outputPath": "D:\\Users\\Desktop\\入职培训\\停车机器人\\MyParking\\MedullaAdapter\\obj\\",
+      "projectStyle": "PackageReference",
+      "fallbackFolders": [
+        "C:\\Program Files (x86)\\Microsoft Visual Studio\\Shared\\NuGetPackages"
+      ],
+      "configFilePaths": [
+        "C:\\Users\\admin\\AppData\\Roaming\\NuGet\\NuGet.Config",
+        "C:\\Program Files (x86)\\NuGet\\Config\\Microsoft.VisualStudio.FallbackLocation.config",
+        "C:\\Program Files (x86)\\NuGet\\Config\\Microsoft.VisualStudio.Offline.config"
+      ],
+      "originalTargetFrameworks": [
+        "net8.0"
+      ],
+      "sources": {
+        "C:\\Program Files (x86)\\Microsoft SDKs\\NuGetPackages\\": {},
+        "https://api.nuget.org/v3/index.json": {}
+      },
+      "frameworks": {
+        "net8.0": {
+          "targetAlias": "net8.0",
+          "projectReferences": {}
+        }
+      },
+      "warningProperties": {
+        "warnAsError": [
+          "NU1605"
+        ]
+      },
+      "restoreAuditProperties": {
+        "enableAudit": "true",
+        "auditLevel": "low",
+        "auditMode": "direct"
+      },
+      "SdkAnalysisLevel": "9.0.300"
+    },
+    "frameworks": {
+      "net8.0": {
+        "targetAlias": "net8.0",
+        "imports": [
+          "net461",
+          "net462",
+          "net47",
+          "net471",
+          "net472",
+          "net48",
+          "net481"
+        ],
+        "assetTargetFallback": true,
+        "warn": true,
+        "frameworkReferences": {
+          "Microsoft.NETCore.App": {
+            "privateAssets": "all"
+          }
+        },
+        "runtimeIdentifierGraphPath": "C:\\Program Files\\dotnet\\sdk\\9.0.316/PortableRuntimeIdentifierGraph.json"
+      }
+    }
+  }
+}

MedullaAdapter/obj/project.nuget.cache

@@ -0,0 +1,8 @@
+{
+  "version": 2,
+  "dgSpecHash": "b9v8vkN2ac8=",
+  "success": true,
+  "projectFilePath": "D:\\Users\\Desktop\\入职培训\\停车机器人\\MyParking\\MedullaAdapter\\MedullaAdapter.csproj",
+  "expectedPackageFiles": [],
+  "logs": []
+}

MultiWheelC.Tests/FleetControllerTests.cs

@@ -1,374 +0,0 @@
-using System;
-using MultiWheelC.Control.Abstractions;
-using MultiWheelC.Control.Allocation;
-using MultiWheelC.Fleet;
-using MultiWheelC.Trajectory;
-using MyParking.Shared;
-
-namespace MultiWheelC.Tests
-{
-    internal static class FleetControllerTests
-    {
-        private const double Tolerance = 1e-9;
-
-        public static void Run()
-        {
-            VerifyInactiveControllerStops();
-            VerifyStraightCommand();
-            VerifyFortyFiveDegreeMotionDirection();
-            VerifyNinetyDegreeMotionDirection();
-            VerifyInvalidVelocityUsesReferenceSpeed();
-            VerifyCompletionStops();
-            VerifyExcessiveTrackingErrorFaults();
-            VerifyCancelStops();
-
-            Console.WriteLine(
-                "FleetController车队中心控制测试通过。共8个场景。");
-        }
-
-        private static void VerifyInactiveControllerStops()
-        {
-            var controller = CreateController();
-            var result = controller.ComputeCommand(
-                CreateState(0.5, 0.0, 0.0, true),
-                0.02,
-                out var command);
-
-            AssertResult(
-                result,
-                FleetControlCycleResult.Inactive,
-                "未启动控制器");
-            AssertStop(command, "未启动控制器");
-        }
-
-        private static void VerifyStraightCommand()
-        {
-            var controller = CreateController();
-            controller.Start(CreateStraightTrajectory());
-
-            var result = controller.ComputeCommand(
-                CreateState(0.5, 0.0, 0.4, true),
-                0.02,
-                out var command);
-
-            AssertResult(
-                result,
-                FleetControlCycleResult.CommandGenerated,
-                "直线控制");
-            AssertNear(
-                command.ReferencePointInFleet.XMeters,
-                0.0,
-                "直线控制参考点X");
-            AssertNear(
-                command.ReferencePointInFleet.YMeters,
-                0.0,
-                "直线控制参考点Y");
-            AssertTwist(
-                command.TwistAtReferencePoint,
-                0.4,
-                0.0,
-                0.0,
-                "直线控制");
-        }
-
-        private static void VerifyInvalidVelocityUsesReferenceSpeed()
-        {
-            var controller = CreateController();
-            controller.Start(CreateStraightTrajectory());
-
-            var result = controller.ComputeCommand(
-                CreateState(0.5, 0.0, 0.0, false),
-                0.02,
-                out var command);
-
-            AssertResult(
-                result,
-                FleetControlCycleResult.CommandGenerated,
-                "速度尚未初始化");
-            AssertTwist(
-                command.TwistAtReferencePoint,
-                0.4,
-                0.0,
-                0.0,
-                "速度尚未初始化");
-        }
-
-        private static void VerifyFortyFiveDegreeMotionDirection()
-        {
-            VerifyMotionDirection(
-                Math.PI / 4.0,
-                "45度运动方向");
-        }
-
-        private static void VerifyNinetyDegreeMotionDirection()
-        {
-            VerifyMotionDirection(
-                Math.PI / 2.0,
-                "90度运动方向");
-        }
-
-        private static void VerifyMotionDirection(
-            double motionDirectionInFleetRadians,
-            string scenario)
-        {
-            var controller = CreateController(
-                motionDirectionInFleetRadians);
-            controller.Start(
-                CreateStraightTrajectory(
-                    motionDirectionInFleetRadians));
-
-            var directionX =
-                Math.Cos(motionDirectionInFleetRadians);
-            var directionY =
-                Math.Sin(motionDirectionInFleetRadians);
-            var result = controller.ComputeCommand(
-                CreateState(
-                    0.5 * directionX,
-                    0.5 * directionY,
-                    0.4 * directionX,
-                    0.4 * directionY,
-                    true),
-                0.02,
-                out var command);
-
-            AssertResult(
-                result,
-                FleetControlCycleResult.CommandGenerated,
-                scenario);
-            AssertTwist(
-                command.TwistAtReferencePoint,
-                0.4 * directionX,
-                0.4 * directionY,
-                0.0,
-                scenario);
-        }
-
-        private static void VerifyCompletionStops()
-        {
-            var controller = CreateController();
-            controller.Start(CreateStraightTrajectory());
-
-            var result = controller.ComputeCommand(
-                CreateState(1.0, 0.0, 0.0, true),
-                0.02,
-                out var command);
-
-            AssertResult(
-                result,
-                FleetControlCycleResult.Completed,
-                "终点完成");
-            AssertStop(command, "终点完成");
-
-            if (controller.IsActive || !controller.IsCompleted)
-            {
-                throw new InvalidOperationException(
-                    "终点完成后控制器状态错误。");
-            }
-        }
-
-        private static void VerifyExcessiveTrackingErrorFaults()
-        {
-            var controller = CreateController();
-            controller.Start(CreateStraightTrajectory());
-
-            var result = controller.ComputeCommand(
-                CreateState(0.5, 0.5, 0.0, true),
-                0.02,
-                out var command);
-
-            AssertResult(
-                result,
-                FleetControlCycleResult.Faulted,
-                "轨迹偏离保护");
-            AssertStop(command, "轨迹偏离保护");
-
-            if (string.IsNullOrWhiteSpace(
-                    controller.LastFailureReason))
-            {
-                throw new InvalidOperationException(
-                    "轨迹偏离故障没有保存原因。");
-            }
-        }
-
-        private static void VerifyCancelStops()
-        {
-            var controller = CreateController();
-            controller.Start(CreateStraightTrajectory());
-            controller.Cancel();
-
-            var result = controller.ComputeCommand(
-                CreateState(0.5, 0.0, 0.0, true),
-                0.02,
-                out var command);
-
-            AssertResult(
-                result,
-                FleetControlCycleResult.Inactive,
-                "取消控制");
-            AssertStop(command, "取消控制");
-        }
-
-        private static FleetController CreateController(
-            double motionDirectionInFleetRadians = 0.0)
-        {
-            return new FleetController(
-                new StraightLateralController(),
-                new ReferenceLongitudinalController(),
-                new GcpCommandAllocator(
-                    Math.PI / 4.0),
-                virtualControlPointRadiusMeters: 0.5,
-                motionDirectionInFleetRadians:
-                    motionDirectionInFleetRadians);
-        }
-
-        private static Trajectory2D CreateStraightTrajectory(
-            double motionDirectionInFleetRadians = 0.0)
-        {
-            var directionX =
-                Math.Cos(motionDirectionInFleetRadians);
-            var directionY =
-                Math.Sin(motionDirectionInFleetRadians);
-
-            return new Trajectory2D(
-                new[]
-                {
-                    new TrajectoryPoint(
-                        0.0,
-                        Pose2D.Identity,
-                        0.0,
-                        0.4),
-                    new TrajectoryPoint(
-                        1.0,
-                        new Pose2D(
-                            directionX,
-                            directionY,
-                            0.0),
-                        0.0,
-                        0.4)
-                });
-        }
-
-        private static FleetState CreateState(
-            double xMeters,
-            double yMeters,
-            double vxMetersPerSecond,
-            bool hasValidVelocityEstimate)
-        {
-            return CreateState(
-                xMeters,
-                yMeters,
-                vxMetersPerSecond,
-                0.0,
-                hasValidVelocityEstimate);
-        }
-
-        private static FleetState CreateState(
-            double xMeters,
-            double yMeters,
-            double vxMetersPerSecond,
-            double vyMetersPerSecond,
-            bool hasValidVelocityEstimate)
-        {
-            return new FleetState(
-                sampleTimestampSeconds: 1.0,
-                fleetPoseInWorld: new Pose2D(
-                    xMeters,
-                    yMeters,
-                    0.0),
-                twistAtFleetOriginInWorld: new Twist2D(
-                    vxMetersPerSecond,
-                    vyMetersPerSecond,
-                    0.0),
-                hasValidVelocityEstimate:
-                    hasValidVelocityEstimate);
-        }
-
-        private static void AssertResult(
-            FleetControlCycleResult actual,
-            FleetControlCycleResult expected,
-            string scenario)
-        {
-            if (actual != expected)
-            {
-                throw new InvalidOperationException(
-                    $"{scenario}结果错误：" +
-                    $"actual={actual}, expected={expected}。");
-            }
-        }
-
-        private static void AssertStop(
-            FleetMotionCommand command,
-            string scenario)
-        {
-            AssertTwist(
-                command.TwistAtReferencePoint,
-                0.0,
-                0.0,
-                0.0,
-                scenario);
-        }
-
-        private static void AssertTwist(
-            Twist2D actual,
-            double expectedVx,
-            double expectedVy,
-            double expectedOmega,
-            string scenario)
-        {
-            AssertNear(
-                actual.VxMetersPerSecond,
-                expectedVx,
-                $"{scenario} Vx");
-            AssertNear(
-                actual.VyMetersPerSecond,
-                expectedVy,
-                $"{scenario} Vy");
-            AssertNear(
-                actual.OmegaRadiansPerSecond,
-                expectedOmega,
-                $"{scenario} Omega");
-        }
-
-        private static void AssertNear(
-            double actual,
-            double expected,
-            string valueName)
-        {
-            if (Math.Abs(actual - expected) > Tolerance)
-            {
-                throw new InvalidOperationException(
-                    $"{valueName}错误：" +
-                    $"actual={actual:F9}, expected={expected:F9}。");
-            }
-        }
-
-        private sealed class StraightLateralController :
-            ILateralController
-        {
-            public LateralControlCommand Compute(
-                PathTrackingContext context)
-            {
-                return LateralControlCommand.Straight;
-            }
-
-            public void Reset()
-            {
-            }
-        }
-
-        private sealed class ReferenceLongitudinalController :
-            ILongitudinalController
-        {
-            public double ComputeSpeedMetersPerSecond(
-                PathTrackingContext context)
-            {
-                return context
-                    .ControlReferenceSpeedMetersPerSecond;
-            }
-
-            public void Reset()
-            {
-            }
-        }
-    }
-}

MultiWheelC.Tests/FleetCoordinatorTests.cs

@@ -1,654 +0,0 @@
-using System;
-using System.Collections.Generic;
-using MultiWheelC.Control.Abstractions;
-using MultiWheelC.Control.Allocation;
-using MultiWheelC.Fleet;
-using MultiWheelC.Trajectory;
-using MyParking.Shared;
-
-namespace MultiWheelC.Tests
-{
-    internal static class FleetCoordinatorTests
-    {
-        private const double Tolerance = 1e-9;
-
-        public static void Run()
-        {
-            VerifyCommandCycle();
-            VerifySmallLayoutErrorIsCorrected();
-            VerifyWarningRangeScalesAllCommands();
-            VerifyUnavailableStateStopsAndRecovers();
-            VerifyUnsafeLayoutFaultsAndLatches();
-            VerifyCompletionStops();
-            VerifyTrackingFaultStops();
-            VerifyCancelReturnsInactive();
-
-            Console.WriteLine(
-                "FleetCoordinator车队协调测试通过。共8个场景。");
-        }
-
-        private static void VerifyCommandCycle()
-        {
-            var layout = CreateLayout();
-            var coordinator = CreateCoordinator();
-            coordinator.Start(layout, CreateTrajectory());
-
-            var result = coordinator.ExecuteCycle(
-                CreateRigidMemberStates(
-                    layout,
-                    new Pose2D(0.5, 0.0, 0.0),
-                    new Twist2D(0.4, 0.0, 0.0),
-                    1.0),
-                targetTimestampSeconds: 1.0,
-                deltaTimeSeconds: 0.02,
-                out var output);
-
-            AssertResult(
-                result,
-                FleetCoordinationCycleResult.CommandGenerated,
-                "正常协调周期");
-            if (!output.State.HasValue)
-            {
-                throw new InvalidOperationException(
-                    "正常协调周期没有返回车队状态。");
-            }
-
-            AssertNear(
-                output.State.Value.FleetPoseInWorld.XMeters,
-                0.5,
-                "正常协调周期中心X");
-            AssertNear(
-                output.SpeedScale,
-                1.0,
-                "正常协调周期速度比例");
-            AssertTwist(
-                output.FleetCommand.TwistAtReferencePoint,
-                0.4,
-                0.0,
-                0.0,
-                "正常协调周期车队命令");
-            AssertTwist(
-                FindCommand(output.MemberCommands, 1)
-                    .TwistInVehicleBody,
-                0.4,
-                0.0,
-                0.0,
-                "正常协调周期车辆1");
-            AssertTwist(
-                FindCommand(output.MemberCommands, 2)
-                    .TwistInVehicleBody,
-                -0.4,
-                0.0,
-                0.0,
-                "正常协调周期车辆2");
-        }
-
-        private static void VerifySmallLayoutErrorIsCorrected()
-        {
-            var layout = CreateLayout();
-            var coordinator = CreateCoordinator();
-            coordinator.Start(layout, CreateTrajectory());
-            var result = coordinator.ExecuteCycle(
-                new[]
-                {
-                    CreateMemberState(
-                        1,
-                        new Pose2D(-0.99, 0.0, 0.0),
-                        1.0),
-                    CreateMemberState(
-                        2,
-                        new Pose2D(0.99, 0.0, Math.PI),
-                        1.0)
-                },
-                targetTimestampSeconds: 1.0,
-                deltaTimeSeconds: 0.02,
-                out var output);
-
-            AssertResult(
-                result,
-                FleetCoordinationCycleResult.CommandGenerated,
-                "小范围布局误差纠偏");
-            AssertNear(
-                output.SpeedScale,
-                1.0,
-                "小范围布局误差速度比例");
-            AssertTwist(
-                FindCommand(output.BaseMemberCommands, 1)
-                    .TwistInVehicleBody,
-                0.4,
-                0.0,
-                0.0,
-                "车辆1基础命令");
-            AssertTwist(
-                FindCommand(output.BaseMemberCommands, 2)
-                    .TwistInVehicleBody,
-                -0.4,
-                0.0,
-                0.0,
-                "车辆2基础命令");
-            AssertTwist(
-                FindCommand(output.MemberCommands, 1)
-                    .TwistInVehicleBody,
-                0.395,
-                0.0,
-                0.0,
-                "车辆1纠偏后命令");
-            AssertTwist(
-                FindCommand(output.MemberCommands, 2)
-                    .TwistInVehicleBody,
-                -0.405,
-                0.0,
-                0.0,
-                "车辆2纠偏后命令");
-        }
-
-        private static void VerifyWarningRangeScalesAllCommands()
-        {
-            var layout = CreateLayout();
-            var coordinator = CreateCoordinator();
-            coordinator.Start(layout, CreateTrajectory());
-            var result = coordinator.ExecuteCycle(
-                new[]
-                {
-                    CreateMemberState(
-                        1,
-                        new Pose2D(-0.965, 0.0, 0.0),
-                        1.0),
-                    CreateMemberState(
-                        2,
-                        new Pose2D(0.965, 0.0, Math.PI),
-                        1.0)
-                },
-                targetTimestampSeconds: 1.0,
-                deltaTimeSeconds: 0.02,
-                out var output);
-
-            AssertResult(
-                result,
-                FleetCoordinationCycleResult.CommandGenerated,
-                "警告区间统一缩放");
-            AssertNear(
-                output.SpeedScale,
-                0.5,
-                "警告区间速度比例");
-            AssertTwist(
-                output.FleetCommand.TwistAtReferencePoint,
-                0.2,
-                0.0,
-                0.0,
-                "警告区间车队命令");
-            AssertTwist(
-                FindCommand(output.MemberCommands, 1)
-                    .TwistInVehicleBody,
-                0.2,
-                0.0,
-                0.0,
-                "警告区间车辆1");
-            AssertTwist(
-                FindCommand(output.MemberCommands, 2)
-                    .TwistInVehicleBody,
-                -0.2,
-                0.0,
-                0.0,
-                "警告区间车辆2");
-
-            if (string.IsNullOrWhiteSpace(output.Reason))
-            {
-                throw new InvalidOperationException(
-                    "警告区间缩放没有返回限制原因。");
-            }
-        }
-
-        private static void VerifyUnavailableStateStopsAndRecovers()
-        {
-            var layout = CreateLayout();
-            var coordinator = CreateCoordinator();
-            coordinator.Start(layout, CreateTrajectory());
-            var unavailableStates = CreateRigidMemberStates(
-                layout,
-                new Pose2D(0.5, 0.0, 0.0),
-                new Twist2D(0.4, 0.0, 0.0),
-                1.0);
-            unavailableStates[1] = new FleetMemberStateSample(
-                unavailableStates[1].VehicleId,
-                unavailableStates[1].SampleTimestampSeconds,
-                unavailableStates[1].PoseInWorld,
-                unavailableStates[1].TwistAtVehicleOriginInWorld,
-                isStateAvailable: false,
-                hasValidVelocityEstimate: true);
-
-            var waitingResult = coordinator.ExecuteCycle(
-                unavailableStates,
-                targetTimestampSeconds: 1.0,
-                deltaTimeSeconds: 0.02,
-                out var waitingOutput);
-
-            AssertResult(
-                waitingResult,
-                FleetCoordinationCycleResult.WaitingForState,
-                "状态暂时不可用");
-            AssertStop(waitingOutput, layout.VehicleCount);
-
-            if (!coordinator.IsActive)
-            {
-                throw new InvalidOperationException(
-                    "状态暂时不可用不应取消车队控制器。");
-            }
-
-            var recoveredResult = coordinator.ExecuteCycle(
-                CreateRigidMemberStates(
-                    layout,
-                    new Pose2D(0.5, 0.0, 0.0),
-                    new Twist2D(0.4, 0.0, 0.0),
-                    1.02),
-                targetTimestampSeconds: 1.02,
-                deltaTimeSeconds: 0.02,
-                out _);
-
-            AssertResult(
-                recoveredResult,
-                FleetCoordinationCycleResult.CommandGenerated,
-                "状态恢复");
-        }
-
-        private static void VerifyUnsafeLayoutFaultsAndLatches()
-        {
-            var layout = CreateLayout();
-            var coordinator = CreateCoordinator();
-            coordinator.Start(layout, CreateTrajectory());
-            var deformedStates = new[]
-            {
-                CreateMemberState(
-                    1,
-                    new Pose2D(-0.9, 0.0, 0.0),
-                    1.0),
-                CreateMemberState(
-                    2,
-                    new Pose2D(0.9, 0.0, Math.PI),
-                    1.0)
-            };
-
-            var result = coordinator.ExecuteCycle(
-                deformedStates,
-                targetTimestampSeconds: 1.0,
-                deltaTimeSeconds: 0.02,
-                out var output);
-
-            AssertResult(
-                result,
-                FleetCoordinationCycleResult.Faulted,
-                "布局误差超限");
-            AssertStop(output, layout.VehicleCount);
-
-            if (!coordinator.IsFaulted ||
-                string.IsNullOrWhiteSpace(
-                    coordinator.LastFailureReason))
-            {
-                throw new InvalidOperationException(
-                    "布局误差故障没有被锁存。");
-            }
-
-            var latchedResult = coordinator.ExecuteCycle(
-                CreateRigidMemberStates(
-                    layout,
-                    new Pose2D(0.5, 0.0, 0.0),
-                    new Twist2D(0.4, 0.0, 0.0),
-                    1.02),
-                targetTimestampSeconds: 1.02,
-                deltaTimeSeconds: 0.02,
-                out var latchedOutput);
-
-            AssertResult(
-                latchedResult,
-                FleetCoordinationCycleResult.Faulted,
-                "布局误差故障锁存");
-            AssertStop(latchedOutput, layout.VehicleCount);
-        }
-
-        private static void VerifyCompletionStops()
-        {
-            var layout = CreateLayout();
-            var coordinator = CreateCoordinator();
-            coordinator.Start(layout, CreateTrajectory());
-
-            var result = coordinator.ExecuteCycle(
-                CreateRigidMemberStates(
-                    layout,
-                    new Pose2D(1.0, 0.0, 0.0),
-                    Twist2D.Zero,
-                    1.0),
-                targetTimestampSeconds: 1.0,
-                deltaTimeSeconds: 0.02,
-                out var output);
-
-            AssertResult(
-                result,
-                FleetCoordinationCycleResult.Completed,
-                "车队轨迹完成");
-            AssertStop(output, layout.VehicleCount);
-
-            if (!coordinator.IsCompleted)
-            {
-                throw new InvalidOperationException(
-                    "车队轨迹完成状态没有被保存。");
-            }
-        }
-
-        private static void VerifyTrackingFaultStops()
-        {
-            var layout = CreateLayout();
-            var coordinator = CreateCoordinator();
-            coordinator.Start(layout, CreateTrajectory());
-
-            var result = coordinator.ExecuteCycle(
-                CreateRigidMemberStates(
-                    layout,
-                    new Pose2D(0.5, 0.5, 0.0),
-                    Twist2D.Zero,
-                    1.0),
-                targetTimestampSeconds: 1.0,
-                deltaTimeSeconds: 0.02,
-                out var output);
-
-            AssertResult(
-                result,
-                FleetCoordinationCycleResult.Faulted,
-                "车队中心跟踪故障");
-            AssertStop(output, layout.VehicleCount);
-
-            if (!coordinator.IsFaulted)
-            {
-                throw new InvalidOperationException(
-                    "车队中心跟踪故障没有传递到协调器。");
-            }
-        }
-
-        private static void VerifyCancelReturnsInactive()
-        {
-            var layout = CreateLayout();
-            var coordinator = CreateCoordinator();
-            coordinator.Start(layout, CreateTrajectory());
-            coordinator.Cancel();
-
-            var result = coordinator.ExecuteCycle(
-                CreateRigidMemberStates(
-                    layout,
-                    new Pose2D(0.5, 0.0, 0.0),
-                    Twist2D.Zero,
-                    1.0),
-                targetTimestampSeconds: 1.0,
-                deltaTimeSeconds: 0.02,
-                out var output);
-
-            AssertResult(
-                result,
-                FleetCoordinationCycleResult.Inactive,
-                "取消车队协调");
-            AssertStop(output, layout.VehicleCount);
-        }
-
-        private static FleetCoordinator CreateCoordinator()
-        {
-            var estimator = new FleetStateEstimator(
-                maximumMemberStateAgeSeconds: 0.25,
-                maximumPositionDisagreementMeters: 0.5,
-                maximumYawDisagreementRadians:
-                    AngleMath.DegreesToRadians(10.0));
-            var controller = new FleetController(
-                new StraightLateralController(),
-                new ReferenceLongitudinalController(),
-                new GcpCommandAllocator(Math.PI / 4.0),
-                virtualControlPointRadiusMeters: 0.5);
-            var commandCorrector =
-                new FleetMemberCommandCorrector(
-                    longitudinalPositionGainPerSecond: 1.0,
-                    lateralPositionGainPerSecond: 1.0,
-                    yawGainPerSecond: 1.0,
-                    positionErrorDeadbandMeters: 0.005,
-                    yawErrorDeadbandRadians:
-                        AngleMath.DegreesToRadians(0.5),
-                    maximumLinearCorrectionMetersPerSecond:
-                        0.03,
-                    maximumAngularCorrectionRadiansPerSecond:
-                        AngleMath.DegreesToRadians(2.0));
-
-            return new FleetCoordinator(
-                estimator,
-                controller,
-                commandCorrector,
-                memberPositionErrorWarningMeters: 0.02,
-                maximumMemberPositionErrorMeters: 0.05,
-                memberYawErrorWarningRadians:
-                    AngleMath.DegreesToRadians(1.0),
-                maximumMemberYawErrorRadians:
-                    AngleMath.DegreesToRadians(3.0));
-        }
-
-        private static FleetLayout CreateLayout()
-        {
-            return new FleetLayout(
-                new[]
-                {
-                    new VehicleLayout(
-                        1,
-                        new Pose2D(-1.0, 0.0, 0.0)),
-                    new VehicleLayout(
-                        2,
-                        new Pose2D(1.0, 0.0, Math.PI))
-                });
-        }
-
-        private static Trajectory2D CreateTrajectory()
-        {
-            return new Trajectory2D(
-                new[]
-                {
-                    new TrajectoryPoint(
-                        0.0,
-                        Pose2D.Identity,
-                        0.0,
-                        0.4),
-                    new TrajectoryPoint(
-                        1.0,
-                        new Pose2D(1.0, 0.0, 0.0),
-                        0.0,
-                        0.4)
-                });
-        }
-
-        private static FleetMemberStateSample[]
-            CreateRigidMemberStates(
-                FleetLayout layout,
-                Pose2D fleetPoseInWorld,
-                Twist2D twistAtFleetOriginInWorld,
-                double timestampSeconds)
-        {
-            var states =
-                new FleetMemberStateSample[layout.VehicleCount];
-
-            for (var index = 0;
-                 index < layout.Vehicles.Count;
-                 index++)
-            {
-                var vehicle = layout.Vehicles[index];
-                var poseInWorld = FrameTransform2D.Compose(
-                    fleetPoseInWorld,
-                    vehicle.PoseInFleet);
-                var offsetX =
-                    poseInWorld.XMeters -
-                    fleetPoseInWorld.XMeters;
-                var offsetY =
-                    poseInWorld.YMeters -
-                    fleetPoseInWorld.YMeters;
-                var twistInWorld = new Twist2D(
-                    twistAtFleetOriginInWorld.VxMetersPerSecond -
-                    twistAtFleetOriginInWorld.OmegaRadiansPerSecond *
-                    offsetY,
-                    twistAtFleetOriginInWorld.VyMetersPerSecond +
-                    twistAtFleetOriginInWorld.OmegaRadiansPerSecond *
-                    offsetX,
-                    twistAtFleetOriginInWorld.OmegaRadiansPerSecond);
-
-                states[index] = new FleetMemberStateSample(
-                    vehicle.VehicleId,
-                    timestampSeconds,
-                    poseInWorld,
-                    twistInWorld,
-                    isStateAvailable: true,
-                    hasValidVelocityEstimate: true);
-            }
-
-            return states;
-        }
-
-        private static FleetMemberStateSample CreateMemberState(
-            int vehicleId,
-            Pose2D poseInWorld,
-            double timestampSeconds)
-        {
-            return new FleetMemberStateSample(
-                vehicleId,
-                timestampSeconds,
-                poseInWorld,
-                Twist2D.Zero,
-                isStateAvailable: true,
-                hasValidVelocityEstimate: true);
-        }
-
-        private static FleetMemberCommand FindCommand(
-            IReadOnlyList<FleetMemberCommand> commands,
-            int vehicleId)
-        {
-            for (var index = 0;
-                 index < commands.Count;
-                 index++)
-            {
-                if (commands[index].VehicleId == vehicleId)
-                {
-                    return commands[index];
-                }
-            }
-
-            throw new InvalidOperationException(
-                $"没有找到车辆{vehicleId}的成员命令。");
-        }
-
-        private static void AssertStop(
-            FleetCoordinationCycleOutput output,
-            int expectedMemberCount)
-        {
-            AssertTwist(
-                output.FleetCommand.TwistAtReferencePoint,
-                0.0,
-                0.0,
-                0.0,
-                "车队停止命令");
-
-            if (output.MemberCommands.Count != expectedMemberCount)
-            {
-                throw new InvalidOperationException(
-                    "停止输出的成员命令数量错误。");
-            }
-
-            if (output.BaseMemberCommands.Count != expectedMemberCount)
-            {
-                throw new InvalidOperationException(
-                    "停止输出的成员基础命令数量错误。");
-            }
-
-            for (var index = 0;
-                 index < output.MemberCommands.Count;
-                 index++)
-            {
-                AssertTwist(
-                    output.BaseMemberCommands[index]
-                        .TwistInVehicleBody,
-                    0.0,
-                    0.0,
-                    0.0,
-                    "成员基础停止命令");
-                AssertTwist(
-                    output.MemberCommands[index].TwistInVehicleBody,
-                    0.0,
-                    0.0,
-                    0.0,
-                    "成员停止命令");
-            }
-        }
-
-        private static void AssertResult(
-            FleetCoordinationCycleResult actual,
-            FleetCoordinationCycleResult expected,
-            string scenario)
-        {
-            if (actual != expected)
-            {
-                throw new InvalidOperationException(
-                    $"{scenario}结果错误：" +
-                    $"actual={actual}, expected={expected}。");
-            }
-        }
-
-        private static void AssertTwist(
-            Twist2D actual,
-            double expectedVx,
-            double expectedVy,
-            double expectedOmega,
-            string scenario)
-        {
-            AssertNear(
-                actual.VxMetersPerSecond,
-                expectedVx,
-                scenario + " Vx");
-            AssertNear(
-                actual.VyMetersPerSecond,
-                expectedVy,
-                scenario + " Vy");
-            AssertNear(
-                actual.OmegaRadiansPerSecond,
-                expectedOmega,
-                scenario + " Omega");
-        }
-
-        private static void AssertNear(
-            double actual,
-            double expected,
-            string valueName)
-        {
-            if (Math.Abs(actual - expected) > Tolerance)
-            {
-                throw new InvalidOperationException(
-                    $"{valueName}错误：" +
-                    $"actual={actual:F9}, expected={expected:F9}。");
-            }
-        }
-
-        private sealed class StraightLateralController :
-            ILateralController
-        {
-            public LateralControlCommand Compute(
-                PathTrackingContext context)
-            {
-                return LateralControlCommand.Straight;
-            }
-
-            public void Reset()
-            {
-            }
-        }
-
-        private sealed class ReferenceLongitudinalController :
-            ILongitudinalController
-        {
-            public double ComputeSpeedMetersPerSecond(
-                PathTrackingContext context)
-            {
-                return context.ControlReferenceSpeedMetersPerSecond;
-            }
-
-            public void Reset()
-            {
-            }
-        }
-    }
-}

MultiWheelC.Tests/FleetKinematicsTests.cs

@@ -1,139 +0,0 @@
-using System;
-using System.Collections.Generic;
-using MyParking.Shared;
-
-namespace MultiWheelC.Tests
-{
-    internal static class FleetKinematicsTests
-    {
-        private const double Tolerance = 1e-9;
-
-        public static void Run()
-        {
-            var layout = CreateTailToTailLayout();
-
-            VerifyTranslation(layout);
-            VerifyRotationAroundFleetCenter(layout);
-            VerifyRotationAroundFirstVehicle(layout);
-            VerifyStop(layout);
-
-            Console.WriteLine(
-                "FleetKinematics刚体速度分配测试通过。共4个场景。");
-        }
-
-        private static FleetLayout CreateTailToTailLayout()
-        {
-            return new FleetLayout(
-                new[]
-                {
-                    new VehicleLayout(
-                        1,
-                        new Pose2D(1.0, 0.0, 0.0)),
-                    new VehicleLayout(
-                        2,
-                        new Pose2D(-1.0, 0.0, Math.PI))
-                });
-        }
-
-        private static void VerifyTranslation(FleetLayout layout)
-        {
-            var commands = FleetKinematics.Decompose(
-                layout,
-                new FleetMotionCommand(
-                    Point2D.Zero,
-                    new Twist2D(0.4, 0.0, 0.0)));
-
-            AssertTwist(Find(commands, 1), 0.4, 0.0, 0.0);
-            AssertTwist(Find(commands, 2), -0.4, 0.0, 0.0);
-        }
-
-        private static void VerifyRotationAroundFleetCenter(
-            FleetLayout layout)
-        {
-            var commands = FleetKinematics.Decompose(
-                layout,
-                FleetMotionCommand.RotateAround(
-                    Point2D.Zero,
-                    0.2));
-
-            AssertTwist(Find(commands, 1), 0.0, 0.2, 0.2);
-            AssertTwist(Find(commands, 2), 0.0, 0.2, 0.2);
-        }
-
-        private static void VerifyRotationAroundFirstVehicle(
-            FleetLayout layout)
-        {
-            var commands = FleetKinematics.Decompose(
-                layout,
-                FleetMotionCommand.RotateAround(
-                    new Point2D(1.0, 0.0),
-                    0.2));
-
-            AssertTwist(Find(commands, 1), 0.0, 0.0, 0.2);
-            AssertTwist(Find(commands, 2), 0.0, 0.4, 0.2);
-        }
-
-        private static void VerifyStop(FleetLayout layout)
-        {
-            var commands = FleetKinematics.Decompose(
-                layout,
-                FleetMotionCommand.Stop());
-
-            AssertTwist(Find(commands, 1), 0.0, 0.0, 0.0);
-            AssertTwist(Find(commands, 2), 0.0, 0.0, 0.0);
-        }
-
-        private static FleetMemberCommand Find(
-            IReadOnlyList<FleetMemberCommand> commands,
-            int vehicleId)
-        {
-            for (var index = 0; index < commands.Count; index++)
-            {
-                if (commands[index].VehicleId == vehicleId)
-                {
-                    return commands[index];
-                }
-            }
-
-            throw new InvalidOperationException(
-                $"没有找到车辆{vehicleId}的分配命令。");
-        }
-
-        private static void AssertTwist(
-            FleetMemberCommand command,
-            double expectedVx,
-            double expectedVy,
-            double expectedOmega)
-        {
-            AssertNear(
-                command.TwistInVehicleBody.VxMetersPerSecond,
-                expectedVx,
-                command.VehicleId,
-                "Vx");
-            AssertNear(
-                command.TwistInVehicleBody.VyMetersPerSecond,
-                expectedVy,
-                command.VehicleId,
-                "Vy");
-            AssertNear(
-                command.TwistInVehicleBody.OmegaRadiansPerSecond,
-                expectedOmega,
-                command.VehicleId,
-                "Omega");
-        }
-
-        private static void AssertNear(
-            double actual,
-            double expected,
-            int vehicleId,
-            string valueName)
-        {
-            if (Math.Abs(actual - expected) > Tolerance)
-            {
-                throw new InvalidOperationException(
-                    $"车辆{vehicleId}的{valueName}错误：" +
-                    $"actual={actual:F9}, expected={expected:F9}。");
-            }
-        }
-    }
-}

MultiWheelC.Tests/FleetLayoutCaptureTests.cs

@@ -1,268 +0,0 @@
-using System;
-using MultiWheelC.Fleet;
-using MyParking.Shared;
-
-namespace MultiWheelC.Tests
-{
-    internal static class FleetLayoutCaptureTests
-    {
-        private const double Tolerance = 1e-9;
-
-        public static void Run()
-        {
-            VerifySymmetricTailToTailLayout();
-            VerifyAsymmetricLayoutAndPoseReconstruction();
-            VerifyInputOrderDoesNotChangeResult();
-            VerifyEmptyInputIsRejected();
-            VerifyDuplicateVehicleIdIsRejected();
-            VerifyMissingLeaderIsRejected();
-
-            Console.WriteLine(
-                "FleetLayoutCapture布局建立测试通过。共6个场景。");
-        }
-
-        private static void VerifySymmetricTailToTailLayout()
-        {
-            var result = FleetLayoutCapture.Capture(
-                new[]
-                {
-                    new FleetMemberPose(
-                        1,
-                        new Pose2D(-1.2, 0.0, 0.0)),
-                    new FleetMemberPose(
-                        2,
-                        new Pose2D(1.2, 0.0, Math.PI))
-                },
-                leaderVehicleId: 1);
-
-            AssertPose(
-                result.FleetPoseInWorld,
-                Pose2D.Identity,
-                "对称双车中心");
-            AssertVehicleLayout(
-                result.Layout,
-                1,
-                new Pose2D(-1.2, 0.0, 0.0),
-                "对称双车主车布局");
-            AssertVehicleLayout(
-                result.Layout,
-                2,
-                new Pose2D(1.2, 0.0, Math.PI),
-                "对称双车从车布局");
-        }
-
-        private static void VerifyAsymmetricLayoutAndPoseReconstruction()
-        {
-            var members = new[]
-            {
-                new FleetMemberPose(
-                    3,
-                    new Pose2D(1.0, 1.0, 0.4)),
-                new FleetMemberPose(
-                    1,
-                    new Pose2D(4.0, 1.0, 0.4)),
-                new FleetMemberPose(
-                    2,
-                    new Pose2D(1.0, 4.0, -0.8))
-            };
-            var result = FleetLayoutCapture.Capture(
-                members,
-                leaderVehicleId: 1);
-
-            AssertPose(
-                result.FleetPoseInWorld,
-                new Pose2D(2.0, 2.0, 0.4),
-                "非对称三车中心");
-
-            for (var index = 0;
-                 index < members.Length;
-                 index++)
-            {
-                if (!result.Layout.TryGetVehicle(
-                        members[index].VehicleId,
-                        out var vehicleLayout))
-                {
-                    throw new InvalidOperationException(
-                        "非对称布局缺少成员车。" +
-                        members[index].VehicleId);
-                }
-
-                var reconstructedPoseInWorld =
-                    FrameTransform2D.Compose(
-                        result.FleetPoseInWorld,
-                        vehicleLayout.PoseInFleet);
-                AssertPose(
-                    reconstructedPoseInWorld,
-                    members[index].PoseInWorld,
-                    "非对称布局世界位姿还原");
-            }
-        }
-
-        private static void VerifyInputOrderDoesNotChangeResult()
-        {
-            var first = FleetLayoutCapture.Capture(
-                new[]
-                {
-                    new FleetMemberPose(
-                        1,
-                        new Pose2D(2.0, 3.0, 0.6)),
-                    new FleetMemberPose(
-                        2,
-                        new Pose2D(4.0, 5.0, -1.0))
-                },
-                leaderVehicleId: 1);
-            var second = FleetLayoutCapture.Capture(
-                new[]
-                {
-                    new FleetMemberPose(
-                        2,
-                        new Pose2D(4.0, 5.0, -1.0)),
-                    new FleetMemberPose(
-                        1,
-                        new Pose2D(2.0, 3.0, 0.6))
-                },
-                leaderVehicleId: 1);
-
-            AssertPose(
-                first.FleetPoseInWorld,
-                second.FleetPoseInWorld,
-                "输入顺序不变中心");
-            AssertSameLayout(first.Layout, second.Layout);
-        }
-
-        private static void VerifyEmptyInputIsRejected()
-        {
-            ExpectException<ArgumentException>(
-                () => FleetLayoutCapture.Capture(
-                    Array.Empty<FleetMemberPose>(),
-                    leaderVehicleId: 1),
-                "空成员集合");
-        }
-
-        private static void VerifyDuplicateVehicleIdIsRejected()
-        {
-            ExpectException<ArgumentException>(
-                () => FleetLayoutCapture.Capture(
-                    new[]
-                    {
-                        new FleetMemberPose(
-                            1,
-                            Pose2D.Identity),
-                        new FleetMemberPose(
-                            1,
-                            new Pose2D(1.0, 0.0, 0.0))
-                    },
-                    leaderVehicleId: 1),
-                "重复车号");
-        }
-
-        private static void VerifyMissingLeaderIsRejected()
-        {
-            ExpectException<ArgumentException>(
-                () => FleetLayoutCapture.Capture(
-                    new[]
-                    {
-                        new FleetMemberPose(
-                            2,
-                            Pose2D.Identity)
-                    },
-                    leaderVehicleId: 1),
-                "缺少主车");
-        }
-
-        private static void AssertSameLayout(
-            FleetLayout first,
-            FleetLayout second)
-        {
-            if (first.VehicleCount != second.VehicleCount)
-            {
-                throw new InvalidOperationException(
-                    "输入顺序变化后成员数量发生变化。");
-            }
-
-            for (var index = 0;
-                 index < first.Vehicles.Count;
-                 index++)
-            {
-                var vehicle = first.Vehicles[index];
-                AssertVehicleLayout(
-                    second,
-                    vehicle.VehicleId,
-                    vehicle.PoseInFleet,
-                    "输入顺序不变布局");
-            }
-        }
-
-        private static void AssertVehicleLayout(
-            FleetLayout layout,
-            int vehicleId,
-            Pose2D expectedPoseInFleet,
-            string scenario)
-        {
-            if (!layout.TryGetVehicle(
-                    vehicleId,
-                    out var vehicle))
-            {
-                throw new InvalidOperationException(
-                    $"{scenario}缺少车辆{vehicleId}。");
-            }
-
-            AssertPose(
-                vehicle.PoseInFleet,
-                expectedPoseInFleet,
-                scenario);
-        }
-
-        private static void AssertPose(
-            Pose2D actual,
-            Pose2D expected,
-            string scenario)
-        {
-            AssertNear(
-                actual.XMeters,
-                expected.XMeters,
-                scenario + " X");
-            AssertNear(
-                actual.YMeters,
-                expected.YMeters,
-                scenario + " Y");
-            AssertNear(
-                AngleMath.NormalizeRadians(
-                    actual.YawRadians -
-                    expected.YawRadians),
-                0.0,
-                scenario + " Yaw");
-        }
-
-        private static void AssertNear(
-            double actual,
-            double expected,
-            string valueName)
-        {
-            if (Math.Abs(actual - expected) > Tolerance)
-            {
-                throw new InvalidOperationException(
-                    $"{valueName}错误：" +
-                    $"actual={actual:F9}, expected={expected:F9}。");
-            }
-        }
-
-        private static void ExpectException<TException>(
-            Action action,
-            string scenario)
-            where TException : Exception
-        {
-            try
-            {
-                action();
-            }
-            catch (TException)
-            {
-                return;
-            }
-
-            throw new InvalidOperationException(
-                $"{scenario}没有抛出{typeof(TException).Name}。");
-        }
-    }
-}

MultiWheelC.Tests/FleetMemberAgentTests.cs

@@ -1,240 +0,0 @@
-using System;
-using System.Numerics;
-using CommonUsage.Chassis;
-using MultiWheelC.Fleet;
-using MyParking.Shared;
-
-namespace MultiWheelC.Tests
-{
-    internal static class FleetMemberAgentTests
-    {
-        private const long PlanId = 11;
-        private const int VehicleId = 2;
-
-        public static void Run()
-        {
-            VerifyActiveWatchdogExpires();
-            VerifyValidMotionCommandRefreshesDeadline();
-            VerifyRepeatedActivationDoesNotRefreshDeadline();
-            VerifyLateActivationCannotResumeMotion();
-            VerifyStopClearsWatchdog();
-
-            Console.WriteLine(
-                "FleetMemberAgent本地命令看门狗测试通过，共5个场景。");
-        }
-
-        private static void VerifyActiveWatchdogExpires()
-        {
-            var agent = CreateReadyAgent();
-            AssertTrue(
-                agent.Activate(
-                    PlanId,
-                    commandReceivedTimeSeconds: 10.0,
-                    validForSeconds: 0.5),
-                "成员车应当成功激活");
-
-            AssertTrue(
-                agent.UpdateCommandWatchdog(10.5),
-                "截止时刻仍应视为有效");
-            AssertFalse(
-                agent.UpdateCommandWatchdog(10.501),
-                "超过命令有效期后应停车");
-            AssertState(
-                agent,
-                FleetMemberAgentState.Faulted,
-                "命令超时");
-            AssertTrue(
-                !string.IsNullOrWhiteSpace(
-                    agent.LastFailureReason),
-                "命令超时应保留故障原因");
-        }
-
-        private static void VerifyValidMotionCommandRefreshesDeadline()
-        {
-            var agent = CreateReadyAgent();
-            agent.Activate(
-                PlanId,
-                commandReceivedTimeSeconds: 20.0,
-                validForSeconds: 0.5);
-
-            var accepted = agent.Execute(
-                PlanId,
-                new FleetMemberCommand(
-                    VehicleId,
-                    Twist2D.Zero),
-                commandReceivedTimeSeconds: 20.4,
-                validForSeconds: 0.7);
-
-            AssertTrue(
-                accepted,
-                "有效速度命令应被接受");
-            AssertNear(
-                agent.LastAcceptedCommandTimeSeconds,
-                20.4,
-                "最近命令接收时间");
-            AssertNear(
-                agent.CommandDeadlineSeconds,
-                21.1,
-                "速度命令刷新后的截止时间");
-            AssertTrue(
-                agent.UpdateCommandWatchdog(20.8),
-                "刷新截止时间后车辆应保持Active");
-        }
-
-        private static void VerifyRepeatedActivationDoesNotRefreshDeadline()
-        {
-            var agent = CreateReadyAgent();
-            agent.Activate(
-                PlanId,
-                commandReceivedTimeSeconds: 30.0,
-                validForSeconds: 0.5);
-
-            AssertTrue(
-                agent.Activate(
-                    PlanId,
-                    commandReceivedTimeSeconds: 30.4,
-                    validForSeconds: 0.5),
-                "未超时的重复激活命令应允许幂等确认");
-            AssertNear(
-                agent.CommandDeadlineSeconds,
-                30.5,
-                "重复激活不能替代运动命令刷新截止时间");
-            AssertFalse(
-                agent.UpdateCommandWatchdog(30.501),
-                "没有收到运动命令时仍应按最初激活期限停车");
-        }
-
-        private static void VerifyLateActivationCannotResumeMotion()
-        {
-            var agent = CreateReadyAgent();
-            agent.Activate(
-                PlanId,
-                commandReceivedTimeSeconds: 40.0,
-                validForSeconds: 0.5);
-
-            AssertFalse(
-                agent.Activate(
-                    PlanId,
-                    commandReceivedTimeSeconds: 40.6,
-                    validForSeconds: 0.5),
-                "迟到的激活命令不能恢复已经失联的车辆");
-            AssertState(
-                agent,
-                FleetMemberAgentState.Faulted,
-                "迟到激活命令");
-        }
-
-        private static void VerifyStopClearsWatchdog()
-        {
-            var agent = CreateReadyAgent();
-            agent.Activate(
-                PlanId,
-                commandReceivedTimeSeconds: 50.0,
-                validForSeconds: 0.5);
-
-            agent.Stop();
-
-            AssertState(
-                agent,
-                FleetMemberAgentState.Idle,
-                "正常停止");
-            AssertTrue(
-                !agent.LastAcceptedCommandTimeSeconds.HasValue &&
-                !agent.CommandDeadlineSeconds.HasValue,
-                "正常停止后应清除命令看门狗");
-        }
-
-        private static FleetMemberAgent CreateReadyAgent()
-        {
-            var chassis = new MultiWheelChassis();
-            chassis.AddWheel(CreateWheel(-500f, 300f));
-            chassis.AddWheel(CreateWheel(-500f, -300f));
-            chassis.AddWheel(CreateWheel(500f, 300f));
-            chassis.AddWheel(CreateWheel(500f, -300f));
-            chassis.Initialize();
-
-            var agent = new FleetMemberAgent(
-                new MultiWheelChassisAdapter(
-                    chassis,
-                    VehicleId),
-                alignmentToleranceRadians:
-                    AngleMath.DegreesToRadians(1.0),
-                alignmentStableSeconds: 0.0);
-
-            AssertTrue(
-                agent.BeginRollingPreparation(
-                    PlanId,
-                    motionDirectionInBodyRadians: 0.0),
-                "滚动运动系准备命令应被接受");
-            AssertState(
-                agent,
-                FleetMemberAgentState.Preparing,
-                "开始准备");
-            AssertTrue(
-                agent.UpdatePreparation(0.01) ==
-                FleetMemberAgentState.Ready,
-                "内存底盘舵轮应立即准备完成");
-
-            return agent;
-        }
-
-        private static SteerWheel CreateWheel(
-            float xMillimeters,
-            float yMillimeters)
-        {
-            var speed = 0f;
-            var angle = 0f;
-
-            return new SteerWheel(
-                new Vector2(
-                    xMillimeters,
-                    yMillimeters),
-                angleLowerLimit: -120f,
-                angleUpperLimit: 120f,
-                speedWriter: value => speed = value,
-                speedReader: () => speed,
-                angleWriter: value => angle = value,
-                angleReader: () => angle);
-        }
-
-        private static void AssertState(
-            FleetMemberAgent agent,
-            FleetMemberAgentState expected,
-            string scenario)
-        {
-            AssertTrue(
-                agent.State == expected,
-                $"{scenario}后的状态应为{expected}，" +
-                $"实际为{agent.State}。");
-        }
-
-        private static void AssertNear(
-            double? actual,
-            double expected,
-            string name)
-        {
-            AssertTrue(
-                actual.HasValue &&
-                Math.Abs(actual.Value - expected) <= 1e-9,
-                $"{name}不正确，期望{expected:F6}，" +
-                $"实际{actual?.ToString("F6") ?? "null"}。");
-        }
-
-        private static void AssertTrue(
-            bool condition,
-            string message)
-        {
-            if (!condition)
-            {
-                throw new InvalidOperationException(message);
-            }
-        }
-
-        private static void AssertFalse(
-            bool condition,
-            string message)
-        {
-            AssertTrue(!condition, message);
-        }
-    }
-}

MultiWheelC.Tests/FleetMemberCommandCorrectorTests.cs

@@ -1,302 +0,0 @@
-using System;
-using System.Collections.Generic;
-using MultiWheelC.Fleet;
-using MyParking.Shared;
-
-namespace MultiWheelC.Tests
-{
-    internal static class FleetMemberCommandCorrectorTests
-    {
-        private const double Tolerance = 1e-9;
-
-        public static void Run()
-        {
-            VerifyZeroErrorsPreserveBaseCommands();
-            VerifyRelativePositionErrorProducesOpposingCorrection();
-            VerifyCommonTranslationIsRemoved();
-            VerifyCommonRotationIsRemoved();
-            VerifyDeadbandSuppressesSmallErrors();
-            VerifyCorrectionLimits();
-
-            Console.WriteLine(
-                "FleetMemberCommandCorrector测试通过，共6个场景。");
-        }
-
-        private static void VerifyZeroErrorsPreserveBaseCommands()
-        {
-            var layout = CreateLayout();
-            var baseCommands = FleetKinematics.Decompose(
-                layout,
-                new FleetMotionCommand(
-                    Point2D.Zero,
-                    new Twist2D(0.4, 0.1, 0.05)));
-            var corrected = CreateCorrector().Correct(
-                layout,
-                baseCommands,
-                CreateErrors(Pose2D.Identity, Pose2D.Identity));
-
-            AssertCommandsEqual(
-                corrected,
-                baseCommands,
-                "零布局误差");
-        }
-
-        private static void
-            VerifyRelativePositionErrorProducesOpposingCorrection()
-        {
-            var layout = CreateLayout();
-            var corrected = CreateCorrector().Correct(
-                layout,
-                CreateStopCommands(layout),
-                CreateErrors(
-                    new Pose2D(0.02, 0.0, 0.0),
-                    new Pose2D(0.02, 0.0, 0.0)));
-
-            AssertTwist(
-                FindCommand(corrected, 1).TwistInVehicleBody,
-                -0.02,
-                0.0,
-                0.0,
-                "车辆1相对位置纠偏");
-            AssertTwist(
-                FindCommand(corrected, 2).TwistInVehicleBody,
-                -0.02,
-                0.0,
-                0.0,
-                "车辆2相对位置纠偏");
-        }
-
-        private static void VerifyCommonTranslationIsRemoved()
-        {
-            var layout = CreateLayout();
-            var corrected = CreateCorrector().Correct(
-                layout,
-                CreateStopCommands(layout),
-                CreateErrors(
-                    new Pose2D(0.02, 0.0, 0.0),
-                    new Pose2D(-0.02, 0.0, 0.0)));
-
-            AssertAllStopped(
-                corrected,
-                "共同平移不应成为成员相对纠偏");
-        }
-
-        private static void VerifyCommonRotationIsRemoved()
-        {
-            const double fleetYawErrorRadians = 0.02;
-            var layout = CreateLayout();
-            var commonRotationError = new Pose2D(
-                0.0,
-                -fleetYawErrorRadians,
-                fleetYawErrorRadians);
-            var corrected = CreateCorrector().Correct(
-                layout,
-                CreateStopCommands(layout),
-                CreateErrors(
-                    commonRotationError,
-                    commonRotationError));
-
-            AssertAllStopped(
-                corrected,
-                "共同旋转不应成为成员相对纠偏");
-        }
-
-        private static void VerifyDeadbandSuppressesSmallErrors()
-        {
-            var layout = CreateLayout();
-            var corrector = new FleetMemberCommandCorrector(
-                longitudinalPositionGainPerSecond: 1.0,
-                lateralPositionGainPerSecond: 1.0,
-                yawGainPerSecond: 1.0,
-                positionErrorDeadbandMeters: 0.005,
-                yawErrorDeadbandRadians: 0.02,
-                maximumLinearCorrectionMetersPerSecond: 1.0,
-                maximumAngularCorrectionRadiansPerSecond: 1.0);
-            var corrected = corrector.Correct(
-                layout,
-                CreateStopCommands(layout),
-                CreateErrors(
-                    new Pose2D(0.004, 0.003, 0.01),
-                    new Pose2D(0.004, -0.003, -0.01)));
-
-            AssertAllStopped(corrected, "布局误差死区");
-        }
-
-        private static void VerifyCorrectionLimits()
-        {
-            var layout = CreateLayout();
-            var corrector = new FleetMemberCommandCorrector(
-                longitudinalPositionGainPerSecond: 1.0,
-                lateralPositionGainPerSecond: 1.0,
-                yawGainPerSecond: 1.0,
-                positionErrorDeadbandMeters: 0.0,
-                yawErrorDeadbandRadians: 0.0,
-                maximumLinearCorrectionMetersPerSecond: 0.03,
-                maximumAngularCorrectionRadiansPerSecond: 0.05);
-            var corrected = corrector.Correct(
-                layout,
-                CreateStopCommands(layout),
-                CreateErrors(
-                    new Pose2D(0.2, 0.0, 0.2),
-                    new Pose2D(0.2, 0.0, -0.2)));
-
-            for (var index = 0; index < corrected.Count; index++)
-            {
-                var twist = corrected[index].TwistInVehicleBody;
-                var linearMagnitude = Math.Sqrt(
-                    twist.VxMetersPerSecond *
-                    twist.VxMetersPerSecond +
-                    twist.VyMetersPerSecond *
-                    twist.VyMetersPerSecond);
-
-                AssertNear(
-                    linearMagnitude,
-                    0.03,
-                    "线速度纠偏限幅");
-                AssertNear(
-                    Math.Abs(twist.OmegaRadiansPerSecond),
-                    0.05,
-                    "角速度纠偏限幅");
-            }
-        }
-
-        private static FleetMemberCommandCorrector CreateCorrector()
-        {
-            return new FleetMemberCommandCorrector(
-                longitudinalPositionGainPerSecond: 1.0,
-                lateralPositionGainPerSecond: 1.0,
-                yawGainPerSecond: 1.0,
-                positionErrorDeadbandMeters: 0.0,
-                yawErrorDeadbandRadians: 0.0,
-                maximumLinearCorrectionMetersPerSecond: 1.0,
-                maximumAngularCorrectionRadiansPerSecond: 1.0);
-        }
-
-        private static FleetLayout CreateLayout()
-        {
-            return new FleetLayout(
-                new[]
-                {
-                    new VehicleLayout(
-                        1,
-                        new Pose2D(-1.0, 0.0, 0.0)),
-                    new VehicleLayout(
-                        2,
-                        new Pose2D(1.0, 0.0, Math.PI))
-                });
-        }
-
-        private static IReadOnlyList<FleetMemberCommand>
-            CreateStopCommands(FleetLayout layout)
-        {
-            return FleetKinematics.Decompose(
-                layout,
-                FleetMotionCommand.Stop());
-        }
-
-        private static FleetMemberLayoutError[] CreateErrors(
-            Pose2D vehicle1Error,
-            Pose2D vehicle2Error)
-        {
-            return new[]
-            {
-                new FleetMemberLayoutError(1, vehicle1Error),
-                new FleetMemberLayoutError(2, vehicle2Error)
-            };
-        }
-
-        private static FleetMemberCommand FindCommand(
-            IReadOnlyList<FleetMemberCommand> commands,
-            int vehicleId)
-        {
-            for (var index = 0; index < commands.Count; index++)
-            {
-                if (commands[index].VehicleId == vehicleId)
-                {
-                    return commands[index];
-                }
-            }
-
-            throw new InvalidOperationException(
-                $"没有找到车辆{vehicleId}的成员命令。");
-        }
-
-        private static void AssertCommandsEqual(
-            IReadOnlyList<FleetMemberCommand> actual,
-            IReadOnlyList<FleetMemberCommand> expected,
-            string scenario)
-        {
-            if (actual.Count != expected.Count)
-            {
-                throw new InvalidOperationException(
-                    $"{scenario}的命令数量不一致。");
-            }
-
-            for (var index = 0; index < expected.Count; index++)
-            {
-                var expectedCommand = expected[index];
-                var actualCommand = FindCommand(
-                    actual,
-                    expectedCommand.VehicleId);
-                AssertTwist(
-                    actualCommand.TwistInVehicleBody,
-                    expectedCommand.TwistInVehicleBody
-                        .VxMetersPerSecond,
-                    expectedCommand.TwistInVehicleBody
-                        .VyMetersPerSecond,
-                    expectedCommand.TwistInVehicleBody
-                        .OmegaRadiansPerSecond,
-                    scenario);
-            }
-        }
-
-        private static void AssertAllStopped(
-            IReadOnlyList<FleetMemberCommand> commands,
-            string scenario)
-        {
-            for (var index = 0; index < commands.Count; index++)
-            {
-                AssertTwist(
-                    commands[index].TwistInVehicleBody,
-                    0.0,
-                    0.0,
-                    0.0,
-                    scenario);
-            }
-        }
-
-        private static void AssertTwist(
-            Twist2D actual,
-            double expectedVx,
-            double expectedVy,
-            double expectedOmega,
-            string scenario)
-        {
-            AssertNear(
-                actual.VxMetersPerSecond,
-                expectedVx,
-                scenario + " Vx");
-            AssertNear(
-                actual.VyMetersPerSecond,
-                expectedVy,
-                scenario + " Vy");
-            AssertNear(
-                actual.OmegaRadiansPerSecond,
-                expectedOmega,
-                scenario + " Omega");
-        }
-
-        private static void AssertNear(
-            double actual,
-            double expected,
-            string valueName)
-        {
-            if (Math.Abs(actual - expected) > Tolerance)
-            {
-                throw new InvalidOperationException(
-                    $"{valueName}错误：" +
-                    $"actual={actual:F9}, expected={expected:F9}。");
-            }
-        }
-    }
-}

MultiWheelC.Tests/FleetPreparationCoordinatorTests.cs

@@ -1,310 +0,0 @@
-using System;
-using MultiWheelC.Fleet;
-using MyParking.Shared;
-
-namespace MultiWheelC.Tests
-{
-    internal static class FleetPreparationCoordinatorTests
-    {
-        private const double Tolerance = 1e-9;
-
-        public static void Run()
-        {
-            VerifyLeaderYawExample();
-            VerifyTailToTailUsesEquivalentAxis();
-            VerifyAllMembersMustBeReady();
-            VerifyStaleStatusIsIgnored();
-            VerifyMemberFaultIsLatched();
-            VerifyFaultAfterAuthorizationIsLatched();
-            VerifyPrematureActiveIsRejected();
-            VerifyCancelClearsPlan();
-
-            Console.WriteLine(
-                "FleetPreparationCoordinator测试通过。共8个场景。");
-        }
-
-        private static void VerifyLeaderYawExample()
-        {
-            var capture = FleetLayoutCapture.Capture(
-                new[]
-                {
-                    new FleetMemberPose(
-                        1,
-                        new Pose2D(
-                            -1.0,
-                            0.0,
-                            AngleMath.DegreesToRadians(20.0))),
-                    new FleetMemberPose(
-                        2,
-                        new Pose2D(1.0, 0.0, 0.0))
-                },
-                leaderVehicleId: 1);
-            var coordinator =
-                new FleetPreparationCoordinator();
-
-            coordinator.StartRollingPreparation(
-                planId: 1,
-                capture.Layout,
-                motionDirectionInFleetRadians: 0.0);
-
-            AssertTargetDegrees(coordinator, 1, 0.0);
-            AssertTargetDegrees(coordinator, 2, 20.0);
-        }
-
-        private static void VerifyTailToTailUsesEquivalentAxis()
-        {
-            var coordinator =
-                new FleetPreparationCoordinator();
-            coordinator.StartRollingPreparation(
-                planId: 2,
-                CreateTailToTailLayout(),
-                motionDirectionInFleetRadians: 0.0);
-
-            AssertTargetDegrees(coordinator, 1, 0.0);
-            AssertTargetDegrees(coordinator, 2, 0.0);
-        }
-
-        private static void VerifyAllMembersMustBeReady()
-        {
-            var coordinator = CreateStartedCoordinator(3);
-
-            AssertState(
-                coordinator.ReportMemberStatus(
-                    CreateStatus(
-                        3,
-                        1,
-                        FleetMemberAgentState.Ready)),
-                FleetPreparationCoordinatorState
-                    .WaitingForMembers,
-                "仅一辆车Ready");
-            AssertState(
-                coordinator.ReportMemberStatus(
-                    CreateStatus(
-                        3,
-                        2,
-                        FleetMemberAgentState.Ready)),
-                FleetPreparationCoordinatorState
-                    .ReadyToActivate,
-                "全部成员Ready");
-            AssertState(
-                coordinator.ReportMemberStatus(
-                    CreateStatus(
-                        3,
-                        1,
-                        FleetMemberAgentState.Preparing)),
-                FleetPreparationCoordinatorState
-                    .WaitingForMembers,
-                "成员失去Ready");
-            AssertState(
-                coordinator.ReportMemberStatus(
-                    CreateStatus(
-                        3,
-                        1,
-                        FleetMemberAgentState.Ready)),
-                FleetPreparationCoordinatorState
-                    .ReadyToActivate,
-                "成员重新Ready");
-
-            if (coordinator.TryAuthorizeActivation(4))
-            {
-                throw new InvalidOperationException(
-                    "错误任务编号不应获得激活授权。");
-            }
-
-            if (!coordinator.TryAuthorizeActivation(3))
-            {
-                throw new InvalidOperationException(
-                    "全部成员Ready后没有获得激活授权。");
-            }
-
-            AssertState(
-                coordinator.State,
-                FleetPreparationCoordinatorState
-                    .ActivationAuthorized,
-                "统一激活授权");
-        }
-
-        private static void VerifyStaleStatusIsIgnored()
-        {
-            var coordinator = CreateStartedCoordinator(5);
-            var state = coordinator.ReportMemberStatus(
-                CreateStatus(
-                    4,
-                    1,
-                    FleetMemberAgentState.Ready));
-
-            AssertState(
-                state,
-                FleetPreparationCoordinatorState
-                    .WaitingForMembers,
-                "旧任务状态报告");
-        }
-
-        private static void VerifyMemberFaultIsLatched()
-        {
-            var coordinator = CreateStartedCoordinator(6);
-            var state = coordinator.ReportMemberStatus(
-                CreateStatus(
-                    6,
-                    2,
-                    FleetMemberAgentState.Faulted,
-                    "舵轮未到位"));
-
-            AssertState(
-                state,
-                FleetPreparationCoordinatorState.Faulted,
-                "成员准备故障");
-            if (string.IsNullOrWhiteSpace(
-                    coordinator.LastFailureReason))
-            {
-                throw new InvalidOperationException(
-                    "成员准备故障没有保存原因。");
-            }
-        }
-
-        private static void VerifyFaultAfterAuthorizationIsLatched()
-        {
-            var coordinator = CreateStartedCoordinator(9);
-            coordinator.ReportMemberStatus(
-                CreateStatus(
-                    9,
-                    1,
-                    FleetMemberAgentState.Ready));
-            coordinator.ReportMemberStatus(
-                CreateStatus(
-                    9,
-                    2,
-                    FleetMemberAgentState.Ready));
-            coordinator.TryAuthorizeActivation(9);
-
-            var state = coordinator.ReportMemberStatus(
-                CreateStatus(
-                    9,
-                    2,
-                    FleetMemberAgentState.Faulted,
-                    "激活失败"));
-
-            AssertState(
-                state,
-                FleetPreparationCoordinatorState.Faulted,
-                "授权后的成员故障");
-        }
-
-        private static void VerifyPrematureActiveIsRejected()
-        {
-            var coordinator = CreateStartedCoordinator(7);
-            var state = coordinator.ReportMemberStatus(
-                CreateStatus(
-                    7,
-                    1,
-                    FleetMemberAgentState.Active));
-
-            AssertState(
-                state,
-                FleetPreparationCoordinatorState.Faulted,
-                "成员提前运动");
-        }
-
-        private static void VerifyCancelClearsPlan()
-        {
-            var coordinator = CreateStartedCoordinator(8);
-            coordinator.Cancel();
-
-            AssertState(
-                coordinator.State,
-                FleetPreparationCoordinatorState.Idle,
-                "取消准备任务");
-            if (coordinator.CurrentPlanId != 0 ||
-                coordinator.Targets.Count != 0)
-            {
-                throw new InvalidOperationException(
-                    "取消后没有清除准备任务数据。");
-            }
-        }
-
-        private static FleetPreparationCoordinator
-            CreateStartedCoordinator(long planId)
-        {
-            var coordinator =
-                new FleetPreparationCoordinator();
-            coordinator.StartRollingPreparation(
-                planId,
-                CreateTailToTailLayout(),
-                motionDirectionInFleetRadians: 0.0);
-            return coordinator;
-        }
-
-        private static FleetLayout CreateTailToTailLayout()
-        {
-            return new FleetLayout(
-                new[]
-                {
-                    new VehicleLayout(
-                        1,
-                        new Pose2D(-1.0, 0.0, 0.0)),
-                    new VehicleLayout(
-                        2,
-                        new Pose2D(1.0, 0.0, Math.PI))
-                });
-        }
-
-        private static FleetMemberPreparationStatus CreateStatus(
-            long planId,
-            int vehicleId,
-            FleetMemberAgentState state,
-            string failureReason = "")
-        {
-            return new FleetMemberPreparationStatus(
-                planId,
-                vehicleId,
-                state,
-                failureReason);
-        }
-
-        private static void AssertTargetDegrees(
-            FleetPreparationCoordinator coordinator,
-            int vehicleId,
-            double expectedDegrees)
-        {
-            if (!coordinator.TryGetTarget(
-                    vehicleId,
-                    out var target))
-            {
-                throw new InvalidOperationException(
-                    $"没有找到车辆{vehicleId}的准备目标。");
-            }
-
-            AssertNear(
-                AngleMath.RadiansToDegrees(
-                    target.MotionDirectionInBodyRadians),
-                expectedDegrees,
-                $"车辆{vehicleId}本地β");
-        }
-
-        private static void AssertState(
-            FleetPreparationCoordinatorState actual,
-            FleetPreparationCoordinatorState expected,
-            string scenario)
-        {
-            if (actual != expected)
-            {
-                throw new InvalidOperationException(
-                    $"{scenario}状态错误：" +
-                    $"actual={actual}, expected={expected}。");
-            }
-        }
-
-        private static void AssertNear(
-            double actual,
-            double expected,
-            string valueName)
-        {
-            if (Math.Abs(actual - expected) > Tolerance)
-            {
-                throw new InvalidOperationException(
-                    $"{valueName}错误：" +
-                    $"actual={actual:F9}, expected={expected:F9}。");
-            }
-        }
-    }
-}

MultiWheelC.Tests/FleetRuntimeTests.cs

@@ -1,430 +0,0 @@
-using System;
-using System.Numerics;
-using CommonUsage.Chassis;
-using MultiWheelC.Control.Abstractions;
-using MultiWheelC.Control.Allocation;
-using MultiWheelC.Fleet;
-using MultiWheelC.StateEstimation;
-using MultiWheelC.Trajectory;
-using MyParking.Shared;
-
-namespace MultiWheelC.Tests
-{
-    internal static class FleetRuntimeTests
-    {
-        private const long PlanId = 21;
-
-        public static void Run()
-        {
-            VerifyTwoVehiclePlanBecomesActive();
-            VerifyStaleCommandIsIgnored();
-            VerifyInvalidCommandLatchesMemberFault();
-            VerifyMissingReportFaultsLeader();
-            VerifyMemberWatchdogStopsLocally();
-            VerifyUnavailableMemberStateFaultsFleet();
-
-            Console.WriteLine(
-                "FleetRuntime端到端测试通过，共6个场景。");
-        }
-
-        private static void VerifyTwoVehiclePlanBecomesActive()
-        {
-            var fleet = CreateFleet();
-            StartAndActivate(fleet);
-
-            AssertState(
-                fleet.LeaderRuntime,
-                FleetRuntimeState.Active,
-                "主车正常激活");
-            AssertState(
-                fleet.MemberRuntime,
-                FleetRuntimeState.Active,
-                "从车正常激活");
-            AssertTrue(
-                fleet.LeaderRuntime.LastCoordinationOutput != null,
-                "主车激活后应产生首周期协调输出");
-            AssertTrue(
-                fleet.MemberRuntime.LastAppliedCommandSequence > 0,
-                "从车应确认已经执行主车命令");
-        }
-
-        private static void VerifyStaleCommandIsIgnored()
-        {
-            var fleet = CreateFleet();
-            StartAndActivate(fleet);
-            var appliedSequence =
-                fleet.MemberRuntime.LastAppliedCommandSequence;
-
-            fleet.LeaderTransport.SendCommand(
-                new FleetCommand(
-                    PlanId,
-                    appliedSequence,
-                    targetVehicleId: 2,
-                    FleetCommandKind.Motion,
-                    motionDirectionInBodyRadians: 0.0,
-                    twistInVehicleBody:
-                        new Twist2D(1.0, 0.0, 0.0),
-                    validForSeconds: 0.2));
-            fleet.MemberProvider.SetTimestamp(0.04);
-            fleet.MemberRuntime.Update(0.04, 0.02);
-
-            AssertState(
-                fleet.MemberRuntime,
-                FleetRuntimeState.Active,
-                "忽略旧序列命令");
-            AssertTrue(
-                fleet.MemberRuntime.LastAppliedCommandSequence ==
-                appliedSequence,
-                "旧序列命令不应更新最近执行序号");
-        }
-
-        private static void VerifyMissingReportFaultsLeader()
-        {
-            var fleet = CreateFleet();
-            AssertTrue(
-                fleet.LeaderRuntime.StartRollingPlan(
-                    PlanId,
-                    fleet.Layout,
-                    CreateTrajectory()),
-                "主车应成功启动测试任务");
-
-            fleet.LeaderRuntime.Update(0.0, 0.02);
-            fleet.LeaderProvider.SetTimestamp(0.25);
-            fleet.LeaderRuntime.Update(0.25, 0.02);
-
-            AssertState(
-                fleet.LeaderRuntime,
-                FleetRuntimeState.Faulted,
-                "成员报告超时");
-            AssertTrue(
-                fleet.LeaderRuntime.LastFailureReason.Contains(
-                    "未收到成员车2"),
-                "通信超时应指出缺失的成员车");
-            AssertTrue(
-                fleet.LeaderAgent.State ==
-                FleetMemberAgentState.Idle,
-                "主车故障后必须立即停止本车执行器");
-        }
-
-        private static void VerifyInvalidCommandLatchesMemberFault()
-        {
-            var fleet = CreateFleet();
-            StartAndActivate(fleet);
-
-            fleet.LeaderTransport.SendCommand(
-                new FleetCommand(
-                    PlanId,
-                    fleet.MemberRuntime.LastAppliedCommandSequence + 1,
-                    targetVehicleId: 2,
-                    FleetCommandKind.Motion,
-                    motionDirectionInBodyRadians: 0.0,
-                    twistInVehicleBody: Twist2D.Zero,
-                    validForSeconds: double.NaN));
-            fleet.MemberRuntime.Update(0.04, 0.02);
-
-            AssertState(
-                fleet.MemberRuntime,
-                FleetRuntimeState.Faulted,
-                "非法命令字段触发并锁存本地故障");
-        }
-
-        private static void VerifyMemberWatchdogStopsLocally()
-        {
-            var fleet = CreateFleet();
-            StartAndActivate(fleet);
-
-            fleet.MemberProvider.SetTimestamp(0.25);
-            fleet.MemberRuntime.Update(0.25, 0.02);
-
-            AssertState(
-                fleet.MemberRuntime,
-                FleetRuntimeState.Faulted,
-                "从车命令看门狗超时");
-            AssertTrue(
-                fleet.MemberRuntime.LastFailureReason.Contains(
-                    "超时"),
-                "从车看门狗应保留超时原因");
-        }
-
-        private static void VerifyUnavailableMemberStateFaultsFleet()
-        {
-            var fleet = CreateFleet();
-            StartAndActivate(fleet);
-
-            fleet.MemberProvider.IsAvailable = false;
-            fleet.MemberRuntime.Update(0.04, 0.02);
-            fleet.LeaderProvider.SetTimestamp(0.04);
-            fleet.LeaderRuntime.Update(0.04, 0.02);
-
-            AssertState(
-                fleet.MemberRuntime,
-                FleetRuntimeState.Faulted,
-                "从车状态不可用时本地停车");
-            AssertState(
-                fleet.LeaderRuntime,
-                FleetRuntimeState.Faulted,
-                "从车状态不可用时整队停车");
-        }
-
-        private static void StartAndActivate(TestFleet fleet)
-        {
-            AssertTrue(
-                fleet.LeaderRuntime.StartRollingPlan(
-                    PlanId,
-                    fleet.Layout,
-                    CreateTrajectory()),
-                "主车应成功启动测试任务");
-
-            fleet.MemberRuntime.Update(0.0, 0.02);
-            fleet.LeaderRuntime.Update(0.0, 0.02);
-            fleet.MemberProvider.SetTimestamp(0.02);
-            fleet.MemberRuntime.Update(0.02, 0.02);
-        }
-
-        private static TestFleet CreateFleet()
-        {
-            var layout = new FleetLayout(
-                new[]
-                {
-                    new VehicleLayout(
-                        1,
-                        new Pose2D(-0.5, 0.0, 0.0)),
-                    new VehicleLayout(
-                        2,
-                        new Pose2D(0.5, 0.0, 0.0))
-                });
-            var network = new InMemoryFleetTransportNetwork(
-                new[] { 1, 2 },
-                leaderVehicleId: 1);
-            var leaderTransport = network.CreateEndpoint(1);
-            var memberTransport = network.CreateEndpoint(2);
-            var leaderProvider = new MutableStateProvider(
-                new Pose2D(-0.5, 0.0, 0.0));
-            var memberProvider = new MutableStateProvider(
-                new Pose2D(0.5, 0.0, 0.0));
-            var leaderAgent = CreateAgent(1);
-            var memberAgent = CreateAgent(2);
-
-            var leaderRuntime = new FleetRuntime(
-                selfVehicleId: 1,
-                leaderVehicleId: 1,
-                leaderTransport,
-                leaderAgent,
-                leaderProvider,
-                new FleetPreparationCoordinator(),
-                CreateCoordinator(),
-                new FleetSafetySupervisor(
-                    communicationTimeoutSeconds: 0.2),
-                commandValidForSeconds: 0.2,
-                preparationTimeoutSeconds: 1.0);
-            var memberRuntime = new FleetRuntime(
-                selfVehicleId: 2,
-                leaderVehicleId: 1,
-                memberTransport,
-                memberAgent,
-                memberProvider,
-                commandValidForSeconds: 0.2);
-
-            return new TestFleet(
-                layout,
-                leaderTransport,
-                leaderProvider,
-                memberProvider,
-                leaderAgent,
-                leaderRuntime,
-                memberRuntime);
-        }
-
-        private static FleetCoordinator CreateCoordinator()
-        {
-            return new FleetCoordinator(
-                new FleetStateEstimator(
-                    maximumMemberStateAgeSeconds: 0.5,
-                    maximumPositionDisagreementMeters: 0.2,
-                    maximumYawDisagreementRadians:
-                        AngleMath.DegreesToRadians(5.0)),
-                new FleetController(
-                    new StraightLateralController(),
-                    new ZeroLongitudinalController(),
-                    new GcpCommandAllocator(Math.PI / 4.0),
-                    virtualControlPointRadiusMeters: 0.5),
-                new FleetMemberCommandCorrector(
-                    longitudinalPositionGainPerSecond: 1.0,
-                    lateralPositionGainPerSecond: 1.0,
-                    yawGainPerSecond: 1.0,
-                    positionErrorDeadbandMeters: 0.005,
-                    yawErrorDeadbandRadians:
-                        AngleMath.DegreesToRadians(0.5),
-                    maximumLinearCorrectionMetersPerSecond: 0.03,
-                    maximumAngularCorrectionRadiansPerSecond:
-                        AngleMath.DegreesToRadians(2.0)),
-                memberPositionErrorWarningMeters: 0.02,
-                maximumMemberPositionErrorMeters: 0.05,
-                memberYawErrorWarningRadians:
-                    AngleMath.DegreesToRadians(1.0),
-                maximumMemberYawErrorRadians:
-                    AngleMath.DegreesToRadians(3.0));
-        }
-
-        private static Trajectory2D CreateTrajectory()
-        {
-            return new Trajectory2D(
-                new[]
-                {
-                    new TrajectoryPoint(
-                        0.0,
-                        Pose2D.Identity,
-                        0.0,
-                        0.2),
-                    new TrajectoryPoint(
-                        1.0,
-                        new Pose2D(1.0, 0.0, 0.0),
-                        0.0,
-                        0.2)
-                });
-        }
-
-        private static FleetMemberAgent CreateAgent(int vehicleId)
-        {
-            var chassis = new MultiWheelChassis();
-            chassis.AddWheel(CreateWheel(-500f, 300f));
-            chassis.AddWheel(CreateWheel(-500f, -300f));
-            chassis.AddWheel(CreateWheel(500f, 300f));
-            chassis.AddWheel(CreateWheel(500f, -300f));
-            chassis.Initialize();
-
-            return new FleetMemberAgent(
-                new MultiWheelChassisAdapter(chassis, vehicleId),
-                alignmentToleranceRadians:
-                    AngleMath.DegreesToRadians(1.0),
-                alignmentStableSeconds: 0.0);
-        }
-
-        private static SteerWheel CreateWheel(float x, float y)
-        {
-            var speed = 0f;
-            var angle = 0f;
-            return new SteerWheel(
-                new Vector2(x, y),
-                angleLowerLimit: -120f,
-                angleUpperLimit: 120f,
-                speedWriter: value => speed = value,
-                speedReader: () => speed,
-                angleWriter: value => angle = value,
-                angleReader: () => angle);
-        }
-
-        private static void AssertState(
-            FleetRuntime runtime,
-            FleetRuntimeState expected,
-            string scenario)
-        {
-            AssertTrue(
-                runtime.State == expected,
-                $"{scenario}状态错误：" +
-                $"actual={runtime.State}, expected={expected}，" +
-                $"reason={runtime.LastFailureReason}");
-        }
-
-        private static void AssertTrue(bool condition, string message)
-        {
-            if (!condition)
-            {
-                throw new InvalidOperationException(message);
-            }
-        }
-
-        private sealed class MutableStateProvider :
-            IVehicleStateProvider
-        {
-            private readonly Pose2D _poseInWorld;
-            private double _timestampSeconds;
-
-            public MutableStateProvider(Pose2D poseInWorld)
-            {
-                _poseInWorld = poseInWorld;
-                IsAvailable = true;
-            }
-
-            public bool IsAvailable { get; set; }
-
-            public void SetTimestamp(double timestampSeconds)
-            {
-                _timestampSeconds = timestampSeconds;
-            }
-
-            public bool TryGetState(out VehicleState state)
-            {
-                state = new VehicleState(
-                    _timestampSeconds,
-                    _poseInWorld,
-                    Twist2D.Zero,
-                    hasValidVelocityEstimate: true);
-                return IsAvailable;
-            }
-        }
-
-        private sealed class StraightLateralController :
-            ILateralController
-        {
-            public LateralControlCommand Compute(
-                PathTrackingContext context)
-            {
-                return LateralControlCommand.Straight;
-            }
-
-            public void Reset()
-            {
-            }
-        }
-
-        private sealed class ZeroLongitudinalController :
-            ILongitudinalController
-        {
-            public double ComputeSpeedMetersPerSecond(
-                PathTrackingContext context)
-            {
-                return 0.0;
-            }
-
-            public void Reset()
-            {
-            }
-        }
-
-        private sealed class TestFleet
-        {
-            public TestFleet(
-                FleetLayout layout,
-                InMemoryFleetTransport leaderTransport,
-                MutableStateProvider leaderProvider,
-                MutableStateProvider memberProvider,
-                FleetMemberAgent leaderAgent,
-                FleetRuntime leaderRuntime,
-                FleetRuntime memberRuntime)
-            {
-                Layout = layout;
-                LeaderTransport = leaderTransport;
-                LeaderProvider = leaderProvider;
-                MemberProvider = memberProvider;
-                LeaderAgent = leaderAgent;
-                LeaderRuntime = leaderRuntime;
-                MemberRuntime = memberRuntime;
-            }
-
-            public FleetLayout Layout { get; }
-
-            public InMemoryFleetTransport LeaderTransport { get; }
-
-            public MutableStateProvider LeaderProvider { get; }
-
-            public MutableStateProvider MemberProvider { get; }
-
-            public FleetMemberAgent LeaderAgent { get; }
-
-            public FleetRuntime LeaderRuntime { get; }
-
-            public FleetRuntime MemberRuntime { get; }
-        }
-    }
-}

MultiWheelC.Tests/FleetSafetySupervisorTests.cs

@@ -1,297 +0,0 @@
-using System;
-using System.Collections.Generic;
-using MultiWheelC.Fleet;
-using MyParking.Shared;
-
-namespace MultiWheelC.Tests
-{
-    internal static class FleetSafetySupervisorTests
-    {
-        private const long PlanId = 7;
-        private const double CurrentTimeSeconds = 10.0;
-        private const double CommunicationTimeoutSeconds = 0.5;
-
-        public static void Run()
-        {
-            VerifyHealthyFleetContinues();
-            VerifyMissingMemberStopsFleet();
-            VerifyCommunicationTimeoutStopsFleet();
-            VerifyUnavailableStateStopsFleet();
-            VerifyMemberFaultStopsFleet();
-            VerifyFailureCodeStopsFleet();
-            VerifyPlanMismatchStopsFleet();
-            VerifyStopIsLatchedUntilNewPlanStarts();
-
-            Console.WriteLine(
-                "FleetSafetySupervisor测试通过，共8个场景。");
-        }
-
-        private static void VerifyHealthyFleetContinues()
-        {
-            var supervisor = CreateStartedSupervisor();
-
-            var decision = supervisor.Evaluate(
-                CreateLayout(),
-                CreateHealthyStatuses(),
-                CurrentTimeSeconds);
-
-            AssertFalse(
-                decision.ShouldStop,
-                "成员状态健康时不应停车");
-        }
-
-        private static void VerifyMissingMemberStopsFleet()
-        {
-            var supervisor = CreateStartedSupervisor();
-
-            var decision = supervisor.Evaluate(
-                CreateLayout(),
-                new[] { CreateHealthyStatus(1) },
-                CurrentTimeSeconds);
-
-            AssertStopFromVehicle(
-                decision,
-                2,
-                "缺少成员报告");
-        }
-
-        private static void VerifyCommunicationTimeoutStopsFleet()
-        {
-            var supervisor = CreateStartedSupervisor();
-            var statuses = new[]
-            {
-                CreateHealthyStatus(1),
-                new FleetMemberSafetyStatus(
-                    vehicleId: 2,
-                    planId: PlanId,
-                    isStateAvailable: true,
-                    isFaulted: false,
-                    failureCode: 0,
-                    lastAcceptedReportTimeSeconds: 9.4)
-            };
-
-            var decision = supervisor.Evaluate(
-                CreateLayout(),
-                statuses,
-                CurrentTimeSeconds);
-
-            AssertStopFromVehicle(
-                decision,
-                2,
-                "通信超时");
-        }
-
-        private static void VerifyUnavailableStateStopsFleet()
-        {
-            var supervisor = CreateStartedSupervisor();
-            var statuses = new[]
-            {
-                CreateHealthyStatus(1),
-                new FleetMemberSafetyStatus(
-                    vehicleId: 2,
-                    planId: PlanId,
-                    isStateAvailable: false,
-                    isFaulted: false,
-                    failureCode: 0,
-                    lastAcceptedReportTimeSeconds: 9.9)
-            };
-
-            var decision = supervisor.Evaluate(
-                CreateLayout(),
-                statuses,
-                CurrentTimeSeconds);
-
-            AssertStopFromVehicle(
-                decision,
-                2,
-                "状态不可用");
-        }
-
-        private static void VerifyMemberFaultStopsFleet()
-        {
-            var supervisor = CreateStartedSupervisor();
-            var statuses = new[]
-            {
-                CreateHealthyStatus(1),
-                new FleetMemberSafetyStatus(
-                    vehicleId: 2,
-                    planId: PlanId,
-                    isStateAvailable: true,
-                    isFaulted: true,
-                    failureCode: 0,
-                    lastAcceptedReportTimeSeconds: 9.9)
-            };
-
-            var decision = supervisor.Evaluate(
-                CreateLayout(),
-                statuses,
-                CurrentTimeSeconds);
-
-            AssertStopFromVehicle(
-                decision,
-                2,
-                "成员故障状态");
-        }
-
-        private static void VerifyFailureCodeStopsFleet()
-        {
-            var supervisor = CreateStartedSupervisor();
-            var statuses = new[]
-            {
-                CreateHealthyStatus(1),
-                new FleetMemberSafetyStatus(
-                    vehicleId: 2,
-                    planId: PlanId,
-                    isStateAvailable: true,
-                    isFaulted: false,
-                    failureCode: 42,
-                    lastAcceptedReportTimeSeconds: 9.9)
-            };
-
-            var decision = supervisor.Evaluate(
-                CreateLayout(),
-                statuses,
-                CurrentTimeSeconds);
-
-            AssertStopFromVehicle(
-                decision,
-                2,
-                "成员故障码");
-        }
-
-        private static void VerifyPlanMismatchStopsFleet()
-        {
-            var supervisor = CreateStartedSupervisor();
-            var statuses = new[]
-            {
-                CreateHealthyStatus(1),
-                new FleetMemberSafetyStatus(
-                    vehicleId: 2,
-                    planId: PlanId - 1,
-                    isStateAvailable: true,
-                    isFaulted: false,
-                    failureCode: 0,
-                    lastAcceptedReportTimeSeconds: 9.9)
-            };
-
-            var decision = supervisor.Evaluate(
-                CreateLayout(),
-                statuses,
-                CurrentTimeSeconds);
-
-            AssertStopFromVehicle(
-                decision,
-                2,
-                "任务编号不一致");
-        }
-
-        private static void VerifyStopIsLatchedUntilNewPlanStarts()
-        {
-            var supervisor = CreateStartedSupervisor();
-            supervisor.Evaluate(
-                CreateLayout(),
-                new[] { CreateHealthyStatus(1) },
-                CurrentTimeSeconds);
-
-            var latchedDecision = supervisor.Evaluate(
-                CreateLayout(),
-                CreateHealthyStatuses(),
-                CurrentTimeSeconds);
-            AssertTrue(
-                latchedDecision.ShouldStop,
-                "故障恢复后停车决定仍应锁存");
-
-            supervisor.Start(PlanId + 1);
-            var recoveredDecision = supervisor.Evaluate(
-                CreateLayout(),
-                new[]
-                {
-                    CreateHealthyStatus(1, PlanId + 1),
-                    CreateHealthyStatus(2, PlanId + 1)
-                },
-                CurrentTimeSeconds);
-            AssertFalse(
-                recoveredDecision.ShouldStop,
-                "开始新任务后应清除旧任务停车锁存");
-        }
-
-        private static FleetSafetySupervisor
-            CreateStartedSupervisor()
-        {
-            var supervisor = new FleetSafetySupervisor(
-                CommunicationTimeoutSeconds);
-            supervisor.Start(PlanId);
-            return supervisor;
-        }
-
-        private static FleetLayout CreateLayout()
-        {
-            return new FleetLayout(
-                new[]
-                {
-                    new VehicleLayout(
-                        1,
-                        new Pose2D(-1.0, 0.0, 0.0)),
-                    new VehicleLayout(
-                        2,
-                        new Pose2D(1.0, 0.0, Math.PI))
-                });
-        }
-
-        private static IReadOnlyList<FleetMemberSafetyStatus>
-            CreateHealthyStatuses()
-        {
-            return new[]
-            {
-                CreateHealthyStatus(1),
-                CreateHealthyStatus(2)
-            };
-        }
-
-        private static FleetMemberSafetyStatus CreateHealthyStatus(
-            int vehicleId,
-            long planId = PlanId)
-        {
-            return new FleetMemberSafetyStatus(
-                vehicleId,
-                planId,
-                isStateAvailable: true,
-                isFaulted: false,
-                failureCode: 0,
-                lastAcceptedReportTimeSeconds: 9.9);
-        }
-
-        private static void AssertStopFromVehicle(
-            FleetSafetyDecision decision,
-            int expectedVehicleId,
-            string scenario)
-        {
-            AssertTrue(
-                decision.ShouldStop,
-                $"{scenario}时应停车");
-            AssertTrue(
-                decision.SourceVehicleId == expectedVehicleId,
-                $"{scenario}的来源车辆不正确");
-            AssertTrue(
-                !string.IsNullOrWhiteSpace(decision.Reason),
-                $"{scenario}应提供停车原因");
-        }
-
-        private static void AssertTrue(
-            bool condition,
-            string message)
-        {
-            if (!condition)
-            {
-                throw new InvalidOperationException(message);
-            }
-        }
-
-        private static void AssertFalse(
-            bool condition,
-            string message)
-        {
-            AssertTrue(!condition, message);
-        }
-    }
-}

MultiWheelC.Tests/FleetStateEstimatorTests.cs

@@ -1,430 +0,0 @@
-using System;
-using System.Collections.Generic;
-using MultiWheelC.Fleet;
-using MyParking.Shared;
-
-namespace MultiWheelC.Tests
-{
-    internal static class FleetStateEstimatorTests
-    {
-        private const double Tolerance = 1e-9;
-
-        public static void Run()
-        {
-            VerifyRigidStateIsRecovered();
-            VerifyOlderSamplesAreAligned();
-            VerifyYawWrapAroundIsAveraged();
-            VerifySmallLayoutErrorIsReported();
-            VerifyInconsistentCentersAreRejected();
-            VerifyMissingMemberIsRejected();
-            VerifyInvalidVelocityRemainsExplicit();
-
-            Console.WriteLine(
-                "FleetStateEstimator车队状态估计测试通过。共7个场景。");
-        }
-
-        private static void VerifyRigidStateIsRecovered()
-        {
-            var layout = CreateLayout();
-            var fleetPose = new Pose2D(4.0, -2.0, 0.4);
-            var fleetTwist = new Twist2D(0.3, -0.1, 0.2);
-            var result = CreateEstimator().Estimate(
-                layout,
-                CreateRigidMemberStates(
-                    layout,
-                    fleetPose,
-                    fleetTwist,
-                    sampleTimestampSeconds: 5.0,
-                    hasValidVelocityEstimate: true),
-                targetTimestampSeconds: 5.0);
-
-            var state = RequireState(result, "刚体状态还原");
-            AssertPose(
-                state.FleetPoseInWorld,
-                fleetPose,
-                "刚体状态还原");
-            AssertTwist(
-                state.TwistAtFleetOriginInWorld,
-                fleetTwist,
-                "刚体速度还原");
-
-            for (var index = 0;
-                 index < result.MemberErrors.Count;
-                 index++)
-            {
-                AssertPose(
-                    result.MemberErrors[index]
-                        .ActualPoseInExpectedVehicleFrame,
-                    Pose2D.Identity,
-                    "刚体布局误差");
-            }
-        }
-
-        private static void VerifyOlderSamplesAreAligned()
-        {
-            var layout = CreateLayout();
-            var sampleFleetPose =
-                new Pose2D(1.0, 2.0, 0.3);
-            var fleetTwist =
-                new Twist2D(0.4, -0.2, 0.0);
-            var result = CreateEstimator().Estimate(
-                layout,
-                CreateRigidMemberStates(
-                    layout,
-                    sampleFleetPose,
-                    fleetTwist,
-                    sampleTimestampSeconds: 0.9,
-                    hasValidVelocityEstimate: true),
-                targetTimestampSeconds: 1.0);
-
-            var state = RequireState(result, "成员时间对齐");
-            AssertPose(
-                state.FleetPoseInWorld,
-                new Pose2D(1.04, 1.98, 0.3),
-                "成员时间对齐");
-            AssertTwist(
-                state.TwistAtFleetOriginInWorld,
-                fleetTwist,
-                "时间对齐后速度");
-        }
-
-        private static void VerifySmallLayoutErrorIsReported()
-        {
-            var layout = CreateLayout();
-            var result = CreateEstimator().Estimate(
-                layout,
-                new[]
-                {
-                    CreateMemberState(
-                        1,
-                        new Pose2D(-0.98, 0.0, 0.0),
-                        Twist2D.Zero,
-                        1.0,
-                        true),
-                    CreateMemberState(
-                        2,
-                        new Pose2D(0.99, 0.0, Math.PI),
-                        Twist2D.Zero,
-                        1.0,
-                        true)
-                },
-                targetTimestampSeconds: 1.0);
-
-            var state = RequireState(result, "小范围布局误差");
-            AssertNear(
-                state.FleetPoseInWorld.XMeters,
-                0.005,
-                "小范围布局误差中心X");
-            AssertNear(
-                FindError(result.MemberErrors, 1)
-                    .ActualPoseInExpectedVehicleFrame.XMeters,
-                0.015,
-                "车辆1布局误差X");
-            AssertNear(
-                FindError(result.MemberErrors, 2)
-                    .ActualPoseInExpectedVehicleFrame.XMeters,
-                0.015,
-                "车辆2布局误差X");
-        }
-
-        private static void VerifyYawWrapAroundIsAveraged()
-        {
-            var layout = CreateLayout();
-            var firstCandidate = new Pose2D(
-                0.0,
-                0.0,
-                AngleMath.DegreesToRadians(179.0));
-            var secondCandidate = new Pose2D(
-                0.0,
-                0.0,
-                AngleMath.DegreesToRadians(-179.0));
-            var result = CreateEstimator().Estimate(
-                layout,
-                new[]
-                {
-                    CreateMemberState(
-                        1,
-                        FrameTransform2D.Compose(
-                            firstCandidate,
-                            layout.Vehicles[0].PoseInFleet),
-                        Twist2D.Zero,
-                        1.0,
-                        true),
-                    CreateMemberState(
-                        2,
-                        FrameTransform2D.Compose(
-                            secondCandidate,
-                            layout.Vehicles[1].PoseInFleet),
-                        Twist2D.Zero,
-                        1.0,
-                        true)
-                },
-                targetTimestampSeconds: 1.0);
-
-            var state = RequireState(result, "跨正负π航向平均");
-            AssertNear(
-                Math.Abs(state.FleetPoseInWorld.YawRadians),
-                Math.PI,
-                "跨正负π航向平均");
-        }
-
-        private static void VerifyInconsistentCentersAreRejected()
-        {
-            var result = CreateEstimator().Estimate(
-                CreateLayout(),
-                new[]
-                {
-                    CreateMemberState(
-                        1,
-                        new Pose2D(-1.0, 0.0, 0.0),
-                        Twist2D.Zero,
-                        1.0,
-                        true),
-                    CreateMemberState(
-                        2,
-                        new Pose2D(1.3, 0.0, Math.PI),
-                        Twist2D.Zero,
-                        1.0,
-                        true)
-                },
-                targetTimestampSeconds: 1.0);
-
-            AssertUnavailable(result, "候选中心冲突");
-        }
-
-        private static void VerifyMissingMemberIsRejected()
-        {
-            var result = CreateEstimator().Estimate(
-                CreateLayout(),
-                new[]
-                {
-                    CreateMemberState(
-                        1,
-                        new Pose2D(-1.0, 0.0, 0.0),
-                        Twist2D.Zero,
-                        1.0,
-                        true)
-                },
-                targetTimestampSeconds: 1.0);
-
-            AssertUnavailable(result, "成员缺失");
-        }
-
-        private static void VerifyInvalidVelocityRemainsExplicit()
-        {
-            var layout = CreateLayout();
-            var result = CreateEstimator().Estimate(
-                layout,
-                CreateRigidMemberStates(
-                    layout,
-                    Pose2D.Identity,
-                    new Twist2D(0.4, 0.0, 0.0),
-                    sampleTimestampSeconds: 1.0,
-                    hasValidVelocityEstimate: false),
-                targetTimestampSeconds: 1.0);
-
-            var state = RequireState(result, "速度未初始化");
-            if (state.HasValidVelocityEstimate)
-            {
-                throw new InvalidOperationException(
-                    "成员速度无效时车队速度不应标记为有效。");
-            }
-
-            AssertTwist(
-                state.TwistAtFleetOriginInWorld,
-                Twist2D.Zero,
-                "速度未初始化");
-        }
-
-        private static FleetStateEstimator CreateEstimator()
-        {
-            return new FleetStateEstimator(
-                maximumMemberStateAgeSeconds: 0.25,
-                maximumPositionDisagreementMeters: 0.1,
-                maximumYawDisagreementRadians:
-                    AngleMath.DegreesToRadians(5.0));
-        }
-
-        private static FleetLayout CreateLayout()
-        {
-            return new FleetLayout(
-                new[]
-                {
-                    new VehicleLayout(
-                        1,
-                        new Pose2D(-1.0, 0.0, 0.0)),
-                    new VehicleLayout(
-                        2,
-                        new Pose2D(1.0, 0.0, Math.PI))
-                });
-        }
-
-        private static FleetMemberStateSample[]
-            CreateRigidMemberStates(
-                FleetLayout layout,
-                Pose2D fleetPoseInWorld,
-                Twist2D twistAtFleetOriginInWorld,
-                double sampleTimestampSeconds,
-                bool hasValidVelocityEstimate)
-        {
-            var states =
-                new FleetMemberStateSample[layout.VehicleCount];
-
-            for (var index = 0;
-                 index < layout.Vehicles.Count;
-                 index++)
-            {
-                var vehicle = layout.Vehicles[index];
-                var memberPoseInWorld =
-                    FrameTransform2D.Compose(
-                        fleetPoseInWorld,
-                        vehicle.PoseInFleet);
-                var xFromFleetOrigin =
-                    memberPoseInWorld.XMeters -
-                    fleetPoseInWorld.XMeters;
-                var yFromFleetOrigin =
-                    memberPoseInWorld.YMeters -
-                    fleetPoseInWorld.YMeters;
-                var memberTwistInWorld = new Twist2D(
-                    twistAtFleetOriginInWorld
-                        .VxMetersPerSecond -
-                    twistAtFleetOriginInWorld
-                        .OmegaRadiansPerSecond *
-                    yFromFleetOrigin,
-                    twistAtFleetOriginInWorld
-                        .VyMetersPerSecond +
-                    twistAtFleetOriginInWorld
-                        .OmegaRadiansPerSecond *
-                    xFromFleetOrigin,
-                    twistAtFleetOriginInWorld
-                        .OmegaRadiansPerSecond);
-
-                states[index] = new FleetMemberStateSample(
-                    vehicle.VehicleId,
-                    sampleTimestampSeconds,
-                    memberPoseInWorld,
-                    memberTwistInWorld,
-                    isStateAvailable: true,
-                    hasValidVelocityEstimate:
-                        hasValidVelocityEstimate);
-            }
-
-            return states;
-        }
-
-        private static FleetMemberStateSample CreateMemberState(
-            int vehicleId,
-            Pose2D poseInWorld,
-            Twist2D twistInWorld,
-            double timestampSeconds,
-            bool hasValidVelocityEstimate)
-        {
-            return new FleetMemberStateSample(
-                vehicleId,
-                timestampSeconds,
-                poseInWorld,
-                twistInWorld,
-                isStateAvailable: true,
-                hasValidVelocityEstimate:
-                    hasValidVelocityEstimate);
-        }
-
-        private static FleetState RequireState(
-            FleetStateEstimateResult result,
-            string scenario)
-        {
-            if (!result.IsAvailable || !result.State.HasValue)
-            {
-                throw new InvalidOperationException(
-                    $"{scenario}应产生可用状态：" +
-                    result.UnavailableReason);
-            }
-
-            return result.State.Value;
-        }
-
-        private static void AssertUnavailable(
-            FleetStateEstimateResult result,
-            string scenario)
-        {
-            if (result.IsAvailable ||
-                string.IsNullOrWhiteSpace(
-                    result.UnavailableReason))
-            {
-                throw new InvalidOperationException(
-                    $"{scenario}应返回带原因的不可用结果。");
-            }
-        }
-
-        private static FleetMemberLayoutError FindError(
-            IReadOnlyList<FleetMemberLayoutError> errors,
-            int vehicleId)
-        {
-            for (var index = 0;
-                 index < errors.Count;
-                 index++)
-            {
-                if (errors[index].VehicleId == vehicleId)
-                {
-                    return errors[index];
-                }
-            }
-
-            throw new InvalidOperationException(
-                $"没有找到车辆{vehicleId}的布局误差。");
-        }
-
-        private static void AssertPose(
-            Pose2D actual,
-            Pose2D expected,
-            string scenario)
-        {
-            AssertNear(
-                actual.XMeters,
-                expected.XMeters,
-                scenario + " X");
-            AssertNear(
-                actual.YMeters,
-                expected.YMeters,
-                scenario + " Y");
-            AssertNear(
-                AngleMath.ShortestDifferenceRadians(
-                    actual.YawRadians,
-                    expected.YawRadians),
-                0.0,
-                scenario + " Yaw");
-        }
-
-        private static void AssertTwist(
-            Twist2D actual,
-            Twist2D expected,
-            string scenario)
-        {
-            AssertNear(
-                actual.VxMetersPerSecond,
-                expected.VxMetersPerSecond,
-                scenario + " Vx");
-            AssertNear(
-                actual.VyMetersPerSecond,
-                expected.VyMetersPerSecond,
-                scenario + " Vy");
-            AssertNear(
-                actual.OmegaRadiansPerSecond,
-                expected.OmegaRadiansPerSecond,
-                scenario + " Omega");
-        }
-
-        private static void AssertNear(
-            double actual,
-            double expected,
-            string valueName)
-        {
-            if (Math.Abs(actual - expected) > Tolerance)
-            {
-                throw new InvalidOperationException(
-                    $"{valueName}错误：" +
-                    $"actual={actual:F9}, expected={expected:F9}。");
-            }
-        }
-    }
-}

MultiWheelC.Tests/InMemoryFleetTransport.cs

@@ -1,240 +0,0 @@
-using System;
-using System.Collections.Generic;
-using MultiWheelC.Fleet;
-using MyParking.Shared;
-
-namespace MultiWheelC.Tests
-{
-    // 为同一测试进程中的各车辆端点提供共享FIFO消息队列。
-    internal sealed class InMemoryFleetTransportNetwork
-    {
-        private readonly SharedState _sharedState;
-        private readonly HashSet<int> _createdEndpointIds =
-            new HashSet<int>();
-
-        public InMemoryFleetTransportNetwork(
-            IReadOnlyList<int> vehicleIds,
-            int leaderVehicleId)
-        {
-            _sharedState = new SharedState(
-                vehicleIds,
-                leaderVehicleId);
-        }
-
-        public InMemoryFleetTransport CreateEndpoint(
-            int vehicleId)
-        {
-            lock (_sharedState.SyncRoot)
-            {
-                if (!_sharedState.CommandQueues.ContainsKey(
-                        vehicleId))
-                {
-                    throw new ArgumentOutOfRangeException(
-                        nameof(vehicleId),
-                        $"车辆{vehicleId}不属于当前内存车队网络。");
-                }
-
-                if (!_createdEndpointIds.Add(vehicleId))
-                {
-                    throw new InvalidOperationException(
-                        $"车辆{vehicleId}的内存通信端点已经创建。");
-                }
-            }
-
-            return new InMemoryFleetTransport(
-                _sharedState,
-                vehicleId);
-        }
-
-        internal sealed class SharedState
-        {
-            public SharedState(
-                IReadOnlyList<int> vehicleIds,
-                int leaderVehicleId)
-            {
-                if (vehicleIds == null)
-                {
-                    throw new ArgumentNullException(
-                        nameof(vehicleIds));
-                }
-
-                if (vehicleIds.Count == 0)
-                {
-                    throw new ArgumentException(
-                        "内存车队网络至少需要一辆车。",
-                        nameof(vehicleIds));
-                }
-
-                if (leaderVehicleId <= 0)
-                {
-                    throw new ArgumentOutOfRangeException(
-                        nameof(leaderVehicleId),
-                        "主车编号必须大于零。");
-                }
-
-                CommandQueues =
-                    new Dictionary<int, Queue<FleetCommand>>();
-
-                for (var index = 0;
-                     index < vehicleIds.Count;
-                     index++)
-                {
-                    var vehicleId = vehicleIds[index];
-                    if (vehicleId <= 0)
-                    {
-                        throw new ArgumentOutOfRangeException(
-                            nameof(vehicleIds),
-                            $"第{index}辆车的编号必须大于零。");
-                    }
-
-                    if (CommandQueues.ContainsKey(vehicleId))
-                    {
-                        throw new ArgumentException(
-                            $"内存车队网络包含重复车号{vehicleId}。",
-                            nameof(vehicleIds));
-                    }
-
-                    CommandQueues.Add(
-                        vehicleId,
-                        new Queue<FleetCommand>());
-                }
-
-                if (!CommandQueues.ContainsKey(
-                        leaderVehicleId))
-                {
-                    throw new ArgumentException(
-                        $"主车{leaderVehicleId}不在车辆列表中。",
-                        nameof(leaderVehicleId));
-                }
-
-                LeaderVehicleId = leaderVehicleId;
-            }
-
-            public object SyncRoot { get; } = new object();
-
-            public int LeaderVehicleId { get; }
-
-            public Dictionary<int, Queue<FleetCommand>>
-                CommandQueues { get; }
-
-            public Queue<FleetMemberReport> ReportQueue { get; } =
-                new Queue<FleetMemberReport>();
-        }
-    }
-
-    // 单辆模拟车辆持有的通信端点；只负责消息路由，不解释控制语义。
-    internal sealed class InMemoryFleetTransport : IFleetTransport
-    {
-        private readonly InMemoryFleetTransportNetwork.SharedState
-            _sharedState;
-        private readonly int _localVehicleId;
-
-        internal InMemoryFleetTransport(
-            InMemoryFleetTransportNetwork.SharedState sharedState,
-            int localVehicleId)
-        {
-            _sharedState = sharedState ??
-                throw new ArgumentNullException(
-                    nameof(sharedState));
-            _localVehicleId = localVehicleId;
-        }
-
-        public void SendCommand(FleetCommand command)
-        {
-            if (_localVehicleId !=
-                _sharedState.LeaderVehicleId)
-            {
-                throw new InvalidOperationException(
-                    "只有主车通信端点可以发送车队命令。");
-            }
-
-            lock (_sharedState.SyncRoot)
-            {
-                if (command.TargetVehicleId ==
-                    FleetProtocol.BroadcastVehicleId)
-                {
-                    foreach (var pair in
-                             _sharedState.CommandQueues)
-                    {
-                        // 主车本地命令由运行入口直接执行，不通过通信回环。
-                        if (pair.Key != _localVehicleId)
-                        {
-                            pair.Value.Enqueue(command);
-                        }
-                    }
-
-                    return;
-                }
-
-                if (!_sharedState.CommandQueues.TryGetValue(
-                        command.TargetVehicleId,
-                        out var queue))
-                {
-                    throw new ArgumentOutOfRangeException(
-                        nameof(command),
-                        $"目标车辆{command.TargetVehicleId}不存在。");
-                }
-
-                queue.Enqueue(command);
-            }
-        }
-
-        public void SendReport(FleetMemberReport report)
-        {
-            if (report.VehicleId != _localVehicleId)
-            {
-                throw new ArgumentException(
-                    $"车辆{_localVehicleId}不能发送属于车辆" +
-                    $"{report.VehicleId}的状态报告。",
-                    nameof(report));
-            }
-
-            lock (_sharedState.SyncRoot)
-            {
-                _sharedState.ReportQueue.Enqueue(report);
-            }
-        }
-
-        public bool TryReceiveCommand(
-            out FleetCommand command)
-        {
-            lock (_sharedState.SyncRoot)
-            {
-                var queue =
-                    _sharedState.CommandQueues[_localVehicleId];
-                if (queue.Count == 0)
-                {
-                    command = default;
-                    return false;
-                }
-
-                command = queue.Dequeue();
-                return true;
-            }
-        }
-
-        public bool TryReceiveReport(
-            out FleetMemberReport report)
-        {
-            if (_localVehicleId !=
-                _sharedState.LeaderVehicleId)
-            {
-                throw new InvalidOperationException(
-                    "只有主车通信端点可以接收成员状态报告。");
-            }
-
-            lock (_sharedState.SyncRoot)
-            {
-                if (_sharedState.ReportQueue.Count == 0)
-                {
-                    report = default;
-                    return false;
-                }
-
-                report =
-                    _sharedState.ReportQueue.Dequeue();
-                return true;
-            }
-        }
-    }
-}

MultiWheelC.Tests/InMemoryFleetTransportTests.cs

@@ -1,198 +0,0 @@
-using System;
-using MultiWheelC.Fleet;
-using MyParking.Shared;
-
-namespace MultiWheelC.Tests
-{
-    internal static class InMemoryFleetTransportTests
-    {
-        public static void Run()
-        {
-            VerifyTargetedCommandRoutingAndFifoOrder();
-            VerifyBroadcastReachesAllFollowersOnly();
-            VerifyMemberReportReturnsToLeader();
-            VerifyEndpointRolesAndVehicleIdentity();
-
-            Console.WriteLine(
-                "InMemoryFleetTransport测试通过，共4个场景。");
-        }
-
-        private static void VerifyTargetedCommandRoutingAndFifoOrder()
-        {
-            var network = CreateNetwork();
-            var leader = network.CreateEndpoint(1);
-            var member2 = network.CreateEndpoint(2);
-            var member3 = network.CreateEndpoint(3);
-
-            leader.SendCommand(CreateCommand(2, sequenceNumber: 1));
-            leader.SendCommand(CreateCommand(2, sequenceNumber: 2));
-
-            AssertTrue(
-                member2.TryReceiveCommand(out var first) &&
-                first.SequenceNumber == 1,
-                "定向命令第一条应到达目标车辆");
-            AssertTrue(
-                member2.TryReceiveCommand(out var second) &&
-                second.SequenceNumber == 2,
-                "定向命令应保持FIFO顺序");
-            AssertFalse(
-                member2.TryReceiveCommand(out _),
-                "目标车辆不应收到额外命令");
-            AssertFalse(
-                member3.TryReceiveCommand(out _),
-                "其他成员不应收到定向命令");
-        }
-
-        private static void VerifyBroadcastReachesAllFollowersOnly()
-        {
-            var network = CreateNetwork();
-            var leader = network.CreateEndpoint(1);
-            var member2 = network.CreateEndpoint(2);
-            var member3 = network.CreateEndpoint(3);
-
-            leader.SendCommand(
-                CreateCommand(
-                    FleetProtocol.BroadcastVehicleId,
-                    sequenceNumber: 3));
-
-            AssertTrue(
-                member2.TryReceiveCommand(out var command2) &&
-                command2.SequenceNumber == 3,
-                "广播命令应到达成员车2");
-            AssertTrue(
-                member3.TryReceiveCommand(out var command3) &&
-                command3.SequenceNumber == 3,
-                "广播命令应到达成员车3");
-            AssertFalse(
-                leader.TryReceiveCommand(out _),
-                "主车本地命令不应通过通信层回环");
-        }
-
-        private static void VerifyMemberReportReturnsToLeader()
-        {
-            var network = CreateNetwork();
-            var leader = network.CreateEndpoint(1);
-            var member2 = network.CreateEndpoint(2);
-
-            member2.SendReport(
-                CreateReport(
-                    vehicleId: 2,
-                    sequenceNumber: 8));
-
-            AssertTrue(
-                leader.TryReceiveReport(out var report),
-                "主车应收到成员报告");
-            AssertTrue(
-                report.VehicleId == 2 &&
-                report.SequenceNumber == 8,
-                "主车收到的成员报告内容不正确");
-            AssertFalse(
-                leader.TryReceiveReport(out _),
-                "报告队列取空后应返回false");
-        }
-
-        private static void VerifyEndpointRolesAndVehicleIdentity()
-        {
-            var network = CreateNetwork();
-            var leader = network.CreateEndpoint(1);
-            var member2 = network.CreateEndpoint(2);
-
-            AssertThrows<InvalidOperationException>(
-                () => member2.SendCommand(
-                    CreateCommand(1, sequenceNumber: 1)),
-                "从车不能发送车队命令");
-            AssertThrows<InvalidOperationException>(
-                () => member2.TryReceiveReport(out _),
-                "从车不能消费全队成员报告");
-            AssertThrows<ArgumentException>(
-                () => member2.SendReport(
-                    CreateReport(
-                        vehicleId: 1,
-                        sequenceNumber: 1)),
-                "端点不能冒用其他车辆身份");
-
-            leader.SendReport(
-                CreateReport(
-                    vehicleId: 1,
-                    sequenceNumber: 2));
-            AssertTrue(
-                leader.TryReceiveReport(out var leaderReport) &&
-                leaderReport.VehicleId == 1,
-                "主车作为成员时也应能上报本车状态");
-        }
-
-        private static InMemoryFleetTransportNetwork CreateNetwork()
-        {
-            return new InMemoryFleetTransportNetwork(
-                new[] { 1, 2, 3 },
-                leaderVehicleId: 1);
-        }
-
-        private static FleetCommand CreateCommand(
-            int targetVehicleId,
-            long sequenceNumber)
-        {
-            return new FleetCommand(
-                planId: 5,
-                sequenceNumber,
-                targetVehicleId,
-                FleetCommandKind.Stop,
-                motionDirectionInBodyRadians: 0.0,
-                twistInVehicleBody: Twist2D.Zero,
-                validForSeconds: 0.5);
-        }
-
-        private static FleetMemberReport CreateReport(
-            int vehicleId,
-            long sequenceNumber)
-        {
-            return new FleetMemberReport(
-                vehicleId,
-                planId: 5,
-                sequenceNumber,
-                sampleTimestampSeconds: 1.0,
-                poseInCommonWorld: Pose2D.Identity,
-                twistAtVehicleOriginInCommonWorld:
-                    Twist2D.Zero,
-                isStateAvailable: true,
-                hasValidVelocityEstimate: true,
-                FleetMemberState.Active,
-                lastAppliedCommandSequence: 1);
-        }
-
-        private static void AssertThrows<TException>(
-            Action action,
-            string scenario)
-            where TException : Exception
-        {
-            try
-            {
-                action();
-            }
-            catch (TException)
-            {
-                return;
-            }
-
-            throw new InvalidOperationException(
-                $"{scenario}时应抛出{typeof(TException).Name}。");
-        }
-
-        private static void AssertTrue(
-            bool condition,
-            string message)
-        {
-            if (!condition)
-            {
-                throw new InvalidOperationException(message);
-            }
-        }
-
-        private static void AssertFalse(
-            bool condition,
-            string message)
-        {
-            AssertTrue(!condition, message);
-        }
-    }
-}

MultiWheelC.Tests/MultiWheelC.Tests.csproj

@@ -1,20 +0,0 @@
-<Project Sdk="Microsoft.NET.Sdk">
-
-  <PropertyGroup>
-    <OutputType>Exe</OutputType>
-    <TargetFramework>net8.0</TargetFramework>
-    <LangVersion>10</LangVersion>
-    <IsPackable>false</IsPackable>
-  </PropertyGroup>
-
-  <ItemGroup>
-    <ProjectReference Include="..\MultiWheelC\MultiWheelC.csproj" />
-  </ItemGroup>
-
-  <ItemGroup>
-    <Reference Include="CommonUsage">
-      <HintPath>..\ref\CommonUsage.dll</HintPath>
-    </Reference>
-  </ItemGroup>
-
-</Project>

MultiWheelC.Tests/Program.cs

@@ -1,209 +0,0 @@
-using System;
-using MultiWheelC.Control.Abstractions;
-using MultiWheelC.Control.Lateral;
-using MultiWheelC.Trajectory;
-using MyParking.Shared;
-
-namespace MultiWheelC.Tests
-{
-    /// <summary>
-    /// 验证Stanley控制器在前进和倒车时的横向误差符号及差动转角方向。
-    /// </summary>
-    internal static class Program
-    {
-        private const double SpeedMagnitudeMetersPerSecond = 0.4;
-        private const double TestLateralErrorMeters = 0.1;
-        private const double TestHeadingErrorRadians = 0.1;
-        private const double TestCurvaturePerMeter = 0.2;
-
-        /// <summary>
-        /// 运行不依赖宿主、Detour或实车底盘的控制器数学测试。
-        /// </summary>
-        private static void Main()
-        {
-            foreach (var travelDirection in new[] { 1.0, -1.0 })
-            {
-                VerifyCrossTrackConvergence(
-                    travelDirection,
-                    TestLateralErrorMeters);
-                VerifyCrossTrackConvergence(
-                    travelDirection,
-                    -TestLateralErrorMeters);
-                VerifyHeadingDirection(travelDirection);
-                VerifyCurvatureDirection(travelDirection);
-            }
-
-            Console.WriteLine(
-                "Stanley前进/倒车横向符号测试通过。共8个场景。");
-            FleetLayoutCaptureTests.Run();
-            FleetStateEstimatorTests.Run();
-            FleetKinematicsTests.Run();
-            FleetControllerTests.Run();
-            FleetMemberCommandCorrectorTests.Run();
-            FleetCoordinatorTests.Run();
-            FleetPreparationCoordinatorTests.Run();
-            FleetMemberAgentTests.Run();
-            FleetSafetySupervisorTests.Run();
-            InMemoryFleetTransportTests.Run();
-            FleetRuntimeTests.Run();
-        }
-
-        /// <summary>
-        /// 验证共同转角产生的横向速度始终使轨迹点序横向误差绝对值减小。
-        /// </summary>
-        private static void VerifyCrossTrackConvergence(
-            double travelDirection,
-            double lateralErrorMeters)
-        {
-            var signedSpeedMetersPerSecond =
-                travelDirection *
-                SpeedMagnitudeMetersPerSecond;
-            var controller = CreateController();
-            var command = controller.Compute(
-                CreateContext(
-                    signedSpeedMetersPerSecond,
-                    lateralErrorMeters,
-                    headingErrorRadians: 0.0,
-                    feedforwardCurvaturePerMeter: 0.0));
-
-            var bodyLateralSpeedMetersPerSecond =
-                signedSpeedMetersPerSecond *
-                Math.Sin(command.CommonAngleRadians);
-
-            // 轨迹执行方向在倒车时与车体X轴相反，因此需要先把车体
-            // 横向速度换算到轨迹点序坐标系，再计算参考轨迹相对车辆的误差变化率。
-            var lateralErrorDerivativeMetersPerSecond =
-                -travelDirection *
-                bodyLateralSpeedMetersPerSecond;
-
-            AssertTrue(
-                lateralErrorMeters *
-                lateralErrorDerivativeMetersPerSecond < 0.0,
-                $"横向误差没有收敛：direction={travelDirection}，" +
-                $"error={lateralErrorMeters:F3}m，" +
-                $"common={command.CommonAngleRadians:F6}rad，" +
-                $"errorDerivative={lateralErrorDerivativeMetersPerSecond:F6}m/s。");
-        }
-
-        /// <summary>
-        /// 验证航向误差差动转角在倒车时仍按行驶方向反号。
-        /// </summary>
-        private static void VerifyHeadingDirection(
-            double travelDirection)
-        {
-            var command = CreateController().Compute(
-                CreateContext(
-                    travelDirection *
-                    SpeedMagnitudeMetersPerSecond,
-                    lateralErrorMeters: 0.0,
-                    headingErrorRadians:
-                        TestHeadingErrorRadians,
-                    feedforwardCurvaturePerMeter: 0.0));
-
-            AssertSameSign(
-                command.DifferentialAngleRadians,
-                travelDirection,
-                "航向误差差动转角");
-        }
-
-        /// <summary>
-        /// 验证正曲率前馈差动转角在倒车时仍按行驶方向反号。
-        /// </summary>
-        private static void VerifyCurvatureDirection(
-            double travelDirection)
-        {
-            var command = CreateController().Compute(
-                CreateContext(
-                    travelDirection *
-                    SpeedMagnitudeMetersPerSecond,
-                    lateralErrorMeters: 0.0,
-                    headingErrorRadians: 0.0,
-                    feedforwardCurvaturePerMeter:
-                        TestCurvaturePerMeter));
-
-            AssertSameSign(
-                command.DifferentialAngleRadians,
-                travelDirection,
-                "曲率前馈差动转角");
-        }
-
-        /// <summary>
-        /// 创建使用固定参数的无状态Stanley控制器。
-        /// </summary>
-        private static StanleyLateralController CreateController()
-        {
-            return new StanleyLateralController(
-                controlPointRadiusMeters: 0.5,
-                crossTrackGainPerSecond: 1.0,
-                headingErrorGain: 1.0,
-                minimumSpeedMetersPerSecond: 0.05,
-                useActualSpeedForGain: true);
-        }
-
-        /// <summary>
-        /// 创建只包含本次符号测试所需字段的轨迹跟踪上下文。
-        /// </summary>
-        private static PathTrackingContext CreateContext(
-            double signedSpeedMetersPerSecond,
-            double lateralErrorMeters,
-            double headingErrorRadians,
-            double feedforwardCurvaturePerMeter)
-        {
-            var actualTwistInBody = new Twist2D(
-                signedSpeedMetersPerSecond,
-                0.0,
-                0.0);
-            var referencePoint = new TrajectoryPoint(
-                arcLengthMeters: 0.0,
-                poseInWorld: Pose2D.Identity,
-                curvaturePerMeter: 0.0,
-                referenceSpeedMetersPerSecond:
-                    signedSpeedMetersPerSecond);
-            var projection = new TrajectoryProjection(
-                segmentStartIndex: 0,
-                referencePoint: referencePoint,
-                lateralErrorMeters: lateralErrorMeters,
-                headingErrorRadians: headingErrorRadians,
-                distanceToTrajectoryMeters:
-                    Math.Abs(lateralErrorMeters),
-                remainingDistanceMeters: 1.0);
-
-            return new PathTrackingContext(
-                actualTwistInBody,
-                true,
-                projection,
-                signedSpeedMetersPerSecond,
-                feedforwardCurvaturePerMeter,
-                deltaTimeSeconds: 0.02);
-        }
-
-        /// <summary>
-        /// 验证实际值与预期符号一致。
-        /// </summary>
-        private static void AssertSameSign(
-            double actualValue,
-            double expectedSign,
-            string valueName)
-        {
-            AssertTrue(
-                Math.Sign(actualValue) ==
-                Math.Sign(expectedSign),
-                $"{valueName}方向错误：actual={actualValue:F6}，" +
-                $"expectedSign={expectedSign:F0}。");
-        }
-
-        /// <summary>
-        /// 条件不成立时抛出异常，使测试进程以非零状态结束。
-        /// </summary>
-        private static void AssertTrue(
-            bool condition,
-            string failureMessage)
-        {
-            if (!condition)
-            {
-                throw new InvalidOperationException(
-                    failureMessage);
-            }
-        }
-    }
-}

MultiWheelC/Configuration/PilotConfig.ParkingControl.cs

@@ -1,204 +0,0 @@
-using ClumsyCore;
-
-namespace MultiWheelC;
-
-/// <summary>
-/// 定义停车状态估计、轨迹跟踪和原地自转使用的车辆级参数。
-/// </summary>
-public partial class PilotConfig
-{
-    #region 停车控制-状态估计
-
-    [FieldMember(desc = "停车控制：Detour最大合理线速度(m/s)")]
-    public float ParkingDetourMaximumLinearSpeed = 1.20f;
-
-    [FieldMember(desc = "停车控制：Detour最大合理角速度(deg/s)")]
-    public float ParkingDetourMaximumAngularSpeedDegrees = 45f;
-
-    [FieldMember(desc = "停车控制：Detour位置跳变余量(m)")]
-    public float ParkingDetourPositionJumpMargin = 0.03f;
-
-    [FieldMember(desc = "停车控制：Detour航向跳变余量(deg)")]
-    public float ParkingDetourHeadingJumpMarginDegrees = 5f;
-
-    [FieldMember(desc = "停车控制：Detour速度预测位置残差(m)")]
-    public float ParkingDetourVelocityPositionResidual = 0.04f;
-
-    [FieldMember(desc = "停车控制：Detour速度预测航向残差(deg)")]
-    public float ParkingDetourVelocityHeadingResidualDegrees = 5f;
-
-    [FieldMember(desc = "停车控制：Detour航向异常确认新帧数")]
-    public int ParkingDetourHeadingOutlierConfirmationFrames = 3;
-
-    [FieldMember(desc = "停车控制：Detour航向异常短时预测超时(s)")]
-    public float ParkingDetourHeadingOutlierPredictionTimeoutSeconds =
-        0.30f;
-
-    [FieldMember(desc = "停车控制：Detour静止确认时间(s)")]
-    public float ParkingDetourStationaryConfirmationSeconds = 0.35f;
-
-    [FieldMember(desc = "停车控制：Detour跳变确认新帧数")]
-    public int ParkingDetourJumpConfirmationFrames = 3;
-
-    [FieldMember(desc = "停车控制：Detour跳变确认超时(s)")]
-    public float ParkingDetourJumpConfirmationTimeoutSeconds = 0.60f;
-
-    [FieldMember(desc = "停车控制：Detour自动坐标连续化最大平移(m)")]
-    public float ParkingDetourMaximumAutomaticFrameShift = 0.15f;
-
-    [FieldMember(desc = "停车控制：Detour自动坐标连续化最大航向变化(deg)")]
-    public float ParkingDetourMaximumAutomaticHeadingShiftDegrees = 5f;
-
-    [FieldMember(desc = "停车控制：Detour缓存帧最大允许时间(s)")]
-    public float ParkingDetourMaximumCachedFrameAgeSeconds = 0.50f;
-
-    [FieldMember(desc = "停车控制：Detour定位质量失效/恢复确认新帧数")]
-    public int ParkingDetourLocalizationQualityConfirmationFrames = 3;
-
-    [FieldMember(desc = "停车控制：Detour线速度滤波时间常数(s)")]
-    public float ParkingDetourLinearVelocityFilterSeconds = 0.15f;
-
-    [FieldMember(desc = "停车控制：Detour角速度滤波时间常数(s)")]
-    public float ParkingDetourAngularVelocityFilterSeconds = 0.20f;
-
-    [FieldMember(desc = "停车控制：电机反馈速度滤波时间常数(s)")]
-    public float ParkingWheelVelocityFilterSeconds = 0.10f;
-
-    #endregion
-
-    #region 停车控制-Stanley
-
-    [FieldMember(desc = "停车控制：Stanley横向误差增益(1/s)")]
-    public float ParkingStanleyCrossTrackGain = 0.4f;
-
-    [FieldMember(desc = "停车控制：Stanley航向误差增益")]
-    public float ParkingStanleyHeadingGain = 1.0f;
-
-    [FieldMember(desc = "停车控制：Stanley最低分母速度(m/s)")]
-    public float ParkingStanleyMinimumSpeed = 0.15f;
-
-    [FieldMember(desc = "停车控制：Stanley使用电机实际速度")]
-    public bool ParkingStanleyUseActualSpeed = true;
-
-    [FieldMember(desc = "停车控制：Stanley曲率前馈预瞄时间(s)，0为关闭")]
-    public float ParkingStanleyCurvaturePreviewSeconds = 0.15f;
-
-    [FieldMember(desc = "停车控制：Stanley曲率前馈最大预瞄距离(m)")]
-    public float ParkingStanleyMaximumCurvaturePreviewMeters = 0.12f;
-
-    [FieldMember(desc = "停车控制：Stanley横向修正上限(deg)")]
-    public float ParkingMaximumCrossTrackCorrectionDegrees = 10f;
-
-    [FieldMember(desc = "停车控制：Stanley航向修正上限(deg)")]
-    public float ParkingMaximumHeadingCorrectionDegrees = 10f;
-
-    #endregion
-
-    #region 停车控制-纵向PID
-
-    [FieldMember(desc = "停车控制：纵向速度Kp")]
-    public float ParkingLongitudinalKp = 0.5f;
-
-    [FieldMember(desc = "停车控制：纵向速度Ki(1/s)")]
-    public float ParkingLongitudinalKi = 0f;
-
-    [FieldMember(desc = "停车控制：纵向速度Kd(s)")]
-    public float ParkingLongitudinalKd = 0f;
-
-    [FieldMember(desc = "停车控制：纵向积分修正上限(m/s)")]
-    public float ParkingMaximumIntegralCorrection = 0.05f;
-
-    [FieldMember(desc = "停车控制：纵向速度误差死区(m/s)")]
-    public float ParkingLongitudinalSpeedErrorDeadband = 0.025f;
-
-    [FieldMember(desc = "停车控制：最大命令速度(m/s)")]
-    public float ParkingMaximumCommandSpeed = 0.50f;
-
-    #endregion
-
-    #region 停车控制-原地自转
-
-    [FieldMember(desc = "停车控制：原地自转Kp")]
-    public float InPlaceRotateKp = 1.0f;
-
-    [FieldMember(desc = "停车控制：原地自转Ki")]
-    public float InPlaceRotateKi = 0f;
-
-    [FieldMember(desc = "停车控制：原地自转Kd")]
-    public float InPlaceRotateKd = 0f;
-
-    [FieldMember(desc = "停车控制：原地自转积分限幅")]
-    public float InPlaceRotateMaxI = 0f;
-
-    [FieldMember(desc = "停车控制：原地自转到位角度容差(deg)")]
-    public float InPlaceRotateArriveDeg = 1.5f;
-
-    [FieldMember(desc = "停车控制：原地自转起转前舵轮对齐容差(deg)")]
-    public float InPlaceRotateWheelAlignDeg = 2f;
-
-    [FieldMember(desc = "停车控制：原地自转最小有效角速度(deg/s)")]
-    public float InPlaceRotateMinimumSpeed = 1f;
-
-    [FieldMember(desc = "停车控制：原地自转最大角速度(deg/s)")]
-    // public float InPlaceRotateMaxSpeed = 47.5f;
-    public float InPlaceRotateMaxSpeed = 45f;
-
-    [FieldMember(desc = "停车控制：原地自转角加速度(deg/s²)")]
-    // public float InPlaceRotateAcc = 60f;
-    public float InPlaceRotateAcc = 45f;
-
-    [FieldMember(desc = "停车控制：原地自转超时(s)")]
-    public float InPlaceRotateTimeoutSec = 15f;
-
-    #endregion
-
-    #region 停车控制-舵轮回正
-
-    [FieldMember(desc = "停车控制：舵轮回正到位容差(deg)")]
-    public float ParkingWheelForwardToleranceDegrees = 2f;
-
-    [FieldMember(desc = "停车控制：舵轮回正稳定确认时间(s)")]
-    public float ParkingWheelForwardStableSeconds = 0.3f;
-
-    [FieldMember(desc = "停车控制：舵轮回正超时(s，0关闭)")]
-    public float ParkingWheelForwardTimeoutSeconds = 10f;
-
-    #endregion
-
-    #region 停车控制-GCP与完成条件
-
-    [FieldMember(desc = "停车控制：最大GCP转角(deg)")]
-    public float ParkingMaximumGcpAngleDegrees = 45f;
-
-    [FieldMember(desc = "停车控制：最大GCP转角速度(deg/s)")]
-    public float ParkingMaximumGcpAngleRateDegreesPerSecond = 15f;
-
-    [FieldMember(desc = "停车控制：终点距离容差(m)")]
-    public float ParkingFinishDistance = 0.03f;
-
-    [FieldMember(desc = "停车控制：终点速度容差(m/s)")]
-    public float ParkingFinishSpeed = 0.02f;
-
-    [FieldMember(desc = "停车控制：终点航向容差(deg)")]
-    public float ParkingFinishHeadingToleranceDegrees = 3f;
-
-    [FieldMember(desc = "停车控制：终点制动预瞄距离(m)")]
-    public float ParkingTerminalBrakingPreview = 0.02f;
-
-    [FieldMember(desc = "停车控制：终点单向逼近范围(m)")]
-    public float ParkingTerminalApproachDistance = 0.10f;
-
-    [FieldMember(desc = "停车控制：终点单向逼近增益(1/s)")]
-    public float ParkingTerminalApproachGain = 0.8f;
-
-    [FieldMember(desc = "停车控制：终点单向逼近最大速度(m/s)")]
-    public float ParkingTerminalMaximumApproachSpeed = 0.05f;
-
-    [FieldMember(desc = "停车控制：最大轨迹偏离距离(m)")]
-    public float ParkingMaximumDistanceToTrajectory = 0.30f;
-
-    [FieldMember(desc = "停车控制：轨迹执行超时(s)")]
-    public float ParkingExecutionTimeoutSeconds = 120f;
-
-    #endregion
-}

MultiWheelC/Control/Abstractions/ILateralController.cs

@@ -1,19 +0,0 @@
-namespace MultiWheelC.Control.Abstractions
-{
-    /// <summary>
-    /// 定义Stanley、LQR和MPC等车体中心横向控制器的统一接口。
-    /// </summary>
-    public interface ILateralController
-    {
-        /// <summary>
-        /// 根据本周期车辆状态和轨迹误差计算车体中心目标曲率。
-        /// </summary>
-        LateralControlCommand Compute(
-            PathTrackingContext context);
-
-        /// <summary>
-        /// 清除控制器跨周期状态，以便开始新轨迹或异常恢复后重新运行。
-        /// </summary>
-        void Reset();
-    }
-}

MultiWheelC/Control/Abstractions/ILongitudinalController.cs

@@ -1,19 +0,0 @@
-namespace MultiWheelC.Control.Abstractions
-{
-    /// <summary>
-    /// 定义根据参考速度和实际纵向速度生成底盘命令速度的统一接口。
-    /// </summary>
-    public interface ILongitudinalController
-    {
-        /// <summary>
-        /// 根据本周期速度目标、速度反馈和时间间隔计算有符号底盘命令速度。
-        /// </summary>
-        double ComputeSpeedMetersPerSecond(
-            PathTrackingContext context);
-
-        /// <summary>
-        /// 清除积分、历史误差和其他跨周期状态，以便安全开始新的控制过程。
-        /// </summary>
-        void Reset();
-    }
-}

MultiWheelC/Control/Abstractions/LateralControlCommand.cs

@@ -1,76 +0,0 @@
-using System;
-
-namespace MultiWheelC.Control.Abstractions
-{
-    /// <summary>
-    /// 表示横向控制器生成的前、后GCP目标转角，单位为rad，逆时针为正。
-    /// </summary>
-    public readonly struct LateralControlCommand
-    {
-        /// <summary>
-        /// 创建前、后GCP目标转角命令。
-        /// </summary>
-        public LateralControlCommand(
-            double frontGcpAngleRadians,
-            double rearGcpAngleRadians)
-        {
-            EnsureFinite(
-                frontGcpAngleRadians,
-                nameof(frontGcpAngleRadians));
-            EnsureFinite(
-                rearGcpAngleRadians,
-                nameof(rearGcpAngleRadians));
-
-            FrontGcpAngleRadians =
-                frontGcpAngleRadians;
-            RearGcpAngleRadians =
-                rearGcpAngleRadians;
-        }
-
-        /// <summary>
-        /// 获取前GCP目标转角，单位为rad，逆时针为正。
-        /// </summary>
-        public double FrontGcpAngleRadians { get; }
-
-        /// <summary>
-        /// 获取后GCP目标转角，单位为rad，逆时针为正。
-        /// </summary>
-        public double RearGcpAngleRadians { get; }
-
-        /// <summary>
-        /// 获取前后GCP的共同转角分量，主要用于横向平移修正。
-        /// </summary>
-        public double CommonAngleRadians =>
-            (FrontGcpAngleRadians +
-             RearGcpAngleRadians) / 2.0;
-
-        /// <summary>
-        /// 获取前后GCP的差动转角分量，主要用于曲率前馈和航向修正。
-        /// </summary>
-        public double DifferentialAngleRadians =>
-            (FrontGcpAngleRadians -
-             RearGcpAngleRadians) / 2.0;
-
-        /// <summary>
-        /// 创建前后GCP均保持车头方向的直线命令。
-        /// </summary>
-        public static LateralControlCommand Straight =>
-            new LateralControlCommand(0.0, 0.0);
-
-        /// <summary>
-        /// 检查GCP目标转角是否为有限值。
-        /// </summary>
-        private static void EnsureFinite(
-            double value,
-            string parameterName)
-        {
-            if (double.IsNaN(value) ||
-                double.IsInfinity(value))
-            {
-                throw new ArgumentOutOfRangeException(
-                    parameterName,
-                    "GCP目标转角必须是有限值。");
-            }
-        }
-    }
-}

MultiWheelC/Control/Abstractions/PathTrackingContext.cs

@@ -1,156 +0,0 @@
-using System;
-using MultiWheelC.Trajectory;
-using MyParking.Shared;
-
-namespace MultiWheelC.Control.Abstractions
-{
-    /// <summary>
-    /// 保存一次轨迹跟踪控制周期使用的刚体速度、轨迹投影和真实时间间隔。
-    /// </summary>
-    public readonly struct PathTrackingContext
-    {
-        /// <summary>
-        /// 创建横向和纵向控制器共享的只读控制输入快照。
-        /// </summary>
-        public PathTrackingContext(
-            Twist2D actualTwistInBody,
-            bool hasValidVelocityEstimate,
-            TrajectoryProjection projection,
-            double controlReferenceSpeedMetersPerSecond,
-            double feedforwardCurvaturePerMeter,
-            double deltaTimeSeconds,
-            double motionDirectionInBodyRadians = 0.0)
-        {
-            EnsureFinite(
-                controlReferenceSpeedMetersPerSecond,
-                nameof(controlReferenceSpeedMetersPerSecond));
-            EnsureFinite(
-                feedforwardCurvaturePerMeter,
-                nameof(feedforwardCurvaturePerMeter));
-            EnsureFinitePositive(
-                deltaTimeSeconds,
-                nameof(deltaTimeSeconds));
-            EnsureFinite(
-                motionDirectionInBodyRadians,
-                nameof(motionDirectionInBodyRadians));
-            NumericGuard.EnsureFinite(
-                actualTwistInBody,
-                nameof(actualTwistInBody));
-
-            ActualTwistInBody = actualTwistInBody;
-            HasValidVelocityEstimate =
-                hasValidVelocityEstimate;
-            Projection = projection;
-            ControlReferenceSpeedMetersPerSecond =
-                controlReferenceSpeedMetersPerSecond;
-            FeedforwardCurvaturePerMeter =
-                feedforwardCurvaturePerMeter;
-            DeltaTimeSeconds = deltaTimeSeconds;
-            MotionDirectionInBodyRadians =
-                AngleMath.NormalizeRadians(
-                    motionDirectionInBodyRadians);
-        }
-
-        /// <summary>
-        /// 获取受控刚体坐标系下的实际速度。
-        /// </summary>
-        public Twist2D ActualTwistInBody { get; }
-
-        /// <summary>
-        /// 获取实际车体中心投影到参考轨迹后得到的参考状态和跟踪误差。
-        /// </summary>
-        public TrajectoryProjection Projection { get; }
-
-        /// <summary>
-        /// 获取本次控制计算距离上次计算的真实时间间隔，单位为s。
-        /// </summary>
-        public double DeltaTimeSeconds { get; }
-
-        /// <summary>
-        /// 获取轨迹原始速度经过起步释放和制动预瞄处理后，本控制周期实际使用的有符号参考速度，单位为m/s。
-        /// </summary>
-        public double ControlReferenceSpeedMetersPerSecond { get; }
-
-        /// <summary>
-        /// 获取车辆沿当前运动坐标系X轴方向的实际纵向速度，单位为m/s。
-        /// </summary>
-        public double ActualLongitudinalSpeedMetersPerSecond =>
-            Math.Cos(MotionDirectionInBodyRadians) *
-                ActualTwistInBody.VxMetersPerSecond +
-            Math.Sin(MotionDirectionInBodyRadians) *
-                ActualTwistInBody.VyMetersPerSecond;
-
-        /// <summary>
-        /// 获取当前运动坐标系X轴在车体系中的方向，单位为rad。
-        /// </summary>
-        public double MotionDirectionInBodyRadians { get; }
-
-        /// <summary>
-        /// 获取沿轨迹执行点序定义的参考曲率，单位为1/m，左弯为正。
-        /// </summary>
-        public double ReferenceCurvaturePerMeter =>
-            Projection.ReferencePoint
-                .CurvaturePerMeter;
-
-        /// <summary>
-        /// 获取沿轨迹点序适量预瞄后专供几何前馈使用的参考曲率，单位为1/m，左弯为正。
-        /// </summary>
-        public double FeedforwardCurvaturePerMeter { get; }
-
-        /// <summary>
-        /// 获取相对轨迹执行点序的有符号横向误差，单位为m，参考轨迹位于执行方向左侧时为正。
-        /// </summary>
-        public double LateralErrorMeters =>
-            Projection.LateralErrorMeters;
-
-        /// <summary>
-        /// 获取参考航向减实际车体航向的最短角差，单位为rad，逆时针为正。
-        /// </summary>
-        public double HeadingErrorRadians =>
-            Projection.HeadingErrorRadians;
-
-        /// <summary>
-        /// 获取当前投影位置沿参考轨迹到终点的剩余距离，单位为m。
-        /// </summary>
-        public double RemainingDistanceMeters =>
-            Projection.RemainingDistanceMeters;
-
-        /// <summary>
-        /// 获取本周期实际速度估计是否可供闭环控制使用。
-        /// </summary>
-        public bool HasValidVelocityEstimate { get; }
-
-        /// <summary>
-        /// 检查控制周期是否为正有限值。
-        /// </summary>
-        private static void EnsureFinitePositive(
-            double value,
-            string parameterName)
-        {
-            if (double.IsNaN(value) ||
-                double.IsInfinity(value) ||
-                value <= 0.0)
-            {
-                throw new ArgumentOutOfRangeException(
-                    parameterName,
-                    "轨迹跟踪控制周期必须是正有限值。");
-            }
-        }
-
-        /// <summary>
-        /// 检查控制参考速度是否为有限值。
-        /// </summary>
-        private static void EnsureFinite(
-            double value,
-            string parameterName)
-        {
-            if (double.IsNaN(value) ||
-                double.IsInfinity(value))
-            {
-                throw new ArgumentOutOfRangeException(
-                    parameterName,
-                    "轨迹跟踪参考速度必须是有限值。");
-            }
-        }
-    }
-}

MultiWheelC/Control/Allocation/GcpCommandAllocator.cs

@@ -1,73 +0,0 @@
-using System;
-using MultiWheelC.Control.Abstractions;
-using MyParking.Shared;
-// 限制目标角度的最大绝对值，例如不能超过60°。
-namespace MultiWheelC.Control.Allocation
-{
-    /// <summary>
-    /// 独立限制前后GCP目标转角并与纵向速度组合成底盘运动命令。
-    /// </summary>
-    public sealed class GcpCommandAllocator
-    {
-        /// <summary>
-        /// 创建使用指定前后GCP最大转角的命令分配器。
-        /// </summary>
-        public GcpCommandAllocator(double maximumGcpAngleRadians)
-        {
-            NumericGuard.EnsureFinitePositive(
-                maximumGcpAngleRadians,
-                nameof(maximumGcpAngleRadians));
-
-            if (maximumGcpAngleRadians >= Math.PI / 2.0)
-            {
-                throw new ArgumentOutOfRangeException(
-                    nameof(maximumGcpAngleRadians),
-                    "最大GCP转角必须小于π/2。");
-            }
-
-            MaximumGcpAngleRadians = maximumGcpAngleRadians;
-        }
-
-        /// <summary>
-        /// 获取前后GCP允许的最大转角绝对值，单位为rad。
-        /// </summary>
-        public double MaximumGcpAngleRadians { get; }
-
-        /// <summary>
-        /// 将纵向速度和前后GCP转角组合为底盘运动命令。
-        /// </summary>
-        public GcpMotionCommand Allocate(
-            double speedMetersPerSecond,
-            LateralControlCommand lateralCommand)
-        {
-            NumericGuard.EnsureFinite(
-                speedMetersPerSecond,
-                nameof(speedMetersPerSecond));
-
-            var frontAngleRadians = ClampSymmetric(
-                lateralCommand.FrontGcpAngleRadians,
-                MaximumGcpAngleRadians);
-            var rearAngleRadians = ClampSymmetric(
-                lateralCommand.RearGcpAngleRadians,
-                MaximumGcpAngleRadians);
-
-            return new GcpMotionCommand(
-                speedMetersPerSecond,
-                frontAngleRadians,
-                rearAngleRadians);
-        }
-
-        /// <summary>
-        /// 将数值按正负对称方式限制在指定绝对值内。
-        /// </summary>
-        private static double ClampSymmetric(
-            double value,
-            double maximumAbsoluteValue)
-        {
-            return Math.Max(
-                -maximumAbsoluteValue,
-                Math.Min(maximumAbsoluteValue, value));
-        }
-
-    }
-}

MultiWheelC/Control/Allocation/GcpKinematics.cs

@@ -1,125 +0,0 @@
-using System;
-using MyParking.Shared;
-
-namespace MultiWheelC.Control.Allocation
-{
-    /// <summary>
-    /// 在对称前后GCP方向命令与车体中心刚体速度之间执行纯几何转换。
-    /// </summary>
-    public static class GcpKinematics
-    {
-        private const double ParallelDirectionTolerance = 1e-9;
-        private const double StopSpeedDeadbandMetersPerSecond = 1e-6;
-
-        /// <summary>
-        /// 将有符号中心速度和前后GCP方向转换为真实车体坐标系中的Twist2D。
-        /// </summary>
-        public static Twist2D ToBodyTwist(
-            GcpMotionCommand command,
-            double controlPointRadiusMeters)
-        {
-            NumericGuard.EnsureFinitePositive(
-                controlPointRadiusMeters,
-                nameof(controlPointRadiusMeters));
-
-            if (Math.Abs(command.SpeedMetersPerSecond) <=
-                StopSpeedDeadbandMetersPerSecond)
-            {
-                return Twist2D.Zero;
-            }
-
-            var frontAngleRadians =
-                command.FrontAngleRadians;
-            var rearAngleRadians =
-                command.RearAngleRadians;
-            var directionDeterminant =
-                Math.Sin(
-                    rearAngleRadians -
-                    frontAngleRadians);
-
-            if (Math.Abs(directionDeterminant) <=
-                ParallelDirectionTolerance)
-            {
-                var averageDirectionRadians =
-                    Math.Atan2(
-                        Math.Sin(frontAngleRadians) +
-                        Math.Sin(rearAngleRadians),
-                        Math.Cos(frontAngleRadians) +
-                        Math.Cos(rearAngleRadians));
-
-                return new Twist2D(
-                    command.SpeedMetersPerSecond *
-                    Math.Cos(averageDirectionRadians),
-                    command.SpeedMetersPerSecond *
-                    Math.Sin(averageDirectionRadians),
-                    0.0);
-            }
-
-            var frontCosine =
-                Math.Cos(frontAngleRadians);
-            var frontSine =
-                Math.Sin(frontAngleRadians);
-            var rearCosine =
-                Math.Cos(rearAngleRadians);
-            var rearSine =
-                Math.Sin(rearAngleRadians);
-
-            // 两个GCP速度方向的法线交点就是瞬时旋转中心，坐标位于真实车体系。
-            var rotationCenterXMeters =
-                controlPointRadiusMeters *
-                (frontCosine * rearSine +
-                 frontSine * rearCosine) /
-                directionDeterminant;
-            var rotationCenterYMeters =
-                -2.0 *
-                controlPointRadiusMeters *
-                frontCosine *
-                rearCosine /
-                directionDeterminant;
-            var centerRadiusMeters =
-                Math.Sqrt(
-                    rotationCenterXMeters *
-                    rotationCenterXMeters +
-                    rotationCenterYMeters *
-                    rotationCenterYMeters);
-
-            if (!NumericGuard.IsFinite(centerRadiusMeters) ||
-                centerRadiusMeters <= 0.0)
-            {
-                throw new InvalidOperationException(
-                    "前后GCP方向不能生成有效的车体中心旋转半径。");
-            }
-
-            // 用有符号速度决定绕ICR的实际转向，避免倒车时把同一组GCP轴向解释成反向运动。
-            var requestedDirectionSign =
-                Math.Sign(command.SpeedMetersPerSecond);
-            var positiveAngularFrontVelocityXMetersPerSecond =
-                rotationCenterYMeters;
-            var positiveAngularFrontVelocityYMetersPerSecond =
-                controlPointRadiusMeters -
-                rotationCenterXMeters;
-            var frontDirectionAlignment =
-                positiveAngularFrontVelocityXMetersPerSecond *
-                requestedDirectionSign *
-                frontCosine +
-                positiveAngularFrontVelocityYMetersPerSecond *
-                requestedDirectionSign *
-                frontSine;
-            var omegaSign =
-                frontDirectionAlignment >= 0.0
-                    ? 1.0
-                    : -1.0;
-            var omegaRadiansPerSecond =
-                omegaSign *
-                Math.Abs(command.SpeedMetersPerSecond) /
-                centerRadiusMeters;
-
-            return new Twist2D(
-                omegaRadiansPerSecond *
-                rotationCenterYMeters,
-                -omegaRadiansPerSecond *
-                rotationCenterXMeters,
-                omegaRadiansPerSecond);
-        }
-    }
-}

MultiWheelC/Control/Allocation/GcpMotionCommand.cs

@@ -1,52 +0,0 @@
-using MyParking.Shared;
-
-namespace MultiWheelC.Control.Allocation
-{
-    /// <summary>
-    /// 表示发送给旧版多舵轮四轮解算前的有符号速度和前后GCP角度命令。
-    /// </summary>
-    public readonly struct GcpMotionCommand
-    {
-        /// <summary>
-        /// 创建统一使用m/s和rad的前后几何控制点运动命令。
-        /// </summary>
-        public GcpMotionCommand(
-            double speedMetersPerSecond,
-            double frontAngleRadians,
-            double rearAngleRadians)
-        {
-            NumericGuard.EnsureFinite(
-                speedMetersPerSecond,
-                nameof(speedMetersPerSecond));
-            NumericGuard.EnsureFinite(
-                frontAngleRadians,
-                nameof(frontAngleRadians));
-            NumericGuard.EnsureFinite(
-                rearAngleRadians,
-                nameof(rearAngleRadians));
-
-            SpeedMetersPerSecond =
-                speedMetersPerSecond;
-            FrontAngleRadians =
-                frontAngleRadians;
-            RearAngleRadians =
-                rearAngleRadians;
-        }
-
-        /// <summary>
-        /// 获取准备交给底盘的有符号纵向速度，单位为m/s，正值表示前进。
-        /// </summary>
-        public double SpeedMetersPerSecond { get; }
-
-        /// <summary>
-        /// 获取前几何控制点相对车体X轴的目标方向，单位为rad，逆时针为正。
-        /// </summary>
-        public double FrontAngleRadians { get; }
-
-        /// <summary>
-        /// 获取后几何控制点相对车体X轴的目标方向，单位为rad，逆时针为正。
-        /// </summary>
-        public double RearAngleRadians { get; }
-
-    }
-}

MultiWheelC/Control/Common/PidController.cs

@@ -1,307 +0,0 @@
-using System;
-
-namespace MultiWheelC.Control.Common
-{
-    /// <summary>
-    /// 使用真实控制周期计算带积分限幅、输出限幅和抗饱和的通用有状态PID输出。
-    /// </summary>
-    public sealed class PidController
-    {
-        private double _integralState;
-        private double _previousError;
-        private double _previousMeasurement;
-        private bool _hasPreviousSample;
-
-        /// <summary>
-        /// 创建具有指定增益、积分输出限制和微分形式的PID控制器。
-        /// </summary>
-        public PidController(
-            double proportionalGain,
-            double integralGainPerSecond,
-            double derivativeGainSeconds,
-            double maximumIntegralOutput,
-            bool derivativeOnMeasurement = true)
-        {
-            EnsureFiniteNonNegative(
-                proportionalGain,
-                nameof(proportionalGain));
-            EnsureFiniteNonNegative(
-                integralGainPerSecond,
-                nameof(integralGainPerSecond));
-            EnsureFiniteNonNegative(
-                derivativeGainSeconds,
-                nameof(derivativeGainSeconds));
-            EnsureFiniteNonNegative(
-                maximumIntegralOutput,
-                nameof(maximumIntegralOutput));
-
-            ProportionalGain = proportionalGain;
-            IntegralGainPerSecond = integralGainPerSecond;
-            DerivativeGainSeconds = derivativeGainSeconds;
-            MaximumIntegralOutput = maximumIntegralOutput;
-            DerivativeOnMeasurement = derivativeOnMeasurement;
-        }
-
-        /// <summary>
-        /// 获取比例增益。
-        /// </summary>
-        public double ProportionalGain { get; }
-
-        /// <summary>
-        /// 获取积分增益，单位为1/s。
-        /// </summary>
-        public double IntegralGainPerSecond { get; }
-
-        /// <summary>
-        /// 获取微分增益，单位为s。
-        /// </summary>
-        public double DerivativeGainSeconds { get; }
-
-        /// <summary>
-        /// 获取积分项允许产生的最大输出绝对值。
-        /// </summary>
-        public double MaximumIntegralOutput { get; }
-
-        /// <summary>
-        /// 获取微分项是否作用于测量值，以避免设定值变化产生微分冲击。
-        /// </summary>
-        public bool DerivativeOnMeasurement { get; }
-
-        /// <summary>
-        /// 获取最近一次设定值减测量值的误差。
-        /// </summary>
-        public double LastError { get; private set; }
-
-        /// <summary>
-        /// 获取最近一次比例项输出。
-        /// </summary>
-        public double LastProportionalOutput { get; private set; }
-
-        /// <summary>
-        /// 获取最近一次积分项输出。
-        /// </summary>
-        public double LastIntegralOutput { get; private set; }
-
-        /// <summary>
-        /// 获取最近一次微分项输出。
-        /// </summary>
-        public double LastDerivativeOutput { get; private set; }
-
-        /// <summary>
-        /// 获取最近一次经过输出范围限制后的PID输出。
-        /// </summary>
-        public double LastOutput { get; private set; }
-
-        /// <summary>
-        /// 根据设定值、测量值、真实时间间隔和本周期输出范围更新PID。
-        /// </summary>
-        public double Update(
-            double setPoint,
-            double measurement,
-            double deltaTimeSeconds,
-            double minimumOutput,
-            double maximumOutput)
-        {
-            EnsureFinite(setPoint, nameof(setPoint));
-            EnsureFinite(measurement, nameof(measurement));
-            EnsureFinitePositive(
-                deltaTimeSeconds,
-                nameof(deltaTimeSeconds));
-            EnsureFinite(minimumOutput, nameof(minimumOutput));
-            EnsureFinite(maximumOutput, nameof(maximumOutput));
-
-            if (minimumOutput > maximumOutput)
-            {
-                throw new ArgumentOutOfRangeException(
-                    nameof(minimumOutput),
-                    "PID最小输出不能大于最大输出。");
-            }
-
-            var error = setPoint - measurement;
-            var proportionalOutput =
-                ProportionalGain * error;
-            var derivativeOutput = CalculateDerivativeOutput(
-                error,
-                measurement,
-                deltaTimeSeconds);
-
-            var candidateIntegralState =
-                _integralState +
-                error * deltaTimeSeconds;
-            var integralOutput = CalculateIntegralOutput(
-                candidateIntegralState);
-
-            // 同步截断积分状态本身，避免积分输出虽已限幅、内部状态仍继续增长。
-            candidateIntegralState =
-                IntegralGainPerSecond > 0.0 &&
-                MaximumIntegralOutput > 0.0
-                    ? integralOutput /
-                      IntegralGainPerSecond
-                    : 0.0;
-
-            var unlimitedOutput =
-                proportionalOutput +
-                integralOutput +
-                derivativeOutput;
-            var output = Clamp(
-                unlimitedOutput,
-                minimumOutput,
-                maximumOutput);
-
-            // 根据实际允许输出反算积分项，避免执行器饱和期间继续积累误差。
-            if (IntegralGainPerSecond > 0.0 &&
-                output != unlimitedOutput)
-            {
-                integralOutput = Clamp(
-                    output -
-                    proportionalOutput -
-                    derivativeOutput,
-                    -MaximumIntegralOutput,
-                    MaximumIntegralOutput);
-                candidateIntegralState =
-                    integralOutput /
-                    IntegralGainPerSecond;
-            }
-
-            _integralState =
-                IntegralGainPerSecond > 0.0 &&
-                MaximumIntegralOutput > 0.0
-                    ? candidateIntegralState
-                    : 0.0;
-            _previousError = error;
-            _previousMeasurement = measurement;
-            _hasPreviousSample = true;
-
-            LastError = error;
-            LastProportionalOutput = proportionalOutput;
-            LastIntegralOutput = integralOutput;
-            LastDerivativeOutput = derivativeOutput;
-            LastOutput = output;
-
-            return output;
-        }
-
-        /// <summary>
-        /// 清除积分、历史采样和最近一次PID诊断输出。
-        /// </summary>
-        public void Reset()
-        {
-            _integralState = 0.0;
-            _previousError = 0.0;
-            _previousMeasurement = 0.0;
-            _hasPreviousSample = false;
-            LastError = 0.0;
-            LastProportionalOutput = 0.0;
-            LastIntegralOutput = 0.0;
-            LastDerivativeOutput = 0.0;
-            LastOutput = 0.0;
-        }
-
-        /// <summary>
-        /// 使用测量值微分或误差微分计算本周期微分项输出。
-        /// </summary>
-        private double CalculateDerivativeOutput(
-            double error,
-            double measurement,
-            double deltaTimeSeconds)
-        {
-            if (!_hasPreviousSample ||
-                DerivativeGainSeconds <= 0.0)
-            {
-                return 0.0;
-            }
-
-            if (DerivativeOnMeasurement)
-            {
-                return -DerivativeGainSeconds *
-                    (measurement - _previousMeasurement) /
-                    deltaTimeSeconds;
-            }
-
-            return DerivativeGainSeconds *
-                (error - _previousError) /
-                deltaTimeSeconds;
-        }
-
-        /// <summary>
-        /// 根据积分状态计算经过绝对值限制的积分项输出。
-        /// </summary>
-        private double CalculateIntegralOutput(
-            double integralState)
-        {
-            if (IntegralGainPerSecond <= 0.0 ||
-                MaximumIntegralOutput <= 0.0)
-            {
-                return 0.0;
-            }
-
-            return Clamp(
-                IntegralGainPerSecond * integralState,
-                -MaximumIntegralOutput,
-                MaximumIntegralOutput);
-        }
-
-        /// <summary>
-        /// 将数值限制在指定闭区间内。
-        /// </summary>
-        private static double Clamp(
-            double value,
-            double minimum,
-            double maximum)
-        {
-            return Math.Max(
-                minimum,
-                Math.Min(maximum, value));
-        }
-
-        /// <summary>
-        /// 检查参数是否为正有限值。
-        /// </summary>
-        private static void EnsureFinitePositive(
-            double value,
-            string parameterName)
-        {
-            EnsureFinite(value, parameterName);
-
-            if (value <= 0.0)
-            {
-                throw new ArgumentOutOfRangeException(
-                    parameterName,
-                    "PID时间间隔必须是正有限值。");
-            }
-        }
-
-        /// <summary>
-        /// 检查参数是否为非负有限值。
-        /// </summary>
-        private static void EnsureFiniteNonNegative(
-            double value,
-            string parameterName)
-        {
-            EnsureFinite(value, parameterName);
-
-            if (value < 0.0)
-            {
-                throw new ArgumentOutOfRangeException(
-                    parameterName,
-                    "PID增益和积分输出限幅必须是非负有限值。");
-            }
-        }
-
-        /// <summary>
-        /// 检查参数是否为有限值。
-        /// </summary>
-        private static void EnsureFinite(
-            double value,
-            string parameterName)
-        {
-            if (double.IsNaN(value) ||
-                double.IsInfinity(value))
-            {
-                throw new ArgumentOutOfRangeException(
-                    parameterName,
-                    "PID参数和输入必须是有限值。");
-            }
-        }
-    }
-}

MultiWheelC/Control/Execution/GcpCommandExecutor.cs

@@ -1,194 +0,0 @@
-using System;
-using MultiWheelC.Control.Allocation;
-using MyParking.Shared;
-
-namespace MultiWheelC.Control.Execution
-{
-    /// <summary>
-    /// 将SI单位的GCP运动命令安全转换为现有多舵轮底盘调用。
-    /// </summary>
-    public sealed class GcpCommandExecutor
-    {
-        private const double StopSpeedDeadbandMetersPerSecond =
-            1e-6;
-
-        private readonly MultiWheelChassisAdapter _chassisAdapter;
-        private readonly double _motionDirectionInBodyRadians;
-        private double _lastFrontAngleRadians;
-        private double _lastRearAngleRadians;
-
-        /// <summary>
-        /// 创建绑定指定单车底盘适配器的GCP命令执行器。
-        /// </summary>
-        public GcpCommandExecutor(
-            MultiWheelChassisAdapter chassisAdapter,
-            double maximumGcpAngleRateRadiansPerSecond =
-                10.0 * Math.PI / 180.0,
-            double motionDirectionInBodyRadians = 0.0)
-        {
-            _chassisAdapter = chassisAdapter ??
-                throw new ArgumentNullException(
-                    nameof(chassisAdapter));
-            EnsureFinitePositive(
-                maximumGcpAngleRateRadiansPerSecond,
-                nameof(maximumGcpAngleRateRadiansPerSecond));
-            NumericGuard.EnsureFinite(
-                motionDirectionInBodyRadians,
-                nameof(motionDirectionInBodyRadians));
-
-            MaximumGcpAngleRateRadiansPerSecond =
-                maximumGcpAngleRateRadiansPerSecond;
-            _motionDirectionInBodyRadians =
-                AngleMath.NormalizeRadians(
-                    motionDirectionInBodyRadians);
-        }
-
-        /// <summary>
-        /// 获取执行器绑定的车辆编号。
-        /// </summary>
-        public int VehicleId =>
-            _chassisAdapter.VehicleId;
-
-        /// <summary>
-        /// 获取前后GCP目标角度允许的最大变化率，单位为rad/s。
-        /// </summary>
-        public double MaximumGcpAngleRateRadiansPerSecond { get; }
-
-        /// <summary>
-        /// 获取最近一次控制器请求的未限速GCP命令。
-        /// </summary>
-        public GcpMotionCommand? LastRequestedCommand { get; private set; }
-
-        /// <summary>
-        /// 获取最近一次经过GCP角速度限制后实际发送给底盘的命令。
-        /// </summary>
-        public GcpMotionCommand? LastSentCommand { get; private set; }
-
-        /// <summary>
-        /// 获取最近一次旧版底盘运动分解失败原因。
-        /// </summary>
-        public string LastFailureReason { get; private set; } =
-            string.Empty;
-
-        /// <summary>
-        /// 使用真实控制周期执行一条GCP命令，并在分解失败时保持停车。
-        /// </summary>
-        public bool Execute(
-            GcpMotionCommand command,
-            double deltaTimeSeconds)
-        {
-            EnsureFinitePositive(
-                deltaTimeSeconds,
-                nameof(deltaTimeSeconds));
-            LastRequestedCommand = command;
-
-            if (Math.Abs(command.SpeedMetersPerSecond) <=
-                StopSpeedDeadbandMetersPerSecond)
-            {
-                Stop();
-                LastSentCommand = new GcpMotionCommand(
-                    0.0,
-                    _lastFrontAngleRadians,
-                    _lastRearAngleRadians);
-                return true;
-            }
-
-            var maximumAngleChangeRadians =
-                MaximumGcpAngleRateRadiansPerSecond *
-                deltaTimeSeconds;
-            _lastFrontAngleRadians = MoveTowards(
-                _lastFrontAngleRadians,
-                command.FrontAngleRadians,
-                maximumAngleChangeRadians);
-            _lastRearAngleRadians = MoveTowards(
-                _lastRearAngleRadians,
-                command.RearAngleRadians,
-                maximumAngleChangeRadians);
-
-            var limitedCommand = new GcpMotionCommand(
-                command.SpeedMetersPerSecond,
-                _lastFrontAngleRadians,
-                _lastRearAngleRadians);
-            LastSentCommand = limitedCommand;
-
-            var motionFrameTwist =
-                GcpKinematics.ToBodyTwist(
-                    limitedCommand,
-                    _chassisAdapter.ControlPointRadiusMeters);
-            var bodyTwist =
-                FrameTransform2D.TransformTwistAtSamePoint(
-                    new Pose2D(
-                        0.0,
-                        0.0,
-                        _motionDirectionInBodyRadians),
-                    motionFrameTwist);
-            var success = _chassisAdapter.SendBodyTwist(
-                bodyTwist,
-                TimeSpan.FromSeconds(deltaTimeSeconds));
-
-            LastFailureReason = success
-                ? string.Empty
-                : BuildFailureReason();
-
-            return success;
-        }
-
-        /// <summary>
-        /// 立即清零底盘驱动速度并清除执行器失败状态。
-        /// </summary>
-        public void Stop()
-        {
-            _chassisAdapter.StopImmediately();
-            LastFailureReason = string.Empty;
-        }
-
-        /// <summary>
-        /// 以不超过指定单周期变化量的速度使当前值接近目标值。
-        /// </summary>
-        private static double MoveTowards(
-            double current,
-            double target,
-            double maximumChange)
-        {
-            var difference = target - current;
-
-            if (Math.Abs(difference) <= maximumChange)
-            {
-                return target;
-            }
-
-            return current +
-                   Math.Sign(difference) *
-                   maximumChange;
-        }
-
-        /// <summary>
-        /// 将底盘返回的空失败原因替换为可诊断的默认说明。
-        /// </summary>
-        private string BuildFailureReason()
-        {
-            return string.IsNullOrWhiteSpace(
-                    _chassisAdapter.LastFailureReason)
-                ? "旧版SendMotion未能完成GCP运动分解。"
-                : _chassisAdapter.LastFailureReason;
-        }
-
-        /// <summary>
-        /// 检查控制周期是否为正有限值且能够转换为TimeSpan。
-        /// </summary>
-        private static void EnsureFinitePositive(
-            double value,
-            string parameterName)
-        {
-            if (double.IsNaN(value) ||
-                double.IsInfinity(value) ||
-                value <= 0.0 ||
-                value > TimeSpan.MaxValue.TotalSeconds)
-            {
-                throw new ArgumentOutOfRangeException(
-                    parameterName,
-                    "GCP命令控制周期必须是TimeSpan可表示的正有限秒数。");
-            }
-        }
-    }
-}

MultiWheelC/Control/Execution/ParkingGeometricController.cs

@@ -1,435 +0,0 @@
-using System;
-using System.Diagnostics;
-using MultiWheelC.Control.Abstractions;
-using MultiWheelC.Control.Allocation;
-using MultiWheelC.StateEstimation;
-using MultiWheelC.Trajectory;
-using MyParking.Shared;
-
-namespace MultiWheelC.Control.Execution
-{
-    // 单周期轨迹控制结果。
-    public enum ParkingControlCycleResult
-    {
-        Inactive = 0,
-        CommandSent = 1,
-        Completed = 2,
-        StateUnavailable = 3,
-        Faulted = 4
-    }
-
-    // 单周期各阶段耗时及定位帧更新状态，用于区分控制计算与底层通信延迟。
-    public readonly struct ParkingControlCycleTiming
-    {
-        public ParkingControlCycleTiming(
-            long cycleIndex,
-            double cycleIntervalMilliseconds,
-            double stateReadMilliseconds,
-            double projectionMilliseconds,
-            double controllerComputeMilliseconds,
-            double commandSendMilliseconds,
-            double totalCycleMilliseconds,
-            bool hasStateTimestamp,
-            double stateTimestampSeconds,
-            bool stateTimestampChanged,
-            ParkingControlCycleResult result)
-        {
-            CycleIndex = cycleIndex;
-            CycleIntervalMilliseconds =
-                cycleIntervalMilliseconds;
-            StateReadMilliseconds = stateReadMilliseconds;
-            ProjectionMilliseconds = projectionMilliseconds;
-            ControllerComputeMilliseconds =
-                controllerComputeMilliseconds;
-            CommandSendMilliseconds = commandSendMilliseconds;
-            TotalCycleMilliseconds = totalCycleMilliseconds;
-            HasStateTimestamp = hasStateTimestamp;
-            StateTimestampSeconds = stateTimestampSeconds;
-            StateTimestampChanged = stateTimestampChanged;
-            Result = result;
-        }
-
-        public long CycleIndex { get; }
-        public double CycleIntervalMilliseconds { get; }
-        public double StateReadMilliseconds { get; }
-        public double ProjectionMilliseconds { get; }
-        public double ControllerComputeMilliseconds { get; }
-        public double CommandSendMilliseconds { get; }
-        public double TotalCycleMilliseconds { get; }
-        public bool HasStateTimestamp { get; }
-        public double StateTimestampSeconds { get; }
-        public bool StateTimestampChanged { get; }
-        public ParkingControlCycleResult Result { get; }
-    }
-
-    // 负责单车状态读取、公共轨迹计算和真实底盘命令发送。
-    public sealed class ParkingGeometricController
-    {
-        private readonly IVehicleStateProvider _stateProvider;
-        private readonly GcpCommandExecutor _commandExecutor;
-        private readonly PathTrackingCore _trackingCore;
-
-        private long _cycleIndex;
-        private bool _hasPreviousStateTimestamp;
-        private double _previousStateTimestampSeconds;
-
-        // 创建具有终点判定、轨迹偏离保护和曲率前馈预瞄的单车轨迹控制器。
-        public ParkingGeometricController(
-            IVehicleStateProvider stateProvider,
-            ILateralController lateralController,
-            ILongitudinalController longitudinalController,
-            GcpCommandAllocator gcpAllocator,
-            GcpCommandExecutor commandExecutor,
-            double finishDistanceMeters = 0.04,
-            double finishSpeedMetersPerSecond = 0.02,
-            double finishHeadingToleranceRadians =
-                3.0 * Math.PI / 180.0,
-            double maximumDistanceToTrajectoryMeters = 0.30,
-            double terminalBrakingPreviewMeters = 0.02,
-            double terminalApproachDistanceMeters = 0.10,
-            double terminalApproachGainPerSecond = 0.8,
-            double maximumTerminalApproachSpeedMetersPerSecond = 0.05,
-            double curvaturePreviewSeconds = 0.20,
-            double maximumCurvaturePreviewMeters = 0.12,
-            double motionDirectionInBodyRadians = 0.0)
-        {
-            _stateProvider = stateProvider ??
-                throw new ArgumentNullException(
-                    nameof(stateProvider));
-            _commandExecutor = commandExecutor ??
-                throw new ArgumentNullException(
-                    nameof(commandExecutor));
-            _trackingCore = new PathTrackingCore(
-                lateralController,
-                longitudinalController,
-                gcpAllocator,
-                finishDistanceMeters,
-                finishSpeedMetersPerSecond,
-                finishHeadingToleranceRadians,
-                maximumDistanceToTrajectoryMeters,
-                terminalBrakingPreviewMeters,
-                terminalApproachDistanceMeters,
-                terminalApproachGainPerSecond,
-                maximumTerminalApproachSpeedMetersPerSecond,
-                curvaturePreviewSeconds,
-                maximumCurvaturePreviewMeters,
-                motionDirectionInBodyRadians);
-        }
-
-        // 以下控制参数由公共轨迹核心统一持有。
-        public double FinishDistanceMeters =>
-            _trackingCore.FinishDistanceMeters;
-
-        public double FinishSpeedMetersPerSecond =>
-            _trackingCore.FinishSpeedMetersPerSecond;
-
-        public double FinishHeadingToleranceRadians =>
-            _trackingCore.FinishHeadingToleranceRadians;
-
-        public double MaximumDistanceToTrajectoryMeters =>
-            _trackingCore.MaximumDistanceToTrajectoryMeters;
-
-        public double TerminalBrakingPreviewMeters =>
-            _trackingCore.TerminalBrakingPreviewMeters;
-
-        public double TerminalApproachDistanceMeters =>
-            _trackingCore.TerminalApproachDistanceMeters;
-
-        public double TerminalApproachGainPerSecond =>
-            _trackingCore.TerminalApproachGainPerSecond;
-
-        public double MaximumTerminalApproachSpeedMetersPerSecond =>
-            _trackingCore.MaximumTerminalApproachSpeedMetersPerSecond;
-
-        public double CurvaturePreviewSeconds =>
-            _trackingCore.CurvaturePreviewSeconds;
-
-        public double MaximumCurvaturePreviewMeters =>
-            _trackingCore.MaximumCurvaturePreviewMeters;
-
-        public bool IsActive => _trackingCore.IsActive;
-
-        public bool IsCompleted => _trackingCore.IsCompleted;
-
-        public string LastFailureReason =>
-            _trackingCore.LastFailureReason;
-
-        public Exception LastException =>
-            _trackingCore.LastException;
-
-        // 最近一次有效车辆状态仍由单车外层保存。
-        public VehicleState? LastVehicleState { get; private set; }
-
-        public TrajectoryProjection? LastProjection =>
-            _trackingCore.LastProjection;
-
-        public GcpMotionCommand? LastRequestedCommand =>
-            _trackingCore.LastRequestedCommand;
-
-        // 经过GCP角速度限制后实际发送给底盘的最近一次命令。
-        public GcpMotionCommand? LastCommand { get; private set; }
-
-        public double? LastControlReferenceSpeedMetersPerSecond =>
-            _trackingCore.LastControlReferenceSpeedMetersPerSecond;
-
-        public double? LastCurvaturePreviewDistanceMeters =>
-            _trackingCore.LastCurvaturePreviewDistanceMeters;
-
-        public double? LastFeedforwardCurvaturePerMeter =>
-            _trackingCore.LastFeedforwardCurvaturePerMeter;
-
-        public ParkingControlCycleTiming? LastCycleTiming { get; private set; }
-
-        // 先停车，再从轨迹起点重置公共控制核心和单车执行诊断。
-        public void Start(Trajectory2D trajectory)
-        {
-            _commandExecutor.Stop();
-            _trackingCore.Start(trajectory);
-            ClearExecutionDiagnostics();
-        }
-
-        // 读取车辆状态、调用公共核心并发送一次真实底盘命令。
-        public ParkingControlCycleResult ExecuteCycle(
-            double deltaTimeSeconds)
-        {
-            NumericGuard.EnsureFinitePositive(
-                deltaTimeSeconds,
-                nameof(deltaTimeSeconds));
-
-            if (!_trackingCore.IsActive)
-            {
-                return ParkingControlCycleResult.Inactive;
-            }
-
-            var cycleIndex = ++_cycleIndex;
-            var cycleStartTimestamp =
-                Stopwatch.GetTimestamp();
-            var stateReadMilliseconds = 0.0;
-            var projectionMilliseconds = 0.0;
-            var controllerComputeMilliseconds = 0.0;
-            var commandSendMilliseconds = 0.0;
-            var hasStateTimestamp = false;
-            var stateTimestampSeconds = 0.0;
-            var stateTimestampChanged = false;
-            var cycleResult =
-                ParkingControlCycleResult.Faulted;
-
-            try
-            {
-                var stateReadStartTimestamp =
-                    Stopwatch.GetTimestamp();
-                bool stateAvailable;
-                VehicleState vehicleState;
-                try
-                {
-                    stateAvailable =
-                        _stateProvider.TryGetState(
-                            out vehicleState);
-                }
-                finally
-                {
-                    stateReadMilliseconds =
-                        GetElapsedMilliseconds(
-                            stateReadStartTimestamp);
-                }
-
-                if (!stateAvailable)
-                {
-                    var stopStartTimestamp =
-                        Stopwatch.GetTimestamp();
-                    try
-                    {
-                        StopForUnavailableState();
-                    }
-                    finally
-                    {
-                        commandSendMilliseconds =
-                            GetElapsedMilliseconds(
-                                stopStartTimestamp);
-                    }
-
-                    cycleResult = ParkingControlCycleResult
-                        .StateUnavailable;
-                    return cycleResult;
-                }
-
-                LastVehicleState = vehicleState;
-                hasStateTimestamp = true;
-                stateTimestampSeconds =
-                    vehicleState.SampleTimestampSeconds;
-                stateTimestampChanged =
-                    !_hasPreviousStateTimestamp ||
-                    stateTimestampSeconds !=
-                    _previousStateTimestampSeconds;
-                _previousStateTimestampSeconds =
-                    stateTimestampSeconds;
-                _hasPreviousStateTimestamp = true;
-
-                var output = _trackingCore.Compute(
-                    vehicleState.PoseInWorld,
-                    vehicleState.TwistInBody,
-                    vehicleState.HasValidVelocityEstimate,
-                    deltaTimeSeconds);
-                projectionMilliseconds =
-                    output.ProjectionMilliseconds;
-                controllerComputeMilliseconds =
-                    output.ControllerComputeMilliseconds;
-
-                if (output.Result ==
-                    PathTrackingCycleResult.Completed)
-                {
-                    commandSendMilliseconds =
-                        StopForCompletedTrajectory();
-                    cycleResult =
-                        ParkingControlCycleResult.Completed;
-                    return cycleResult;
-                }
-
-                if (output.Result ==
-                    PathTrackingCycleResult.Faulted)
-                {
-                    commandSendMilliseconds =
-                        StopForTrackingFault();
-                    cycleResult =
-                        ParkingControlCycleResult.Faulted;
-                    return cycleResult;
-                }
-
-                if (output.Result !=
-                        PathTrackingCycleResult.CommandGenerated ||
-                    !output.Command.HasValue)
-                {
-                    cycleResult =
-                        ParkingControlCycleResult.Inactive;
-                    return cycleResult;
-                }
-
-                var commandSendStartTimestamp =
-                    Stopwatch.GetTimestamp();
-                bool commandSucceeded;
-                try
-                {
-                    commandSucceeded =
-                        _commandExecutor.Execute(
-                            output.Command.Value,
-                            deltaTimeSeconds);
-                }
-                finally
-                {
-                    commandSendMilliseconds =
-                        GetElapsedMilliseconds(
-                            commandSendStartTimestamp);
-                }
-
-                if (!commandSucceeded)
-                {
-                    cycleResult = EnterFault(
-                        string.IsNullOrWhiteSpace(
-                            _commandExecutor.LastFailureReason)
-                            ? "GCP底盘命令执行失败。"
-                            : _commandExecutor.LastFailureReason);
-                    return cycleResult;
-                }
-
-                LastCommand =
-                    _commandExecutor.LastSentCommand;
-                cycleResult =
-                    ParkingControlCycleResult.CommandSent;
-                return cycleResult;
-            }
-            catch (Exception exception)
-            {
-                cycleResult = EnterFault(
-                    "停车机器人轨迹控制周期异常：" +
-                    exception.Message,
-                    exception);
-                return cycleResult;
-            }
-            finally
-            {
-                LastCycleTiming =
-                    new ParkingControlCycleTiming(
-                        cycleIndex,
-                        deltaTimeSeconds * 1000.0,
-                        stateReadMilliseconds,
-                        projectionMilliseconds,
-                        controllerComputeMilliseconds,
-                        commandSendMilliseconds,
-                        GetElapsedMilliseconds(
-                            cycleStartTimestamp),
-                        hasStateTimestamp,
-                        stateTimestampSeconds,
-                        stateTimestampChanged,
-                        cycleResult);
-            }
-        }
-
-        // 主动取消当前轨迹、立即停车并清除全部控制状态。
-        public void Cancel()
-        {
-            _commandExecutor.Stop();
-            _trackingCore.Cancel();
-            ClearExecutionDiagnostics();
-        }
-
-        // 状态不可用时停车并重置反馈历史，同时保留轨迹等待恢复。
-        private void StopForUnavailableState()
-        {
-            _commandExecutor.Stop();
-            _trackingCore.PauseForUnavailableState(
-                "当前无法获得有效车辆状态，底盘已停车并等待定位恢复。");
-            LastCommand = null;
-        }
-
-        // 公共核心完成轨迹后发送停车，并保留零命令供实验记录。
-        private double StopForCompletedTrajectory()
-        {
-            var startTimestamp = Stopwatch.GetTimestamp();
-            _commandExecutor.Stop();
-            LastCommand = new GcpMotionCommand(
-                0.0,
-                0.0,
-                0.0);
-            return GetElapsedMilliseconds(startTimestamp);
-        }
-
-        // 公共核心故障后只负责真实底盘停车，不覆盖核心保存的失败原因。
-        private double StopForTrackingFault()
-        {
-            var startTimestamp = Stopwatch.GetTimestamp();
-            _commandExecutor.Stop();
-            LastCommand = null;
-            return GetElapsedMilliseconds(startTimestamp);
-        }
-
-        // 将底盘执行或外层异常同步到公共核心，并立即停车。
-        private ParkingControlCycleResult EnterFault(
-            string reason,
-            Exception exception = null)
-        {
-            _commandExecutor.Stop();
-            _trackingCore.Fail(reason, exception);
-            LastCommand = null;
-            return ParkingControlCycleResult.Faulted;
-        }
-
-        // 清除只属于单车状态读取、发送和周期计时的诊断。
-        private void ClearExecutionDiagnostics()
-        {
-            _cycleIndex = 0;
-            _hasPreviousStateTimestamp = false;
-            _previousStateTimestampSeconds = 0.0;
-            LastVehicleState = null;
-            LastCommand = null;
-            LastCycleTiming = null;
-        }
-
-        private static double GetElapsedMilliseconds(
-            long startTimestamp)
-        {
-            return (Stopwatch.GetTimestamp() - startTimestamp) *
-                   1000.0 /
-                   Stopwatch.Frequency;
-        }
-    }
-}

MultiWheelC/Control/Execution/PathTrackingCore.cs

@@ -1,793 +0,0 @@
-using System;
-using System.Diagnostics;
-using MultiWheelC.Control.Abstractions;
-using MultiWheelC.Control.Allocation;
-using MultiWheelC.Trajectory;
-using MyParking.Shared;
-
-namespace MultiWheelC.Control.Execution
-{
-    // 表示公共轨迹跟踪核心单周期的计算结果，不包含底盘发送结果。
-    public enum PathTrackingCycleResult
-    {
-        Inactive = 0,
-        CommandGenerated = 1,
-        Completed = 2,
-        Faulted = 3
-    }
-
-    // 保存公共核心生成的GCP命令、轨迹投影和分阶段计算耗时。
-    public readonly struct PathTrackingCycleOutput
-    {
-        public PathTrackingCycleOutput(
-            PathTrackingCycleResult result,
-            GcpMotionCommand? command,
-            TrajectoryProjection? projection,
-            double projectionMilliseconds,
-            double controllerComputeMilliseconds)
-        {
-            Result = result;
-            Command = command;
-            Projection = projection;
-            ProjectionMilliseconds = projectionMilliseconds;
-            ControllerComputeMilliseconds =
-                controllerComputeMilliseconds;
-        }
-
-        public PathTrackingCycleResult Result { get; }
-
-        public GcpMotionCommand? Command { get; }
-
-        public TrajectoryProjection? Projection { get; }
-
-        public double ProjectionMilliseconds { get; }
-
-        public double ControllerComputeMilliseconds { get; }
-    }
-
-    // 统一处理单车和虚拟车队共有的轨迹投影、速度整形及GCP命令生成。
-    public sealed class PathTrackingCore
-    {
-        private const double ZeroReferenceSpeedToleranceMetersPerSecond =
-            1e-6;
-        private const double StartupRegionMeters = 0.02;
-        private const double StartupPreviewDistanceMeters = 0.05;
-        private const double MaximumStartupSpeedMetersPerSecond = 0.08;
-        private const double ProjectionBackwardSearchDistanceMeters =
-            0.10;
-        private const double ProjectionForwardSearchDistanceMeters =
-            1.00;
-
-        private readonly ILateralController _lateralController;
-        private readonly ILongitudinalController _longitudinalController;
-        private readonly GcpCommandAllocator _gcpAllocator;
-        private readonly double _motionDirectionInBodyRadians;
-
-        private Trajectory2D _trajectory;
-        private double _terminalTravelDirection = 1.0;
-
-        public PathTrackingCore(
-            ILateralController lateralController,
-            ILongitudinalController longitudinalController,
-            GcpCommandAllocator gcpAllocator,
-            double finishDistanceMeters = 0.04,
-            double finishSpeedMetersPerSecond = 0.02,
-            double finishHeadingToleranceRadians =
-                3.0 * Math.PI / 180.0,
-            double maximumDistanceToTrajectoryMeters = 0.30,
-            double terminalBrakingPreviewMeters = 0.02,
-            double terminalApproachDistanceMeters = 0.10,
-            double terminalApproachGainPerSecond = 0.8,
-            double maximumTerminalApproachSpeedMetersPerSecond = 0.05,
-            double curvaturePreviewSeconds = 0.20,
-            double maximumCurvaturePreviewMeters = 0.12,
-            double motionDirectionInBodyRadians = 0.0)
-        {
-            _lateralController = lateralController ??
-                throw new ArgumentNullException(
-                    nameof(lateralController));
-            _longitudinalController = longitudinalController ??
-                throw new ArgumentNullException(
-                    nameof(longitudinalController));
-            _gcpAllocator = gcpAllocator ??
-                throw new ArgumentNullException(
-                    nameof(gcpAllocator));
-
-            NumericGuard.EnsureFinitePositive(
-                finishDistanceMeters,
-                nameof(finishDistanceMeters));
-            NumericGuard.EnsureFiniteNonNegative(
-                finishSpeedMetersPerSecond,
-                nameof(finishSpeedMetersPerSecond));
-            NumericGuard.EnsureFinitePositive(
-                finishHeadingToleranceRadians,
-                nameof(finishHeadingToleranceRadians));
-            NumericGuard.EnsureFinitePositive(
-                maximumDistanceToTrajectoryMeters,
-                nameof(maximumDistanceToTrajectoryMeters));
-            NumericGuard.EnsureFiniteNonNegative(
-                terminalBrakingPreviewMeters,
-                nameof(terminalBrakingPreviewMeters));
-            NumericGuard.EnsureFinitePositive(
-                terminalApproachDistanceMeters,
-                nameof(terminalApproachDistanceMeters));
-            NumericGuard.EnsureFinitePositive(
-                terminalApproachGainPerSecond,
-                nameof(terminalApproachGainPerSecond));
-            NumericGuard.EnsureFinitePositive(
-                maximumTerminalApproachSpeedMetersPerSecond,
-                nameof(maximumTerminalApproachSpeedMetersPerSecond));
-            NumericGuard.EnsureFiniteNonNegative(
-                curvaturePreviewSeconds,
-                nameof(curvaturePreviewSeconds));
-            NumericGuard.EnsureFiniteNonNegative(
-                maximumCurvaturePreviewMeters,
-                nameof(maximumCurvaturePreviewMeters));
-            NumericGuard.EnsureFinite(
-                motionDirectionInBodyRadians,
-                nameof(motionDirectionInBodyRadians));
-
-            if (terminalApproachDistanceMeters <=
-                finishDistanceMeters)
-            {
-                throw new ArgumentOutOfRangeException(
-                    nameof(terminalApproachDistanceMeters),
-                    "终点单向逼近范围必须大于终点位置容差。");
-            }
-
-            FinishDistanceMeters = finishDistanceMeters;
-            FinishSpeedMetersPerSecond =
-                finishSpeedMetersPerSecond;
-            FinishHeadingToleranceRadians =
-                finishHeadingToleranceRadians;
-            MaximumDistanceToTrajectoryMeters =
-                maximumDistanceToTrajectoryMeters;
-            TerminalBrakingPreviewMeters =
-                terminalBrakingPreviewMeters;
-            TerminalApproachDistanceMeters =
-                terminalApproachDistanceMeters;
-            TerminalApproachGainPerSecond =
-                terminalApproachGainPerSecond;
-            MaximumTerminalApproachSpeedMetersPerSecond =
-                maximumTerminalApproachSpeedMetersPerSecond;
-            CurvaturePreviewSeconds = curvaturePreviewSeconds;
-            MaximumCurvaturePreviewMeters =
-                maximumCurvaturePreviewMeters;
-            _motionDirectionInBodyRadians =
-                AngleMath.NormalizeRadians(
-                    motionDirectionInBodyRadians);
-        }
-
-        public double FinishDistanceMeters { get; }
-
-        public double FinishSpeedMetersPerSecond { get; }
-
-        public double FinishHeadingToleranceRadians { get; }
-
-        public double MaximumDistanceToTrajectoryMeters { get; }
-
-        public double TerminalBrakingPreviewMeters { get; }
-
-        public double TerminalApproachDistanceMeters { get; }
-
-        public double TerminalApproachGainPerSecond { get; }
-
-        public double MaximumTerminalApproachSpeedMetersPerSecond { get; }
-
-        public double CurvaturePreviewSeconds { get; }
-
-        public double MaximumCurvaturePreviewMeters { get; }
-
-        public bool IsActive { get; private set; }
-
-        public bool IsCompleted { get; private set; }
-
-        public string LastFailureReason { get; private set; } =
-            string.Empty;
-
-        public Exception LastException { get; private set; }
-
-        public TrajectoryProjection? LastProjection { get; private set; }
-
-        public GcpMotionCommand? LastRequestedCommand { get; private set; }
-
-        public double? LastControlReferenceSpeedMetersPerSecond { get; private set; }
-
-        public double? LastCurvaturePreviewDistanceMeters { get; private set; }
-
-        public double? LastFeedforwardCurvaturePerMeter { get; private set; }
-
-        // 重置跨周期状态，并从轨迹起点开始新的跟踪过程。
-        public void Start(Trajectory2D trajectory)
-        {
-            if (trajectory == null)
-            {
-                throw new ArgumentNullException(
-                    nameof(trajectory));
-            }
-
-            var terminalTravelDirection =
-                ResolveTerminalTravelDirection(trajectory);
-
-            ResetFeedbackControllers();
-            _trajectory = trajectory;
-            _terminalTravelDirection =
-                terminalTravelDirection;
-            IsActive = true;
-            IsCompleted = false;
-            ClearDiagnostics();
-        }
-
-        // 将受控刚体的位姿和速度转换为本周期GCP命令。
-        public PathTrackingCycleOutput Compute(
-            Pose2D poseInWorld,
-            Twist2D actualTwistInBody,
-            bool hasValidVelocityEstimate,
-            double deltaTimeSeconds)
-        {
-            NumericGuard.EnsureFinite(
-                poseInWorld,
-                nameof(poseInWorld));
-            NumericGuard.EnsureFinite(
-                actualTwistInBody,
-                nameof(actualTwistInBody));
-            NumericGuard.EnsureFinitePositive(
-                deltaTimeSeconds,
-                nameof(deltaTimeSeconds));
-
-            if (!IsActive || _trajectory == null)
-            {
-                return new PathTrackingCycleOutput(
-                    PathTrackingCycleResult.Inactive,
-                    null,
-                    LastProjection,
-                    0.0,
-                    0.0);
-            }
-
-            var projectionStartTimestamp =
-                Stopwatch.GetTimestamp();
-            var projectionCompleted = false;
-            var projectionMilliseconds = 0.0;
-            var controllerComputeStartTimestamp = 0L;
-
-            try
-            {
-                var projection = LastProjection.HasValue
-                    ? TrajectoryProjector.Project(
-                        _trajectory,
-                        poseInWorld,
-                        LastProjection.Value.ArcLengthMeters,
-                        ProjectionBackwardSearchDistanceMeters,
-                        ProjectionForwardSearchDistanceMeters)
-                    : TrajectoryProjector.Project(
-                        _trajectory,
-                        poseInWorld);
-
-                projectionMilliseconds =
-                    GetElapsedMilliseconds(
-                        projectionStartTimestamp);
-                projectionCompleted = true;
-                LastProjection = projection;
-                controllerComputeStartTimestamp =
-                    Stopwatch.GetTimestamp();
-
-                if (projection.DistanceToTrajectoryMeters >
-                    MaximumDistanceToTrajectoryMeters)
-                {
-                    Fail(
-                        "受控刚体距离参考轨迹" +
-                        $"{projection.DistanceToTrajectoryMeters:F3}m，" +
-                        "超过允许值" +
-                        $"{MaximumDistanceToTrajectoryMeters:F3}m。");
-                    return CreateOutput(
-                        PathTrackingCycleResult.Faulted,
-                        null,
-                        projection,
-                        projectionMilliseconds,
-                        controllerComputeStartTimestamp);
-                }
-
-                if (HasReachedEnd(
-                        poseInWorld,
-                        actualTwistInBody,
-                        hasValidVelocityEstimate,
-                        projection))
-                {
-                    CompleteTrajectory();
-                    return CreateOutput(
-                        PathTrackingCycleResult.Completed,
-                        null,
-                        projection,
-                        projectionMilliseconds,
-                        controllerComputeStartTimestamp);
-                }
-
-                if (HasStoppedAtUnsatisfiedTerminal(
-                        poseInWorld,
-                        actualTwistInBody,
-                        hasValidVelocityEstimate,
-                        projection,
-                        out var terminalFailureReason))
-                {
-                    Fail(terminalFailureReason);
-                    return CreateOutput(
-                        PathTrackingCycleResult.Faulted,
-                        null,
-                        projection,
-                        projectionMilliseconds,
-                        controllerComputeStartTimestamp);
-                }
-
-                var controlReferenceSpeedMetersPerSecond =
-                    ResolveControlReferenceSpeed(
-                        poseInWorld,
-                        projection);
-                LastControlReferenceSpeedMetersPerSecond =
-                    controlReferenceSpeedMetersPerSecond;
-                var curvaturePreviewDistanceMeters =
-                    ResolveCurvaturePreviewDistanceMeters(
-                        actualTwistInBody,
-                        hasValidVelocityEstimate,
-                        controlReferenceSpeedMetersPerSecond);
-                LastCurvaturePreviewDistanceMeters =
-                    curvaturePreviewDistanceMeters;
-                var feedforwardCurvaturePerMeter =
-                    ResolveFeedforwardCurvaturePerMeter(
-                        projection,
-                        curvaturePreviewDistanceMeters);
-                LastFeedforwardCurvaturePerMeter =
-                    feedforwardCurvaturePerMeter;
-                var context = new PathTrackingContext(
-                    actualTwistInBody,
-                    hasValidVelocityEstimate,
-                    projection,
-                    controlReferenceSpeedMetersPerSecond,
-                    feedforwardCurvaturePerMeter,
-                    deltaTimeSeconds,
-                    _motionDirectionInBodyRadians);
-                var lateralCommand =
-                    _lateralController.Compute(context);
-                var commandSpeedMetersPerSecond =
-                    _longitudinalController
-                        .ComputeSpeedMetersPerSecond(context);
-                var command = _gcpAllocator.Allocate(
-                    commandSpeedMetersPerSecond,
-                    lateralCommand);
-
-                LastRequestedCommand = command;
-                LastFailureReason = string.Empty;
-                LastException = null;
-
-                return CreateOutput(
-                    PathTrackingCycleResult.CommandGenerated,
-                    command,
-                    projection,
-                    projectionMilliseconds,
-                    controllerComputeStartTimestamp);
-            }
-            catch (Exception exception)
-            {
-                if (!projectionCompleted)
-                {
-                    projectionMilliseconds =
-                        GetElapsedMilliseconds(
-                            projectionStartTimestamp);
-                }
-
-                Fail(
-                    "轨迹跟踪核心计算异常：" +
-                    exception.Message,
-                    exception);
-
-                return CreateOutput(
-                    PathTrackingCycleResult.Faulted,
-                    null,
-                    LastProjection,
-                    projectionMilliseconds,
-                    controllerComputeStartTimestamp);
-            }
-        }
-
-        // 状态暂不可用时重置反馈历史，但保留当前轨迹和投影进度等待恢复。
-        public void PauseForUnavailableState(string reason)
-        {
-            ResetFeedbackControllers();
-            LastRequestedCommand = null;
-            LastFailureReason = reason ?? string.Empty;
-            LastException = null;
-        }
-
-        // 将外层执行故障同步到公共核心，并终止当前轨迹。
-        public void Fail(
-            string reason,
-            Exception exception = null)
-        {
-            ResetFeedbackControllers();
-            _trajectory = null;
-            IsActive = false;
-            IsCompleted = false;
-            LastRequestedCommand = null;
-            LastFailureReason = reason ?? string.Empty;
-            LastException = exception;
-        }
-
-        // 取消当前轨迹并清除全部跟踪状态。
-        public void Cancel()
-        {
-            ResetFeedbackControllers();
-            _trajectory = null;
-            _terminalTravelDirection = 1.0;
-            IsActive = false;
-            IsCompleted = false;
-            ClearDiagnostics();
-        }
-
-        private double ResolveControlReferenceSpeed(
-            Pose2D poseInWorld,
-            TrajectoryProjection projection)
-        {
-            if (projection.RemainingDistanceMeters >
-                TerminalApproachDistanceMeters)
-            {
-                return ResolveReferenceSpeedForControl(
-                    projection);
-            }
-
-            return ResolveTerminalApproachSpeed(
-                poseInWorld);
-        }
-
-        private double ResolveCurvaturePreviewDistanceMeters(
-            Twist2D actualTwistInBody,
-            bool hasValidVelocityEstimate,
-            double controlReferenceSpeedMetersPerSecond)
-        {
-            if (CurvaturePreviewSeconds <= 0.0 ||
-                MaximumCurvaturePreviewMeters <= 0.0)
-            {
-                return 0.0;
-            }
-
-            var previewSpeedMetersPerSecond =
-                hasValidVelocityEstimate
-                    ? CalculateActualLongitudinalSpeedMetersPerSecond(
-                        actualTwistInBody)
-                    : Math.Abs(
-                        controlReferenceSpeedMetersPerSecond);
-
-            return Math.Min(
-                MaximumCurvaturePreviewMeters,
-                previewSpeedMetersPerSecond *
-                CurvaturePreviewSeconds);
-        }
-
-        private double ResolveFeedforwardCurvaturePerMeter(
-            TrajectoryProjection projection,
-            double previewDistanceMeters)
-        {
-            var previewArcLengthMeters = Math.Min(
-                _trajectory.TotalLengthMeters,
-                projection.ArcLengthMeters +
-                previewDistanceMeters);
-
-            return _trajectory
-                .SampleAtArcLength(previewArcLengthMeters)
-                .CurvaturePerMeter;
-        }
-
-        private double ResolveTerminalApproachSpeed(
-            Pose2D poseInWorld)
-        {
-            var distanceToEndMeters =
-                CalculateDistanceToEndMeters(
-                    poseInWorld);
-            var headingErrorToEndRadians =
-                CalculateHeadingErrorToEndRadians(
-                    poseInWorld);
-
-            if (distanceToEndMeters <=
-                    FinishDistanceMeters &&
-                headingErrorToEndRadians <=
-                    FinishHeadingToleranceRadians)
-            {
-                return 0.0;
-            }
-
-            var endPose = _trajectory.EndPoint.PoseInWorld;
-            var deltaX = endPose.XMeters -
-                         poseInWorld.XMeters;
-            var deltaY = endPose.YMeters -
-                         poseInWorld.YMeters;
-            var longitudinalErrorMeters =
-                deltaX * Math.Cos(endPose.YawRadians) +
-                deltaY * Math.Sin(endPose.YawRadians);
-            var remainingAlongTravelMeters =
-                _terminalTravelDirection *
-                longitudinalErrorMeters;
-
-            // 越过终点后不生成与原轨迹方向相反的修正速度。
-            if (remainingAlongTravelMeters <= 0.0)
-            {
-                return 0.0;
-            }
-
-            var speedMagnitudeMetersPerSecond =
-                Math.Min(
-                    MaximumTerminalApproachSpeedMetersPerSecond,
-                    TerminalApproachGainPerSecond *
-                    remainingAlongTravelMeters);
-
-            return _terminalTravelDirection *
-                   speedMagnitudeMetersPerSecond;
-        }
-
-        private double ResolveReferenceSpeedForControl(
-            TrajectoryProjection projection)
-        {
-            var currentReferenceSpeed =
-                ApplyTerminalBrakingPreview(
-                    projection,
-                    projection.ReferencePoint
-                        .ReferenceSpeedMetersPerSecond);
-            var isInStartupRegion =
-                projection.ArcLengthMeters <=
-                    StartupRegionMeters &&
-                projection.RemainingDistanceMeters >
-                    FinishDistanceMeters;
-
-            if (!isInStartupRegion)
-            {
-                return currentReferenceSpeed;
-            }
-
-            var previewArcLengthMeters = Math.Min(
-                _trajectory.TotalLengthMeters,
-                projection.ArcLengthMeters +
-                StartupPreviewDistanceMeters);
-            var previewReferenceSpeed =
-                _trajectory
-                    .SampleAtArcLength(previewArcLengthMeters)
-                    .ReferenceSpeedMetersPerSecond;
-
-            if (Math.Abs(previewReferenceSpeed) <=
-                ZeroReferenceSpeedToleranceMetersPerSecond)
-            {
-                return currentReferenceSpeed;
-            }
-
-            var startupReleaseSpeed =
-                Math.Sign(previewReferenceSpeed) *
-                Math.Min(
-                    Math.Abs(previewReferenceSpeed),
-                    MaximumStartupSpeedMetersPerSecond);
-
-            if (Math.Sign(currentReferenceSpeed) ==
-                    Math.Sign(startupReleaseSpeed) &&
-                Math.Abs(currentReferenceSpeed) >=
-                    Math.Abs(startupReleaseSpeed))
-            {
-                return currentReferenceSpeed;
-            }
-
-            return startupReleaseSpeed;
-        }
-
-        private double ApplyTerminalBrakingPreview(
-            TrajectoryProjection projection,
-            double currentReferenceSpeed)
-        {
-            if (TerminalBrakingPreviewMeters <= 0.0)
-            {
-                return currentReferenceSpeed;
-            }
-
-            var previewArcLengthMeters = Math.Min(
-                _trajectory.TotalLengthMeters,
-                projection.ArcLengthMeters +
-                TerminalBrakingPreviewMeters);
-            var previewReferenceSpeed =
-                _trajectory
-                    .SampleAtArcLength(previewArcLengthMeters)
-                    .ReferenceSpeedMetersPerSecond;
-            var previewIsStop =
-                Math.Abs(previewReferenceSpeed) <=
-                ZeroReferenceSpeedToleranceMetersPerSecond;
-            var hasSameDirection =
-                Math.Sign(previewReferenceSpeed) ==
-                Math.Sign(currentReferenceSpeed);
-            var previewIsSlower =
-                Math.Abs(previewReferenceSpeed) <
-                Math.Abs(currentReferenceSpeed);
-
-            if (previewIsSlower &&
-                (previewIsStop || hasSameDirection))
-            {
-                return previewReferenceSpeed;
-            }
-
-            return currentReferenceSpeed;
-        }
-
-        private static double ResolveTerminalTravelDirection(
-            Trajectory2D trajectory)
-        {
-            for (var index = trajectory.Count - 1;
-                 index >= 0;
-                 index--)
-            {
-                var referenceSpeedMetersPerSecond =
-                    trajectory[index]
-                        .ReferenceSpeedMetersPerSecond;
-
-                if (Math.Abs(referenceSpeedMetersPerSecond) >
-                    ZeroReferenceSpeedToleranceMetersPerSecond)
-                {
-                    return Math.Sign(
-                        referenceSpeedMetersPerSecond);
-                }
-            }
-
-            throw new ArgumentException(
-                "轨迹必须在终点前包含至少一个非零参考速度。",
-                nameof(trajectory));
-        }
-
-        private bool HasReachedEnd(
-            Pose2D poseInWorld,
-            Twist2D actualTwistInBody,
-            bool hasValidVelocityEstimate,
-            TrajectoryProjection projection)
-        {
-            if (!hasValidVelocityEstimate)
-            {
-                return false;
-            }
-
-            return projection.RemainingDistanceMeters <=
-                       FinishDistanceMeters &&
-                   CalculateDistanceToEndMeters(poseInWorld) <=
-                       FinishDistanceMeters &&
-                   CalculateHeadingErrorToEndRadians(poseInWorld) <=
-                       FinishHeadingToleranceRadians &&
-                   CalculateActualLongitudinalSpeedMetersPerSecond(
-                       actualTwistInBody) <=
-                       FinishSpeedMetersPerSecond;
-        }
-
-        private bool HasStoppedAtUnsatisfiedTerminal(
-            Pose2D poseInWorld,
-            Twist2D actualTwistInBody,
-            bool hasValidVelocityEstimate,
-            TrajectoryProjection projection,
-            out string failureReason)
-        {
-            failureReason = string.Empty;
-
-            var isTerminalZeroSpeedReference =
-                projection.RemainingDistanceMeters <=
-                    FinishDistanceMeters &&
-                Math.Abs(
-                    projection.ReferencePoint
-                        .ReferenceSpeedMetersPerSecond) <=
-                    ZeroReferenceSpeedToleranceMetersPerSecond;
-
-            if (!isTerminalZeroSpeedReference ||
-                !hasValidVelocityEstimate ||
-                CalculateActualLongitudinalSpeedMetersPerSecond(
-                    actualTwistInBody) >
-                FinishSpeedMetersPerSecond)
-            {
-                return false;
-            }
-
-            var positionErrorMeters =
-                CalculateDistanceToEndMeters(
-                    poseInWorld);
-            var headingErrorRadians =
-                CalculateHeadingErrorToEndRadians(
-                    poseInWorld);
-
-            failureReason =
-                "受控刚体已在终点零速参考处停稳，但终点精度不满足要求：" +
-                $"位置误差={positionErrorMeters:F3}m，" +
-                "航向误差=" +
-                $"{AngleMath.RadiansToDegrees(headingErrorRadians):F2}°。";
-            return true;
-        }
-
-        private double CalculateDistanceToEndMeters(
-            Pose2D poseInWorld)
-        {
-            var endPose = _trajectory.EndPoint.PoseInWorld;
-            var deltaX = poseInWorld.XMeters -
-                         endPose.XMeters;
-            var deltaY = poseInWorld.YMeters -
-                         endPose.YMeters;
-
-            return Math.Sqrt(
-                deltaX * deltaX +
-                deltaY * deltaY);
-        }
-
-        private double CalculateHeadingErrorToEndRadians(
-            Pose2D poseInWorld)
-        {
-            return Math.Abs(
-                AngleMath.ShortestDifferenceRadians(
-                    _trajectory.EndPoint
-                        .PoseInWorld.YawRadians,
-                    poseInWorld.YawRadians));
-        }
-
-        private double CalculateActualLongitudinalSpeedMetersPerSecond(
-            Twist2D actualTwistInBody)
-        {
-            return Math.Abs(
-                Math.Cos(_motionDirectionInBodyRadians) *
-                    actualTwistInBody.VxMetersPerSecond +
-                Math.Sin(_motionDirectionInBodyRadians) *
-                    actualTwistInBody.VyMetersPerSecond);
-        }
-
-        private void CompleteTrajectory()
-        {
-            ResetFeedbackControllers();
-            _trajectory = null;
-            IsActive = false;
-            IsCompleted = true;
-            LastRequestedCommand = new GcpMotionCommand(
-                0.0,
-                0.0,
-                0.0);
-            LastFailureReason = string.Empty;
-            LastException = null;
-        }
-
-        private void ResetFeedbackControllers()
-        {
-            _lateralController.Reset();
-            _longitudinalController.Reset();
-        }
-
-        private void ClearDiagnostics()
-        {
-            LastProjection = null;
-            LastRequestedCommand = null;
-            LastControlReferenceSpeedMetersPerSecond = null;
-            LastCurvaturePreviewDistanceMeters = null;
-            LastFeedforwardCurvaturePerMeter = null;
-            LastFailureReason = string.Empty;
-            LastException = null;
-        }
-
-        private static PathTrackingCycleOutput CreateOutput(
-            PathTrackingCycleResult result,
-            GcpMotionCommand? command,
-            TrajectoryProjection? projection,
-            double projectionMilliseconds,
-            long controllerComputeStartTimestamp)
-        {
-            var controllerComputeMilliseconds =
-                controllerComputeStartTimestamp == 0L
-                    ? 0.0
-                    : GetElapsedMilliseconds(
-                        controllerComputeStartTimestamp);
-
-            return new PathTrackingCycleOutput(
-                result,
-                command,
-                projection,
-                projectionMilliseconds,
-                controllerComputeMilliseconds);
-        }
-
-        private static double GetElapsedMilliseconds(
-            long startTimestamp)
-        {
-            return (Stopwatch.GetTimestamp() - startTimestamp) *
-                   1000.0 /
-                   Stopwatch.Frequency;
-        }
-    }
-}

MultiWheelC/Control/Lateral/StanleyLateralController.cs

@@ -1,254 +0,0 @@
-using System;
-using MultiWheelC.Control.Abstractions;
-
-namespace MultiWheelC.Control.Lateral
-{
-    /// <summary>
-    /// 将参考曲率、横向误差和航向误差分别转换为前、后GCP目标转角。
-    /// </summary>
-    public sealed class StanleyLateralController : ILateralController
-    {
-        /// <summary>
-        /// 创建使用指定GCP几何、Stanley增益和转角保护参数的横向控制器。
-        /// </summary>
-        public StanleyLateralController(
-            double controlPointRadiusMeters,
-            double crossTrackGainPerSecond,
-            double headingErrorGain,
-            double minimumSpeedMetersPerSecond,
-            bool useActualSpeedForGain = true,
-            double maximumCrossTrackCorrectionRadians =
-                10.0 * Math.PI / 180.0,
-            double maximumHeadingCorrectionRadians =
-                10.0 * Math.PI / 180.0)
-        {
-            EnsureFinitePositive(
-                controlPointRadiusMeters,
-                nameof(controlPointRadiusMeters));
-            EnsureFiniteNonNegative(
-                crossTrackGainPerSecond,
-                nameof(crossTrackGainPerSecond));
-            EnsureFiniteNonNegative(
-                headingErrorGain,
-                nameof(headingErrorGain));
-            EnsureFinitePositive(
-                minimumSpeedMetersPerSecond,
-                nameof(minimumSpeedMetersPerSecond));
-            EnsureFinitePositive(
-                maximumCrossTrackCorrectionRadians,
-                nameof(maximumCrossTrackCorrectionRadians));
-            EnsureFinitePositive(
-                maximumHeadingCorrectionRadians,
-                nameof(maximumHeadingCorrectionRadians));
-
-            ControlPointRadiusMeters = controlPointRadiusMeters;
-            CrossTrackGainPerSecond = crossTrackGainPerSecond;
-            HeadingErrorGain = headingErrorGain;
-            MinimumSpeedMetersPerSecond = minimumSpeedMetersPerSecond;
-            UseActualSpeedForGain = useActualSpeedForGain;
-            MaximumCrossTrackCorrectionRadians =
-                maximumCrossTrackCorrectionRadians;
-            MaximumHeadingCorrectionRadians =
-                maximumHeadingCorrectionRadians;
-        }
-
-        /// <summary>
-        /// 获取车体中心到前、后GCP的距离，单位为m。
-        /// </summary>
-        public double ControlPointRadiusMeters { get; }
-
-        /// <summary>
-        /// 获取横向误差增益，单位为1/s。
-        /// </summary>
-        public double CrossTrackGainPerSecond { get; }
-
-        /// <summary>
-        /// 获取航向误差的无量纲增益。
-        /// </summary>
-        public double HeadingErrorGain { get; }
-
-        /// <summary>
-        /// 获取Stanley分母使用的最小速度绝对值，单位为m/s。
-        /// </summary>
-        public double MinimumSpeedMetersPerSecond { get; }
-
-        /// <summary>
-        /// 获取是否优先使用当前状态源提供的实际纵向速度计算横向修正。
-        /// </summary>
-        public bool UseActualSpeedForGain { get; }
-
-        /// <summary>
-        /// 获取横向误差共同转角分量的最大绝对值，单位为rad。
-        /// </summary>
-        public double MaximumCrossTrackCorrectionRadians { get; }
-
-        /// <summary>
-        /// 获取航向误差差动转角分量的最大绝对值，单位为rad。
-        /// </summary>
-        public double MaximumHeadingCorrectionRadians { get; }
-
-        /// <summary>
-        /// 分别计算横向共同转角以及曲率和航向差动转角，并生成前后GCP命令。
-        /// </summary>
-        public LateralControlCommand Compute(
-            PathTrackingContext context)
-        {
-            var speedForGain = SelectSpeedForGain(context);
-            var speedMagnitude = Math.Max(
-                Math.Abs(speedForGain),
-                MinimumSpeedMetersPerSecond);
-            var travelDirection = SelectTravelDirection(context);
-
-            // 参考曲率按轨迹点序的实际行进方向定义；倒车时底盘有符号
-            // 纵向速度反向，因此GCP曲率前馈也必须反向才能保持相同几何曲率。
-            var feedforwardAngleRadians =
-                travelDirection *
-                Math.Atan(
-                    context.FeedforwardCurvaturePerMeter *
-                    ControlPointRadiusMeters);
-
-            // 横向误差生成前后同向的共同转角，使四舵轮车辆平稳靠近轨迹。
-            var crossTrackCorrectionRadians =
-                ClampSymmetric(
-                    Math.Atan(
-                        CrossTrackGainPerSecond *
-                        context.LateralErrorMeters /
-                        speedMagnitude),
-                    MaximumCrossTrackCorrectionRadians);
-
-            // 航向误差生成前后反向的差动转角，只负责调整车身朝向。
-            var headingCorrectionRadians =
-                ClampSymmetric(
-                    HeadingErrorGain *
-                    context.HeadingErrorRadians,
-                    MaximumHeadingCorrectionRadians);
-
-            // 横向误差已经按轨迹执行点序定义；倒车轨迹的点序会自然
-            // 翻转横向轴，因此共同转角不能再按行驶方向重复反号。
-            var commonAngleRadians =
-                crossTrackCorrectionRadians;
-            var differentialAngleRadians =
-                feedforwardAngleRadians +
-                travelDirection *
-                headingCorrectionRadians;
-
-            return new LateralControlCommand(
-                commonAngleRadians +
-                differentialAngleRadians,
-                commonAngleRadians -
-                differentialAngleRadians);
-        }
-
-        /// <summary>
-        /// 清除横向控制器状态；当前Stanley实现没有跨周期状态。
-        /// </summary>
-        public void Reset()
-        {
-        }
-
-        /// <summary>
-        /// 选择Stanley横向误差项使用的实际速度或参考速度。
-        /// </summary>
-        private double SelectSpeedForGain(
-            PathTrackingContext context)
-        {
-            if (UseActualSpeedForGain &&
-                context.HasValidVelocityEstimate)
-            {
-                return context
-                    .ActualLongitudinalSpeedMetersPerSecond;
-            }
-
-            return context.ControlReferenceSpeedMetersPerSecond;
-        }
-
-        /// <summary>
-        /// 根据有符号参考速度确定前进或倒车时的反馈修正方向。
-        /// </summary>
-        private static double SelectTravelDirection(
-            PathTrackingContext context)
-        {
-            const double directionDeadbandMetersPerSecond = 1e-6;
-
-            if (Math.Abs(context.ControlReferenceSpeedMetersPerSecond) >
-                directionDeadbandMetersPerSecond)
-            {
-                return Math.Sign(
-                    context.ControlReferenceSpeedMetersPerSecond);
-            }
-
-            if (context.HasValidVelocityEstimate &&
-                Math.Abs(
-                    context.ActualLongitudinalSpeedMetersPerSecond) >
-                directionDeadbandMetersPerSecond)
-            {
-                return Math.Sign(
-                    context.ActualLongitudinalSpeedMetersPerSecond);
-            }
-
-            return 1.0;
-        }
-
-        /// <summary>
-        /// 将数值按正负对称方式限制在指定绝对值内。
-        /// </summary>
-        private static double ClampSymmetric(
-            double value,
-            double maximumAbsoluteValue)
-        {
-            return Math.Max(
-                -maximumAbsoluteValue,
-                Math.Min(maximumAbsoluteValue, value));
-        }
-
-        /// <summary>
-        /// 检查控制参数是否为正有限值。
-        /// </summary>
-        private static void EnsureFinitePositive(
-            double value,
-            string parameterName)
-        {
-            EnsureFinite(value, parameterName);
-
-            if (value <= 0.0)
-            {
-                throw new ArgumentOutOfRangeException(
-                    parameterName,
-                    "Stanley控制器的几何尺寸、速度和角度限制必须是正有限值。");
-            }
-        }
-
-        /// <summary>
-        /// 检查控制增益是否为非负有限值。
-        /// </summary>
-        private static void EnsureFiniteNonNegative(
-            double value,
-            string parameterName)
-        {
-            EnsureFinite(value, parameterName);
-
-            if (value < 0.0)
-            {
-                throw new ArgumentOutOfRangeException(
-                    parameterName,
-                    "Stanley控制增益必须是非负有限值。");
-            }
-        }
-
-        /// <summary>
-        /// 检查控制参数是否为有限值。
-        /// </summary>
-        private static void EnsureFinite(
-            double value,
-            string parameterName)
-        {
-            if (double.IsNaN(value) ||
-                double.IsInfinity(value))
-            {
-                throw new ArgumentOutOfRangeException(
-                    parameterName,
-                    "Stanley控制参数必须是有限值。");
-            }
-        }
-    }
-}

MultiWheelC/Control/Longitudinal/PidLongitudinalController.cs

@@ -1,224 +0,0 @@
-using System;
-using MultiWheelC.Control.Abstractions;
-using MultiWheelC.Control.Common;
-
-namespace MultiWheelC.Control.Longitudinal
-{
-    /// <summary>
-    /// 将轨迹参考速度前馈与通用PID速度反馈组合为有符号底盘命令速度。
-    /// </summary>
-    public sealed class PidLongitudinalController
-        : ILongitudinalController
-    {
-        private const double ReferenceStopDeadbandMetersPerSecond =
-            1e-6;
-
-        private readonly PidController _feedbackPid;
-
-        /// <summary>
-        /// 创建具有积分抗饱和和命令速度限幅的纵向速度外环。
-        /// </summary>
-        public PidLongitudinalController(
-            double proportionalGain,
-            double integralGainPerSecond,
-            double derivativeGainSeconds,
-            double maximumIntegralCorrectionMetersPerSecond,
-            double maximumCommandSpeedMetersPerSecond,
-            double speedErrorDeadbandMetersPerSecond = 0.025)
-        {
-            EnsureFinitePositive(
-                maximumCommandSpeedMetersPerSecond,
-                nameof(maximumCommandSpeedMetersPerSecond));
-            EnsureFiniteNonNegative(
-                speedErrorDeadbandMetersPerSecond,
-                nameof(speedErrorDeadbandMetersPerSecond));
-
-            _feedbackPid = new PidController(
-                proportionalGain,
-                integralGainPerSecond,
-                derivativeGainSeconds,
-                maximumIntegralCorrectionMetersPerSecond,
-                derivativeOnMeasurement: true);
-            MaximumCommandSpeedMetersPerSecond =
-                maximumCommandSpeedMetersPerSecond;
-            SpeedErrorDeadbandMetersPerSecond =
-                speedErrorDeadbandMetersPerSecond;
-        }
-
-        /// <summary>
-        /// 获取负责计算速度误差修正量的通用PID控制器。
-        /// </summary>
-        public PidController FeedbackPid => _feedbackPid;
-
-        /// <summary>
-        /// 获取底盘命令速度的最大绝对值，单位为m/s。
-        /// </summary>
-        public double MaximumCommandSpeedMetersPerSecond { get; }
-
-        /// <summary>
-        /// 获取不触发纵向PID修正的速度误差死区，单位为m/s。
-        /// </summary>
-        public double SpeedErrorDeadbandMetersPerSecond { get; }
-
-        /// <summary>
-        /// 获取最近一次有效控制周期的参考速度减实际速度，单位为m/s。
-        /// </summary>
-        public double LastSpeedErrorMetersPerSecond =>
-            _feedbackPid.LastError;
-
-        /// <summary>
-        /// 获取最近一次比例项产生的速度修正，单位为m/s。
-        /// </summary>
-        public double LastProportionalCorrectionMetersPerSecond =>
-            _feedbackPid.LastProportionalOutput;
-
-        /// <summary>
-        /// 获取最近一次积分项产生的速度修正，单位为m/s。
-        /// </summary>
-        public double LastIntegralCorrectionMetersPerSecond =>
-            _feedbackPid.LastIntegralOutput;
-
-        /// <summary>
-        /// 获取最近一次微分项产生的速度修正，单位为m/s。
-        /// </summary>
-        public double LastDerivativeCorrectionMetersPerSecond =>
-            _feedbackPid.LastDerivativeOutput;
-
-        /// <summary>
-        /// 根据本周期控制参考速度和实际纵向速度计算底盘命令速度。
-        /// </summary>
-        public double ComputeSpeedMetersPerSecond(
-            PathTrackingContext context)
-        {
-            var controlReferenceSpeedMetersPerSecond =
-                context.ControlReferenceSpeedMetersPerSecond;
-
-            // 轨迹明确要求停车时直接输出零，防止速度反馈使车辆在终点反向纠偏。
-            if (Math.Abs(controlReferenceSpeedMetersPerSecond) <=
-                ReferenceStopDeadbandMetersPerSecond)
-            {
-                Reset();
-                return 0.0;
-            }
-
-            // 定位速度尚不可用时只透传参考速度，不使用无效反馈更新PID状态。
-            if (!context.HasValidVelocityEstimate)
-            {
-                Reset();
-                return LimitReferenceSpeed(
-                    controlReferenceSpeedMetersPerSecond);
-            }
-
-            var speedErrorMetersPerSecond =
-                controlReferenceSpeedMetersPerSecond -
-                context.ActualLongitudinalSpeedMetersPerSecond;
-
-            // Detour差分速度在参考速度附近会有小幅波动；死区内只使用速度前馈，
-            // 同时清除PID历史，避免噪声持续积累后产生突发修正。
-            if (Math.Abs(speedErrorMetersPerSecond) <=
-                SpeedErrorDeadbandMetersPerSecond)
-            {
-                Reset();
-                return LimitReferenceSpeed(
-                    controlReferenceSpeedMetersPerSecond);
-            }
-
-            GetCorrectionOutputRange(
-                controlReferenceSpeedMetersPerSecond,
-                out var minimumCorrectionMetersPerSecond,
-                out var maximumCorrectionMetersPerSecond);
-
-            var correctionMetersPerSecond =
-                _feedbackPid.Update(
-                    controlReferenceSpeedMetersPerSecond,
-                    context
-                        .ActualLongitudinalSpeedMetersPerSecond,
-                    context.DeltaTimeSeconds,
-                    minimumCorrectionMetersPerSecond,
-                    maximumCorrectionMetersPerSecond);
-
-            return controlReferenceSpeedMetersPerSecond +
-                   correctionMetersPerSecond;
-        }
-
-        /// <summary>
-        /// 清除纵向速度外环的积分、历史测量值和诊断输出。
-        /// </summary>
-        public void Reset()
-        {
-            _feedbackPid.Reset();
-        }
-
-        /// <summary>
-        /// 根据参考行驶方向计算PID修正量允许使用的动态输出范围。
-        /// </summary>
-        private void GetCorrectionOutputRange(
-            double referenceSpeedMetersPerSecond,
-            out double minimumCorrectionMetersPerSecond,
-            out double maximumCorrectionMetersPerSecond)
-        {
-            if (referenceSpeedMetersPerSecond > 0.0)
-            {
-                minimumCorrectionMetersPerSecond =
-                    -referenceSpeedMetersPerSecond;
-                maximumCorrectionMetersPerSecond =
-                    MaximumCommandSpeedMetersPerSecond -
-                    referenceSpeedMetersPerSecond;
-                return;
-            }
-
-            minimumCorrectionMetersPerSecond =
-                -MaximumCommandSpeedMetersPerSecond -
-                referenceSpeedMetersPerSecond;
-            maximumCorrectionMetersPerSecond =
-                -referenceSpeedMetersPerSecond;
-        }
-
-        /// <summary>
-        /// 在没有有效速度反馈时限制参考速度的绝对值。
-        /// </summary>
-        private double LimitReferenceSpeed(
-            double referenceSpeedMetersPerSecond)
-        {
-            return Math.Max(
-                -MaximumCommandSpeedMetersPerSecond,
-                Math.Min(
-                    MaximumCommandSpeedMetersPerSecond,
-                    referenceSpeedMetersPerSecond));
-        }
-
-        /// <summary>
-        /// 检查最大命令速度是否为正有限值。
-        /// </summary>
-        private static void EnsureFinitePositive(
-            double value,
-            string parameterName)
-        {
-            if (double.IsNaN(value) ||
-                double.IsInfinity(value) ||
-                value <= 0.0)
-            {
-                throw new ArgumentOutOfRangeException(
-                    parameterName,
-                    "纵向控制器最大命令速度必须是正有限值。");
-            }
-        }
-
-        /// <summary>
-        /// 检查速度误差死区是否为非负有限值。
-        /// </summary>
-        private static void EnsureFiniteNonNegative(
-            double value,
-            string parameterName)
-        {
-            if (double.IsNaN(value) ||
-                double.IsInfinity(value) ||
-                value < 0.0)
-            {
-                throw new ArgumentOutOfRangeException(
-                    parameterName,
-                    "纵向控制器速度误差死区必须是非负有限值。");
-            }
-        }
-    }
-}

MultiWheelC/Experiments/CompositeMotionPlanTests.cs

@@ -1,381 +0,0 @@
-using System;
-using System.Drawing;
-using System.Numerics;
-using ClumsyCore;
-using ClumsyCore.DTools;
-using ClumsyCore.Interfaces;
-using ClumsyCore.Pilot;
-using CommonUsage.Chassis;
-using MultiWheelC.Control.Execution;
-using MultiWheelC.StateEstimation;
-using MultiWheelC.Trajectory;
-using MyParking.Shared;
-
-namespace MultiWheelC
-{
-    /// <summary>
-    /// 测试平滑左转轨迹、停车原地左转90°和再次直行的组合运动执行过程。
-    /// </summary>
-    [MovementTest(name = "新版控制器：直线-圆弧-折线组合测试")]
-    public sealed class CompositeStopTurnGoTest : MovementTest
-    {
-        private const float MillimetersPerMeter = 1000f;
-
-        private readonly Painter _painter =
-            UI.GetPainter("CompositeStopTurnGoTest");
-
-        private DriveTask _task;
-        private TrackingExperimentRecorder _recorder;
-
-        public int TrialNumber = 1; // 重复实验编号。
-        public double StraightLengthMeters = 2.0; // 圆弧前后直线长度，单位m。
-        public double TurnRadiusMeters = 2.0; // 平滑左转名义半径，单位m。
-        public double TurnAngleDegrees = 90.0; // 含过渡段在内的总左转角度。
-        public double CurvatureTransitionLengthMeters = 0.80; // 单侧过渡长度，单位m。
-        public double InPlaceLeftTurnDegrees = 90.0; // 停车后的原地左转角度。
-        public double FinalStraightLengthMeters = 4.5; // 自转后的直线长度，单位m。
-        public double StraightMaximumSpeedMetersPerSecond = 0.40; // 直线限速。
-        public double CurveMaximumSpeedMetersPerSecond = 0.30; // 转弯和过渡段限速。
-        public double AccelerationMetersPerSecondSquared = 0.20; // 参考加速度。
-        public double DecelerationMetersPerSecondSquared = 0.08; // 参考减速度。
-        public double PointSpacingMeters = 0.02; // 离散轨迹点间距。
-
-        /// <summary>
-        /// 从当前Detour位姿构造完整计划并依次执行连续跟踪、原地自转和最终直线。
-        /// </summary>
-        public override void Test()
-        {
-            if (_task != null)
-            {
-                Console.WriteLine(
-                    "曲线-停车自转-直线组合测试已经在运行。");
-                return;
-            }
-
-            if (!TrajectoryExperimentInput
-                    .TryReadLateralOffsetMeters(
-                        out var lateralOffsetMeters))
-            {
-                return;
-            }
-
-            var chassis = PilotDefinition.Chassis as MultiWheelChassis;
-            if (chassis == null)
-            {
-                Console.WriteLine(
-                    "当前底盘不是MultiWheelChassis，无法执行组合运动测试。");
-                return;
-            }
-
-            var stateProvider =
-                ParkingVehicleStateProviderFactory.Create(
-                    chassis);
-            if (!stateProvider.TryGetState(
-                    out var initialState))
-            {
-                Console.WriteLine(
-                    "无法读取组合运动起点状态：" +
-                    stateProvider.LastFailureReason);
-                return;
-            }
-
-            var planStartPose =
-                TrajectoryExperimentInput.OffsetPoseLaterally(
-                    initialState.PoseInWorld,
-                    lateralOffsetMeters);
-            var firstTrajectory =
-                TestTrajectoryFactory
-                    .CreateStraightSmoothLeftTurnStraight(
-                        planStartPose,
-                        StraightLengthMeters,
-                        TurnRadiusMeters,
-                        AngleMath.DegreesToRadians(
-                            TurnAngleDegrees),
-                        CurvatureTransitionLengthMeters,
-                        StraightMaximumSpeedMetersPerSecond,
-                        CurveMaximumSpeedMetersPerSecond,
-                        AccelerationMetersPerSecondSquared,
-                        DecelerationMetersPerSecondSquared,
-                        PointSpacingMeters);
-
-            var firstStopPose =
-                firstTrajectory.EndPoint.PoseInWorld;
-            var finalStraightYawRadians =
-                AngleMath.NormalizeRadians(
-                    firstStopPose.YawRadians +
-                    AngleMath.DegreesToRadians(
-                        InPlaceLeftTurnDegrees));
-            var finalStraightStartPose =
-                new Pose2D(
-                    firstStopPose.XMeters,
-                    firstStopPose.YMeters,
-                    finalStraightYawRadians);
-            var finalTrajectory =
-                TestTrajectoryFactory.CreateStraight(
-                    finalStraightStartPose,
-                    FinalStraightLengthMeters,
-                    StraightMaximumSpeedMetersPerSecond,
-                    AccelerationMetersPerSecondSquared,
-                    DecelerationMetersPerSecondSquared,
-                    PointSpacingMeters);
-
-            DrawPlan(
-                firstTrajectory,
-                finalTrajectory,
-                firstStopPose);
-
-            var plan = new MotionPlanSegment[]
-            {
-                new TrackMotionPlanSegment(firstTrajectory)
-                {
-                    // 中间停车点允许后续原地转向和末段跟踪继续收敛位置误差。
-                    FinishDistanceMeters = 0.05,
-                    FinishSpeedMetersPerSecond = 0.03,
-                    FinishHeadingToleranceRadians =
-                        AngleMath.DegreesToRadians(3.0)
-                },
-                new RotateInPlaceMotionPlanSegment(
-                    finalStraightYawRadians),
-                new TrackMotionPlanSegment(finalTrajectory)
-            };
-
-            _recorder = new TrackingExperimentRecorder(
-                controllerName: "NewStanleyPidComposite",
-                trajectoryName:
-                    TrajectoryExperimentInput.BuildTrajectoryName(
-                        "SmoothTurnStopRotateStraight",
-                        lateralOffsetMeters),
-                trialNumber: TrialNumber,
-                referenceStart: ToMillimeterVector(
-                    firstTrajectory.StartPoint.PoseInWorld),
-                referenceEnd: ToMillimeterVector(
-                    finalTrajectory.EndPoint.PoseInWorld),
-                referenceSpeed:
-                    (float)StraightMaximumSpeedMetersPerSecond,
-                sampleIntervalMs: 50,
-                referenceAccelerationMetersPerSecondSquared:
-                    (float)AccelerationMetersPerSecondSquared,
-                referenceDecelerationMetersPerSecondSquared:
-                    (float)DecelerationMetersPerSecondSquared,
-                diagnosticChassis: chassis,
-                diagnosticStateProvider: stateProvider);
-            _recorder.Start();
-
-            var controlPointRadiusMeters =
-                chassis.ControlPointRadius /
-                MillimetersPerMeter;
-            var movement = new MotionPlanExecutor
-            {
-                Segments = plan,
-                StateProvider = stateProvider,
-                SegmentStarted = (index, segment) =>
-                {
-                    _recorder?.ClearControlReference();
-                    _recorder?.ClearGcpCommand();
-                    _recorder?.UpdateCommand(0f, 0f);
-                    Console.WriteLine(
-                        $"组合运动开始第{index + 1}段：" +
-                        segment.GetType().Name);
-                },
-                TrackingCycleObserver = (index, controller) =>
-                    RecordTrackingCycle(
-                        index,
-                        controller,
-                        controlPointRadiusMeters,
-                        stateProvider),
-                RotationCommandObserver = (index, omega) =>
-                    _recorder?.UpdateCommand(
-                        0f,
-                        (float)omega)
-            };
-
-            try
-            {
-                _task = new DriveTask(movement.Get());
-                _task.Wait();
-            }
-            finally
-            {
-                _task?.Stop();
-                _recorder?.UpdateCommand(0f, 0f);
-                _recorder?.StopAndSave();
-                _task = null;
-                _recorder = null;
-                _painter?.Clear();
-            }
-        }
-
-        /// <summary>
-        /// 停止组合运动、保存已有实验数据并清除计划轨迹。
-        /// </summary>
-        public override void TestStop()
-        {
-            _task?.Stop();
-            _recorder?.UpdateCommand(0f, 0f);
-            _recorder?.StopAndSave();
-            _task = null;
-            _recorder = null;
-            _painter?.Clear();
-        }
-
-        /// <summary>
-        /// 将两段轨迹和中间原地自转位置绘制到Clumsy界面。
-        /// </summary>
-        private void DrawPlan(
-            Trajectory2D firstTrajectory,
-            Trajectory2D finalTrajectory,
-            Pose2D rotationPoseInWorld)
-        {
-            _painter.Clear();
-            DrawTrajectory(
-                firstTrajectory,
-                Color.DeepSkyBlue);
-            DrawTrajectory(
-                finalTrajectory,
-                Color.Gold);
-
-            var rotationPoint =
-                ToMillimeterVector(rotationPoseInWorld);
-            _painter.DrawDot(
-                Color.Magenta,
-                rotationPoint.X,
-                rotationPoint.Y,
-                10f);
-        }
-
-        /// <summary>
-        /// 绘制一段离散世界坐标系轨迹。
-        /// </summary>
-        private void DrawTrajectory(
-            Trajectory2D trajectory,
-            Color color)
-        {
-            for (var index = 0;
-                 index < trajectory.Count;
-                 index++)
-            {
-                var point = ToMillimeterVector(
-                    trajectory[index].PoseInWorld);
-                _painter.DrawDot(
-                    color,
-                    point.X,
-                    point.Y,
-                    3f);
-
-                if (index == 0)
-                {
-                    continue;
-                }
-
-                var previousPoint = ToMillimeterVector(
-                    trajectory[index - 1].PoseInWorld);
-                _painter.DrawLine(
-                    color,
-                    previousPoint.X,
-                    previousPoint.Y,
-                    point.X,
-                    point.Y,
-                    width: 2);
-            }
-        }
-
-        /// <summary>
-        /// 将轨迹控制周期使用的状态、参考误差和最终GCP命令写入记录器。
-        /// </summary>
-        private void RecordTrackingCycle(
-            int motionSegmentIndex,
-            ParkingGeometricController controller,
-            double controlPointRadiusMeters,
-            WheelFeedbackVehicleStateProvider stateProvider)
-        {
-            if (controller.LastCycleTiming.HasValue)
-            {
-                _recorder?.RecordControlCycleTiming(
-                    controller.LastCycleTiming.Value,
-                    motionSegmentIndex,
-                    controller.LastRequestedCommand,
-                    controller.LastCommand);
-            }
-
-            if (controller.LastVehicleState.HasValue)
-            {
-                _recorder?.UpdateProcessedState(
-                    controller.LastVehicleState.Value);
-            }
-
-            if (stateProvider.TryGetLatestVelocityDiagnostics(
-                    out var detourBodyVx,
-                    out var detourVelocityValid,
-                    out var rawWheelBodyVx,
-                    out var filteredWheelBodyVx,
-                    out var rawWheelBodyVy,
-                    out var filteredWheelBodyVy,
-                    out var wheelVelocityValid))
-            {
-                _recorder?.UpdateVelocityDiagnostics(
-                    detourBodyVx,
-                    detourVelocityValid,
-                    rawWheelBodyVx,
-                    filteredWheelBodyVx,
-                    rawWheelBodyVy,
-                    filteredWheelBodyVy,
-                    wheelVelocityValid);
-            }
-
-            if (!controller.LastCommand.HasValue)
-            {
-                return;
-            }
-
-            if (controller.LastProjection.HasValue &&
-                controller.LastControlReferenceSpeedMetersPerSecond.HasValue)
-            {
-                var projection =
-                    controller.LastProjection.Value;
-                _recorder?.UpdateControlReference(
-                    projection.ArcLengthMeters,
-                    controller
-                        .LastControlReferenceSpeedMetersPerSecond.Value,
-                    projection.LateralErrorMeters,
-                    projection.HeadingErrorRadians,
-                    projection.DistanceToTrajectoryMeters,
-                    projection.RemainingDistanceMeters,
-                    controller.LastCurvaturePreviewDistanceMeters ?? 0.0,
-                    controller.LastFeedforwardCurvaturePerMeter ??
-                    projection.ReferencePoint.CurvaturePerMeter);
-            }
-
-            var requestedCommand =
-                controller.LastRequestedCommand ??
-                controller.LastCommand.Value;
-            var command = controller.LastCommand.Value;
-            _recorder?.UpdateGcpCommand(
-                requestedCommand.FrontAngleRadians,
-                requestedCommand.RearAngleRadians,
-                command.FrontAngleRadians,
-                command.RearAngleRadians);
-            var curvaturePerMeter = Math.Tan(
-                command.FrontAngleRadians) /
-                controlPointRadiusMeters;
-            var angularSpeedRadiansPerSecond =
-                command.SpeedMetersPerSecond *
-                curvaturePerMeter;
-            _recorder?.UpdateCommand(
-                (float)command.SpeedMetersPerSecond,
-                (float)angularSpeedRadiansPerSecond);
-        }
-
-        /// <summary>
-        /// 将米制世界位姿转换为Clumsy绘图和记录使用的毫米坐标。
-        /// </summary>
-        private static Vector2 ToMillimeterVector(
-            Pose2D poseInWorld)
-        {
-            return new Vector2(
-                (float)(poseInWorld.XMeters *
-                    MillimetersPerMeter),
-                (float)(poseInWorld.YMeters *
-                    MillimetersPerMeter));
-        }
-    }
-}

MultiWheelC/Experiments/DetourStaticDiagnosticTest.cs

@@ -1,696 +0,0 @@
-using System;
-using System.Collections.Generic;
-using System.Diagnostics;
-using System.Globalization;
-using System.IO;
-using System.Text;
-using System.Threading;
-using ClumsyCore;
-using ClumsyCore.Interfaces;
-using ClumsyCore.Pilot;
-using CommonUsage.Chassis;
-using FundamentalLib;
-using MyParking.Shared;
-
-namespace MultiWheelC
-{
-    /// <summary>
-    /// 从C层测试界面启动定时的Detour静态定位与轮组反馈诊断记录。
-    /// </summary>
-    [MovementTest(name = "诊断：Detour静态定位记录")]
-    public sealed class DetourStaticDiagnosticTest : MovementTest
-    {
-        private sealed class DiagnosticSample
-        {
-            public double ElapsedSeconds;
-            public string LocalTimestamp;
-            public bool DetourReadSucceeded;
-            public double DetourCallDurationMilliseconds;
-            public long? DetourTickRaw;
-            public string DetourTimestamp;
-            public bool DetourTimestampValid;
-            public double? DetourDataAgeMilliseconds;
-            public double? DetourLStep;
-            public double? DetourXMillimeters;
-            public double? DetourYMillimeters;
-            public double? DetourYawDegrees;
-            public double? LocalDeltaMilliseconds;
-            public double? DetourTickDeltaMilliseconds;
-            public double? DeltaXMillimeters;
-            public double? DeltaYMillimeters;
-            public double? DeltaYawDegrees;
-            public double? DeltaPositionMillimeters;
-            public bool IsRepeatedTick;
-            public bool IsRepeatedPose;
-            public bool IsOutOfOrderTick;
-            public bool WheelReadSucceeded;
-            public double? WheelBodyVxMetersPerSecond;
-            public double? WheelBodyVyMetersPerSecond;
-            public double? WheelBodyOmegaDegreesPerSecond;
-            public double? ActualSteerLeftFrontDegrees;
-            public double? ActualSteerLeftRearDegrees;
-            public double? ActualSteerRightFrontDegrees;
-            public double? ActualSteerRightRearDegrees;
-            public double? ActualSpeedLeftFrontMetersPerSecond;
-            public double? ActualSpeedLeftRearMetersPerSecond;
-            public double? ActualSpeedRightFrontMetersPerSecond;
-            public double? ActualSpeedRightRearMetersPerSecond;
-            public string FailureReason;
-        }
-
-        private readonly object _sampleSyncRoot = new object();
-        private readonly List<DiagnosticSample> _samples =
-            new List<DiagnosticSample>();
-        private readonly Stopwatch _clock = new Stopwatch();
-
-        private MultiWheelChassis _chassis;
-        private Thread _samplingThread;
-        private volatile bool _sampling;
-        private int _testRunning;
-        private int _stopRequested;
-        private int _sessionId;
-        private bool _hasPreviousDetourSample;
-        private double _previousElapsedSeconds;
-        private long _previousDetourTick;
-        private double _previousDetourXMillimeters;
-        private double _previousDetourYMillimeters;
-        private double _previousDetourYawDegrees;
-
-        /// <summary>
-        /// 获取或设置自动结束前的记录时长，单位为min。
-        /// </summary>
-        public double DurationMinutes = 20.0;
-
-        /// <summary>
-        /// 获取或设置本机主动读取Detour的周期，单位为ms。
-        /// </summary>
-        public int SampleIntervalMilliseconds = 50;
-
-        /// <summary>
-        /// 获取最近一次静态诊断CSV的完整路径。
-        /// </summary>
-        public string SavedFilePath { get; private set; } =
-            string.Empty;
-
-        /// <summary>
-        /// 停车后开始静态采样，并在到达设定时长时自动保存CSV。
-        /// </summary>
-        public override void Test()
-        {
-            if (Interlocked.CompareExchange(
-                    ref _testRunning,
-                    1,
-                    0) != 0)
-            {
-                Console.WriteLine("Detour静态诊断已经在运行。");
-                return;
-            }
-
-            var samplingStarted = false;
-            var completedAutomatically = false;
-            Exception testFailure = null;
-
-            try
-            {
-                ValidateSettings();
-                _chassis =
-                    PilotDefinition.Chassis as MultiWheelChassis;
-                if (_chassis == null)
-                {
-                    throw new InvalidOperationException(
-                        "当前底盘不是MultiWheelChassis，无法读取四轮反馈。");
-                }
-
-                var sessionId = ResetSession();
-                _chassis.PredefinedDriveStop();
-                _sampling = true;
-                _clock.Restart();
-                samplingStarted = true;
-
-                _samplingThread = new Thread(
-                    () => SamplingLoop(sessionId))
-                {
-                    IsBackground = true,
-                    Name = "DetourStaticDiagnostic"
-                };
-                _samplingThread.Start();
-
-                Console.WriteLine(
-                    $"Detour静态诊断开始：时长={DurationMinutes:F1}min，" +
-                    $"主动读取周期={SampleIntervalMilliseconds}ms，" +
-                    "车辆必须保持静止。");
-                Hedingben.ToastText(
-                    $"Detour静态诊断开始，预计{DurationMinutes:F1}分钟后自动结束。");
-
-                var durationSeconds = DurationMinutes * 60.0;
-                while (Volatile.Read(ref _stopRequested) == 0 &&
-                       _clock.Elapsed.TotalSeconds < durationSeconds)
-                {
-                    Thread.Sleep(100);
-                }
-
-                completedAutomatically =
-                    Volatile.Read(ref _stopRequested) == 0;
-            }
-            catch (Exception exception)
-            {
-                testFailure = exception;
-                Console.WriteLine(
-                    "Detour静态诊断失败：" +
-                    exception.Message);
-            }
-            finally
-            {
-                _sampling = false;
-                _chassis?.PredefinedDriveStop();
-
-                if (_samplingThread != null &&
-                    _samplingThread != Thread.CurrentThread)
-                {
-                    _samplingThread.Join(
-                        Math.Max(
-                            1000,
-                            SampleIntervalMilliseconds * 4));
-                }
-
-                _clock.Stop();
-
-                if (samplingStarted)
-                {
-                    try
-                    {
-                        SaveCsvAndReport(
-                            completedAutomatically,
-                            testFailure);
-                    }
-                    catch (Exception exception)
-                    {
-                        Console.WriteLine(
-                            "Detour静态诊断CSV保存失败：" +
-                            exception.Message);
-                        Hedingben.ToastText(
-                            "Detour静态诊断CSV保存失败：" +
-                            exception.Message);
-                    }
-                }
-
-                _samplingThread = null;
-                _chassis = null;
-                Interlocked.Exchange(ref _testRunning, 0);
-            }
-        }
-
-        /// <summary>
-        /// 请求提前停止采样；测试线程随后保存已经采集的数据。
-        /// </summary>
-        public override void TestStop()
-        {
-            Interlocked.Exchange(ref _stopRequested, 1);
-            _sampling = false;
-            _chassis?.PredefinedDriveStop();
-        }
-
-        /// <summary>
-        /// 清除上一次测试的样本、时间基准和输出路径。
-        /// </summary>
-        private int ResetSession()
-        {
-            lock (_sampleSyncRoot)
-            {
-                _samples.Clear();
-            }
-
-            Interlocked.Exchange(ref _stopRequested, 0);
-            _hasPreviousDetourSample = false;
-            _previousElapsedSeconds = 0.0;
-            _previousDetourTick = 0;
-            _previousDetourXMillimeters = 0.0;
-            _previousDetourYMillimeters = 0.0;
-            _previousDetourYawDegrees = 0.0;
-            SavedFilePath = string.Empty;
-            return Interlocked.Increment(ref _sessionId);
-        }
-
-        /// <summary>
-        /// 检查测试时长和主动采样周期是否适合执行。
-        /// </summary>
-        private void ValidateSettings()
-        {
-            NumericGuard.EnsureFinitePositive(
-                DurationMinutes,
-                nameof(DurationMinutes));
-
-            if (SampleIntervalMilliseconds < 20 ||
-                SampleIntervalMilliseconds > 5000)
-            {
-                throw new ArgumentOutOfRangeException(
-                    nameof(SampleIntervalMilliseconds),
-                    "Detour主动读取周期必须在20ms到5000ms之间。");
-            }
-        }
-
-        /// <summary>
-        /// 按设定周期持续采样，直至测试到时或收到停止请求。
-        /// </summary>
-        private void SamplingLoop(int sessionId)
-        {
-            while (_sampling &&
-                   sessionId == Volatile.Read(ref _sessionId))
-            {
-                CaptureSample(sessionId);
-                Thread.Sleep(SampleIntervalMilliseconds);
-            }
-        }
-
-        /// <summary>
-        /// 采集一帧原始Detour定位、接口耗时和四轮实际反馈。
-        /// </summary>
-        private void CaptureSample(int sessionId)
-        {
-            var sample = new DiagnosticSample
-            {
-                ElapsedSeconds = _clock.Elapsed.TotalSeconds,
-                LocalTimestamp =
-                    DateTimeOffset.Now.ToString(
-                        "O",
-                        CultureInfo.InvariantCulture),
-                FailureReason = string.Empty
-            };
-
-            CaptureDetour(sample, sessionId);
-
-            if (sessionId != Volatile.Read(ref _sessionId))
-            {
-                return;
-            }
-
-            CaptureWheelFeedback(sample);
-
-            if (!_sampling ||
-                sessionId != Volatile.Read(ref _sessionId))
-            {
-                return;
-            }
-
-            lock (_sampleSyncRoot)
-            {
-                _samples.Add(sample);
-            }
-        }
-
-        /// <summary>
-        /// 读取Detour原始字段并计算与上一成功读取之间的时间和位姿差。
-        /// </summary>
-        private void CaptureDetour(
-            DiagnosticSample sample,
-            int sessionId)
-        {
-            var callClock = Stopwatch.StartNew();
-
-            try
-            {
-                var location =
-                    DetourInterface.getCartLocation();
-                callClock.Stop();
-
-                sample.DetourCallDurationMilliseconds =
-                    callClock.Elapsed.TotalMilliseconds;
-                sample.DetourReadSucceeded = true;
-                sample.DetourTickRaw = Convert.ToInt64(
-                    location.tick,
-                    CultureInfo.InvariantCulture);
-                sample.DetourLStep = Convert.ToDouble(
-                    location.l_step,
-                    CultureInfo.InvariantCulture);
-                sample.DetourXMillimeters = Convert.ToDouble(
-                    location.x,
-                    CultureInfo.InvariantCulture);
-                sample.DetourYMillimeters = Convert.ToDouble(
-                    location.y,
-                    CultureInfo.InvariantCulture);
-                sample.DetourYawDegrees = Convert.ToDouble(
-                    location.th,
-                    CultureInfo.InvariantCulture);
-
-                if (sessionId != Volatile.Read(ref _sessionId))
-                {
-                    return;
-                }
-
-                CaptureDetourTimestamp(sample);
-                CaptureDetourDelta(sample);
-            }
-            catch (Exception exception)
-            {
-                callClock.Stop();
-                sample.DetourCallDurationMilliseconds =
-                    callClock.Elapsed.TotalMilliseconds;
-                AppendFailure(
-                    sample,
-                    "Detour读取失败：" +
-                    exception.Message);
-            }
-        }
-
-        /// <summary>
-        /// 将Detour原始tick按.NET DateTime ticks解释并记录数据年龄。
-        /// </summary>
-        private static void CaptureDetourTimestamp(
-            DiagnosticSample sample)
-        {
-            try
-            {
-                var detourTime = new DateTime(
-                    sample.DetourTickRaw.Value,
-                    DateTimeKind.Local);
-                sample.DetourTimestamp =
-                    detourTime.ToString(
-                        "O",
-                        CultureInfo.InvariantCulture);
-                sample.DetourDataAgeMilliseconds =
-                    (DateTime.Now - detourTime)
-                    .TotalMilliseconds;
-                sample.DetourTimestampValid = true;
-            }
-            catch (ArgumentOutOfRangeException)
-            {
-                sample.DetourTimestamp = string.Empty;
-            }
-        }
-
-        /// <summary>
-        /// 计算Detour帧间差并更新下一帧使用的原始基准。
-        /// </summary>
-        private void CaptureDetourDelta(
-            DiagnosticSample sample)
-        {
-            var tick = sample.DetourTickRaw.Value;
-            var xMillimeters = sample.DetourXMillimeters.Value;
-            var yMillimeters = sample.DetourYMillimeters.Value;
-            var yawDegrees = sample.DetourYawDegrees.Value;
-
-            if (_hasPreviousDetourSample)
-            {
-                sample.LocalDeltaMilliseconds =
-                    (sample.ElapsedSeconds -
-                     _previousElapsedSeconds) * 1000.0;
-                sample.DetourTickDeltaMilliseconds =
-                    (tick - _previousDetourTick) /
-                    (double)TimeSpan.TicksPerMillisecond;
-                sample.DeltaXMillimeters =
-                    xMillimeters - _previousDetourXMillimeters;
-                sample.DeltaYMillimeters =
-                    yMillimeters - _previousDetourYMillimeters;
-                sample.DeltaYawDegrees =
-                    AngleMath.ShortestDifferenceDegrees(
-                        yawDegrees,
-                        _previousDetourYawDegrees);
-                sample.DeltaPositionMillimeters = Math.Sqrt(
-                    sample.DeltaXMillimeters.Value *
-                    sample.DeltaXMillimeters.Value +
-                    sample.DeltaYMillimeters.Value *
-                    sample.DeltaYMillimeters.Value);
-                sample.IsRepeatedTick =
-                    tick == _previousDetourTick;
-                sample.IsOutOfOrderTick =
-                    tick < _previousDetourTick;
-                sample.IsRepeatedPose =
-                    sample.DeltaPositionMillimeters.Value <= 1e-6 &&
-                    Math.Abs(sample.DeltaYawDegrees.Value) <= 1e-9;
-            }
-
-            _hasPreviousDetourSample = true;
-            _previousElapsedSeconds = sample.ElapsedSeconds;
-            _previousDetourTick = tick;
-            _previousDetourXMillimeters = xMillimeters;
-            _previousDetourYMillimeters = yMillimeters;
-            _previousDetourYawDegrees = yawDegrees;
-        }
-
-        /// <summary>
-        /// 读取底盘反算速度与按物理安装位置识别的四轮实际反馈。
-        /// </summary>
-        private void CaptureWheelFeedback(
-            DiagnosticSample sample)
-        {
-            try
-            {
-                var carSpeed = _chassis.GetCarSpeed(true);
-                sample.WheelBodyVxMetersPerSecond = carSpeed.Vx;
-                sample.WheelBodyVyMetersPerSecond = carSpeed.Vy;
-                // CommonUsage的CarSpeed.Vw以deg/s表达。
-                sample.WheelBodyOmegaDegreesPerSecond = carSpeed.Vw;
-
-#pragma warning disable CS0612, CS0618
-                var wheels = _chassis.GetSteerWheels();
-#pragma warning restore CS0612, CS0618
-
-                var leftFront = FindWheel(wheels, true, true);
-                var leftRear = FindWheel(wheels, false, true);
-                var rightFront = FindWheel(wheels, true, false);
-                var rightRear = FindWheel(wheels, false, false);
-
-                if (leftFront == null || leftRear == null ||
-                    rightFront == null || rightRear == null)
-                {
-                    throw new InvalidOperationException(
-                        "未能按物理安装位置识别四个舵轮。");
-                }
-
-                sample.ActualSteerLeftFrontDegrees =
-                    leftFront.ReadAngle();
-                sample.ActualSteerLeftRearDegrees =
-                    leftRear.ReadAngle();
-                sample.ActualSteerRightFrontDegrees =
-                    rightFront.ReadAngle();
-                sample.ActualSteerRightRearDegrees =
-                    rightRear.ReadAngle();
-                sample.ActualSpeedLeftFrontMetersPerSecond =
-                    leftFront.ReadSpeed();
-                sample.ActualSpeedLeftRearMetersPerSecond =
-                    leftRear.ReadSpeed();
-                sample.ActualSpeedRightFrontMetersPerSecond =
-                    rightFront.ReadSpeed();
-                sample.ActualSpeedRightRearMetersPerSecond =
-                    rightRear.ReadSpeed();
-                sample.WheelReadSucceeded = true;
-            }
-            catch (Exception exception)
-            {
-                AppendFailure(
-                    sample,
-                    "四轮反馈读取失败：" +
-                    exception.Message);
-            }
-        }
-
-        /// <summary>
-        /// 根据真实车体X向前、Y向左的物理安装位置查找指定舵轮。
-        /// </summary>
-        private static SteerWheel FindWheel(
-            IReadOnlyList<SteerWheel> wheels,
-            bool requireFront,
-            bool requireLeft)
-        {
-            foreach (var wheel in wheels)
-            {
-                var isFront = wheel.PhysicalPosition.X >= 0f;
-                var isLeft = wheel.PhysicalPosition.Y >= 0f;
-
-                if (isFront == requireFront &&
-                    isLeft == requireLeft)
-                {
-                    return wheel;
-                }
-            }
-
-            return null;
-        }
-
-        /// <summary>
-        /// 追加本帧诊断失败原因且保留先前错误信息。
-        /// </summary>
-        private static void AppendFailure(
-            DiagnosticSample sample,
-            string reason)
-        {
-            sample.FailureReason =
-                string.IsNullOrWhiteSpace(sample.FailureReason)
-                    ? reason
-                    : sample.FailureReason + "；" + reason;
-        }
-
-        /// <summary>
-        /// 保存采样快照并输出自动结束或手动停止后的摘要提示。
-        /// </summary>
-        private void SaveCsvAndReport(
-            bool completedAutomatically,
-            Exception testFailure)
-        {
-            List<DiagnosticSample> snapshot;
-            lock (_sampleSyncRoot)
-            {
-                snapshot =
-                    new List<DiagnosticSample>(_samples);
-            }
-
-            var outputDirectory = Path.Combine(
-                AppContext.BaseDirectory,
-                "DetourStaticDiagnostics");
-            Directory.CreateDirectory(outputDirectory);
-            SavedFilePath = Path.Combine(
-                outputDirectory,
-                $"{DateTime.Now:yyyyMMdd_HHmmss_fff}_" +
-                "DetourStaticDiagnostic.csv");
-
-            using (var writer = new StreamWriter(
-                       SavedFilePath,
-                       false,
-                       new UTF8Encoding(true)))
-            {
-                WriteCsvRow(writer,
-                    "ElapsedSeconds", "LocalTimestamp",
-                    "DetourReadSucceeded", "DetourCallDurationMilliseconds",
-                    "DetourTickRaw", "DetourTimestamp",
-                    "DetourTimestampValid", "DetourDataAgeMilliseconds",
-                    "DetourLStep", "DetourXMillimeters",
-                    "DetourYMillimeters", "DetourYawDegrees",
-                    "LocalDeltaMilliseconds", "DetourTickDeltaMilliseconds",
-                    "DeltaXMillimeters", "DeltaYMillimeters",
-                    "DeltaYawDegrees", "DeltaPositionMillimeters",
-                    "IsRepeatedTick", "IsRepeatedPose", "IsOutOfOrderTick",
-                    "WheelReadSucceeded", "WheelBodyVxMetersPerSecond",
-                    "WheelBodyVyMetersPerSecond",
-                    "WheelBodyOmegaDegreesPerSecond",
-                    "ActualSteerLeftFrontDegrees",
-                    "ActualSteerLeftRearDegrees",
-                    "ActualSteerRightFrontDegrees",
-                    "ActualSteerRightRearDegrees",
-                    "ActualSpeedLeftFrontMetersPerSecond",
-                    "ActualSpeedLeftRearMetersPerSecond",
-                    "ActualSpeedRightFrontMetersPerSecond",
-                    "ActualSpeedRightRearMetersPerSecond",
-                    "FailureReason");
-
-                foreach (var sample in snapshot)
-                {
-                    WriteCsvRow(writer,
-                        sample.ElapsedSeconds, sample.LocalTimestamp,
-                        sample.DetourReadSucceeded,
-                        sample.DetourCallDurationMilliseconds,
-                        sample.DetourTickRaw, sample.DetourTimestamp,
-                        sample.DetourTimestampValid,
-                        sample.DetourDataAgeMilliseconds,
-                        sample.DetourLStep, sample.DetourXMillimeters,
-                        sample.DetourYMillimeters, sample.DetourYawDegrees,
-                        sample.LocalDeltaMilliseconds,
-                        sample.DetourTickDeltaMilliseconds,
-                        sample.DeltaXMillimeters, sample.DeltaYMillimeters,
-                        sample.DeltaYawDegrees,
-                        sample.DeltaPositionMillimeters,
-                        sample.IsRepeatedTick, sample.IsRepeatedPose,
-                        sample.IsOutOfOrderTick,
-                        sample.WheelReadSucceeded,
-                        sample.WheelBodyVxMetersPerSecond,
-                        sample.WheelBodyVyMetersPerSecond,
-                        sample.WheelBodyOmegaDegreesPerSecond,
-                        sample.ActualSteerLeftFrontDegrees,
-                        sample.ActualSteerLeftRearDegrees,
-                        sample.ActualSteerRightFrontDegrees,
-                        sample.ActualSteerRightRearDegrees,
-                        sample.ActualSpeedLeftFrontMetersPerSecond,
-                        sample.ActualSpeedLeftRearMetersPerSecond,
-                        sample.ActualSpeedRightFrontMetersPerSecond,
-                        sample.ActualSpeedRightRearMetersPerSecond,
-                        sample.FailureReason);
-                }
-            }
-
-            var successfulSamples = 0;
-            var maximumPositionStepMillimeters = 0.0;
-            var maximumAbsoluteYawStepDegrees = 0.0;
-            foreach (var sample in snapshot)
-            {
-                if (sample.DetourReadSucceeded)
-                {
-                    successfulSamples++;
-                }
-
-                maximumPositionStepMillimeters = Math.Max(
-                    maximumPositionStepMillimeters,
-                    sample.DeltaPositionMillimeters ?? 0.0);
-                maximumAbsoluteYawStepDegrees = Math.Max(
-                    maximumAbsoluteYawStepDegrees,
-                    Math.Abs(sample.DeltaYawDegrees ?? 0.0));
-            }
-
-            var completionReason = testFailure != null
-                ? "因异常提前结束"
-                : completedAutomatically
-                    ? "到达设定时长，已自动结束"
-                    : "收到手动停止请求";
-            var message =
-                $"Detour静态诊断{completionReason}；" +
-                $"样本={snapshot.Count}，有效Detour样本={successfulSamples}，" +
-                $"最大位置阶跃={maximumPositionStepMillimeters:F2}mm，" +
-                $"最大航向阶跃={maximumAbsoluteYawStepDegrees:F3}°；" +
-                $"CSV={SavedFilePath}";
-
-            Console.WriteLine(message);
-            Hedingben.ToastText(message);
-        }
-
-        /// <summary>
-        /// 使用InvariantCulture格式化并转义一行CSV字段。
-        /// </summary>
-        private static void WriteCsvRow(
-            TextWriter writer,
-            params object[] values)
-        {
-            var fields = new string[values.Length];
-            for (var index = 0; index < values.Length; index++)
-            {
-                fields[index] = FormatCsvValue(values[index]);
-            }
-
-            writer.WriteLine(string.Join(",", fields));
-        }
-
-        /// <summary>
-        /// 将单个值转换为区域无关且符合CSV转义规则的文本。
-        /// </summary>
-        private static string FormatCsvValue(object value)
-        {
-            if (value == null)
-            {
-                return string.Empty;
-            }
-
-            string text;
-            if (value is bool boolean)
-            {
-                text = boolean ? "1" : "0";
-            }
-            else if (value is IFormattable formattable)
-            {
-                text = formattable.ToString(
-                    null,
-                    CultureInfo.InvariantCulture);
-            }
-            else
-            {
-                text = value.ToString();
-            }
-
-            if (text.IndexOfAny(
-                    new[] { ',', '"', '\r', '\n' }) < 0)
-            {
-                return text;
-            }
-
-            return "\"" +
-                   text.Replace("\"", "\"\"") +
-                   "\"";
-        }
-    }
-}

MultiWheelC/Experiments/LongitudinalIdentificationTests.cs

@@ -1,562 +0,0 @@
-using System;
-using System.Diagnostics;
-using System.Globalization;
-using System.Numerics;
-using System.Threading;
-using ClumsyCore;
-using ClumsyCore.Pilot;
-using CommonUsage.Chassis;
-using FundamentalLib;
-using MultiWheelC.StateEstimation;
-using MyParking.Shared;
-
-namespace MultiWheelC
-{
-    /// <summary>
-    /// 提供纵向开环辨识测试共用的舵轮准备、速度下发、采样和安全停车流程。
-    /// </summary>
-    public abstract class LongitudinalIdentificationTestBase
-        : MovementTest
-    {
-        private const double MaximumTargetSpeedMetersPerSecond =
-            0.70;
-        private const double MinimumTargetSpeedMetersPerSecond =
-            0.02;
-        private const float MillimetersPerMeter = 1000f;
-
-        private DriveTask _preparationTask;
-        private MultiWheelChassis _chassis;
-        private MultiWheelChassisAdapter _adapter;
-        private WheelFeedbackVehicleStateProvider _stateProvider;
-        private TrackingExperimentRecorder _recorder;
-        private int _testRunning;
-        private int _stopRequested;
-
-        /// <summary>
-        /// 获取或设置本次重复实验编号。
-        /// </summary>
-        public int TrialNumber = 1;
-
-        /// <summary>
-        /// 获取或设置速度阶跃前的静止记录时间，单位为s。
-        /// </summary>
-        public double BaselineSeconds = 1.0;
-
-        /// <summary>
-        /// 获取或设置目标速度保持时间，单位为s。
-        /// </summary>
-        public double CommandHoldSeconds = 5.0;
-
-        /// <summary>
-        /// 获取或设置停车后的继续记录时间，单位为s。
-        /// </summary>
-        public double PostStopSeconds = 2.0;
-
-        /// <summary>
-        /// 获取或设置速度命令循环周期，单位为ms。
-        /// </summary>
-        public int ControlIntervalMilliseconds = 50;
-
-        /// <summary>
-        /// 派生测试选择是否使用底盘DeAccPerSecond完成正常减速。
-        /// </summary>
-        protected abstract bool UseConfiguredDeceleration { get; }
-
-        /// <summary>
-        /// 获取用于CSV文件名区分停车方式的标识。
-        /// </summary>
-        protected abstract string StopModeName { get; }
-
-        /// <summary>
-        /// 读取目标速度，完成舵轮回正后执行纵向开环命令并保存CSV。
-        /// </summary>
-        public override void Test()
-        {
-            if (Interlocked.CompareExchange(
-                    ref _testRunning,
-                    1,
-                    0) != 0)
-            {
-                Console.WriteLine(
-                    "纵向辨识测试已经在运行，请先停止当前测试。");
-                return;
-            }
-
-            Interlocked.Exchange(ref _stopRequested, 0);
-
-            try
-            {
-                ValidateSettings();
-                if (!TryReadTargetSpeed(
-                        out var targetSpeedMetersPerSecond))
-                {
-                    return;
-                }
-
-                _chassis =
-                    PilotDefinition.Chassis as MultiWheelChassis;
-                if (_chassis == null)
-                {
-                    throw new InvalidOperationException(
-                        "当前底盘不是MultiWheelChassis，无法执行纵向辨识。");
-                }
-
-                ValidateChassisAndTarget(
-                    targetSpeedMetersPerSecond);
-
-                Console.WriteLine(
-                    "纵向辨识开始前请确认M层轮速诊断记录已经开启。");
-                Hedingben.ToastText(
-                    "请确认M层轮速诊断记录已开启。",
-                    "LongitudinalIdentification");
-
-                if (!MovementTestPreparation.AlignWheelsForward(
-                        ref _preparationTask))
-                {
-                    throw new InvalidOperationException(
-                        "四个舵轮未能稳定回正，纵向辨识已经取消。");
-                }
-
-                ThrowIfStopRequested();
-
-                _adapter = new MultiWheelChassisAdapter(
-                    _chassis,
-                    PilotDefinition.Self.CarNum);
-                _adapter.ResetToBodyFrame();
-                _adapter.StopImmediately();
-
-                _stateProvider =
-                    ParkingVehicleStateProviderFactory.Create(
-                        _chassis);
-                if (!_stateProvider.TryGetState(
-                        out var initialState))
-                {
-                    throw new InvalidOperationException(
-                        "无法读取纵向辨识起点位姿：" +
-                        _stateProvider.LastFailureReason);
-                }
-
-                _recorder = CreateRecorder(
-                    targetSpeedMetersPerSecond,
-                    initialState.PoseInWorld);
-                _recorder.Start();
-
-                Console.WriteLine(
-                    $"纵向辨识参数：目标速度={targetSpeedMetersPerSecond:F3}m/s，" +
-                    $"AccPerSecond={_chassis.AccPerSecond:F3}m/s²，" +
-                    $"DeAccPerSecond={_chassis.DeAccPerSecond:F3}m/s²，" +
-                    $"保持={CommandHoldSeconds:F1}s，停车方式={StopModeName}。");
-
-                RunStationaryPhase(
-                    BaselineSeconds,
-                    "静止基线");
-                RunTargetSpeedPhase(
-                    targetSpeedMetersPerSecond);
-
-                if (UseConfiguredDeceleration)
-                {
-                    RunConfiguredDecelerationPhase(
-                        targetSpeedMetersPerSecond);
-                }
-                else
-                {
-                    _adapter.StopImmediately();
-                    RunStationaryPhase(
-                        PostStopSeconds,
-                        "立即停车后记录");
-                }
-
-                Console.WriteLine(
-                    "纵向辨识测试完成。请同时保存对应的M层轮速诊断CSV。");
-                Hedingben.ToastText(
-                    "纵向辨识完成，请停止并保存M层轮速诊断记录。",
-                    "LongitudinalIdentification");
-            }
-            catch (OperationCanceledException)
-            {
-                Console.WriteLine("纵向辨识已由用户停止。");
-            }
-            catch (Exception exception)
-            {
-                Console.WriteLine(
-                    "纵向辨识失败：" +
-                    exception.Message);
-                Hedingben.ToastText(
-                    "纵向辨识失败：" +
-                    exception.Message,
-                    "LongitudinalIdentification");
-            }
-            finally
-            {
-                _adapter?.StopImmediately();
-                _chassis?.PredefinedDriveStop();
-                _recorder?.UpdateBodyCommand(0f, 0f, 0f);
-                _recorder?.StopAndSave();
-
-                _preparationTask?.Stop();
-                _preparationTask = null;
-                _recorder = null;
-                _stateProvider = null;
-                _adapter = null;
-                _chassis = null;
-                Interlocked.Exchange(ref _testRunning, 0);
-            }
-        }
-
-        /// <summary>
-        /// 请求停止当前辨识并立即清零底盘驱动速度。
-        /// </summary>
-        public override void TestStop()
-        {
-            Interlocked.Exchange(ref _stopRequested, 1);
-            _preparationTask?.Stop();
-            _adapter?.StopImmediately();
-            _chassis?.PredefinedDriveStop();
-        }
-
-        /// <summary>
-        /// 在固定周期内保持零命令并记录静止数据。
-        /// </summary>
-        private void RunStationaryPhase(
-            double durationSeconds,
-            string phaseName)
-        {
-            _recorder.UpdateBodyCommand(0f, 0f, 0f);
-            RunPeriodicPhase(
-                durationSeconds,
-                phaseName,
-                _ => UpdateWheelDiagnostics());
-        }
-
-        /// <summary>
-        /// 通过标准车体速度适配器持续下发直线目标速度。
-        /// </summary>
-        private void RunTargetSpeedPhase(
-            double targetSpeedMetersPerSecond)
-        {
-            _recorder.UpdateBodyCommand(
-                (float)targetSpeedMetersPerSecond,
-                0f,
-                0f);
-
-            RunPeriodicPhase(
-                CommandHoldSeconds,
-                "目标速度保持",
-                interval =>
-                {
-                    var accepted = _adapter.SendBodyTwist(
-                        new Twist2D(
-                            targetSpeedMetersPerSecond,
-                            0.0,
-                            0.0),
-                        interval);
-                    if (!accepted)
-                    {
-                        throw new InvalidOperationException(
-                            "底盘拒绝纵向速度命令：" +
-                            _adapter.LastFailureReason);
-                    }
-
-                    UpdateWheelDiagnostics();
-                });
-        }
-
-        /// <summary>
-        /// 重复发送零速SendMotion，使底盘内部DeAccPerSecond斜坡真实参与减速。
-        /// </summary>
-        private void RunConfiguredDecelerationPhase(
-            double targetSpeedMetersPerSecond)
-        {
-            _recorder.UpdateBodyCommand(0f, 0f, 0f);
-
-            var rampDurationSeconds =
-                Math.Abs(targetSpeedMetersPerSecond) /
-                _chassis.DeAccPerSecond;
-            var totalDurationSeconds =
-                rampDurationSeconds +
-                PostStopSeconds;
-
-            RunPeriodicPhase(
-                totalDurationSeconds,
-                "配置化正常减速",
-                interval =>
-                {
-                    // SendBodyTwist(Zero)会立即停车；辨识DeAccPerSecond时必须
-                    // 直接保持SendMotion零目标，让底盘内部发送速度逐周期下降。
-                    var accepted = _chassis.SendMotion(
-                        0f,
-                        0f,
-                        0f,
-                        interval);
-                    if (!accepted)
-                    {
-                        throw new InvalidOperationException(
-                            "底盘拒绝正常减速命令：" +
-                            _chassis.LastMotionDecomposeFailureReason);
-                    }
-
-                    UpdateWheelDiagnostics();
-                });
-        }
-
-        /// <summary>
-        /// 按实际循环间隔运行一个阶段，并响应测试界面的停止请求。
-        /// </summary>
-        private void RunPeriodicPhase(
-            double durationSeconds,
-            string phaseName,
-            Action<TimeSpan> cycleAction)
-        {
-            Console.WriteLine(
-                $"纵向辨识阶段：{phaseName}，预计{durationSeconds:F2}s。");
-
-            var periodSeconds =
-                ControlIntervalMilliseconds / 1000.0;
-            var phaseClock = Stopwatch.StartNew();
-            var previousCycleSeconds = -periodSeconds;
-
-            while (phaseClock.Elapsed.TotalSeconds <
-                   durationSeconds)
-            {
-                ThrowIfStopRequested();
-
-                var cycleStartSeconds =
-                    phaseClock.Elapsed.TotalSeconds;
-                var deltaTimeSeconds =
-                    cycleStartSeconds -
-                    previousCycleSeconds;
-                previousCycleSeconds = cycleStartSeconds;
-
-                cycleAction(
-                    TimeSpan.FromSeconds(
-                        deltaTimeSeconds));
-
-                var elapsedMilliseconds =
-                    (phaseClock.Elapsed.TotalSeconds -
-                     cycleStartSeconds) * 1000.0;
-                var remainingMilliseconds =
-                    ControlIntervalMilliseconds -
-                    elapsedMilliseconds;
-                if (remainingMilliseconds > 1.0)
-                {
-                    Thread.Sleep(
-                        (int)Math.Floor(
-                            remainingMilliseconds));
-                }
-            }
-        }
-
-        /// <summary>
-        /// 刷新轮组原始与滤波速度，供后台CSV记录器读取最新诊断值。
-        /// </summary>
-        private void UpdateWheelDiagnostics()
-        {
-            _stateProvider.TryGetWheelTwist(
-                out _,
-                out _);
-        }
-
-        /// <summary>
-        /// 创建复用现有字段格式的纵向辨识CSV记录器。
-        /// </summary>
-        private TrackingExperimentRecorder CreateRecorder(
-            double targetSpeedMetersPerSecond,
-            Pose2D initialPoseInWorld)
-        {
-            var expectedTravelMeters =
-                targetSpeedMetersPerSecond *
-                CommandHoldSeconds;
-            var referenceStart = new Vector2(
-                (float)(
-                    initialPoseInWorld.XMeters *
-                    MillimetersPerMeter),
-                (float)(
-                    initialPoseInWorld.YMeters *
-                    MillimetersPerMeter));
-            var referenceEnd = new Vector2(
-                referenceStart.X +
-                (float)(
-                    Math.Cos(initialPoseInWorld.YawRadians) *
-                    expectedTravelMeters *
-                    MillimetersPerMeter),
-                referenceStart.Y +
-                (float)(
-                    Math.Sin(initialPoseInWorld.YawRadians) *
-                    expectedTravelMeters *
-                    MillimetersPerMeter));
-            var speedMillimetersPerSecond =
-                targetSpeedMetersPerSecond *
-                MillimetersPerMeter;
-            var trajectoryName =
-                "LongitudinalStep_" +
-                speedMillimetersPerSecond.ToString(
-                    "+0;-0;0",
-                    CultureInfo.InvariantCulture) +
-                "mmps_" +
-                StopModeName;
-
-            return new TrackingExperimentRecorder(
-                controllerName:
-                    "OpenLoopLongitudinalIdentification",
-                trajectoryName: trajectoryName,
-                trialNumber: TrialNumber,
-                referenceStart: referenceStart,
-                referenceEnd: referenceEnd,
-                referenceSpeed:
-                    (float)targetSpeedMetersPerSecond,
-                sampleIntervalMs:
-                    ControlIntervalMilliseconds,
-                referenceMotionFrameYawDegrees: 0f,
-                referenceAccelerationMetersPerSecondSquared:
-                    _chassis.AccPerSecond,
-                referenceDecelerationMetersPerSecondSquared:
-                    _chassis.DeAccPerSecond,
-                diagnosticChassis: _chassis,
-                diagnosticStateProvider: _stateProvider);
-        }
-
-        /// <summary>
-        /// 从测试界面读取带方向的纵向目标速度，正值前进、负值倒车。
-        /// </summary>
-        private static bool TryReadTargetSpeed(
-            out double targetSpeedMetersPerSecond)
-        {
-            targetSpeedMetersPerSecond = 0.0;
-            var input = UI.GetInput(
-                "输入纵向目标速度（m/s，正数前进、负数倒车，" +
-                $"范围-{MaximumTargetSpeedMetersPerSecond:F1}～" +
-                $"{MaximumTargetSpeedMetersPerSecond:F1}且不能为0）：");
-            var parsed = double.TryParse(
-                input,
-                NumberStyles.Float,
-                CultureInfo.CurrentCulture,
-                out targetSpeedMetersPerSecond) ||
-                double.TryParse(
-                    input,
-                    NumberStyles.Float,
-                    CultureInfo.InvariantCulture,
-                    out targetSpeedMetersPerSecond);
-
-            if (!parsed ||
-                double.IsNaN(targetSpeedMetersPerSecond) ||
-                double.IsInfinity(targetSpeedMetersPerSecond) ||
-                Math.Abs(targetSpeedMetersPerSecond) <
-                    MinimumTargetSpeedMetersPerSecond ||
-                Math.Abs(targetSpeedMetersPerSecond) >
-                    MaximumTargetSpeedMetersPerSecond)
-            {
-                Console.WriteLine(
-                    "目标速度必须是绝对值位于" +
-                    $"{MinimumTargetSpeedMetersPerSecond:F2}～" +
-                    $"{MaximumTargetSpeedMetersPerSecond:F2}m/s之间的有限数值。");
-                return false;
-            }
-
-            return true;
-        }
-
-        /// <summary>
-        /// 验证实验时间、循环周期和编号配置。
-        /// </summary>
-        private void ValidateSettings()
-        {
-            NumericGuard.EnsureFiniteNonNegative(
-                BaselineSeconds,
-                nameof(BaselineSeconds));
-            NumericGuard.EnsureFinitePositive(
-                CommandHoldSeconds,
-                nameof(CommandHoldSeconds));
-            NumericGuard.EnsureFiniteNonNegative(
-                PostStopSeconds,
-                nameof(PostStopSeconds));
-
-            if (CommandHoldSeconds > 30.0 ||
-                BaselineSeconds > 10.0 ||
-                PostStopSeconds > 10.0)
-            {
-                throw new ArgumentOutOfRangeException(
-                    nameof(CommandHoldSeconds),
-                    "辨识阶段时间超出测试允许范围。");
-            }
-
-            if (ControlIntervalMilliseconds < 20 ||
-                ControlIntervalMilliseconds > 200)
-            {
-                throw new ArgumentOutOfRangeException(
-                    nameof(ControlIntervalMilliseconds),
-                    "纵向辨识命令周期必须在20～200ms之间。");
-            }
-
-            if (TrialNumber <= 0)
-            {
-                throw new ArgumentOutOfRangeException(
-                    nameof(TrialNumber),
-                    "测试编号必须大于零。");
-            }
-        }
-
-        /// <summary>
-        /// 验证底盘运行时加载的速度、加速度和减速度配置。
-        /// </summary>
-        private void ValidateChassisAndTarget(
-            double targetSpeedMetersPerSecond)
-        {
-            NumericGuard.EnsureFinitePositive(
-                _chassis.MaxSpeed,
-                nameof(_chassis.MaxSpeed));
-            NumericGuard.EnsureFinitePositive(
-                _chassis.AccPerSecond,
-                nameof(_chassis.AccPerSecond));
-            NumericGuard.EnsureFinitePositive(
-                _chassis.DeAccPerSecond,
-                nameof(_chassis.DeAccPerSecond));
-
-            if (Math.Abs(targetSpeedMetersPerSecond) >
-                _chassis.MaxSpeed)
-            {
-                throw new ArgumentOutOfRangeException(
-                    nameof(targetSpeedMetersPerSecond),
-                    "目标速度超过底盘当前MaxSpeed配置。");
-            }
-        }
-
-        /// <summary>
-        /// 在收到停止请求时中断当前阶段并转入finally安全停车。
-        /// </summary>
-        private void ThrowIfStopRequested()
-        {
-            if (Volatile.Read(ref _stopRequested) != 0)
-            {
-                throw new OperationCanceledException();
-            }
-        }
-    }
-
-    /// <summary>
-    /// 测量速度阶跃、加速与稳态，并在保持结束后立即清零驱动速度。
-    /// </summary>
-    [MovementTest(name = "纵向辨识：速度阶跃与立即停车")]
-    public sealed class LongitudinalStepImmediateStopTest
-        : LongitudinalIdentificationTestBase
-    {
-        protected override bool UseConfiguredDeceleration =>
-            false;
-
-        protected override string StopModeName =>
-            "ImmediateStop";
-    }
-
-    /// <summary>
-    /// 测量速度阶跃、稳态以及由底盘DeAccPerSecond形成的正常减速过程。
-    /// </summary>
-    [MovementTest(name = "纵向辨识：速度阶跃与正常减速")]
-    public sealed class LongitudinalStepConfiguredDecelerationTest
-        : LongitudinalIdentificationTestBase
-    {
-        protected override bool UseConfiguredDeceleration =>
-            true;
-
-        protected override string StopModeName =>
-            "ConfiguredDeceleration";
-    }
-}

MultiWheelC/Experiments/NewControllerTrackingTests.cs

@@ -1,979 +0,0 @@
-using System;
-using System.Drawing;
-using System.Globalization;
-using System.Numerics;
-using System.Threading;
-using ClumsyCore;
-using ClumsyCore.DTools;
-using ClumsyCore.Interfaces;
-using ClumsyCore.Pilot;
-using CommonUsage.Chassis;
-using MultiWheelC.Control.Execution;
-using MultiWheelC.StateEstimation;
-using MyParking.Shared;
-
-namespace MultiWheelC
-{
-    /// <summary>
-    /// 统一读取轨迹实验的有符号横向偏移，并将车体局部偏移转换到世界坐标系。
-    /// </summary>
-    internal static class TrajectoryExperimentInput
-    {
-        private const double MaximumOffsetCentimeters = 30.0;
-
-        /// <summary>
-        /// 从Clumsy输入框读取车体左正右负的横向偏移，单位转换为m。
-        /// </summary>
-        public static bool TryReadLateralOffsetMeters(
-            out double lateralOffsetMeters)
-        {
-            lateralOffsetMeters = 0.0;
-            var input = UI.GetInput(
-                "输入轨迹横向偏移（cm，左正右负，范围-30～30）：");
-            var parsed = double.TryParse(
-                input,
-                NumberStyles.Float,
-                CultureInfo.CurrentCulture,
-                out var offsetCentimeters) ||
-                double.TryParse(
-                    input,
-                    NumberStyles.Float,
-                    CultureInfo.InvariantCulture,
-                    out offsetCentimeters);
-
-            if (!parsed ||
-                double.IsNaN(offsetCentimeters) ||
-                double.IsInfinity(offsetCentimeters) ||
-                Math.Abs(offsetCentimeters) >
-                    MaximumOffsetCentimeters)
-            {
-                Console.WriteLine(
-                    "轨迹横向偏移必须是-30～30cm之间的有限数值，测试未启动。");
-                return false;
-            }
-
-            lateralOffsetMeters =
-                offsetCentimeters / 100.0;
-            return true;
-        }
-
-        /// <summary>
-        /// 沿初始车体左方向平移参考轨迹起点，同时保持世界坐标航向不变。
-        /// </summary>
-        public static Pose2D OffsetPoseLaterally(
-            Pose2D poseInWorld,
-            double lateralOffsetMeters)
-        {
-            var yawRadians = poseInWorld.YawRadians;
-            return new Pose2D(
-                poseInWorld.XMeters -
-                    Math.Sin(yawRadians) *
-                    lateralOffsetMeters,
-                poseInWorld.YMeters +
-                    Math.Cos(yawRadians) *
-                    lateralOffsetMeters,
-                yawRadians);
-        }
-
-        /// <summary>
-        /// 生成带毫米偏移标识的实验轨迹名称。
-        /// </summary>
-        public static string BuildTrajectoryName(
-            string baseName,
-            double lateralOffsetMeters)
-        {
-            return baseName +
-                   "_Offset" +
-                   (lateralOffsetMeters * 1000.0)
-                   .ToString("+0;-0;0", CultureInfo.InvariantCulture) +
-                   "mm";
-        }
-    }
-
-    /// <summary>
-    /// 从当前Detour位姿开始执行新版控制器4m直线跟踪并保存实验数据。
-    /// </summary>
-    [MovementTest(name = "新版控制器：4m直线轨迹跟踪")]
-    public class NewControllerStraight4mTest
-        : MovementTest
-    {
-        private const float MillimetersPerMeter = 1000f;
-
-        private readonly Painter _painter =
-            UI.GetPainter("NewControllerStraight4m");
-
-        private DriveTask _task;
-        private TrackingExperimentRecorder _recorder;
-        private IVehicleStateProvider _stateProvider;
-
-        /// <summary>
-        /// 获取或设置本次测试编号，用于区分重复实验CSV。
-        /// </summary>
-        public int TrialNumber = 1;
-
-        /// <summary>
-        /// 获取或设置4m直线的巡航参考速度，单位为m/s。
-        /// </summary>
-        public double CruiseSpeedMetersPerSecond = 0.40;
-
-        /// <summary>
-        /// 获取或设置参考速度加速度，单位为m/s²。
-        /// </summary>
-        public double AccelerationMetersPerSecondSquared = 0.20;
-
-        /// <summary>
-        /// 获取或设置参考速度减速度，单位为m/s²。
-        /// </summary>
-        public double DecelerationMetersPerSecondSquared = 0.08;
-
-        /// <summary>
-        /// 获取或设置离散轨迹点间距，单位为m。
-        /// </summary>
-        public double PointSpacingMeters = 0.02;
-
-        /// <summary>
-        /// 获取实验记录使用的轨迹基础名称，供同一套直线测试流程区分前进和倒车。
-        /// </summary>
-        protected virtual string ExperimentTrajectoryBaseName =>
-            "ProfiledStraight4m";
-
-        /// <summary>
-        /// 获取直线主运动方向相对车头的夹角，单位为rad。
-        /// </summary>
-        protected virtual double MotionDirectionInBodyRadians =>
-            0.0;
-
-        /// <summary>
-        /// 获取是否由生成后的轨迹自动推导底盘运动坐标系方向。
-        /// </summary>
-        protected virtual bool ResolveMotionDirectionFromTrajectory =>
-            false;
-
-        /// <summary>
-        /// 获取轨迹完成后是否需要将舵轮主动恢复到车头方向。
-        /// </summary>
-        protected virtual bool ReturnWheelsForwardAfterCompletion =>
-            false;
-
-        /// <summary>
-        /// 读取当前位姿、绘制离散轨迹并启动新版轨迹跟踪动作。
-        /// </summary>
-        public override void Test()
-        {
-            if (_task != null)
-            {
-                Console.WriteLine(
-                    "新版4m直线轨迹测试已经在运行，请先停止当前测试。");
-                return;
-            }
-
-            if (!TrajectoryExperimentInput
-                    .TryReadLateralOffsetMeters(
-                        out var lateralOffsetMeters))
-            {
-                return;
-            }
-
-            var chassis =
-                PilotDefinition.Chassis as MultiWheelChassis;
-            if (chassis == null)
-            {
-                Console.WriteLine(
-                    "当前底盘不是MultiWheelChassis，无法执行新版轨迹测试。");
-                return;
-            }
-
-            var stateProvider =
-                ParkingVehicleStateProviderFactory.Create(
-                    chassis);
-            if (!stateProvider.TryGetState(
-                    out var initialState))
-            {
-                Console.WriteLine(
-                    "无法读取有效停车状态起点位姿：" +
-                    stateProvider.LastFailureReason);
-                _stateProvider = null;
-                return;
-            }
-
-            _stateProvider = stateProvider;
-
-            var trajectoryStartPose =
-                TrajectoryExperimentInput.OffsetPoseLaterally(
-                    initialState.PoseInWorld,
-                    lateralOffsetMeters);
-            var trajectory =
-                TestTrajectoryFactory.CreateStraight4Meters(
-                    trajectoryStartPose,
-                    CruiseSpeedMetersPerSecond,
-                    AccelerationMetersPerSecondSquared,
-                    DecelerationMetersPerSecondSquared,
-                    PointSpacingMeters,
-                    MotionDirectionInBodyRadians);
-
-            DrawTrajectory(trajectory);
-
-            var referenceStart = ToMillimeterVector(
-                trajectory.StartPoint.PoseInWorld);
-            var referenceEnd = ToMillimeterVector(
-                trajectory.EndPoint.PoseInWorld);
-            var recorder =
-                new TrackingExperimentRecorder(
-                    controllerName: "NewStanleyPid",
-                    trajectoryName:
-                        TrajectoryExperimentInput.BuildTrajectoryName(
-                            ExperimentTrajectoryBaseName,
-                            lateralOffsetMeters),
-                    trialNumber: TrialNumber,
-                    referenceStart: referenceStart,
-                    referenceEnd: referenceEnd,
-                    referenceSpeed:
-                        (float)CruiseSpeedMetersPerSecond,
-                    sampleIntervalMs: 50,
-                    referenceMotionFrameYawDegrees:
-                        (float)AngleMath.RadiansToDegrees(
-                            MotionDirectionInBodyRadians),
-                    referenceAccelerationMetersPerSecondSquared:
-                        (float)AccelerationMetersPerSecondSquared,
-                    referenceDecelerationMetersPerSecondSquared:
-                        (float)DecelerationMetersPerSecondSquared,
-                    diagnosticChassis: chassis,
-                    diagnosticStateProvider: stateProvider);
-            _recorder = recorder;
-
-            var controlPointRadiusMeters =
-                chassis.ControlPointRadius /
-                MillimetersPerMeter;
-            var movement =
-                new TrajectoryTrackingMovement
-                {
-                    Trajectory = trajectory,
-                    StateProvider = _stateProvider,
-                    MotionDirectionInBodyRadians =
-                        ResolveMotionDirectionFromTrajectory
-                            ? (double?)null
-                            : MotionDirectionInBodyRadians,
-                    ReturnWheelsForwardAfterCompletion =
-                        ReturnWheelsForwardAfterCompletion,
-                    CycleObserver = controller =>
-                        RecordControlCycle(
-                            recorder,
-                            controller,
-                            controlPointRadiusMeters,
-                            _stateProvider as
-                                WheelFeedbackVehicleStateProvider)
-                };
-
-            recorder.Start();
-
-            try
-            {
-                _task = new DriveTask(movement.Get());
-                _task.Wait();
-
-                // 保留少量停车后原始Detour数据，并生成一帧处理后的静止状态。
-                Thread.Sleep(400);
-                recorder.UpdateCommand(0f, 0f);
-                if (_stateProvider.TryGetState(
-                        out var stoppedState))
-                {
-                    recorder.UpdateProcessedState(
-                        stoppedState);
-                }
-            }
-            finally
-            {
-                _task?.Stop();
-                recorder.UpdateCommand(0f, 0f);
-                recorder.StopAndSave();
-                _painter.Clear();
-                _task = null;
-                _recorder = null;
-                _stateProvider = null;
-            }
-        }
-
-        /// <summary>
-        /// 停止正在运行的测试、保存已有数据并清除Clumsy轨迹可视化。
-        /// </summary>
-        public override void TestStop()
-        {
-            _task?.Stop();
-            _recorder?.UpdateCommand(0f, 0f);
-            _recorder?.StopAndSave();
-            _painter.Clear();
-            _task = null;
-            _recorder = null;
-            _stateProvider = null;
-        }
-
-        /// <summary>
-        /// 将离散轨迹点和相邻线段从SI单位转换为Clumsy毫米坐标后绘制。
-        /// </summary>
-        private void DrawTrajectory(
-            Trajectory.Trajectory2D trajectory)
-        {
-            _painter.Clear();
-
-            for (var index = 0;
-                 index < trajectory.Count;
-                 index++)
-            {
-                var point = ToMillimeterVector(
-                    trajectory[index].PoseInWorld);
-                _painter.DrawDot(
-                    Color.Cyan,
-                    point.X,
-                    point.Y,
-                    3f);
-
-                if (index == 0)
-                {
-                    continue;
-                }
-
-                var previousPoint = ToMillimeterVector(
-                    trajectory[index - 1].PoseInWorld);
-                _painter.DrawLine(
-                    Color.DeepSkyBlue,
-                    previousPoint.X,
-                    previousPoint.Y,
-                    point.X,
-                    point.Y,
-                    width: 2);
-            }
-
-            var start = ToMillimeterVector(
-                trajectory.StartPoint.PoseInWorld);
-            var end = ToMillimeterVector(
-                trajectory.EndPoint.PoseInWorld);
-            _painter.DrawDot(
-                Color.LimeGreen,
-                start.X,
-                start.Y,
-                8f);
-            _painter.DrawDot(
-                Color.OrangeRed,
-                end.X,
-                end.Y,
-                8f);
-        }
-
-        /// <summary>
-        /// 将控制器本周期使用的状态和最终GCP命令同步给实验记录器。
-        /// </summary>
-        private static void RecordControlCycle(
-            TrackingExperimentRecorder recorder,
-            ParkingGeometricController controller,
-            double controlPointRadiusMeters,
-            WheelFeedbackVehicleStateProvider stateProvider)
-        {
-            if (controller.LastCycleTiming.HasValue)
-            {
-                recorder.RecordControlCycleTiming(
-                    controller.LastCycleTiming.Value,
-                    requestedCommand:
-                        controller.LastRequestedCommand,
-                    sentCommand:
-                        controller.LastCommand);
-            }
-
-            if (controller.LastVehicleState.HasValue)
-            {
-                recorder.UpdateProcessedState(
-                    controller.LastVehicleState.Value);
-            }
-
-            UpdateVelocityDiagnostics(
-                recorder,
-                stateProvider);
-
-            if (!controller.LastCommand.HasValue)
-            {
-                return;
-            }
-
-            if (controller.LastProjection.HasValue &&
-                controller.LastControlReferenceSpeedMetersPerSecond.HasValue)
-            {
-                var projection =
-                    controller.LastProjection.Value;
-                recorder.UpdateControlReference(
-                    projection.ArcLengthMeters,
-                    controller.LastControlReferenceSpeedMetersPerSecond.Value,
-                    projection.LateralErrorMeters,
-                    projection.HeadingErrorRadians,
-                    projection.DistanceToTrajectoryMeters,
-                    projection.RemainingDistanceMeters,
-                    controller.LastCurvaturePreviewDistanceMeters ?? 0.0,
-                    controller.LastFeedforwardCurvaturePerMeter ??
-                    projection.ReferencePoint.CurvaturePerMeter);
-            }
-
-            var requestedCommand =
-                controller.LastRequestedCommand ??
-                controller.LastCommand.Value;
-            var command = controller.LastCommand.Value;
-            recorder.UpdateGcpCommand(
-                requestedCommand.FrontAngleRadians,
-                requestedCommand.RearAngleRadians,
-                command.FrontAngleRadians,
-                command.RearAngleRadians);
-            var curvaturePerMeter = Math.Tan(
-                command.FrontAngleRadians) /
-                controlPointRadiusMeters;
-            var angularSpeedRadiansPerSecond =
-                command.SpeedMetersPerSecond *
-                curvaturePerMeter;
-
-            recorder.UpdateCommand(
-                (float)command.SpeedMetersPerSecond,
-                (float)angularSpeedRadiansPerSecond);
-        }
-
-        /// <summary>
-        /// 将同一周期的Detour速度和轮速解算速度写入实验记录器。
-        /// </summary>
-        private static void UpdateVelocityDiagnostics(
-            TrackingExperimentRecorder recorder,
-            WheelFeedbackVehicleStateProvider stateProvider)
-        {
-            if (stateProvider == null ||
-                !stateProvider.TryGetLatestVelocityDiagnostics(
-                    out var detourBodyVx,
-                    out var detourVelocityValid,
-                    out var rawWheelBodyVx,
-                    out var filteredWheelBodyVx,
-                    out var rawWheelBodyVy,
-                    out var filteredWheelBodyVy,
-                    out var wheelVelocityValid))
-            {
-                return;
-            }
-
-            recorder.UpdateVelocityDiagnostics(
-                detourBodyVx,
-                detourVelocityValid,
-                rawWheelBodyVx,
-                filteredWheelBodyVx,
-                rawWheelBodyVy,
-                filteredWheelBodyVy,
-                wheelVelocityValid);
-        }
-
-        /// <summary>
-        /// 将Shared世界坐标系米制位姿转换为Clumsy绘图和旧记录器使用的毫米坐标。
-        /// </summary>
-        private static Vector2 ToMillimeterVector(
-            Pose2D poseInWorld)
-        {
-            return new Vector2(
-                (float)(
-                    poseInWorld.XMeters *
-                    MillimetersPerMeter),
-                (float)(
-                    poseInWorld.YMeters *
-                    MillimetersPerMeter));
-        }
-
-    }
-
-    /// <summary>
-    /// 从当前Detour位姿开始，沿车体后方执行新版控制器4m直线倒车跟踪并保存实验数据。
-    /// </summary>
-    [MovementTest(name = "新版控制器：4m直线倒车轨迹跟踪")]
-    public sealed class NewControllerReverseStraight4mTest
-        : NewControllerStraight4mTest
-    {
-        /// <summary>
-        /// 使用负参考速度，使轨迹工厂沿车尾方向生成轨迹并触发倒车控制语义。
-        /// </summary>
-        public NewControllerReverseStraight4mTest()
-        {
-            CruiseSpeedMetersPerSecond = -0.40;
-        }
-
-        /// <summary>
-        /// 将倒车实验与前进直线实验的CSV名称明确区分。
-        /// </summary>
-        protected override string ExperimentTrajectoryBaseName =>
-            "ProfiledReverseStraight4m";
-    }
-
-    /// <summary>
-    /// 将舵轮准备到车体左前45°，以0.4m/s跟踪4m直线，停车后再恢复车头方向。
-    /// </summary>
-    [MovementTest(name = "新版控制器：45°蟹行4m直线轨迹跟踪")]
-    public sealed class NewControllerCrab45Straight4mTest
-        : NewControllerStraight4mTest
-    {
-        /// <summary>
-        /// 使用车体左前45°作为本次直线轨迹的固定运动方向。
-        /// </summary>
-        protected override double MotionDirectionInBodyRadians =>
-            Math.PI / 4.0;
-
-        /// <summary>
-        /// 只用45°定义参考轨迹，底盘β由轨迹切线和车身参考航向自动推导。
-        /// </summary>
-        protected override bool ResolveMotionDirectionFromTrajectory =>
-            true;
-
-        /// <summary>
-        /// 蟹行轨迹正常完成后主动将四个舵轮恢复到车头方向。
-        /// </summary>
-        protected override bool ReturnWheelsForwardAfterCompletion =>
-            true;
-
-        /// <summary>
-        /// 将45°蟹行实验与普通前进和倒车实验的CSV名称明确区分。
-        /// </summary>
-        protected override string ExperimentTrajectoryBaseName =>
-            "ProfiledCrab45Straight4m";
-    }
-
-    /// <summary>
-    /// 从当前Detour位姿开始执行“3m直线—左半圆—3m直线”新版控制器跟踪实验。
-    /// </summary>
-    [MovementTest(name = "新版控制器：直线-左半圆-直线轨迹跟踪")]
-    public class NewControllerStraightSemicircleStraightTest
-        : MovementTest
-    {
-        private const float MillimetersPerMeter = 1000f;
-
-        private readonly Painter _painter =
-            UI.GetPainter(
-                "NewControllerStraightSemicircleStraight");
-
-        private DriveTask _task;
-        private TrackingExperimentRecorder _recorder;
-        private IVehicleStateProvider _stateProvider;
-
-        /// <summary>
-        /// 获取或设置本次测试编号，用于区分重复实验CSV。
-        /// </summary>
-        public int TrialNumber = 1;
-
-        /// <summary>
-        /// 获取或设置半圆前后两段直线的长度，单位为m。
-        /// </summary>
-        public double StraightLengthMeters = 3.0;
-
-        /// <summary>
-        /// 获取或设置左转半圆的转弯半径，单位为m。
-        /// </summary>
-        public double TurnRadiusMeters = 2.0;
-
-        /// <summary>
-        /// 获取或设置直线与等曲率转弯之间的曲率过渡长度，单位为m。
-        /// </summary>
-        public double CurvatureTransitionLengthMeters = 0.70;
-
-        /// <summary>
-        /// 获取或设置两段直线的最大参考速度，单位为m/s。
-        /// </summary>
-        public double StraightMaximumSpeedMetersPerSecond = 0.40;
-
-        /// <summary>
-        /// 获取或设置半圆段的最大参考速度，单位为m/s。
-        /// </summary>
-        public double SemicircleMaximumSpeedMetersPerSecond = 0.30;
-
-        /// <summary>
-        /// 获取或设置参考速度加速度，单位为m/s²。
-        /// </summary>
-        public double AccelerationMetersPerSecondSquared = 0.20;
-
-        /// <summary>
-        /// 获取或设置参考速度减速度，单位为m/s²。
-        /// </summary>
-        public double DecelerationMetersPerSecondSquared = 0.08;
-
-        /// <summary>
-        /// 获取或设置离散轨迹点间距，单位为m。
-        /// </summary>
-        public double PointSpacingMeters = 0.02;
-
-        /// <summary>
-        /// 获取组合轨迹主运动方向相对车头的夹角，单位为rad。
-        /// </summary>
-        protected virtual double MotionDirectionInBodyRadians =>
-            0.0;
-
-        /// <summary>
-        /// 获取是否由生成后的轨迹自动推导底盘运动坐标系方向。
-        /// </summary>
-        protected virtual bool ResolveMotionDirectionFromTrajectory =>
-            false;
-
-        /// <summary>
-        /// 获取轨迹完成后是否需要将舵轮主动恢复到车头方向。
-        /// </summary>
-        protected virtual bool ReturnWheelsForwardAfterCompletion =>
-            false;
-
-        /// <summary>
-        /// 获取实验记录使用的轨迹基础名称。
-        /// </summary>
-        protected virtual string ExperimentTrajectoryBaseName =>
-            "ProfiledStraightSmoothLeftTurnStraight";
-
-        /// <summary>
-        /// 读取当前位姿、绘制组合轨迹并启动新版轨迹跟踪动作。
-        /// </summary>
-        public override void Test()
-        {
-            if (_task != null)
-            {
-                Console.WriteLine(
-                    "新版直线-左半圆-直线测试已经在运行，请先停止当前测试。");
-                return;
-            }
-
-            if (!TrajectoryExperimentInput
-                    .TryReadLateralOffsetMeters(
-                        out var lateralOffsetMeters))
-            {
-                return;
-            }
-
-            var chassis =
-                PilotDefinition.Chassis as MultiWheelChassis;
-            if (chassis == null)
-            {
-                Console.WriteLine(
-                    "当前底盘不是MultiWheelChassis，无法执行新版轨迹测试。");
-                return;
-            }
-
-            var stateProvider =
-                ParkingVehicleStateProviderFactory.Create(
-                    chassis);
-            if (!stateProvider.TryGetState(
-                    out var initialState))
-            {
-                Console.WriteLine(
-                    "无法读取有效停车状态起点位姿：" +
-                    stateProvider.LastFailureReason);
-                _stateProvider = null;
-                return;
-            }
-
-            _stateProvider = stateProvider;
-
-            var trajectoryStartPose =
-                TrajectoryExperimentInput.OffsetPoseLaterally(
-                    initialState.PoseInWorld,
-                    lateralOffsetMeters);
-            var trajectory =
-                TestTrajectoryFactory
-                    .CreateStraightLeftSemicircleStraight(
-                        trajectoryStartPose,
-                        StraightLengthMeters,
-                        TurnRadiusMeters,
-                        CurvatureTransitionLengthMeters,
-                        StraightMaximumSpeedMetersPerSecond,
-                        SemicircleMaximumSpeedMetersPerSecond,
-                        AccelerationMetersPerSecondSquared,
-                        DecelerationMetersPerSecondSquared,
-                        PointSpacingMeters,
-                        MotionDirectionInBodyRadians);
-
-            DrawTrajectory(trajectory);
-
-            var referenceStart = ToMillimeterVector(
-                trajectory.StartPoint.PoseInWorld);
-            var referenceEnd = ToMillimeterVector(
-                trajectory.EndPoint.PoseInWorld);
-            var recorder =
-                new TrackingExperimentRecorder(
-                    controllerName: "NewStanleyPid",
-                    trajectoryName:
-                        TrajectoryExperimentInput.BuildTrajectoryName(
-                            ExperimentTrajectoryBaseName,
-                            lateralOffsetMeters),
-                    trialNumber: TrialNumber,
-                    referenceStart: referenceStart,
-                    referenceEnd: referenceEnd,
-                    referenceSpeed:
-                        (float)StraightMaximumSpeedMetersPerSecond,
-                    sampleIntervalMs: 50,
-                    referenceMotionFrameYawDegrees:
-                        (float)AngleMath.RadiansToDegrees(
-                            MotionDirectionInBodyRadians),
-                    referenceAccelerationMetersPerSecondSquared:
-                        (float)AccelerationMetersPerSecondSquared,
-                    referenceDecelerationMetersPerSecondSquared:
-                        (float)DecelerationMetersPerSecondSquared,
-                    diagnosticChassis: chassis,
-                    diagnosticStateProvider: stateProvider);
-            _recorder = recorder;
-
-            var controlPointRadiusMeters =
-                chassis.ControlPointRadius /
-                MillimetersPerMeter;
-            var movement =
-                new TrajectoryTrackingMovement
-                {
-                    Trajectory = trajectory,
-                    StateProvider = _stateProvider,
-                    MotionDirectionInBodyRadians =
-                        ResolveMotionDirectionFromTrajectory
-                            ? (double?)null
-                            : MotionDirectionInBodyRadians,
-                    ReturnWheelsForwardAfterCompletion =
-                        ReturnWheelsForwardAfterCompletion,
-                    CycleObserver = controller =>
-                        RecordControlCycle(
-                            recorder,
-                            controller,
-                            controlPointRadiusMeters,
-                            _stateProvider as
-                                WheelFeedbackVehicleStateProvider)
-                };
-
-            recorder.Start();
-
-            try
-            {
-                _task = new DriveTask(movement.Get());
-                _task.Wait();
-
-                // 保留少量停车后原始Detour数据，并生成一帧处理后的静止状态。
-                Thread.Sleep(400);
-                recorder.UpdateCommand(0f, 0f);
-                if (_stateProvider.TryGetState(
-                        out var stoppedState))
-                {
-                    recorder.UpdateProcessedState(
-                        stoppedState);
-                }
-            }
-            finally
-            {
-                _task?.Stop();
-                recorder.UpdateCommand(0f, 0f);
-                recorder.StopAndSave();
-                _painter.Clear();
-                _task = null;
-                _recorder = null;
-                _stateProvider = null;
-            }
-        }
-
-        /// <summary>
-        /// 停止组合轨迹测试、保存已有数据并清除Clumsy轨迹可视化。
-        /// </summary>
-        public override void TestStop()
-        {
-            _task?.Stop();
-            _recorder?.UpdateCommand(0f, 0f);
-            _recorder?.StopAndSave();
-            _painter.Clear();
-            _task = null;
-            _recorder = null;
-            _stateProvider = null;
-        }
-
-        /// <summary>
-        /// 将组合轨迹的离散点和相邻线段转换为Clumsy毫米坐标后绘制。
-        /// </summary>
-        private void DrawTrajectory(
-            Trajectory.Trajectory2D trajectory)
-        {
-            _painter.Clear();
-
-            for (var index = 0;
-                 index < trajectory.Count;
-                 index++)
-            {
-                var point = ToMillimeterVector(
-                    trajectory[index].PoseInWorld);
-                _painter.DrawDot(
-                    Color.Cyan,
-                    point.X,
-                    point.Y,
-                    3f);
-
-                if (index == 0)
-                {
-                    continue;
-                }
-
-                var previousPoint = ToMillimeterVector(
-                    trajectory[index - 1].PoseInWorld);
-                _painter.DrawLine(
-                    Color.DeepSkyBlue,
-                    previousPoint.X,
-                    previousPoint.Y,
-                    point.X,
-                    point.Y,
-                    width: 2);
-            }
-
-            var start = ToMillimeterVector(
-                trajectory.StartPoint.PoseInWorld);
-            var end = ToMillimeterVector(
-                trajectory.EndPoint.PoseInWorld);
-            _painter.DrawDot(
-                Color.LimeGreen,
-                start.X,
-                start.Y,
-                8f);
-            _painter.DrawDot(
-                Color.OrangeRed,
-                end.X,
-                end.Y,
-                8f);
-        }
-
-        /// <summary>
-        /// 将组合轨迹控制周期的状态、参考量和最终GCP命令同步给实验记录器。
-        /// </summary>
-        private static void RecordControlCycle(
-            TrackingExperimentRecorder recorder,
-            ParkingGeometricController controller,
-            double controlPointRadiusMeters,
-            WheelFeedbackVehicleStateProvider stateProvider)
-        {
-            if (controller.LastCycleTiming.HasValue)
-            {
-                recorder.RecordControlCycleTiming(
-                    controller.LastCycleTiming.Value,
-                    requestedCommand:
-                        controller.LastRequestedCommand,
-                    sentCommand:
-                        controller.LastCommand);
-            }
-
-            if (controller.LastVehicleState.HasValue)
-            {
-                recorder.UpdateProcessedState(
-                    controller.LastVehicleState.Value);
-            }
-
-            UpdateVelocityDiagnostics(
-                recorder,
-                stateProvider);
-
-            if (!controller.LastCommand.HasValue)
-            {
-                return;
-            }
-
-            if (controller.LastProjection.HasValue &&
-                controller.LastControlReferenceSpeedMetersPerSecond.HasValue)
-            {
-                var projection =
-                    controller.LastProjection.Value;
-                recorder.UpdateControlReference(
-                    projection.ArcLengthMeters,
-                    controller.LastControlReferenceSpeedMetersPerSecond.Value,
-                    projection.LateralErrorMeters,
-                    projection.HeadingErrorRadians,
-                    projection.DistanceToTrajectoryMeters,
-                    projection.RemainingDistanceMeters,
-                    controller.LastCurvaturePreviewDistanceMeters ?? 0.0,
-                    controller.LastFeedforwardCurvaturePerMeter ??
-                    projection.ReferencePoint.CurvaturePerMeter);
-            }
-
-            var requestedCommand =
-                controller.LastRequestedCommand ??
-                controller.LastCommand.Value;
-            var command = controller.LastCommand.Value;
-            recorder.UpdateGcpCommand(
-                requestedCommand.FrontAngleRadians,
-                requestedCommand.RearAngleRadians,
-                command.FrontAngleRadians,
-                command.RearAngleRadians);
-            var curvaturePerMeter = Math.Tan(
-                command.FrontAngleRadians) /
-                controlPointRadiusMeters;
-            var angularSpeedRadiansPerSecond =
-                command.SpeedMetersPerSecond *
-                curvaturePerMeter;
-
-            recorder.UpdateCommand(
-                (float)command.SpeedMetersPerSecond,
-                (float)angularSpeedRadiansPerSecond);
-        }
-
-        /// <summary>
-        /// 将同一周期的Detour速度和轮速解算速度写入实验记录器。
-        /// </summary>
-        private static void UpdateVelocityDiagnostics(
-            TrackingExperimentRecorder recorder,
-            WheelFeedbackVehicleStateProvider stateProvider)
-        {
-            if (stateProvider == null ||
-                !stateProvider.TryGetLatestVelocityDiagnostics(
-                    out var detourBodyVx,
-                    out var detourVelocityValid,
-                    out var rawWheelBodyVx,
-                    out var filteredWheelBodyVx,
-                    out var rawWheelBodyVy,
-                    out var filteredWheelBodyVy,
-                    out var wheelVelocityValid))
-            {
-                return;
-            }
-
-            recorder.UpdateVelocityDiagnostics(
-                detourBodyVx,
-                detourVelocityValid,
-                rawWheelBodyVx,
-                filteredWheelBodyVx,
-                rawWheelBodyVy,
-                filteredWheelBodyVy,
-                wheelVelocityValid);
-        }
-
-        /// <summary>
-        /// 将Shared世界坐标系米制位姿转换为Clumsy绘图和记录器使用的毫米坐标。
-        /// </summary>
-        private static Vector2 ToMillimeterVector(
-            Pose2D poseInWorld)
-        {
-            return new Vector2(
-                (float)(
-                    poseInWorld.XMeters *
-                    MillimetersPerMeter),
-                (float)(
-                    poseInWorld.YMeters *
-                    MillimetersPerMeter));
-        }
-    }
-
-    /// <summary>
-    /// 将舵轮准备到车体左前45°，跟踪直线—左半圆—直线轨迹，并在停车后恢复车头方向。
-    /// </summary>
-    [MovementTest(name = "新版控制器：45°蟹行直线-左半圆-直线轨迹跟踪")]
-    public sealed class NewControllerCrab45StraightSemicircleStraightTest
-        : NewControllerStraightSemicircleStraightTest
-    {
-        /// <summary>
-        /// 使用车体左前45°作为组合轨迹的固定运动方向。
-        /// </summary>
-        protected override double MotionDirectionInBodyRadians =>
-            Math.PI / 4.0;
-
-        /// <summary>
-        /// 只用45°定义参考轨迹，底盘β由整段轨迹自动推导并检查一致性。
-        /// </summary>
-        protected override bool ResolveMotionDirectionFromTrajectory =>
-            true;
-
-        /// <summary>
-        /// 蟹行组合轨迹正常完成后主动将四个舵轮恢复到车头方向。
-        /// </summary>
-        protected override bool ReturnWheelsForwardAfterCompletion =>
-            true;
-
-        /// <summary>
-        /// 将45°蟹行组合实验与普通组合轨迹实验的CSV名称明确区分。
-        /// </summary>
-        protected override string ExperimentTrajectoryBaseName =>
-            "ProfiledCrab45StraightSmoothLeftTurnStraight";
-    }
-}

MultiWheelC/Experiments/RotationTests.cs

@@ -1,277 +0,0 @@
-
-
-using System;
-using System.Globalization;
-using System.Numerics;
-using System.Threading;
-using ClumsyCore;
-using ClumsyCore.Interfaces;
-using ClumsyCore.Pilot;
-using CommonUsage.Chassis;
-using FundamentalLib;
-using MDCSToolBox.Clumsy.Movements;
-using MDCSToolBox.Clumsy.Pilot;
-using MyParking.Shared;
-using MultiWheelC.StateEstimation;
-
-namespace MultiWheelC
-{
-    public abstract class InPlaceRotateTestBase : MovementTest
-    {
-        public float RelativeAngleDegrees; // 相对当前航向的旋转角度，逆时针为正。
-        public int TrialNumber = 1; // 重复实验编号。
-        public InPlaceRotationFeedbackMode FeedbackMode =
-            InPlaceRotationFeedbackMode.DetourAbsoluteHeading;
-
-        private DriveTask _task;
-        private TrackingExperimentRecorder _recorder;
-        private readonly string _trajectoryName;
-
-        protected InPlaceRotateTestBase(
-            float relativeAngleDegrees,
-            string trajectoryName)
-        {
-            RelativeAngleDegrees =
-                relativeAngleDegrees;
-            _trajectoryName =
-                trajectoryName;
-        }
-
-        // 从当前Detour航向开始，原地相对旋转指定角度并记录实验数据。
-        public override void Test()
-        {
-            var config = PilotDefinition.Conf;
-
-            if (float.IsNaN(RelativeAngleDegrees) ||
-                float.IsInfinity(RelativeAngleDegrees) ||
-                float.IsNaN(config.InPlaceRotateMaxSpeed) ||
-                float.IsInfinity(config.InPlaceRotateMaxSpeed) ||
-                config.InPlaceRotateMaxSpeed <= 0f ||
-                float.IsNaN(config.InPlaceRotateMinimumSpeed) ||
-                float.IsInfinity(config.InPlaceRotateMinimumSpeed) ||
-                config.InPlaceRotateMinimumSpeed <= 0f ||
-                config.InPlaceRotateMinimumSpeed >
-                    config.InPlaceRotateMaxSpeed)
-            {
-                Console.WriteLine("原地旋转测试参数无效。");
-                return;
-            }
-
-            var location = DetourInterface.getCartLocation();
-            if (double.IsNaN(location.x) ||
-                double.IsInfinity(location.x) ||
-                double.IsNaN(location.y) ||
-                double.IsInfinity(location.y) ||
-                double.IsNaN(location.th) ||
-                double.IsInfinity(location.th))
-            {
-                Console.WriteLine(
-                    "Detour当前位姿无效，取消原地旋转测试。");
-                return;
-            }
-
-            var chassis =
-                PilotDefinition.Chassis as MultiWheelChassis;
-            if (chassis == null)
-            {
-                Console.WriteLine(
-                    "当前底盘不是MultiWheelChassis，无法执行原地旋转测试。");
-                return;
-            }
-
-            var stateProvider =
-                ParkingVehicleStateProviderFactory.Create(
-                    chassis);
-            if (!stateProvider.TryGetState(out _))
-            {
-                Console.WriteLine(
-                    "无法读取原地旋转起点状态：" +
-                    stateProvider.LastFailureReason);
-                return;
-            }
-
-            var rotationCenter =
-                new Vector2((float)location.x, (float)location.y);
-            var targetWorldAngle =
-                (float)AngleMath.NormalizeDegrees(
-                    location.th + RelativeAngleDegrees);
-            var movementAngleTarget =
-                FeedbackMode ==
-                    InPlaceRotationFeedbackMode
-                        .RelativeWheelOdometry
-                    ? RelativeAngleDegrees
-                    : targetWorldAngle;
-
-            Console.WriteLine(
-                "原地自转实际参数：" +
-                $"Kp={config.InPlaceRotateKp:F3}，" +
-                $"Ki={config.InPlaceRotateKi:F3}，" +
-                $"Kd={config.InPlaceRotateKd:F3}，" +
-                $"到位误差={config.InPlaceRotateArriveDeg:F2}°，" +
-                $"最小角速度={config.InPlaceRotateMinimumSpeed:F2}°/s，" +
-                $"最大角速度={config.InPlaceRotateMaxSpeed:F2}°/s，" +
-                $"角加速度={config.InPlaceRotateAcc:F2}°/s²，" +
-                $"舵轮到位误差={config.InPlaceRotateWheelAlignDeg:F2}°，" +
-                $"旋转超时={config.InPlaceRotateTimeoutSec:F1}s；" +
-                $"起点航向={location.th:F2}°，" +
-                $"目标航向={targetWorldAngle:F2}°，" +
-                $"反馈模式={FeedbackMode}。");
-            Console.WriteLine(
-                "原地自转CSV保存目录：" +
-                TrackingExperimentRecorder.DefaultOutputDirectory);
-
-            _recorder = new TrackingExperimentRecorder(
-                controllerName:
-                    FeedbackMode ==
-                        InPlaceRotationFeedbackMode
-                            .RelativeWheelOdometry
-                        ? "InPlaceRotateWheelOdometry"
-                        : "InPlaceRotateFilteredPID",
-                trajectoryName: _trajectoryName,
-                trialNumber: TrialNumber,
-                referenceStart: rotationCenter,
-                referenceEnd: rotationCenter,
-                referenceSpeed: 0f,
-                referenceAngularSpeed:
-                    (float)AngleMath.DegreesToRadians(
-                        config.InPlaceRotateMaxSpeed),
-                diagnosticChassis: chassis,
-                diagnosticStateProvider: stateProvider);
-            _recorder.Start();
-
-            try
-            {
-                _task = new DriveTask(
-                    new MultiWheelRotateInPlace
-                    {
-                        AngleTarget = movementAngleTarget,
-                        FeedbackMode = FeedbackMode,
-                        Chassis = chassis,
-                        StateProvider = stateProvider,
-                        CommandAngularSpeedObserver =
-                            commandAngularSpeed =>
-                                _recorder?.UpdateCommand(
-                                    0f,
-                                    (float)AngleMath.DegreesToRadians(
-                                        commandAngularSpeed))
-                    }.Get());
-
-                _task.Wait();
-
-                // 保留少量停止后的样本，用于观察角速度是否回到零。
-                Thread.Sleep(300);
-            }
-            finally
-            {
-                _task?.Stop();
-                _recorder?.UpdateCommand(0f, 0f);
-                _recorder?.StopAndSave();
-                _task = null;
-                _recorder = null;
-            }
-        }
-
-        // 停止原地旋转并保存当前已经采集的实验数据。
-        public override void TestStop()
-        {
-            _task?.Stop();
-            _recorder?.UpdateCommand(0f, 0f);
-            _recorder?.StopAndSave();
-        }
-
-        // 读取并校验测试使用的有符号相对旋转角度。
-        protected static bool TryReadRelativeAngleDegrees(
-            out float relativeAngleDegrees)
-        {
-            var input = UI.GetInput(
-                "输入相对旋转角度（deg，正数逆时针，负数顺时针，范围-180到180之间）：");
-
-            if ((!float.TryParse(
-                     input,
-                     NumberStyles.Float,
-                     CultureInfo.CurrentCulture,
-                     out relativeAngleDegrees) &&
-                 !float.TryParse(
-                     input,
-                     NumberStyles.Float,
-                     CultureInfo.InvariantCulture,
-                     out relativeAngleDegrees)) ||
-                float.IsNaN(relativeAngleDegrees) ||
-                float.IsInfinity(relativeAngleDegrees))
-            {
-                Console.WriteLine("旋转角度输入无效，测试已经取消。");
-                return false;
-            }
-
-            if (Math.Abs(relativeAngleDegrees) < 1e-3f)
-            {
-                Console.WriteLine("旋转角度不能为0，测试已经取消。");
-                return false;
-            }
-
-            if (Math.Abs(relativeAngleDegrees) >= 180f)
-            {
-                Console.WriteLine(
-                    "输入角度必须满足-180° < angle < 180°。");
-                return false;
-            }
-
-            return true;
-        }
-
-    }
-
-    [MovementTest(name = "SendXYThSpeed：输入角度原地自转")]
-    public sealed class TestRotateAngle :
-        InPlaceRotateTestBase
-    {
-        public TestRotateAngle()
-            : base(0f, "RotateCustomAngle")
-        {
-        }
-
-        /// <summary>
-        /// 读取相对旋转角度并按正值逆时针、负值顺时针执行原地自转。
-        /// </summary>
-        public override void Test()
-        {
-            if (!TryReadRelativeAngleDegrees(
-                    out var relativeAngleDegrees))
-            {
-                return;
-            }
-
-            RelativeAngleDegrees = relativeAngleDegrees;
-            base.Test();
-        }
-    }
-
-    [MovementTest(name = "轮组里程计：输入角度原地相对自转")]
-    public sealed class TestWheelOdometryRotateAngle :
-        InPlaceRotateTestBase
-    {
-        public TestWheelOdometryRotateAngle()
-            : base(0f, "RotateWheelOdometryCustomAngle")
-        {
-            FeedbackMode =
-                InPlaceRotationFeedbackMode
-                    .RelativeWheelOdometry;
-        }
-
-        /// <summary>
-        /// 读取相对角度并仅用滤波后的轮组角速度积分完成自转。
-        /// </summary>
-        public override void Test()
-        {
-            if (!TryReadRelativeAngleDegrees(
-                    out var relativeAngleDegrees))
-            {
-                return;
-            }
-
-            RelativeAngleDegrees = relativeAngleDegrees;
-            base.Test();
-        }
-    }
-
-}

MultiWheelC/Experiments/TestTrajectoryFactory.cs

@@ -1,656 +0,0 @@
-using System;
-using System.Collections.Generic;
-using MultiWheelC.Trajectory;
-using MyParking.Shared;
-
-namespace MultiWheelC
-{
-    /// <summary>
-    /// 为新版控制器实验生成不依赖正式规划层的简单世界坐标系参考轨迹。
-    /// </summary>
-    public static class TestTrajectoryFactory
-    {
-        private const double StraightLengthMeters = 4.0;
-
-        /// <summary>
-        /// 从给定车体中心位姿按速度符号沿车头或车尾方向生成带梯形速度规划的4m直线轨迹。
-        /// </summary>
-        public static Trajectory2D CreateStraight4Meters(
-            Pose2D startPoseInWorld,
-            double cruiseSpeedMetersPerSecond = 0.30,
-            double accelerationMetersPerSecondSquared = 0.20,
-            double decelerationMetersPerSecondSquared = 0.20,
-            double pointSpacingMeters = 0.02,
-            double motionDirectionInBodyRadians = 0.0)
-        {
-            return CreateStraight(
-                startPoseInWorld,
-                StraightLengthMeters,
-                cruiseSpeedMetersPerSecond,
-                accelerationMetersPerSecondSquared,
-                decelerationMetersPerSecondSquared,
-                pointSpacingMeters,
-                motionDirectionInBodyRadians);
-        }
-
-        /// <summary>
-        /// 从给定车体中心位姿按速度符号沿车头或车尾方向生成指定长度并在终点停车的直线轨迹。
-        /// </summary>
-        public static Trajectory2D CreateStraight(
-            Pose2D startPoseInWorld,
-            double lengthMeters,
-            double cruiseSpeedMetersPerSecond = 0.30,
-            double accelerationMetersPerSecondSquared = 0.20,
-            double decelerationMetersPerSecondSquared = 0.20,
-            double pointSpacingMeters = 0.02,
-            double motionDirectionInBodyRadians = 0.0)
-        {
-            NumericGuard.EnsureFinite(
-                startPoseInWorld,
-                nameof(startPoseInWorld));
-            NumericGuard.EnsureFinitePositive(
-                lengthMeters,
-                nameof(lengthMeters));
-            var travelDirection = GetTravelDirection(
-                cruiseSpeedMetersPerSecond,
-                nameof(cruiseSpeedMetersPerSecond));
-            NumericGuard.EnsureFinitePositive(
-                accelerationMetersPerSecondSquared,
-                nameof(accelerationMetersPerSecondSquared));
-            NumericGuard.EnsureFinitePositive(
-                decelerationMetersPerSecondSquared,
-                nameof(decelerationMetersPerSecondSquared));
-            NumericGuard.EnsureFinitePositive(
-                pointSpacingMeters,
-                nameof(pointSpacingMeters));
-            NumericGuard.EnsureFinite(
-                motionDirectionInBodyRadians,
-                nameof(motionDirectionInBodyRadians));
-
-            if (pointSpacingMeters > lengthMeters)
-            {
-                throw new ArgumentOutOfRangeException(
-                    nameof(pointSpacingMeters),
-                    "直线轨迹点间距不能大于轨迹总长度。");
-            }
-
-            var segmentCount = (int)Math.Ceiling(
-                lengthMeters /
-                pointSpacingMeters);
-            var points = new List<TrajectoryPoint>(
-                segmentCount + 1);
-            var worldMotionYawRadians =
-                startPoseInWorld.YawRadians +
-                motionDirectionInBodyRadians;
-            var directionX = travelDirection *
-                Math.Cos(worldMotionYawRadians);
-            var directionY = travelDirection *
-                Math.Sin(worldMotionYawRadians);
-
-            for (var index = 0;
-                 index <= segmentCount;
-                 index++)
-            {
-                // 均分后最后一个点严格落在指定终点，避免浮点累加越界。
-                var arcLengthMeters =
-                    lengthMeters *
-                    index /
-                    segmentCount;
-                var remainingDistanceMeters =
-                    lengthMeters -
-                    arcLengthMeters;
-                var referenceSpeedMetersPerSecond =
-                    CalculateReferenceSpeed(
-                        arcLengthMeters,
-                        remainingDistanceMeters,
-                        cruiseSpeedMetersPerSecond,
-                        accelerationMetersPerSecondSquared,
-                        decelerationMetersPerSecondSquared);
-
-                points.Add(
-                    new TrajectoryPoint(
-                        arcLengthMeters,
-                        new Pose2D(
-                            startPoseInWorld.XMeters +
-                                directionX * arcLengthMeters,
-                            startPoseInWorld.YMeters +
-                                directionY * arcLengthMeters,
-                            startPoseInWorld.YawRadians),
-                        curvaturePerMeter: 0.0,
-                        referenceSpeedMetersPerSecond:
-                            referenceSpeedMetersPerSecond));
-            }
-
-            return new Trajectory2D(points);
-        }
-
-        /// <summary>
-        /// 从当前位姿沿指定车体运动方向生成“3m直线、平滑左弯180°、3m直线”的轨迹。
-        /// </summary>
-        public static Trajectory2D CreateStraightLeftSemicircleStraight(
-            Pose2D startPoseInWorld,
-            double straightLengthMeters = 3.0,
-            double turnRadiusMeters = 2.0,
-            double curvatureTransitionLengthMeters = 0.60,
-            double straightMaximumSpeedMetersPerSecond = 0.30,
-            double semicircleMaximumSpeedMetersPerSecond = 0.25,
-            double accelerationMetersPerSecondSquared = 0.20,
-            double decelerationMetersPerSecondSquared = 0.12,
-            double pointSpacingMeters = 0.02,
-            double motionDirectionInBodyRadians = 0.0)
-        {
-            return CreateStraightSmoothLeftTurnStraight(
-                startPoseInWorld,
-                straightLengthMeters,
-                turnRadiusMeters,
-                Math.PI,
-                curvatureTransitionLengthMeters,
-                straightMaximumSpeedMetersPerSecond,
-                semicircleMaximumSpeedMetersPerSecond,
-                accelerationMetersPerSecondSquared,
-                decelerationMetersPerSecondSquared,
-                pointSpacingMeters,
-                motionDirectionInBodyRadians);
-        }
-
-        /// <summary>
-        /// 沿指定车体运动方向生成“直线、平滑左弯、直线”轨迹，并使总转角严格等于指定角度。
-        /// </summary>
-        public static Trajectory2D CreateStraightSmoothLeftTurnStraight(
-            Pose2D startPoseInWorld,
-            double straightLengthMeters,
-            double turnRadiusMeters,
-            double turnAngleRadians,
-            double curvatureTransitionLengthMeters,
-            double straightMaximumSpeedMetersPerSecond,
-            double turnMaximumSpeedMetersPerSecond,
-            double accelerationMetersPerSecondSquared,
-            double decelerationMetersPerSecondSquared,
-            double pointSpacingMeters,
-            double motionDirectionInBodyRadians = 0.0)
-        {
-            NumericGuard.EnsureFinite(
-                startPoseInWorld,
-                nameof(startPoseInWorld));
-            NumericGuard.EnsureFinitePositive(
-                straightLengthMeters,
-                nameof(straightLengthMeters));
-            NumericGuard.EnsureFinitePositive(
-                turnRadiusMeters,
-                nameof(turnRadiusMeters));
-            NumericGuard.EnsureFinitePositive(
-                turnAngleRadians,
-                nameof(turnAngleRadians));
-            NumericGuard.EnsureFinitePositive(
-                curvatureTransitionLengthMeters,
-                nameof(curvatureTransitionLengthMeters));
-            var travelDirection = GetCommonTravelDirection(
-                straightMaximumSpeedMetersPerSecond,
-                nameof(straightMaximumSpeedMetersPerSecond),
-                turnMaximumSpeedMetersPerSecond,
-                nameof(turnMaximumSpeedMetersPerSecond));
-            NumericGuard.EnsureFinitePositive(
-                accelerationMetersPerSecondSquared,
-                nameof(accelerationMetersPerSecondSquared));
-            NumericGuard.EnsureFinitePositive(
-                decelerationMetersPerSecondSquared,
-                nameof(decelerationMetersPerSecondSquared));
-            NumericGuard.EnsureFinitePositive(
-                pointSpacingMeters,
-                nameof(pointSpacingMeters));
-            NumericGuard.EnsureFinite(
-                motionDirectionInBodyRadians,
-                nameof(motionDirectionInBodyRadians));
-
-            if (turnAngleRadians > 2.0 * Math.PI)
-            {
-                throw new ArgumentOutOfRangeException(
-                    nameof(turnAngleRadians),
-                    "单段平滑左转角度不能大于2π。");
-            }
-
-            var nominalTurnArcLengthMeters =
-                turnAngleRadians * turnRadiusMeters;
-            var constantCurvatureLengthMeters =
-                nominalTurnArcLengthMeters -
-                curvatureTransitionLengthMeters;
-
-            if (constantCurvatureLengthMeters <= 0.0)
-            {
-                throw new ArgumentOutOfRangeException(
-                    nameof(curvatureTransitionLengthMeters),
-                    "曲率过渡段长度必须小于指定转角对应的圆弧长度。");
-            }
-
-            // 两段平滑过渡的平均曲率均为最大曲率的一半；
-            // 将等曲率段缩短一个过渡长度后，总曲率积分仍严格等于指定转角。
-            var turnLengthMeters =
-                2.0 * curvatureTransitionLengthMeters +
-                constantCurvatureLengthMeters;
-            var turnStartArcLengthMeters =
-                straightLengthMeters;
-            var turnEndArcLengthMeters =
-                straightLengthMeters +
-                turnLengthMeters;
-            var maximumCurvaturePerMeter =
-                1.0 / turnRadiusMeters;
-
-            var sampleArcLengths =
-                BuildCompositeArcLengthSamples(
-                    straightLengthMeters,
-                    curvatureTransitionLengthMeters,
-                    constantCurvatureLengthMeters,
-                    pointSpacingMeters);
-            var speedLimits = new double[
-                sampleArcLengths.Count];
-            var referenceSpeeds = new double[
-                sampleArcLengths.Count];
-
-            for (var index = 0;
-                 index < sampleArcLengths.Count;
-                 index++)
-            {
-                var arcLengthMeters =
-                    sampleArcLengths[index];
-
-                // 整个转弯及两侧曲率过渡段采用转弯限速。
-                speedLimits[index] =
-                    arcLengthMeters >=
-                        turnStartArcLengthMeters &&
-                    arcLengthMeters <=
-                        turnEndArcLengthMeters
-                        ? Math.Abs(
-                            turnMaximumSpeedMetersPerSecond)
-                        : Math.Abs(
-                            straightMaximumSpeedMetersPerSecond);
-            }
-
-            ApplyAccelerationAndBrakingLimits(
-                sampleArcLengths,
-                speedLimits,
-                referenceSpeeds,
-                accelerationMetersPerSecondSquared,
-                decelerationMetersPerSecondSquared);
-
-            for (var index = 0;
-                 index < referenceSpeeds.Length;
-                 index++)
-            {
-                referenceSpeeds[index] *=
-                    travelDirection;
-            }
-
-            var points = new List<TrajectoryPoint>(
-                sampleArcLengths.Count);
-            var worldMotionStartYawRadians =
-                startPoseInWorld.YawRadians +
-                motionDirectionInBodyRadians;
-            var startCos = Math.Cos(
-                worldMotionStartYawRadians);
-            var startSin = Math.Sin(
-                worldMotionStartYawRadians);
-            var localX = 0.0;
-            var localY = 0.0;
-            var localYawRadians = 0.0;
-            var previousArcLengthMeters = 0.0;
-
-            for (var index = 0;
-                 index < sampleArcLengths.Count;
-                 index++)
-            {
-                var arcLengthMeters =
-                    sampleArcLengths[index];
-                if (index > 0)
-                {
-                    var segmentLengthMeters =
-                        arcLengthMeters -
-                        previousArcLengthMeters;
-                    var segmentMiddleArcLengthMeters =
-                        (arcLengthMeters +
-                         previousArcLengthMeters) /
-                        2.0;
-                    var segmentCurvaturePerMeter =
-                        CalculateSmoothTurnCurvature(
-                            segmentMiddleArcLengthMeters -
-                                turnStartArcLengthMeters,
-                            curvatureTransitionLengthMeters,
-                            constantCurvatureLengthMeters,
-                            maximumCurvaturePerMeter);
-                    var segmentYawChangeRadians =
-                        segmentCurvaturePerMeter *
-                        segmentLengthMeters;
-
-                    if (Math.Abs(segmentCurvaturePerMeter) <=
-                        1e-12)
-                    {
-                        localX += travelDirection *
-                            Math.Cos(localYawRadians) *
-                            segmentLengthMeters;
-                        localY += travelDirection *
-                            Math.Sin(localYawRadians) *
-                            segmentLengthMeters;
-                    }
-                    else
-                    {
-                        var nextYawRadians =
-                            localYawRadians +
-                            segmentYawChangeRadians;
-                        localX += travelDirection *
-                            (Math.Sin(nextYawRadians) -
-                             Math.Sin(localYawRadians)) /
-                            segmentCurvaturePerMeter;
-                        localY += travelDirection *
-                            (Math.Cos(localYawRadians) -
-                             Math.Cos(nextYawRadians)) /
-                            segmentCurvaturePerMeter;
-                    }
-
-                    localYawRadians +=
-                        segmentYawChangeRadians;
-                }
-
-                var curvaturePerMeter =
-                    CalculateSmoothTurnCurvature(
-                        arcLengthMeters -
-                            turnStartArcLengthMeters,
-                        curvatureTransitionLengthMeters,
-                        constantCurvatureLengthMeters,
-                        maximumCurvaturePerMeter);
-
-                var worldX =
-                    startPoseInWorld.XMeters +
-                    startCos * localX -
-                    startSin * localY;
-                var worldY =
-                    startPoseInWorld.YMeters +
-                    startSin * localX +
-                    startCos * localY;
-                var worldYawRadians =
-                    AngleMath.NormalizeRadians(
-                        startPoseInWorld.YawRadians +
-                        localYawRadians);
-
-                points.Add(
-                    new TrajectoryPoint(
-                        arcLengthMeters,
-                        new Pose2D(
-                            worldX,
-                            worldY,
-                            worldYawRadians),
-                        curvaturePerMeter,
-                        referenceSpeeds[index]));
-
-                previousArcLengthMeters =
-                    arcLengthMeters;
-            }
-
-            return new Trajectory2D(points);
-        }
-
-        /// <summary>
-        /// 分别采样直线、入弯过渡、等曲率段和出弯过渡，保证所有边界均为精确轨迹点。
-        /// </summary>
-        private static List<double> BuildCompositeArcLengthSamples(
-            double straightLengthMeters,
-            double curvatureTransitionLengthMeters,
-            double constantCurvatureLengthMeters,
-            double pointSpacingMeters)
-        {
-            var samples = new List<double> { 0.0 };
-            var accumulatedArcLengthMeters = 0.0;
-
-            AppendSectionArcLengthSamples(
-                samples,
-                ref accumulatedArcLengthMeters,
-                straightLengthMeters,
-                pointSpacingMeters);
-            AppendSectionArcLengthSamples(
-                samples,
-                ref accumulatedArcLengthMeters,
-                curvatureTransitionLengthMeters,
-                pointSpacingMeters);
-            AppendSectionArcLengthSamples(
-                samples,
-                ref accumulatedArcLengthMeters,
-                constantCurvatureLengthMeters,
-                pointSpacingMeters);
-            AppendSectionArcLengthSamples(
-                samples,
-                ref accumulatedArcLengthMeters,
-                curvatureTransitionLengthMeters,
-                pointSpacingMeters);
-            AppendSectionArcLengthSamples(
-                samples,
-                ref accumulatedArcLengthMeters,
-                straightLengthMeters,
-                pointSpacingMeters);
-
-            return samples;
-        }
-
-        /// <summary>
-        /// 计算平滑左转中连续变化的参考曲率，过渡段两端的曲率变化率均为零。
-        /// </summary>
-        private static double CalculateSmoothTurnCurvature(
-            double distanceInTurnMeters,
-            double transitionLengthMeters,
-            double constantCurvatureLengthMeters,
-            double maximumCurvaturePerMeter)
-        {
-            var totalTurnLengthMeters =
-                2.0 * transitionLengthMeters +
-                constantCurvatureLengthMeters;
-
-            if (distanceInTurnMeters <= 0.0 ||
-                distanceInTurnMeters >= totalTurnLengthMeters)
-            {
-                return 0.0;
-            }
-
-            if (distanceInTurnMeters < transitionLengthMeters)
-            {
-                return maximumCurvaturePerMeter *
-                       SmoothStep01(
-                           distanceInTurnMeters /
-                           transitionLengthMeters);
-            }
-
-            var exitTransitionStartMeters =
-                transitionLengthMeters +
-                constantCurvatureLengthMeters;
-            if (distanceInTurnMeters <=
-                exitTransitionStartMeters)
-            {
-                return maximumCurvaturePerMeter;
-            }
-
-            var exitRatio =
-                (distanceInTurnMeters -
-                 exitTransitionStartMeters) /
-                transitionLengthMeters;
-            return maximumCurvaturePerMeter *
-                   (1.0 - SmoothStep01(exitRatio));
-        }
-
-        /// <summary>
-        /// 将零到一的比例转换为两端一阶导数均为零的三次平滑比例。
-        /// </summary>
-        private static double SmoothStep01(double ratio)
-        {
-            var limitedRatio = Math.Max(
-                0.0,
-                Math.Min(1.0, ratio));
-            return limitedRatio *
-                   limitedRatio *
-                   (3.0 - 2.0 * limitedRatio);
-        }
-
-        /// <summary>
-        /// 将一段指定长度的轨迹追加为均匀弧长采样，并使最后一个点严格落在该段终点。
-        /// </summary>
-        private static void AppendSectionArcLengthSamples(
-            ICollection<double> samples,
-            ref double accumulatedArcLengthMeters,
-            double sectionLengthMeters,
-            double pointSpacingMeters)
-        {
-            var sectionStartArcLengthMeters =
-                accumulatedArcLengthMeters;
-            var segmentCount = (int)Math.Ceiling(
-                sectionLengthMeters /
-                pointSpacingMeters);
-
-            for (var index = 1;
-                 index <= segmentCount;
-                 index++)
-            {
-                samples.Add(
-                    sectionStartArcLengthMeters +
-                    sectionLengthMeters *
-                    index /
-                    segmentCount);
-            }
-
-            accumulatedArcLengthMeters =
-                sectionStartArcLengthMeters +
-                sectionLengthMeters;
-        }
-
-        /// <summary>
-        /// 对逐点速度幅值上限执行前向加速约束和反向制动约束，生成连续可执行的空间速度曲线。
-        /// </summary>
-        private static void ApplyAccelerationAndBrakingLimits(
-            IReadOnlyList<double> arcLengthsMeters,
-            IReadOnlyList<double> speedLimitsMetersPerSecond,
-            double[] referenceSpeedsMetersPerSecond,
-            double accelerationMetersPerSecondSquared,
-            double decelerationMetersPerSecondSquared)
-        {
-            referenceSpeedsMetersPerSecond[0] = 0.0;
-
-            for (var index = 1;
-                 index < arcLengthsMeters.Count;
-                 index++)
-            {
-                var segmentLengthMeters =
-                    arcLengthsMeters[index] -
-                    arcLengthsMeters[index - 1];
-                var accelerationLimitedSpeed = Math.Sqrt(
-                    referenceSpeedsMetersPerSecond[index - 1] *
-                    referenceSpeedsMetersPerSecond[index - 1] +
-                    2.0 *
-                    accelerationMetersPerSecondSquared *
-                    segmentLengthMeters);
-
-                referenceSpeedsMetersPerSecond[index] =
-                    Math.Min(
-                        speedLimitsMetersPerSecond[index],
-                        accelerationLimitedSpeed);
-            }
-
-            var finalIndex =
-                referenceSpeedsMetersPerSecond.Length - 1;
-            referenceSpeedsMetersPerSecond[finalIndex] = 0.0;
-
-            for (var index = finalIndex - 1;
-                 index >= 0;
-                 index--)
-            {
-                var segmentLengthMeters =
-                    arcLengthsMeters[index + 1] -
-                    arcLengthsMeters[index];
-                var brakingLimitedSpeed = Math.Sqrt(
-                    referenceSpeedsMetersPerSecond[index + 1] *
-                    referenceSpeedsMetersPerSecond[index + 1] +
-                    2.0 *
-                    decelerationMetersPerSecondSquared *
-                    segmentLengthMeters);
-
-                referenceSpeedsMetersPerSecond[index] =
-                    Math.Min(
-                        referenceSpeedsMetersPerSecond[index],
-                        brakingLimitedSpeed);
-            }
-        }
-
-        /// <summary>
-        /// 根据起步、巡航和制动能力计算指定弧长位置允许的有符号参考速度。
-        /// </summary>
-        private static double CalculateReferenceSpeed(
-            double arcLengthMeters,
-            double remainingDistanceMeters,
-            double cruiseSpeedMetersPerSecond,
-            double accelerationMetersPerSecondSquared,
-            double decelerationMetersPerSecondSquared)
-        {
-            // 由v²=2as分别得到从静止起步和到终点静止允许的速度上限。
-            var accelerationLimitedSpeed = Math.Sqrt(
-                2.0 *
-                accelerationMetersPerSecondSquared *
-                Math.Max(0.0, arcLengthMeters));
-            var brakingLimitedSpeed = Math.Sqrt(
-                2.0 *
-                decelerationMetersPerSecondSquared *
-                Math.Max(0.0, remainingDistanceMeters));
-
-            var travelDirection = GetTravelDirection(
-                cruiseSpeedMetersPerSecond,
-                nameof(cruiseSpeedMetersPerSecond));
-            var speedMagnitude = Math.Min(
-                Math.Abs(cruiseSpeedMetersPerSecond),
-                Math.Min(
-                    accelerationLimitedSpeed,
-                    brakingLimitedSpeed));
-
-            return travelDirection * speedMagnitude;
-        }
-
-        /// <summary>
-        /// 获取非零有符号速度表示的前进或倒车方向。
-        /// </summary>
-        private static double GetTravelDirection(
-            double signedSpeedMetersPerSecond,
-            string parameterName)
-        {
-            NumericGuard.EnsureFinite(
-                signedSpeedMetersPerSecond,
-                parameterName);
-
-            if (signedSpeedMetersPerSecond == 0.0)
-            {
-                throw new ArgumentOutOfRangeException(
-                    parameterName,
-                    "测试轨迹的最大速度不能为零；正值表示前进，负值表示倒车。");
-            }
-
-            return Math.Sign(
-                signedSpeedMetersPerSecond);
-        }
-
-        /// <summary>
-        /// 确保直线段和转弯段速度使用相同的前进或倒车方向。
-        /// </summary>
-        private static double GetCommonTravelDirection(
-            double firstSpeedMetersPerSecond,
-            string firstParameterName,
-            double secondSpeedMetersPerSecond,
-            string secondParameterName)
-        {
-            var firstDirection = GetTravelDirection(
-                firstSpeedMetersPerSecond,
-                firstParameterName);
-            var secondDirection = GetTravelDirection(
-                secondSpeedMetersPerSecond,
-                secondParameterName);
-
-            if (firstDirection != secondDirection)
-            {
-                throw new ArgumentException(
-                    "同一条测试轨迹的直线段和转弯段速度必须同号，不能在运动中直接切换前进与倒车方向。",
-                    firstParameterName);
-            }
-
-            return firstDirection;
-        }
-    }
-}