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

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

.codex/skills/commit/SKILL.md

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

.codex/skills/readme/SKILL.md

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

.cursor/rules/karpathy-guidelines.mdc

@@ -0,0 +1,70 @@
+---
+description: Behavioral guidelines to reduce common LLM coding mistakes. Use when writing, reviewing, or refactoring code to avoid overcomplication, make surgical changes, surface assumptions, and define verifiable success criteria.
+alwaysApply: true
+---
+
+# Karpathy behavioral guidelines
+
+Behavioral guidelines to reduce common LLM coding mistakes. Merge with project-specific instructions as needed.
+
+**Tradeoff:** These guidelines bias toward caution over speed. For trivial tasks, use judgment.
+
+## 1. Think Before Coding
+
+**Don't assume. Don't hide confusion. Surface tradeoffs.**
+
+Before implementing:
+- State your assumptions explicitly. If uncertain, ask.
+- If multiple interpretations exist, present them - don't pick silently.
+- If a simpler approach exists, say so. Push back when warranted.
+- If something is unclear, stop. Name what's confusing. Ask.
+
+## 2. Simplicity First
+
+**Minimum code that solves the problem. Nothing speculative.**
+
+- No features beyond what was asked.
+- No abstractions for single-use code.
+- No "flexibility" or "configurability" that wasn't requested.
+- No error handling for impossible scenarios.
+- If you write 200 lines and it could be 50, rewrite it.
+
+Ask yourself: "Would a senior engineer say this is overcomplicated?" If yes, simplify.
+
+## 3. Surgical Changes
+
+**Touch only what you must. Clean up only your own mess.**
+
+When editing existing code:
+- Don't "improve" adjacent code, comments, or formatting.
+- Don't refactor things that aren't broken.
+- Match existing style, even if you'd do it differently.
+- If you notice unrelated dead code, mention it - don't delete it.
+
+When your changes create orphans:
+- Remove imports/variables/functions that YOUR changes made unused.
+- Don't remove pre-existing dead code unless asked.
+
+The test: Every changed line should trace directly to the user's request.
+
+## 4. Goal-Driven Execution
+
+**Define success criteria. Loop until verified.**
+
+Transform tasks into verifiable goals:
+- "Add validation" → "Write tests for invalid inputs, then make them pass"
+- "Fix the bug" → "Write a test that reproduces it, then make it pass"
+- "Refactor X" → "Ensure tests pass before and after"
+
+For multi-step tasks, state a brief plan:
+```
+1. [Step] → verify: [check]
+2. [Step] → verify: [check]
+3. [Step] → verify: [check]
+```
+
+Strong success criteria let you loop independently. Weak criteria ("make it work") require constant clarification.
+
+---
+
+**These guidelines are working if:** fewer unnecessary changes in diffs, fewer rewrites due to overcomplication, and clarifying questions come before implementation rather than after mistakes.

.gitignore

@@ -88,3 +88,5 @@ _ReSharper*/
 *.db
 *.sqlite
 *.sqlite3
+
+*.csv

.vs/ProjectSettings.json

@@ -1,3 +0,0 @@
-{
-  "CurrentProjectSetting": null
-}

.vs/VSWorkspaceState.json

@@ -1,6 +0,0 @@
-{
-  "ExpandedNodes": [
-    ""
-  ],
-  "PreviewInSolutionExplorer": false
-}

ClumsyPilot/AGV.cs

@@ -1,534 +1,21 @@
 using ClumsyCore;
-using ClumsyCore.Interfaces;
-using ClumsyCore.Sensors;
-using CommonUsage.Chassis;
-using CommonUsage.Mathematics;
-using FundamentalLib;
 using MDCSToolBox.Clumsy.AgvInterfaces;
-using MDCSToolBox.Clumsy.Calibration;
 using MDCSToolBox.Clumsy.MotionControllers;
-using MDCSToolBox.Clumsy.Tracks;
-using MDCSToolBox.Commons.Controllers;
-using Newtonsoft.Json;
-using System;
-using System.Collections.Generic;
-using System.Net.Http;
-using System.Numerics;
-using System.Security.Cryptography;
-using System.Threading;
-using System.Threading.Tasks;
-using static ClumsyCore.DTools.Painter;
 
 namespace MultiWheelC
 {
-    public class SetLocationRes
-    {
-        public float x, y, th;
-        public int l_step;
-        public long tick;
-        public string error;
-    }
     public class AGV : MultiWheelInterface
     {
         public override AbstractGeometricController GetController()
-        {
+                => new ChassisController().Get();
-            return new ChassisController().Get();
-        }
-
         public override MultiWheelMagTracker GetMagController()
-        {
+            => new MultiWheelMagTracker();
-            return new MultiWheelMagTracker();
-        }
-
         public override NaiveMagnetController GetNaiveMagnetController()
-        {
+            => new NaiveMagnetController();
-            return new NaiveMagnetController();
-        }
 
-        public void Sleep(float s)
+        public void Sleep(float seconds)
         {
-            new DriveTask(new Sleep() { Second = s }.Get()).Wait();
+            new DriveTask(new Sleep { Second = seconds }.Get()).Wait();
-        }
-
-        public void ControlChargePort(bool open)
-        {
-            DLog.Log($"call ControlChargePort({open})");
-            PilotDefinition.Self.OpenChargeByClumsy = open;
-        }
-
-        public void SwitchLidarArea(int area)
-        {
-            DLog.Log($"call SwitchLidarArea({area})");
-            PilotDefinition.Self.AreaChoose = area;
-        }
-
-        public void SwitchIoArea(int area)
-        {
-            if (area != -1)
-            {
-                PilotDefinition.Self.IOObstacleArea = area;
-            }
-        }
-        public void RotateToTarget(float target)
-        {
-            //if (!needrotate) return;
-            var dl = new DriveTask(new MultiWheelRotateInPlace()
-            {
-                AngleTarget = target,
-                PidparamsRead = () => new PIDParams()
-                {
-                    Kp = PilotDefinition.Conf.TireFollowingThkp,
-                    Ki = PilotDefinition.Conf.TireFollowingThki,
-                    Kd = PilotDefinition.Conf.TireFollowingThkd,
-                    DeadZone = PilotDefinition.Conf.TireFollowingThDeadZone,
-                    SpeedAccPerSec = PilotDefinition.Conf.TireFollowingThSpeedAccPerSec,
-                    OutputUpperThreshold = PilotDefinition.Conf.TireFollowingThThresh,
-                    MaxI = PilotDefinition.Conf.TireFollowingThMaxI,
-                }
-            }.Get());
-            dl.Wait();
-        }
-
-        //参数1：tireNum 需要钻过的轮胎对数量
-        //参数2：frontLidarDetect true:前雷达识别  false:后雷达识别
-        public void TireFollowing(int tireNum, bool frontLidarDetect, int srcId, int dstId)
-        {
-            while (!TryLock(dstId))
-            {
-                Thread.Sleep(50);
-            }
-            DLog.Log($"锁点{dstId}完成", "TireFollowing");
-            var lidarName = frontLidarDetect ? "前雷达" : "后雷达";
-            DLog.Log($"开始钻车动作，通过{lidarName}识别结果钻{tireNum}对轮胎", "TireFollowing");
-            if (tireNum != 1 && tireNum != 2)
-            {
-                DLog.Log($"TireNum必须是1或2 (当前输入:{tireNum})", "TireFollowing");
-                return;
-            }
-
-            if (PilotDefinition.Self.GhostMode)
-            {
-                while (!TryLock(dstId))
-                {
-                    Console.WriteLine("等待锁取货点中...");
-                    Thread.Sleep(200);
-                }
-                Console.WriteLine($"锁点{dstId}完成");
-                Thread.Sleep(1000);
-                Console.WriteLine($"开始钻车动作，通过{lidarName}识别结果钻{tireNum}对轮胎");
-                Thread.Sleep(1000);
-                Leave(srcId);
-                Console.WriteLine($"开始第一段盲走，此时释放预取货点{srcId}");
-                Thread.Sleep(2000);
-                //Leave(dstId);
-                //Console.WriteLine($"结束第一段盲走，此时释放取货点{dstId}");
-                Thread.Sleep(2000);
-                Console.WriteLine($"结束钻车动作");
-                return;
-            }
-
-            var detectors = new List<TireFollowing.DetectorDefinition>()
-            {
-                new TireFollowing.DetectorDefinition()
-                {
-                    DetectFunction = (_, lastDetectX, filters) => TireDetect.Detect(lastDetectX, filters, frontLidarDetect),
-                    StartGuessingX = frontLidarDetect ? PilotDefinition.Conf.TireFollowingStage1GuessX : -PilotDefinition.Conf.TireFollowingStage1GuessX,
-                    StartGuessingY = 0,
-                    SwitchWalkBlindCondition = rd => rd <= PilotDefinition.Conf.TireFollowingWalkBlindSwitchingDistance,
-                    FinishWalkBlindCondition = rd => rd <= PilotDefinition.Conf.TireFollowingWalkBlindFinishDistance,
-                    PathTransformation = new Tuple<float, float, float>(
-                    frontLidarDetect ? PilotDefinition.Conf.TireFollowingFrontLidarPathTransformationX : PilotDefinition.Conf.TireFollowingBackLidarPathTransformationX,
-                    frontLidarDetect ? PilotDefinition.Conf.TireFollowingFrontLidarPathTransformationY : PilotDefinition.Conf.TireFollowingBackLidarPathTransformationY,
-                    0),
-                    LeaveSrcFunction = Leave,
-                    SrcId = srcId,
-                    DstId = dstId,
-                },
-                new TireFollowing.DetectorDefinition()
-                {
-                    DetectFunction = (_, lastDetectX, filters) => TireDetect.Detect(lastDetectX, filters, frontLidarDetect),
-                    StartGuessingX = frontLidarDetect ? PilotDefinition.Conf.TireFollowingStage2GuessX : -PilotDefinition.Conf.TireFollowingStage2GuessX,
-                    StartGuessingY = 0,
-                    SwitchWalkBlindCondition = rd => rd <= PilotDefinition.Conf.TireFollowingWalkBlindSwitchingDistance,
-                    FinishWalkBlindCondition = rd => rd <= PilotDefinition.Conf.TireFollowingWalkBlindFinishDistance,
-                    PathTransformation = new Tuple<float, float, float>(
-                    frontLidarDetect ? PilotDefinition.Conf.TireFollowingFrontLidarPathTransformationX : PilotDefinition.Conf.TireFollowingBackLidarPathTransformationX,
-                    frontLidarDetect ? PilotDefinition.Conf.TireFollowingFrontLidarPathTransformationY : PilotDefinition.Conf.TireFollowingBackLidarPathTransformationY,
-                    0)
-                },
-            };
-            DLog.Log($"钻胎为{tireNum}", "TireFollowing");
-            var following = new TireFollowing()
-            {
-                GetController = () => new ChassisController().Get(),
-                GuessRangeX = PilotDefinition.Conf.TireFilterLength / 2,
-                GuessRangeY = PilotDefinition.Conf.TireFilterWidth / 2,
-                detectors = detectors,
-                SlowDistance = PilotDefinition.Conf.TireFollowingSlowDistance,
-                MaxSpeed = PilotDefinition.Conf.TireFollowingMaxSpeed,
-                TireNum = tireNum,
-                CarDirection = frontLidarDetect ? 0f : 180f,
-                WalkBlindTh = frontLidarDetect ? PilotDefinition.Conf.TireFollowingFrontLidarWalkBlindTh : PilotDefinition.Conf.TireFollowingBackLidarWalkBlindTh,
-            };
-            var _dt = new DriveTask(following.Get());
-            _dt.Wait();
-            DLog.Log("钻车动作结束", "TireFollowing");
-        }
-
-        //离车一定是后雷达识别一个轮胎
-        public void LeaveCar(int srcId, float srcX, float srcY, int dstId, float dstX, float dstY)
-        {
-            while (!TryLock(dstId))
-            {
-                Thread.Sleep(50);
-            }
-            DLog.Log($"锁点{dstId}完成", "TireFollowing");
-            DLog.Log($"开始钻车动作，通过后雷达识别结果钻1对轮胎", "TireFollowing");
-            var chassis = (MultiWheelChassis)PilotDefinition.Chassis;
-            chassis.SetOriginBias(0, 0, 0);
-            var following = new TireFollowing()
-            {
-                GetController = () => new ChassisController().Get(),
-                GuessRangeX = PilotDefinition.Conf.TireFilterLength / 2,
-                GuessRangeY = PilotDefinition.Conf.TireFilterWidth / 2,
-                detectors = new List<TireFollowing.DetectorDefinition>()
-                {
-                    new TireFollowing.DetectorDefinition()
-                    {
-                        DetectFunction = (_, lastDetectX, filters) => TireDetect.Detect(lastDetectX, filters, false),
-                        StartGuessingX = -PilotDefinition.Conf.TireFollowingStage2GuessX,
-                        StartGuessingY = 0,
-                        SwitchWalkBlindCondition = rd => rd <= PilotDefinition.Conf.TireFollowingLeaveCarWalkBlindSwitchingDistance,
-                        FinishWalkBlindCondition = rd => rd <= PilotDefinition.Conf.TireFollowingWalkBlindFinishDistance,
-                        PathTransformation = new Tuple<float, float, float>(
-                        PilotDefinition.Conf.TireFollowingLeaveCarBackLidarPathTransformationX,
-                        PilotDefinition.Conf.TireFollowingBackLidarPathTransformationY,
-                        0),
-                        LeaveSrcFunction = Leave,
-                        SrcId = srcId,
-                        DstId = dstId,
-                    },
-                },
-                CarDirection = 180f,
-                SlowDistance = PilotDefinition.Conf.TireFollowingSlowDistance,
-                MaxSpeed = 0.25f,
-                EnableHandover = true,
-                HandoverDistance = 200f,
-                HandoverSpeed = 0.3f,
-                WalkBlindTh = 0,
-                TireNum = 1
-            };
-
-            IEnumerable<bool> LeaveThenFollow()
-            {
-                foreach (var running in following.Get())
-                {
-                    if (!running) break;
-                    yield return true;
-                }
-                DLog.Log($"释放锁点{srcId}完成", "TireFollowing");
-                DLog.Log("离车TireFollowing结束，开始DstTracker", "TireFollowing");
-
-                foreach (var running in new DstTracker()
-                {
-                    Src = new Vector2(srcX, srcY),
-                    Dst = new Vector2(dstX, dstY),
-                    CarDirectionBias = 180f,
-                    InitialSendSpeed = 0.3f
-                }.Get())
-                {
-                    if (!running) break;
-                    yield return true;
-                }
-
-                yield return false;
-            }
-
-            var _dt = new DriveTask(LeaveThenFollow());
-            _dt.Wait();
-            DLog.Log("离车动作1结束", "TireFollowing");
-        }
-
-        public void LineTracking(int srcId, float srcX, float srcY, int dstId, float dstX, float dstY)
-        {
-            while (!TryLock(dstId))
-            {
-                Thread.Sleep(50);
-            }
-            var chassis = (MultiWheelChassis)PilotDefinition.Chassis;
-            chassis.SetOriginBias(0, 0, 0);
-            DLog.Log($"锁点{dstId}完成", "TireFollowing");
-            IEnumerable<bool> TrackThenFollow()
-            {
-                foreach (var running in new LineTracking()
-                {
-                    Target = PilotDefinition.Conf.LineTrackDistance + (PilotDefinition.Self.LFLActualPos + PilotDefinition.Self.LFRActualPos) / 2,
-                    LeaveSrcFunction = Leave,
-                    SrcId = srcId,
-                    EnableHandover = true,
-                    HandoverDistance = 200, 
-                    HandoverSpeed = 0.3f,
-                }.Get())
-                {
-                    if (!running) break;
-                    yield return true;
-                }
-                DLog.Log($"释放锁点{srcId}完成", "TireFollowing");
-                DLog.Log("离车LineTracking结束，开始DstTracker", "TireFollowing");
-                while (!TryLock(426))
-                {
-                    Thread.Sleep(20);
-                }
-                Leave(dstId);
-                DLog.Log($"释放锁点{dstId}完成", "TireFollowing");
-                foreach (var running in new DstTracker()
-                {
-                    Src = new Vector2(srcX, srcY),
-                    Dst = new Vector2(dstX, dstY),
-                    InitialSendSpeed = 0.3f
-                }.Get())
-                {
-                    if (!running) break;
-                    yield return true;
-                }
-                yield return false;
-            }
-            var _dt = new DriveTask(TrackThenFollow());
-            _dt.Wait();
-            DLog.Log("离车动作2结束", "TireFollowing");
-        }
-
-        //驱动器上使能
-        public void DriverAble()
-        {
-            var dl = new DriveTask(new DriverAble() { }.Get());
-            dl.Wait();
-            DLog.Log("驱动器上使能完成", "TireFollowing");
-        }
-
-        //驱动器下使能
-        public void DriverDisable()
-        {
-            var dl = new DriveTask(new DriverDisable() { }.Get());
-            dl.Wait();
-            DLog.Log("驱动器下使能完成", "TireFollowing");
-        }
-
-        // 夹抱：close 为 true 时关闭夹抱，否则打开夹抱。
-        public void ClamptoTarget(bool close)
-        {
-            if (PilotDefinition.Self.GhostMode)
-            {
-                Thread.Sleep(2000);
-                Console.WriteLine("夹抱完成");
-                return;
-            }
-            new DriveTask(new ClampToTarget()
-            {
-                LeftClampTarget = close ? PilotDefinition.Self.LeftArmUpperPos : PilotDefinition.Self.LeftArmLowerPos,
-                RightClampTarget = close ? PilotDefinition.Self.RightArmUpperPos : PilotDefinition.Self.RightArmLowerPos
-            }.Get()).Wait();
-        }
-        // Fleet crab walk: convert scheduler src/dst into the same relative crab-walk path used by MovementTest.
-        public void FleetCrabWalk(float srcX, float srcY, int srcId, float dstX, float dstY, int dstId,
-            float speed)
-        {
-            var dx = dstX - srcX;
-            var dy = dstY - srcY;
-            var pathLength = (float)Math.Sqrt(dx * dx + dy * dy);
-
-            if (pathLength <= 1f)
-            {
-                DLog.Log("FleetCrabWalk abort: path length is too short.", "FleetCrabDbg");
-                return;
-            }
-
-            var self = PilotDefinition.Self;
-            if (!self.TryGetFleetCenterFromMembers(out var centerX, out var centerY, out var centerTh) &&
-                !self.TryGetFleetCenterFromSlam(out centerX, out centerY, out centerTh))
-            {
-                DLog.Log("FleetCrabWalk abort: failed to read fleet center.", "FleetCrabDbg");
-                Hedingben.ToastText("FleetCrab requires master localization", "FleetCrab");
-                return;
-            }
-
-            var pathAngle = (float)CommonMath.RoundTh((float)(Math.Atan2(dy, dx) / Math.PI * 180.0));
-            var crabAngle = (float)CommonMath.ThDiff(pathAngle, centerTh);
-            var targetBodyWorldHeading = (float)CommonMath.RoundTh(PilotDefinition.Conf.FleetCrabBodyWorldHeadingDeg);
-            var bodyToPathAngle = (float)CommonMath.ThDiff(pathAngle, targetBodyWorldHeading);
-            DLog.Log(
-                $"call FleetCrabWalk(src=({srcX:0},{srcY:0},id:{srcId}), dst=({dstX:0},{dstY:0},id:{dstId}), " +
-                $"len={pathLength:0.0}, speed={speed:0.000}, pathAngle={pathAngle:0.0}, " +
-                $"center=({centerX:0},{centerY:0},{centerTh:0.0}), crabAngle={crabAngle:0.0}, " +
-                $"targetBodyWorld={targetBodyWorldHeading:0.0}, bodyToPath={bodyToPathAngle:0.0})",
-                "FleetCrabDbg");
-
-            if (dstId != -1)
-            {
-                while (!TryLock(dstId))
-                {
-                    Thread.Sleep(50);
-                }
-                DLog.Log($"锁点{dstId}完成", "FleetCrabDbg");
-            }
-
-            var action = new MultiWheelC.FleetCrabWalk
-            {
-                CrabAngleDeg = crabAngle,
-                BodyToPathAngleDeg = bodyToPathAngle,
-                CrabLengthMm = pathLength,
-                CrabSpeed = speed,
-                FleetCrabAccel = PilotDefinition.Conf.FleetCrabAccel,
-                FleetCrabStartAccel = PilotDefinition.Conf.FleetCrabStartAccel,
-                FleetCrabSlowDistance = PilotDefinition.Conf.FleetCrabSlowDistance,
-                FleetCrabFinishDistance = PilotDefinition.Conf.FleetCrabFinishDistance,
-                FleetCrabFinishSpeed = PilotDefinition.Conf.FleetCrabFinishSpeed,
-                FleetCrabSlowingPow = PilotDefinition.Conf.FleetCrabSlowingPow,
-                GcpThetaThreshold = PilotDefinition.Conf.FleetCrabGcpThetaThreshold
-            };
-
-            try
-            {
-                new DriveTask(action.Get()).Wait();
-            }
-            finally
-            {
-                if (srcId != -1)
-                {
-                    Leave(srcId);
-                    DLog.Log($"释放放车点{srcId}", "FleetCrabDbg");
         }
     }
 }
-
-        public void FleetCurveWalk(float srcX, float srcY, int srcId, float dstX, float dstY, int dstId,
-            float speed, params float[] trackTypeInfo)
-        {
-            if (trackTypeInfo == null || trackTypeInfo.Length < 2)
-            {
-                DLog.Log("FleetCurveWalk abort: invalid trackTypeInfo, expected Bezier type info.", "FleetCurveDbg");
-                Hedingben.ToastText("FleetCurve invalid trackTypeInfo", "FleetCurve");
-                return;
-            }
-
-            var trackType = (int)trackTypeInfo[0];
-            if (trackType != 2)
-            {
-                DLog.Log($"FleetCurveWalk abort: unsupported trackType={trackType}, only Bezier(type=2) is supported.",
-                    "FleetCurveDbg");
-                Hedingben.ToastText("FleetCurve only supports Bezier trackType=2", "FleetCurve");
-                return;
-            }
-
-            var controlPointNum = (int)trackTypeInfo[1];
-            var expectedLength = 2 + controlPointNum * 2;
-            if (controlPointNum < 3 || trackTypeInfo.Length < expectedLength)
-            {
-                DLog.Log(
-                    $"FleetCurveWalk abort: invalid Bezier trackTypeInfo. controlPointNum={controlPointNum}, " +
-                    $"length={trackTypeInfo.Length}, expected>={expectedLength}.",
-                    "FleetCurveDbg");
-                Hedingben.ToastText("FleetCurve invalid Bezier trackTypeInfo", "FleetCurve");
-                return;
-            }
-
-            BezierTrack track;
-            try
-            {
-                track = ProcessTrackTypeInfo(srcX, srcY, dstX, dstY, trackTypeInfo) as BezierTrack;
-            }
-            catch (Exception ex)
-            {
-                DLog.Log($"FleetCurveWalk abort: failed to process trackTypeInfo. {ex.Message}", "FleetCurveDbg");
-                Hedingben.ToastText("FleetCurve failed to process track", "FleetCurve");
-                return;
-            }
-
-            if (track == null)
-            {
-                DLog.Log("FleetCurveWalk abort: ProcessTrackTypeInfo did not return BezierTrack.", "FleetCurveDbg");
-                Hedingben.ToastText("FleetCurve requires BezierTrack", "FleetCurve");
-                return;
-            }
-
-            track.Speed = speed;
-            track.CarDirectionBias = 0f;
-
-            DLog.Log(
-                $"call FleetCurveWalk(src=({srcX:0},{srcY:0},id:{srcId}), dst=({dstX:0},{dstY:0},id:{dstId}), " +
-                $"speed={speed:0.000}, trackType={trackType}, controls={controlPointNum}, track={track.GetType().Name}, " +
-                $"carDirectionBias=0.0)",
-                "FleetCurveDbg");
-
-            if (dstId != -1)
-            {
-                while (!TryLock(dstId))
-                {
-                    Thread.Sleep(50);
-                }
-                DLog.Log($"閿佺偣{dstId}瀹屾垚", "FleetCurveDbg");
-            }
-
-            var action = new MultiWheelC.FleetCurveWalk
-            {
-                Track = track,
-                CurveSpeed = speed,
-                CarDirectionBias = 0f,
-                SlowDistance = PilotDefinition.Conf.FleetCurveSlowDistance,
-                FinishDistance = PilotDefinition.Conf.FleetCurveFinishDistance,
-                FinishSpeed = PilotDefinition.Conf.FleetCurveFinishSpeed,
-                SlowingPow = PilotDefinition.Conf.FleetCurveSlowingPow,
-                GcpThetaThreshold = PilotDefinition.Conf.FleetCrabGcpThetaThreshold,
-                StartSyncTimeoutSec = PilotDefinition.Conf.FleetCrabStartSyncTimeoutSec
-            };
-
-            try
-            {
-                new DriveTask(action.Get()).Wait();
-            }
-            finally
-            {
-                if (srcId != -1)
-                {
-                    Leave(srcId);
-                    DLog.Log($"release srcId={srcId}", "FleetCurveDbg");
-                }
-            }
-        }
-        
-        public void ChangeAvoidanceDistance(float stopDistance, float slowDistance)
-        {
-            DLog.Log($"call ChangeAvoidanceDistance({stopDistance},{slowDistance})");
-            PilotDefinition.Self.SlowDistance = slowDistance;
-            PilotDefinition.Self.StopDistance = stopDistance;
-        }
-        public void ChangeAvoidanceParam(float length = -1, float width = -1)
-        {
-
-            PilotDefinition.Self.CarLength = length;
-            PilotDefinition.Self.CarWidth = width;
-
-        }
-        public void SetLocation(float x, float y, float th)
-        {
-            DLog.Log($"call SetLocation({x},{y},{th})");
-            Console.WriteLine($"call SetLocation({x},{y},{th})");
-            Queue(() =>
-            {
-                while (true)
-                {
-                    var str1 = new HttpClient()
-                        .GetStringAsync(
-                            $"http://127.0.0.1:4321/setLocation?x={x}&y={y}&th={th}")
-                        .Result;
-                    Thread.Sleep(500);
-                    Console.WriteLine($"SetLocation str={str1}");
-                    var setLocationRes = JsonConvert.DeserializeObject<SetLocationRes>(str1);
-                    Console.WriteLine(setLocationRes.l_step);
-                    if (setLocationRes != null && setLocationRes.l_step == 2) break;
-                }
-            });
-        }
-        public float baseSpeed = 0;
-    }
-}

ClumsyPilot/ChassisController.cs

@@ -1,9 +1,5 @@
-using System;
-using System.Numerics;
 using ClumsyCore;
-using ClumsyCore.Interfaces;
 using ClumsyCore.Pilot;
-using FundamentalLib;
 using MDCSToolBox.Clumsy.MotionControllers;
 using MDCSToolBox.Clumsy.Movements;
 using MDCSToolBox.Clumsy.Pilot;
@@ -14,8 +10,7 @@ public class ChassisController : MovementDefinition<MultiWheelGeometricControlle
 {
     public float BaseSpeed = Configuration.conf.basicSpeed;
 
-    private DateTime _sendMotionDbgLast = DateTime.MinValue;
+    // 创建单车几何跟踪控制器（直接控本车底盘，不走多车 Auto 通道）
-
     public override MultiWheelGeometricController Get()
     {
         return new MultiWheelGeometricController
@@ -45,56 +40,6 @@ public class ChassisController : MovementDefinition<MultiWheelGeometricControlle
             DthLinearThreshold = PilotDefinition.Conf.DthLinearThreshold,
             BiasFac = PilotDefinition.Conf.BiasFac,
             BiasThreshold = PilotDefinition.Conf.BiasThreshold,
-
-            MultiVehicleSendMotion = (speed, frontTh, rearTh, idealPos, idealAngle) =>
-            {
-                var self = PilotDefinition.Self;
-                self.MultiVehicleAutoEnabled = true;
-                // A: 用固定锁对象（不再锁会被替换的字段引用）。
-                int fleetCnt;
-                lock (self.FleetLock)
-                    fleetCnt = self.MultiVehicleFleet.Count;
-
-                // 诊断（节流 ~300ms）：确认回调被调用、编队是否就绪、是否因数量不符提前 return（导致不下发速度）。
-                if ((DateTime.Now - _sendMotionDbgLast).TotalMilliseconds >= 300)
-                {
-                    _sendMotionDbgLast = DateTime.Now;
-                    DLog.Log(
-                        $"SENDMOTION speed={speed:0.000} fTh={frontTh:0.0} rTh={rearTh:0.0} " +
-                        $"ideal=({idealPos.X:0},{idealPos.Y:0},{idealAngle:0.0}) " +
-                        $"editCnt={fleetCnt}/{PilotDefinition.Conf.MultiVehicleFleetNum} " +
-                        $"earlyReturn={fleetCnt != PilotDefinition.Conf.MultiVehicleFleetNum}",
-                        "FleetCrabDbg");
-                }
-
-                if (fleetCnt != PilotDefinition.Conf.MultiVehicleFleetNum)
-                    return;
-
-                self.MultiVehicleAutoVx = speed;
-                self.MultiVehicleAutoFrontTh = frontTh;
-                self.MultiVehicleAutoRearTh = rearTh;
-                // D: 透传路径控制器算出的理想车队中心位姿（此前被丢弃），供各车按 layout 做前馈。
-                self.MultiVehicleAutoIdealX = idealPos.X;
-                self.MultiVehicleAutoIdealY = idealPos.Y;
-                self.MultiVehicleAutoIdealTh = idealAngle;
-                self.MultiVehicleAutoHasIdeal = true;
-                // B: 标记命令新鲜度。路径结束/早退/卡顿不再刷新此时刻 → 主车超时后清零速度，避免滑行。
-                self.MultiVehicleAutoCmdTime = DateTime.Now;
-            },
-
-            // G: 读取车队中心原子快照，避免跨线程读到撕裂的 x/y/th 组合。
-            MultiVehicleGetFleetPos = () =>
-            {
-                var snap = PilotDefinition.Self.GetFleetCenterSnapshot();
-                return new Location
-                {
-                    x = snap.X,
-                    y = snap.Y,
-                    th = snap.Th,
-                    l_step = 1,
-                    tick = DateTime.Now.Ticks
-                };
-            }
         };
     }
 }

ClumsyPilot/ClumsyPilot.csproj

@@ -4,19 +4,23 @@
     <TargetFramework>netstandard2.0</TargetFramework>
     <LangVersion>10</LangVersion>
     <AllowUnsafeBlocks>true</AllowUnsafeBlocks>
+    <AssemblyName>ClumsyPilot</AssemblyName>
+    <RootNamespace>MultiWheelC</RootNamespace>
     <AppendTargetFrameworkToOutputPath>false</AppendTargetFrameworkToOutputPath>
-	<OutputPath>..\..\build\Clumsy\</OutputPath>
+    <OutputPath>build\Clumsy\</OutputPath>
   </PropertyGroup>
 
   <ItemGroup>
-    <PackageReference Include="Newtonsoft.Json" Version="13.0.4" />
+    <PackageReference Include="Newtonsoft.Json" Version="13.0.3" />
     <PackageReference Include="System.Numerics.Vectors" Version="4.6.1" />
   </ItemGroup>
 
   <ItemGroup>
-    <Reference Include="CommonUsage">
+  <Compile Include="..\Shared\*.cs"
-      <HintPath>ref\CommonUsage.dll</HintPath>
+           Link="Shared\%(Filename)%(Extension)" />
-    </Reference>
+</ItemGroup>
+
+  <ItemGroup>
     <Reference Include="LessokajiWeaverUtilities">
       <HintPath>ref\LessokajiWeaverUtilities.dll</HintPath>
     </Reference>
@@ -32,10 +36,9 @@
     <Reference Include="FundamentalLib">
       <HintPath>ref\RefFundamentalLib.dll</HintPath>
     </Reference>
+    <Reference Include="CommonUsage">
+      <HintPath>..\ref\CommonUsage.dll</HintPath>
+    </Reference>
   </ItemGroup>
 
-  <Target Name="PostBuild" AfterTargets="PostBuildEvent">
-	  <Exec Command="if not exist $(SolutionDir)build\Clumsy mkdir $(SolutionDir)build\Clumsy&#xD;&#xA;if not exist $(SolutionDir)build\Clumsy_AGV2 mkdir $(SolutionDir)build\Clumsy_AGV2&#xD;&#xA;copy /Y D:\MDCS\Release\Clumsy\ClumsyLite.exe $(SolutionDir)build\Clumsy\&#xD;&#xA;copy /Y D:\MDCS\Release\Clumsy\ClumsyLite.deps.json $(SolutionDir)build\Clumsy\&#xD;&#xA;copy /Y D:\MDCS\Release\Clumsy\ClumsyLite.runtimeconfig.json $(SolutionDir)build\Clumsy\&#xD;&#xA;copy /Y D:\MDCS\Release\Clumsy\RefClumsyCore.dll $(SolutionDir)build\Clumsy\&#xD;&#xA;copy /Y D:\MDCS\Release\Clumsy\RefClumsyDance.dll $(SolutionDir)build\Clumsy\&#xD;&#xA;copy /Y D:\MDCS\Release\MDCSToolBox.dll $(SolutionDir)build\Clumsy\&#xD;&#xA;copy /Y D:\MDCS\Release\CommonUsage.dll $(SolutionDir)build\Clumsy\&#xD;&#xA;copy /Y D:\MDCS\Release\Clumsy\ClumsyLite.exe $(SolutionDir)build\Clumsy_AGV2\&#xD;&#xA;copy /Y D:\MDCS\Release\Clumsy\ClumsyLite.deps.json $(SolutionDir)build\Clumsy_AGV2\&#xD;&#xA;copy /Y D:\MDCS\Release\Clumsy\ClumsyLite.runtimeconfig.json $(SolutionDir)build\Clumsy_AGV2\&#xD;&#xA;copy /Y D:\MDCS\Release\Clumsy\RefClumsyCore.dll $(SolutionDir)build\Clumsy_AGV2\&#xD;&#xA;copy /Y D:\MDCS\Release\Clumsy\RefClumsyDance.dll $(SolutionDir)build\Clumsy_AGV2\&#xD;&#xA;copy /Y D:\MDCS\Release\MDCSToolBox.dll $(SolutionDir)build\Clumsy_AGV2\&#xD;&#xA;copy /Y D:\MDCS\Release\CommonUsage.dll $(SolutionDir)build\Clumsy_AGV2\&#xD;&#xA;copy /Y $(TargetDir)$(TargetName).dll $(SolutionDir)build\Clumsy_AGV2\&#xD;&#xA;copy /Y $(TargetDir)$(TargetName).pdb $(SolutionDir)build\Clumsy_AGV2\" />
-  </Target>
-	
 </Project>

ClumsyPilot/FleetCrabWalk.cs

@@ -1,526 +0,0 @@
-using System;
-using System.Collections.Generic;
-using System.Numerics;
-using ClumsyCore;
-using ClumsyCore.Interfaces;
-using ClumsyCore.Pilot;
-using FundamentalLib;
-using CommonUsage.Chassis;
-using CommonUsage.Mathematics;
-using MDCSToolBox.Clumsy.Movements;
-using MDCSToolBox.Clumsy.Pilot;
-
-namespace MultiWheelC;
-
-// ===== 车队联动-自动蟹行动作 =====
-// 以当前车队中心为起点，构造指定方向和长度的直线路径；
-// 执行侧直接写 MultiVehicleAuto...，由 TickMultiVehicle 自动分支统一下发。
-//
-// 控制思路参考 MDCSToolbox 几何控制器，但实现收在 MultiWheelC 内：
-// 1) 读取主车 Detour 反推车队中心，计算沿直线的进度、横向偏差和车身目标朝向偏差；
-// 2) 根据横向偏差给前后 GCP 同向修正，根据车身目标朝向偏差给前后 GCP 反向修正；
-// 3) 根据终点距离减速，并发布 ideal fleet center 给从车做前馈。
-//
-// 前提：在主车(MultiVehicleMasterEndpoint=="/")运行，且主车有 Detour 定位。
-public class FleetCrabWalk : MovementDefinition
-{
-    /// <summary>路径方向相对启动时车队朝向的夹角(deg，逆时针为正)。</summary>
-    public float CrabAngleDeg = 45f;
-
-    /// <summary>路径方向相对车身目标朝向的夹角(deg，逆时针为正)。MovementTest 会设为 CrabAngleDeg，以保持启动时车身朝向。</summary>
-    public float BodyToPathAngleDeg = 45f;
-
-    /// <summary>路径长度(mm)。</summary>
-    public float CrabLengthMm = 2000f;
-
-    /// <summary>行驶速度(m/s)。</summary>
-    public float CrabSpeed = 0.2f;
-
-    /// <summary>速度命令加速度限制(m/s^2)，小于等于 0 表示不限制。</summary>
-    public float FleetCrabAccel = 0.2f;
-
-    /// <summary>预对齐后正式下发速度前 5 秒加速度限制(m/s^2)，小于等于 0 表示不限制。</summary>
-    public float FleetCrabStartAccel = 0.01f;
-
-    /// <summary>末端开始减速距离(mm)。</summary>
-    public float FleetCrabSlowDistance = 2000f;
-
-    /// <summary>完成距离(mm)，低于该剩余距离结束动作。</summary>
-    public float FleetCrabFinishDistance = 20f;
-
-    /// <summary>末端最低速度(m/s)。</summary>
-    public float FleetCrabFinishSpeed = 0.02f;
-
-    /// <summary>末端减速曲线指数。</summary>
-    public float FleetCrabSlowingPow = 0.8f;
-
-    /// <summary>前后 GCP 舵角修正上限(deg)。</summary>
-    public float GcpThetaThreshold = 95f;
-
-    private bool _stopping;
-
-    private void Cleanup()
-    {
-        var self = PilotDefinition.Self;
-        self.MultiVehicleScriptVx = 0;
-        self.MultiVehicleScriptVy = 0;
-        self.MultiVehicleScriptVth = 0;
-        self.MultiVehicleScriptMode = 0;
-        self.MultiVehicleScriptEnabled = false;
-        self.MultiVehicleAutoVx = 0;
-        self.MultiVehicleAutoFrontTh = 0;
-        self.MultiVehicleAutoRearTh = 0;
-        self.MultiVehicleAutoHasIdeal = false;
-        self.MultiVehicleAutoEnabled = false;
-    }
-
-    public void Stop()
-    {
-        _stopping = true;
-        Cleanup();
-    }
-
-    private static float Clamp(float value, float min, float max)
-    {
-        if (value < min) return min;
-        if (value > max) return max;
-        return value;
-    }
-
-    private static float ClampAbs(float value, float limit)
-    {
-        var absLimit = Math.Abs(limit);
-        if (absLimit <= 0) return value;
-        if (value > absLimit) return absLimit;
-        if (value < -absLimit) return -absLimit;
-        return value;
-    }
-
-    private static float Slew(float current, float target, float maxDelta)
-    {
-        if (maxDelta <= 0) return target;
-        if (target > current + maxDelta) return current + maxDelta;
-        if (target < current - maxDelta) return current - maxDelta;
-        return target;
-    }
-
-    private static float AverageAngle(float frontTh, float rearTh)
-    {
-        var diff = (float)CommonMath.ThDiff(frontTh, rearTh);
-        return (float)CommonMath.RoundTh(rearTh + diff / 2f);
-    }
-
-    private static void ResolveCrabDriveEquivalent(float speed, float rawFrontTh, float rawRearTh, float steerLimit,
-        out float driveSpeed, out float frontTh, out float rearTh, out bool reverseEquivalent, out float rawBaseTh)
-    {
-        var limit = Math.Min(179f, Math.Max(1f, Math.Abs(steerLimit)));
-        rawBaseTh = AverageAngle(rawFrontTh, rawRearTh);
-        driveSpeed = speed;
-        frontTh = rawFrontTh;
-        rearTh = rawRearTh;
-        reverseEquivalent = false;
-
-        if (rawBaseTh > limit)
-        {
-            frontTh = (float)CommonMath.RoundTh(frontTh - 180f);
-            rearTh = (float)CommonMath.RoundTh(rearTh - 180f);
-            driveSpeed = -driveSpeed;
-            reverseEquivalent = true;
-        }
-        else if (rawBaseTh < -limit)
-        {
-            frontTh = (float)CommonMath.RoundTh(frontTh + 180f);
-            rearTh = (float)CommonMath.RoundTh(rearTh + 180f);
-            driveSpeed = -driveSpeed;
-            reverseEquivalent = true;
-        }
-
-        frontTh = ClampAbs(frontTh, limit);
-        rearTh = ClampAbs(rearTh, limit);
-    }
-
-    private static float ProbeSpeed(float speed)
-    {
-        return Math.Abs(speed) > 1e-4f ? speed : 1f;
-    }
-
-    private static bool TryGetMotionYawSign(float frontTh, float rearTh, float driveSpeed, float controlRadius,
-        out float yawSign)
-    {
-        yawSign = 0f;
-        if (Math.Abs(CommonMath.ThDiff(frontTh, rearTh)) <= 1e-3f)
-            return false;
-
-        var radius = Math.Max(1f, Math.Abs(controlRadius));
-        Vector2 pFront = new(radius, 0), pRear = new(-radius, 0),
-            normFront = CommonMath.Transform2D(pFront, frontTh + 90f, Vector2.UnitX),
-            normRear = CommonMath.Transform2D(pRear, rearTh + 90f, Vector2.UnitX);
-        var (intersect, center) = CommonMath.TwoLinesIntersection(pFront, normFront, pRear, normRear);
-        if (!intersect)
-            return false;
-
-        // Match MultiWheelChassis.SendMotion: the tangent side is selected by
-        // rotCenter.Y > 1, and reverse-equivalent motion flips the yaw direction.
-        var tangentSign = center.Y > 1f ? 1f : -1f;
-        var speedSign = driveSpeed >= 0f ? 1f : -1f;
-        yawSign = speedSign * tangentSign;
-        return true;
-    }
-
-    private static float GetYawSplitSign(float baseTh, float speed, float steerLimit, float controlRadius)
-    {
-        const float probeDth = 1f;
-        ResolveCrabDriveEquivalent(ProbeSpeed(speed), baseTh + probeDth, baseTh - probeDth, steerLimit,
-            out var probeSpeed, out var probeFrontTh, out var probeRearTh, out _, out _);
-        return TryGetMotionYawSign(probeFrontTh, probeRearTh, probeSpeed, controlRadius, out var yawSign)
-            ? yawSign
-            : 1f;
-    }
-
-    private static float EstimateLateralVelocity(float bodyTh, float frontTh, float rearTh, float driveSpeed,
-        Vector2 pathLeft)
-    {
-        var motionTh = (float)CommonMath.RoundTh(bodyTh + AverageAngle(frontTh, rearTh));
-        var rad = motionTh / 180f * Math.PI;
-        var dir = new Vector2((float)Math.Cos(rad), (float)Math.Sin(rad));
-        if (driveSpeed < 0f)
-            dir = -dir;
-        return Vector2.Dot(dir, pathLeft);
-    }
-
-    private static float ScoreBiasSign(float baseTh, float bodyTh, float speed, float steerLimit, Vector2 pathLeft,
-        float lateral, float biasProbe)
-    {
-        ResolveCrabDriveEquivalent(ProbeSpeed(speed), baseTh + biasProbe, baseTh + biasProbe, steerLimit,
-            out var probeSpeed, out var probeFrontTh, out var probeRearTh, out _, out _);
-        var lateralVelocity = EstimateLateralVelocity(bodyTh, probeFrontTh, probeRearTh, probeSpeed, pathLeft);
-        return -Math.Sign(lateral) * lateralVelocity;
-    }
-
-    private static float GetLateralBiasSign(float baseTh, float bodyTh, float speed, float steerLimit, Vector2 pathLeft,
-        float lateral)
-    {
-        if (Math.Abs(lateral) <= 1e-3f)
-            return 1f;
-
-        const float probeBias = 1f;
-        var positiveScore = ScoreBiasSign(baseTh, bodyTh, speed, steerLimit, pathLeft, lateral, probeBias);
-        var negativeScore = ScoreBiasSign(baseTh, bodyTh, speed, steerLimit, pathLeft, lateral, -probeBias);
-        return positiveScore >= negativeScore ? 1f : -1f;
-    }
-
-    private static bool TryGetControlFleetCenter(PilotDefinition self, out float centerX, out float centerY,
-        out float centerTh, out string source)
-    {
-        if (self.TryGetFleetCenterFromMembers(out centerX, out centerY, out centerTh))
-        {
-            source = "fleet";
-            return true;
-        }
-
-        if (self.TryGetFleetCenterFromSlam(out centerX, out centerY, out centerTh))
-        {
-            source = "slam";
-            return true;
-        }
-
-        source = "none";
-        return false;
-    }
-
-    public override IEnumerable<bool> Get()
-    {
-        var self = PilotDefinition.Self;
-        var conf = PilotDefinition.Conf;
-        var chassis = BasicPilotBase.Chassis as MultiWheelChassis;
-        if (chassis == null)
-        {
-            DLog.Log("ABORT: FleetCrabWalk requires MultiWheelChassis.", "FleetCrabDbg");
-            yield break;
-        }
-        _stopping = false;
-
-        DLog.Log(
-            $"ENTER master?={conf.MultiVehicleMasterEndpoint == "/"} endpoint={conf.MultiVehicleMasterEndpoint} " +
-            $"fleetNum={conf.MultiVehicleFleetNum} useDetect={conf.MultiVehicleUseDetect} " +
-            $"syncUseDetour={conf.MultiVehicleSyncUseDetour} useIdealCenter={conf.MultiVehicleAutoUseIdealCenter} " +
-            $"autoFields=true pathMode=relative pathAngle={CrabAngleDeg:0.0} " +
-            $"bodyToPath={BodyToPathAngleDeg:0.0} gcpLimit={GcpThetaThreshold:0.0} " +
-            $"biasFac={conf.BiasFac:0.00} fleetCrabDthFac={conf.FleetCrabDthLinearFac:0.00}",
-            "FleetCrabDbg");
-
-        if (conf.MultiVehicleMasterEndpoint != "/")
-        {
-            DLog.Log($"ABORT: 非主车 (endpoint={conf.MultiVehicleMasterEndpoint})", "FleetCrabDbg");
-            Hedingben.ToastText("车队蟹行需在主车(主车端点=\"/\")运行", "FleetCrab");
-            yield break;
-        }
-
-        // 注意：getCartLocation() 在无有效 Detour 定位时会阻塞——若卡在这里且后面看不到 CENTER 日志，即定位未就绪。
-        DLog.Log("主车校验通过，开始读取车队中心 (getCartLocation 无定位会阻塞)…", "FleetCrabDbg");
-        if (!TryGetControlFleetCenter(self, out var x0, out var y0, out var theta, out var initialCenterSource))
-        {
-            DLog.Log("ABORT: TryGetFleetCenterFromSlam 返回 false (无定位)", "FleetCrabDbg");
-            Hedingben.ToastText("车队蟹行需要主车 Detour 定位", "FleetCrab");
-            yield break;
-        }
-        DLog.Log($"CENTER 车队中心=({x0:0},{y0:0},{theta:0.0})", "FleetCrabDbg");
-
-        DLog.Log($"CENTER_SOURCE source={initialCenterSource} center=({x0:0},{y0:0},{theta:0.0})", "FleetCrabDbg");
-
-        var pathStart = new Vector2(x0, y0);
-        var pathLengthMm = CrabLengthMm;
-        var phi = CommonMath.RoundTh(theta + CrabAngleDeg);
-        var dst = CommonMath.Transform2D(pathStart, phi, new Vector2(pathLengthMm, 0));
-        var targetBodyTh = CommonMath.RoundTh(phi - BodyToPathAngleDeg);
-        var phiRad = phi / 180.0 * Math.PI;
-        var pathDir = new Vector2((float)Math.Cos(phiRad), (float)Math.Sin(phiRad));
-        var pathLeft = new Vector2(-pathDir.Y, pathDir.X);
-
-        DLog.Log(
-            $"START center=({x0:0},{y0:0},{theta:0.0}) pathMode=relative " +
-            $"src=({pathStart.X:0},{pathStart.Y:0}) pathAngle={CrabAngleDeg:0.0} bodyToPath={BodyToPathAngleDeg:0.0} " +
-            $"phi={phi:0.0} targetBody={targetBodyTh:0.0} " +
-            $"len={pathLengthMm:0} dst=({dst.X:0},{dst.Y:0}) speed={CrabSpeed:0.000} startAccel={FleetCrabStartAccel:0.000} accel={FleetCrabAccel:0.000} " +
-            $"slow={FleetCrabSlowDistance:0} finishDist={FleetCrabFinishDistance:0} " +
-            $"finishSpeed={FleetCrabFinishSpeed:0.000} slowingPow={FleetCrabSlowingPow:0.00}",
-            "FleetCrabDbg");
-
-        var gcpLimit = Math.Max(1f, Math.Abs(GcpThetaThreshold));
-        var controlRadius = Math.Max(1f, Math.Abs(conf.TestCarSyncDistance) / 2f);
-        ResolveCrabDriveEquivalent(0f, (float)CommonMath.ThDiff(phi, theta),
-            (float)CommonMath.ThDiff(phi, theta), gcpLimit, out _, out var holdFrontTh, out var holdRearTh,
-            out _, out _);
-        var warmStart = DateTime.Now;
-        var warmSeqBaseline = self.BeginFleetMotionWarmup();
-
-        self.MultiVehicleScriptEnabled = false;
-        self.MultiVehicleScriptMode = 0;
-        self.MultiVehicleScriptVx = 0;
-        self.MultiVehicleScriptVy = 0;
-        self.MultiVehicleScriptVth = 0;
-        self.MultiVehicleAutoEnabled = true;
-        self.MultiVehicleAutoVx = 0;
-        self.MultiVehicleAutoFrontTh = holdFrontTh;
-        self.MultiVehicleAutoRearTh = holdRearTh;
-        self.MultiVehicleAutoIdealX = pathStart.X;
-        self.MultiVehicleAutoIdealY = pathStart.Y;
-        self.MultiVehicleAutoIdealTh = targetBodyTh;
-        self.MultiVehicleAutoHasIdeal = true;
-        self.MultiVehicleAutoCmdTime = DateTime.Now;
-        self.PrimeMasterAutoFromSlam();
-        DLog.Log(
-            $"WARMUP auto fields enabled, waiting for fleet startup sync seqBase={warmSeqBaseline} " +
-            $"hold=({holdFrontTh:0.00},{holdRearTh:0.00})",
-            "FleetCrabDbg");
-
-        var warmEnd = warmStart.AddSeconds(Math.Max(1.0f, conf.FleetCrabStartSyncTimeoutSec));
-        var warmIter = 0;
-        var warmReady = false;
-        var warmDetail = "";
-        while (!_stopping && DateTime.Now < warmEnd)
-        {
-            warmIter++;
-            self.MultiVehicleScriptEnabled = false;
-            self.MultiVehicleScriptMode = 0;
-            self.MultiVehicleAutoEnabled = true;
-            self.MultiVehicleAutoVx = 0;
-            self.MultiVehicleAutoFrontTh = holdFrontTh;
-            self.MultiVehicleAutoRearTh = holdRearTh;
-            self.MultiVehicleAutoIdealX = pathStart.X;
-            self.MultiVehicleAutoIdealY = pathStart.Y;
-            self.MultiVehicleAutoIdealTh = targetBodyTh;
-            self.MultiVehicleAutoHasIdeal = true;
-            self.MultiVehicleAutoCmdTime = DateTime.Now;
-            self.PrimeMasterAutoFromSlam();
-            var snap = self.GetFleetCenterSnapshot();
-            int cnt;
-            lock (self.FleetLock) cnt = self.MultiVehicleFleet.Count;
-            if (warmIter % 5 == 0)
-                DLog.Log(
-                    $"WARMUP#{warmIter} 快照=({snap.X:0},{snap.Y:0},{snap.Th:0.0}) tick={snap.Tick} " +
-                    $"autoEn={self.MultiVehicleAutoEnabled} scriptEn={self.MultiVehicleScriptEnabled} cnt={cnt}/{conf.MultiVehicleFleetNum} " +
-                    $"detail={warmDetail}",
-                    "FleetCrabDbg");
-            if (self.IsFleetMotionWarmupReady(warmStart, warmSeqBaseline,
-                    conf.TestCarSyncTh, conf.TestCarSyncDistance, out warmDetail))
-            {
-                warmReady = true;
-                DLog.Log(
-                    $"WARMUP done iter={warmIter} 快照=({snap.X:0},{snap.Y:0},{snap.Th:0.0}) cnt={cnt} detail={warmDetail}",
-                    "FleetCrabDbg");
-                break;
-            }
-            yield return true;
-        }
-        if (!warmReady)
-        {
-            DLog.Log($"WARMUP timeout: fleet startup sync failed, abort action. detail={warmDetail}",
-                "FleetCrabDbg");
-            Hedingben.ToastText("车队蟹行启动同步超时，已取消", "FleetCrab");
-            Cleanup();
-            yield break;
-        }
-
-        Hedingben.ToastText($"车队蟹行 路径{phi:0.0}° 车身夹角{BodyToPathAngleDeg:0.0}° 长度{pathLengthMm:0}mm", "FleetCrab");
-
-        if (warmReady && self.TryGetFleetCenterFromMembers(out var warmX, out var warmY, out var warmTh))
-        {
-            x0 = warmX;
-            y0 = warmY;
-            theta = warmTh;
-            pathStart = new Vector2(x0, y0);
-            phi = CommonMath.RoundTh(theta + CrabAngleDeg);
-            dst = CommonMath.Transform2D(pathStart, phi, new Vector2(pathLengthMm, 0));
-            targetBodyTh = CommonMath.RoundTh(phi - BodyToPathAngleDeg);
-            phiRad = phi / 180.0 * Math.PI;
-            pathDir = new Vector2((float)Math.Cos(phiRad), (float)Math.Sin(phiRad));
-            pathLeft = new Vector2(-pathDir.Y, pathDir.X);
-            self.MultiVehicleAutoIdealX = pathStart.X;
-            self.MultiVehicleAutoIdealY = pathStart.Y;
-            self.MultiVehicleAutoIdealTh = targetBodyTh;
-            self.MultiVehicleAutoCmdTime = DateTime.Now;
-            DLog.Log(
-                $"WARMUP_REBASE source=fleet center=({x0:0},{y0:0},{theta:0.0}) phi={phi:0.0} targetBody={targetBodyTh:0.0} dst=({dst.X:0},{dst.Y:0})",
-                "FleetCrabDbg");
-        }
-
-        var iter = 0;
-        var lastLog = DateTime.MinValue;
-        var finishDistance = Math.Max(0f, FleetCrabFinishDistance);
-        var slowDistance = Math.Max(finishDistance + 1f, FleetCrabSlowDistance);
-        var baseSpeed = Math.Abs(CrabSpeed);
-        var finishSpeed = Math.Min(baseSpeed, Math.Abs(FleetCrabFinishSpeed));
-        var slowingPow = Math.Max(0.01f, FleetCrabSlowingPow);
-        var accel = Math.Abs(FleetCrabAccel);
-        var startAccel = Math.Abs(FleetCrabStartAccel);
-        var cmdSpeed = 0f;
-        var lastTick = DateTime.Now;
-        var speedRampStart = DateTime.Now;
-        var stopReason = "done";
-
-        while (!_stopping)
-        {
-            iter++;
-
-            if (!TryGetControlFleetCenter(self, out var cx, out var cy, out var cth, out var centerSource))
-            {
-                stopReason = "fleet center invalid";
-                DLog.Log("ABORT: TryGetControlFleetCenter returned false during auto crab.", "FleetCrabDbg");
-                break;
-            }
-            var delta = new Vector2(cx - pathStart.X, cy - pathStart.Y);
-            var along = Vector2.Dot(delta, pathDir);
-            var lateral = Vector2.Dot(delta, pathLeft);
-            var remain = pathLengthMm - along;
-            if (remain <= finishDistance)
-                break;
-
-            var targetSpeed = baseSpeed;
-            var slowRatio = 1f;
-            if (remain < slowDistance)
-            {
-                slowRatio = (float)Math.Pow(Clamp(Math.Max(0, remain) / slowDistance, 0f, 1f), slowingPow);
-                targetSpeed = slowRatio * (baseSpeed - finishSpeed) + finishSpeed;
-            }
-            var now = DateTime.Now;
-            var dt = Math.Max(0.001f, (float)(now - lastTick).TotalSeconds);
-            lastTick = now;
-            var rampElapsed = (now - speedRampStart).TotalSeconds;
-            var activeAccel = rampElapsed < 5.0 ? startAccel : accel;
-            var speed = activeAccel > 0 ? Slew(cmdSpeed, targetSpeed, activeAccel * dt) : targetSpeed;
-            cmdSpeed = speed;
-
-            var baseCrabTh = (float)CommonMath.ThDiff(phi, cth);
-            var headingErr = (float)CommonMath.ThDiff(targetBodyTh, cth);
-            var headingErrReverse = (float)CommonMath.ThDiff(cth, targetBodyTh);
-            var targetBodyToPath = (float)CommonMath.ThDiff(phi, targetBodyTh);
-            var rawBiasMagnitude = (float)(Math.Atan(conf.BiasFac * Math.Abs(lateral) / 1000f /
-                Math.Max(speed, 0.3f)) / Math.PI * 180.0);
-            var biasSign = GetLateralBiasSign(baseCrabTh, cth, speed, gcpLimit, pathLeft, lateral);
-            var rawBiasItem = rawBiasMagnitude * biasSign;
-            var biasItem = ClampAbs(rawBiasItem, conf.BiasThreshold);
-            var yawSplitSign = GetYawSplitSign(baseCrabTh + biasItem, speed, gcpLimit, controlRadius);
-            var rawDthItem = conf.FleetCrabDthLinearFac * headingErr * yawSplitSign;
-            var dthItem = ClampAbs(rawDthItem, conf.FleetCrabDthLinearThreshold);
-            var rawFrontTh = baseCrabTh + biasItem + dthItem;
-            var rawRearTh = baseCrabTh + biasItem - dthItem;
-            ResolveCrabDriveEquivalent(speed, rawFrontTh, rawRearTh, gcpLimit, out var driveSpeed,
-                out var frontTh, out var rearTh, out var reverseEquivalent, out var rawBaseTh);
-            holdFrontTh = frontTh;
-            holdRearTh = rearTh;
-            var idealAlong = Clamp(along, 0f, pathLengthMm);
-            var ideal = pathStart + pathDir * idealAlong;
-
-            self.MultiVehicleScriptEnabled = false;
-            self.MultiVehicleScriptMode = 0;
-            self.MultiVehicleScriptVx = 0;
-            self.MultiVehicleScriptVy = 0;
-            self.MultiVehicleScriptVth = 0;
-            self.MultiVehicleAutoEnabled = true;
-            self.MultiVehicleAutoVx = driveSpeed;
-            self.MultiVehicleAutoFrontTh = frontTh;
-            self.MultiVehicleAutoRearTh = rearTh;
-            self.MultiVehicleAutoIdealX = ideal.X;
-            self.MultiVehicleAutoIdealY = ideal.Y;
-            self.MultiVehicleAutoIdealTh = targetBodyTh;
-            self.MultiVehicleAutoHasIdeal = true;
-            self.MultiVehicleAutoCmdTime = DateTime.Now;
-
-            if ((DateTime.Now - lastLog).TotalMilliseconds >= 300)
-            {
-                lastLog = DateTime.Now;
-                var snap = self.GetFleetCenterSnapshot();
-                int fleetCnt;
-                lock (self.FleetLock) fleetCnt = self.MultiVehicleFleet.Count;
-                DLog.Log(
-                    $"ITER#{iter} centerSrc={centerSource} center=({cx:0},{cy:0},{cth:0.0}) snap=({snap.X:0},{snap.Y:0},{snap.Th:0.0}) " +
-                    $"along={along:0} lateral={lateral:0} remain={remain:0} headingErr={headingErr:0.0} " +
-                    $"baseTh={baseCrabTh:0.0} bias={biasItem:0.0} dth={dthItem:0.0} " +
-                    $"slowRatio={slowRatio:0.000} targetV={targetSpeed:0.000} rampT={rampElapsed:0.0} accel={activeAccel:0.000} auto=(vx:{driveSpeed:0.000},fTh:{frontTh:0.0},rTh:{rearTh:0.0}) " +
-                    $"ideal=({ideal.X:0},{ideal.Y:0},{targetBodyTh:0.0}) scriptEn={self.MultiVehicleScriptEnabled} " +
-                    $"cnt={fleetCnt}/{conf.MultiVehicleFleetNum}",
-                    "FleetCrabDbg");
-                DLog.Log(
-                    $"CTRL iter={iter} centerSrc:{centerSource} phi:{phi:0.00} targetBody:{targetBodyTh:0.00} startTheta:{theta:0.00} " +
-                    $"cth:{cth:0.00} crabAngle:{CrabAngleDeg:0.00} bodyToPathCfg:{BodyToPathAngleDeg:0.00} " +
-                    $"targetBodyToPath:{targetBodyToPath:0.00} bodyToPathNow:{baseCrabTh:0.00} " +
-                    $"headingErr(target-current):{headingErr:0.00} reverse(current-target):{headingErrReverse:0.00} yawSign:{yawSplitSign:0} " +
-                    $"fleetCrabDthFac:{conf.FleetCrabDthLinearFac:0.000} rawDth:{rawDthItem:0.00} dth:{dthItem:0.00} dthLimit:{conf.FleetCrabDthLinearThreshold:0.00} " +
-                    $"lateral:{lateral:0.0} biasFac:{conf.BiasFac:0.000} biasSign:{biasSign:0} rawBias:{rawBiasItem:0.00} bias:{biasItem:0.00} biasLimit:{conf.BiasThreshold:0.00} " +
-                    $"baseTh:{baseCrabTh:0.00} rawBase:{rawBaseTh:0.00} rawOut(f:{rawFrontTh:0.00},r:{rawRearTh:0.00}) " +
-                    $"out(f:{frontTh:0.00},r:{rearTh:0.00}) gcpLimit:{gcpLimit:0.00} revEq:{reverseEquivalent} " +
-                    $"speedRaw:{speed:0.000} speed:{driveSpeed:0.000} rampT:{rampElapsed:0.0} accel:{activeAccel:0.000} along:{along:0.0} remain:{remain:0.0} ideal=({ideal.X:0.0},{ideal.Y:0.0},{targetBodyTh:0.00})",
-                    "FleetCrabHeadingDbg");
-            }
-            yield return true;
-        }
-
-        if (_stopping)
-            stopReason = "stop";
-
-        self.MultiVehicleAutoVx = 0;
-        self.MultiVehicleAutoFrontTh = holdFrontTh;
-        self.MultiVehicleAutoRearTh = holdRearTh;
-        self.MultiVehicleAutoCmdTime = DateTime.Now;
-        DLog.Log(
-            $"STOP_HOLD iter={iter} reason={stopReason} hold=(fTh:{holdFrontTh:0.0},rTh:{holdRearTh:0.0}) cmdSpeed={cmdSpeed:0.000}",
-            "FleetCrabDbg");
-        var settleEnd = DateTime.Now.AddMilliseconds(Math.Max(100, conf.MultiVehicleSyncInterval * 3));
-        while (!_stopping && DateTime.Now < settleEnd)
-        {
-            self.MultiVehicleScriptEnabled = false;
-            self.MultiVehicleScriptMode = 0;
-            self.MultiVehicleAutoEnabled = true;
-            self.MultiVehicleAutoVx = 0;
-            self.MultiVehicleAutoFrontTh = holdFrontTh;
-            self.MultiVehicleAutoRearTh = holdRearTh;
-            self.MultiVehicleAutoCmdTime = DateTime.Now;
-            yield return true;
-        }
-
-        Cleanup();
-        Hedingben.ToastText("车队蟹行完成", "FleetCrab");
-        DLog.Log($"DONE iter={iter} reason={stopReason}", "FleetCrabDbg");
-    }
-}

ClumsyPilot/FleetCurveWalk.cs

@@ -1,411 +0,0 @@
-using System;
-using System.Collections.Generic;
-using System.Globalization;
-using System.Numerics;
-using ClumsyCore;
-using ClumsyCore.Interfaces;
-using ClumsyCore.Pilot;
-using FundamentalLib;
-using CommonUsage.Chassis;
-using CommonUsage.Mathematics;
-using MDCSToolBox.Clumsy.MotionControllers;
-using MDCSToolBox.Clumsy.Movements;
-using MDCSToolBox.Clumsy.Pilot;
-using MDCSToolBox.Clumsy.Tracks;
-
-namespace MultiWheelC;
-
-public class FleetCurveWalk : MovementDefinition
-{
-    public BezierTrack Track;
-    public List<Vector2> ControlPoints = new();
-    public float CurveSpeed = 0.2f;
-    public float CarDirectionBias = 0f;
-    public int BezierResolution = 100;
-    public float SlowDistance = 2000f;
-    public float FinishDistance = 20f;
-    public float FinishSpeed = 0.02f;
-    public float SlowingPow = 0.8f;
-    public float GcpThetaThreshold = 95f;
-    public float StartSyncTimeoutSec = 8f;
-
-    private bool _stopping;
-    private MultiWheelGeometricController _controller;
-    private MultiWheelChassis _chassis;
-    private bool _savedControlPoints;
-    private float _savedControlRadius;
-    private Vector2 _savedGcp0;
-    private Vector2 _savedGcp1;
-
-    public void Stop()
-    {
-        _stopping = true;
-        if (_controller != null)
-            _controller.BreakAndHold = true;
-        Cleanup();
-    }
-
-    public static bool TryParsePointList(string text, out List<Vector2> points, out string error)
-    {
-        points = new List<Vector2>();
-        error = "";
-        if (string.IsNullOrWhiteSpace(text))
-        {
-            error = "empty control point list";
-            return false;
-        }
-
-        var segments = text.Split(new[] { ';', '|' }, StringSplitOptions.RemoveEmptyEntries);
-        for (var i = 0; i < segments.Length; i++)
-        {
-            var pair = segments[i].Split(new[] { ',', ' ', '\t' }, StringSplitOptions.RemoveEmptyEntries);
-            if (pair.Length != 2)
-            {
-                error = $"invalid point #{i + 1}: {segments[i]}";
-                return false;
-            }
-
-            if (!TryParseFloat(pair[0], out var x) || !TryParseFloat(pair[1], out var y))
-            {
-                error = $"invalid number in point #{i + 1}: {segments[i]}";
-                return false;
-            }
-            points.Add(new Vector2(x, y));
-        }
-
-        if (points.Count < 3)
-        {
-            error = "Bezier curve requires at least 3 control points";
-            return false;
-        }
-
-        return true;
-    }
-
-    public static List<Vector2> BuildRelativeControlPoints(Vector2 start, float startTh, List<Vector2> relativePoints)
-    {
-        var source = relativePoints ?? new List<Vector2>();
-        var normalized = new List<Vector2>();
-        if (source.Count == 0 || Vector2.Distance(source[0], Vector2.Zero) > 1f)
-            normalized.Add(Vector2.Zero);
-        for (var i = 0; i < source.Count; i++)
-            normalized.Add(source[i]);
-        if (normalized.Count < 2)
-            normalized.Add(new Vector2(1000f, 0f));
-        if (normalized.Count < 3)
-            normalized.Add(new Vector2(2000f, 0f));
-
-        var result = new List<Vector2>();
-        for (var i = 0; i < normalized.Count; i++)
-            result.Add(CommonMath.Transform2D(start, startTh, normalized[i]));
-        return result;
-    }
-
-    public static List<Vector2> BuildAgvControlPoints(float srcX, float srcY, float dstX, float dstY,
-        params float[] controlPointCoords)
-    {
-        var src = new Vector2(srcX, srcY);
-        var dst = new Vector2(dstX, dstY);
-        var result = new List<Vector2>();
-        if (controlPointCoords == null || controlPointCoords.Length == 0)
-        {
-            result.Add(src);
-            result.Add((src + dst) / 2f);
-            result.Add(dst);
-            return result;
-        }
-
-        if (controlPointCoords.Length % 2 != 0)
-            throw new ArgumentException("FleetCurve controlPointCoords must contain x,y pairs.");
-
-        var supplied = new List<Vector2>();
-        for (var i = 0; i < controlPointCoords.Length; i += 2)
-            supplied.Add(new Vector2(controlPointCoords[i], controlPointCoords[i + 1]));
-
-        if (supplied.Count >= 3 &&
-            Vector2.Distance(supplied[0], src) <= 10f &&
-            Vector2.Distance(supplied[supplied.Count - 1], dst) <= 10f)
-            return supplied;
-
-        result.Add(src);
-        for (var i = 0; i < supplied.Count; i++)
-            result.Add(supplied[i]);
-        result.Add(dst);
-        if (result.Count < 3)
-            result.Insert(1, (src + dst) / 2f);
-        return result;
-    }
-
-    private static bool TryParseFloat(string text, out float value)
-    {
-        return float.TryParse(text, NumberStyles.Float, CultureInfo.InvariantCulture, out value) ||
-               float.TryParse(text, out value);
-    }
-
-    private static float ClampAbs(float value, float limit)
-    {
-        var absLimit = Math.Abs(limit);
-        if (absLimit <= 0) return value;
-        if (value > absLimit) return absLimit;
-        if (value < -absLimit) return -absLimit;
-        return value;
-    }
-
-    private static bool TryGetControlFleetCenter(PilotDefinition self, out float centerX, out float centerY,
-        out float centerTh, out string source)
-    {
-        if (self.TryGetFleetCenterFromMembers(out centerX, out centerY, out centerTh))
-        {
-            source = "fleet";
-            return true;
-        }
-
-        if (self.TryGetFleetCenterFromSlam(out centerX, out centerY, out centerTh))
-        {
-            source = "slam";
-            return true;
-        }
-
-        source = "none";
-        return false;
-    }
-
-    private void Cleanup()
-    {
-        var self = PilotDefinition.Self;
-        self.MultiVehicleScriptVx = 0;
-        self.MultiVehicleScriptVy = 0;
-        self.MultiVehicleScriptVth = 0;
-        self.MultiVehicleScriptMode = 0;
-        self.MultiVehicleScriptEnabled = false;
-        self.MultiVehicleAutoVx = 0;
-        self.MultiVehicleAutoFrontTh = 0;
-        self.MultiVehicleAutoRearTh = 0;
-        self.MultiVehicleAutoHasIdeal = false;
-        self.MultiVehicleAutoEnabled = false;
-        RestoreControlPointRadius();
-    }
-
-    private void ApplyFleetControlPointRadius(MultiWheelChassis chassis, float radius)
-    {
-        if (!_savedControlPoints)
-        {
-            _chassis = chassis;
-            _savedControlRadius = chassis.ControlPointRadius;
-            var gcps = chassis.GetGeometricControlPoints();
-            if (gcps.Count >= 2)
-            {
-                _savedGcp0 = gcps[0].Position;
-                _savedGcp1 = gcps[1].Position;
-            }
-            _savedControlPoints = true;
-        }
-
-        chassis.ControlPointRadius = radius;
-        var points = chassis.GetGeometricControlPoints();
-        if (points.Count >= 2)
-        {
-            points[0].Position = new Vector2(radius, 0);
-            points[1].Position = new Vector2(-radius, 0);
-        }
-    }
-
-    private void RestoreControlPointRadius()
-    {
-        if (!_savedControlPoints || _chassis == null)
-            return;
-
-        _chassis.ControlPointRadius = _savedControlRadius;
-        var points = _chassis.GetGeometricControlPoints();
-        if (points.Count >= 2)
-        {
-            points[0].Position = _savedGcp0;
-            points[1].Position = _savedGcp1;
-        }
-        _savedControlPoints = false;
-    }
-
-    private static void WriteWarmupAuto(PilotDefinition self, Vector2 idealPos, float idealTh,
-        float frontTh, float rearTh)
-    {
-        self.MultiVehicleScriptEnabled = false;
-        self.MultiVehicleScriptMode = 0;
-        self.MultiVehicleScriptVx = 0;
-        self.MultiVehicleScriptVy = 0;
-        self.MultiVehicleScriptVth = 0;
-        self.MultiVehicleAutoEnabled = true;
-        self.MultiVehicleAutoVx = 0;
-        self.MultiVehicleAutoFrontTh = frontTh;
-        self.MultiVehicleAutoRearTh = rearTh;
-        self.MultiVehicleAutoIdealX = idealPos.X;
-        self.MultiVehicleAutoIdealY = idealPos.Y;
-        self.MultiVehicleAutoIdealTh = idealTh;
-        self.MultiVehicleAutoHasIdeal = true;
-        self.MultiVehicleAutoCmdTime = DateTime.Now;
-    }
-
-    public override IEnumerable<bool> Get()
-    {
-        var self = PilotDefinition.Self;
-        var conf = PilotDefinition.Conf;
-        var chassis = BasicPilotBase.Chassis as MultiWheelChassis;
-        _stopping = false;
-
-        if (chassis == null)
-        {
-            DLog.Log("ABORT: FleetCurveWalk requires MultiWheelChassis.", "FleetCurveDbg");
-            yield break;
-        }
-
-        if (conf.MultiVehicleMasterEndpoint != "/")
-        {
-            DLog.Log($"ABORT: FleetCurveWalk must run on master endpoint, endpoint={conf.MultiVehicleMasterEndpoint}",
-                "FleetCurveDbg");
-            Hedingben.ToastText("FleetCurve requires master vehicle", "FleetCurve");
-            yield break;
-        }
-
-        if (Track == null && (ControlPoints == null || ControlPoints.Count < 3))
-        {
-            DLog.Log("ABORT: FleetCurveWalk requires a BezierTrack or at least 3 control points.", "FleetCurveDbg");
-            Hedingben.ToastText("FleetCurve requires track or >=3 control points", "FleetCurve");
-            yield break;
-        }
-
-        if (!TryGetControlFleetCenter(self, out var x0, out var y0, out var theta, out var initialCenterSource))
-        {
-            DLog.Log("ABORT: FleetCurveWalk failed to read fleet center.", "FleetCurveDbg");
-            Hedingben.ToastText("FleetCurve requires master localization", "FleetCurve");
-            yield break;
-        }
-
-        var baseSpeed = Math.Abs(CurveSpeed);
-        if (baseSpeed <= 1e-4f)
-        {
-            DLog.Log("ABORT: FleetCurveWalk speed is zero.", "FleetCurveDbg");
-            yield break;
-        }
-
-        var resolution = Math.Max(2, BezierResolution);
-        var speedFinish = Math.Min(baseSpeed, Math.Abs(FinishSpeed));
-        var gcpLimit = Math.Max(1f, Math.Abs(GcpThetaThreshold));
-        var controlRadius = Math.Max(1f, Math.Abs(conf.TestCarSyncDistance) / 2f);
-
-        ApplyFleetControlPointRadius(chassis, controlRadius);
-
-        try
-        {
-            var track = Track;
-            var trackSource = "external";
-            if (track == null)
-            {
-                var points = new List<Vector2>(ControlPoints);
-                track = new BezierTrack(points, resolution);
-                trackSource = "controlPoints";
-            }
-            track.CarDirectionBias = CarDirectionBias;
-            track.Speed = baseSpeed;
-
-            var center = new Vector2(x0, y0);
-            var (idealPos, idealAngle, bias, pd) = track.QueryTangentPoint(center);
-            var carDirection = (float)CommonMath.ThDiff(theta, CarDirectionBias);
-            var holdTh = ClampAbs((float)CommonMath.ThDiff(idealAngle, carDirection), gcpLimit);
-            var targetBodyTh = (float)CommonMath.RoundTh(idealAngle + CarDirectionBias);
-
-            DLog.Log(
-                $"START center=({x0:0},{y0:0},{theta:0.0}) source={initialCenterSource} " +
-                $"track={track.GetType().Name} trackSource={trackSource} controls={ControlPoints?.Count ?? 0} " +
-                $"len={track.Length():0} speed={baseSpeed:0.000} bias={CarDirectionBias:0.0} " +
-                $"query=({idealPos.X:0},{idealPos.Y:0}) tangent={idealAngle:0.0} targetBody={targetBodyTh:0.0} " +
-                $"pathBias={bias:0.0} pd={pd:0.0} hold={holdTh:0.0} radius={controlRadius:0}",
-                "FleetCurveDbg");
-
-            var warmStart = DateTime.Now;
-            var warmSeqBaseline = self.BeginFleetMotionWarmup();
-            WriteWarmupAuto(self, idealPos, targetBodyTh, holdTh, holdTh);
-            self.PrimeMasterAutoFromSlam();
-
-            var warmEnd = warmStart.AddSeconds(Math.Max(1.0f, StartSyncTimeoutSec));
-            var warmIter = 0;
-            var warmReady = false;
-            var warmDetail = "";
-            while (!_stopping && DateTime.Now < warmEnd)
-            {
-                warmIter++;
-                WriteWarmupAuto(self, idealPos, targetBodyTh, holdTh, holdTh);
-                self.PrimeMasterAutoFromSlam();
-
-                if (warmIter % 5 == 0)
-                {
-                    var snap = self.GetFleetCenterSnapshot();
-                    int cnt;
-                    lock (self.FleetLock) cnt = self.MultiVehicleFleet.Count;
-                    DLog.Log(
-                        $"WARMUP#{warmIter} snap=({snap.X:0},{snap.Y:0},{snap.Th:0.0}) " +
-                        $"cnt={cnt}/{conf.MultiVehicleFleetNum} detail={warmDetail}",
-                        "FleetCurveDbg");
-                }
-
-                if (self.IsFleetMotionWarmupReady(warmStart, warmSeqBaseline,
-                        conf.TestCarSyncTh, conf.TestCarSyncDistance, out warmDetail))
-                {
-                    warmReady = true;
-                    DLog.Log($"WARMUP done iter={warmIter} detail={warmDetail}", "FleetCurveDbg");
-                    break;
-                }
-                yield return true;
-            }
-
-            if (!warmReady)
-            {
-                DLog.Log($"WARMUP timeout: fleet startup sync failed, abort curve action. detail={warmDetail}",
-                    "FleetCurveDbg");
-                Hedingben.ToastText("FleetCurve startup sync timeout", "FleetCurve");
-                Cleanup();
-                yield break;
-            }
-
-            _controller = new ChassisController { BaseSpeed = baseSpeed }.Get();
-            _controller.MultiVehicleSync = true;
-            _controller.BaseSpeed = baseSpeed;
-            _controller.SlowDistance = Math.Max(FinishDistance + 1f, SlowDistance);
-            _controller.FinishDistance = Math.Max(0f, FinishDistance);
-            _controller.FinishSpeed = speedFinish;
-            _controller.SlowingPow = Math.Max(0.01f, SlowingPow);
-            _controller.GcpThetaThreshold = gcpLimit;
-            _controller.AddTrack(track, "FleetCurve");
-
-            Hedingben.ToastText($"FleetCurve len {track.Length():0}mm speed {baseSpeed:0.00}", "FleetCurve");
-
-            foreach (var running in _controller.Track())
-            {
-                if (_stopping)
-                    break;
-                if (!running)
-                    break;
-                yield return true;
-            }
-
-            if (!_stopping)
-            {
-                self.MultiVehicleAutoVx = 0;
-                self.MultiVehicleAutoCmdTime = DateTime.Now;
-                var settleEnd = DateTime.Now.AddMilliseconds(Math.Max(100, conf.MultiVehicleSyncInterval * 3));
-                while (!_stopping && DateTime.Now < settleEnd)
-                {
-                    self.MultiVehicleAutoEnabled = true;
-                    self.MultiVehicleAutoVx = 0;
-                    self.MultiVehicleAutoCmdTime = DateTime.Now;
-                    yield return true;
-                }
-            }
-
-            DLog.Log($"DONE stopping={_stopping}", "FleetCurveDbg");
-        }
-        finally
-        {
-            Cleanup();
-            _controller = null;
-        }
-    }
-}

ClumsyPilot/MovementTests.TireFollowing.cs

@@ -1,606 +0,0 @@
-using ClumsyCore;
-using ClumsyCore.DTools;
-using ClumsyCore.Interfaces;
-using ClumsyCore.Pilot;
-using ClumsyCore.Utilities;
-using ClumsyDance.ClumsyDance.Detectors;
-using ClumsyDance.ClumsyWalk.Detectors;
-using CommonUsage.Chassis;
-using CommonUsage.Mathematics;
-using FundamentalLib;
-using MDCSToolBox.Clumsy.Calibration;
-using MDCSToolBox.Clumsy.Tracks;
-using MDCSToolBox.Commons.Controllers;
-using System;
-using System.Collections.Generic;
-using System.Drawing;
-using System.Linq;
-using System.Numerics;
-using System.Security.Cryptography;
-using System.Threading;
-using LineSegment = ClumsyCore.Utilities.LineSegment;
-
-namespace MultiWheelC
-{
-    [MovementTest(name = "轮胎检测")]
-    public class TireDetect : MovementTest
-    {
-        public override void TestStop()
-        {
-            _running = false;
-        }
-
-        public override void Test()
-        {
-            _painter = UI.GetPainter("TwoLegDetectTest", false);
-            _painter.Clear();
-
-            var frontlidar = UI.GetInput("1是用前雷达识别，2是用后雷达识别");
-            var result = int.Parse(frontlidar.ToString());
-            var lastDetectX = result == 1 ? PilotDefinition.Conf.TireFollowingStage1GuessX : -PilotDefinition.Conf.TireFollowingStage1GuessX;
-            var lastDetectY = 0f;
-            while (_running)
-            {
-                var ld = Detect(lastDetectX, SetFilters(lastDetectX, lastDetectY), result == 1 ? true : false);
-                if(ld == null)
-                {
-                    //Console.WriteLine("ld == null");
-                    continue;
-                }
-                _painter.Clear();
-                var center = (ld.Src + ld.Dst) / 2;
-                var distanceToCarOrigin = Vector2.Distance(Vector2.Zero, center);
-                var distanceLabelPos = center / 2;
-                _painter.DrawLine(Color.Cyan, Vector2.Zero, center, width: 2);
-                _painter.DrawText(Color.Yellow, $"{distanceToCarOrigin:F3}", distanceLabelPos.X, distanceLabelPos.Y);
-                lastDetectX = center.X;
-                lastDetectY = center.Y;
-                Thread.Sleep(100);
-            }
-        }
-
-        public static LineSegment Detect(float guessX, List<DetectFilter> filters, bool frontlidar)
-        {
-            return new Lidar2dDetect2LegTray()
-            {
-                BlobDist = frontlidar ? PilotDefinition.Conf.TireFrontTwoLegBlobDist : PilotDefinition.Conf.TireBackTwoLegBlobDist,
-                BlobPtCount = frontlidar ? PilotDefinition.Conf.TireTwoLegBlobPtCount : PilotDefinition.Conf.TireTwoLegBlobPtCount,
-                BlobSize = frontlidar ? PilotDefinition.Conf.TireFrontTwoLegBlobSize : PilotDefinition.Conf.TireBackTwoLegBlobSize,
-                CenterChange = Tuple.Create(frontlidar ? PilotDefinition.Conf.TireFrontTwoLegCenterChangeX : PilotDefinition.Conf.TireBackTwoLegCenterChangeX, 0f, 0f),
-                LegWidth = PilotDefinition.Conf.TireTwoLegWidth,
-                LegWidthErr = frontlidar ? PilotDefinition.Conf.TireTwoLegWidthErr : PilotDefinition.Conf.TireTwoLegWidthErr,
-                Padding = frontlidar ? PilotDefinition.Conf.TireFrontPadding : PilotDefinition.Conf.TireBackPadding,
-                PillarFindingScope = frontlidar ? PilotDefinition.Conf.TireFrontTwoLegPillarFindingScope : PilotDefinition.Conf.TireBackTwoLegPillarFindingScope,
-                SgnDir = PilotDefinition.Conf.TwoLegSgnDir,
-            }.DetectWithGuess(frontlidar ? "frontlidar" : "leftlidar,rightlidar", new LineSegment(new Vector2(guessX, 0), Vector2.Zero),
-                guessCoordinateSystem: CoordinateSystem.Car2D, outCoordinateSystem: CoordinateSystem.Car2D, filters);
-        }
-
-        private List<DetectFilter> SetFilters(float guessCenterX, float guessCenterY)
-        {
-            var painter = UI.GetPainter("GeneralFollowing.SetFilters", false);
-            painter.Clear();
-            painter.Clear(3000);
-
-            var box = new Vector2[]
-            {
-                new (guessCenterX - PilotDefinition.Conf.TireFilterLength / 2, guessCenterY - PilotDefinition.Conf.TireFilterWidth / 2),
-                new (guessCenterX + PilotDefinition.Conf.TireFilterLength / 2, guessCenterY - PilotDefinition.Conf.TireFilterWidth / 2),
-                new (guessCenterX + PilotDefinition.Conf.TireFilterLength / 2, guessCenterY + PilotDefinition.Conf.TireFilterWidth / 2),
-                new (guessCenterX - PilotDefinition.Conf.TireFilterLength / 2, guessCenterY + PilotDefinition.Conf.TireFilterWidth / 2),
-            };
-
-            for (var i = 0; i < box.Length; ++i)
-                painter.DrawLine(Color.DarkOliveGreen, box[i], box[(i + 1) % 4]);
-
-            // PC filter in car coordinate frame
-            return new List<DetectFilter>()
-            {
-                new(CoordinateSystem.Car2D,
-                    p => LessMath.IsPointInPolygon4(
-                        box.Select(v => new PointF(v.X, v.Y)).ToArray(), new PointF(p.X, p.Y))),
-            };
-        }
-
-        private Painter _painter;
-        private bool _running = true;
-    }
-
-    [MovementTest(name = "钻车测试")]
-    public class FollowTire : MovementTest
-    {
-        public override void TestStop()
-        {
-            _dt?.Stop();
-        }
-
-        public override void Test()
-        {
-            var front = UI.GetInput("1是用前雷达识别，2是用后雷达识别");
-            var result = int.Parse(front.ToString());
-            var lidarname = result == 1 ? "前雷达" : "后雷达";
-            DLog.Log($"开始钻车测试，用{lidarname}识别", "TireFollowing");
-
-            var following = new TireFollowing()
-            {
-                GetController = () => new ChassisController().Get(),
-                GuessRangeX = PilotDefinition.Conf.TireFilterLength / 2,
-                GuessRangeY = PilotDefinition.Conf.TireFilterWidth / 2,
-                detectors = new List<TireFollowing.DetectorDefinition>()
-                {
-                    new TireFollowing.DetectorDefinition()
-                    {
-                        DetectFunction = (_, lastDetectX, filters) => TireDetect.Detect(lastDetectX, filters, result == 1 ? true : false),
-                        StartGuessingX = result == 1 ? PilotDefinition.Conf.TireFollowingStage1GuessX : -PilotDefinition.Conf.TireFollowingStage1GuessX,
-                        StartGuessingY = 0,
-                        SwitchWalkBlindCondition = rd => rd <= PilotDefinition.Conf.TireFollowingWalkBlindSwitchingDistance,
-                        FinishWalkBlindCondition = rd => rd <= PilotDefinition.Conf.TireFollowingWalkBlindFinishDistance,
-                        PathTransformation = new Tuple<float, float, float>(
-                        result == 1 ? PilotDefinition.Conf.TireFollowingFrontLidarPathTransformationX : PilotDefinition.Conf.TireFollowingBackLidarPathTransformationX,
-                        result == 1 ? PilotDefinition.Conf.TireFollowingFrontLidarPathTransformationY : PilotDefinition.Conf.TireFollowingBackLidarPathTransformationY,
-                        0)
-                    },
-                    new TireFollowing.DetectorDefinition()
-                    {
-                        DetectFunction = (_, lastDetectX, filters) => TireDetect.Detect(lastDetectX, filters, result == 1 ? true : false),
-                        StartGuessingX = result == 1 ? PilotDefinition.Conf.TireFollowingStage2GuessX : -PilotDefinition.Conf.TireFollowingStage2GuessX,
-                        StartGuessingY = 0,
-                        SwitchWalkBlindCondition = rd => rd <= PilotDefinition.Conf.TireFollowingWalkBlindSwitchingDistance,
-                        FinishWalkBlindCondition = rd => rd <= PilotDefinition.Conf.TireFollowingWalkBlindFinishDistance,
-                        PathTransformation = new Tuple<float, float, float>(
-                        result == 1 ? PilotDefinition.Conf.TireFollowingFrontLidarPathTransformationX : PilotDefinition.Conf.TireFollowingBackLidarPathTransformationX,
-                        result == 1 ? PilotDefinition.Conf.TireFollowingFrontLidarPathTransformationY : PilotDefinition.Conf.TireFollowingBackLidarPathTransformationY,
-                        0)
-                    },
-                },
-                CarDirection = result == 1 ? 0f : 180f,
-                SlowDistance = PilotDefinition.Conf.TireFollowingSlowDistance,
-                MaxSpeed = PilotDefinition.Conf.TireFollowingMaxSpeed,
-                TireNum = PilotDefinition.Conf.TireFollowingTireNum,
-                WalkBlindTh = result == 1 ? PilotDefinition.Conf.TireFollowingFrontLidarWalkBlindTh : PilotDefinition.Conf.TireFollowingBackLidarWalkBlindTh,
-            };
-            _dt = new DriveTask(following.Get());
-            _dt.Wait();
-            DLog.Log($"结束钻车测试", "TireFollowing");
-        }
-        private DriveTask _dt;
-    }
-
-    [MovementTest(name = "离车测试")]
-    public class LeaveCar : MovementTest
-    {
-        public override void TestStop()
-        {
-            _dt?.Stop();
-        }
-
-        public override void Test()
-        {
-            DLog.Log($"开始离车测试，用后雷达识别", "TireFollowing");
-            var following = new TireFollowing()
-            {
-                GetController = () => new ChassisController().Get(),
-                GuessRangeX = PilotDefinition.Conf.TireFilterLength / 2,
-                GuessRangeY = PilotDefinition.Conf.TireFilterWidth / 2,
-                detectors = new List<TireFollowing.DetectorDefinition>()
-                {
-                    new TireFollowing.DetectorDefinition()
-                    {
-                        DetectFunction = (_, lastDetectX, filters) => TireDetect.Detect(lastDetectX, filters, false),
-                        StartGuessingX = -PilotDefinition.Conf.TireFollowingStage2GuessX,
-                        StartGuessingY = 0,
-                        SwitchWalkBlindCondition = rd => rd <= PilotDefinition.Conf.TireFollowingLeaveCarWalkBlindSwitchingDistance,
-                        FinishWalkBlindCondition = rd => rd <= PilotDefinition.Conf.TireFollowingWalkBlindFinishDistance,
-                        PathTransformation = new Tuple<float, float, float>(
-                        PilotDefinition.Conf.TireFollowingLeaveCarBackLidarPathTransformationX,
-                        PilotDefinition.Conf.TireFollowingBackLidarPathTransformationY,
-                        0)
-                    },
-                },
-                CarDirection = 180f,
-                SlowDistance = PilotDefinition.Conf.TireFollowingSlowDistance,
-                MaxSpeed = PilotDefinition.Conf.TireFollowingMaxSpeed,
-                WalkBlindTh = 0,
-                TireNum = 1
-            };
-            _dt = new DriveTask(following.Get());
-            _dt.Wait();
-            DLog.Log($"结束离车测试", "TireFollowing");
-        }
-        private DriveTask _dt;
-    }
-
-    [MovementTest(name = "抱夹关闭")]
-    public class ClampTest1 : MovementTest
-    {
-        public override void TestStop()
-        {
-            _dt?.Stop();
-            PilotDefinition.Self.SpeedLeftArm = 0;
-            PilotDefinition.Self.SpeedRightArm = 0;
-        }
-
-        public override void Test()
-        {
-            _dt = new DriveTask(new ClampToTarget()
-            {
-                LeftClampTarget = PilotDefinition.Self.LeftArmUpperPos,
-                RightClampTarget = PilotDefinition.Self.RightArmUpperPos
-            }.Get());
-            _dt.Wait();
-        }
-
-        private DriveTask _dt;
-    }
-
-    [MovementTest(name = "抱夹打开")]
-    public class ClampTest2 : MovementTest
-    {
-        public override void TestStop()
-        {
-            _dt?.Stop();
-            PilotDefinition.Self.SpeedLeftArm = 0;
-            PilotDefinition.Self.SpeedRightArm = 0;
-        }
-
-        public override void Test()
-        {
-            _dt = new DriveTask(new ClampToTarget()
-            {
-                LeftClampTarget = PilotDefinition.Self.LeftArmLowerPos,
-                RightClampTarget = PilotDefinition.Self.RightArmLowerPos
-            }.Get());
-            _dt.Wait();
-        }
-
-        private DriveTask _dt;
-    }
-
-    [MovementTest(name = "测试前进基于轮里程")]
-    public class LineTrackingTest : MovementTest
-    {
-        public override void TestStop()
-        {
-            _dt?.Stop();
-        }
-        public override void Test()
-        {
-            _dt = new DriveTask(new LineTracking()
-            {
-                Target = PilotDefinition.Conf.LineTrackDistance + (PilotDefinition.Self.LFLActualPos + PilotDefinition.Self.LFRActualPos) / 2,
-            }.Get());
-            _dt.Wait();
-        }
-        private DriveTask _dt;
-    }
-
-    [MovementTest(name = "测试后退基于轮里程")]
-    public class ReverseLineTrackingTest : MovementTest
-    {
-        public override void TestStop()
-        {
-            _dt?.Stop();
-        }
-        public override void Test()
-        {
-            _dt = new DriveTask(new LineTracking()
-            {
-                Target = -PilotDefinition.Conf.LineTrackDistance + (PilotDefinition.Self.LFLActualPos + PilotDefinition.Self.LFRActualPos) / 2,
-            }.Get());
-            _dt.Wait();
-        }
-        private DriveTask _dt;
-    }
-
-    [MovementTest(name = "测试终点跟踪动作-前进")]
-    public class DstTrackerForward : MovementTest
-    {
-        public bool UseInteractivePick = true;
-        public float srcX;
-        public float srcY;
-        public float dstX;
-        public float dstY;
-        public float carDirectionBias = 0f;
-        private readonly Painter _painter = UI.GetPainter("DstTrackerTest");
-
-        public override void TestStop()
-        {
-            _dt?.Stop();
-            _painter?.Clear();
-        }
-        public override void Test()
-        {
-            var p1 = UI.GetPoint("point1");
-            var p2 = UI.GetPoint("point2");
-
-            _painter.Clear();
-            _dt = new DriveTask(new DstTracker()
-            {
-                Src = p1,
-                Dst = p2,
-                CarDirectionBias = carDirectionBias,
-            }.Get());
-            _dt.Wait();
-        }
-
-        private DriveTask _dt;
-    }
-
-    [MovementTest(name = "测试终点跟踪动作-后退")]
-    public class DstTrackerhoutui : MovementTest
-    {
-        public bool UseInteractivePick = true;
-        public float srcX;
-        public float srcY;
-        public float dstX;
-        public float dstY;
-        public float carDirectionBias = 180f;
-        private readonly Painter _painter = UI.GetPainter("DstTrackerTest");
-
-        public override void TestStop()
-        {
-            _dt?.Stop();
-            _painter?.Clear();
-        }
-        public override void Test()
-        {
-            var p1 = UI.GetPoint("point1");
-            var p2 = UI.GetPoint("point2");
-
-            _painter.Clear();
-            _dt = new DriveTask(new DstTracker()
-            {
-                Src = p1,
-                Dst = p2,
-                CarDirectionBias = carDirectionBias,
-            }.Get());
-            _dt.Wait();
-        }
-
-        private DriveTask _dt;
-    }
-
-    [MovementTest(name = "测试先直行再终点跟踪")]
-    public class LineTrackThenDstTrackerTest : MovementTest
-    {
-        public float carDirectionBias = 0f;
-        private readonly Painter _painter = UI.GetPainter("LineTrackThenDstTrackerTest");
-        public override void TestStop()
-        {
-            _dt?.Stop();
-            _painter?.Clear();
-        }
-        public override void Test()
-        {
-            var src = UI.GetPoint("请在上位机选择起点(src)");
-            var dst = UI.GetPoint("请在上位机选择终点(dst)");
-            _painter.Clear();
-            _painter.DrawLine(Color.Cyan, src.X, src.Y, dst.X, dst.Y, width: 3);
-            _painter.DrawCircle(Color.LimeGreen, src.X, src.Y, 80f);
-            _painter.DrawCircle(Color.OrangeRed, dst.X, dst.Y, 80f);
-            _painter.DrawText(Color.LimeGreen, "src", src.X + 80f, src.Y + 80f);
-            _painter.DrawText(Color.OrangeRed, "dst", dst.X + 80f, dst.Y + 80f);
-            IEnumerable<bool> TrackThenFollow()
-            {
-                foreach (var running in new LineTracking()
-                {
-                    Target = PilotDefinition.Conf.LineTrackDistance + (PilotDefinition.Self.LFLActualPos + PilotDefinition.Self.LFRActualPos) / 2,
-                    EnableHandover = true,
-                    HandoverDistance = 200f,
-                    HandoverSpeed = 0.3f,
-                }.Get())
-                {
-                    if (!running) break;
-                    yield return true;
-                }
-                foreach (var running in new DstTracker()
-                {
-                    Src = src,
-                    Dst = dst,
-                    CarDirectionBias = carDirectionBias,
-                    InitialSendSpeed = 0.3f
-                }.Get())
-                {
-                    if (!running) break;
-                    yield return true;
-                }
-                yield return false;
-            }
-            _dt = new DriveTask(TrackThenFollow());
-            _dt.Wait();
-        }
-        private DriveTask _dt;
-    }
-
-    [MovementTest(name = "测试先离车再终点跟踪")]
-    public class LeaveCarThenDstTrackerTest : MovementTest
-    {
-        private readonly Painter _painter = UI.GetPainter("LeaveCarThenDstTrackerTest");
-
-        public override void TestStop()
-        {
-            _dt?.Stop();
-            _painter?.Clear();
-        }
-
-        public override void Test()
-        {
-            var src = UI.GetPoint("请在上位机选择离车后起点(src)");
-            var dst = UI.GetPoint("请在上位机选择终点(dst)");
-
-            _painter.Clear();
-            _painter.DrawLine(Color.Cyan, src.X, src.Y, dst.X, dst.Y, width: 3);
-            _painter.DrawCircle(Color.LimeGreen, src.X, src.Y, 80f);
-            _painter.DrawCircle(Color.OrangeRed, dst.X, dst.Y, 80f);
-            _painter.DrawText(Color.LimeGreen, "src", src.X + 80f, src.Y + 80f);
-            _painter.DrawText(Color.OrangeRed, "dst", dst.X + 80f, dst.Y + 80f);
-
-            IEnumerable<bool> LeaveThenFollow()
-            {
-                var following = new TireFollowing()
-                {
-                    GetController = () => new ChassisController().Get(),
-                    GuessRangeX = PilotDefinition.Conf.TireFilterLength / 2,
-                    GuessRangeY = PilotDefinition.Conf.TireFilterWidth / 2,
-                    detectors = new List<TireFollowing.DetectorDefinition>()
-                    {
-                        new TireFollowing.DetectorDefinition()
-                        {
-                            DetectFunction = (_, lastDetectX, filters) => TireDetect.Detect(lastDetectX, filters, false),
-                            StartGuessingX = -PilotDefinition.Conf.TireFollowingStage2GuessX,
-                            StartGuessingY = 0,
-                            SwitchWalkBlindCondition = rd => rd <= PilotDefinition.Conf.TireFollowingWalkBlindSwitchingDistance,
-                            FinishWalkBlindCondition = rd => rd <= PilotDefinition.Conf.TireFollowingWalkBlindFinishDistance,
-                            PathTransformation = new Tuple<float, float, float>(
-                                PilotDefinition.Conf.TireFollowingLeaveCarBackLidarPathTransformationX,
-                                PilotDefinition.Conf.TireFollowingBackLidarPathTransformationY,
-                                0),
-                        },
-                    },
-                    CarDirection = 180f,
-                    SlowDistance = PilotDefinition.Conf.TireFollowingSlowDistance,
-                    MaxSpeed = PilotDefinition.Conf.TireFollowingMaxSpeed,
-                    WalkBlindTh = 0,
-                    TireNum = 1
-                };
-
-                foreach (var running in following.Get())
-                {
-                    if (!running) break;
-                    yield return true;
-                }
-
-                foreach (var running in new DstTracker()
-                {
-                    Src = src,
-                    Dst = dst,
-                    CarDirectionBias = 180f,
-                }.Get())
-                {
-                    if (!running) break;
-                    yield return true;
-                }
-
-                yield return false;
-            }
-
-            _dt = new DriveTask(LeaveThenFollow());
-            _dt.Wait();
-        }
-
-        private DriveTask _dt;
-    }
-
-
-    [MovementTest(name = "驱动器下使能测试")]
-    public class DriverDisableTest : MovementTest
-    {
-        public override void TestStop()
-        {
-            throw new NotImplementedException();
-        }
-
-        public override void Test()
-        {
-            new DriveTask(new DriverDisable(){ }.Get()).Wait();
-        }
-    }
-
-    [MovementTest(name = "驱动器复位测试")]
-    public class DriverAbleTest : MovementTest
-    {
-        public override void TestStop()
-        {
-            throw new NotImplementedException();
-        }
-
-        public override void Test()
-        {
-            new DriveTask(new DriverAble(){ }.Get()).Wait();
-        }
-    }
-
-    [MovementTest(name = "底盘旋转测试")]
-    public class RotateToAngleTest : MovementTest
-    {
-        public override void TestStop()
-        {
-            throw new NotImplementedException();
-        }
-
-        public override void Test()
-        {
-            var target = UI.GetInput("输入旋转角度：");
-            var chassis = (MultiWheelChassis)PilotDefinition.Chassis;
-            new DriveTask(new MultiWheelRotateInPlace()
-            {
-                AngleTarget = float.Parse(target),
-                PidparamsRead = () => new PIDParams()
-                {
-                    Kp = PilotDefinition.Conf.TireFollowingThkp,
-                    Ki = PilotDefinition.Conf.TireFollowingThki,
-                    Kd = PilotDefinition.Conf.TireFollowingThkd,
-                    DeadZone = PilotDefinition.Conf.TireFollowingThDeadZone,
-                    SpeedAccPerSec = PilotDefinition.Conf.TireFollowingThSpeedAccPerSec,
-                    OutputUpperThreshold = PilotDefinition.Conf.TireFollowingThThresh,
-                    MaxI = PilotDefinition.Conf.TireFollowingThMaxI,
-                }
-            }.Get()).Wait();
-        }
-    }
-
-    public class utils
-    {
-        public static List<(float x, float y, float th)> RemoveOutliers(List<(float x, float y, float th)> data, float threshold = 2.0f)
-        {
-            var means = CalculateMean(data);
-            var stdDevs = CalculateStandardDeviation(data, means);
-
-            return data.Where(point =>
-                Math.Abs(point.x - means.x) <= threshold * stdDevs.x &&
-                Math.Abs(point.y - means.y) <= threshold * stdDevs.y &&
-                AngularDistance(point.th, means.th) <= threshold * stdDevs.th
-            ).ToList();
-        }
-
-        public static (float x, float y, float th) CalculateMean(List<(float x, float y, float th)> data)
-        {
-            float meanX = data.Average(point => point.x);
-            float meanY = data.Average(point => point.y);
-            float sinSum = data.Sum(point => (float)Math.Sin(DegreeToRadian(point.th)));
-            float cosSum = data.Sum(point => (float)Math.Cos(DegreeToRadian(point.th)));
-            float meanTh = RadianToDegree((float)Math.Atan2(sinSum, cosSum));
-
-            return (meanX, meanY, meanTh);
-        }
-
-        public static (float x, float y, float th) CalculateStandardDeviation(List<(float x, float y, float th)> data, (float x, float y, float th) means)
-        {
-            float varianceX = data.Average(point => (point.x - means.x) * (point.x - means.x));
-            float varianceY = data.Average(point => (point.y - means.y) * (point.y - means.y));
-
-            // 计算角度的方差
-            float varianceTh = data.Average(point => AngularDistance(point.th, means.th) * AngularDistance(point.th, means.th));
-
-            return ((float)Math.Sqrt(varianceX), (float)Math.Sqrt(varianceY), (float)Math.Sqrt(varianceTh));
-        }
-
-        public static float DegreeToRadian(float degree)
-        {
-            return (float)(degree * Math.PI / 180.0);
-        }
-
-        public static float RadianToDegree(float radian)
-        {
-            return (float)(radian * 180.0 / Math.PI);
-        }
-
-        public static float AngularDistance(float angle1, float angle2)
-        {
-            return CommonMath.ThDiff(angle1, angle2);
-        }
-    }
-
-}
-
-

ClumsyPilot/MovementTests.TwoLegDetect.cs

@@ -1,136 +0,0 @@
-using System;
-using System.Collections.Generic;
-using System.Drawing;
-using System.Linq;
-using System.Numerics;
-using System.Threading;
-using ClumsyCore;
-using ClumsyCore.DTools;
-using ClumsyCore.Pilot;
-using ClumsyCore.Utilities;
-using ClumsyDance.ClumsyDance.Detectors;
-using ClumsyDance.ClumsyWalk.Detectors;
-using FundamentalLib;
-using LineSegment = ClumsyCore.Utilities.LineSegment;
-
-namespace MultiWheelC;
-
-/// <summary>
-/// 2腿检测：用单线激光雷达识别两腿托盘/轮胎，按上一帧结果作为下一帧猜测做闭环检测。
-/// 从 StandardMultiWheelLifter 移植；参数全部走 PilotConfig（Fields 面板），雷达选择改为配置项而非阻塞输入。
-/// </summary>
-[MovementTest(name = "多舵轮-2腿检测")]
-public class TwoLegDetect : MovementTest
-{
-    private Painter _painter;
-    private bool _running = true;
-
-    public override void TestStop() => _running = false;
-
-    public override void Test()
-    {
-        _running = true;
-        _painter = UI.GetPainter("MultiWheelTwoLegDetect", false);
-        _painter.Clear();
-
-        var lidar = PilotDefinition.Conf.TwoLegLidarName;
-        var lastDetectX = PilotDefinition.Conf.TwoLegGuessX;
-        var lastDetectY = 0f;
-
-        while (_running)
-        {
-            var ld = Detect(lidar, lastDetectX, SetFilters(lastDetectX, lastDetectY));
-            if (ld == null)
-            {
-                Thread.Sleep(100);
-                continue;
-            }
-
-            var center = (ld.Src + ld.Dst) / 2;
-            var distanceToCarOrigin = Vector2.Distance(Vector2.Zero, center);
-
-            _painter.Clear();
-            _painter.DrawLine(Color.Cyan, Vector2.Zero, center, width: 2);
-            _painter.DrawText(Color.Yellow, $"{distanceToCarOrigin:F1}", center.X / 2, center.Y / 2);
-
-            Hedingben.ToastText(
-                $"[Test检测] lidar:{lidar} guess x:{lastDetectX:F0} y:{lastDetectY:F0} | " +
-                $"中心 x:{center.X:F0} y:{center.Y:F0} dist:{distanceToCarOrigin:F0}",
-                "MultiWheelTwoLegDetect-test");
-
-            // 用本帧中心作为下一帧猜测，实现闭环跟踪
-            lastDetectX = center.X;
-            lastDetectY = center.Y;
-            Thread.Sleep(100);
-        }
-
-        _painter.Clear();
-    }
-
-    /// <summary>在车体坐标系下，按猜测位置检测两腿，返回连接两腿的线段（车体系）。</summary>
-    public static LineSegment Detect(string lidarName, float guessX, List<DetectFilter> filters)
-    {
-        var conf = PilotDefinition.Conf;
-#pragma warning disable CS0612, CS0618
-        var detector = new Lidar2dDetect2LegTray
-        {
-            BlobDist = conf.TwoLegBlobDist,
-            BlobPtCount = conf.TwoLegBlobPtCount,
-            BlobSize = conf.TwoLegBlobSize,
-            CenterChange = Tuple.Create(conf.TwoLegCenterChangeX, 0f, 0f),
-            LegWidth = conf.TwoLegWidth,
-            LegWidthErr = conf.TwoLegWidthErr,
-            Padding = conf.TwoLegPadding,
-            PillarFindingScope = conf.TwoLegPillarFindingScope,
-            SgnDir = conf.TwoLegSgnDir,
-        };
-#pragma warning restore CS0612, CS0618
-
-        var result = detector.DetectWithGuess(
-            lidarName,
-            new LineSegment(new Vector2(guessX, 0), Vector2.Zero),
-            guessCoordinateSystem: CoordinateSystem.Car2D,
-            outCoordinateSystem: CoordinateSystem.Car2D,
-            filters);
-        return ApplyOutputBias(result);
-    }
-
-    private static LineSegment ApplyOutputBias(LineSegment result)
-    {
-        if (result == null) return null;
-
-        var conf = PilotDefinition.Conf;
-        if (Math.Abs(conf.TwoLegOutputBiasX) < 1e-6f && Math.Abs(conf.TwoLegOutputBiasY) < 1e-6f)
-            return result;
-
-        var bias = new Vector2(conf.TwoLegOutputBiasX, conf.TwoLegOutputBiasY);
-        return new LineSegment(result.Src + bias, result.Dst + bias);
-    }
-
-    /// <summary>在猜测中心周围构造一个矩形 ROI，过滤掉框外点云，降低误识别。</summary>
-    public static List<DetectFilter> SetFilters(float guessCenterX, float guessCenterY)
-    {
-        var conf = PilotDefinition.Conf;
-        var painter = UI.GetPainter("MultiWheelTwoLegDetect.Filter", false);
-        painter.Clear();
-
-        var box = new[]
-        {
-            new Vector2(guessCenterX - conf.TwoLegFilterLength / 2, guessCenterY - conf.TwoLegFilterWidth / 2),
-            new Vector2(guessCenterX + conf.TwoLegFilterLength / 2, guessCenterY - conf.TwoLegFilterWidth / 2),
-            new Vector2(guessCenterX + conf.TwoLegFilterLength / 2, guessCenterY + conf.TwoLegFilterWidth / 2),
-            new Vector2(guessCenterX - conf.TwoLegFilterLength / 2, guessCenterY + conf.TwoLegFilterWidth / 2),
-        };
-
-        for (var i = 0; i < box.Length; ++i)
-            painter.DrawLine(Color.DarkOliveGreen, box[i], box[(i + 1) % 4]);
-
-        // 点云滤波在车体坐标系下进行
-        return new List<DetectFilter>
-        {
-            new(CoordinateSystem.Car2D,
-                p => LessMath.IsPointInPolygon4(
-                    box.Select(v => new PointF(v.X, v.Y)).ToArray(), new PointF(p.X, p.Y))),
-        };
-    }
-}

ClumsyPilot/Movements.cs

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

ClumsyPilot/PilotConfig.cs

@@ -6,108 +6,19 @@ namespace MultiWheelC;
 
 public class PilotConfig : MultiWheelPilotConfig
 {
-    [FieldMember(desc = "[sync] steering angle acceleration(deg/s^2)")] public float SyncThAccPerSec = 30f;
+    #region 单车-轨迹跟踪
-    [FieldMember(desc = "[sync] fleet member distance(mm)")] public float TestCarSyncDistance = 2400f;
-    [FieldMember(desc = "[sync] fleet layout bias angle(deg)")] public float TestCarSyncTh = 0f;
-    // Fleet manual remote IO values are normalized joystick ratios. Keep all speed/angle scaling here.
-    [FieldMember(desc = "[sync] fleet manual max linear speed(m/s)")] public float FleetManualMaxSpeed = 0.3f;
-    [FieldMember(desc = "[sync] fleet manual normal-mode full-stick steering angle(deg)")] public float FleetManualMaxSteerAngleDeg = 45f;
-    [FieldMember(desc = "[sync] fleet manual crab-mode full-stick steering angle(deg)")] public float FleetManualMaxCrabAngleDeg = 60f;
-    [FieldMember(desc = "[sync] fleet manual rotate-mode full-stick angular speed(deg/s)")] public float FleetManualMaxRotateOmegaDegPerSec = 45f;
-    [FieldMember(desc = "[sync] 蟹行舵角上限(deg，应与Medulla舵轮角度限制匹配，默认120)")] public float MultiVehicleCrabSteerLimitDeg = 120f;
-    [FieldMember(desc = "[sync] 检测中心偏移(mm)")] public float DeltaDetectCenter = 350f;
-    // 仅控制"车队内姿态纠正"(POS 补偿)是否使用 Detour 的 SLAM 位姿，不影响"整个车队姿态的计算"。
-    // 默认 false：定位不参与车队内姿态纠正（各车按编队几何/互识别保持队形，不做 SLAM 逐车纠偏）。
-    // 为 true：额外用 getCartLocation() 反推每台车相对编队中心的偏差并做 POS 补偿。
-    // 注意：无论该开关如何，自动模式下整队姿态(反推/广播车队中心、SLAM 间距、自动安全门)始终依赖 Detour 全局定位；
-    // 主车自动模式必调用 getCartLocation()，若无有效全局定位该调用会阻塞 → 联动线程阻塞不下发速度（安全停车）。
-    [FieldMember(desc = "[sync] 定位是否参与车队内姿态纠正(不影响整队姿态计算)")] public bool MultiVehicleSyncUseDetour = false;
-    // 手动外部遥控联动默认只走 2 腿检测/几何同步，避免 Detour getCartLocation 阻塞导致遥控和检测可视化变慢。
-    [FieldMember(desc = "[sync] 手动联动是否启用定位姿态纠正(默认关闭)")] public bool MultiVehicleManualUseDetourCorrection = false;
 
-    [FieldMember(desc = "多车联动：总车数")] public int MultiVehicleFleetNum = 2;
+    [FieldMember(desc = "直线行走距离")] public float LineTrackDistance = 1000f;
-    [FieldMember(desc = "联动线程周期(ms)")] public int MultiVehicleSyncInterval = 50;
+    [FieldMember(desc = "直线行走最大速度")] public float LineTrackMaxSpeed = 0.3f;
-    [FieldMember(desc = "多车联动：主车端点 ip:port，/ 表示本车为主车")] public string MultiVehicleMasterEndpoint = "/";
+    [FieldMember(desc = "直线行走Kp")] public float LineTrackKp = 0.2f;
-    [FieldMember(desc = "多车联动：本车同步 IP")] public string SimpleIp = "127.0.0.1";
+    [FieldMember(desc = "直线行走Ki")] public float LineTrackKi = 0f;
+    [FieldMember(desc = "直线行走Kd")] public float LineTrackKd = 0f;
+    [FieldMember(desc = "直线行走DeadZone")] public float LineTrackDeadZone = 50f;
 
-    [FieldMember(desc = "多车联动：本车回连端点 ip:port，供主车 notify 回连，空=127.0.0.1:本车port")] public string MultiVehicleSelfEndpoint = "";
+    [FieldMember(desc = "终点跟踪:速度")] public float DstTrackerMaxSpeed = 0.3f;
-
+    #endregion
-    [JsonProperty("MultiVehicleMasterIp")]
-    private string LegacyMasterIpSetter
-    {
-        set
-        {
-            if (string.IsNullOrEmpty(value) || value == "/") return;
-            if (MultiVehicleMasterEndpoint == "/")
-                MultiVehicleMasterEndpoint = value.Contains(":") ? value : $"{value}:8008";
-        }
-    }
-
-    [FieldMember(desc = "多车联动：启用互识别纠正")] public bool MultiVehicleUseDetect = false;
-
-    // B: 自动速度命令新鲜度(ms)。主车超过此时长未从路径控制器收到新速度命令(路径结束/早退/卡顿)，
-    //    即视为失效并清零下发速度，避免车队按末速度滑行。0 表示自动取 max(200, interval*4)。
-    [FieldMember(desc = "多车联动：自动速度命令超时(ms，0=auto)")] public int MultiVehicleAutoCmdTimeoutMs = 0;
-    // C: fleet 成员存活 TTL(ms)。主车剔除超过此时长未 register/刷新的从车；编队就绪要求所有成员新鲜。
-    //    0 表示自动取 max(500, interval*6)。
-    [FieldMember(desc = "多车联动：成员存活TTL(ms，0=auto)")] public int MultiVehicleMemberTtlMs = 0;
-    // D: 自动模式下用主车路径控制器的理想车队中心(idealPos/idealAngle)作为各车 layout 目标，
-    //    弧线路径上做 per-car 前馈而非仅共用 frontTh/rearTh 事后纠偏。
-    [FieldMember(desc = "多车联动：自动模式按理想中心前馈(弧线)")] public bool MultiVehicleAutoUseIdealCenter = true;
-    // H: 自动模式必须有有效车队中心(SLAM 可反推)，全程定位丢失时停车，避免纯 SLAM 下盲跑。
-    [FieldMember(desc = "多车联动：自动模式要求有效车队中心")] public bool MultiVehicleAutoRequireFleetCenter = true;
-    [FieldMember(desc = "多车联动：SLAM X补偿系数")] public float MultiVehiclePosBiasXFac = 0.5f;
-    [FieldMember(desc = "多车联动：SLAM Y补偿系数")] public float MultiVehiclePosBiasYFac = 0.5f;
-    [FieldMember(desc = "多车联动：SLAM Th补偿系数")] public float MultiVehiclePosBiasThFac = 0.5f;
-    [FieldMember(desc = "多车联动：X补偿阈值(mm)")] public float MultiVehiclePosBiasXThreshold = 50f;
-    [FieldMember(desc = "多车联动：Y补偿阈值(mm)")] public float MultiVehiclePosBiasYThreshold = 50f;
-    [FieldMember(desc = "多车联动：Th补偿阈值(deg)")] public float MultiVehiclePosBiasThThreshold = 5f;
-
-    [FieldMember(desc = "多车联动：互识别 X补偿系数")] public float MultiVehicleDetectBiasXFac = 0.5f;
-    [FieldMember(desc = "多车联动：互识别 Y补偿系数")] public float MultiVehicleDetectBiasYFac = 0.5f;
-    [FieldMember(desc = "多车联动：互识别 Th补偿系数")] public float MultiVehicleDetectBiasThFac = 0.5f;
-    [FieldMember(desc = "多车联动：互识别 X补偿阈值(mm)")] public float MultiVehicleDetectBiasXThreshold = 50f;
-    [FieldMember(desc = "多车联动：互识别 Y补偿阈值(mm)")] public float MultiVehicleDetectBiasYThreshold = 50f;
-    [FieldMember(desc = "多车联动：互识别 Th补偿阈值(deg)")] public float MultiVehicleDetectBiasThThreshold = 5f;
-
-    // 原地旋转(mode2)闭环纠偏(PI)：把"本车应移动到的位置(dx,dy,mm)/应转角(dth,deg)"作为误差，
-    // 用 PI 控制器换算成车体系修正速度叠加到绕队心旋转上。纯 P 对抗恒定横向滑移扰动有稳态残差，
-    // 加积分项把稳态误差拉到 0；积分带限幅(抗 windup)，总输出限幅在 Max 内防过冲/振荡。
-    // Fac=比例增益(mm/s per mm、deg/s per deg)；IFac=积分增益(mm/s per mm·s、deg/s per deg·s)；Max=总输出上限。
-    [FieldMember(desc = "原地旋转纠偏：平移比例增益P(mm/s per mm)")] public float MultiVehicleRotateCompXyFac = 1.2f;
-    [FieldMember(desc = "原地旋转纠偏：平移积分增益I(mm/s per mm·s)")] public float MultiVehicleRotateCompXyIFac = 0.8f;
-    [FieldMember(desc = "原地旋转纠偏：平移速度上限(mm/s)")] public float MultiVehicleRotateCompXyMax = 150f;
-    [FieldMember(desc = "原地旋转纠偏：转向比例增益P(deg/s per deg)")] public float MultiVehicleRotateCompThFac = 0.8f;
-    [FieldMember(desc = "原地旋转纠偏：转向积分增益I(deg/s per deg·s)")] public float MultiVehicleRotateCompThIFac = 0.8f;
-    [FieldMember(desc = "原地旋转纠偏：转向速度上限(deg/s)")] public float MultiVehicleRotateCompThMax = 15f;
-    // 仅当车队实际被指令旋转(|fleetOmega|超过此阈值)时才运行纠偏 PI；否则清零并复位积分，
-    // 避免松开摇杆后积分残留持续驱动车辆"自行旋转停不下来"。
-    [FieldMember(desc = "原地旋转纠偏：生效的最小角速度阈值(deg/s)")] public float MultiVehicleRotateActiveOmega = 0.5f;
-    // 安全网：每轮纠偏速度幅值 <= 该比例 * 本轮旋转切向速度，限制合速度相对纯切向的最大偏角。
-    // 旧配置若仍为 <0，运行时按安全默认 0.10 处理；确需放宽时可在主车显式调大并同步给从车。
-    [FieldMember(desc = "原地旋转纠偏：纠偏/旋转切向比例硬上限，<0使用安全默认0.10")] public float MultiVehicleRotateCompTangentFrac = 0.10f;
-
-    [FieldMember(desc = "单车同步 xy 精度(mm)")] public float SingleCarSyncPrecisionXy = 10f;
-    [FieldMember(desc = "单车同步 th 精度(deg)")] public float SingleCarSyncPrecisionTh = 0.2f;
-
-    [FieldMember(desc = "Playground WebAPI 基地址")]
-    public string PlaygroundWebApiUrl = "http://localhost:18090";
-
-    [FieldMember(desc = "MultiVehicle rotate pose WebAPI diagnostics (simulation only)")]
-    public bool MultiVehicleRotatePoseWebApiDiagEnabled = false;
-
-    [FieldMember(desc = "Playground 小车名称（场景 robots[].name）")]
-    public string PlaygroundRobotName = "agv_multi_1";
-
-    [FieldMember(desc = "Playground 邻车名称（仅主车用于原地旋转位姿诊断）")]
-    public string PlaygroundNeighborRobotName = "agv_multi_2";
-
-    [FieldMember(desc = "WebAPI 平移测试：平移距离(mm)")]
-    public float WebApiTranslateMm = 100f;
-
-    [FieldMember(desc = "WebAPI 旋转测试：旋转角度(deg)")]
-    public float WebApiRotateDeg = 5f;
 
+    #region 单车-原地旋转
     [FieldMember(desc = "原地旋转：目标朝向(世界坐标系, deg)")]
     public float InPlaceRotateTargetWorldDeg = 90f;
 
@@ -123,102 +34,35 @@ public class PilotConfig : MultiWheelPilotConfig
     [FieldMember(desc = "原地旋转：旋转过程中舵轮偏差重对齐阈值(deg)")]
     public float InPlaceRotateActiveWheelAlignDeg = 10f;
 
-    // ===== 车队联动-原地旋转动作（FleetRotateInPlace / 对应 FleetRemote 原地旋转模式）=====
+    #endregion
-    // 通过 Clumsy 内部脚本字段驱动 TickMultiVehicle 的 mode2 旋转（绕车队中心 + PI 纠偏），需主车运行。
-    [FieldMember(desc = "车队原地旋转：角速度大小(deg/s，方向由目标角符号决定)")]
-    public float FleetRotateOmega = 15f;
 
-    [FieldMember(desc = "车队原地旋转：目标相对转角(deg，+逆时针)")]
+    #region 单车-临时
-    public float FleetRotateTargetDeltaDeg = 90f;
+    [FieldMember(desc = "原地旋转Kp")]
+    public float InPlaceRotateKp = 0.2f;
 
-    [FieldMember(desc = "车队原地旋转：到位角度精度(deg)")]
+    [FieldMember(desc = "原地旋转Ki")]
-    public float FleetRotateArriveDeg = 1.5f;
+    public float InPlaceRotateKi = 0.01f;
 
-    [FieldMember(desc = "车队原地旋转：减速区宽度(deg)，抑制收尾惯性超调")]
+    [FieldMember(desc = "原地旋转Kd")]
-    public float FleetRotateSlowDeg = 25f;
+    public float InPlaceRotateKd = 0f;
 
-    [FieldMember(desc = "车队原地旋转：减速区末段最小角速度(deg/s)")]
+    [FieldMember(desc = "原地旋转积分限幅")]
-    public float FleetRotateMinOmega = 3f;
+    public float InPlaceRotateMaxI = 0.01f;
 
-    [FieldMember(desc = "车队原地旋转：起步缓启动角加速度(deg/s²，<=0关闭)")]
+    [FieldMember(desc = "原地旋转最大角速度(deg/s)")]
-    public float FleetRotateAccel = 20f;
+    public float InPlaceRotateMaxSpeed = 30f;
 
-    [FieldMember(desc = "车队原地旋转：到位后安定时长(s)")]
+    [FieldMember(desc = "原地旋转角加速度(deg/s²)")]
-    public float FleetRotateSettleSec = 0.5f;
+    public float InPlaceRotateAcc = 30f;
 
-    // 与 MultiVehicleSyncUseDetour 解耦：转到指定角度需航向反馈，默认 true 读主车 SLAM 航向闭环判停。
+    [FieldMember(desc = "原地旋转超时(s)")]
-    // false 时退化为按估算时长开环停止（实际转速≠指令时不精确，易出现"没转到目标就停"）。
+    public float InPlaceRotateTimeoutSec = 15f;
-    [FieldMember(desc = "车队原地旋转：用Detour主车航向闭环判停(默认true，false=按时长开环)")]
+    #endregion
-    public bool FleetRotateUseDetourHeading = true;
 
-    // ===== 车队联动-自动蟹行（FleetCrabWalk）=====
-    // 以当前车队中心为起点，构造一条直线路径；MovementTest 中车身保持启动朝向追踪该路径。
-    // 动作侧参考几何控制器的路径跟踪思路，直接写入 MultiVehicleAuto... 字段，不再复用脚本手动链路。
-    [FieldMember(desc = "车队蟹行：路径方向相对启动时车队朝向夹角(deg，逆时针为正；路径在车右侧x度时填-x)")]
-    public float FleetCrabAngleDeg = 45f;
 
-    [FieldMember(desc = "车队蟹行：AGV入口使用的车队世界系目标朝向(deg)")]
-    public float FleetCrabBodyWorldHeadingDeg = 0f;
 
-    [FieldMember(desc = "车队蟹行：路径长度(mm)")]
-    public float FleetCrabLengthMm = 2000f;
 
-    [FieldMember(desc = "车队蟹行：行驶速度(m/s)")]
+    #region 单车-钻车与夹抱
-    public float FleetCrabSpeed = 0.2f;
-
-    [FieldMember(desc = "车队蟹行：速度命令加速度限制(m/s^2，<=0表示不限制)")]
-    public float FleetCrabAccel = 0.2f;
-
-    [FieldMember(desc = "车队蟹行：预对齐后正式下发速度前5秒加速度(m/s^2，<=0表示不限制)")]
-    public float FleetCrabStartAccel = 0.01f;
-
-    [FieldMember(desc = "车队蟹行：末端开始减速距离(mm)")]
-    public float FleetCrabSlowDistance = 2000f;
-
-    [FieldMember(desc = "车队蟹行：完成距离(mm)，低于该剩余距离结束动作")]
-    public float FleetCrabFinishDistance = 20f;
-
-    [FieldMember(desc = "车队蟹行：末端最低速度(m/s)")]
-    public float FleetCrabFinishSpeed = 0.02f;
-
-    [FieldMember(desc = "车队蟹行：末端减速曲线指数")]
-    public float FleetCrabSlowingPow = 0.8f;
-
-    [FieldMember(desc = "车队蟹行：GCP舵角修正上限(deg)")]
-    public float FleetCrabGcpThetaThreshold = 95f;
-
-    [FieldMember(desc = "车队蟹行：headingErr角度纠偏比例系数")]
-    public float FleetCrabDthLinearFac = 1f;
-
-    [FieldMember(desc = "车队蟹行：headingErr角度纠偏舵角限幅(deg)")]
-    public float FleetCrabDthLinearThreshold = 10f;
-
-    [FieldMember(desc = "FleetCrab startup sync timeout(s)")]
-    public float FleetCrabStartSyncTimeoutSec = 8f;
-
-    [FieldMember(desc = "FleetCrab startup wheel alignment tolerance(deg)")]
-    public float FleetCrabStartWheelAlignDeg = 2f;
-
-    // ===== Fleet linked Bezier curve walk =====
-    [FieldMember(desc = "FleetCurve MovementTest Bezier control point count")]
-    public int FleetCurveTestControlPointCount = 4;
-
-    [FieldMember(desc = "FleetCurve speed(m/s)")]
-    public float FleetCurveSpeed = 0.2f;
-
-    [FieldMember(desc = "FleetCurve slow distance(mm)")]
-    public float FleetCurveSlowDistance = 2000f;
-
-    [FieldMember(desc = "FleetCurve finish distance(mm)")]
-    public float FleetCurveFinishDistance = 20f;
-
-    [FieldMember(desc = "FleetCurve finish speed(m/s)")]
-    public float FleetCurveFinishSpeed = 0.02f;
-
-    [FieldMember(desc = "FleetCurve slowing curve exponent")]
-    public float FleetCurveSlowingPow = 0.8f;
-
-    // ===== 2腿检测（单线雷达识别两腿托盘 / 轮胎）=====
     [FieldMember(desc = "2腿检测：雷达名（逗号分隔可多个）")]
     public string TwoLegLidarName = "rear_left_lidar_1,rear_right_lidar_1";
 
@@ -264,7 +108,6 @@ public class PilotConfig : MultiWheelPilotConfig
     [FieldMember(desc = "2腿检测：ROI滤波框宽(mm)")]
     public float TwoLegFilterWidth = 600f;
 
-    #region
     [FieldMember(desc = "轮胎识别:识别框长")] public float TireFilterLength = 1800f;
     [FieldMember(desc = "轮胎识别:识别框宽")] public float TireFilterWidth = 600f;
     [FieldMember(desc = "轮胎识别:轮胎间距")] public float TireTwoLegWidth = 800f;
@@ -284,22 +127,6 @@ public class PilotConfig : MultiWheelPilotConfig
     [FieldMember(desc = "轮胎识别:后雷达参数")] public int TireBackTwoLegSgnDir = 1;
     [FieldMember(desc = "轮胎识别:后雷达参数")] public float TireBackTwoLegCenterChangeX = 0;
 
-    [FieldMember(desc = "抱夹控制pid:Kp")] public float ClampControlKp = 0.1f;
-    [FieldMember(desc = "抱夹控制pid:Ki")] public float ClampControlKi = 0f;
-    [FieldMember(desc = "抱夹控制pid:Kd")] public float ClampControlKd = 0f;
-    [FieldMember(desc = "抱夹控制pid:MaxI")] public float ClampControlMaxI = 0f;
-    [FieldMember(desc = "抱夹控制pid:Acc")] public float ClampControlSpeedAcc = 1f;
-    [FieldMember(desc = "抱夹控制pid:Thresh")] public float ClampControlThresh = 0.2f;
-    [FieldMember(desc = "抱夹控制pid:DeadZone")] public float ClampControlDeadZone = 5f;
-    [FieldMember(desc = "抱夹最大速度")] public float MaxClampSpeed = 1.5f;
-
-    [FieldMember(desc = "直线行走距离")] public float LineTrackDistance = 1000f;
-    [FieldMember(desc = "直线行走最大速度")] public float LineTrackMaxSpeed = 0.3f;
-    [FieldMember(desc = "直线行走Kp")] public float LineTrackKp = 0.2f;
-    [FieldMember(desc = "直线行走Ki")] public float LineTrackKi = 0f;
-    [FieldMember(desc = "直线行走Kd")] public float LineTrackKd = 0f;
-    [FieldMember(desc = "直线行走DeadZone")] public float LineTrackDeadZone = 50f;
-
     [FieldMember(desc = "轮胎跟踪:切换至盲走距离")] public float TireFollowingWalkBlindSwitchingDistance = 1200f;
     [FieldMember(desc = "轮胎跟踪:识别第一对轮胎的初始距离")] public float TireFollowingStage1GuessX = 2000f;
     [FieldMember(desc = "轮胎跟踪:识别第二对轮胎的初始距离")] public float TireFollowingStage2GuessX = 2475f;
@@ -320,9 +147,7 @@ public class PilotConfig : MultiWheelPilotConfig
     [FieldMember(desc = "轮胎跟踪:距离过近角度忽略阈值")] public float TireFollowingAngleIgnoreThr = 0.2f;
     [FieldMember(desc = "轮胎跟踪:Y最大平均数")] public int TireFollowingYAverageFrameCount = 5;
 
-    [FieldMember(desc = "终点跟踪:速度")] public float DstTrackerMaxSpeed = 0.3f;
     [FieldMember(desc = "轮胎跟踪:释放锁点距离")] public float TireFollowingReleaseDistance = 1600;
-    #endregion
 
     [FieldMember(desc = "轮胎跟踪:角度调整kp")] public float TireFollowingThkp = 0.05f;
     [FieldMember(desc = "轮胎跟踪:角度调整ki")] public float TireFollowingThki = 0.01f;
@@ -331,4 +156,178 @@ public class PilotConfig : MultiWheelPilotConfig
     [FieldMember(desc = "轮胎跟踪:角度调整Thresh")] public float TireFollowingThThresh = 0.1f;
     [FieldMember(desc = "轮胎跟踪:角度调整DeadZone")] public float TireFollowingThDeadZone = 5f;
     [FieldMember(desc = "轮胎跟踪:角度调整MaxI")] public float TireFollowingThMaxI = 0.01f;
+
+    [FieldMember(desc = "抱夹控制pid:Kp")] public float ClampControlKp = 0.1f;
+    [FieldMember(desc = "抱夹控制pid:Ki")] public float ClampControlKi = 0f;
+    [FieldMember(desc = "抱夹控制pid:Kd")] public float ClampControlKd = 0f;
+    [FieldMember(desc = "抱夹控制pid:MaxI")] public float ClampControlMaxI = 0f;
+    [FieldMember(desc = "抱夹控制pid:Acc")] public float ClampControlSpeedAcc = 1f;
+    [FieldMember(desc = "抱夹控制pid:Thresh")] public float ClampControlThresh = 0.2f;
+    [FieldMember(desc = "抱夹控制pid:DeadZone")] public float ClampControlDeadZone = 5f;
+    [FieldMember(desc = "抱夹最大速度")] public float MaxClampSpeed = 1.5f;
+    #endregion
+#if false
+    #region 多车-编队与遥控
+    [FieldMember(desc = "联动时转向角爬升加速度")] public float SyncThAccPerSec = 30f;
+    [FieldMember(desc = "两车间距 (mm)")] public float TestCarSyncDistance = 2400f;
+    [FieldMember(desc = "编队排布偏角")] public float TestCarSyncTh = 0f;
+    // Fleet manual remote IO values are normalized joystick ratios. Keep all speed/angle scaling here.
+    [FieldMember(desc = "车队遥控最大线速度")] public float FleetManualMaxSpeed = 0.3f;
+    [FieldMember(desc = "常规模式满杆舵角")] public float FleetManualMaxSteerAngleDeg = 45f;
+    [FieldMember(desc = "蟹行满杆舵角")] public float FleetManualMaxCrabAngleDeg = 60f;
+    [FieldMember(desc = "旋转满杆角速度")] public float FleetManualMaxRotateOmegaDegPerSec = 45f;
+    [FieldMember(desc = "蟹行舵角上限（对齐 ±120）")] public float MultiVehicleCrabSteerLimitDeg = 120f;
+    [FieldMember(desc = "互识别检测中心偏移")] public float DeltaDetectCenter = 350f;
+    #endregion
+
+    #region 多车-通信
+    [FieldMember(desc = "多车联动：总车数")] public int MultiVehicleFleetNum = 2;
+    [FieldMember(desc = "联动线程周期(ms)")] public int MultiVehicleSyncInterval = 50;
+    [FieldMember(desc = "多车联动：主车端点 ip:port，/ 表示本车为主车")] public string MultiVehicleMasterEndpoint = "/";
+    [FieldMember(desc = "多车联动：本车同步 IP")] public string SimpleIp = "127.0.0.1";
+
+    [FieldMember(desc = "多车联动：本车回连端点 ip:port，供主车 notify 回连，空=127.0.0.1:本车port")] public string MultiVehicleSelfEndpoint = "";
+    [FieldMember(desc = "多车联动：自动速度命令超时(ms，0=auto)")] public int MultiVehicleAutoCmdTimeoutMs = 0;
+    [FieldMember(desc = "多车联动：成员存活TTL(ms，0=auto)")] public int MultiVehicleMemberTtlMs = 0;
+
+    [JsonProperty("MultiVehicleMasterIp")]
+    private string LegacyMasterIpSetter
+    {
+        set
+        {
+            if (string.IsNullOrEmpty(value) || value == "/") return;
+            if (MultiVehicleMasterEndpoint == "/")
+                MultiVehicleMasterEndpoint = value.Contains(":") ? value : $"{value}:8008";
+        }
+    }
+
+    #endregion
+
+    #region 多车-队形补偿
+    [FieldMember(desc = "定位是否参与车队内姿态纠正(不影响整队姿态计算)")] public bool MultiVehicleSyncUseDetour = false;
+    [FieldMember(desc = "手动联动是否启用定位姿态纠正(默认关闭)")] public bool MultiVehicleManualUseDetourCorrection = false;
+    [FieldMember(desc = "多车联动：启用互识别纠正")] public bool MultiVehicleUseDetect = false;
+    [FieldMember(desc = "多车联动：自动模式按理想中心前馈(弧线)")] public bool MultiVehicleAutoUseIdealCenter = true;
+    [FieldMember(desc = "多车联动：自动模式要求有效车队中心")] public bool MultiVehicleAutoRequireFleetCenter = true;
+    [FieldMember(desc = "多车联动：SLAM X补偿系数")] public float MultiVehiclePosBiasXFac = 0.5f;
+    [FieldMember(desc = "多车联动：SLAM Y补偿系数")] public float MultiVehiclePosBiasYFac = 0.5f;
+    [FieldMember(desc = "多车联动：SLAM Th补偿系数")] public float MultiVehiclePosBiasThFac = 0.5f;
+    [FieldMember(desc = "多车联动：X补偿阈值(mm)")] public float MultiVehiclePosBiasXThreshold = 50f;
+    [FieldMember(desc = "多车联动：Y补偿阈值(mm)")] public float MultiVehiclePosBiasYThreshold = 50f;
+    [FieldMember(desc = "多车联动：Th补偿阈值(deg)")] public float MultiVehiclePosBiasThThreshold = 5f;
+    [FieldMember(desc = "多车联动：互识别 X补偿系数")] public float MultiVehicleDetectBiasXFac = 0.5f;
+    [FieldMember(desc = "多车联动：互识别 Y补偿系数")] public float MultiVehicleDetectBiasYFac = 0.5f;
+    [FieldMember(desc = "多车联动：互识别 Th补偿系数")] public float MultiVehicleDetectBiasThFac = 0.5f;
+    [FieldMember(desc = "多车联动：互识别 X补偿阈值(mm)")] public float MultiVehicleDetectBiasXThreshold = 50f;
+    [FieldMember(desc = "多车联动：互识别 Y补偿阈值(mm)")] public float MultiVehicleDetectBiasYThreshold = 50f;
+    [FieldMember(desc = "多车联动：互识别 Th补偿阈值(deg)")] public float MultiVehicleDetectBiasThThreshold = 5f;
+    #endregion
+
+    #region 多车-旋转补偿
+    [FieldMember(desc = "原地旋转纠偏：平移比例增益P(mm/s per mm)")] public float MultiVehicleRotateCompXyFac = 1.2f;
+    [FieldMember(desc = "原地旋转纠偏：平移积分增益I(mm/s per mm·s)")] public float MultiVehicleRotateCompXyIFac = 0.8f;
+    [FieldMember(desc = "原地旋转纠偏：平移速度上限(mm/s)")] public float MultiVehicleRotateCompXyMax = 150f;
+    [FieldMember(desc = "原地旋转纠偏：转向比例增益P(deg/s per deg)")] public float MultiVehicleRotateCompThFac = 0.8f;
+    [FieldMember(desc = "原地旋转纠偏：转向积分增益I(deg/s per deg·s)")] public float MultiVehicleRotateCompThIFac = 0.8f;
+    [FieldMember(desc = "原地旋转纠偏：转向速度上限(deg/s)")] public float MultiVehicleRotateCompThMax = 15f;
+    [FieldMember(desc = "原地旋转纠偏：生效的最小角速度阈值(deg/s)")] public float MultiVehicleRotateActiveOmega = 0.5f;
+    [FieldMember(desc = "原地旋转纠偏：纠偏/旋转切向比例硬上限，<0使用安全默认0.10")] public float MultiVehicleRotateCompTangentFrac = 0.10f;
+
+    [FieldMember(desc = "单车同步 xy 精度(mm)")] public float SingleCarSyncPrecisionXy = 10f;
+    [FieldMember(desc = "单车同步 th 精度(deg)")] public float SingleCarSyncPrecisionTh = 0.2f;
+    #endregion
+
+    #region 多车-联动动作
+    [FieldMember(desc = "车队原地旋转：角速度大小(deg/s，方向由目标角符号决定)")]
+    public float FleetRotateOmega = 15f;
+
+    [FieldMember(desc = "车队原地旋转：目标相对转角(deg，+逆时针)")]
+    public float FleetRotateTargetDeltaDeg = 90f;
+
+    [FieldMember(desc = "车队原地旋转：到位角度精度(deg)")]
+    public float FleetRotateArriveDeg = 1.5f;
+
+    [FieldMember(desc = "车队原地旋转：减速区宽度(deg)，抑制收尾惯性超调")]
+    public float FleetRotateSlowDeg = 25f;
+
+    [FieldMember(desc = "车队原地旋转：减速区末段最小角速度(deg/s)")]
+    public float FleetRotateMinOmega = 3f;
+
+    [FieldMember(desc = "车队原地旋转：起步缓启动角加速度(deg/s²，<=0关闭)")]
+    public float FleetRotateAccel = 20f;
+
+    [FieldMember(desc = "车队原地旋转：到位后安定时长(s)")]
+    public float FleetRotateSettleSec = 0.5f;
+
+    [FieldMember(desc = "车队原地旋转：用Detour主车航向闭环判停(默认true，false=按时长开环)")]
+    public bool FleetRotateUseDetourHeading = true;
+
+    [FieldMember(desc = "车队蟹行：路径方向相对启动时车队朝向夹角(deg，逆时针为正；路径在车右侧x度时填-x)")]
+    public float FleetCrabAngleDeg = 45f;
+
+    [FieldMember(desc = "车队蟹行：AGV入口使用的车队世界系目标朝向(deg)")]
+    public float FleetCrabBodyWorldHeadingDeg = 0f;
+
+    [FieldMember(desc = "车队蟹行：路径长度(mm)")]
+    public float FleetCrabLengthMm = 2000f;
+
+    [FieldMember(desc = "车队蟹行：行驶速度(m/s)")]
+    public float FleetCrabSpeed = 0.2f;
+
+    [FieldMember(desc = "车队蟹行：速度命令加速度限制(m/s^2，<=0表示不限制)")]
+    public float FleetCrabAccel = 0.2f;
+
+    [FieldMember(desc = "车队蟹行：预对齐后正式下发速度前5秒加速度(m/s^2，<=0表示不限制)")]
+    public float FleetCrabStartAccel = 0.01f;
+
+    [FieldMember(desc = "车队蟹行：末端开始减速距离(mm)")]
+    public float FleetCrabSlowDistance = 2000f;
+
+    [FieldMember(desc = "车队蟹行：完成距离(mm)，低于该剩余距离结束动作")]
+    public float FleetCrabFinishDistance = 20f;
+
+    [FieldMember(desc = "车队蟹行：末端最低速度(m/s)")]
+    public float FleetCrabFinishSpeed = 0.02f;
+
+    [FieldMember(desc = "车队蟹行：末端减速曲线指数")]
+    public float FleetCrabSlowingPow = 0.8f;
+
+    [FieldMember(desc = "车队蟹行：GCP舵角修正上限(deg)")]
+    public float FleetCrabGcpThetaThreshold = 95f;
+
+    [FieldMember(desc = "车队蟹行：headingErr角度纠偏比例系数")]
+    public float FleetCrabDthLinearFac = 1f;
+
+    [FieldMember(desc = "车队蟹行：headingErr角度纠偏舵角限幅(deg)")]
+    public float FleetCrabDthLinearThreshold = 10f;
+
+    [FieldMember(desc = "FleetCrab startup sync timeout(s)")]
+    public float FleetCrabStartSyncTimeoutSec = 8f;
+
+    [FieldMember(desc = "FleetCrab startup wheel alignment tolerance(deg)")]
+    public float FleetCrabStartWheelAlignDeg = 2f;
+
+    [FieldMember(desc = "FleetCurve MovementTest Bezier control point count")]
+    public int FleetCurveTestControlPointCount = 4;
+
+    [FieldMember(desc = "FleetCurve speed(m/s)")]
+    public float FleetCurveSpeed = 0.2f;
+
+    [FieldMember(desc = "FleetCurve slow distance(mm)")]
+    public float FleetCurveSlowDistance = 2000f;
+
+    [FieldMember(desc = "FleetCurve finish distance(mm)")]
+    public float FleetCurveFinishDistance = 20f;
+
+    [FieldMember(desc = "FleetCurve finish speed(m/s)")]
+    public float FleetCurveFinishSpeed = 0.02f;
+
+    [FieldMember(desc = "FleetCurve slowing curve exponent")]
+    public float FleetCurveSlowingPow = 0.8f;
+    #endregion
+
+#endif
+
+
+
 }

ClumsyPilot/PlaygroundWebApi.cs

@@ -1,81 +0,0 @@
-using System;
-using System.Net.Http;
-using System.Text;
-using Newtonsoft.Json;
-using Newtonsoft.Json.Linq;
-
-namespace MultiWheelC;
-
-/// <summary>
-/// Playground 仿真器 HTTP Web API 轻量客户端：查询小车位姿、瞬移小车。
-/// 服务端实现见 Playground/Web/PlaygroundWebApi.cs，默认监听 http://localhost:18090。
-/// 坐标单位 mm，朝向 yawDeg 单位为度，世界坐标系与场景 JSON 一致。
-/// </summary>
-public static class PlaygroundWebApi
-{
-    // 禁用系统代理：本机 Playground 走 localhost，若经系统代理(如 127.0.0.1:7890)会连接失败。
-    private static readonly HttpClient Http = new HttpClient(new HttpClientHandler { UseProxy = false })
-    {
-        Timeout = TimeSpan.FromSeconds(3)
-    };
-
-    public struct Pose
-    {
-        public float X;
-        public float Y;
-        public float YawDeg;
-    }
-
-    /// <summary>查询单台小车的世界位姿。GET /api/robots/{name}。</summary>
-    public static Pose GetPose(string baseUrl, string robotName)
-    {
-        var url = $"{baseUrl.TrimEnd('/')}/api/robots/{Uri.EscapeDataString(robotName)}";
-        var json = Http.GetStringAsync(url).GetAwaiter().GetResult();
-        var o = JObject.Parse(json);
-        return new Pose
-        {
-            X = o.Value<float>("x"),
-            Y = o.Value<float>("y"),
-            YawDeg = o.Value<float>("yawDeg")
-        };
-    }
-
-    /// <summary>将小车瞬移到目标世界位姿。POST /api/robots/{name}/move。</summary>
-    public static void Move(string baseUrl, string robotName, float x, float y, float yawDeg)
-    {
-        var url = $"{baseUrl.TrimEnd('/')}/api/robots/{Uri.EscapeDataString(robotName)}/move";
-        var body = JsonConvert.SerializeObject(new { x, y, yaw = yawDeg, stop = true });
-        using var content = new StringContent(body, Encoding.UTF8, "application/json");
-        var resp = Http.PostAsync(url, content).GetAwaiter().GetResult();
-        resp.EnsureSuccessStatusCode();
-    }
-
-    /// <summary>查询车辆运动是否启用（暂停时为 false）。GET /api/motion。</summary>
-    public static bool MotionEnabled(string baseUrl)
-    {
-        var url = $"{baseUrl.TrimEnd('/')}/api/motion";
-        var json = Http.GetStringAsync(url).GetAwaiter().GetResult();
-        return JObject.Parse(json).Value<bool>("motionEnabled");
-    }
-
-    /// <summary>恢复车辆运动。POST /api/motion/resume。</summary>
-    public static void ResumeMotion(string baseUrl)
-    {
-        var url = $"{baseUrl.TrimEnd('/')}/api/motion/resume";
-        var resp = Http.PostAsync(url, null).GetAwaiter().GetResult();
-        resp.EnsureSuccessStatusCode();
-    }
-
-    /// <summary>
-    /// 暂停车辆运动（仅冻结运动，不停止仿真；传感器继续扫描）。POST /api/motion/pause。
-    /// feedback: "zero"(默认,反馈归零) / "none"(不上报) / "hold"(保留暂停瞬间值)。
-    /// </summary>
-    public static void PauseMotion(string baseUrl, string feedback = "zero")
-    {
-        var url = $"{baseUrl.TrimEnd('/')}/api/motion/pause";
-        var body = JsonConvert.SerializeObject(new { feedback });
-        using var content = new StringContent(body, Encoding.UTF8, "application/json");
-        var resp = Http.PostAsync(url, content).GetAwaiter().GetResult();
-        resp.EnsureSuccessStatusCode();
-    }
-}

ClumsyPilot/TireFollowing.cs

@@ -1,511 +0,0 @@
-using System;
-using System.Collections.Generic;
-using System.Drawing;
-using System.Linq;
-using System.Numerics;
-using System.Reflection;
-using System.Text;
-using System.Threading;
-using ClumsyCore;
-using ClumsyCore.DTools;
-using ClumsyCore.Interfaces;
-using ClumsyCore.Pilot;
-using ClumsyCore.Utilities;
-using ClumsyDance.ClumsyWalk.Detectors;
-using CommonUsage.Chassis;
-using FundamentalLib;
-using MDCSToolBox;
-using MDCSToolBox.Clumsy.Calibration;
-using MDCSToolBox.Clumsy.MotionControllers;
-using MDCSToolBox.Clumsy.Movements;
-using MDCSToolBox.Clumsy.Pilot;
-using MDCSToolBox.Clumsy.Tracks;
-using MDCSToolBox.Commons.Controllers;
-using static ClumsyCore.DTools.Painter;
-using LineSegment = ClumsyCore.Utilities.LineSegment;
-
-namespace MultiWheelC
-{
-    public class TireFollowing : MovementDefinition
-    {
-        public Func<AbstractGeometricController> GetController;
-
-        /// <summary>
-        /// 车辆方向
-        /// </summary>
-        public float CarDirection = 0;
-
-        /// <summary>
-        /// 停止距离
-        /// </summary>
-        //public float FinishDistance = 1000;
-
-        /// <summary>
-        /// 减速距离
-        /// </summary>
-        public float SlowDistance = 1000;
-
-        /// <summary>
-        /// 最大速度
-        /// </summary>
-        public float MaxSpeed = 0.3f;
-
-        // 末段衔接：接近盲走终点时给非零速度，供后续动作连续接管
-        public bool EnableHandover = false;
-        public float HandoverDistance = 200f; // mm
-        public float HandoverSpeed = 0.2f;    // m/s
-
-        /// <summary>
-        /// 钻轮胎数量
-        /// </summary>
-        public int TireNum = 1;
-
-        /// <summary>
-        /// 盲走角度偏移
-        /// </summary>
-        public float WalkBlindTh = -1f;
-
-        /// <summary>
-        /// 是否检测到目标
-        /// </summary>
-        public bool NoTarget = false;
-
-        public float GuessRangeX;
-
-        public float GuessRangeY;
-
-        /// <summary>
-        /// 检测器定义
-        /// </summary>
-        public class DetectorDefinition
-        {
-            /// <summary>
-            /// 开始检测距离
-            /// </summary>
-            public float StartGuessingX;
-
-            /// <summary>
-            /// 开始检测距离
-            /// </summary>
-            public float StartGuessingY;
-
-            /// <summary>
-            /// 检测函数
-            /// </summary>
-            public Func<float, float, List<DetectFilter>, LineSegment> DetectFunction = null;
-
-            public Action<int> LeaveSrcFunction = null;
-
-            public int SrcId = -1;
-            public int DstId = -1;
-
-            /// <summary>
-            /// 路径偏移
-            /// </summary>
-            public Tuple<float, float, float> PathTransformation = Tuple.Create(0f, 0f, 0f);
-
-            public float PathTransformationAnchorDistance = 0f;
-
-            /// <summary>
-            /// 切换条件
-            /// </summary>
-            public Func<float, bool> SwitchWalkBlindCondition = null;
-
-            /// <summary>
-            /// 盲走停止距离
-            /// </summary>
-            public Func<float, bool> FinishWalkBlindCondition = null;
-        }
-
-        public Func<bool> FinishCondition;
-
-        /// <summary>
-        /// 多个检测器列表
-        /// </summary>
-        public List<DetectorDefinition> detectors = null;
-
-        private Painter _painter;
-        private List<float> _remainDistanceList = new List<float>();
-        private List<float> _remainAngleList = new List<float>();
-        private List<float> _targetYList = new List<float>();
-
-        private List<DetectFilter> SetFilters(float guessCenterX, float guessCenterY)
-        {
-            var painter = UI.GetPainter("GeneralFollowing.SetFilters", false);
-            painter.Clear();
-            painter.Clear(3000);
-
-            var box = new Vector2[]
-            {
-                new (guessCenterX - GuessRangeX, guessCenterY - GuessRangeY),
-                new (guessCenterX + GuessRangeX, guessCenterY - GuessRangeY),
-                new (guessCenterX + GuessRangeX, guessCenterY + GuessRangeY),
-                new (guessCenterX - GuessRangeX, guessCenterY + GuessRangeY),
-            };
-
-            for (var i = 0; i < box.Length; ++i)
-                painter.DrawLine(Color.DarkOliveGreen, box[i], box[(i + 1) % 4]);
-
-            // PC filter in car coordinate frame
-            return new List<DetectFilter>()
-            {
-                new(CoordinateSystem.Car2D,
-                    p => LessMath.IsPointInPolygon4(
-                        box.Select(v => new PointF(v.X, v.Y)).ToArray(), new PointF(p.X, p.Y))),
-            };
-        }
-
-        public void Stop()
-        {
-            _dt?.Stop();
-        }
-
-        /// <summary>
-        ///计算车体中心的位移和角度增量
-        /// </summary>
-        /// <param name="a">a轮在车体坐标系下位置</param>
-        /// <param name="va">a轮在车体坐标系下位移增量</param>
-        /// <param name="b">b轮在车体坐标系下位置</param>
-        /// <param name="vb">b轮在车体坐标系下位移增量</param>
-        /// <returns></returns>
-        private static (float, float, float) CenterMoveFromPoints(Vector2 a,
-            Vector2 aDelta,
-            Vector2 b,
-            Vector2 bDelta)
-        {
-            float th_x = 0, th_y = 0, th = 0, x = 0, y = 0;
-            var eps = 0.0000001;
-            if (Math.Abs(a.Y - b.Y) > eps)
-            {
-                th_x = (aDelta.X - bDelta.X) / (b.Y - a.Y);
-            }
-            if (Math.Abs(a.X - b.X) > eps)
-            {
-                th_y = (aDelta.Y - bDelta.Y) / (a.X - b.X);
-            }
-            th = th_x == 0 ? th_y : th_x;
-
-            x = (aDelta.X + bDelta.X) / 2f - (a.Y - b.Y) / 2f * th;
-
-            y = (aDelta.Y + bDelta.Y) / 2f + (a.X - b.X) / 2f * th;
-
-            return (x, y, th);
-        }
-
-
-        public override IEnumerable<bool> Get()
-        {
-            _painter = UI.GetPainter("GeneralFollowing", false);
-            var lastDetectX = detectors[0].StartGuessingX;
-            var lastDetectY = detectors[0].StartGuessingY;
-            var detectorIndex = 0;
-
-            var controller = (MultiWheelGeometricController)GetController.Invoke();
-            controller.BaseSpeed = MaxSpeed;
-            controller.FinishDistance = float.MinValue;
-            controller.FirstThAccuracy = 999;
-            _dt = new DriveTask(controller.Track(true, CoordinateSystem.Car2D));
-            void HardStop()
-            {
-                _dt?.Stop();
-                ((MultiWheelChassis)PilotDefinition.Chassis).DriveStop();
-                DLog.Log($"Hard Stop!", "TireFollowing");
-            }
-            float WalkBlindCarPathDstX = -1f, WalkBlindCarPathDstY = -1f, WalkBlindCarPathDstTh = -1f;
-            bool WalkBlindStage1 = false, WalkBlindStage2 = false;
-            var angle2target = -1f;
-            float _lastLFLEncoder = -1, _lastLFREncoder = -1, _lastRFLEncoder = -1, _lastRFREncoder = -1;
-            float _lastLRLEncoder = -1, _lastLRREncoder = -1, _lastRRLEncoder = -1, _lastRRREncoder = -1;
-
-            (float, float, float) GetCurrentPos2Dst(float lastX, float lastY, float lastTh)
-            {
-                // Read current encoders
-                var curLFLEncoder = PilotDefinition.Self.LFLActualPos;
-                var curLFREncoder = PilotDefinition.Self.LFRActualPos;
-                var curRFLEncoder = PilotDefinition.Self.RFLActualPos;
-                var curRFREncoder = PilotDefinition.Self.RFRActualPos;
-                var curLRLEncoder = PilotDefinition.Self.LRLActualPos;
-                var curLRREncoder = PilotDefinition.Self.LRRActualPos;
-                var curRRLEncoder = PilotDefinition.Self.RRLActualPos;
-                var curRRREncoder = PilotDefinition.Self.RRRActualPos;
-
-                // Average delta per wheel pair (LF, LR, RF, RR)
-                var lfDelta = (curLFLEncoder - _lastLFLEncoder + curLFREncoder - _lastLFREncoder) / 2f;
-                var lrDelta = (curLRLEncoder - _lastLRLEncoder + curLRREncoder - _lastLRREncoder) / 2f;
-                var rfDelta = (curRFLEncoder - _lastRFLEncoder + curRFREncoder - _lastRFREncoder) / 2f;
-                var rrDelta = (curRRLEncoder - _lastRRLEncoder + curRRREncoder - _lastRRREncoder) / 2f;
-                var deltaList = new List<float> { lfDelta, lrDelta, rfDelta, rrDelta };
-
-                var xs = new List<float>();
-                var ys = new List<float>();
-                var ths = new List<float>();
-                var chassis = (MultiWheelChassis)BasicPilotBase.Chassis;
-                var steerWheels = chassis.GetSteerWheels();
-                for (var i = 0; i < steerWheels.Count; ++i)
-                {
-                    var sw1 = steerWheels[i];
-                    var a = sw1.Position;
-                    var tha = sw1.ReadAngle() / 180f * (float)Math.PI;
-                    var deltaa = deltaList[i];
-                    var va = new Vector2(deltaa * (float)Math.Cos(tha), deltaa * (float)Math.Sin(tha));
-                    for (var j = i + 1; j < steerWheels.Count; ++j)
-                    {
-                        var sw2 = steerWheels[j];
-                        var b = sw2.Position;
-                        var thb = sw2.ReadAngle() / 180f * (float)Math.PI;
-                        var deltab = deltaList[j];
-                        var vb = new Vector2(deltab * (float)Math.Cos(thb), deltab * (float)Math.Sin(thb));
-                        var (tempx, tempy, tempth) = CenterMoveFromPoints(a, va, b, vb);
-                        Hedingben.ToastText($"{tempx:f2} {tempy:f2} {tempth / Math.PI * 180f:f2} ", $"{i}_{j}");
-                        xs.Add(tempx);
-                        ys.Add(tempy);
-                        ths.Add(tempth);
-                    }
-                }
-                var x = xs.Average();
-                var y = ys.Average();
-                var Th = ths.Average() / (float)Math.PI * 180;
-                var moveTup = Tuple.Create(x, y, Th);
-                var moved = MathTools.SolveTransform2D(MathTools.SolveTransform2D(Tuple.Create(lastX, lastY, lastTh), moveTup), Tuple.Create(0f, 0f, 0f));
-
-                _lastLFLEncoder = curLFLEncoder;
-                _lastLFREncoder = curLFREncoder;
-                _lastRFLEncoder = curRFLEncoder;
-                _lastRFREncoder = curRFREncoder;
-                _lastLRLEncoder = curLRLEncoder;
-                _lastLRREncoder = curLRREncoder;
-                _lastRRLEncoder = curRRLEncoder;
-                _lastRRREncoder = curRRREncoder;
-                return (moved.Item1, moved.Item2, moved.Item3);
-            }
-
-            while (true)
-            {
-                if (detectorIndex > detectors.Count - 1)
-                    throw new Exception("detector index out of range!");
-
-                _painter.Clear();
-                if (WalkBlindStage1 || WalkBlindStage2)
-                {
-                    //第二次盲走时或只钻一个轮胎时
-                    if (WalkBlindStage2 || detectors.Count == 1 || TireNum == 1)
-                    {
-                        //controller.FinishDistance = 10f;
-                        controller.SlowDistance = SlowDistance;
-                        controller.SlowingPow = 0.7f;
-                    }
-                    if (EnableHandover)
-                    {
-                        controller.SlowDistance = float.MinValue;
-                        controller.FinishSpeed = 0.2f;
-                        controller.FinishDistance = 50;
-                    }
-                    (WalkBlindCarPathDstX, WalkBlindCarPathDstY, WalkBlindCarPathDstTh) = GetCurrentPos2Dst(WalkBlindCarPathDstX, WalkBlindCarPathDstY, WalkBlindCarPathDstTh);
-                    var walkBlindPathEnd = Tuple.Create(WalkBlindCarPathDstX, WalkBlindCarPathDstY, WalkBlindCarPathDstTh);
-                    var walkBlindPathStart = LessMath.Transform2D(walkBlindPathEnd, Tuple.Create(CarDirection == 0 ? -3000f : 3000f, 0f, 0f));
-                    var walkBlindPathDst = new Vector2(WalkBlindCarPathDstX, WalkBlindCarPathDstY);
-                    var walkBlindPathSrc = new Vector2(walkBlindPathStart.Item1, walkBlindPathStart.Item2);
-                    var walkBlindPath = new LineSegment(walkBlindPathSrc, walkBlindPathDst);
-
-                    DLog.Log($"盲走目标点:{walkBlindPath.Src.X:F2} {walkBlindPath.Src.Y:F2} {walkBlindPath.Dst.X:F2} {walkBlindPath.Dst.Y:F2}", "TireFollowing");
-                    _painter.DrawDot(Color.Purple, walkBlindPathDst, sz: 3);
-                    _painter.DrawLine(Color.GreenYellow, walkBlindPath.Src, walkBlindPath.Dst, endArrow: true, width: 2);
-
-                    var track = new LineTrack(walkBlindPath.Src, walkBlindPath.Dst);
-                    track.CarDirectionBias = CarDirection;
-                    controller.UpdateTracks(new List<AbstractTrack> { track });
-                    var rd = (float)LessMath.PerpendicularPosition(0, 0, walkBlindPath.Dst.X, walkBlindPath.Dst.Y,
-                        walkBlindPath.Src.X, walkBlindPath.Src.Y);
-                    _remainDistanceList.Add(rd);
-                    while (_remainDistanceList.Count > 3) _remainDistanceList.RemoveAt(0);
-                    rd = _remainDistanceList.Average();
-                    DLog.Log($"盲走投影点剩余距离:{rd:0.0} ", "TireFollowing");
-                    // 检查是否达到盲走结束条件
-                    if (detectors[detectorIndex].FinishWalkBlindCondition(rd))
-                    {
-                        if (WalkBlindStage1)
-                        {
-                            DLog.Log("达到第一次盲走停止距离，停下或开始钻第二对轮胎", "TireFollowing");
-                            //if (detectors[detectorIndex].DstId != -1 && detectors[detectorIndex].LeaveSrcFunction != null)
-                            //{
-                            //    detectors[detectorIndex].LeaveSrcFunction(detectors[detectorIndex].DstId);
-                            //    DLog.Log($"释放取车点{detectors[detectorIndex].DstId}", "TireFollowing");
-                            //}
-                            WalkBlindStage1 = false;
-                            _remainAngleList.Clear();
-                            _remainDistanceList.Clear();
-                            detectorIndex++;
-                            if ((detectors.Count == 1 || TireNum == 1) && !EnableHandover)
-                            {
-                                HardStop();
-                                yield return false;
-                            }
-                        }
-                        else if (WalkBlindStage2)
-                        {
-                            DLog.Log("达到第二对轮胎处，停止移动", "TireFollowing");
-                            if (!EnableHandover)
-                            {
-                                HardStop();
-                            }
-                            yield return false;
-                        }
-                    }
-                    yield return true;
-                    continue;
-                }
-
-                var target = detectors[detectorIndex].DetectFunction(CarDirection, lastDetectX,
-                        SetFilters(lastDetectX, lastDetectY));
-
-                if (target == null)
-                {
-                    DLog.Log("无目标，等待下一帧", "TireFollowing");
-                    controller.FirstRotateMaxSpeed = 0;
-                    yield return true;
-                    continue;
-                }
-                else controller.FirstRotateMaxSpeed = 5;
-
-                var targetAngle = CalculateAngle2YAxis(target.Src, target.Dst);
-                var targetPos = new Vector2((target.Src.X + target.Dst.X) / 2f, (target.Src.Y + target.Dst.Y) / 2f);
-                var dis2target = (float)Math.Sqrt(Math.Pow(targetPos.X, 2) + Math.Pow(targetPos.Y, 2));
-                //距离较近以后角度容易跳变
-                if (dis2target < PilotDefinition.Conf.TireFollowingCloseDistance && Math.Abs(targetAngle) > PilotDefinition.Conf.TireFollowingAngleIgnoreThr)
-                {
-                    yield return true;
-                    continue;
-                }
-                else _remainAngleList.Add(targetAngle);
-                while (_remainAngleList.Count > 10) _remainAngleList.RemoveAt(0);
-                angle2target = _remainAngleList.Average();
-                var distanceLabelPos = targetPos / 2f;
-                _painter.DrawLine(Color.Cyan, Vector2.Zero, targetPos, width: 2);
-                _painter.DrawText(Color.Yellow, $"{dis2target:F3}", distanceLabelPos.X, distanceLabelPos.Y);
-
-                var path = DetectorHelper.GetApproachPath(target, CoordinateSystem.Car2D, pathLen: 3000,
-                                                          bias: detectors[detectorIndex].PathTransformation,
-                                                          biasAnchorDistance: detectors[detectorIndex].PathTransformationAnchorDistance);
-
-                if (path == null)
-                {
-                    DLog.Log("no path!", "TireFollowing");
-                    NoTarget = true;
-                }
-                else
-                {
-                    lastDetectX = ((target.Src + target.Dst) / 2f).X;
-                    lastDetectY = ((target.Src + target.Dst) / 2f).Y;
-
-                    var currentY = path.CarPath.Dst.Y;
-                    if (Math.Abs(targetAngle) < PilotDefinition.Conf.TireFollowingAngleIgnoreThr &&
-                        dis2target < PilotDefinition.Conf.TireFollowingCloseDistance)
-                    {
-                        _targetYList.Add(currentY);
-                        while (_targetYList.Count > PilotDefinition.Conf.TireFollowingYAverageFrameCount) _targetYList.RemoveAt(0);
-                    }
-
-                    var trackDstY = _targetYList.Count > 0 ? _targetYList.Average() : currentY;
-                    Hedingben.ToastText($"target Y:{_targetYList.Count}  {trackDstY}", "target Y");
-
-                    var trackDst = new Vector2(path.CarPath.Dst.X, trackDstY);
-                    _painter.DrawLine(Color.GreenYellow, path.CarPath.Src, trackDst, endArrow: true);
-
-                    var rd = (float)LessMath.PerpendicularPosition(0, 0, trackDst.X, trackDst.Y,
-                        path.CarPath.Src.X, path.CarPath.Src.Y);
-                    _remainDistanceList.Add(rd);
-                    while (_remainDistanceList.Count > 3) _remainDistanceList.RemoveAt(0);
-                    rd = _remainDistanceList.Average();
-                    _painter.DrawText(Color.Green, $"{rd:F3}", distanceLabelPos.X, distanceLabelPos.Y - 200);
-                    if(rd < PilotDefinition.Conf.TireFollowingReleaseDistance)
-                    {
-                        if (detectors[detectorIndex].SrcId != -1 && detectors[detectorIndex].LeaveSrcFunction != null)
-                        {
-                            detectors[detectorIndex].LeaveSrcFunction(detectors[detectorIndex].SrcId);
-                            DLog.Log($"释放预取车点{detectors[detectorIndex].SrcId}", "TireFollowing");
-                        }
-                    }
-
-                    if (detectorIndex < detectors.Count - 1)
-                    {
-                        controller.SlowDistance = 1;
-                        if (detectors[detectorIndex].SwitchWalkBlindCondition(rd))
-                        {
-                            WalkBlindStage1 = true;
-                            //if (detectors[detectorIndex].SrcId != -1 && detectors[detectorIndex].LeaveSrcFunction != null)
-                            //{
-                            //    detectors[detectorIndex].LeaveSrcFunction(detectors[detectorIndex].SrcId);
-                            //    DLog.Log($"释放预取车点{detectors[detectorIndex].SrcId}", "TireFollowing");
-                            //}
-                            WalkBlindCarPathDstX = trackDst.X;
-                            WalkBlindCarPathDstY = trackDst.Y;
-                            WalkBlindCarPathDstTh = angle2target + WalkBlindTh;
-                            DLog.Log($"切换至第一次盲走时刻目标点:{WalkBlindCarPathDstX:F2} " +
-                                                                $"{WalkBlindCarPathDstY:F2} " +
-                                                                $"{WalkBlindCarPathDstTh:F2}", "TireFollowing");
-                            _lastLFLEncoder = PilotDefinition.Self.LFLActualPos;
-                            _lastLFREncoder = PilotDefinition.Self.LFRActualPos;
-                            _lastRFLEncoder = PilotDefinition.Self.RFLActualPos;
-                            _lastRFREncoder = PilotDefinition.Self.RFRActualPos;
-                            _lastLRLEncoder = PilotDefinition.Self.LRLActualPos;
-                            _lastLRREncoder = PilotDefinition.Self.LRRActualPos;
-                            _lastRRLEncoder = PilotDefinition.Self.RRLActualPos;
-                            _lastRRREncoder = PilotDefinition.Self.RRRActualPos;
-                            _remainDistanceList.Clear();
-                            _targetYList.Clear();
-                            lastDetectX = detectors[detectorIndex + 1].StartGuessingX;
-                            lastDetectY = detectors[detectorIndex + 1].StartGuessingY;
-                            continue;
-                        }
-                    }
-                    else if (detectorIndex == detectors.Count - 1)
-                    {
-                        if (detectors[detectorIndex].SwitchWalkBlindCondition(rd))
-                        {
-                            WalkBlindStage2 = true;
-                            WalkBlindCarPathDstX = trackDst.X;
-                            WalkBlindCarPathDstY = trackDst.Y;
-                            WalkBlindCarPathDstTh = angle2target + WalkBlindTh;
-                            DLog.Log($"切换至最后一次盲走时刻目标点:{WalkBlindCarPathDstX:F2} " +
-                                                                $"{WalkBlindCarPathDstY:F2} " +
-                                                                $"{WalkBlindCarPathDstTh:F2}", "TireFollowing");
-                            if (detectors.Count == 1)
-                            {
-                                if (detectors[detectorIndex].SrcId != -1 && detectors[detectorIndex].LeaveSrcFunction != null)
-                                {
-                                    detectors[detectorIndex].LeaveSrcFunction(detectors[detectorIndex].SrcId);
-                                    DLog.Log($"释放预取车点{detectors[detectorIndex].SrcId}", "TireFollowing");
-                                }
-                            }
-
-                            _lastLFLEncoder = PilotDefinition.Self.LFLActualPos;
-                            _lastLFREncoder = PilotDefinition.Self.LFRActualPos;
-                            _lastRFLEncoder = PilotDefinition.Self.RFLActualPos;
-                            _lastRFREncoder = PilotDefinition.Self.RFRActualPos;
-                            _lastLRLEncoder = PilotDefinition.Self.LRLActualPos;
-                            _lastLRREncoder = PilotDefinition.Self.LRRActualPos;
-                            _lastRRLEncoder = PilotDefinition.Self.RRLActualPos;
-                            _lastRRREncoder = PilotDefinition.Self.RRRActualPos;
-                            _remainDistanceList.Clear();
-                            _targetYList.Clear();
-                            continue;
-                        }
-                    }
-                    DLog.Log($"投影点剩余距离:{rd:F2}", "TireFollowing");
-                    var track = new LineTrack(path.CarPath.Src, trackDst);
-                    track.CarDirectionBias = CarDirection;
-                    controller.UpdateTracks(new List<AbstractTrack> { track });
-                    NoTarget = false;
-                }
-                yield return true;
-            }
-        }
-
-        private static float CalculateAngle2YAxis(Vector2 point1, Vector2 point2)
-        {
-            return -(float)(Math.Atan((point1.X - point2.X) / (point1.Y - point2.Y)) * 180 / Math.PI);
-        }
-
-        private DriveTask _dt;
-    }
-}

ClumsyPilot/TrackingExperimentRecorder.cs

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

ClumsyPilot/VehicleSyncBinaryCodec.cs

@@ -1,277 +0,0 @@
-using System;
-using System.Collections.Generic;
-using System.IO;
-using System.Text;
-
-namespace MultiWheelC;
-
-internal static class VehicleSyncBinaryCodec
-{
-    private const byte Version = 2;
-    private const byte RegisterType = 1;
-    private const byte NotificationType = 2;
-    private static readonly byte[] Magic = Encoding.ASCII.GetBytes("MVS1");
-
-    public static byte[] EncodeRegister(int carNum, VehicleSyncInfo info)
-    {
-        using var stream = new MemoryStream();
-        using var writer = new BinaryWriter(stream, Encoding.UTF8);
-        WriteHeader(writer, RegisterType);
-        writer.Write(carNum);
-        WriteInfo(writer, info);
-        writer.Flush();
-        return stream.ToArray();
-    }
-
-    public static (int CarNum, VehicleSyncInfo Info) DecodeRegister(byte[] payload)
-    {
-        using var stream = new MemoryStream(payload ?? throw new ArgumentNullException(nameof(payload)));
-        using var reader = new BinaryReader(stream, Encoding.UTF8);
-        var version = ReadHeader(reader, RegisterType);
-        var carNum = reader.ReadInt32();
-        var info = ReadInfo(reader, version);
-        EnsureFullyRead(stream);
-        return (carNum, info);
-    }
-
-    public static byte[] EncodeNotification(VehicleSyncNotification notification)
-    {
-        using var stream = new MemoryStream();
-        using var writer = new BinaryWriter(stream, Encoding.UTF8);
-        WriteHeader(writer, NotificationType);
-
-        writer.Write(notification.Seq);
-        writer.Write(BuildNotificationFlags(notification));
-        writer.Write(notification.Mode);
-        writer.Write(notification.FleetStopSourceCar);
-        writer.Write(notification.CenterX);
-        writer.Write(notification.CenterY);
-        writer.Write(notification.CenterTh);
-        writer.Write(notification.FleetVx);
-        writer.Write(notification.FleetFrontTh);
-        writer.Write(notification.FleetRearTh);
-        writer.Write(notification.FleetOmega);
-        writer.Write(notification.RequestedFleetOmega);
-        writer.Write(notification.SyncTh);
-        writer.Write(notification.SyncDistance);
-        writer.Write(notification.DeltaDetectCenter);
-        writer.Write(notification.RotateActiveOmega);
-        writer.Write(notification.RotateCompXyFac);
-        writer.Write(notification.RotateCompXyIFac);
-        writer.Write(notification.RotateCompXyMax);
-        writer.Write(notification.RotateCompThFac);
-        writer.Write(notification.RotateCompThIFac);
-        writer.Write(notification.RotateCompThMax);
-        writer.Write(notification.RotateCompTangentFrac);
-        writer.Write(notification.RotateStartWheelAlignDeg);
-        writer.Write(notification.RotateActiveWheelAlignDeg);
-        writer.Write(notification.IdealX);
-        writer.Write(notification.IdealY);
-        writer.Write(notification.IdealTh);
-        WriteString(writer, notification.FleetStopReason);
-
-        var fleet = notification.Fleet ?? new Dictionary<int, VehicleSyncInfo>();
-        if (fleet.Count > ushort.MaxValue)
-            throw new InvalidOperationException($"Fleet count {fleet.Count} exceeds binary protocol limit.");
-        writer.Write((ushort)fleet.Count);
-        foreach (var kv in fleet)
-        {
-            writer.Write(kv.Key);
-            WriteInfo(writer, kv.Value);
-        }
-
-        writer.Flush();
-        return stream.ToArray();
-    }
-
-    public static VehicleSyncNotification DecodeNotification(byte[] payload)
-    {
-        using var stream = new MemoryStream(payload ?? throw new ArgumentNullException(nameof(payload)));
-        using var reader = new BinaryReader(stream, Encoding.UTF8);
-        var version = ReadHeader(reader, NotificationType);
-
-        var notification = new VehicleSyncNotification
-        {
-            Seq = reader.ReadInt64()
-        };
-
-        ApplyNotificationFlags(notification, reader.ReadUInt16());
-        notification.Mode = reader.ReadInt32();
-        notification.FleetStopSourceCar = reader.ReadInt32();
-        notification.CenterX = reader.ReadSingle();
-        notification.CenterY = reader.ReadSingle();
-        notification.CenterTh = reader.ReadSingle();
-        notification.FleetVx = reader.ReadSingle();
-        notification.FleetFrontTh = reader.ReadSingle();
-        notification.FleetRearTh = reader.ReadSingle();
-        notification.FleetOmega = reader.ReadSingle();
-        notification.RequestedFleetOmega = reader.ReadSingle();
-        notification.SyncTh = reader.ReadSingle();
-        notification.SyncDistance = reader.ReadSingle();
-        notification.DeltaDetectCenter = reader.ReadSingle();
-        notification.RotateActiveOmega = reader.ReadSingle();
-        notification.RotateCompXyFac = reader.ReadSingle();
-        notification.RotateCompXyIFac = reader.ReadSingle();
-        notification.RotateCompXyMax = reader.ReadSingle();
-        notification.RotateCompThFac = reader.ReadSingle();
-        notification.RotateCompThIFac = reader.ReadSingle();
-        notification.RotateCompThMax = reader.ReadSingle();
-        notification.RotateCompTangentFrac = reader.ReadSingle();
-        notification.RotateStartWheelAlignDeg = reader.ReadSingle();
-        notification.RotateActiveWheelAlignDeg = reader.ReadSingle();
-        notification.IdealX = reader.ReadSingle();
-        notification.IdealY = reader.ReadSingle();
-        notification.IdealTh = reader.ReadSingle();
-        notification.FleetStopReason = ReadString(reader);
-
-        var fleetCount = reader.ReadUInt16();
-        notification.Fleet = new Dictionary<int, VehicleSyncInfo>(fleetCount);
-        for (var i = 0; i < fleetCount; ++i)
-        {
-            var carNum = reader.ReadInt32();
-            notification.Fleet[carNum] = ReadInfo(reader, version);
-        }
-
-        EnsureFullyRead(stream);
-        return notification;
-    }
-
-    private static void WriteHeader(BinaryWriter writer, byte type)
-    {
-        writer.Write(Magic);
-        writer.Write(Version);
-        writer.Write(type);
-        writer.Write((ushort)0);
-    }
-
-    private static byte ReadHeader(BinaryReader reader, byte expectedType)
-    {
-        for (var i = 0; i < Magic.Length; ++i)
-        {
-            if (reader.ReadByte() != Magic[i])
-                throw new InvalidDataException("Invalid multi-vehicle sync binary magic.");
-        }
-
-        var version = reader.ReadByte();
-        if (version < 1 || version > Version)
-            throw new InvalidDataException($"Unsupported multi-vehicle sync binary version {version}.");
-
-        var type = reader.ReadByte();
-        if (type != expectedType)
-            throw new InvalidDataException($"Unexpected multi-vehicle sync packet type {type}.");
-
-        var reserved = reader.ReadUInt16();
-        if (reserved != 0)
-            throw new InvalidDataException("Invalid multi-vehicle sync binary reserved field.");
-
-        return version;
-    }
-
-    private static void WriteInfo(BinaryWriter writer, VehicleSyncInfo info)
-    {
-        writer.Write(BuildInfoFlags(info));
-        WriteString(writer, info.Ip);
-        writer.Write(info.Port);
-        writer.Write(info.X);
-        writer.Write(info.Y);
-        writer.Write(info.Th);
-        writer.Write(info.LayoutX);
-        writer.Write(info.LayoutY);
-        writer.Write(info.LayoutTh);
-        WriteString(writer, info.MotionInfeasibleReason);
-        WriteString(writer, info.RotateWheelAlignDetail);
-        writer.Write(info.AppliedNotificationSeq);
-    }
-
-    private static VehicleSyncInfo ReadInfo(BinaryReader reader, byte version)
-    {
-        var info = new VehicleSyncInfo();
-        ApplyInfoFlags(info, reader.ReadUInt16());
-        info.Ip = ReadString(reader);
-        info.Port = reader.ReadInt32();
-        info.X = reader.ReadSingle();
-        info.Y = reader.ReadSingle();
-        info.Th = reader.ReadSingle();
-        info.LayoutX = reader.ReadSingle();
-        info.LayoutY = reader.ReadSingle();
-        info.LayoutTh = reader.ReadSingle();
-        info.MotionInfeasibleReason = ReadString(reader);
-        info.RotateWheelAlignDetail = ReadString(reader);
-        info.AppliedNotificationSeq = version >= 2 ? reader.ReadInt64() : -1;
-        return info;
-    }
-
-    private static ushort BuildInfoFlags(VehicleSyncInfo info)
-    {
-        ushort flags = 0;
-        if (info.Master) flags |= 1 << 0;
-        if (info.PosAvailable) flags |= 1 << 1;
-        if (info.Aligned) flags |= 1 << 2;
-        if (info.DetectOk) flags |= 1 << 3;
-        if (info.MotionFeasible) flags |= 1 << 4;
-        if (info.RotateWheelsAligned) flags |= 1 << 5;
-        return flags;
-    }
-
-    private static void ApplyInfoFlags(VehicleSyncInfo info, ushort flags)
-    {
-        info.Master = (flags & (1 << 0)) != 0;
-        info.PosAvailable = (flags & (1 << 1)) != 0;
-        info.Aligned = (flags & (1 << 2)) != 0;
-        info.DetectOk = (flags & (1 << 3)) != 0;
-        info.MotionFeasible = (flags & (1 << 4)) != 0;
-        info.RotateWheelsAligned = (flags & (1 << 5)) != 0;
-    }
-
-    private static ushort BuildNotificationFlags(VehicleSyncNotification notification)
-    {
-        ushort flags = 0;
-        if (notification.PosAvailable) flags |= 1 << 0;
-        if (notification.Aligned) flags |= 1 << 1;
-        if (notification.FleetMotionReleased) flags |= 1 << 2;
-        if (notification.FleetStopActive) flags |= 1 << 3;
-        if (notification.AutoEnabled) flags |= 1 << 4;
-        if (notification.ManualEnabled) flags |= 1 << 5;
-        if (notification.HasIdeal) flags |= 1 << 6;
-        if (notification.RotateParamsValid) flags |= 1 << 7;
-        if (notification.UseDetourCorrection) flags |= 1 << 8;
-        return flags;
-    }
-
-    private static void ApplyNotificationFlags(VehicleSyncNotification notification, ushort flags)
-    {
-        notification.PosAvailable = (flags & (1 << 0)) != 0;
-        notification.Aligned = (flags & (1 << 1)) != 0;
-        notification.FleetMotionReleased = (flags & (1 << 2)) != 0;
-        notification.FleetStopActive = (flags & (1 << 3)) != 0;
-        notification.AutoEnabled = (flags & (1 << 4)) != 0;
-        notification.ManualEnabled = (flags & (1 << 5)) != 0;
-        notification.HasIdeal = (flags & (1 << 6)) != 0;
-        notification.RotateParamsValid = (flags & (1 << 7)) != 0;
-        notification.UseDetourCorrection = (flags & (1 << 8)) != 0;
-    }
-
-    private static void WriteString(BinaryWriter writer, string value)
-    {
-        var bytes = Encoding.UTF8.GetBytes(value ?? "");
-        if (bytes.Length > ushort.MaxValue)
-            throw new InvalidOperationException($"String payload length {bytes.Length} exceeds binary protocol limit.");
-        writer.Write((ushort)bytes.Length);
-        writer.Write(bytes);
-    }
-
-    private static string ReadString(BinaryReader reader)
-    {
-        var length = reader.ReadUInt16();
-        var bytes = reader.ReadBytes(length);
-        if (bytes.Length != length)
-            throw new EndOfStreamException("Truncated multi-vehicle sync string payload.");
-        return Encoding.UTF8.GetString(bytes);
-    }
-
-    private static void EnsureFullyRead(MemoryStream stream)
-    {
-        if (stream.Position != stream.Length)
-            throw new InvalidDataException("Unexpected trailing bytes in multi-vehicle sync packet.");
-    }
-}

ClumsyPilot/VehicleSyncModels.cs

@@ -1,75 +0,0 @@
-using System.Collections.Generic;
-using ClumsyCore;
-using Newtonsoft.Json;
-
-namespace MultiWheelC;
-
-public class VehicleSyncInfo
-{
-    [JsonProperty("Master")] public bool Master { get; set; }
-    [JsonProperty("Ip")] public string Ip { get; set; } = "";
-    [JsonProperty("Port")] public int Port { get; set; } = 8008;
-    [JsonProperty("PosAvailable")] public bool PosAvailable { get; set; }
-    [JsonProperty("X")] public float X { get; set; }
-    [JsonProperty("Y")] public float Y { get; set; }
-    [JsonProperty("Th")] public float Th { get; set; }
-    [JsonProperty("LayoutX")] public float LayoutX { get; set; }
-    [JsonProperty("LayoutY")] public float LayoutY { get; set; }
-    [JsonProperty("LayoutTh")] public float LayoutTh { get; set; }
-    [JsonProperty("Aligned")] public bool Aligned { get; set; }
-    // 本车本轮是否成功识别到邻车（关闭互识别时恒为 true）。任一车为 false 则整队停车。
-    [JsonProperty("DetectOk")] public bool DetectOk { get; set; }
-    [JsonProperty("MotionFeasible")] public bool MotionFeasible { get; set; } = true;
-    [JsonProperty("MotionInfeasibleReason")] public string MotionInfeasibleReason { get; set; } = "";
-    [JsonProperty("RotateWheelsAligned")] public bool RotateWheelsAligned { get; set; } = true;
-    [JsonProperty("RotateWheelAlignDetail")] public string RotateWheelAlignDetail { get; set; } = "";
-    [JsonProperty("AppliedNotificationSeq")] public long AppliedNotificationSeq { get; set; } = -1;
-}
-
-public class VehicleSyncNotification
-{
-    [JsonProperty("PosAvailable")] public bool PosAvailable { get; set; }
-    [JsonProperty("CenterX")] public float CenterX { get; set; }
-    [JsonProperty("CenterY")] public float CenterY { get; set; }
-    [JsonProperty("CenterTh")] public float CenterTh { get; set; }
-    [JsonProperty("Aligned")] public bool Aligned { get; set; }
-    [JsonProperty("Fleet")] public Dictionary<int, VehicleSyncInfo> Fleet { get; set; } = new();
-    [JsonProperty("FleetVx")] public float FleetVx { get; set; }
-    [JsonProperty("FleetFrontTh")] public float FleetFrontTh { get; set; }
-    [JsonProperty("FleetRearTh")] public float FleetRearTh { get; set; }
-    // 联动运动模式：0=常规(前进+转向) 1=蟹行(四轮同向平移) 2=原地旋转(绕车队中心)
-    [JsonProperty("Mode")] public int Mode { get; set; }
-    // 原地旋转角速度(deg/s，逆时针为正)，仅 Mode==2 有效
-    [JsonProperty("FleetOmega")] public float FleetOmega { get; set; }
-    [JsonProperty("RequestedFleetOmega")] public float RequestedFleetOmega { get; set; }
-    [JsonProperty("FleetMotionReleased")] public bool FleetMotionReleased { get; set; } = true;
-    [JsonProperty("FleetStopActive")] public bool FleetStopActive { get; set; }
-    [JsonProperty("FleetStopReason")] public string FleetStopReason { get; set; } = "";
-    [JsonProperty("FleetStopSourceCar")] public int FleetStopSourceCar { get; set; }
-    [JsonProperty("AutoEnabled")] public bool AutoEnabled { get; set; }
-    [JsonProperty("ManualEnabled")] public bool ManualEnabled { get; set; }
-    [JsonProperty("UseDetourCorrection")] public bool UseDetourCorrection { get; set; }
-    [JsonProperty("SyncTh")] public float SyncTh { get; set; }
-    [JsonProperty("SyncDistance")] public float SyncDistance { get; set; }
-    [JsonProperty("DeltaDetectCenter")] public float DeltaDetectCenter { get; set; }
-    // 原地旋转纠偏参数由主车广播，从车运行时使用同一套增益/限幅，避免主从补偿强度不一致。
-    [JsonProperty("RotateParamsValid")] public bool RotateParamsValid { get; set; }
-    [JsonProperty("RotateActiveOmega")] public float RotateActiveOmega { get; set; }
-    [JsonProperty("RotateCompXyFac")] public float RotateCompXyFac { get; set; }
-    [JsonProperty("RotateCompXyIFac")] public float RotateCompXyIFac { get; set; }
-    [JsonProperty("RotateCompXyMax")] public float RotateCompXyMax { get; set; }
-    [JsonProperty("RotateCompThFac")] public float RotateCompThFac { get; set; }
-    [JsonProperty("RotateCompThIFac")] public float RotateCompThIFac { get; set; }
-    [JsonProperty("RotateCompThMax")] public float RotateCompThMax { get; set; }
-    [JsonProperty("RotateCompTangentFrac")] public float RotateCompTangentFrac { get; set; }
-    [JsonProperty("RotateStartWheelAlignDeg")] public float RotateStartWheelAlignDeg { get; set; }
-    [JsonProperty("RotateActiveWheelAlignDeg")] public float RotateActiveWheelAlignDeg { get; set; }
-    // F: 单调递增序列号，从车据此丢弃乱序到达的旧 notify 包。
-    [JsonProperty("Seq")] public long Seq { get; set; }
-    // D: 自动模式下主车路径控制器算出的车队中心理想位姿（世界系），由 idealPos/idealAngle 透传而来。
-    // HasIdeal=true 时各从车按各自 layout 推算 per-car 目标位姿做前馈+补偿，弧线路径不再只靠事后纠偏。
-    [JsonProperty("HasIdeal")] public bool HasIdeal { get; set; }
-    [JsonProperty("IdealX")] public float IdealX { get; set; }
-    [JsonProperty("IdealY")] public float IdealY { get; set; }
-    [JsonProperty("IdealTh")] public float IdealTh { get; set; }
-}

ClumsyPilot/build/Clumsy/ClumsyPilot.deps.json

@@ -0,0 +1,163 @@
+{
+  "runtimeTarget": {
+    "name": ".NETStandard,Version=v2.0/",
+    "signature": ""
+  },
+  "compilationOptions": {},
+  "targets": {
+    ".NETStandard,Version=v2.0": {},
+    ".NETStandard,Version=v2.0/": {
+      "ClumsyPilot/1.0.0": {
+        "dependencies": {
+          "NETStandard.Library": "2.0.3",
+          "Newtonsoft.Json": "13.0.3",
+          "System.Numerics.Vectors": "4.6.1",
+          "CommonUsage": "1.0.0.0",
+          "LessokajiWeaverUtilities": "1.0.0.0",
+          "MDCSToolBox": "1.0.0.0",
+          "RefClumsyCore": "0.0.0.0",
+          "RefClumsyDance": "0.0.0.0",
+          "RefFundamentalLib": "0.0.0.0"
+        },
+        "runtime": {
+          "ClumsyPilot.dll": {}
+        }
+      },
+      "Microsoft.NETCore.Platforms/1.1.0": {},
+      "NETStandard.Library/2.0.3": {
+        "dependencies": {
+          "Microsoft.NETCore.Platforms": "1.1.0"
+        }
+      },
+      "Newtonsoft.Json/13.0.3": {
+        "runtime": {
+          "lib/netstandard2.0/Newtonsoft.Json.dll": {
+            "assemblyVersion": "13.0.0.0",
+            "fileVersion": "13.0.3.27908"
+          }
+        }
+      },
+      "System.Numerics.Vectors/4.6.1": {
+        "runtime": {
+          "lib/netstandard2.0/System.Numerics.Vectors.dll": {
+            "assemblyVersion": "4.1.3.0",
+            "fileVersion": "4.600.125.16908"
+          }
+        }
+      },
+      "CommonUsage/1.0.0.0": {
+        "runtime": {
+          "CommonUsage.dll": {
+            "assemblyVersion": "1.0.0.0",
+            "fileVersion": "1.0.0.0"
+          }
+        }
+      },
+      "LessokajiWeaverUtilities/1.0.0.0": {
+        "runtime": {
+          "LessokajiWeaverUtilities.dll": {
+            "assemblyVersion": "1.0.0.0",
+            "fileVersion": "1.0.0.0"
+          }
+        }
+      },
+      "MDCSToolBox/1.0.0.0": {
+        "runtime": {
+          "MDCSToolBox.dll": {
+            "assemblyVersion": "1.0.0.0",
+            "fileVersion": "1.0.0.0"
+          }
+        }
+      },
+      "RefClumsyCore/0.0.0.0": {
+        "runtime": {
+          "RefClumsyCore.dll": {
+            "assemblyVersion": "0.0.0.0",
+            "fileVersion": "0.0.0.0"
+          }
+        }
+      },
+      "RefClumsyDance/0.0.0.0": {
+        "runtime": {
+          "RefClumsyDance.dll": {
+            "assemblyVersion": "0.0.0.0",
+            "fileVersion": "0.0.0.0"
+          }
+        }
+      },
+      "RefFundamentalLib/0.0.0.0": {
+        "runtime": {
+          "RefFundamentalLib.dll": {
+            "assemblyVersion": "0.0.0.0",
+            "fileVersion": "0.0.0.0"
+          }
+        }
+      }
+    }
+  },
+  "libraries": {
+    "ClumsyPilot/1.0.0": {
+      "type": "project",
+      "serviceable": false,
+      "sha512": ""
+    },
+    "Microsoft.NETCore.Platforms/1.1.0": {
+      "type": "package",
+      "serviceable": true,
+      "sha512": "sha512-kz0PEW2lhqygehI/d6XsPCQzD7ff7gUJaVGPVETX611eadGsA3A877GdSlU0LRVMCTH/+P3o2iDTak+S08V2+A==",
+      "path": "microsoft.netcore.platforms/1.1.0",
+      "hashPath": "microsoft.netcore.platforms.1.1.0.nupkg.sha512"
+    },
+    "NETStandard.Library/2.0.3": {
+      "type": "package",
+      "serviceable": true,
+      "sha512": "sha512-st47PosZSHrjECdjeIzZQbzivYBJFv6P2nv4cj2ypdI204DO+vZ7l5raGMiX4eXMJ53RfOIg+/s4DHVZ54Nu2A==",
+      "path": "netstandard.library/2.0.3",
+      "hashPath": "netstandard.library.2.0.3.nupkg.sha512"
+    },
+    "Newtonsoft.Json/13.0.3": {
+      "type": "package",
+      "serviceable": true,
+      "sha512": "sha512-HrC5BXdl00IP9zeV+0Z848QWPAoCr9P3bDEZguI+gkLcBKAOxix/tLEAAHC+UvDNPv4a2d18lOReHMOagPa+zQ==",
+      "path": "newtonsoft.json/13.0.3",
+      "hashPath": "newtonsoft.json.13.0.3.nupkg.sha512"
+    },
+    "System.Numerics.Vectors/4.6.1": {
+      "type": "package",
+      "serviceable": true,
+      "sha512": "sha512-sQxefTnhagrhoq2ReR0D/6K0zJcr9Hrd6kikeXsA1I8kOCboTavcUC4r7TSfpKFeE163uMuxZcyfO1mGO3EN8Q==",
+      "path": "system.numerics.vectors/4.6.1",
+      "hashPath": "system.numerics.vectors.4.6.1.nupkg.sha512"
+    },
+    "CommonUsage/1.0.0.0": {
+      "type": "reference",
+      "serviceable": false,
+      "sha512": ""
+    },
+    "LessokajiWeaverUtilities/1.0.0.0": {
+      "type": "reference",
+      "serviceable": false,
+      "sha512": ""
+    },
+    "MDCSToolBox/1.0.0.0": {
+      "type": "reference",
+      "serviceable": false,
+      "sha512": ""
+    },
+    "RefClumsyCore/0.0.0.0": {
+      "type": "reference",
+      "serviceable": false,
+      "sha512": ""
+    },
+    "RefClumsyDance/0.0.0.0": {
+      "type": "reference",
+      "serviceable": false,
+      "sha512": ""
+    },
+    "RefFundamentalLib/0.0.0.0": {
+      "type": "reference",
+      "serviceable": false,
+      "sha512": ""
+    }
+  }
+}

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

@@ -1,23 +1,23 @@
 {
   "format": 1,
   "restore": {
-    "D:\\@FariyLandTask\\@FRLD-GitProject\\ParkingRobot\\ClumsyPilot\\ClumsyPilot.csproj": {}
+    "D:\\Users\\Desktop\\入职培训\\停车机器人\\MyParking\\ClumsyPilot\\ClumsyPilot.csproj": {}
   },
   "projects": {
-    "D:\\@FariyLandTask\\@FRLD-GitProject\\ParkingRobot\\ClumsyPilot\\ClumsyPilot.csproj": {
+    "D:\\Users\\Desktop\\入职培训\\停车机器人\\MyParking\\ClumsyPilot\\ClumsyPilot.csproj": {
       "version": "1.0.0",
       "restore": {
-        "projectUniqueName": "D:\\@FariyLandTask\\@FRLD-GitProject\\ParkingRobot\\ClumsyPilot\\ClumsyPilot.csproj",
+        "projectUniqueName": "D:\\Users\\Desktop\\入职培训\\停车机器人\\MyParking\\ClumsyPilot\\ClumsyPilot.csproj",
         "projectName": "ClumsyPilot",
-        "projectPath": "D:\\@FariyLandTask\\@FRLD-GitProject\\ParkingRobot\\ClumsyPilot\\ClumsyPilot.csproj",
+        "projectPath": "D:\\Users\\Desktop\\入职培训\\停车机器人\\MyParking\\ClumsyPilot\\ClumsyPilot.csproj",
-        "packagesPath": "C:\\Users\\Fairyland\\.nuget\\packages\\",
+        "packagesPath": "C:\\Users\\admin\\.nuget\\packages\\",
-        "outputPath": "D:\\@FariyLandTask\\@FRLD-GitProject\\ParkingRobot\\ClumsyPilot\\obj\\",
+        "outputPath": "D:\\Users\\Desktop\\入职培训\\停车机器人\\MyParking\\ClumsyPilot\\obj\\",
         "projectStyle": "PackageReference",
         "fallbackFolders": [
-          "D:\\VS2022\\Shared\\NuGetPackages"
+          "C:\\Program Files (x86)\\Microsoft Visual Studio\\Shared\\NuGetPackages"
         ],
         "configFilePaths": [
-          "C:\\Users\\Fairyland\\AppData\\Roaming\\NuGet\\NuGet.Config",
+          "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"
         ],
@@ -58,7 +58,7 @@
             },
             "Newtonsoft.Json": {
               "target": "Package",
-              "version": "[13.0.4, )"
+              "version": "[13.0.3, )"
             },
             "System.Numerics.Vectors": {
               "target": "Package",
@@ -76,7 +76,7 @@
           ],
           "assetTargetFallback": true,
           "warn": true,
-          "runtimeIdentifierGraphPath": "C:\\Program Files\\dotnet\\sdk\\9.0.308\\RuntimeIdentifierGraph.json"
+          "runtimeIdentifierGraphPath": "C:\\Program Files\\dotnet\\sdk\\9.0.316\\RuntimeIdentifierGraph.json"
         }
       }
     }

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

@@ -5,12 +5,12 @@
     <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\Fairyland\.nuget\packages\;D:\VS2022\Shared\NuGetPackages</NuGetPackageFolders>
+    <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.2</NuGetToolVersion>
+    <NuGetToolVersion Condition=" '$(NuGetToolVersion)' == '' ">6.14.3</NuGetToolVersion>
   </PropertyGroup>
   <ItemGroup Condition=" '$(ExcludeRestorePackageImports)' != 'true' ">
-    <SourceRoot Include="C:\Users\Fairyland\.nuget\packages\" />
+    <SourceRoot Include="C:\Users\admin\.nuget\packages\" />
-    <SourceRoot Include="D:\VS2022\Shared\NuGetPackages\" />
+    <SourceRoot Include="C:\Program Files (x86)\Microsoft Visual Studio\Shared\NuGetPackages\" />
   </ItemGroup>
 </Project>

ClumsyPilot/obj/Debug/ClumsyPilot.AssemblyInfo.cs

@@ -1,10 +1,9 @@
 //------------------------------------------------------------------------------
 // <auto-generated>
-//     此代码由工具生成。
+//     This code was generated by a tool.
-//     运行时版本:4.0.30319.42000
 //
-//     对此文件的更改可能会导致不正确的行为，并且如果
+//     Changes to this file may cause incorrect behavior and will be lost if
-//     重新生成代码，这些更改将会丢失。
+//     the code is regenerated.
 // </auto-generated>
 //------------------------------------------------------------------------------
 

ClumsyPilot/obj/Debug/ClumsyPilot.GeneratedMSBuildEditorConfig.editorconfig

@@ -1,6 +1,6 @@
 is_global = true
-build_property.RootNamespace = ClumsyPilot
+build_property.RootNamespace = MultiWheelC
-build_property.ProjectDir = D:\@FariyLandTask\@FRLD-GitProject\ParkingRobot\ClumsyPilot\
+build_property.ProjectDir = D:\Users\Desktop\入职培训\停车机器人\MyParking\ClumsyPilot\
 build_property.EnableComHosting = 
 build_property.EnableGeneratedComInterfaceComImportInterop = 
 build_property.CsWinRTUseWindowsUIXamlProjections = false

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

@@ -1 +1 @@
-622a214d5f25f6580d0c98fb28e188cba923d74f280b70d75d03c4009c18c55b
+e972a413d047c4137a8ce86cbff54a8d2e2558806d9d974d3d6312467ee8ba4d

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

@@ -1,11 +1,34 @@
-D:\@FariyLandTask\@FRLD-GitProject\build\Clumsy\ClumsyPilot.deps.json
+D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\build\Clumsy\ClumsyPilot.deps.json
-D:\@FariyLandTask\@FRLD-GitProject\build\Clumsy\ClumsyPilot.dll
+D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\build\Clumsy\ClumsyPilot.dll
-D:\@FariyLandTask\@FRLD-GitProject\build\Clumsy\ClumsyPilot.pdb
+D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\build\Clumsy\ClumsyPilot.pdb
-D:\@FariyLandTask\@FRLD-GitProject\ParkingRobot\ClumsyPilot\obj\Debug\ClumsyPilot.csproj.AssemblyReference.cache
+D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\build\Clumsy\CommonUsage.dll
-D:\@FariyLandTask\@FRLD-GitProject\ParkingRobot\ClumsyPilot\obj\Debug\ClumsyPilot.GeneratedMSBuildEditorConfig.editorconfig
+D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\build\Clumsy\LessokajiWeaverUtilities.dll
-D:\@FariyLandTask\@FRLD-GitProject\ParkingRobot\ClumsyPilot\obj\Debug\ClumsyPilot.AssemblyInfoInputs.cache
+D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\build\Clumsy\MDCSToolBox.dll
-D:\@FariyLandTask\@FRLD-GitProject\ParkingRobot\ClumsyPilot\obj\Debug\ClumsyPilot.AssemblyInfo.cs
+D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\build\Clumsy\RefClumsyCore.dll
-D:\@FariyLandTask\@FRLD-GitProject\ParkingRobot\ClumsyPilot\obj\Debug\ClumsyPilot.csproj.CoreCompileInputs.cache
+D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\build\Clumsy\RefClumsyDance.dll
-D:\@FariyLandTask\@FRLD-GitProject\ParkingRobot\ClumsyPilot\obj\Debug\ClumsyPi.5EF10E9F.Up2Date
+D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\build\Clumsy\RefFundamentalLib.dll
-D:\@FariyLandTask\@FRLD-GitProject\ParkingRobot\ClumsyPilot\obj\Debug\ClumsyPilot.dll
+D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\obj\Debug\ClumsyPilot.csproj.AssemblyReference.cache
-D:\@FariyLandTask\@FRLD-GitProject\ParkingRobot\ClumsyPilot\obj\Debug\ClumsyPilot.pdb
+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/Debug/netstandard2.0/.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/netstandard2.0/ClumsyPilot.AssemblyInfo.cs

@@ -1,23 +0,0 @@
-//------------------------------------------------------------------------------
-// <auto-generated>
-//     此代码由工具生成。
-//     运行时版本:4.0.30319.42000
-//
-//     对此文件的更改可能会导致不正确的行为，并且如果
-//     重新生成代码，这些更改将会丢失。
-// </auto-generated>
-//------------------------------------------------------------------------------
-
-using System;
-using System.Reflection;
-
-[assembly: System.Reflection.AssemblyCompanyAttribute("ClumsyPilot")]
-[assembly: System.Reflection.AssemblyConfigurationAttribute("Debug")]
-[assembly: System.Reflection.AssemblyFileVersionAttribute("1.0.0.0")]
-[assembly: System.Reflection.AssemblyInformationalVersionAttribute("1.0.0")]
-[assembly: System.Reflection.AssemblyProductAttribute("ClumsyPilot")]
-[assembly: System.Reflection.AssemblyTitleAttribute("ClumsyPilot")]
-[assembly: System.Reflection.AssemblyVersionAttribute("1.0.0.0")]
-
-// 由 MSBuild WriteCodeFragment 类生成。
-

ClumsyPilot/obj/Debug/netstandard2.0/ClumsyPilot.AssemblyInfoInputs.cache

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

ClumsyPilot/obj/Debug/netstandard2.0/ClumsyPilot.GeneratedMSBuildEditorConfig.editorconfig

@@ -1,8 +0,0 @@
-is_global = true
-build_property.RootNamespace = ClumsyPilot
-build_property.ProjectDir = D:\@FariyLandTask\@FRLD-GitProject\ParkingRobot\ClumsyPilot\
-build_property.EnableComHosting = 
-build_property.EnableGeneratedComInterfaceComImportInterop = 
-build_property.CsWinRTUseWindowsUIXamlProjections = false
-build_property.EffectiveAnalysisLevelStyle = 
-build_property.EnableCodeStyleSeverity = 

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

@@ -1 +0,0 @@
-66346c5ecc38869ab6b6f447e1f54c24aba69f81ee10b5736dee140cd5a474b8

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

@@ -1,5 +0,0 @@
-D:\@FariyLandTask\@FRLD-GitProject\ParkingRobot\ClumsyPilot\obj\Debug\netstandard2.0\ClumsyPilot.csproj.AssemblyReference.cache
-D:\@FariyLandTask\@FRLD-GitProject\ParkingRobot\ClumsyPilot\obj\Debug\netstandard2.0\ClumsyPilot.GeneratedMSBuildEditorConfig.editorconfig
-D:\@FariyLandTask\@FRLD-GitProject\ParkingRobot\ClumsyPilot\obj\Debug\netstandard2.0\ClumsyPilot.AssemblyInfoInputs.cache
-D:\@FariyLandTask\@FRLD-GitProject\ParkingRobot\ClumsyPilot\obj\Debug\netstandard2.0\ClumsyPilot.AssemblyInfo.cs
-D:\@FariyLandTask\@FRLD-GitProject\ParkingRobot\ClumsyPilot\obj\Debug\netstandard2.0\ClumsyPilot.csproj.CoreCompileInputs.cache

ClumsyPilot/obj/project.assets.json

@@ -26,7 +26,7 @@
           "build/netstandard2.0/NETStandard.Library.targets": {}
         }
       },
-      "Newtonsoft.Json/13.0.4": {
+      "Newtonsoft.Json/13.0.3": {
         "type": "package",
         "compile": {
           "lib/netstandard2.0/Newtonsoft.Json.dll": {
@@ -199,10 +199,10 @@
         "netstandard.library.nuspec"
       ]
     },
-    "Newtonsoft.Json/13.0.4": {
+    "Newtonsoft.Json/13.0.3": {
-      "sha512": "pdgNNMai3zv51W5aq268sujXUyx7SNdE2bj1wZcWjAQrKMFZV260lbqYop1d2GM67JI1huLRwxo9ZqnfF/lC6A==",
+      "sha512": "HrC5BXdl00IP9zeV+0Z848QWPAoCr9P3bDEZguI+gkLcBKAOxix/tLEAAHC+UvDNPv4a2d18lOReHMOagPa+zQ==",
       "type": "package",
-      "path": "newtonsoft.json/13.0.4",
+      "path": "newtonsoft.json/13.0.3",
       "files": [
         ".nupkg.metadata",
         ".signature.p7s",
@@ -224,7 +224,7 @@
         "lib/netstandard1.3/Newtonsoft.Json.xml",
         "lib/netstandard2.0/Newtonsoft.Json.dll",
         "lib/netstandard2.0/Newtonsoft.Json.xml",
-        "newtonsoft.json.13.0.4.nupkg.sha512",
+        "newtonsoft.json.13.0.3.nupkg.sha512",
         "newtonsoft.json.nuspec",
         "packageIcon.png"
       ]
@@ -254,28 +254,28 @@
   "projectFileDependencyGroups": {
     ".NETStandard,Version=v2.0": [
       "NETStandard.Library >= 2.0.3",
-      "Newtonsoft.Json >= 13.0.4",
+      "Newtonsoft.Json >= 13.0.3",
       "System.Numerics.Vectors >= 4.6.1"
     ]
   },
   "packageFolders": {
-    "C:\\Users\\Fairyland\\.nuget\\packages\\": {},
+    "C:\\Users\\admin\\.nuget\\packages\\": {},
-    "D:\\VS2022\\Shared\\NuGetPackages": {}
+    "C:\\Program Files (x86)\\Microsoft Visual Studio\\Shared\\NuGetPackages": {}
   },
   "project": {
     "version": "1.0.0",
     "restore": {
-      "projectUniqueName": "D:\\@FariyLandTask\\@FRLD-GitProject\\ParkingRobot\\ClumsyPilot\\ClumsyPilot.csproj",
+      "projectUniqueName": "D:\\Users\\Desktop\\入职培训\\停车机器人\\MyParking\\ClumsyPilot\\ClumsyPilot.csproj",
       "projectName": "ClumsyPilot",
-      "projectPath": "D:\\@FariyLandTask\\@FRLD-GitProject\\ParkingRobot\\ClumsyPilot\\ClumsyPilot.csproj",
+      "projectPath": "D:\\Users\\Desktop\\入职培训\\停车机器人\\MyParking\\ClumsyPilot\\ClumsyPilot.csproj",
-      "packagesPath": "C:\\Users\\Fairyland\\.nuget\\packages\\",
+      "packagesPath": "C:\\Users\\admin\\.nuget\\packages\\",
-      "outputPath": "D:\\@FariyLandTask\\@FRLD-GitProject\\ParkingRobot\\ClumsyPilot\\obj\\",
+      "outputPath": "D:\\Users\\Desktop\\入职培训\\停车机器人\\MyParking\\ClumsyPilot\\obj\\",
       "projectStyle": "PackageReference",
       "fallbackFolders": [
-        "D:\\VS2022\\Shared\\NuGetPackages"
+        "C:\\Program Files (x86)\\Microsoft Visual Studio\\Shared\\NuGetPackages"
       ],
       "configFilePaths": [
-        "C:\\Users\\Fairyland\\AppData\\Roaming\\NuGet\\NuGet.Config",
+        "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"
       ],
@@ -316,7 +316,7 @@
           },
           "Newtonsoft.Json": {
             "target": "Package",
-            "version": "[13.0.4, )"
+            "version": "[13.0.3, )"
           },
           "System.Numerics.Vectors": {
             "target": "Package",
@@ -334,7 +334,7 @@
         ],
         "assetTargetFallback": true,
         "warn": true,
-        "runtimeIdentifierGraphPath": "C:\\Program Files\\dotnet\\sdk\\9.0.308\\RuntimeIdentifierGraph.json"
+        "runtimeIdentifierGraphPath": "C:\\Program Files\\dotnet\\sdk\\9.0.316\\RuntimeIdentifierGraph.json"
       }
     }
   }

ClumsyPilot/obj/project.nuget.cache

@@ -1,13 +1,13 @@
 {
   "version": 2,
-  "dgSpecHash": "gHGYwM2i+So=",
+  "dgSpecHash": "YBvImiCcgSo=",
   "success": true,
-  "projectFilePath": "D:\\@FariyLandTask\\@FRLD-GitProject\\ParkingRobot\\ClumsyPilot\\ClumsyPilot.csproj",
+  "projectFilePath": "D:\\Users\\Desktop\\入职培训\\停车机器人\\MyParking\\ClumsyPilot\\ClumsyPilot.csproj",
   "expectedPackageFiles": [
-    "C:\\Users\\Fairyland\\.nuget\\packages\\microsoft.netcore.platforms\\1.1.0\\microsoft.netcore.platforms.1.1.0.nupkg.sha512",
+    "C:\\Users\\admin\\.nuget\\packages\\microsoft.netcore.platforms\\1.1.0\\microsoft.netcore.platforms.1.1.0.nupkg.sha512",
-    "C:\\Users\\Fairyland\\.nuget\\packages\\netstandard.library\\2.0.3\\netstandard.library.2.0.3.nupkg.sha512",
+    "C:\\Users\\admin\\.nuget\\packages\\netstandard.library\\2.0.3\\netstandard.library.2.0.3.nupkg.sha512",
-    "C:\\Users\\Fairyland\\.nuget\\packages\\newtonsoft.json\\13.0.4\\newtonsoft.json.13.0.4.nupkg.sha512",
+    "C:\\Users\\admin\\.nuget\\packages\\newtonsoft.json\\13.0.3\\newtonsoft.json.13.0.3.nupkg.sha512",
-    "C:\\Users\\Fairyland\\.nuget\\packages\\system.numerics.vectors\\4.6.1\\system.numerics.vectors.4.6.1.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,48 @@
+<Project Sdk="Microsoft.NET.Sdk">
+
+  <PropertyGroup>
+    <TargetFramework>netstandard2.0</TargetFramework>
+  </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>..\..\MedullaAdapter\ref\RefFundamentalLib.dll</HintPath>
+    </Reference>
+    <Reference Include="ClumsyCore">
+      <HintPath>..\..\ClumsyPilot\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 ∞)  
+    }
+}