优化差速转舵并增加纵向辨识测试

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

.codex/skills/commit/SKILL.md

@@ -1,77 +0,0 @@
----
-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

@@ -1,92 +0,0 @@
----
-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,90 +1,56 @@
-##################################################
-# Visual Studio
-##################################################
+# .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和用户配置
 .vs/
-**/.vs/
-
-# 用户配置
+.idea/
+.vscode/
 *.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
+*.sqlite3
+
+# 操作系统生成文件
+Thumbs.db
+Desktop.ini
+.DS_Store
+
+# 不要全局忽略*.dll：MedullaAdapter/ref和MultiWheelC/ref中的宿主依赖需要保留。

AGENTS.md

@@ -0,0 +1,87 @@
+# 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

@@ -1,21 +0,0 @@
-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

@@ -1,45 +0,0 @@
-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/MovementTests.cs

@@ -1,126 +0,0 @@
-using ClumsyCore;
-using ClumsyCore.DTools;
-using ClumsyCore.Interfaces;
-using ClumsyCore.Pilot;
-using MDCSToolBox.Commons.Controllers;
-using System;
-using System.Numerics;
-
-namespace MultiWheelC
-{
-    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;
-                }
-            }
-        }
-    }
-}

ClumsyPilot/Movements.cs

@@ -1,128 +0,0 @@
-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;
-
-namespace MultiWheelC
-{
-    public class DstTracker : MovementDefinition
-    {
-        public Vector2 Src;
-        public Vector2 Dst;
-        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().Get();
-                var linePath = new LineTrack(Src, Dst)
-                {
-                    CarDirectionBias = CarDirectionBias,
-                    Speed = PilotDefinition.Conf.DstTrackerMaxSpeed
-                };
-                tracker.AddTrack(linePath);
-                task = new DriveTask(tracker.Track());
-                task.Wait();
-                yield return false;
-            }
-            finally
-            {
-                task?.Stop();
-                chassis.SendXYThSpeed(0f, 0f, 0f);
-            }
-        }
-
-    }
-
-    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 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;
-
-        // 将角度归一化到零到三百六十度范围内。
-        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}");
-                    Chassis.SendXYThSpeed(0, 0, s);
-                    if (thPid.IsArrived()) break;
-                    yield return true;
-                }
-
-                Console.WriteLine($"final rotate to {targetAngle}");
-            }
-            finally
-            {
-                Chassis.SendXYThSpeed(0, 0, 0);
-            }
-        }
-    }
-
-
-}

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

@@ -1,84 +0,0 @@
-{
-  "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

@@ -1,16 +0,0 @@
-<?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

@@ -1,6 +0,0 @@
-<?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

@@ -1,4 +0,0 @@
-// <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

@@ -1,22 +0,0 @@
-//------------------------------------------------------------------------------
-// <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+580a936a830dcb7a25ef327cf553341405264033")]
-[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

@@ -1 +0,0 @@
-038d57c5b1714403d308c9343b385ef762951607b63a9fb275f4d7d2b8fd29cb

ClumsyPilot/obj/Debug/ClumsyPilot.GeneratedMSBuildEditorConfig.editorconfig

@@ -1,8 +0,0 @@
-is_global = true
-build_property.RootNamespace = MultiWheelC
-build_property.ProjectDir = d:\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

@@ -1 +0,0 @@
-3920568398d269aea7b71fb4581ad111777c15ca3f2f2bb272ee9e0a989215c4

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

@@ -1,34 +0,0 @@
-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

@@ -1,341 +0,0 @@
-{
-  "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

@@ -1,13 +0,0 @@
-{
-  "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/.gitattributes

@@ -0,0 +1,2 @@
+*.cs text eol=crlf
+.gitattributes text eol=lf

CommonUsage-MultiVehicleSync/commonusage/.gitignore

@@ -0,0 +1,362 @@
+## Ignore Visual Studio temporary files, build results, and
+## files generated by popular Visual Studio add-ons.
+##
+## Get latest from https://github.com/github/gitignore/blob/master/VisualStudio.gitignore
+
+# User-specific files
+*.rsuser
+*.suo
+*.user
+*.userosscache
+*.sln.docstates
+
+# User-specific files (MonoDevelop/Xamarin Studio)
+*.userprefs
+
+# Mono auto generated files
+mono_crash.*
+
+# Build results
+[Dd]ebug/
+[Dd]ebugPublic/
+[Rr]elease/
+[Rr]eleases/
+x64/
+x86/
+[Ww][Ii][Nn]32/
+[Aa][Rr][Mm]/
+[Aa][Rr][Mm]64/
+bld/
+[Bb]in/
+[Oo]bj/
+[Ll]og/
+[Ll]ogs/
+
+# Visual Studio 2015/2017 cache/options directory
+.vs/
+# Uncomment if you have tasks that create the project's static files in wwwroot
+#wwwroot/
+
+# Visual Studio 2017 auto generated files
+Generated\ Files/
+
+# MSTest test Results
+[Tt]est[Rr]esult*/
+[Bb]uild[Ll]og.*
+
+# NUnit
+*.VisualState.xml
+TestResult.xml
+nunit-*.xml
+
+# Build Results of an ATL Project
+[Dd]ebugPS/
+[Rr]eleasePS/
+dlldata.c
+
+# Benchmark Results
+BenchmarkDotNet.Artifacts/
+
+# .NET Core
+project.lock.json
+project.fragment.lock.json
+artifacts/
+
+# ASP.NET Scaffolding
+ScaffoldingReadMe.txt
+
+# StyleCop
+StyleCopReport.xml
+
+# Files built by Visual Studio
+*_i.c
+*_p.c
+*_h.h
+*.ilk
+*.meta
+*.obj
+*.iobj
+*.pch
+*.pdb
+*.ipdb
+*.pgc
+*.pgd
+*.rsp
+*.sbr
+*.tlb
+*.tli
+*.tlh
+*.tmp
+*.tmp_proj
+*_wpftmp.csproj
+*.log
+*.vspscc
+*.vssscc
+.builds
+*.pidb
+*.svclog
+*.scc
+
+# Chutzpah Test files
+_Chutzpah*
+
+# Visual C++ cache files
+ipch/
+*.aps
+*.ncb
+*.opendb
+*.opensdf
+*.sdf
+*.cachefile
+*.VC.db
+*.VC.VC.opendb
+
+# Visual Studio profiler
+*.psess
+*.vsp
+*.vspx
+*.sap
+
+# Visual Studio Trace Files
+*.e2e
+
+# TFS 2012 Local Workspace
+$tf/
+
+# Guidance Automation Toolkit
+*.gpState
+
+# ReSharper is a .NET coding add-in
+_ReSharper*/
+*.[Rr]e[Ss]harper
+*.DotSettings.user
+
+# TeamCity is a build add-in
+_TeamCity*
+
+# DotCover is a Code Coverage Tool
+*.dotCover
+
+# AxoCover is a Code Coverage Tool
+.axoCover/*
+!.axoCover/settings.json
+
+# Coverlet is a free, cross platform Code Coverage Tool
+coverage*.json
+coverage*.xml
+coverage*.info
+
+# Visual Studio code coverage results
+*.coverage
+*.coveragexml
+
+# NCrunch
+_NCrunch_*
+.*crunch*.local.xml
+nCrunchTemp_*
+
+# MightyMoose
+*.mm.*
+AutoTest.Net/
+
+# Web workbench (sass)
+.sass-cache/
+
+# Installshield output folder
+[Ee]xpress/
+
+# DocProject is a documentation generator add-in
+DocProject/buildhelp/
+DocProject/Help/*.HxT
+DocProject/Help/*.HxC
+DocProject/Help/*.hhc
+DocProject/Help/*.hhk
+DocProject/Help/*.hhp
+DocProject/Help/Html2
+DocProject/Help/html
+
+# Click-Once directory
+publish/
+
+# Publish Web Output
+*.[Pp]ublish.xml
+*.azurePubxml
+# Note: Comment the next line if you want to checkin your web deploy settings,
+# but database connection strings (with potential passwords) will be unencrypted
+*.pubxml
+*.publishproj
+
+# Microsoft Azure Web App publish settings. Comment the next line if you want to
+# checkin your Azure Web App publish settings, but sensitive information contained
+# in these scripts will be unencrypted
+PublishScripts/
+
+# NuGet Packages
+*.nupkg
+# NuGet Symbol Packages
+*.snupkg
+# The packages folder can be ignored because of Package Restore
+**/[Pp]ackages/*
+# except build/, which is used as an MSBuild target.
+!**/[Pp]ackages/build/
+# Uncomment if necessary however generally it will be regenerated when needed
+#!**/[Pp]ackages/repositories.config
+# NuGet v3's project.json files produces more ignorable files
+*.nuget.props
+*.nuget.targets
+
+# Microsoft Azure Build Output
+csx/
+*.build.csdef
+
+# Microsoft Azure Emulator
+ecf/
+rcf/
+
+# Windows Store app package directories and files
+AppPackages/
+BundleArtifacts/
+Package.StoreAssociation.xml
+_pkginfo.txt
+*.appx
+*.appxbundle
+*.appxupload
+
+# Visual Studio cache files
+# files ending in .cache can be ignored
+*.[Cc]ache
+# but keep track of directories ending in .cache
+!?*.[Cc]ache/
+
+# Others
+ClientBin/
+~$*
+*~
+*.dbmdl
+*.dbproj.schemaview
+*.jfm
+*.pfx
+*.publishsettings
+orleans.codegen.cs
+
+# Including strong name files can present a security risk
+# (https://github.com/github/gitignore/pull/2483#issue-259490424)
+#*.snk
+
+# Since there are multiple workflows, uncomment next line to ignore bower_components
+# (https://github.com/github/gitignore/pull/1529#issuecomment-104372622)
+#bower_components/
+
+# RIA/Silverlight projects
+Generated_Code/
+
+# Backup & report files from converting an old project file
+# to a newer Visual Studio version. Backup files are not needed,
+# because we have git ;-)
+_UpgradeReport_Files/
+Backup*/
+UpgradeLog*.XML
+UpgradeLog*.htm
+ServiceFabricBackup/
+*.rptproj.bak
+
+# SQL Server files
+*.mdf
+*.ldf
+*.ndf
+
+# Business Intelligence projects
+*.rdl.data
+*.bim.layout
+*.bim_*.settings
+*.rptproj.rsuser
+*- [Bb]ackup.rdl
+*- [Bb]ackup ([0-9]).rdl
+*- [Bb]ackup ([0-9][0-9]).rdl
+
+# Microsoft Fakes
+FakesAssemblies/
+
+# GhostDoc plugin setting file
+*.GhostDoc.xml
+
+# Node.js Tools for Visual Studio
+.ntvs_analysis.dat
+node_modules/
+
+# Visual Studio 6 build log
+*.plg
+
+# Visual Studio 6 workspace options file
+*.opt
+
+# Visual Studio 6 auto-generated workspace file (contains which files were open etc.)
+*.vbw
+
+# Visual Studio LightSwitch build output
+**/*.HTMLClient/GeneratedArtifacts
+**/*.DesktopClient/GeneratedArtifacts
+**/*.DesktopClient/ModelManifest.xml
+**/*.Server/GeneratedArtifacts
+**/*.Server/ModelManifest.xml
+_Pvt_Extensions
+
+# Paket dependency manager
+.paket/paket.exe
+paket-files/
+
+# FAKE - F# Make
+.fake/
+
+# CodeRush personal settings
+.cr/personal
+
+# Python Tools for Visual Studio (PTVS)
+__pycache__/
+*.pyc
+
+# Cake - Uncomment if you are using it
+# tools/**
+# !tools/packages.config
+
+# Tabs Studio
+*.tss
+
+# Telerik's JustMock configuration file
+*.jmconfig
+
+# BizTalk build output
+*.btp.cs
+*.btm.cs
+*.odx.cs
+*.xsd.cs
+
+# OpenCover UI analysis results
+OpenCover/
+
+# Azure Stream Analytics local run output
+ASALocalRun/
+
+# MSBuild Binary and Structured Log
+*.binlog
+
+# NVidia Nsight GPU debugger configuration file
+*.nvuser
+
+# MFractors (Xamarin productivity tool) working folder
+.mfractor/
+
+# Local History for Visual Studio
+.localhistory/
+
+# BeatPulse healthcheck temp database
+healthchecksdb
+
+# Backup folder for Package Reference Convert tool in Visual Studio 2017
+MigrationBackup/
+
+# Ionide (cross platform F# VS Code tools) working folder
+.ionide/
+
+# Fody - auto-generated XML schema
+FodyWeavers.xsd

CommonUsage-MultiVehicleSync/commonusage/Chassis/AbstractChassis.cs

@@ -0,0 +1,126 @@
+using System;
+using System.Collections.Generic;
+using System.Diagnostics;
+using System.IO;
+using System.Numerics;
+using System.Security.Cryptography.X509Certificates;
+using System.Text;
+using FundamentalLib;
+using Newtonsoft.Json;
+
+namespace CommonUsage.Chassis
+{
+    public abstract class AbstractChassis
+    {
+	    protected AbstractChassis()
+	    {
+		    Valid = false;
+	    }
+
+	    public abstract void Initialize();
+
+        public abstract void Visualize();
+
+	    public abstract void AfterDirectionChanged();
+
+        /// <summary>
+        /// 当前行进方向。
+        /// </summary>
+        [Obsolete]
+        public float DirectionAngle
+        {
+	        get => _originBiasTh;
+	        set
+	        {
+                _originBiasTh = value;
+				if (_originBiasTh != _lastDirectionAngle) AfterDirectionChanged();
+                _lastDirectionAngle = _originBiasTh;
+	        }
+        }
+
+        protected float _originBiasX = 0f, _originBiasY = 0f, _originBiasTh;
+
+        public Vector3 GetOriginBias()
+        {
+            return new Vector3(_originBiasX, _originBiasY, _originBiasTh);
+        }
+
+        public class CarSpeed
+        {
+            public float Vx,Vy,Vw;
+        }
+
+        public abstract CarSpeed GetCarSpeed(bool isActual = false);
+        public List<GeometricControlPoint> GetGeometricControlPoints()
+        {
+	        return GeometricControlPoints;
+        }
+
+        public void ComputeWheelsGeometrically(float speed)
+        {
+            // 打印调用位置信息
+            var stackTrace = new StackTrace(true);
+            var callerFrame = stackTrace.GetFrame(1); // 获取调用者的帧
+            if (callerFrame != null)
+            {
+                var fileName = callerFrame.GetFileName();
+                var lineNumber = callerFrame.GetFileLineNumber();
+                DLog.Log($"s:{speed:0.000} from {fileName} ln.{lineNumber}", $"WheelComputeCaller");
+            }
+
+            DefineGeometricWheelComputation(speed);
+        }
+
+        protected abstract void DefineGeometricWheelComputation(float speed);
+
+        public void DriveStop()
+        {
+            PredefinedDriveStop();
+            CustomDriveStop?.Invoke();
+        }
+
+        public abstract void PredefinedDriveStop();
+
+        public Action CustomDriveStop;
+
+        public abstract bool ComputeRotateWheels(float rotSpeed);
+
+        public abstract float CalculateTurningSpeedDecayFac(float turn);
+
+        public enum ChassisState
+        {
+	        Standby,
+            Running,
+            AbnormalFeedback,
+            ExceedMotionAbility,
+        }
+
+        protected ChassisState State;
+
+        protected string StateDescription;
+
+        public (ChassisState State, string Description) GetChassisState()
+        {
+	        return (State, StateDescription);
+        }
+
+        public bool Debug = false;
+        public float AccPerSecond = 0.2f;
+        public float DeAccPerSecond = 0.2f;
+        public float MaxSpeed = 1; // m/s
+        public float MinTurnSpeedFac = 0.5f;
+        public float MaxTurnThreshold = 90f;
+
+        public float GcpThetaPerSecond = 10f;
+
+        public DateTime LastMoveTime = DateTime.MinValue;
+
+		protected bool Valid = false;
+		protected List<GeometricControlPoint> GeometricControlPoints = new();
+		protected bool RotatingActive = false;
+		protected bool GoingActive = false;
+		protected bool GoingWheelAligned = false;
+
+		private float _lastDirectionAngle = 0;
+    }
+}

CommonUsage-MultiVehicleSync/commonusage/Chassis/DiffSteerWheel.cs

@@ -0,0 +1,47 @@
+using System;
+using System.Collections.Generic;
+using System.Numerics;
+using System.Text;
+
+namespace CommonUsage.Chassis
+{
+    public class DiffSteerWheel:SteerWheel
+    {
+        public DiffSteerWheel(float wheelDistance,Vector2 position, float angleLowerLimit, float angleUpperLimit, Action<float> speedWriter,
+            Func<float> speedReader, Action<float> angleWriter, Func<float> angleReader, Action<float> leftSpeedWriter, Action<float> rightSpeedWriter,
+            float angleLimitMarginDeg = 15f) : base(position,
+            angleLowerLimit, angleUpperLimit, speedWriter, speedReader, angleWriter, angleReader, angleLimitMarginDeg)
+        {
+            _leftSpeedWriter = leftSpeedWriter;
+            _rightSpeedWriter = rightSpeedWriter;
+            WheelDistance = wheelDistance;
+        }
+
+        public float GetLeftSendSpeed()
+        {
+            return _leftSendSpeed;
+        }
+
+        public float GetRightSendSpeed()
+        {
+            return _rightSendSpeed;
+        }
+
+        public void WriteLeftSpeed(float speed)
+        {
+            _leftSpeedWriter(_leftSendSpeed = speed);
+        }
+
+        public void WriteRightSpeed(float speed)
+        {
+            _rightSpeedWriter(_rightSendSpeed = speed);
+        }
+
+        public float WheelDistance;
+        private readonly Action<float> _leftSpeedWriter;
+        private readonly Action<float> _rightSpeedWriter;
+
+        private float _leftSendSpeed;
+        private float _rightSendSpeed;
+    }
+}

CommonUsage-MultiVehicleSync/commonusage/Chassis/DifferentialChassis.cs

@@ -0,0 +1,170 @@
+using FundamentalLib;
+using System;
+using System.Collections.Generic;
+using System.Numerics;
+using System.Text;
+using System.Diagnostics;
+
+namespace CommonUsage.Chassis
+{
+	public class DifferentialChassis : AbstractChassis
+	{
+		public void SetLeftRightWheels(Wheel wheelL, Wheel wheelR)
+		{
+			_leftWheel = wheelL;
+			_rightWheel = wheelR;
+			_halfWheelTrack = Math.Abs(_leftWheel.Position.Y);
+		}
+
+        public override void Visualize()
+        {
+            
+        }
+
+        public override CarSpeed GetCarSpeed(bool isActual = false)
+        {
+            if (!isActual)
+            {
+                return new CarSpeed()
+                {
+                    Vx = (_speedL + _speedR) / 2f,
+                    Vw = (_speedR - _speedL) / Math.Abs(_leftWheel.Position.Y - _rightWheel.Position.Y) /
+                        (float)Math.PI * 180f * 1000f,
+                    Vy = 0
+                };
+            }
+            else
+            {
+                return new CarSpeed()
+                {
+                    Vx = GetLinearSpeed(),
+                    Vw = (_rightWheel.ReadSpeed() - _leftWheel.ReadSpeed()) /
+                        Math.Abs(_leftWheel.Position.Y - _rightWheel.Position.Y) /
+                        (float)Math.PI * 180f * 1000f,
+                    Vy = 0
+                };
+            }
+            
+        }
+
+        public (Wheel,Wheel) GetWheels()
+        {
+            return (_leftWheel, _rightWheel);
+        }
+
+		public float GetLinearSpeed()
+		{
+			return (_leftWheel.ReadSpeed() + _rightWheel.ReadSpeed()) / 2f;
+		}
+
+		public override void Initialize()
+		{
+			GeometricControlPoints.Add(new GeometricControlPoint(new Vector2(0, 0)));
+			Valid = true;
+		}
+
+		public override void AfterDirectionChanged()
+		{
+
+		}
+
+		public override void PredefinedDriveStop()
+		{
+			if (!Valid) return;
+			_sendSpeedL = _sendSpeedR = 0;
+            _speedL = _speedR = 0;
+			_leftWheel.WriteSpeed(_sendSpeedL);
+			_rightWheel.WriteSpeed(_sendSpeedR);
+			GoingActive = false;
+			RotatingActive = false;
+		}
+
+        protected override void DefineGeometricWheelComputation(float speed)
+        {
+            var now = DateTime.Now;
+            if (!GoingActive) LastMoveTime = now;
+
+            SendSpeed(speed, GeometricControlPoints[0].Theta, now - LastMoveTime);
+
+            GoingActive = true;
+            RotatingActive = false;
+        }
+
+		public override bool ComputeRotateWheels(float rotSpeed)
+		{
+			if (!RotatingActive) LastMoveTime = DateTime.Now;
+
+			SendSpeed(0, rotSpeed);
+
+			GoingActive = false;
+			RotatingActive = true;
+			return true;
+		}
+
+		public override float CalculateTurningSpeedDecayFac(float turn)
+		{
+			return 1 - Math.Min(turn, MaxTurnThreshold) / MaxTurnThreshold * MinTurnSpeedFac;
+		}
+
+		public void SendSpeed(float linearSpeed, float angularSpeed, TimeSpan? deltaTime = null)
+		{
+			var edgeLinearSpeed = (float)(angularSpeed / 180f * Math.PI * _halfWheelTrack / 1000);
+			var vl = linearSpeed - edgeLinearSpeed;
+			var vr = linearSpeed + edgeLinearSpeed;
+            _speedL = vl;
+			_speedR = vr;
+			AccumulateSpeed(vl, vr, deltaTime);
+            LastMoveTime = DateTime.Now;
+        }
+
+		private void AccumulateSpeed(float vl, float vr, TimeSpan? deltaTime = null)
+		{
+            // var dTime = (float)(deltaTime ?? DateTime.Now - LastMoveTime).TotalSeconds;
+            //
+            // var speedSignL = Math.Sign(vl - _sendSpeedL);
+            // var accL = Math.Abs(vl) > Math.Abs(_sendSpeedL) ? AccPerSecond : DeAccPerSecond;
+            // _sendSpeedL += speedSignL * Math.Min(Math.Abs(vl - _sendSpeedL), accL * dTime);
+            // _leftWheel.WriteSpeed(_sendSpeedL);
+            //
+            // var speedSignR = Math.Sign(vr - _sendSpeedR);
+            // var accR = Math.Abs(vr) > Math.Abs(_sendSpeedR) ? AccPerSecond : DeAccPerSecond;
+            // _sendSpeedR += speedSignR * Math.Min(Math.Abs(vr - _sendSpeedR), accR * dTime);
+            // _rightWheel.WriteSpeed(_sendSpeedR);
+
+            // if (Debug)
+            // 	Console.WriteLine($"DiffChassis, target:{v:0.00},send:{_sendSpeed:0.0}");
+            // var dTime = (float)(deltaTime ?? DateTime.Now - LastMoveTime).TotalSeconds;
+            var dTime = (float)(deltaTime ?? DateTime.Now - LastMoveTime).TotalSeconds;
+            float diffL = vl - _sendSpeedL;
+            float diffR = vr - _sendSpeedR;
+
+            float accL = Math.Abs(vl) > Math.Abs(_sendSpeedL) ? AccPerSecond : DeAccPerSecond;
+            float accR = Math.Abs(vr) > Math.Abs(_sendSpeedR) ? AccPerSecond : DeAccPerSecond;
+
+            float maxDeltaL = accL * dTime;
+            float maxDeltaR = accR * dTime;
+
+            float factorL = Math.Abs(diffL) > maxDeltaL ? maxDeltaL / Math.Abs(diffL) : 1.0f;
+            float factorR = Math.Abs(diffR) > maxDeltaR ? maxDeltaR / Math.Abs(diffR) : 1.0f;
+
+            float factor = Math.Min(factorL, factorR);
+
+            _sendSpeedL += diffL * factor;
+            _sendSpeedR += diffR * factor;
+
+            _leftWheel.WriteSpeed(_sendSpeedL);
+            _rightWheel.WriteSpeed(_sendSpeedR);
+            // Console.WriteLine($"DiffChassis, target:{vl:0.00},send:{_sendSpeedL:0.00} dTime：{dTime} diffL:{diffL} factor:{factor}" );
+        }
+
+        private Wheel _leftWheel;
+		private Wheel _rightWheel;
+		private float _halfWheelTrack; // millimeter
+
+		private float _sendSpeedL;
+		private float _sendSpeedR;
+		private int _direction = 1; // 1 forward, -1 backward
+        private float _speedL;
+        private float _speedR;
+    }
+}

CommonUsage-MultiVehicleSync/commonusage/Chassis/SingleSteerChassis.cs

@@ -0,0 +1,172 @@
+using System;
+using System.Collections.Generic;
+using System.Linq;
+using System.Numerics;
+using System.Text;
+using System.Diagnostics;
+using CommonUsage.Mathematics;
+using FundamentalLib;
+
+namespace CommonUsage.Chassis
+{
+	public class SingleSteerChassis : AbstractChassis
+	{
+		public void SetSteerWheel(SteerWheel wheel)
+		{
+			_steerWheel = wheel;
+		}
+
+		public SteerWheel GetSteerWheel()
+		{
+			return _steerWheel;
+		}
+
+        public override void Visualize()
+        {
+            
+        }
+
+        public override CarSpeed GetCarSpeed(bool isActual = false)
+        {
+            if (!isActual)
+            {
+                var sendAngle = _steerWheel.GetSendAngle();
+                var sendAngleRad = _steerWheel.GetSendAngle() / 180f * Math.PI;
+                // VSteer* Cos = v;
+                var vsteer = _sendSpeed / ((Math.Cos(Math.Abs(sendAngleRad)) + 0.000001));
+                var vsteerY = vsteer * Math.Sin(sendAngleRad);
+				// Console.WriteLine($"{vsteer} {vsteerY} {sendAngleRad} {_sendSpeed}");
+                return new CarSpeed()
+                {
+                    Vx = (float)(_sendSpeed * Math.Cos(Math.Abs(sendAngle) / 180f * Math.PI)),
+                    Vy = 0,
+                    Vw = (float)(_sendSpeed * Math.Sin(Math.Abs(sendAngle) / 180f * Math.PI) /
+                        Math.Abs(_steerWheel.Position.X / 1000f) / Math.PI * 180f)
+					//阿克曼
+                    // Vx = (float)(_sendSpeed),
+                    // Vy = 0,
+                    // Vw = (float)(vsteerY / Math.Abs(_steerWheel.Position.X / 1000f) / Math.PI * 180f)
+                };
+            }
+            else
+            {
+                return new CarSpeed()
+                {
+                    Vx = (float)(_steerWheel.ReadSpeed() *
+                                 Math.Cos(Math.Abs(_steerWheel.ReadAngle()) / 180f * Math.PI)),
+                    Vy = 0,
+                    Vw = (float)(_steerWheel.ReadSpeed() *
+                        Math.Sin(Math.Abs(_steerWheel.ReadAngle()) / 180f * Math.PI) /
+                        Math.Abs(_steerWheel.Position.X / 1000f) / Math.PI * 180f)
+                };
+            }
+            
+        }
+
+        public override void Initialize()
+		{
+			GeometricControlPoints = new List<GeometricControlPoint>()
+			{
+				new (_steerWheel.Position),
+				new (Vector2.Zero)
+			};
+			Valid = true;
+		}
+
+		public override void AfterDirectionChanged()
+		{
+			if (Math.Abs(CommonMath.ThDiff(0, _originBiasTh)) > 90)
+			{
+				GeometricControlPoints = new List<GeometricControlPoint>()
+				{
+					new (-_steerWheel.Position),
+				};
+				_direction = -1;
+			}
+			else
+			{
+				GeometricControlPoints = new List<GeometricControlPoint>()
+				{
+					new (_steerWheel.Position),
+				};
+				_direction = 1;
+			}
+		}
+
+		public override void PredefinedDriveStop()
+		{
+			if (!Valid) return;
+			_sendSpeed = 0;
+			_steerWheel.WriteSpeed(_sendSpeed);
+			GoingActive = false;
+			RotatingActive = false;
+		}
+
+        protected override void DefineGeometricWheelComputation(float speed)
+        {
+            var now = DateTime.Now;
+
+            if (!GoingActive)
+            {
+                LastMoveTime = now;
+                GoingWheelAligned = false;
+            }
+
+            SendSteerMotion(speed * _direction, GeometricControlPoints[0].Theta, now - LastMoveTime);
+
+            GoingActive = true;
+            RotatingActive = false;
+        }
+
+		public override bool ComputeRotateWheels(float rotSpeed)
+		{
+			if (!RotatingActive)
+            {
+                LastMoveTime = DateTime.Now;
+                GoingWheelAligned = false;
+            }
+
+			SendSteerMotion(rotSpeed, 90);
+
+			GoingActive = false;
+			RotatingActive = true;
+			return true;
+		}
+
+		public void SendSteerMotion(float speed, float theta, TimeSpan? deltaTime = null)
+		{
+			_steerWheel.WriteAngle(theta);
+
+            if (!GoingWheelAligned && Math.Abs(CommonMath.ThDiff(_steerWheel.ReadAngle(), theta)) < 1)
+                GoingWheelAligned = true;
+			if (!GoingWheelAligned) speed = 0;
+
+			var turnThresholdSpeed = CalculateTurningSpeedDecayFac(Math.Abs(theta)) * MaxSpeed;
+			AccumulateSpeed(Math.Min(turnThresholdSpeed, Math.Abs(speed)) * Math.Sign(speed), deltaTime);
+
+            LastMoveTime = DateTime.Now;
+        }
+
+		public override float CalculateTurningSpeedDecayFac(float turn)
+		{
+			return 1 - Math.Min(turn, MaxTurnThreshold) / MaxTurnThreshold * MinTurnSpeedFac;
+		}
+
+		private void AccumulateSpeed(float v, TimeSpan? deltaTime = null)
+		{
+			// _targetSpeed = v;
+			var speedSign = Math.Sign(v - _sendSpeed);
+			var acc = Math.Abs(v) > Math.Abs(_sendSpeed) ? AccPerSecond : DeAccPerSecond;
+			_sendSpeed += speedSign * Math.Min(Math.Abs(v - _sendSpeed),
+				acc * (float)(deltaTime ?? DateTime.Now - LastMoveTime).TotalSeconds);
+			_steerWheel.WriteSpeed(_sendSpeed);
+
+			if (Debug)
+				Console.WriteLine($"SingleSteer, target:{v:0.00},send:{_sendSpeed:0.0}");
+		}
+
+		private SteerWheel _steerWheel;
+		private float _sendSpeed;
+		private int _direction = 1; // 1 forward, -1 backward
+	}
+}

CommonUsage-MultiVehicleSync/commonusage/Chassis/SteerWheel.cs

@@ -0,0 +1,100 @@
+using Newtonsoft.Json;
+using System;
+using System.Collections.Generic;
+using System.Numerics;
+using System.Text;
+using CommonUsage.Mathematics;
+
+namespace CommonUsage.Chassis
+{
+	public class SteerWheel : Wheel
+	{
+		public SteerWheel(Vector2 position, float angleLowerLimit, float angleUpperLimit, Action<float> speedWriter,
+			Func<float> speedReader, Action<float> angleWriter, Func<float> angleReader, float angleLimitMarginDeg = 15f) : base(position, speedWriter,
+			speedReader)
+		{
+			_angleLowerLimit = angleLowerLimit;
+			_angleUpperLimit = angleUpperLimit;
+			_angleWriter = angleWriter;
+			_angleReader = angleReader;
+			_centerDistance = position.Length();
+            AngleLimitMarginDeg = angleLimitMarginDeg;
+		}
+
+        public float AngleLimitMarginDeg = 15f;
+
+		public bool TrySetDirection(bool allowReverse, ref float desireDirection, ref int dir)
+		{
+            if (TryNormalizeAngleInLimit(desireDirection, out var normalized))
+			{
+				dir = 1;
+                desireDirection = normalized;
+				return true;
+			}
+
+			if (!allowReverse) return false;
+
+			var oppositeTh = (float)CommonMath.RoundTh(desireDirection + 180);
+			if (TryNormalizeAngleInLimit(oppositeTh, out normalized))
+			{
+				dir = -1;
+				desireDirection = normalized;
+				return true;
+			}
+
+			dir = 0;
+			return false;
+		}
+
+        private bool TryNormalizeAngleInLimit(float angle, out float normalized)
+        {
+            var lower = CommonMath.RoundTh(_angleLowerLimit);
+            var upper = CommonMath.RoundTh(_angleUpperLimit);
+            while (upper < lower) upper += 360;
+
+            normalized = (float)CommonMath.RoundTh(angle);
+            while (normalized < lower) normalized += 360;
+            while (normalized > upper && normalized - 360 >= lower) normalized -= 360;
+
+            var margin = Math.Min(normalized - lower, upper - normalized);
+            return normalized >= lower && normalized <= upper && margin >= Math.Max(0, AngleLimitMarginDeg);
+        }
+
+		public float ReadAngle()
+		{
+			return _angleReader();
+		}
+
+		public void WriteAngle(float angle)
+        {
+            _angleWriter.Invoke(_sendAngle = Math.Max(_angleLowerLimit, Math.Min(angle, _angleUpperLimit)));
+        }
+
+        public float GetSendAngle()
+        {
+            return _sendAngle;
+        }
+
+        public float GetAngleRelativeToChassis()
+        {
+            return ZeroDirection + _sendAngle;
+        }
+
+		public float CenterDistance()
+		{
+			return _centerDistance;
+		}
+
+        public float AngleLowerLimit => _angleLowerLimit;
+
+		public float AngleUpperLimit => _angleUpperLimit;
+
+		[JsonIgnore] private readonly Action<float> _angleWriter;
+		[JsonIgnore] private readonly Func<float> _angleReader;
+
+		private float _angleLowerLimit = -90, _angleUpperLimit = 90;
+		private float _centerDistance;
+
+        public float _sendAngle = 0f;
+    }
+}

CommonUsage-MultiVehicleSync/commonusage/Chassis/Wheel.cs

@@ -0,0 +1,56 @@
+using System;
+using System.Collections.Generic;
+using System.Numerics;
+using System.Text;
+using Newtonsoft.Json;
+
+namespace CommonUsage.Chassis
+{
+    public class Wheel
+    {
+	    public Wheel(Vector2 position, Action<float> speedWriter, Func<float> speedReader)
+	    {
+		    PhysicalPosition = Position = position;
+			SpeedWriter = speedWriter;
+			SpeedReader = speedReader;
+	    }
+
+	    public void WriteSpeed(float speed)
+	    {
+		    SpeedWriter.Invoke(_sendSpeed = speed);
+        }
+
+        public float GetSendSpeed()
+        {
+            return _sendSpeed;
+        }
+
+	    public float ReadSpeed()
+	    {
+		    return SpeedReader();
+	    }
+
+        // PhysicalPosition ===(chassis transform)===> Position
+		// useful in dual agv coordination
+        public readonly Vector2 PhysicalPosition;
+        public Vector2 Position;
+
+        public float ZeroDirection = 0;
+
+        [JsonIgnore] public readonly Action<float> SpeedWriter;
+        [JsonIgnore] public readonly Func<float> SpeedReader;
+
+        public float _sendSpeed;
+    }
+
+    public class GeometricControlPoint
+    {
+	    public GeometricControlPoint(Vector2 position)
+	    {
+			Position = position;
+	    }
+
+	    public Vector2 Position;
+	    public float Theta;
+    }
+}

CommonUsage-MultiVehicleSync/commonusage/CommonUsage.csproj

@@ -0,0 +1,50 @@
+<Project Sdk="Microsoft.NET.Sdk">
+
+  <PropertyGroup>
+    <TargetFramework>netstandard2.0</TargetFramework>
+    <AssemblyName>CommonUsage</AssemblyName>
+    <RootNamespace>CommonUsage</RootNamespace>
+  </PropertyGroup>
+
+  <PropertyGroup>
+   <LangVersion>latest</LangVersion>
+   <AllowUnsafeBlocks>True</AllowUnsafeBlocks>
+  </PropertyGroup>
+
+  <PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Debug|AnyCPU'">
+    <DebugType>embedded</DebugType>
+  </PropertyGroup>
+
+  <PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Release|AnyCPU'">
+    <DebugType>embedded</DebugType>
+  </PropertyGroup>
+
+  <ItemGroup>
+    <Compile Remove="Hedingben.cs" />
+    <Compile Remove="IPCConcurrentDictionary.cs" />
+  </ItemGroup>
+
+  <ItemGroup>
+    <PackageReference Include="MQTTnet" Version="4.3.7.1207" />
+    <PackageReference Include="MQTTnet.Extensions.ManagedClient" Version="4.3.7.1207" />
+    <PackageReference Include="Newtonsoft.Json" Version="13.0.3" />
+    <PackageReference Include="System.Buffers" Version="4.5.1" />
+    <PackageReference Include="System.Numerics.Vectors" Version="4.5.0" />
+  </ItemGroup>
+
+  <ItemGroup>
+    <Reference Include="FundamentalLib">
+      <HintPath>.\ref\RefFundamentalLib.dll</HintPath>
+    </Reference>
+    <!-- <Reference Include="ClumsyCore">
+      <HintPath>..\..\MultiWheelC\ref\RefClumsyCore.dll</HintPath>
+    </Reference> -->
+  </ItemGroup>
+
+  <Target Name="CopyCommonUsageToMyParkingRef" AfterTargets="Build">
+    <MakeDir Directories="..\..\ref" />
+    <Copy SourceFiles="$(TargetPath)"
+          DestinationFolder="..\..\ref" />
+  </Target>
+
+</Project>

CommonUsage-MultiVehicleSync/commonusage/CommonUsageSln.sln

@@ -0,0 +1,25 @@
+
+Microsoft Visual Studio Solution File, Format Version 12.00
+# Visual Studio Version 17
+VisualStudioVersion = 17.5.33424.131
+MinimumVisualStudioVersion = 10.0.40219.1
+Project("{9A19103F-16F7-4668-BE54-9A1E7A4F7556}") = "CommonUsage", "CommonUsage.csproj", "{E1C5DEA8-3785-40A9-9965-E51E65BF5947}"
+EndProject
+Global
+	GlobalSection(SolutionConfigurationPlatforms) = preSolution
+		Debug|Any CPU = Debug|Any CPU
+		Release|Any CPU = Release|Any CPU
+	EndGlobalSection
+	GlobalSection(ProjectConfigurationPlatforms) = postSolution
+		{E1C5DEA8-3785-40A9-9965-E51E65BF5947}.Debug|Any CPU.ActiveCfg = Debug|Any CPU
+		{E1C5DEA8-3785-40A9-9965-E51E65BF5947}.Debug|Any CPU.Build.0 = Debug|Any CPU
+		{E1C5DEA8-3785-40A9-9965-E51E65BF5947}.Release|Any CPU.ActiveCfg = Release|Any CPU
+		{E1C5DEA8-3785-40A9-9965-E51E65BF5947}.Release|Any CPU.Build.0 = Release|Any CPU
+	EndGlobalSection
+	GlobalSection(SolutionProperties) = preSolution
+		HideSolutionNode = FALSE
+	EndGlobalSection
+	GlobalSection(ExtensibilityGlobals) = postSolution
+		SolutionGuid = {709F9C19-45DB-46AD-B70A-0E1E3C6CFB0C}
+	EndGlobalSection
+EndGlobal

CommonUsage-MultiVehicleSync/commonusage/Geometries/AbstractGeometry.cs

@@ -0,0 +1,93 @@
+using System;
+using System.Collections.Generic;
+using System.Drawing;
+using System.Numerics;
+using System.Text;
+using static CommonUsage.Geometries.CircularArc;
+
+namespace CommonUsage.Geometries
+{
+	/// <summary>
+	/// 便于直接创建几何形状并求几何形状的切点、切线等。
+	/// </summary>
+	public abstract class AbstractGeometry
+	{
+		protected AbstractGeometry()
+		{
+			PaddingType = Padding.StartExtendEndExtend;
+			VisualizeOption = new VisualizeOption(Color.Red, Color.Gray);
+		}
+
+		public Padding PaddingType;
+
+		public abstract (Vector2 Pt, float Angle, float Bias, float Position) QueryTangentPoint(Vector2 point);
+
+		public abstract void Visualize(Action<VisDot> processDot, Action<VisLine> processLine,
+			bool visExtendedPart = false);
+
+		public VisualizeOption VisualizeOption;
+
+		/// <summary>
+		/// 查询指定位置的曲率。
+		/// </summary>
+		/// <param name="position">从起点到查询位置的距离。</param>
+		/// <returns></returns>
+		public abstract float QueryCurvature(float position);
+
+		public abstract float Length();
+	}
+
+	public class VisualizeOption
+	{
+		public VisualizeOption(Color mainColor, Color auxiliaryColor)
+		{
+			MainColor = mainColor;
+			AuxiliaryColor = auxiliaryColor;
+		}
+
+		public Color MainColor;
+		public Color AuxiliaryColor;
+		public bool DrawAuxiliary = true;
+		public bool VisualizeDirection = true;
+	}
+
+	public enum Padding
+	{
+		StartLineEndLine = 0b_0001_0001,
+		StartLineEndExtend = 0b_0001_0010,
+		StartExtendEndLine = 0b_0010_0001,
+		StartExtendEndExtend = 0b_0010_0010,
+	}
+
+	public class VisDot
+	{
+		public VisDot(Vector2 point, Color color)
+		{
+			Point = point;
+			Color = color;
+		}
+
+		public Vector2 Point;
+		public Color Color;
+	}
+
+	public class VisLine
+	{
+		public VisLine(Vector2 start, Vector2 end, bool startArrow, bool endArrow, Color color, float width = 1)
+		{
+			Start = start;
+			End = end;
+			StartArrow = startArrow;
+			EndArrow = endArrow;
+			Color = color;
+			Width = width;
+		}
+
+		public Vector2 Start;
+		public Vector2 End;
+		public bool StartArrow = false;
+		public bool EndArrow = false;
+		public Color Color;
+		public float Width;
+	}
+}

CommonUsage-MultiVehicleSync/commonusage/Geometries/BezierCurve.cs

@@ -0,0 +1,334 @@
+using System;
+using System.Collections.Generic;
+using System.Diagnostics;
+using System.Linq;
+using System.Numerics;
+using System.Reflection;
+using CommonUsage.Mathematics;
+
+namespace CommonUsage.Geometries
+{
+    public class BezierCurve : AbstractGeometry
+    {
+
+        public BezierCurve(List<Vector2> controlPoints, int resolution = 100)
+        {
+            // Console.WriteLine($"BezierCurve1");
+            // Console.WriteLine(string.Join("  ",controlPoints.Select(p=>$"{p.X:f2},{p.Y:f2}")));
+            _controlPoints = controlPoints;
+            _resolution = resolution;
+            InitializeBezier();
+        }
+
+        public override void Visualize(Action<VisDot> processDot, Action<VisLine> processLine, bool visExtendedPart = false)
+        {
+            if (VisualizeOption.DrawAuxiliary)
+                for (var i = 0; i < _controlPoints.Count - 1; ++i)
+                {
+                    processLine(new VisLine(_controlPoints[i], _controlPoints[i + 1],
+                        false, false, VisualizeOption.AuxiliaryColor));
+                    if (i == 0) continue;
+                    processDot(new VisDot(_controlPoints[i], VisualizeOption.AuxiliaryColor));
+                }
+
+            for (var i = 0; i < _bezierPoints.Count - 1; ++i)
+            {
+                if (Direction == -1)
+                {
+                    processLine(new VisLine(_bezierPoints[i + 1], _bezierPoints[i],
+                        false, i == (int)(_bezierPoints.Count / 2), VisualizeOption.MainColor, 2));
+                }
+                else
+                {
+                    processLine(new VisLine(_bezierPoints[i], _bezierPoints[i + 1],
+                        false, i == (int)(_bezierPoints.Count / 2), VisualizeOption.MainColor, 2));
+                }
+
+
+            }
+        }
+
+        public (Vector2 Point, int Id) QueryPoint(Vector2 point)
+        {
+            var p = new Vector2();
+            var id = -1;
+            var bestDistance = float.MaxValue;
+
+            var hashes = _bias.Select(bb => CalculateHash(point, 100, bb.X, bb.Y)).ToList();
+
+            void TryQuery(Dictionary<uint, List<(Vector2 Point, int Id)>> dict, List<uint> hashList)
+            {
+                foreach (var hash in hashList)
+                {
+                    if (!dict.TryGetValue(hash, out var ll)) continue;
+                    foreach (var (q, qId) in ll)
+                    {
+                        var d = Vector2.Distance(q, point);
+                        if (d < bestDistance)
+                        {
+                            p = q;
+                            id = qId;
+                            bestDistance = d;
+                        }
+                    }
+                }
+            }
+            //map目前有bug,取消cpu占用也不严重，必要时候在优化
+            // TryQuery(_pointsMappingSmall, hashes);
+            //
+            // if (id == -1)
+            // {
+            // 	hashes = _bias.Select(bb => CalculateHash(point, 1000, bb.X, bb.Y)).ToList();
+            // 	TryQuery(_pointsMappingBig, hashes);
+            // }
+
+            if (id == -1)
+            {
+                // todo: improve the way to find closest point if mappings fail
+                (p, id) = _bezierPoints.Select((p, i) => (p, i))
+                    .OrderBy(pair => CommonMath.dist(pair.p.X, pair.p.Y, point.X, point.Y)).First();
+            }
+
+            return (p, id);
+        }
+
+        public override (Vector2 Pt, float Angle, float Bias, float Position) QueryTangentPoint(Vector2 point)
+        {
+            var (p, id) = QueryPoint(point);
+            var tangent = _tangents[id];
+            var (bias, lp, fd) = CommonMath.Project2DLine(point, p, tangent);
+            var next = fd > 0 ? id + 1 : id - 1;
+            if (id == 0) next = 1;
+            // Console.WriteLine($"id:{id} next:{next} tangent:{tangent} _tangents.Count:{_tangents.Count}");
+            if (next > 0 && next < _tangents.Count)//线性插值
+            {
+                var (_, _, t) = CommonMath.Project2DLine(point, _bezierPoints[id], _bezierPoints[next]);
+                var partial = t / Vector2.Distance(_bezierPoints[id], _bezierPoints[next]);
+                if (partial >= 0 && partial <= 1)
+                {
+                    tangent = CommonMath.RoundTh(_tangents[id] +
+                                                 partial * CommonMath.RoundTh(_tangents[next] - _tangents[id]));
+                    if (CommonMath.RoundTh(_tangents[next] - _tangents[id]) > 5)
+                        Console.WriteLine($"bezier tangents bug, tanget: {id}:{_tangents[id]} {next}:{_tangents[next]}");
+                }
+                // else Console.WriteLine("bezier tangents bug");
+            }
+            return (lp, tangent, bias, fd + _sumDistances[id]);
+        }
+
+        public override float Length()
+        {
+            return _length;
+        }
+
+        public override float QueryCurvature(float position)
+        {
+            int id = _sumDistances.Count - 1;
+            if (position <= 0) id = 0;
+            else
+            {
+                for (int i = 1; i < _sumDistances.Count; i++)
+                {
+                    if (position > _sumDistances[i - 1] && position <= _sumDistances[i])
+                    {
+                        id = i;
+                        break;
+                    }
+                }
+            }
+
+            var result = _curvatures[id];
+            if (id > 0 && id < _sumDistances.Count - 1)//插值
+            {
+                var partial = (position - _sumDistances[id - 1]) / (_sumDistances[id] - _sumDistances[id - 1]);
+                if (partial >= 0 && partial <= 1) result = (1 - partial) * _curvatures[id - 1] + partial * _curvatures[id];
+                else Console.WriteLine("bezier curvature bug");
+            }
+
+            return result;
+        }
+
+        public Vector3 QueryBezierPointsById(int id)
+        {
+            if (id < 0 || id > Resolution)
+            {
+                Console.WriteLine($"QueryBezierPointsById out of range, Resolution:{Resolution},id:{id}.");
+                return new Vector3(0, 0, 0);
+            }
+
+            return new Vector3(_bezierPoints[id].X, _bezierPoints[id].Y, _tangents[id]);
+        }
+
+        public List<Vector2> ControlPoints => _controlPoints;
+
+        public int Resolution => _resolution;
+        /// <summary>
+        /// 仅用于simple显示路径方向
+        /// </summary>
+        public int Direction = 1;
+        public int Order => _order;
+
+        // public List<float> Tangents => _tangents;
+
+        public void UpdateControlPoint(int id, Vector2 point)
+        {
+            _controlPoints[id] = point;
+            InitializeBezier();
+        }
+
+        public void AddControlPoint(int id, Vector2 point)
+        {
+            _controlPoints.Insert(id, point);
+            InitializeBezier();
+        }
+
+        public void RemoveControlPoint(int id)
+        {
+            _controlPoints.RemoveAt(id);
+            InitializeBezier();
+        }
+
+        public Vector2 GetMidPoint()
+        {
+            return _bezierPoints[(int)Math.Ceiling(_resolution / 2d)];
+        }
+
+        private void InitializeBezier()
+        {
+            _order = _controlPoints.Count - 1;
+            // _bezierPoints = new List<Vector2>();
+            var delta = 1.0f / _resolution;
+            // for (int t = 0; t <= _resolution; t += 1)//下面循环算了，没必要先递归算一遍
+            // 	_bezierPoints.Add(new Vector2(DeCasteljauX(_order, 0, t*delta), DeCasteljauY(_order, 0, t*delta)));
+            var allPoints = new List<List<List<Vector2>>>();
+            for (var i = 0; i < _order; i++)
+            {
+                var size = allPoints.Count;
+                var morePoints = new List<List<Vector2>>();
+                for (var j = 0; j < _order - i; j++)
+                {
+                    var points = new List<Vector2>();
+                    for (int t = 0; t <= _resolution; t += 1)
+                    {
+                        float p0x;
+                        float p1x;
+                        float p0y;
+                        float p1y;
+                        var z = t;
+                        if (size > 0)
+                        {
+                            p0x = allPoints[i - 1][j][z].X;
+                            p1x = allPoints[i - 1][j + 1][z].X;
+                            p0y = allPoints[i - 1][j][z].Y;
+                            p1y = allPoints[i - 1][j + 1][z].Y;
+                        }
+                        else
+                        {
+                            p0x = _controlPoints[j].X;
+                            p1x = _controlPoints[j + 1].X;
+                            p0y = _controlPoints[j].Y;
+                            p1y = _controlPoints[j + 1].Y;
+                        }
+
+                        var part = t * delta;
+                        points.Add(new Vector2((1 - part) * p0x + part * p1x, (1 - part) * p0y + part * p1y));
+                    }
+                    morePoints.Add(points);
+                }
+                allPoints.Add(morePoints);
+            }
+
+            _bezierPoints = allPoints.Last().Last();
+            _tangentInfo = allPoints;
+            _tangents = Enumerable.Repeat(0f, _bezierPoints.Count).ToList();
+            _curvatures = Enumerable.Repeat(0f, _bezierPoints.Count).ToList();
+            var p2 = allPoints[Order - 2];
+            for (var id = 0; id < _bezierPoints.Count; ++id)
+            {
+                _tangents[id] =
+                    (float)(Math.Atan2(p2[1][id].Y - p2[0][id].Y, p2[1][id].X - p2[0][id].X) / Math.PI * 180);
+                if (id != 0) _curvatures[id] = (float)((CommonMath.ThDiff(_tangents[id], _tangents[id - 1]) / 180 * Math.PI)
+                    / (Vector2.Distance(_bezierPoints[id], _bezierPoints[id - 1]) / 1000));
+            }
+            // Console.WriteLine($"{string.Join("\n", _tangents.Select((val, i) => $"{i}: {val}"))}");
+            _tangents[0] = _tangents[1]; // todo: here is temporary fix
+            _curvatures[0] = _curvatures[1];
+            for (var id = 1; id < _bezierPoints.Count - 1; ++id)//前移0.5
+                _curvatures[id] = (_curvatures[id] + _curvatures[id + 1]) / 2;
+
+            _remainDistances = Enumerable.Repeat(0f, _bezierPoints.Count).ToList();
+            _sumDistances = Enumerable.Repeat(0f, _bezierPoints.Count).ToList();
+            for (var i = _bezierPoints.Count - 2; i >= 0; --i)
+            {
+                _remainDistances[i] =
+                    _remainDistances[i + 1] + Vector2.Distance(_bezierPoints[i], _bezierPoints[i + 1]);
+            }
+            for (var i = 1; i < _bezierPoints.Count; ++i)
+            {
+                _sumDistances[i] =
+                    _sumDistances[i - 1] + Vector2.Distance(_bezierPoints[i], _bezierPoints[i - 1]);
+            }
+            _length = _sumDistances.Last();
+
+            _minX = _bezierPoints.Min(pp => pp.X);
+            _minY = _bezierPoints.Min(pp => pp.Y);
+
+            var tmpList = _bezierPoints.Select((point, index) => (point, index)).ToList();
+            return;
+            void GenerateGridMapping(ref Dictionary<uint, List<(Vector2 Point, int Id)>> dict, float gSize)
+            {
+                dict = new Dictionary<uint, List<(Vector2 Point, int Id)>>();
+                foreach (var (point, index) in tmpList)
+                {
+                    var hash = CalculateHash(point, gSize);
+                    if (dict.TryGetValue(hash, out var ll))
+                        ll.Add((point, index));
+                    else dict[hash] = new List<(Vector2 Point, int Id)>() { (point, index) };
+                }
+            }
+
+            GenerateGridMapping(ref _pointsMappingSmall, 100);
+            GenerateGridMapping(ref _pointsMappingBig, 1000);
+        }
+
+        private uint CalculateHash(Vector2 point, float gridSize, int xBias = 0, int yBias = 0)
+        {
+            return (uint)(((int)((point.X - _minX) / gridSize) + xBias) << 16 + (((int)((point.Y - _minY) / gridSize) + yBias) & 0xffff));
+        }
+
+        private readonly List<(int X, int Y)> _bias = new()
+        {
+            new(-1, -1), new(-1, 0), new(-1, 1),
+            new(0, -1), new(0, 0), new(0, 1),
+            new(1, -1), new(1, 0), new(1, 1),
+        };
+
+        private float DeCasteljauX(int i, int j, float t)
+        {
+            if (i == 1)
+                return (1 - t) * _controlPoints[j].X + t * _controlPoints[j + 1].X;
+            return (1 - t) * DeCasteljauX(i - 1, j, t) + t * DeCasteljauX(i - 1, j + 1, t);
+        }
+
+        private float DeCasteljauY(int i, int j, float t)
+        {
+            if (i == 1)
+                return (1 - t) * _controlPoints[j].Y + t * _controlPoints[j + 1].Y;
+            return (1 - t) * DeCasteljauY(i - 1, j, t) + t * DeCasteljauY(i - 1, j + 1, t);
+        }
+
+        private int _order;
+        private int _resolution;
+        private List<Vector2> _controlPoints;
+        private List<Vector2> _bezierPoints;
+        private List<List<List<Vector2>>> _tangentInfo;
+        private List<float> _tangents;
+        private List<float> _remainDistances;
+        private List<float> _sumDistances;
+        private List<float> _curvatures;
+        private Dictionary<uint, List<(Vector2 Point, int Id)>> _pointsMappingSmall;
+        private Dictionary<uint, List<(Vector2 Point, int Id)>> _pointsMappingBig;
+        private float _minX, _minY;
+        private float _length;
+    }
+}

CommonUsage-MultiVehicleSync/commonusage/Geometries/CircularArc.cs

@@ -0,0 +1,256 @@
+using System;
+using System.Collections.Generic;
+using System.Drawing;
+using System.Numerics;
+using System.Reflection;
+using System.Security.Cryptography;
+using System.Text;
+using CommonUsage.Mathematics;
+
+namespace CommonUsage.Geometries
+{
+	public class CircularArc : AbstractGeometry
+	{
+		/// <summary>
+		/// 以center为圆心、radius为半径，从angleStart逆时针转到angleEnd所构成的圆弧。direction表示圆弧走向。
+		/// </summary>
+		/// <param name="center"></param>
+		/// <param name="radius"></param>
+		/// <param name="angleStart"></param>
+		/// <param name="angleEnd"></param>
+		/// <param name="direction">表示圆弧走向，1为angleStart到angleEnd，-1为angleEnd到angleStart</param>
+		public CircularArc(Vector2 center, float radius, float angleStart, float angleEnd, int direction, Padding paddingType)
+		{
+			_center = center;
+			_radius = radius;
+			_angleStart = angleStart;
+			_angleEnd = angleEnd;
+			_direction = direction;
+			PaddingType = paddingType;
+
+			ChangeShape();
+			CalculateVisPoints();
+		}
+
+		public override void Visualize(Action<VisDot> processDot, Action<VisLine> processLine,
+			bool visExtendedPart = false)
+		{
+			lock (_visPoints)
+			{
+				if (visExtendedPart)
+				{
+
+				}
+
+				for (var i = 0; i < _visPoints.Length - 1; ++i)
+				{
+					if ((i == 0 || i == _visPoints.Length - 2) && !visExtendedPart) continue;
+					var color = Color.Red;
+					if (i == 0 || i == _visPoints.Length - 2) color = Color.Gray;
+					processLine(new VisLine(_visPoints[i], _visPoints[i + 1], 
+						false, i == (_visPoints.Length - 1) / 2, color));
+				}
+
+				if (visExtendedPart)
+				{
+
+				}
+			}
+		}
+
+		public void SwitchSide()
+		{
+			(_angleStart, _angleEnd) = (_angleEnd, _angleStart);
+			ChangeShape();
+			CalculateVisPoints();
+		}
+
+		public float VisAngleResolution = 1;
+
+		public Vector2 Center
+		{
+			get => _center;
+			set
+			{
+				_center = value;
+				ChangeShape();
+				CalculateVisPoints();
+			}
+		}
+
+		public float Radius
+		{
+			get => _radius;
+			set
+			{
+				_radius = value;
+				ChangeShape();
+				CalculateVisPoints();
+			}
+		}
+
+		public float AngleStart
+		{
+			get => _angleStart;
+			set
+			{
+				_angleStart = value;
+				ChangeShape();
+				CalculateVisPoints();
+			}
+		}
+
+		public float AngleEnd
+		{
+			get => _angleEnd;
+			set
+			{
+				_angleEnd = value;
+				ChangeShape();
+				CalculateVisPoints();
+			}
+		}
+
+		public int Direction
+		{
+			get => _direction;
+			set
+			{
+				_direction = value;
+				ChangeShape();
+				CalculateVisPoints();
+			}
+		}
+
+        public float AngleRange => _totalTh;
+
+		public Vector2 PointStart => _center + new Vector2(_radius * (float)Math.Cos(_angleStart / 180 * Math.PI),
+			_radius * (float)Math.Sin(_angleStart / 180 * Math.PI));
+
+		public Vector2 PointEnd => _center + new Vector2(_radius * (float)Math.Cos(_angleEnd / 180 * Math.PI),
+			_radius * (float)Math.Sin(_angleEnd / 180 * Math.PI));
+
+		public Vector2 Src => _src;
+
+		public Vector2 Dst => _dst;
+
+		public float TangentSrc => _tangentSrc;
+
+		public float TangentDst => _tangentDst;
+
+		public override (Vector2 Pt, float Angle, float Bias, float Position) QueryTangentPoint(Vector2 point)
+		{
+			var queryTh = (float)(Math.Atan2(point.Y - _center.Y, point.X - _center.X) / Math.PI * 180);
+
+			var p = new Vector2();
+			var tangent = 0f;
+			var pd = 0f;
+			var bestBias = float.MaxValue;
+
+			if ((((int)PaddingType >> 4) & 0x1) == 1)
+			{
+				var (bias1, hPnt1, fd1) = CommonMath.Project2DLine(point, _beforeStartSrc, _src);
+                if (fd1 <= 1000)
+				{
+					p = hPnt1;
+					tangent = (_direction >= 0 ? _angleStart : _angleEnd) + 90 * _direction;
+					pd = fd1;
+					bestBias = bias1;
+				}
+			}
+
+			if (((int)PaddingType & 0x1) == 1)
+			{
+				var (bias2, hPnt2, fd2) = CommonMath.Project2DLine(point, _dst, _afterEndDst);
+				if (fd2 >= 0 && Math.Abs(bias2) < Math.Abs(bestBias))
+				{
+					p = hPnt2;
+					tangent = (_direction >= 0 ? _angleEnd : _angleStart) + 90 * _direction;
+					pd = _totalLen + fd2;
+					bestBias = bias2;
+				}
+			}
+
+			var th1 = _direction == 1 ? CommonMath.ThDiff(queryTh, _angleStart) : CommonMath.ThDiff(_angleEnd, queryTh);
+			// todo: urgent bug! should use better strategy to prevent sign problem
+            if (th1 < -55) th1 += 360;
+			var arcBias = (_radius - Vector2.Distance(point, _center)) * _direction;
+			if (Math.Abs(arcBias) < Math.Abs(bestBias))
+			{
+				p = _center + _radius * new Vector2((float)Math.Cos(queryTh / 180 * Math.PI),
+					(float)Math.Sin(queryTh / 180 * Math.PI));
+				tangent = queryTh + 90 * _direction;
+				pd = _radius * th1 / 180 * (float)Math.PI;
+				bestBias = arcBias;
+			}
+
+			return (p, tangent, bestBias, pd);
+		}
+
+		public override float QueryCurvature(float position)
+		{
+			// var theta = (float)(_angleEnd - position / _radius / Math.PI * 180f + Math.PI);
+			// return Vectoriel.FromAngleLen(theta, 1f / _radius);
+            return 1000f / _radius * _direction;
+        }
+
+		public override float Length()
+		{
+			return _totalLen;
+		}
+
+		private void CalculateVisPoints()
+		{
+			lock (_visPoints)
+			{
+				// todo: overlapping start and end is problematic
+				var ptCnt = (int)Math.Ceiling((_angleEnd + 360 - _angleStart) % 360 / VisAngleResolution);
+				_visPoints = new Vector2[ptCnt + 2];
+
+				var starting = _angleStart;
+				if (_direction == -1) starting = _angleEnd;
+				_visPoints[0] = _beforeStartSrc;
+
+				for (var j = 0; j < ptCnt; ++j)
+				{
+					var th = starting + j * VisAngleResolution * _direction;
+					var radAngle = (float)(th / 180f * Math.PI);
+					_visPoints[j + 1] = Center + new Vector2((float)Math.Cos(radAngle), (float)Math.Sin(radAngle)) * Radius;
+				}
+				
+				_visPoints[ptCnt + 1] = _afterEndDst;
+			}
+		}
+
+		private void ChangeShape()
+		{
+			_totalTh = CommonMath.ThDiff(_angleEnd, _angleStart);
+			if (_totalTh < 0) _totalTh += 360;
+			_totalLen = _radius * _totalTh / 180 * (float)Math.PI;
+
+			var radAngleStart = _angleStart / 180 * Math.PI;
+			var radAngleEnd = _angleEnd / 180 * Math.PI;
+			double srcAngle = radAngleStart, dstAngle = radAngleEnd;
+			if (_direction == -1) (srcAngle, dstAngle) = (dstAngle, srcAngle);
+			_src = _center + new Vector2((float)Math.Cos(srcAngle), (float)Math.Sin(srcAngle)) * _radius;
+			_dst = _center + new Vector2((float)Math.Cos(dstAngle), (float)Math.Sin(dstAngle)) * _radius;
+			_beforeStartSrc = CommonMath.Transform2D(_src,
+				(_direction >= 0 ? _angleStart : _angleEnd) + 90 * _direction, new Vector2(-1000, 0));
+			_afterEndDst = CommonMath.Transform2D(_dst, (_direction >= 0 ? _angleEnd : _angleStart) + 90 * _direction,
+				new Vector2(1000, 0));
+			_tangentSrc = QueryTangentPoint(_src).Angle;
+			_tangentDst = QueryTangentPoint(_dst).Angle;
+		}
+
+		private Vector2 _center;
+		private float _radius, _angleStart, _angleEnd;
+		private int _direction;
+
+		private float _totalTh, _totalLen;
+		private Vector2 _src, _dst;
+		private float _tangentSrc, _tangentDst;
+		private Vector2 _beforeStartSrc, _afterEndDst;
+
+		private Vector2[] _visPoints = Array.Empty<Vector2>();
+	}
+}

CommonUsage-MultiVehicleSync/commonusage/Geometries/NurbsCurve.cs

@@ -0,0 +1,349 @@
+using CommonUsage.Mathematics;
+using System.Collections.Generic;
+using System.Numerics;
+using System;
+using System.Linq;
+
+namespace CommonUsage.Geometries
+{
+    public class NurbsCurve : AbstractGeometry
+    {
+
+        public NurbsCurve(List<Vector2> controlPoints, List<float> weights, List<float> knotVector, int frame = 100)
+        {
+            _controlPoints = controlPoints;
+            _weights = weights;
+            _knotVector = knotVector;
+            _frame = frame;
+            InitializeNurbs();
+        }
+
+        public override void Visualize(Action<VisDot> processDot, Action<VisLine> processLine, bool visExtendedPart = false)
+        {
+            if (VisualizeOption.DrawAuxiliary)
+                for (var i = 0; i < _controlPoints.Count - 1; ++i)
+                {
+                    processLine(new VisLine(_controlPoints[i], _controlPoints[i + 1],
+                        false, false, VisualizeOption.AuxiliaryColor));
+                    if (i == 0) continue;
+                    processDot(new VisDot(_controlPoints[i], VisualizeOption.AuxiliaryColor));
+                }
+
+            for (var i = 0; i < _nurbsPoints.Count - 1; ++i)
+            {
+                if (Direction == -1)
+                {
+                    processLine(new VisLine(_nurbsPoints[i + 1], _nurbsPoints[i],
+                        false, i == (int)(_nurbsPoints.Count / 2), VisualizeOption.MainColor, 2));
+                }
+                else
+                {
+                    processLine(new VisLine(_nurbsPoints[i], _nurbsPoints[i + 1],
+                        false, i == (int)(_nurbsPoints.Count / 2), VisualizeOption.MainColor, 2));
+                }
+            }
+        }
+        public (Vector2 Point, int Id) QueryPoint(Vector2 point)
+        {
+            var p = new Vector2();
+            var id = -1;
+            var bestDistance = float.MaxValue;
+
+            var hashes = _bias.Select(bb => CalculateHash(point, 100, bb.X, bb.Y)).ToList();
+
+            void TryQuery(Dictionary<uint, List<(Vector2 Point, int Id)>> dict, List<uint> hashList)
+            {
+                foreach (var hash in hashList)
+                {
+                    if (!dict.TryGetValue(hash, out var ll)) continue;
+                    foreach (var (q, qId) in ll)
+                    {
+                        var d = Vector2.Distance(q, point);
+                        if (d < bestDistance)
+                        {
+                            p = q;
+                            id = qId;
+                            bestDistance = d;
+                        }
+                    }
+                }
+            }
+
+            if (id == -1)
+            {
+                // todo: improve the way to find closest point if mappings fail
+                (p, id) = _nurbsPoints.Select((p, i) => (p, i))
+                    .OrderBy(pair => CommonMath.dist(pair.p.X, pair.p.Y, point.X, point.Y)).First();
+            }
+
+            return (p, id);
+        }
+
+        private uint CalculateHash(Vector2 point, float gridSize, int xBias = 0, int yBias = 0)
+        {
+            return (uint)(((int)((point.X - _minX) / gridSize) + xBias) << 16 + (((int)((point.Y - _minY) / gridSize) + yBias) & 0xffff));
+        }
+
+        private readonly List<(int X, int Y)> _bias = new()
+        {
+            new(-1, -1), new(-1, 0), new(-1, 1),
+            new(0, -1), new(0, 0), new(0, 1),
+            new(1, -1), new(1, 0), new(1, 1),
+        };
+
+        public override (Vector2 Pt, float Angle, float Bias, float Position) QueryTangentPoint(Vector2 point)
+        {
+            var (p, id) = QueryPoint(point);
+            var tangent = _tangents[id];
+            var (bias, lp, fd) = CommonMath.Project2DLine(point, p, tangent);
+            var next = fd > 0 ? id + 1 : id - 1;
+            if (next > 0 && next < _tangents.Count)//线性插值
+            {
+                var (_, _, t) = CommonMath.Project2DLine(point, _nurbsPoints[id], _nurbsPoints[next]);
+                var partial = t / Vector2.Distance(_nurbsPoints[id], _nurbsPoints[next]);
+                if (partial >= 0 && partial <= 1)
+                {
+                    tangent = CommonMath.RoundTh(_tangents[id] +
+                                                 partial * CommonMath.RoundTh(_tangents[next] - _tangents[id]));
+                    if (CommonMath.RoundTh(_tangents[next] - _tangents[id]) > 5)
+                        Console.WriteLine($"Nurbs tangents bug, tanget: {id}:{_tangents[id]} {next}:{_tangents[next]}");
+                }
+                else Console.WriteLine("Nurbs tangents bug");
+            }
+            return (lp, tangent, bias, fd + _sumDistances[id]);
+        }
+
+        public override float QueryCurvature(float position)
+        {
+            int id = _sumDistances.Count - 1;
+            if (position <= 0) id = 0;
+            else
+            {
+                for (int i = 1; i < _sumDistances.Count; i++)
+                {
+                    if (position > _sumDistances[i - 1] && position <= _sumDistances[i])
+                    {
+                        id = i;
+                        break;
+                    }
+                }
+            }
+
+            var result = _curvatures[id];
+            if (id > 0 && id < _sumDistances.Count - 1)//插值
+            {
+                var partial = (position - _sumDistances[id - 1]) / (_sumDistances[id] - _sumDistances[id - 1]);
+                if (partial >= 0 && partial <= 1) result = (1 - partial) * _curvatures[id - 1] + partial * _curvatures[id];
+                else Console.WriteLine("Nurbs curvature bug");
+            }
+
+            return result;
+        }
+
+        public Vector3 QueryNurbsPointsById(int id)
+        {
+            if (id < 0 || id > Frame)
+            {
+                Console.WriteLine($"QueryBezierPointsById out of range, Resolution:{Frame},id:{id}.");
+                return new Vector3(0, 0, 0);
+            }
+
+            return new Vector3(_nurbsPoints[id].X, _nurbsPoints[id].Y, _tangents[id]);
+        }
+        public override float Length()
+        {
+            return _length;
+        }
+
+        public int Order => _order;
+        public List<Vector2> ControlPoints => _controlPoints;
+        public List<float> Weights => _weights;
+        public List<float> KnotVector => _knotVector;
+        public int Frame => _frame;
+
+        public int Direction = 1;
+
+
+        public void UpdateControlPoint(int id, Vector2 point)
+        {
+            _controlPoints[id] = point;
+            InitializeNurbs();
+        }
+        public void UpdateNurbsWeihgts(int id, float weight)
+        {
+            _weights[id] = weight;
+            InitializeNurbs();
+
+        }
+        public void AddControlPoint(int id, Vector2 point)
+        {
+            _controlPoints.Insert(id, point);
+            InitializeNurbs();
+        }
+
+        public void RemoveControlPoint(int id)
+        {
+            _controlPoints.RemoveAt(id);
+            InitializeNurbs();
+        }
+
+        public Vector2 GetMidPoint()
+        {
+            return _nurbsPoints[(int)Math.Ceiling(_frame / 2d)];
+        }
+
+        private void InitializeNurbs()
+        {
+            _order = _controlPoints.Count - 1;
+            List<List<Vector2>> allpoints = new List<List<Vector2>>();
+            List<Vector2> nurbsCurvePoints = new List<Vector2>();
+            float delta = 1.0f / Frame;
+
+            for (float t = 0; t <= 1; t += delta)
+            {
+                var (point, tangent) = DeBoorAlgorithm(t);
+                var points = new List<Vector2>
+                {
+                    point,
+                    point + tangent // Tangent endpoint
+                };
+                allpoints.Add(points);
+                nurbsCurvePoints.Add(point); // Store the curve point separately
+            }
+
+            _nurbsPoints = nurbsCurvePoints;
+            _tangents = Enumerable.Repeat(0f, _nurbsPoints.Count).ToList();
+            _curvatures = Enumerable.Repeat(0f, _nurbsPoints.Count).ToList();
+
+            for (var id = 0; id < _nurbsPoints.Count - 1; ++id)
+            {
+                Vector2 p1 = _nurbsPoints[id];
+                Vector2 p2 = _nurbsPoints[id + 1];
+
+                float tangentAngle = (float)Math.Atan2(p2.Y - p1.Y, p2.X - p1.X) * 180 / (float)Math.PI;
+                _tangents[id] = tangentAngle;
+
+                // Calculate curvature using finite differences of tangent (second derivative approximation)
+                if (id > 0)
+                {
+                    float previousTangent = _tangents[id - 1];
+                    float curvature = (float)(CommonMath.ThDiff(tangentAngle, previousTangent) * Math.PI / 180) /
+                                      (Vector2.Distance(p1, p2) / 1000);
+                    _curvatures[id] = curvature;
+                }
+            }
+
+            _curvatures.Insert(0, _curvatures[0]);
+            for (var id = 1; id < _curvatures.Count - 1; ++id)
+            {
+                _curvatures[id] = (_curvatures[id] + _curvatures[id + 1]) / 2;
+            }
+
+            _remainDistances = Enumerable.Repeat(0f, _nurbsPoints.Count).ToList();
+            _sumDistances = Enumerable.Repeat(0f, _nurbsPoints.Count).ToList();
+            _remainDistances[_nurbsPoints.Count - 1] = 0;
+
+            for (var i = _nurbsPoints.Count - 2; i >= 0; --i)
+            {
+                _remainDistances[i] = _remainDistances[i + 1] + Vector2.Distance(_nurbsPoints[i], _nurbsPoints[i + 1]);
+            }
+
+            _sumDistances[0] = 0;
+            for (var i = 1; i < _nurbsPoints.Count; ++i)
+            {
+                _sumDistances[i] = _sumDistances[i - 1] + Vector2.Distance(_nurbsPoints[i], _nurbsPoints[i - 1]);
+            }
+
+            _length = _sumDistances.Last();
+
+            _minX = _nurbsPoints.Min(pp => pp.X);
+            _minY = _nurbsPoints.Min(pp => pp.Y);
+
+            var tmpList = _nurbsPoints.Select((point, index) => (point, index)).ToList();
+            // GenerateGridMapping(ref _pointsMappingSmall, 100, tmpList);
+            // GenerateGridMapping(ref _pointsMappingBig, 1000, tmpList);
+        }
+
+
+       
+
+        private float CalculateLength()
+        {
+            return _nurbsPoints.Zip(_nurbsPoints.Skip(1), Vector2.Distance).Sum();
+        }
+
+        private (Vector2, Vector2) DeBoorAlgorithm(float t)
+        {
+            Vector2 numerator = Vector2.Zero;
+            Vector2 tangentNumerator = Vector2.Zero;
+            float denominator = 0f;
+
+            // Calculate the point on the curve
+            for (int i = 0; i < ControlPoints.Count; ++i)
+            {
+                float basis = BasisFunction(i, _order, t) * Weights[i];
+                numerator += basis * ControlPoints[i];
+                denominator += basis;
+            }
+
+            Vector2 point = numerator / denominator;
+
+            // Calculate the tangent vector using the analytical derivative
+            for (int i = 0; i < ControlPoints.Count; ++i)
+            {
+                float basisDerivative = BasisFunctionDerivative(i, _order, t) * Weights[i];
+                tangentNumerator += basisDerivative * ControlPoints[i];
+            }
+            Vector2 tangent = tangentNumerator / denominator;
+
+            return (point, tangent);
+        }
+
+        private float BasisFunction(int i, int p, float t)
+        {
+            if (p == 0)
+                return (KnotVector[i] <= t && t < KnotVector[i + 1]) ? 1.0f : 0.0f;
+
+            float denom1 = KnotVector[i + p] - KnotVector[i];
+            float term1 = denom1 == 0 ? 0 : ((t - KnotVector[i]) / denom1) * BasisFunction(i, p - 1, t);
+
+            float denom2 = KnotVector[i + p + 1] - KnotVector[i + 1];
+            float term2 = denom2 == 0 ? 0 : ((KnotVector[i + p + 1] - t) / denom2) * BasisFunction(i + 1, p - 1, t);
+
+            return term1 + term2;
+        }
+
+        private float BasisFunctionDerivative(int i, int k, float t)
+        {
+            if (k == 0) return 0;
+
+            float denom1 = KnotVector[i + k] - KnotVector[i];
+            float denom2 = KnotVector[i + k + 1] - KnotVector[i + 1];
+
+            float term1 = denom1 != 0 ? BasisFunction(i, k - 1, t) / denom1 : 0;
+            float term2 = denom1 != 0 ? (t - KnotVector[i]) * BasisFunctionDerivative(i, k - 1, t) / denom1 : 0;
+
+            float term3 = denom2 != 0 ? -BasisFunction(i + 1, k - 1, t) / denom2 : 0;
+            float term4 = denom2 != 0 ? (KnotVector[i + k + 1] - t) * BasisFunctionDerivative(i + 1, k - 1, t) / denom2 : 0;
+
+            return term1 + term2 + term3 + term4;
+        }
+       
+        private int _order;
+        private List<Vector2> _controlPoints;
+        private List<float> _weights;
+        private List<float> _knotVector;
+        private int _frame;
+        private List<Vector2> _nurbsPoints;
+        private List<List<List<Vector2>>> _tangentPoints;
+        private List<float> _curvatures;
+        private List<float> _sumDistances;
+        private List<float> _remainDistances;
+        private float _minX, _minY;
+        private float _length;
+        private Dictionary<uint, List<(Vector2 Point, int Id)>> _pointsMappingSmall;
+        private Dictionary<uint, List<(Vector2 Point, int Id)>> _pointsMappingBig;
+        private List<float> _tangents;
+
+    }
+}

CommonUsage-MultiVehicleSync/commonusage/Geometries/Vectoriel.cs

@@ -0,0 +1,71 @@
+using System;
+using System.Collections.Generic;
+using System.Numerics;
+using System.Text;
+
+namespace CommonUsage.Geometries
+{
+	/// <summary>
+	/// MDCS数学类：向量。
+	/// </summary>
+	public class Vectoriel
+	{
+        public Vectoriel()
+        {
+            _vec2 = Vector2.Zero;
+            _dir2 = Vector2.Normalize(_vec2);
+            _len = _vec2.Length();
+            _angle = (float)(Math.Atan2(_vec2.Y, _vec2.X) / Math.PI * 180f);
+        }
+
+		public Vectoriel(Vector2 vec)
+		{
+			_vec2 = vec;
+			_dir2 = Vector2.Normalize(_vec2);
+			_len = _vec2.Length();
+			_angle = (float)(Math.Atan2(_vec2.Y, _vec2.X) / Math.PI * 180f);
+		}
+
+		/// <summary>
+		/// 通过笛卡尔坐标系X和Y值构建向量。
+		/// </summary>
+		/// <param name="x"></param>
+		/// <param name="y"></param>
+		/// <returns></returns>
+		public static Vectoriel FromXY(float x, float y)
+		{
+			return new Vectoriel(new Vector2(x, y));
+		}
+
+		/// <summary>
+		/// 通过极坐标系的角度和距离值构建向量。
+		/// </summary>
+		/// <param name="angle"></param>
+		/// <param name="len"></param>
+		/// <returns></returns>
+		public static Vectoriel FromAngleLen(float angle, float len)
+		{
+			var rad = angle / 180f * Math.PI;
+			return new Vectoriel(new Vector2((float)Math.Cos(rad), (float)Math.Sin(rad)) * len);
+		}
+
+		public static implicit operator Vector2(Vectoriel vec)
+		{
+			return vec._vec2;
+		}
+
+		public static explicit operator Vectoriel(Vector2 vec)
+		{
+			return FromXY(vec.X, vec.Y);
+		}
+
+		public Vector2 Direction => _dir2;
+
+		public float Length => _len;
+
+		public float Angle => _angle;
+
+		private Vector2 _vec2, _dir2;
+		private float _len, _angle;
+	}
+}

CommonUsage-MultiVehicleSync/commonusage/Mathematics/CommonMath.cs

@@ -0,0 +1,774 @@
+using System;
+using System.Collections.Generic;
+using System.Drawing;
+using System.Linq;
+using System.Numerics;
+using System.Reflection;
+using System.Runtime.CompilerServices;
+using System.Runtime.InteropServices;
+
+namespace CommonUsage.Mathematics
+{
+
+    using T3 = Tuple<float, float, float>;
+    using D3 = Tuple<double, double, double>;
+
+    public class CommonMath
+    {
+	    public class PrimeEnumerator<T>
+	    {
+		    public PrimeEnumerator(List<T> items, Func<T, bool> process)
+		    {
+                _n = items.Count;
+                _items = items;
+                _process = process;
+
+                foreach (var pNum in _primes)
+                {
+	                if (_n % pNum != 0)
+	                {
+		                _a = pNum;
+		                _b = 11;
+		                break;
+	                }
+                }
+			}
+
+		    public void Enumerate()
+		    {
+			    using (var enumerator = Get().GetEnumerator())
+			    {
+                    while (enumerator.MoveNext()) { }
+			    }
+		    }
+
+		    private readonly int _n, _a, _b;
+		    private readonly int[] _primes = new[] { 29, 23, 19, 17, 13 };
+		    private List<T> _items;
+            private readonly Func<T, bool> _process;
+
+		    private IEnumerable<bool> Get()
+		    {
+			    for (var i = 0; i < _n; ++i)
+			    {
+				    var id = (i * _a + _b) % _n;
+				    yield return _process(_items[id]);
+			    }
+			}
+	    }
+
+        private static IEnumerable<IEnumerable<T>> GetPermutationsInternal<T>(IEnumerable<T> list, int length)
+        {
+            if (length == 1) return list.Select(t => new T[] { t });
+
+            return GetPermutationsInternal(list, length - 1)
+                .SelectMany(t => list.Where(e => !t.Contains(e)),
+                    (t1, t2) => t1.Concat(new T[] { t2 }));
+        }
+
+        /// <summary>
+        /// 得到一组数据的所有排列。
+        /// </summary>
+        /// <typeparam name="T">元素数据类型</typeparam>
+        /// <param name="list">所有待选元素</param>
+        /// <param name="selectNum">所选出的元素数量</param>
+        /// <returns></returns>
+        public static List<List<T>> GetPermutations<T>(List<T> list, int selectNum)
+        {
+            return GetPermutationsInternal(list, selectNum).Select(ll => ll.ToList()).ToList();
+        }
+
+        public static (float bias, Vector2 hPnt, float d) Project2DLine(Vector2 pnt, Vector2 segSt,
+            Vector2 segEnd)
+        {
+            var dir = Vector2.Normalize(segEnd - segSt);
+            var fd = Vector2.Dot(pnt - segSt, dir);
+            var hPnt = segSt + fd * dir;
+            var bias = dir.X * (pnt.Y-segSt.Y) - (pnt.X-segSt.X) * dir.Y;
+            return (bias, hPnt, fd);
+        }
+
+        public static (float bias, Vector2 hPnt, float fd) Project2DLine(Vector2 pnt, Vector2 segSt, float tangent)
+        {
+	        var dir = new Vector2((float)System.Math.Cos(tangent / 180 * System.Math.PI), (float)System.Math.Sin(tangent / 180 * System.Math.PI));
+	        var fd = Vector2.Dot(pnt - segSt, dir);
+	        var hPnt = segSt + fd * dir;
+	        var bias = dir.X * (pnt.Y - segSt.Y) - (pnt.X - segSt.X) * dir.Y;
+	        return (bias, hPnt, fd);
+		}
+
+        public class LineEqu
+        {
+            public double A, B, C, ln, dAB;
+            public double px1, px2, py1, py2;
+            public float midX;
+            public float midY;
+        }
+        // Fit line with PCA.
+        public LineEqu CalcLine(IEnumerable<Vector2> tls)
+        {
+            var lidarPoint2Ds = tls as Vector2[] ?? tls.ToArray();
+            float fx = lidarPoint2Ds.Average(f => f.X);
+            float fy = lidarPoint2Ds.Average(f => f.Y);
+            float fxx = lidarPoint2Ds.Average(f => f.X * f.X);
+            float fxy = lidarPoint2Ds.Average(f => f.X * f.Y);
+            float fyy = lidarPoint2Ds.Average(f => f.Y * f.Y);
+            float a = fxx - fx * fx, b = fxy - fx * fy, c = fyy - fy * fy;
+            double sqt = System.Math.Sqrt((a - c) * (a - c) + 4 * b * b);
+            double l1 = a + c + sqt;
+            double l2 = a + c - sqt;
+            double dx, dy;
+            if (System.Math.Abs(a - l1 / 2) > System.Math.Abs(c - l1 / 2))
+            {
+                dy = l1 / 2 - a; dx = b;
+            }
+            else
+            {
+                dx = l1 / 2 - c; dy = b;
+            }
+            double norm = System.Math.Sqrt(dx * dx + dy * dy);
+            dx /= norm; dy /= norm;
+            double A = dy, B = -dx, C = dx * fy - dy * fx;
+            double dAB = System.Math.Sqrt(A * A + B * B);
+
+            return new CommonMath.LineEqu
+            {
+                A = A,
+                B = B,
+                C = C,
+                ln = lidarPoint2Ds.Average(p => System.Math.Abs(p.X * A + p.Y * B + C) / dAB),
+                midX = fx,
+                midY = fy
+            };
+        }
+
+        public static double QuadInterp3(double[] confsF)
+        {
+            if (confsF[0] > confsF[1] && confsF[0] > confsF[2])
+            {
+                //printf("left overflow...\n");
+                return -1;
+            }
+
+            if (confsF[1] > confsF[0] && confsF[1] > confsF[2])
+            {
+                return (-(confsF[2] - confsF[0]) / 2.0f / (confsF[0] + confsF[2] - 2.0f * confsF[1] + 0.0001f));
+            }
+            if (confsF[2] > confsF[0] && confsF[2] > confsF[1])
+            {
+                //printf("right overflow...\n");
+                return 1;
+            }
+            return 0;
+        }
+
+        public static double cross(PointF O, PointF A, PointF B)
+        {
+            return (A.X - O.X) * (B.Y - O.Y) - (A.Y - O.Y) * (B.X - O.X);
+        }
+
+        public static List<PointF> GetConvexHull(List<PointF> points)
+        {
+            if (points == null)
+                return null;
+
+            if (points.Count() <= 1)
+                return points;
+
+            int n = points.Count(), k = 0;
+            List<PointF> H = new List<PointF>(new PointF[2 * n]);
+
+            points.Sort((a, b) =>
+                a.X == b.X ? a.Y.CompareTo(b.Y) : a.X.CompareTo(b.X));
+
+            // Build lower hull
+            for (int i = 0; i < n; ++i)
+            {
+                while (k >= 2 && cross(H[k - 2], H[k - 1], points[i]) <= 0)
+                    k--;
+                H[k++] = points[i];
+            }
+
+            // Build upper hull
+            for (int i = n - 2, t = k + 1; i >= 0; i--)
+            {
+                while (k >= t && cross(H[k - 2], H[k - 1], points[i]) <= 0)
+                    k--;
+                H[k++] = points[i];
+            }
+
+            return H.Take(k - 1).ToList();
+        }
+
+        public static bool IsPointInPolygon4(PointF[] polygon, PointF testPoint)
+        {
+            // ray casting odd even test.
+            bool result = false;
+            int j = polygon.Count() - 1;
+            for (int i = 0; i < polygon.Count(); i++)
+            {
+                if (polygon[i].Y < testPoint.Y && polygon[j].Y >= testPoint.Y ||
+                    polygon[j].Y < testPoint.Y && polygon[i].Y >= testPoint.Y)
+                {
+                    if (polygon[i].X + (testPoint.Y - polygon[i].Y) / (polygon[j].Y - polygon[i].Y) *
+                        (polygon[j].X - polygon[i].X) < testPoint.X)
+                    {
+                        result = !result;
+                    }
+                }
+
+                j = i;
+            }
+
+            return result;
+        }
+
+        public static double Exp(double val)
+        {
+            if (val < -20) return 0.0000001;
+            if (val > 20) return 99999999999999;
+            long tmp = (long)(1512775 * val + 1072632447);
+            return BitConverter.Int64BitsToDouble(tmp << 32);
+        }
+        public static double gaussmf(double x, double sig, double c)
+        {
+            return Exp(-(x - c) * (x - c) / (2 * sig * sig));
+        }
+
+        public static float Exp(float x)
+        {
+            if (x < -10) return 0;
+            if (x > 10) return 99999999999999;
+            x = 1.0f + x / 64f;
+            x *= x;
+            x *= x;
+            x *= x;
+            x *= x;
+            x *= x;
+            x *= x;
+            return x;
+        }
+        public static float gaussmf(float x, float sig, float c)
+        {
+            return Exp(-(x - c) * (x - c) / (2 * sig * sig));
+        }
+
+        public static D3 Transform2D(D3 src, D3 t)
+        {
+            var rth = src.Item3 / 180.0 * System.Math.PI;
+            var p1dtx = (src.Item1 + System.Math.Cos(rth) * t.Item1 -
+                                System.Math.Sin(rth) * t.Item2);
+            var p1dty = (src.Item2 + System.Math.Sin(rth) * t.Item1 +
+                                System.Math.Cos(rth) * t.Item2);
+            var p1dtth = src.Item3 + t.Item3;
+            return Tuple.Create(p1dtx, p1dty, p1dtth);
+        }
+
+        public struct LngLatToXY
+        {
+            public double scale;
+            public double rad;
+            public double biasX, biasY;
+        }
+
+        public static LngLatToXY GetTransformLngLatToXY(Vector2 lnglat1, Vector2 xy1, Vector2 lnglat2, Vector2 xy2)
+        {
+            var scale = (xy1 - xy2).Length() / (lnglat1 - lnglat2).Length();
+            var dxy = (xy1 - xy2);
+            var dlnglat = lnglat1 - lnglat2;
+            var rad = System.Math.Atan2(dxy.X, dxy.Y) - System.Math.Atan2(dlnglat.X, dlnglat.Y);
+            var intm = lnglat1 * scale;
+            var biasX = xy1.X - (intm.X * System.Math.Cos(rad) - intm.Y * System.Math.Sin(rad));
+            var biasY = xy1.Y - (intm.X * System.Math.Sin(rad) + intm.Y * System.Math.Cos(rad));
+            return new LngLatToXY {rad = rad, biasX = biasX, biasY = biasY, scale = scale};
+        }
+
+        public Vector2 TransformLngLatToXY(Vector2 lnglat, LngLatToXY t)
+        {
+            var intm = lnglat * (float) t.scale;
+            return new Vector2((float) (intm.X * System.Math.Cos(t.rad) - intm.Y * System.Math.Sin(t.rad) + t.biasX),
+                (float) (intm.X * System.Math.Sin(t.rad) + intm.Y * System.Math.Cos(t.rad) + t.biasY));
+        }
+
+        public static D3 ReverseTransform(D3 dest, D3 t)
+        {
+            var rth = (dest.Item3 - t.Item3) / 180.0 * System.Math.PI;
+            var nxT = (dest.Item1 - System.Math.Cos(rth) * t.Item1 +
+                              System.Math.Sin(rth) * t.Item2);
+            var nyT = (dest.Item2 - System.Math.Sin(rth) * t.Item1 -
+                              System.Math.Cos(rth) * t.Item2);
+            var pth = dest.Item3 - t.Item3;
+            return Tuple.Create(nxT, nyT, pth);
+        }
+
+        public static D3 SolveTransform2D(D3 src, D3 dest)
+        {
+            var th = dest.Item3 - src.Item3;
+            th = (th - System.Math.Round((th) / 360.0f) * 360);
+            var rth = src.Item3 / 180.0 * System.Math.PI;
+            var x = ((dest.Item1 - src.Item1) * System.Math.Cos(rth) +
+                            (dest.Item2 - src.Item2) * System.Math.Sin(rth));
+            var y = (-(dest.Item1 - src.Item1) * System.Math.Sin(rth) +
+                            (dest.Item2 - src.Item2) * System.Math.Cos(rth));
+            return Tuple.Create(x, y, th);
+        }
+
+        public static T3 Transform2D(T3 src, T3 t)
+        {
+            var rth = src.Item3 / 180.0 * System.Math.PI;
+            var p1dtx = (float)(src.Item1 + System.Math.Cos(rth) * t.Item1 -
+                                System.Math.Sin(rth) * t.Item2);
+            var p1dty = (float)(src.Item2 + System.Math.Sin(rth) * t.Item1 +
+                                System.Math.Cos(rth) * t.Item2);
+            var p1dtth = src.Item3 + t.Item3;
+            return Tuple.Create(p1dtx, p1dty, p1dtth);
+        }
+
+        public static Vector2 Transform2D(Vector3 src, Vector3 t)
+        {
+	        var tup = Transform2D(Tuple.Create(src.X, src.Y, src.Z), Tuple.Create(t.X, t.Y, t.Z));
+	        return new Vector2(tup.Item1, tup.Item2);
+        }
+
+        public static Vector2 Transform2D(Vector2 srcPos, float srcTh, Vector2 dt, float dth = 0)
+        {
+	        var tup = Transform2D(Tuple.Create(srcPos.X, srcPos.Y, srcTh), Tuple.Create(dt.X, dt.Y, dth));
+	        return new Vector2(tup.Item1, tup.Item2);
+		}
+
+		public static T3 ReverseTransform(T3 dest, T3 t)
+        {
+            var rth = (dest.Item3 - t.Item3) / 180.0 * System.Math.PI;
+            var nxT = (float)(dest.Item1 - System.Math.Cos(rth) * t.Item1 +
+                              System.Math.Sin(rth) * t.Item2);
+            var nyT = (float)(dest.Item2 - System.Math.Sin(rth) * t.Item1 -
+                              System.Math.Cos(rth) * t.Item2);
+            var pth = dest.Item3 - t.Item3;
+            return Tuple.Create(nxT, nyT, pth);
+        }
+
+        public static T3 SolveTransform2D(T3 src, T3 dest)
+        {
+            var th = dest.Item3 - src.Item3;
+            th = (float)(th - System.Math.Round((th) / 360.0f) * 360);
+            var rth = src.Item3 / 180.0 * System.Math.PI;
+            var x = (float)((dest.Item1 - src.Item1) * System.Math.Cos(rth) +
+                            (dest.Item2 - src.Item2) * System.Math.Sin(rth));
+            var y = (float)(-(dest.Item1 - src.Item1) * System.Math.Sin(rth) +
+                            (dest.Item2 - src.Item2) * System.Math.Cos(rth));
+            return Tuple.Create(x, y, th);
+        }
+
+		public static Vector2 SolveTransform2D(Vector2 srcPos, float srcTh, Vector2 dt, float dth = 0)
+		{
+			var tup = SolveTransform2D(Tuple.Create(srcPos.X, srcPos.Y, srcTh), Tuple.Create(dt.X, dt.Y, dth));
+			return new Vector2(tup.Item1, tup.Item2);
+		}
+
+		public static double dist(double x1, double y1, double x2, double y2)
+        {
+            return System.Math.Sqrt((x1 - x2) * (x1 - x2) + (y1 - y2) * (y1 - y2));
+        }
+
+        [StructLayout(LayoutKind.Explicit)]
+        private struct FloatIntUnion
+        {
+            [FieldOffset(0)] public float f;
+
+            [FieldOffset(0)] public int tmp;
+        }
+
+        public static float Sqrt(float z)
+        {
+            FloatIntUnion u;
+            u.tmp = 0;
+            u.f = z;
+            u.tmp -= 1 << 23; /* Subtract 2^m. */
+            u.tmp >>= 1; /* Divide by 2. */
+            u.tmp += 1 << 29; /* Add ((b + 1) / 2) * 2^m. */
+            return u.f;
+        }
+
+        public static float dist2(float x1, float y1, float x2, float y2)
+        {
+            return ((x1 - x2) * (x1 - x2) + (y1 - y2) * (y1 - y2));
+        }
+
+
+        public static float d2(float x1, float y1, float x2, float y2)
+        {
+            return (x1 - x2) * (x1 - x2) + (y1 - y2) * (y1 - y2);
+        }
+
+        public static float ThAverage(List<float> angles)
+        {
+	        var anchor = angles[0];
+	        var diff = 0f;
+	        foreach (var angle in angles)
+		        diff += ThDiff(angle, anchor);
+	        return RoundTh(anchor + diff / angles.Count);
+        }
+
+        public static float ThDiff(float th1, float th2)
+        {
+            return (float)(th1 - th2 -
+                           System.Math.Round((th1 - th2) / 360.0f) * 360);
+        }
+
+        public static double ThDiff(double th1, double th2)
+        {
+            return th1 - th2 -
+                           System.Math.Round((th1 - th2) / 360.0f) * 360;
+        }
+
+        public static double refine(double x)
+        {
+            if (x < 1 && x > -1) return x;
+            if (x > 1)
+                return (2 / (1 + System.Math.Exp(-((x - 1) * 2))));
+            return (2 / (1 + System.Math.Exp(-((x + 1) * 2)))) - 2;
+        }
+
+        /// <summary>
+        /// 求点p到两点式直线p1p2的距离
+        /// </summary>
+        /// <param name="x">点p的x坐标</param>
+        /// <param name="y">点p的y坐标</param>
+        /// <param name="x1">直线点p1的x坐标</param>
+        /// <param name="y1">直线点p1的y坐标</param>
+        /// <param name="x2">直线点p2的x坐标</param>
+        /// <param name="y2">直线点p2的y坐标</param>
+        /// <returns></returns>
+        public static double Point2LineDist(double x, double y, double x1, double y1, double x2, double y2)
+        {
+            double a1 = -(y1 - y2) / 10;
+            double b1 = (x1 - x2) / 10;
+            double c1 = (x1 * (y1 - y2) - y1 * (x1 - x2)) / 10;
+            return System.Math.Abs(a1 * x + b1 * y + c1) / System.Math.Sqrt(a1 * a1 + b1 * b1);
+        }
+
+        public static double Point2LineDist(Vector2 p, LineSegment ll)
+        {
+            double a1 = -(ll.Src.Y - ll.Dst.Y) / 10;
+            double b1 = (ll.Src.X - ll.Dst.X) / 10;
+            double c1 = (ll.Src.X * (ll.Src.Y - ll.Dst.Y) - ll.Src.Y * (ll.Src.X - ll.Dst.X)) / 10;
+            return System.Math.Abs(a1 * p.X + b1 * p.Y + c1) / System.Math.Sqrt(a1 * a1 + b1 * b1);
+        }
+
+        /// <summary>
+        /// 两条两点式直线间的夹角
+        /// </summary>
+        /// <param name="x1"></param>
+        /// <param name="y1"></param>
+        /// <param name="x2"></param>
+        /// <param name="y2"></param>
+        /// <param name="x3"></param>
+        /// <param name="y3"></param>
+        /// <param name="x4"></param>
+        /// <param name="y4"></param>
+        /// <returns>角度制</returns>
+        public static double AngleBetweenLines(double x1, double y1, double x2, double y2, double x3, double y3,
+            double x4, double y4)
+        {
+            var vec1 = new Vector2((float)(x2 - x1), (float)(y2 - y1));
+            var vec2 = new Vector2((float)(x4 - x3), (float)(y4 - y3));
+            return System.Math.Acos(System.Math.Abs(Vector2.Dot(vec1, vec2) / vec1.Length() / vec2.Length())) / System.Math.PI * 180;
+        }
+
+        public static double AngleBetweenLines(LineSegment ls1, LineSegment ls2)
+        {
+	        return AngleBetweenLines(ls1.Src.X, ls1.Src.Y, ls1.Dst.X, ls1.Dst.Y, ls2.Src.X, ls2.Src.Y, ls2.Dst.X,
+		        ls2.Dst.Y);
+        }
+
+		/// <summary>
+		/// 两向量间夹角
+		/// </summary>
+		/// <param name="x1"></param>
+		/// <param name="y1"></param>
+		/// <param name="x2"></param>
+		/// <param name="y2"></param>
+		/// <param name="x3"></param>
+		/// <param name="y3"></param>
+		/// <param name="x4"></param>
+		/// <param name="y4"></param>
+		/// <returns>角度制</returns>
+		public static double AngleBetweenVectors(double x1, double y1, double x2, double y2, double x3, double y3,
+            double x4, double y4)
+        {
+            var vec1 = new Vector2((float)(x2 - x1), (float)(y2 - y1));
+            var vec2 = new Vector2((float)(x4 - x3), (float)(y4 - y3));
+            return System.Math.Acos(Vector2.Dot(vec1, vec2) / vec1.Length() / vec2.Length()) / System.Math.PI * 180;
+        }
+
+        /// <summary>
+        /// 两向量间夹角.
+        /// </summary>
+        /// <param name="vec1"></param>
+        /// <param name="vec2"></param>
+        /// <returns>角度制</returns>
+        public static double AngleBetweenVectors(Vector2 vec1, Vector2 vec2)
+        {
+            return System.Math.Acos(Vector2.Dot(vec1, vec2) / vec1.Length() / vec2.Length()) / System.Math.PI * 180;
+        }
+
+        public static double AngleBetweenVectors(Vector3 vector1, Vector3 vector2)
+        {
+	        float dotProduct = Vector3.Dot(vector1, vector2);
+	        float magnitude1 = vector1.Length();
+	        float magnitude2 = vector2.Length();
+	        float cosine = dotProduct / (magnitude1 * magnitude2);
+
+	        return System.Math.Acos(cosine) / System.Math.PI * 180;
+        }
+
+		/// <summary>
+		/// 求点到直线的垂足
+		/// </summary>
+		/// <param name="x"></param>
+		/// <param name="y"></param>
+		/// <param name="x1"></param>
+		/// <param name="y1"></param>
+		/// <param name="x2"></param>
+		/// <param name="y2"></param>
+		/// <returns></returns>
+		public static (double, double) PerpendicularPoint(double x, double y, double x1, double y1, double x2, double y2)
+        {
+            double lx = x2 - x1, ly = y2 - y1, dAB = lx * lx + ly * ly;
+            var u = ((x - x1) * lx + (y - y1) * ly) / dAB;
+            return new(x1 + u * lx, y1 + u * ly);
+        }
+
+        public static Vector2 PerpendicularPoint(Vector2 p, LineSegment ls)
+        {
+            double lx = ls.Dst.X - ls.Src.X, ly = ls.Dst.Y - ls.Src.Y, dAB = lx * lx + ly * ly;
+            var u = ((p.X - ls.Src.X) * lx + (p.Y - ls.Src.Y) * ly) / dAB;
+            return new Vector2((float)(ls.Src.X + u * lx), (float)(ls.Src.Y + u * ly));
+        }
+
+        public static double PerpendicularPosition(double x, double y, double x1, double y1, double x2, double y2)
+        {
+            double lx = x2 - x1, ly = y2 - y1;
+            var dAB = CommonMath.Sqrt((float)(lx * lx + ly * ly));
+            lx /= dAB;
+            ly /= dAB;
+            return (x - x1) * lx + (y - y1) * ly;
+        }
+
+
+        /// <summary>
+        /// 最小二乘法拟合直线，得到两点式。
+        /// </summary>
+        /// <param name="pts">待拟合的点集，应至少有2个点。</param>
+        /// <param name="maxDist2Line">检查是否所有点距直线的距离均小于maxDist2Line，若为-1则不检查。</param>
+        /// <returns>返回两点式的两个端点坐标。若坐标为全0，则拟合失败。</returns>
+        public static (bool, Vector2, Vector2) FitLineSegment(List<Vector2> pts, double maxDist2Line = -1)
+        {
+            if (pts.Count < 2)
+            {
+                Console.WriteLine($"Points too Few! {pts.Count}! Cannot perform line fitting!",
+                    MethodBase.GetCurrentMethod()?.Name ?? "FitLine");
+                return (false, Vector2.Zero, Vector2.Zero);
+            };
+
+            // y = kx + b
+            double A = 0, B = 0, C = 0, D = 0;
+            foreach (var p in pts)
+            {
+                A += p.X * p.X;
+                B += p.X;
+                C += p.X * p.Y;
+                D += p.Y;
+            }
+
+            var tmp = A * pts.Count - B * B;
+            var k = (C * pts.Count - B * D) / tmp;
+            var b = (A * D - C * B) / tmp;
+
+            double x1 = 0,
+                y1 = k * x1 + b,
+                x2 = 1000,
+                y2 = k * x2 + b;
+
+            double CalcDist(ref bool fail, ref Vector2 endP, ref Vector2 endQ)
+            {
+                double distSum = 0;
+                double lx = x2 - x1, ly = y2 - y1, dAB = lx * lx + ly * ly;
+                double minU = double.MaxValue, maxU = double.MinValue;
+
+                foreach (var p in pts)
+                {
+                    var u = ((p.X - x1) * lx + (p.Y - y1) * ly) / dAB;
+                    var perp = new Vector2((float)(x1 + u * lx), (float)(y1 + u * ly));
+                    if (u < minU)
+                    {
+                        endP = perp;
+                        minU = u;
+                    }
+                    if (u > maxU)
+                    {
+                        endQ = perp;
+                        maxU = u;
+                    }
+
+                    var curDist = dist(perp.X, perp.Y, p.X, p.Y);
+                    if (maxDist2Line > -1 && curDist > maxDist2Line) fail = true;
+                    distSum += curDist;
+                }
+
+                return distSum;
+            }
+
+            var kbFail = false;
+            Vector2 endP1 = new Vector2(), endQ1 = new Vector2();
+            double kbDist = CalcDist(ref kbFail, ref endP1, ref endQ1);
+            
+            // x = my + n
+            A = 0;
+            B = 0;
+            C = 0;
+            D = 0;
+            foreach (var p in pts)
+            {
+                A += p.X * p.Y;
+                B += p.Y * p.Y;
+                C += p.Y;
+                D += p.X;
+            }
+
+            tmp = C * C - B * pts.Count;
+            var m = (C * D - A * pts.Count) / tmp;
+            var n = (A * C - B * D) / tmp;
+            y1 = 0;
+            x1 = m * y1 + n;
+            y2 = 1000;
+            x2 = m * y2 + n;
+
+            var mnFail = false;
+            Vector2 endP2 = new Vector2(), endQ2 = new Vector2();
+            double mnDist = CalcDist(ref mnFail, ref endP2, ref endQ2);
+
+            Vector2 endP = endP1, endQ = endQ1;
+            if (mnDist < kbDist)
+            {
+                if (mnFail) return (false, new Vector2(), new Vector2());
+                endP = endP2;
+                endQ = endQ2;
+            }
+            else if (kbFail) return (false, new Vector2(), new Vector2());
+
+            return (true, endP, endQ);
+        }
+
+        [MethodImpl(MethodImplOptions.AggressiveInlining)]
+        public static int toId(int x, int y, int z)
+        {
+            return (x * 1140671485 + 12820163) ^ (y * 134775813 + 1) ^ (z * 1103515245 + 12345);
+        }
+
+        public class Clustering<T>
+        {
+	        public int numIteration = 3;
+	        public int itemNumThreshold = 10;
+
+	        public Func<T, T, bool> inRange;
+	        public Func<List<T>, T> average;
+	        private readonly List<T> _inputData;
+	        private Dictionary<T, List<T>> _clusters = new Dictionary<T, List<T>>();
+
+	        public Clustering(List<T> data, Func<T, T, bool> inRange, Func<List<T>, T> average)
+	        {
+		        _inputData = data;
+		        this.inRange = inRange;
+		        this.average = average;
+	        }
+
+	        public Dictionary<T, List<T>> GetClusters()
+	        {
+		        var tmp = new List<(T center, List<T> items)>();
+
+		        for (var iter = 0; iter < numIteration; iter++)
+		        {
+			        tmp = tmp.Where(cluster => cluster.items.Count > itemNumThreshold)
+				        .Select(cluster => (average(cluster.items), new List<T>())).ToList();
+
+			        foreach (var data in _inputData)
+			        {
+				        var added = false;
+				        foreach (var cluster in tmp)
+				        {
+					        if (inRange(cluster.center, data))
+					        {
+						        cluster.items.Add(data);
+						        added = true;
+						        break;
+					        }
+				        }
+
+				        if (!added)
+					        tmp.Add((data, new List<T>() { data }));
+			        }
+		        }
+
+		        _clusters = tmp.Where(cluster => cluster.items.Count > itemNumThreshold)
+			        .ToDictionary(cluster => cluster.center, cluster => cluster.items);
+		        return _clusters;
+	        }
+        }
+
+        public static (bool, Vector2) TwoLinesIntersection(Vector2 A, Vector2 B, Vector2 C, Vector2 D)
+        {
+	        // Line AB represented as a1x + b1y = c1
+	        double a1 = B.Y - A.Y;
+	        double b1 = A.X - B.X;
+	        double c1 = a1 * (A.X) + b1 * (A.Y);
+
+	        // Line CD represented as a2x + b2y = c2
+	        double a2 = D.Y - C.Y;
+	        double b2 = C.X - D.X;
+	        double c2 = a2 * (C.X) + b2 * (C.Y);
+
+	        double determinant = a1 * b2 - a2 * b1;
+
+	        if (determinant == 0)
+	        {
+		        // The lines are parallel. This is simplified
+		        // by returning a pair of FLT_MAX
+		        return new(false, new Vector2());
+	        }
+	        else
+	        {
+		        double x = (b2 * c1 - b1 * c2) / determinant;
+		        double y = (a1 * c2 - a2 * c1) / determinant;
+		        return (true, new Vector2((float)x, (float)y));
+	        }
+        }
+
+        public static bool IsAtLeft(Vector2 anchor, Vector2 dest, Vector2 p)
+        {
+	        var v1 = new Vector3(anchor - p, 0);
+	        var v2 = new Vector3(dest - p, 0);
+	        return Vector3.Cross(v1, v2).Z > 0;
+        }
+
+        /// <summary>
+        /// 将角度转化至-180到180度的范围内。
+        /// </summary>
+        /// <param name="th"></param>
+        /// <returns></returns>
+        public static double RoundTh(double th)
+        {
+	        return th - System.Math.Round(th / 360) * 360;
+        }
+
+        /// <summary>
+        /// 将角度转化至-180到180度的范围内。
+        /// </summary>
+        /// <param name="th"></param>
+        /// <returns></returns>
+        public static float RoundTh(float th)
+        {
+	        return th - (float)System.Math.Round(th / 360f) * 360f;
+        }
+	}
+}

CommonUsage-MultiVehicleSync/commonusage/Mathematics/LineSegment.cs

@@ -0,0 +1,161 @@
+using System;
+using System.Numerics;
+
+namespace CommonUsage.Mathematics
+{
+	/// <summary>
+	/// 表示一条线段。
+	/// </summary>
+	public class LineSegment
+	{
+		/// <summary>
+		/// 默认构造函数。所有坐标初始化为0。
+		/// </summary>
+		public LineSegment()
+		{
+
+		}
+
+		/// <summary>
+		/// 使用两个端点初始化一段2D线段。
+		/// </summary>
+		/// <param name="src">线段起点。</param>
+		/// <param name="dst">线段终点。</param>
+		public LineSegment(Vector2 src, Vector2 dst)
+		{
+			Src = src;
+			Dst = dst;
+		}
+
+		/// <summary>
+		/// 使用两个端点初始化一段3D线段。
+		/// </summary>
+		/// <param name="src">线段起点。</param>
+		/// <param name="dst">线段终点。</param>
+		public LineSegment(Vector3 src, Vector3 dst)
+		{
+			Src3D = src;
+			Dst3D = dst;
+		}
+
+		/// <summary>
+		/// 使用两个端点初始化一条线段，2D。
+		/// </summary>
+		/// <param name="x1">线段起点x坐标。</param>
+		/// <param name="y1">线段起点y坐标。</param>
+		/// <param name="x2">线段终点x坐标。</param>
+		/// <param name="y2">线段终点y坐标。</param>
+		public LineSegment(double x1, double y1, double x2, double y2)
+		{
+			Src = new Vector2((float)x1, (float)y1);
+			Dst = new Vector2((float)x2, (float)y2);
+		}
+
+		/// <summary>
+		/// 使用两个端点初始化一条线段，2D。
+		/// </summary>
+		/// <param name="x1">线段起点x坐标。</param>
+		/// <param name="y1">线段起点y坐标。</param>
+		/// <param name="z1">线段起点y坐标。</param>
+		/// <param name="x2">线段终点x坐标。</param>
+		/// <param name="y2">线段终点y坐标。</param>
+		/// <param name="z2">线段终点y坐标。</param>
+		public LineSegment(double x1, double y1, double z1, double x2, double y2, double z2)
+		{
+			Src3D = new Vector3((float)x1, (float)y1, (float)z1);
+			Dst3D = new Vector3((float)x2, (float)y2, (float)z2);
+		}
+
+		/// <summary>
+		/// 返回线段长度，2D。
+		/// </summary>
+		/// <returns></returns>
+		public double Length()
+		{
+			return Vector2.Distance(Src, Dst);
+		}
+
+		/// <summary>
+		/// 返回线段长度，3D。
+		/// </summary>
+		/// <returns></returns>
+		public double Length3D()
+		{
+			return Vector3.Distance(Src3D, Dst3D);
+		}
+
+		/// <summary>
+		/// 返回线段与x轴正方向夹角度数，角度制。
+		/// </summary>
+		/// <returns></returns>
+		public double Angle()
+		{
+			return Math.Atan2(Dst.Y - Src.Y, Dst.X - Src.X) / Math.PI * 180;
+		}
+
+		/// <summary>
+		/// 返回一段方向相反的线段。
+		/// </summary>
+		/// <returns></returns>
+		public LineSegment Reverse()
+		{
+			return new LineSegment(Dst3D, Src3D);
+		}
+
+		/// <summary>
+		/// 线段2D起点。
+		/// </summary>
+		public Vector2 Src
+		{
+			get => new(_srcX, _srcY);
+			set
+			{
+				_srcX = value.X;
+				_srcY = value.Y;
+			}
+		}
+
+		/// <summary>
+		/// 线段2D终点。
+		/// </summary>
+		public Vector2 Dst
+		{
+			get => new(_dstX, _dstY);
+			set
+			{
+				_dstX = value.X;
+				_dstY = value.Y;
+			}
+		}
+
+		/// <summary>
+		/// 线段3D起点。
+		/// </summary>
+		public Vector3 Src3D
+		{
+			get => new(_srcX, _srcY, _srcZ);
+			set
+			{
+				_srcX = value.X;
+				_srcY = value.Y;
+				_srcZ = value.Z;
+			}
+		}
+
+		/// <summary>
+		/// 线段3D终点。
+		/// </summary>
+		public Vector3 Dst3D
+		{
+			get => new(_dstX, _dstY, _dstZ);
+			set
+			{
+				_dstX = value.X;
+				_dstY = value.Y;
+				_dstZ = value.Z;
+			}
+		}
+
+		private float _srcX, _srcY, _srcZ, _dstX, _dstY, _dstZ;
+	}
+}

CommonUsage-MultiVehicleSync/commonusage/Properties/launchSettings.json

@@ -0,0 +1,12 @@
+{
+  "profiles": {
+    "CommonUsage": {
+      "commandName": "Project"
+    },
+    "配置文件 1": {
+      "commandName": "Executable",
+      "executablePath": "D:\\Code\\Core\\Medulla\\build\\Medulla.exe",
+      "workingDirectory": "D:\\Code\\Core\\Medulla\\build\\"
+    }
+  }
+}

CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/CommunicationProtocolFactory.cs

@@ -0,0 +1,19 @@
+using System;
+using System.Collections.Generic;
+using System.Text;
+
+namespace CommonUsage.Protocols.VDA5050
+{
+    public class CommunicationProtocolFactory
+    {
+        public static IVDACommunicationProtocol CreateProtocol(string protocolType, string host, int port)
+        {
+            return protocolType.ToLower() switch
+            {
+                "http" => new HTTPCommunication(host, port),
+                "mqtt" => new MQTTCommunication(host, port),
+                _ => throw new NotSupportedException($"Protocol {protocolType} is not supported")
+            };
+        }
+    }
+}

CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/HTTPCommunication.cs

@@ -0,0 +1,94 @@
+using System;
+using System.Collections.Generic;
+using System.Net.Http;
+using System.Text;
+using System.Threading.Tasks;
+using CommonUsage.Protocols.VDA5050.Messages;
+using FundamentalLib;
+using Newtonsoft.Json;
+
+namespace CommonUsage.Protocols.VDA5050
+{
+    public class HTTPCommunication : IVDACommunicationProtocol
+    {
+        private readonly string _host;
+        private readonly int _port;
+
+        public HTTPCommunication(string host, int port)
+        {
+            _host = host;
+            _port = port;
+        }
+
+        public async Task PublishConnectionStatus(string status)
+        {
+            var message = new connectionMessage()
+            {
+                serialNumber = "test-01",
+                headerId = 1,
+                timestamp = DateTime.Now,
+                connectionState = status
+            };
+
+            await SendMessageAsync(message, "vda5050/connection");
+        }
+
+        public void SetupOrderListener(Action<orderMessage> orderReceived)
+        {
+            PicoHttpServer.AddPostTextHandler("/order", new { }, (_, str) =>
+            {
+                var order = JsonConvert.DeserializeObject<orderMessage>(str);
+                orderReceived(order);
+                return "";
+            });
+        }
+
+        public void SetUpInstanActionListener(Action<instanceAction> onInstanceActionReceived)
+        {
+            PicoHttpServer.AddPostTextHandler("/instanceAction", new { }, (_, str) =>
+            {
+                var instanceAction = JsonConvert.DeserializeObject<instanceAction>(str);
+                onInstanceActionReceived(instanceAction);
+                return "";
+            });
+        }
+
+        public void SetUpImmediateCommandListener(Action<string> onChangeCarFieldsReceived)
+        {
+            throw new NotImplementedException();
+        }
+
+        public async Task SendMessageAsync<T>(T message, string topic)
+        {
+            try
+            {
+                var url = $"http://{_host}:{_port}/{topic}";
+                using var client = new HttpClient();
+                var response = await client.PostAsync(url, new StringContent(JsonConvert.SerializeObject(message), Encoding.UTF8, "application/json"));
+
+                if (!response.IsSuccessStatusCode)
+                {
+                    Console.WriteLine($" >> Sending Message: Failed to send message. Status Code: {response.StatusCode}");
+                }
+            }
+            catch (Exception ex)
+            {
+                Console.WriteLine($"Error in sending message: {ex.Message}");
+            }
+        }
+
+        public async Task SendVisualizationMessageAsync<T>(T msg, string topic)
+        {
+            throw new NotImplementedException();
+        }
+
+        public void SetupTestListener(Action<string> testMsg)
+        {
+            throw new NotImplementedException();
+        }
+        public async Task PublishFactSheet(factsheetMessage message)
+        {
+            throw new NotImplementedException();
+        }
+    }
+}

CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/IVDACommunicationProtocol.cs

@@ -0,0 +1,19 @@
+using System;
+using System.Collections.Generic;
+using System.Text;
+using System.Threading.Tasks;
+using CommonUsage.Protocols.VDA5050.Messages;
+
+namespace CommonUsage.Protocols.VDA5050
+{
+    public interface IVDACommunicationProtocol
+    {
+        Task PublishConnectionStatus(string status);
+        Task PublishFactSheet(factsheetMessage msg);
+        void SetupOrderListener(Action<orderMessage> orderReceived);
+        void SetUpInstanActionListener(Action<instanceAction> onInstanceActionReceived);
+        void SetUpImmediateCommandListener(Action<string> onChangeCarFieldsReceived);
+        Task SendMessageAsync<T>(T message, string topic);
+        Task SendVisualizationMessageAsync<T>(T message, string topic);
+    }
+}

CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/MQTTCommunication.cs

@@ -0,0 +1,319 @@
+using System.IO;
+using System;
+using System.Collections.Generic;
+using System.Text;
+using System.Threading;
+using System.Threading.Tasks;
+using CommonUsage.Protocols.VDA5050.Messages;
+using MQTTnet;
+using MQTTnet.Client;
+using MQTTnet.Extensions.ManagedClient;
+using MQTTnet.Packets;
+using MQTTnet.Protocol;
+using MQTTnet.Server;
+using Newtonsoft.Json;
+using FundamentalLib;
+using CommonUsage.Protocols.VDA5050.Objects;
+using FundamentalLib.MiscHelpers;
+
+namespace CommonUsage.Protocols.VDA5050
+{
+    public class MQTTCommunication : IVDACommunicationProtocol
+    {
+        private readonly string _host;
+        private readonly int _port;
+        private readonly string _orderTopic = "vda5050/frldAGV/order";
+        private readonly string _connectionTopic = "vda5050/frldAGV/connection";
+        private readonly string _instanceAction = "vda5050/frldAGV/instantActions";
+        private readonly string _factsheet = "vda5050/frldAGV/factsheet";
+        private readonly string _changeCarFields = "vda5050/frldAGV/changeCarFields";
+
+        private IManagedMqttClient _client;
+        private IManagedMqttClient _visualizationClient;
+
+        public MQTTCommunication(string host, int port)
+        {
+            _host = host;
+            _port = port;
+            InitializeClient();
+            InitializeVisualizationClient();
+        }
+
+        private void InitializeClient()
+        {
+            var willMessage = new connectionMessage()
+            {
+                headerId = 1,
+                timestamp = DateTime.Now,
+                version = "00",
+                manufacturer = "frld",
+                serialNumber = "test-01",
+                connectionState = "CONNECTIONBROKEN"
+            };
+
+            var mqttClientOptions = new MqttClientOptionsBuilder()
+                .WithClientId("AGV-Client-frldAGV")
+                .WithTcpServer(_host, _port)
+                .WithWillTopic(_connectionTopic)
+                .WithWillPayload(JsonConvert.SerializeObject(willMessage))
+                 .WithWillRetain(true)
+                .Build();
+
+            var managedMqttClientOptions = new ManagedMqttClientOptionsBuilder()
+                .WithClientOptions(mqttClientOptions)
+                .WithMaxPendingMessages(20)
+                .WithPendingMessagesOverflowStrategy(MqttPendingMessagesOverflowStrategy.DropOldestQueuedMessage)
+                .Build();
+
+            _client = new MqttFactory().CreateManagedMqttClient();
+            _client.StartAsync(managedMqttClientOptions).GetAwaiter().GetResult();
+            Console.WriteLine($" >> MQTT client initialized and connected to broker at {_host} - {_port}");
+        }
+
+        private void InitializeVisualizationClient()
+        {
+            var mqttClientOptions = new MqttClientOptionsBuilder()
+                .WithClientId("AGV-Visualization")
+                .WithTcpServer(_host, _port)
+                .Build();
+
+            var managedMqttClientOptions = new ManagedMqttClientOptionsBuilder()
+                .WithClientOptions(mqttClientOptions)
+                .WithMaxPendingMessages(10) // Prevent overloading
+                .WithPendingMessagesOverflowStrategy(MqttPendingMessagesOverflowStrategy.DropOldestQueuedMessage)
+                .Build();
+
+            _visualizationClient = new MqttFactory().CreateManagedMqttClient();
+            _visualizationClient.StartAsync(managedMqttClientOptions).GetAwaiter().GetResult();
+            Console.WriteLine("MQTT Visualization client initialized.");
+        }
+
+        public void SetUpImmediateCommandListener(Action<string> onChangeCarFieldsReceived)
+        {
+            Console.WriteLine($"Subscribed to the topic: {_changeCarFields}");
+            _client.SubscribeAsync(_changeCarFields).GetAwaiter().GetResult();
+
+            _client.ApplicationMessageReceivedAsync += async e =>
+            {
+                if (e.ApplicationMessage.Topic == _changeCarFields)
+                {
+                    var payload = Encoding.UTF8.GetString(e.ApplicationMessage.Payload);
+                    LogMessage($"RECEIVE-{e.ApplicationMessage.Topic}", e.ApplicationMessage.Topic, payload);
+                    //var script = JsonConvert.DeserializeObject<string>(payload);
+                    Console.WriteLine($"Change car field: {payload}");
+                    onChangeCarFieldsReceived(payload);
+                }
+            };
+        }
+
+
+        public void SetUpInstanActionListener(Action<instanceAction> onInstanceActionReceived)
+        {
+            Console.WriteLine($"Subscribed to the topic: {_instanceAction}");
+
+            // Subscribe to the instanceAction topic
+            _client.SubscribeAsync(_instanceAction).GetAwaiter().GetResult();
+
+            _client.ApplicationMessageReceivedAsync += async e =>
+            {
+                if (e.ApplicationMessage.Topic == _instanceAction)
+                {
+                    var payload = Encoding.UTF8.GetString(e.ApplicationMessage.Payload);
+                    LogMessage($"RECEIVE-{e.ApplicationMessage.Topic}", e.ApplicationMessage.Topic, payload);
+                    var actions = JsonConvert.DeserializeObject<instanceAction>(payload);
+                    Console.WriteLine($"Received instance action: Header ID = {actions.headerId}, Timestamp = {actions.timestamp}");
+                    foreach (var action in actions.actions)
+                    {
+                        Console.WriteLine($"Action ID: {action.actionId}, Type: {action.actionType}");
+                    }
+
+                    onInstanceActionReceived(actions);
+                }
+            };
+        }
+
+        public async Task PublishConnectionStatus(string status)
+        {
+            var message = new connectionMessage()
+            {
+                headerId = 1,
+                timestamp = DateTime.Now,
+                version = "00",
+                manufacturer = "frld",
+                serialNumber = "test-01",
+                connectionState = status
+            };
+
+            var payload = JsonConvert.SerializeObject(message);
+
+            var content = new MqttApplicationMessageBuilder()
+                .WithTopic(_connectionTopic)
+                .WithPayload(payload)
+                .WithQualityOfServiceLevel(MqttQualityOfServiceLevel.AtLeastOnce)
+                .WithRetainFlag(true)
+                .Build();
+
+            await _client.EnqueueAsync(content);
+            LogMessage($"SEND-{_connectionTopic}", _connectionTopic, payload);
+
+            //await SendMessageAsync(message, _connectionTopic);
+        }
+
+        public async Task PublishFactSheet(factsheetMessage message)
+        {
+         
+            //await SendMessageAsync(message, _factsheet);
+            var payload = JsonConvert.SerializeObject(message);
+
+            var content = new MqttApplicationMessageBuilder()
+                .WithTopic(_factsheet)
+                .WithPayload(payload)
+                .WithQualityOfServiceLevel(MqttQualityOfServiceLevel.AtMostOnce)
+                //.WithRetainFlag(true)
+                .Build();
+
+            await _client.EnqueueAsync(content);
+            LogMessage($"SEND-{_factsheet}", _factsheet, payload);
+
+        }
+
+        public void SetupOrderListener(Action<orderMessage> orderReceived)
+        {
+            // Subscribe to the orders topic
+            _client.SubscribeAsync(_orderTopic, MqttQualityOfServiceLevel.AtMostOnce).GetAwaiter().GetResult();
+            _client.ApplicationMessageReceivedAsync += async e =>
+            {
+                if (e.ApplicationMessage.Topic == _orderTopic)
+                {
+                    var payload = Encoding.UTF8.GetString(e.ApplicationMessage.Payload);
+                    LogMessage($"RECEIVE-{e.ApplicationMessage.Topic}", e.ApplicationMessage.Topic, payload);
+                    var order = JsonConvert.DeserializeObject<orderMessage>(payload);
+
+                    // Save the order message to a file for debugging
+                    // SaveOrderToFile(payload);
+                    orderReceived(order);
+                }
+                await Task.CompletedTask;
+            };
+        }
+
+        private void SaveOrderToFile(string orderJson)
+        {
+            try
+            {
+                // Specify the file path (e.g., orders_log.txt in the current directory)
+                string filePath = "orders_log.txt";
+
+                // Append the order JSON along with a timestamp
+                File.AppendAllText(filePath, $"{DateTime.UtcNow:yyyy-MM-dd HH:mm:ss} - {orderJson}{Environment.NewLine}");
+            }
+            catch (Exception ex)
+            {
+                // Handle any exceptions that occur while writing to the file
+                Console.WriteLine($"Failed to save order to file: {ex.Message}");
+            }
+        }
+
+        public async Task SendMessageAsync<T>(T message, string topic)
+        {
+
+            var payload = JsonConvert.SerializeObject(message);
+
+            var content = new MqttApplicationMessageBuilder()
+                .WithTopic(topic)
+                .WithPayload(payload)
+                .WithQualityOfServiceLevel(MqttQualityOfServiceLevel.AtMostOnce)
+                .Build();
+
+            await _client.EnqueueAsync(content);
+
+            if (topic != "vda5050/frldAGV/visualization")
+            {
+
+                LogMessage($"SEND-{topic}", topic, payload);
+            }
+           
+        }
+
+        public async Task SendVisualizationMessageAsync<T>(T message, string topic)
+        {
+            if (_visualizationClient == null) return; // Ensure client is initialized
+
+            var payload = JsonConvert.SerializeObject(message);
+
+            var content = new MqttApplicationMessageBuilder()
+                .WithTopic(topic)
+                .WithPayload(payload)
+                .WithQualityOfServiceLevel(MqttQualityOfServiceLevel.AtMostOnce) // QoS 0 for lightweight visualization
+                .Build();
+
+            if (_visualizationClient.PendingApplicationMessagesCount < 5) // Prevent flooding
+            {
+                await _visualizationClient.EnqueueAsync(content);
+            }
+            else
+            {
+                Console.WriteLine("Skipping visualization update to avoid MQTT congestion.");
+            }
+        }
+
+
+        public void LogMessage(string direction, string topic, string payload)
+        {
+            string formattedPayload = payload;
+            string logDirectory = "Logs"; // Directory for log files
+            var filePreName = direction;
+            filePreName = filePreName.Replace("/", "_");
+            string logFilePath = Path.Combine(logDirectory, filePreName + $"-{DateTime.Now:yyyy-MM-dd}.log");
+
+            if(!Directory.Exists(logDirectory)) Directory.CreateDirectory(logDirectory);
+            // Try to parse the payload as JSON and pretty-print it
+            try
+            {
+                var jsonObject = JsonConvert.DeserializeObject(payload);
+                formattedPayload = JsonConvert.SerializeObject(jsonObject, Formatting.Indented);
+            }
+            catch (JsonReaderException)
+            {
+                // If the payload is not valid JSON, just leave it as is
+                formattedPayload = payload;
+            }
+
+            string logMessage = $"[{DateTime.Now:HH:mm:ss}] [{direction}] Topic: {topic}, Payload:\n{formattedPayload}\n";
+            RotateLogFile(logFilePath, logDirectory);
+
+            try
+            {
+                File.AppendAllText(logFilePath, logMessage + Environment.NewLine);
+            }
+            catch (Exception ex)
+            {
+                Console.WriteLine($"Error writing to log file: {ex.Message}");
+            }
+            // Console.WriteLine($"[{DateTime.Now:HH:mm:ss}] [{direction}] Topic: {topic}, Payload: {formattedPayload}");
+            // DLog.Log($"[{DateTime.Now:HH:mm:ss}] [{direction}] Topic: {topic}, Payload: {formattedPayload}");
+        }
+
+
+        private void RotateLogFile(string logFilePath, string logDirectory)
+        {
+            const long maxFileSize = 10 * 1024 * 1024; // 10 MB in bytes
+
+            FileInfo fileInfo = new FileInfo(logFilePath);
+            if (fileInfo.Exists && fileInfo.Length > maxFileSize)
+            {
+                string archivePath = Path.Combine(logDirectory, $"log_{DateTime.Now:yyyy-MM-dd_HH-mm-ss}.log");
+
+                try
+                {
+                    File.Move(logFilePath, archivePath); // Rename the current log file
+                    Console.WriteLine($"Log file rotated: {archivePath}");
+                }
+                catch (Exception ex)
+                {
+                    Console.WriteLine($"Error rotating log file: {ex.Message}");
+                }
+            }
+        }
+    }
+}

CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/Messages/connectionMessage.cs

@@ -0,0 +1,18 @@
+using System;
+using System.Collections.Generic;
+using System.Text;
+
+namespace CommonUsage.Protocols.VDA5050.Messages
+{
+    public class connectionMessage
+    {
+        public int headerId;
+        public DateTime timestamp;
+        public string version = "";
+        public string manufacturer = "";
+        public string serialNumber = "";
+        public string connectionState = "";  // Enum: {'ONLINE', 'OFFLINE', 'CONNECTIONBROKEN'}
+        
+    }
+
+}

CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/Messages/errorMessage.cs

@@ -0,0 +1,16 @@
+using System;
+using System.Collections.Generic;
+using System.Text;
+
+namespace CommonUsage.Protocols.VDA5050.Messages
+{
+    public class errorMessage
+    {
+        public string serialNumber = "";
+        public string errorCode = "";  // Unique error code
+        public string description = "";  // Error description
+        public string severity = "";  // Enum {'WARNING', 'FATAL'}
+        public DateTime timestamp;  // Time of the error
+    }
+
+}

CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/Messages/factsheetMessage.cs

@@ -0,0 +1,23 @@
+using System;
+using System.Collections.Generic;
+using System.Text;
+using CommonUsage.Protocols.VDA5050.Objects;
+
+namespace CommonUsage.Protocols.VDA5050.Messages
+{
+    public class factsheetMessage
+    {
+        public int headerId;
+        public DateTime timestamp;
+        public string version = "";
+        public string manufacturer = "";
+        public string serialNumber = "";
+
+        public typeSpecification typeSpecification;
+        public physicalParameters physicalParameters;
+        public protocolLimits protocolLimits;
+        public protocolFeatures protocolFeatures;
+        public agvGeometry agvGeometry;
+    }
+
+}

CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/Messages/instanceAction.cs

@@ -0,0 +1,19 @@
+using System;
+using System.Collections.Generic;
+using System.Text;
+using CommonUsage.Protocols.VDA5050.Objects;
+
+namespace CommonUsage.Protocols.VDA5050.Messages
+{
+    public class instanceAction
+    {
+
+        public uint headerId { get; set; } // Incremented for each new message.
+        public string timestamp { get; set; } // ISO 8601 UTC timestamp.
+        public string version { get; set; } // Protocol version.
+        public string manufacturer { get; set; } // AGV manufacturer.
+        public string serialNumber { get; set; } // Unique AGV serial number.
+
+        public List<actionState> actions { get; set; }
+    }
+}

CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/Messages/orderMessage.cs

@@ -0,0 +1,27 @@
+using System;
+using System.Collections.Generic;
+using System.Text;
+using CommonUsage.Protocols.VDA5050.Objects;
+
+namespace CommonUsage.Protocols.VDA5050.Messages
+{
+    public class orderMessage
+    {
+        public uint headerId;
+        public string timestamp = "";
+        public string version = "";
+        public string manufacturer = "";
+        public string serialNumber = "";
+
+        public string orderId { get; set; }
+
+        public uint orderUpdateId { get; set; }
+
+        public node[] nodes { get; set; }
+
+        public edge[] edges { get; set; }
+
+        //public action[] action { get; set; }
+    }
+}
+

CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/Messages/stateMessage.cs

@@ -0,0 +1,69 @@
+using System;
+using System.Collections.Generic;
+using System.ComponentModel;
+using CommonUsage.Protocols.VDA5050.Objects;
+
+namespace CommonUsage.Protocols.VDA5050.Messages
+{
+    /// <summary>
+    /// 6.10 Topic: "state" (from AGV to master control)
+    /// todo: complete all fields required by VDA5050
+    /// </summary>
+    public class stateMessage
+    {
+        public uint headerId;
+        public string timestamp = "";
+        public string version = "";
+        public string manufacturer = "";
+        public string serialNumber = "";
+        /// <summary>
+        /// Unique order identification of the current order or the previously finished order.
+        /// The orderId is kept until a new order is received.
+        /// Empty string (""), if no previous orderId is available.
+        /// </summary>
+        public string orderId = "";
+
+        /// <summary>
+        /// Order update identification to identify, that an order update has been accepted by the AGV.
+        /// "0" if no previous orderUpdateId is available.
+        /// </summary>
+        public uint orderUpdatedId = 0;
+
+        public string lastNodeId;
+
+        public uint lastNodeSequenceId;
+
+        /// <summary>
+        /// Array of nodeState objects that need to be traversed for fulfilling the order (empty array if idle)
+        /// </summary>
+        public nodeState[] nodeStates = [];
+
+        /// <summary>
+        /// Array of edgeState objects that need to be traversed for fulfilling the order (empty array if idle)
+        /// </summary>
+        public edgeState[] edgeStates = [];
+
+        public agvPosition agvPosition;
+
+        public velocity velocity;
+        public load[] loads = [];
+
+        public bool driving;
+
+        public bool paused;
+
+        public bool newBaseRequest;
+
+        public double distanceSinceLastNode;
+
+        public batteryState batteryState;
+
+        public actionState[] actionStates = Array.Empty<actionState>();
+        public string operatingMode = "";
+        public List<errorState> errors { get; set; } = new List<errorState>(); // Array of errorState objects
+
+        public info[] information = [];
+        public safetyState safetyState;
+
+    }
+}

CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/Messages/visualizationMessage.cs

@@ -0,0 +1,12 @@
+using CommonUsage.Protocols.VDA5050.Objects;
+using System;
+using System.Collections.Generic;
+using System.Text;
+
+namespace CommonUsage.Protocols.VDA5050.Messages
+{
+    public class visualizationMessage
+    {
+        public agvPosition agvPosition;
+    }
+}

CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/Objects/action.cs

@@ -0,0 +1,18 @@
+using System;
+using System.Collections.Generic;
+using System.Text;
+
+namespace CommonUsage.Protocols.VDA5050.Objects
+{
+    public class action
+    {
+
+        public string actionId { get; set; }
+
+        public string actionType { get; set; }
+        public string actionDescription { get; set; }
+
+        public string blockingType { get; set; }
+
+    }
+}

CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/Objects/actionState.cs

@@ -0,0 +1,45 @@
+using Newtonsoft.Json.Converters;
+using Newtonsoft.Json;
+using System;
+using System.Collections.Generic;
+using System.Text;
+
+namespace CommonUsage.Protocols.VDA5050.Objects
+{
+    public class actionState
+    {
+        public actionState(action action,ActionStateEnum state)
+        {
+            actionId = action.actionId;
+            actionDescription = action.actionDescription;
+            actionType = action.actionType;
+            actionStatus = state;
+        }
+
+        public actionState()
+        {
+        }
+
+        public string actionId { get; set; }
+
+        public string actionType { get; set; }
+        public string actionDescription { get; set; }
+
+        [JsonConverter(typeof(StringEnumConverter))]
+        public ActionStateEnum actionStatus { get; set; }
+
+        public string resultDescription { get; set; }
+
+        public enum ActionStateEnum
+        {
+            WAITING,
+            INITIALIZING,
+            RUNNING,
+            PAUSED,
+            FINISHED,
+            FAILED
+        }
+
+    }
+
+}

CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/Objects/agvGeometry.cs

@@ -0,0 +1,63 @@
+using System;
+using System.Collections.Generic;
+using System.Text;
+using static CommonUsage.Protocols.VDA5050.Objects.agvGeometry;
+
+namespace CommonUsage.Protocols.VDA5050.Objects
+{
+    public class agvGeometry
+    {
+        // Wheel Definitions
+        public List<wheelDefinition> wheelDefinitions { get; set; } = new List<wheelDefinition>();
+
+        // 2D Envelopes
+        public List<envelope2D> envelopes2D { get; set; } = new List<envelope2D>();
+
+        // 3D Envelopes
+        public List<envelope3D> envelopes3D { get; set; } = new List<envelope3D>();
+
+        public class wheelDefinition
+        {
+            public enum WheelType { DRIVE, CASTER, FIXED, MECANUM }
+
+            public WheelType type { get; set; }
+            public bool isActiveDriven { get; set; }
+            public bool isActiveSteered { get; set; }
+
+            // Wheel Position
+            public double positionX { get; set; }
+            public double positionY { get; set; }
+            public double positionTheta { get; set; }  // Required for fixed wheels
+
+            // Wheel Properties
+            public double diameter { get; set; }
+            public double width { get; set; }
+            public double centerDisplacement { get; set; } = 0;  // Default to 0 if not defined
+            public string constraints { get; set; }
+        }
+
+        public class envelope2D
+        {
+            public string set { get; set; }
+            public List<polygonPoint> polygonPoints { get; set; } = new List<polygonPoint>();
+            public string description { get; set; }
+
+            public class polygonPoint
+            {
+                public double x { get; set; }
+                public double y { get; set; }
+            }
+        }
+
+        public class envelope3D
+        {
+            public string set { get; set; }
+            public string format { get; set; }
+            public object data { get; set; }  // JSON object for 3D envelope data
+            public string url { get; set; }
+            public string description { get; set; }
+        }
+    }
+
+}
+

CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/Objects/agvPosition.cs

@@ -0,0 +1,21 @@
+using System;
+using System.Collections.Generic;
+using System.Text;
+
+namespace CommonUsage.Protocols.VDA5050.Objects
+{
+    public class agvPosition
+    {
+        public bool positionInitialized;
+
+        public double x;
+
+        public double y;
+
+        public double theta;
+        public double localizationScore;
+        public double deviationRange;
+        public string mapId = "";
+        public string mapDescription = "";
+    }
+}

CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/Objects/batteryState.cs

@@ -0,0 +1,15 @@
+using System;
+using System.Collections.Generic;
+using System.Text;
+
+namespace CommonUsage.Protocols.VDA5050.Objects
+{
+    public class batteryState
+    {
+        public double batteryCharge;
+        public double batteryVoltage;
+        public double batteryHealth;
+        public bool charging;
+        public int reach;
+    }
+}

CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/Objects/boundingBoxReference.cs

@@ -0,0 +1,14 @@
+using System;
+using System.Collections.Generic;
+using System.Text;
+
+namespace CommonUsage.Protocols.VDA5050.Objects
+{
+    public class boundingBoxReference
+    {
+        public double X { get; set; } // Reference point X in AGV coordinate system
+        public double Y { get; set; } // Reference point Y in AGV coordinate system
+        public double Z { get; set; } // Reference point Z in AGV coordinate system
+        public double Theta { get; set; } // Orientation of the load bounding box
+    }
+}

CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/Objects/controlPoint.cs

@@ -0,0 +1,21 @@
+using System;
+using System.Collections.Generic;
+using System.Text;
+
+namespace CommonUsage.Protocols.VDA5050.Objects
+{
+    public class controlPoint
+    {
+        public float x;
+
+        public float y;
+
+        public float weight;
+        public controlPoint(float x, float y, float weight)
+        {
+            this.x = x;
+            this.y = y;
+            this.weight = weight;
+        }
+    }
+}

CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/Objects/edge.cs

@@ -0,0 +1,31 @@
+using System;
+using System.Collections.Generic;
+using System.Numerics;
+using System.Text;
+
+namespace CommonUsage.Protocols.VDA5050.Objects
+{
+    public class edge : sequenceItem
+    {
+        public string edgeId;
+
+        public string edgeDescription;
+
+        public string startNodeId;
+
+        public string endNodeId;
+
+        public double maxSpeed;
+
+        public double orientation;
+
+        public trajectory? trajectory;
+
+        public float[] trackTypeInfo;
+        // public List<Vector2> controlPoints;
+        //
+        // public List<float> weights;
+
+        public action[] action = [];
+    }
+}

CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/Objects/edgeState.cs

@@ -0,0 +1,15 @@
+using System;
+using System.Collections.Generic;
+using System.Text;
+
+namespace CommonUsage.Protocols.VDA5050.Objects
+{
+    public class edgeState : sequenceItem
+    {
+        public string edgeId;
+
+        public string edgeDescription;
+
+        public trajectory trajectory;
+    }
+}

CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/Objects/errorReference.cs

@@ -0,0 +1,12 @@
+using System;
+using System.Collections.Generic;
+using System.Text;
+
+namespace CommonUsage.Protocols.VDA5050.Objects
+{
+    public class errorReference
+    {
+        public string referenceKey { get; set; } // Type of reference (e.g., nodeId, edgeId, actionId)
+        public string referenceValue { get; set; } // Value corresponding to the referenceKey
+    }
+}

CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/Objects/errorState.cs

@@ -0,0 +1,15 @@
+using System;
+using System.Collections.Generic;
+using System.Text;
+
+namespace CommonUsage.Protocols.VDA5050.Objects
+{
+    public class errorState
+    {
+        public List<errorReference> errorReferences { get; set; } = new List<errorReference>(); // Array of references
+        public string errorType { get; set; } // Required: Type/name of the error
+        public string errorDescription { get; set; } // Verbose description of the error
+        public string errorHint { get; set; } // Hint for resolving the error
+        public string errorLevel { get; set; } // Required: WARNING or FATAL
+    }
+}

CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/Objects/info.cs

@@ -0,0 +1,26 @@
+using System;
+using System.Collections.Generic;
+using System.Text;
+
+namespace CommonUsage.Protocols.VDA5050.Objects
+{
+    public class info
+    {
+        public string infoType { get; set; } // Type/name of the information
+        public List<infoReference> infoReferences { get; set; } = new List<infoReference>(); // List of references
+        public string infoDescription { get; set; } // Description of the information
+        public infoLevelEnum infoLevel { get; set; } // Debugging or visualization level
+
+        public class infoReference
+        {
+            public string ReferenceKey { get; set; } // Reference type (e.g., headerId, orderId)
+            public string ReferenceValue { get; set; } // The actual referenced field value
+        }
+
+        public enum infoLevelEnum
+        {
+            DEBUG, // Used for debugging
+            INFO   // Used for visualization
+        }
+    }
+}

CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/Objects/load.cs

@@ -0,0 +1,16 @@
+using System;
+using System.Collections.Generic;
+using System.Text;
+
+namespace CommonUsage.Protocols.VDA5050.Objects
+{
+    public class load
+    {
+        public string loadId { get; set; } // Unique ID (barcode, RFID, etc.)
+        public string loadType { get; set; } // Type of load
+        public string loadPosition { get; set; } // Load handling position (e.g., "front", "back")
+        public boundingBoxReference boundingBoxReference { get; set; } = new boundingBoxReference();
+        public loadDimensions loadDimensions { get; set; } = new loadDimensions();
+        public double weight { get; set; } // Weight of load in kg (0.0 to ∞)  
+    }
+}

CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/Objects/loadDimensions.cs

@@ -0,0 +1,13 @@
+using System;
+using System.Collections.Generic;
+using System.Text;
+
+namespace CommonUsage.Protocols.VDA5050.Objects
+{
+    public class loadDimensions
+    {
+        public double Length { get; set; } // Length of the bounding box
+        public double Width { get; set; } // Width of the bounding box
+        public double Height { get; set; } // Height of the bounding box (optional)
+    }
+}

CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/Objects/node.cs

@@ -0,0 +1,17 @@
+using System;
+using System.Collections.Generic;
+using System.Text;
+
+namespace CommonUsage.Protocols.VDA5050.Objects
+{
+    public class node : sequenceItem
+    {
+        public string nodeId;
+
+        public string nodeDescription;
+
+        public nodePosition nodePosition;
+
+        public action[] actions = [];
+    }
+}

CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/Objects/nodePosition.cs

@@ -0,0 +1,36 @@
+using System;
+using System.Collections.Generic;
+using System.Text;
+
+namespace CommonUsage.Protocols.VDA5050.Objects
+{
+    /// <summary>
+    /// Defines the position on a map in a global project-specific world coordinate system.
+    /// Each floor has its own map.
+    /// All maps shall use the same project-specific global origin.
+    /// </summary>
+    public class nodePosition
+    {
+        /// <summary>
+        /// X-position on the map in reference to the map coordinate system.
+        /// Precision is up to the specific implementation.
+        /// </summary>
+        public double x;
+
+        /// <summary>
+        /// Y-position on the map in reference to the map coordinate system.
+        /// Precision is up to the specific implementation.
+        /// </summary>
+        public double y;
+
+        /// <summary>
+        /// Range: [-Pi ... Pi]
+        /// Absolute orientation of the AGV on the node.
+        /// Optional: vehicle can plan the path by itself. If defined, the AGV has to assume the theta angle on this node.
+        /// If previous edge disallows rotation, the AGV shall rotate on the node.
+        /// If following edge has a differing orientation defined but disallows rotation,
+        /// the AGV is to rotate on the node to the edges desired rotation before entering the edge.
+        /// </summary>
+        public double theta;
+    }
+}

CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/Objects/nodeState.cs

@@ -0,0 +1,26 @@
+using System;
+using System.Collections.Generic;
+using System.Text;
+
+namespace CommonUsage.Protocols.VDA5050.Objects
+{
+    public class nodeState : sequenceItem
+    {
+        /// <summary>
+        /// Unique node identification.
+        /// </summary>
+        public string nodeId;
+
+        /// <summary>
+        /// Additional information on the node.
+        /// </summary>
+        public string nodeDescription;
+
+        /// <summary>
+        /// Node position.
+        /// The object is defined in 6.6 Topic: "order" (from master control to AGV)
+        /// Optional: Master control has this information. Can be sent additionally, e.g., for debugging purposes.
+        /// </summary>
+        public nodePosition nodePosition;
+    }
+}

CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/Objects/physicalParameters.cs

@@ -0,0 +1,20 @@
+using System;
+using System.Collections.Generic;
+using System.Text;
+
+namespace CommonUsage.Protocols.VDA5050.Objects
+{
+    public class physicalParameters
+    {
+        public double speedMin;
+        public double speedMax;
+        public double angularSpeedMin;
+        public double angularSpeedMax;
+        public double accelerationMax;
+        public double decelerationMax;
+        public double heightMin;
+        public double heightMax;
+        public double width;
+        public double length;
+    }
+}

CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/Objects/protocolFeatures.cs

@@ -0,0 +1,10 @@
+using System;
+using System.Collections.Generic;
+using System.Text;
+
+namespace CommonUsage.Protocols.VDA5050.Objects
+{
+    public class protocolFeatures
+    {
+    }
+}

CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/Objects/protocolLimits.cs

@@ -0,0 +1,10 @@
+using System;
+using System.Collections.Generic;
+using System.Text;
+
+namespace CommonUsage.Protocols.VDA5050.Objects
+{
+    public class protocolLimits
+    {
+    }
+}

CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/Objects/safetyState.cs

@@ -0,0 +1,20 @@
+using System;
+using System.Collections.Generic;
+using System.Text;
+
+namespace CommonUsage.Protocols.VDA5050.Objects
+{
+    public class safetyState
+    {
+        public eStopEnum eStop { get; set; } // Emergency stop status
+        public bool fieldViolation { get; set; } // "true" if a safety field is violated, "false" otherwise
+
+        public enum eStopEnum
+        {
+            AUTOACK, // Auto-acknowledged emergency stop (e.g., triggered by a bumper)
+            MANUAL,  // Manually confirmed emergency stop
+            REMOTE,  // Remote-confirmed emergency stop
+            NONE     // No emergency stop activated
+        }
+    }
+}

CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/Objects/sequenceItem.cs

@@ -0,0 +1,13 @@
+using System;
+using System.Collections.Generic;
+using System.Text;
+
+namespace CommonUsage.Protocols.VDA5050.Objects
+{
+    public class sequenceItem
+    {
+        public uint sequenceId;
+
+        public bool released;
+    }
+}

CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/Objects/trajectory.cs

@@ -0,0 +1,15 @@
+using System;
+using System.Collections.Generic;
+using System.Text;
+
+namespace CommonUsage.Protocols.VDA5050.Objects
+{
+    public class trajectory
+    {
+        public float degree;
+
+        public float[] knotVector;
+
+        public controlPoint[] controlPoints;
+    }
+}

CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/Objects/typeSpecification.cs

@@ -0,0 +1,15 @@
+using System;
+using System.Collections.Generic;
+using System.Text;
+
+namespace CommonUsage.Protocols.VDA5050.Objects
+{
+    public class typeSpecification
+    {
+        public string agvKinemantic = "";
+        public string agvClass = "";
+        public double maxLoadMass;
+        public string[] localizationTypes; // Simplified description of localization type (e.g., NATURAL, REFLECTOR, RFID, DMC, GRID)
+        public string[] navigationTypes;  // Path planning types (e.g., 'AUTONOMOUS', 'VIRTUAL_LINE_GUIDED')
+    }
+}

CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/Objects/velocity.cs

@@ -0,0 +1,15 @@
+using System;
+using System.Collections.Generic;
+using System.Text;
+
+namespace CommonUsage.Protocols.VDA5050.Objects
+{
+    public class velocity
+    {
+        public double vx;
+
+        public double vy;
+
+        public double omega;
+    }
+}

CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/VDA5050Basic.cs

@@ -0,0 +1,118 @@
+//using CommonUsage.Protocols.VDA5050.Messages;
+//using System;
+//using System.Collections.Generic;
+//using System.Net.Http.Headers;
+//using System.Runtime.CompilerServices;
+//using System.Text;
+//using System.Threading;
+//using System.Threading.Tasks;
+//using CommonUsage.Protocols.VDA5050.Objects;
+//using ClumsyCore.Utilities;
+//using System.Linq;
+//using ClumsyCore.Pilot;
+//using System.Numerics;
+
+//namespace CommonUsage.Protocols.VDA5050
+//{
+//    public abstract class VDA5050Basic
+//    {
+//        protected static IVDACommunicationProtocol _communicationProtocol;
+
+//        public void Enable(IVDACommunicationProtocol protocol)
+//        {
+//            _communicationProtocol = protocol;
+//            Task.Run(() => ManageConnection());
+//            StartVisualizationLoop();
+//        }
+//        private void StartVisualizationLoop()
+//        {
+//            Console.WriteLine("Visualization in CommonUsage");
+//            new Thread(async () =>
+//                {
+//                    while (true)
+//                    {
+//                        var position = GetAGVPosition();
+//                        if (position != null)
+//                        {
+//                            var msg = new stateMessage()
+//                            {
+//                                serialNumber = "test-01",
+//                                agvPosition = new()
+//                                {
+//                                    x = position.Value.X,
+//                                    y = position.Value.Y,
+//                                    theta = position.Value.Theta,
+//                                    positionInitialized = true
+//                                }
+
+//                            };
+//                            await _communicationProtocol.SendMessageAsync(msg, "vda5050/frldAGV/visualization");
+//                        }
+
+//                        Thread.Sleep(100);
+//                    }
+//                })
+//            { Name = "VDA5050TopicVisualization" }.Start();
+//        }
+
+//        private void ManageConnection()
+//        {
+//            while (true)
+//            {
+//                var status = CheckConnectionStatus() ? "ONLINE" : "OFFLINE";
+//                _communicationProtocol.PublishConnectionStatus(status);
+//                Thread.Sleep(1000);
+//            }
+//        }
+
+//        protected List<sequenceItem> OrganizeReceivedSequence(orderMessage order)
+//        {
+//            List<sequenceItem> receivedSequence = new();
+
+//            int ii = 0, jj = 0;
+//            while (true)
+//            {
+//                var edge = order.edges[ii];
+//                var node = order.nodes[jj];
+//                var takeEdge = edge.sequenceId < node.sequenceId;
+
+//                if (takeEdge)
+//                {
+//                    receivedSequence.Add(edge);
+//                    ii++;
+//                    if (ii == order.edges.Length) break;
+//                }
+//                else
+//                {
+//                    receivedSequence.Add(node);
+//                    jj++;
+//                    if (jj == order.nodes.Length) break;
+//                }
+//            }
+//            for (var i = ii; i < order.edges.Length; ++i) receivedSequence.Add(order.edges[i]);
+//            for (var j = jj; j < order.nodes.Length; ++j) receivedSequence.Add(order.nodes[j]);
+
+//            for (var i = 1; i < receivedSequence.Count; i++)
+//            {
+//                if (receivedSequence[i - 1].sequenceId + 1 != receivedSequence[i].sequenceId)
+//                    throw new Exception("stateMessage not continuous!");
+//            }
+
+//            return receivedSequence;
+//        }
+
+//        public virtual bool CheckConnectionStatus()
+//        {
+//            return false;
+//        }
+
+//        protected abstract Vector3? GetAGVPosition();
+//        public struct Vector3
+//        {
+//            public double X;
+//            public double Y;
+//            public double Theta;
+//        }
+//    }
+
+//}

CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/VDA5050Helper.cs

@@ -0,0 +1,11 @@
+using System;
+using System.Collections.Generic;
+using System.Text;
+
+namespace CommonUsage.Protocols.VDA5050
+{
+    public class VDA5050Helper
+    {
+
+    }
+}

CommonUsage-MultiVehicleSync/commonusage/Visualizer.cs

@@ -0,0 +1,28 @@
+using System;
+using System.Collections.Generic;
+using System.Drawing;
+using System.Numerics;
+using System.Text;
+
+namespace CommonUsage
+{
+    public class Visualizer
+    {
+        public Action<Color, Vector2, Vector2, bool, bool, int> LineAction;
+
+        public Action<Color, string, Vector2> TextAction;
+
+        public Action Clear;
+
+        public void DrawLine(Color color, Vector2 src, Vector2 dst, bool startArrow = false, bool endArrow = false,
+            int width = 1)
+        {
+            LineAction?.Invoke(color, src, dst, startArrow, endArrow, width);
+        }
+
+        public void DrawText(Color color, string text, Vector2 pos)
+        {
+            TextAction?.Invoke(color, text, pos);
+        }
+    }
+}

MedullaAdapter/DiverCartDefinition.cs

@@ -16,6 +16,14 @@ namespace MedullaAdapter
     [UseManualController(manualController = typeof(Remote))]
     public class DiverCartDefinition : MultiWheelCartDefinition
     {
+        public DiverCartDefinition()
+        {
+            // 实车配置可覆盖这些回退值；四个舵轮在MotorRoutine中统一读取它们。
+            DiffSteerKp = 0.0042f;
+            DiffSteerKi = 0f;
+            DiffSteerKd = 0f;
+        }
+
         #region 基本成员
         public MCUSerialBridge Bridge;
 
@@ -64,10 +72,51 @@ 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;
@@ -87,8 +136,85 @@ 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;
@@ -114,6 +240,22 @@ 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()
@@ -225,27 +367,131 @@ namespace MedullaAdapter
             }
 
             var speed = speedThreshold * y;
-            var omega = CalculateManualOmega(speed, x);
+            var normalizedSteering =
+                (float)Math.Pow(
+                    Math.Abs(x),
+                    ManualThetaPow) *
+                Math.Sign(x);
+            var steeringDegrees =
+                -normalizedSteering * MaxManualTheta;
             ManualMode = (int)mode;
 
             switch (mode)
             {
                 case ManualControlMode.Normal:
-                    SendBodyCommand(vx: speed, vy: 0.0, omegaRadiansPerSecond: omega, interval);
+                    // 普通模式统一使用车体速度命令：
+                    // 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);
+                    }
                     break;
                 case ManualControlMode.Crab:
-                    SendBodyCommand(vx: 0.0, vy: speed, omegaRadiansPerSecond: omega, interval);
+                    // 舵轮机械范围为[-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);
+                    }
                     break;
                 case ManualControlMode.Spin:
-                    var spinOmega =
-                        speed * MaxAngularSpeed *
-                        Math.PI / 180.0;
+                    // 摇杆处于中位时只清零驱动速度，保持已经准备好的
+                    // 自转舵角；下次推动摇杆时仍会重新检查实际舵角。
+                    if (Math.Abs(speed) < 1e-6f)
+                    {
+                        adapter
+                            .StopXYThDrivePreserveSteeringState();
+                        break;
+                    }
 
-                    SendBodyCommand(
-                        vx: 0.0,
-                        vy: 0.0,
-                        omegaRadiansPerSecond: spinOmega,
-                        interval);
+                    // 自转时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);
+                    }
                     break;
                 default:
                     ManualMode = -1;
@@ -274,6 +520,10 @@ namespace MedullaAdapter
             if (_pendingManualMode != mode)
             {
                 adapter.StopImmediately();
+                // 所有模式的准备角度均按真实机械舵角表达；
+                // 先退出上一模式的虚拟运动坐标系，再执行预对齐。
+                adapter.ResetToBodyFrame();
+                _activeManualMode = null;
 
                 var preparationAccepted = mode switch
                 {
@@ -303,8 +553,8 @@ namespace MedullaAdapter
             // 后续控制周期保持停车，并读取实际舵角判断是否到位。
             adapter.StopImmediately();
 
-            const double toleranceRadians =
-                2.0 * Math.PI / 180.0;
+            var toleranceRadians =
+                AngleMath.DegreesToRadians(2.0);
 
             bool aligned;
 
@@ -334,56 +584,33 @@ 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()
@@ -398,6 +625,11 @@ namespace MedullaAdapter
                     new MultiWheelChassisAdapter(Chassis, CarNum);
             }
 
+            _chassisAdapter.SteeringAlignmentSigmaDegrees =
+                Math.Max(
+                    ManualSteeringAlignmentSigmaDegrees,
+                    0.1f);
+
             return _chassisAdapter;
         }
 

MedullaAdapter/MCURoutine.cs

@@ -4,6 +4,9 @@ using FundamentalLib;
 using MCUSerialBridgeCLR;
 using System;
 using System.Collections.Generic;
+using System.Diagnostics;
+using System.IO;
+using System.Threading;
 
 namespace MedullaAdapter
 {
@@ -26,6 +29,9 @@ 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)
@@ -47,9 +53,24 @@ 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;
@@ -158,6 +179,7 @@ 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)
             {
@@ -266,6 +288,7 @@ namespace MedullaAdapter
                     return;
                 }
             }
+
             // 没有错误 没有节点保护 就发06 07 0F使能
             else if (_operationTime == 4)
             {
@@ -386,6 +409,44 @@ 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);
@@ -428,6 +489,79 @@ 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)
         {
@@ -485,7 +619,9 @@ namespace MedullaAdapter
                     handler(msg);
                 }
             });
-            _canCallbackRegistered = true;
+            // _canCallbackRegistered = true;
+            _canCallbackRegistered =
+    err0 == MCUSerialBridgeError.OK;
         }
 
         // M层串口通信：按需注册电池等串口设备回调。
@@ -601,8 +737,19 @@ namespace MedullaAdapter
                     var payload = msg.Payload;
                     if (payload == null || payload.Length < 8) return;
                     var rpm = DecodeRpmFromPayload(payload);
-                    cart.ActualSpeedLeftFrontLeft = ConvertRpm2Mps(rpm);
-                    cart.LFLActualPos = ConvertR2MM(BitConverter.ToInt32(payload, 0) / 10000f);
+                    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);
                 },
                 [0x282] = (msg) =>
                 {
@@ -610,8 +757,19 @@ namespace MedullaAdapter
                     var payload = msg.Payload;
                     if (payload == null || payload.Length < 8) return;
                     var rpm = DecodeRpmFromPayload(payload);
-                    cart.ActualSpeedLeftFrontRight = -ConvertRpm2Mps(rpm);
-                    cart.LFRActualPos = ConvertR2MM(-BitConverter.ToInt32(payload, 0) / 10000f);
+                    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);
                 },
                 [0x283] = (msg) =>
                 {
@@ -619,8 +777,19 @@ namespace MedullaAdapter
                     var payload = msg.Payload;
                     if (payload == null || payload.Length < 8) return;
                     var rpm = DecodeRpmFromPayload(payload);
-                    cart.ActualSpeedRightFrontLeft = ConvertRpm2Mps(rpm);
-                    cart.RFLActualPos = ConvertR2MM(BitConverter.ToInt32(payload, 0) / 10000f);
+                    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);
                 },
                 [0x284] = (msg) =>
                 {
@@ -628,8 +797,19 @@ namespace MedullaAdapter
                     var payload = msg.Payload;
                     if (payload == null || payload.Length < 8) return;
                     var rpm = DecodeRpmFromPayload(payload);
-                    cart.ActualSpeedRightFrontRight = -ConvertRpm2Mps(rpm);
-                    cart.RFRActualPos = ConvertR2MM(-BitConverter.ToInt32(payload, 0) / 10000f);
+                    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);
                 },
                 [0x285] = (msg) =>
                 {
@@ -637,8 +817,19 @@ namespace MedullaAdapter
                     var payload = msg.Payload;
                     if (payload == null || payload.Length < 8) return;
                     var rpm = DecodeRpmFromPayload(payload);
-                    cart.ActualSpeedLeftRearLeft = ConvertRpm2Mps(rpm);
-                    cart.LRLActualPos = ConvertR2MM(BitConverter.ToInt32(payload, 0) / 10000f);
+                    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);
                 },
                 [0x286] = (msg) =>
                 {
@@ -646,8 +837,19 @@ namespace MedullaAdapter
                     var payload = msg.Payload;
                     if (payload == null || payload.Length < 8) return;
                     var rpm = DecodeRpmFromPayload(payload);
-                    cart.ActualSpeedLeftRearRight = -ConvertRpm2Mps(rpm);
-                    cart.LRRActualPos = ConvertR2MM(-BitConverter.ToInt32(payload, 0) / 10000f);
+                    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);
                 },
                 [0x287] = (msg) =>
                 {
@@ -655,8 +857,19 @@ namespace MedullaAdapter
                     var payload = msg.Payload;
                     if (payload == null || payload.Length < 8) return;
                     var rpm = DecodeRpmFromPayload(payload);
-                    cart.ActualSpeedRightRearLeft = ConvertRpm2Mps(rpm);
-                    cart.RRLActualPos = ConvertR2MM(BitConverter.ToInt32(payload, 0) / 10000f);
+                    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);
                 },
                 [0x288] = (msg) =>
                 {
@@ -664,8 +877,19 @@ namespace MedullaAdapter
                     var payload = msg.Payload;
                     if (payload == null || payload.Length < 8) return;
                     var rpm = DecodeRpmFromPayload(payload);
-                    cart.ActualSpeedRightRearRight = -ConvertRpm2Mps(rpm);
-                    cart.RRRActualPos = ConvertR2MM(-BitConverter.ToInt32(payload, 0) / 10000f);
+                    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);
                 },
                 [0x289] = (msg) =>
                 {
@@ -783,10 +1007,18 @@ namespace MedullaAdapter
                     //Console.WriteLine("Received 0x18B CAN Message");
                     var payload = msg.Payload;
                     if (payload == null || payload.Length < 4) return;
-                    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;
+                    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);
                 },
                 [0x18C] = (msg) =>
                 {
@@ -796,6 +1028,14 @@ 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) =>
@@ -803,20 +1043,36 @@ 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;
-                    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;
+                    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);
                 },
                 [0x18E] = (msg) =>
                 {
                     //Console.WriteLine("Received 0x18E CAN Message");
                     var payload = msg.Payload;
                     if (payload == null || payload.Length < 4) return;
-                    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;
+                    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);
                 },
 
                 // 远程帧

MedullaAdapter/MedullaAdapter.csproj

@@ -27,31 +27,35 @@
       <Private>false</Private>
     </Reference>
 
-    <Reference Include="CommonUsage">
-      <HintPath>ref\CommonUsage.dll</HintPath>
-      <Private>false</Private>
-    </Reference>
-
     <Reference Include="MDCSToolBox">
       <HintPath>ref\MDCSToolBox.dll</HintPath>
       <Private>false</Private>
     </Reference>
 
-    <Reference Include="CycleGUI">
+    <!-- <Reference Include="CycleGUI">
       <HintPath>ref\CycleGUI.dll</HintPath>
       <Private>false</Private>
+    </Reference> -->
+    <Reference Include="CommonUsage">
+      <HintPath>..\ref\CommonUsage.dll</HintPath>
     </Reference>
   </ItemGroup>
 
   <ItemGroup>
-  <Compile Include="..\Shared\ChassisCommand.cs"
-            Link="Shared\ChassisCommand.cs" />
+  <Compile Include="..\Shared\Models\MotionModels.cs"
+           Link="Shared\Models\MotionModels.cs" />
 
-  <Compile Include="..\Shared\FrameTransform2D.cs"
-            Link="Shared\FrameTransform2D.cs" />
+  <Compile Include="..\Shared\Mathematics\FrameTransform2D.cs"
+           Link="Shared\Mathematics\FrameTransform2D.cs" />
 
-  <Compile Include="..\Shared\MultiWheelChassisAdapter.cs"
-            Link="Shared\MultiWheelChassisAdapter.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" />
 </ItemGroup>
 
 </Project>

MedullaAdapter/MotorRoutine.cs

@@ -1,20 +1,43 @@
 // 计算8个驱动电机的目标速度和舵角PID
 using CartActivator;
-using CommonUsage.Mathematics;
 using FundamentalLib;
 using MDCSToolBox.Commons;
-using MDCSToolBox.Commons.Controllers;
 using System;
-using System.Collections.Generic;
-using System.Linq;
+using System.Diagnostics;
+using System.Threading;
 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;
@@ -67,38 +90,91 @@ 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;
-            switch (cart.Transmitter_SB)
+
+            if (!TryGetStableTransmitterControlMode(
+                    out var stableControlMode))
             {
-                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,
-                        cart.TransmitterSpeed,
-                        interval);
-                    StopClampArms();
-                    return;
+                // SB处于中间档或尚未稳定时立即停车，
+                // 保持当前模式，不下发新的模式目标角。
+                cart.ManualControl(
+                    cart.TransmitterControlMode,
+                    0, 0, 0,
+                    cart.TransmitterSpeed,
+                    interval);
+                StopClampArms();
+                return;
             }
+
+            cart.TransmitterControlMode = stableControlMode;
+
             // SA关闭后立即停车。
             if (!cart.Transmitter_SA)
             {
@@ -165,7 +241,9 @@ namespace MedullaAdapter
             cart.SpeedRightArm = 0;
         }
 
-        // M层单车底盘：根据四个舵轮的目标角度和实际角度修正8个驱动电机速度。
+        /// <summary>
+        /// 根据四个舵轮的目标角速度前馈和实际角度反馈修正八个驱动电机速度。
+        /// </summary>
         private void UpdateDiffSteerWheelSpeeds()
         {
             if (cart.LeftFrontPid == null ||
@@ -181,6 +259,7 @@ namespace MedullaAdapter
                 cart.SpeedLRR = 0;
                 cart.SpeedRRL = 0;
                 cart.SpeedRRR = 0;
+                ResetDiffSteerRateFeedforward();
                 return;
             }
 
@@ -224,20 +303,54 @@ namespace MedullaAdapter
                 cart.DiffSteerThresh,
                 cart.DiffSteerSpeedAcc);
 
-            // 根据实际舵角计算四条腿的差速修正量。
-            var diffLf = cart.LeftFrontPid.GetResponse(
+            // 根据实际舵角计算四条腿的PID反馈修正量。
+            var feedbackLf = cart.LeftFrontPid.GetResponse(
                 cart.ThLeftFront, false, false, "LF");
 
-            var diffLr = cart.LeftRearPid.GetResponse(
+            var feedbackLr = cart.LeftRearPid.GetResponse(
                 cart.ThLeftRear, false, false, "LR");
 
-            var diffRf = cart.RightFrontPid.GetResponse(
+            var feedbackRf = cart.RightFrontPid.GetResponse(
                 cart.ThRightFront, false, false, "RF");
 
-            var diffRr = cart.RightRearPid.GetResponse(
+            var feedbackRr = cart.RightRearPid.GetResponse(
                 cart.ThRightRear, false, false, "RR");
 
-            // 左前腿：左右电机施加方向相反的PID修正量。
+            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;
+
+            // 左前腿：左右电机施加方向相反的合成差速修正量。
             cart.SpeedLFL = cart.SpeedLeftFrontLeft - diffLf;
             cart.SpeedLFR = cart.SpeedLeftFrontRight + diffLf;
 
@@ -254,6 +367,444 @@ 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

@@ -0,0 +1,692 @@
+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

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

MedullaAdapter/build/Medulla/plugins/MedullaAdapter.deps.json

@@ -7,9 +7,20 @@
   "targets": {
     ".NETCoreApp,Version=v8.0": {
       "MedullaAdapter/1.0.0": {
+        "dependencies": {
+          "CommonUsage": "1.0.0.0"
+        },
         "runtime": {
           "MedullaAdapter.dll": {}
         }
+      },
+      "CommonUsage/1.0.0.0": {
+        "runtime": {
+          "CommonUsage.dll": {
+            "assemblyVersion": "1.0.0.0",
+            "fileVersion": "1.0.0.0"
+          }
+        }
       }
     }
   },
@@ -18,6 +29,11 @@
       "type": "project",
       "serviceable": false,
       "sha512": ""
+    },
+    "CommonUsage/1.0.0.0": {
+      "type": "reference",
+      "serviceable": false,
+      "sha512": ""
     }
   }
 }

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

@@ -1,73 +0,0 @@
-{
-  "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

@@ -1,16 +0,0 @@
-<?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>