commit d9432bd52961473bb882832c3bdbf786e8c19033 Author: shenyuxiang Date: Tue Aug 4 10:29:21 2026 +0800 Initial commit from MyParking project diff --git a/.codex/skills/commit/SKILL.md b/.codex/skills/commit/SKILL.md new file mode 100644 index 0000000..e78e37b --- /dev/null +++ b/.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` 强推，交给用户决定 +- 如果改动很大很杂，主动提示用户考虑拆分提交，但不强制 diff --git a/.codex/skills/readme/SKILL.md b/.codex/skills/readme/SKILL.md new file mode 100644 index 0000000..917b6c1 --- /dev/null +++ b/.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 \ No newline at end of file diff --git a/.cursor/rules/karpathy-guidelines.mdc b/.cursor/rules/karpathy-guidelines.mdc new file mode 100644 index 0000000..edd317f --- /dev/null +++ b/.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. diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..556a47c --- /dev/null +++ b/.gitignore @@ -0,0 +1,93 @@ +################################################## +# Visual Studio +################################################## + +# Visual Studio 工作区缓存 +.vs/ +**/.vs/ + +# 用户配置 +*.user +*.suo +*.userosscache +*.sln.docstates + +################################################## +# Build 输出 +################################################## + +# 编译输出目录 +bin/ +obj/ +**/bin/ +**/obj/ + +################################################## +# Rider / VS Code +################################################## + +.idea/ +.vscode/ + +################################################## +# NuGet +################################################## + +*.nupkg +packages/ + +################################################## +# 日志 +################################################## + +*.log + +################################################## +# 临时文件 +################################################## + +*.tmp +*.temp + +################################################## +# 测试结果 +################################################## + +TestResults/ + +################################################## +# 发布目录 +################################################## + +publish/ + +################################################## +# Windows +################################################## + +Thumbs.db +Desktop.ini + +################################################## +# JetBrains +################################################## + +_ReSharper*/ +*.DotSettings.user + +################################################## +# 缓存 +################################################## + +*.cache + +################################################## +# 数据库（如果有） +################################################## + +*.db +*.sqlite +*.sqlite3 + +*.csv +*.png \ No newline at end of file diff --git a/CommonUsage-MultiVehicleSync/commonusage/.gitattributes b/CommonUsage-MultiVehicleSync/commonusage/.gitattributes new file mode 100644 index 0000000..c6c8654 --- /dev/null +++ b/CommonUsage-MultiVehicleSync/commonusage/.gitattributes @@ -0,0 +1,2 @@ +*.cs text eol=crlf +.gitattributes text eol=lf diff --git a/CommonUsage-MultiVehicleSync/commonusage/.gitignore b/CommonUsage-MultiVehicleSync/commonusage/.gitignore new file mode 100644 index 0000000..1ee5385 --- /dev/null +++ b/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 diff --git a/CommonUsage-MultiVehicleSync/commonusage/Chassis/AbstractChassis.cs b/CommonUsage-MultiVehicleSync/commonusage/Chassis/AbstractChassis.cs new file mode 100644 index 0000000..41b0e23 --- /dev/null +++ b/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(); + + /// + /// 当前行进方向。 + /// + [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 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 GeometricControlPoints = new(); + protected bool RotatingActive = false; + protected bool GoingActive = false; + protected bool GoingWheelAligned = false; + + private float _lastDirectionAngle = 0; + } +} diff --git a/CommonUsage-MultiVehicleSync/commonusage/Chassis/DiffSteerWheel.cs b/CommonUsage-MultiVehicleSync/commonusage/Chassis/DiffSteerWheel.cs new file mode 100644 index 0000000..4f179ae --- /dev/null +++ b/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 speedWriter, + Func speedReader, Action angleWriter, Func angleReader, Action leftSpeedWriter, Action 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 _leftSpeedWriter; + private readonly Action _rightSpeedWriter; + + private float _leftSendSpeed; + private float _rightSendSpeed; + } +} diff --git a/CommonUsage-MultiVehicleSync/commonusage/Chassis/DifferentialChassis.cs b/CommonUsage-MultiVehicleSync/commonusage/Chassis/DifferentialChassis.cs new file mode 100644 index 0000000..e6a7279 --- /dev/null +++ b/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; + } +} diff --git a/CommonUsage-MultiVehicleSync/commonusage/Chassis/MultiWheelChassis.cs b/CommonUsage-MultiVehicleSync/commonusage/Chassis/MultiWheelChassis.cs new file mode 100644 index 0000000..1576213 --- /dev/null +++ b/CommonUsage-MultiVehicleSync/commonusage/Chassis/MultiWheelChassis.cs @@ -0,0 +1,1396 @@ +using System; +using System.Collections.Generic; +using System.Linq; +using System.Numerics; +using System.Diagnostics; +using System.Text; +using CommonUsage.Mathematics; +using FundamentalLib; +using System.Drawing; + +namespace CommonUsage.Chassis +{ + public class MultiWheelChassis : AbstractChassis + { + public float ThConsistentThreshold = 1.0f; + + public float ControlPointRadius = 500f; + + // 原地旋转纠偏钳位：SendRotateMotion 中每轮纠偏速度幅值不超过 该比例×本轮旋转切向速度， + // 防止减速末段旋转切向变小时纠偏盖过它、使轮向矢量乱摆（频繁打方向/卡死）。<0 关闭钳位。 + public float RotateCompTangentFrac = 0.5f; + + // 上一次 SendRotateMotion 的舵轮对齐状态：false 表示舵轮未追上目标角(gate=0、车未真正转动)。 + // 供上层做积分抗饱和（卡死时冻结积分）。 + public bool LastRotateAligned { get; private set; } = true; + + public string LastMotionDecomposeFailureReason { get; private set; } = ""; + + // 为true时，在两个等价舵角都可用的情况下优先选择机械转动距离更小的方案。 + // 默认关闭以保持旧项目原有行为，由需要该策略的车型适配器主动开启。 + public bool PreferMinimumSteeringTravel { get; set; } + + public float MinimumTurningAngleForAckermann = 60f; + + + public MultiWheelChassis() : base() + { + + } + + public void AddWheel(SteerWheel wheel) + { + _steerWheels.Add(wheel); + } + + public override void Initialize() + { + _steerWheels = _steerWheels.OrderByDescending(sw => sw.Position.X).ToList(); + _targetSpeeds = new float[_steerWheels.Count].ToList(); + _sendSpeeds = new float[_steerWheels.Count].ToList(); + _tmpSpeeds = new float[_steerWheels.Count].ToList(); + _wheelDirs = Enumerable.Repeat(1, _steerWheels.Count).ToList(); + _sendAngle = new float[_steerWheels.Count].ToList(); + _debugSpeeds = new float[_steerWheels.Count].ToList(); + + if (_steerWheels.Count < 2) + { + Console.WriteLine($"steer wheel num: {_steerWheels.Count}. invalid!"); + Valid = false; + return; + } + + CalculateAxes(); + Valid = true; + } + + public Visualizer MainVisualizer = null; + + public override void Visualize() + { + MainVisualizer?.Clear(); + + if (_rotCenter != Vector2.Zero) + { + MainVisualizer?.DrawText(Color.Chartreuse, $"({_rotCenter.X:f1}, {_rotCenter.Y:f2})", _rotCenter); + } + + for (var i = 0; i < _steerWheels.Count; ++i) + { + var sw = _steerWheels[i]; + if (sw is DiffSteerWheel dsw) + { + var pointing = CommonMath.Transform2D(dsw.Position, dsw.ZeroDirection, new Vector2(200, 0)); + MainVisualizer?.DrawLine(Color.Gray, dsw.Position, pointing); + MainVisualizer?.DrawText(Color.Gray, $"{dsw.GetSendSpeed():f2} {sw.GetSendAngle():f2}", dsw.Position); + + var left = CommonMath.Transform2D(dsw.Position, dsw.GetAngleRelativeToChassis() + 90, new Vector2(dsw.WheelDistance, 0)); + MainVisualizer?.DrawLine(Color.GreenYellow, left, + CommonMath.Transform2D(left, dsw.GetAngleRelativeToChassis(), new Vector2(200, 0)), endArrow: true); + MainVisualizer?.DrawText(Color.GreenYellow, $"{dsw.GetLeftSendSpeed():0.00}", left); + + var right = CommonMath.Transform2D(dsw.Position, dsw.GetAngleRelativeToChassis() - 90, new Vector2(dsw.WheelDistance, 0)); + MainVisualizer?.DrawLine(Color.DeepPink, right, + CommonMath.Transform2D(right, dsw.GetAngleRelativeToChassis(), new Vector2(200, 0)), endArrow: true); + MainVisualizer?.DrawText(Color.DeepPink, $"{dsw.GetRightSendSpeed():0.00}", right); + } + else + { + MainVisualizer?.DrawLine(Color.Green, sw.Position, + CommonMath.Transform2D(sw.Position, sw.GetAngleRelativeToChassis(), new Vector2(200, 0)), endArrow: true); + MainVisualizer?.DrawText(Color.Chartreuse, $"{sw.GetSendSpeed():f2} {sw.GetAngleRelativeToChassis():f2}", sw.Position); + } + + if (_rotCenter != Vector2.Zero) + { + MainVisualizer?.DrawLine(Color.DodgerBlue, sw.Position, _rotCenter); + } + } + } + + public override void AfterDirectionChanged() + { + Hedingben.ToastText($"AfterDirectionChanged", "MultiWheelChassis-AfterDirectionChanged"); + + foreach (var sw in _steerWheels) + { + sw.Position = CommonMath.Transform2D(new Vector2(_originBiasX, _originBiasY), _originBiasTh, + sw.PhysicalPosition); + sw.ZeroDirection = _originBiasTh; + } + + CalculateAxes(); + GoingWheelAligned = false; + } + + protected override void DefineGeometricWheelComputation(float speed) + { + if (!GoingActive) ResetMotionState(); + var targetGcpTheta0 = GeometricControlPoints[0].Theta; + var targetGcpTheta1 = GeometricControlPoints[1].Theta; + var lastGcpTheta0Before = _lastGcpTheta0; + var lastGcpTheta1Before = _lastGcpTheta1; + + void Process(ref float current, float target, int id) + { + current += Math.Sign(target - current) * Math.Min(Math.Abs(target - current), + GcpThetaPerSecond * (float)(DateTime.Now - LastMoveTime).TotalSeconds); + // Hedingben.ToastText($"current:{current:0.00} target:{target:0.00}", $"gcp-accumulation-{id}"); + } + + Process(ref _lastGcpTheta0, GeometricControlPoints[0].Theta, 0); + Process(ref _lastGcpTheta1, GeometricControlPoints[1].Theta, 1); + if ((DateTime.Now - _geometricComputeLastLog).TotalMilliseconds >= 200) + { + _geometricComputeLastLog = DateTime.Now; + DLog.Log( + $"ComputeWheelsGeometrically speed:{speed:F3} " + + $"targetGcp:({targetGcpTheta0:F1},{targetGcpTheta1:F1}) " + + $"smoothBefore:({lastGcpTheta0Before:F1},{lastGcpTheta1Before:F1}) " + + $"smoothAfter:({_lastGcpTheta0:F1},{_lastGcpTheta1:F1}) " + + $"originBias:({_originBiasX:F1},{_originBiasY:F1},{_originBiasTh:F1}) goingActive:{GoingActive}", + "MultiWheelGoDiag"); + } + SendMotion(speed, _lastGcpTheta0, _lastGcpTheta1); + + GoingActive = true; + RotatingActive = false; + XYThActive = false; + } + + private float _lastGcpTheta0; + + private float _lastGcpTheta1; + + /// + /// 转速单位为度/s，逆时针为正。 + /// + /// + public override bool ComputeRotateWheels(float rotSpeed) + { + if (!RotatingActive) ResetMotionState(); + + var feasible = SendRotateMotion(rotSpeed); + + GoingActive = false; + RotatingActive = true; + XYThActive = false; + return feasible; + } + + public override void PredefinedDriveStop() + { + if (!Valid) return; + for (var i = 0; i < _steerWheels.Count; i++) + { + _sendSpeeds[i] = 0; + _debugSpeeds[i] = 0; + if (_steerWheels[i] is DiffSteerWheel dfw) + { + dfw.WriteLeftSpeed(0); + dfw.WriteRightSpeed(0); + } + else + { + _steerWheels[i].WriteSpeed(0); + } + } + GoingActive = false; + RotatingActive = false; + XYThActive = false; + } + + public void RampStop(TimeSpan? deltaTime = null) + { + if (!Valid) return; + + for (var i = 0; i < _steerWheels.Count; i++) + { + _debugSpeeds[i] = 0; + AccumulateSpeed(i, 0, false, Vector2.Zero, deltaTime); + } + + GoingActive = false; + RotatingActive = false; + XYThActive = false; + LastMoveTime = DateTime.Now; + } + + /// + /// 立即清零XYTh驱动轮速度，同时保留已经准备好的舵角目标和轮速方向。 + /// 下一条非零命令仍需重新确认四轮实际舵角到位后才会开放驱动速度。 + /// + public void StopXYThDrivePreserveSteeringState() + { + if (!Valid) return; + + // 只有已完成Prepare/Adopt交接的XYTh模式才能保留状态。 + if (!XYThActive) + { + PredefinedDriveStop(); + return; + } + + for (var i = 0; i < _steerWheels.Count; i++) + { + _targetSpeeds[i] = 0; + _sendSpeeds[i] = 0; + _debugSpeeds[i] = 0; + + if (_steerWheels[i] is DiffSteerWheel diffSteerWheel) + { + diffSteerWheel.WriteLeftSpeed(0); + diffSteerWheel.WriteRightSpeed(0); + } + else + { + _steerWheels[i].WriteSpeed(0); + } + } + + GoingActive = false; + RotatingActive = false; + + // 保留XYThActive、_sendAngle和_wheelDirs，避免重新选择等价舵角； + // 清除到位标记，使下一次推动摇杆时重新核对实际反馈。 + _xyThWheelsAligned = false; + LastMoveTime = DateTime.Now; + LastMotionDecomposeFailureReason = ""; + } + + private struct WheelAngleCandidate + { + public bool Valid; + public float RawAngle; + public float NormalizedAngle; + public float Margin; + public int Direction; + public float Lower; + public float Upper; + } + + private WheelAngleCandidate BuildWheelAngleCandidate(SteerWheel sw, float rawAngle, int direction) + { + var lower = (float)CommonMath.RoundTh(sw.AngleLowerLimit); + var upper = (float)CommonMath.RoundTh(sw.AngleUpperLimit); + while (upper < lower) upper += 360; + + var normalized = (float)CommonMath.RoundTh(rawAngle); + while (normalized < lower) normalized += 360; + while (normalized > upper && normalized - 360 >= lower) normalized -= 360; + + var margin = Math.Min(normalized - lower, upper - normalized); + var marginRequired = Math.Max(0, sw.AngleLimitMarginDeg); + return new WheelAngleCandidate + { + Valid = normalized >= lower && normalized <= upper && margin >= marginRequired, + RawAngle = rawAngle, + NormalizedAngle = normalized, + Margin = margin, + Direction = direction, + Lower = lower, + Upper = upper + }; + } + + private bool TryResolveWheelAngle(int wheelIndex, float targetAngle, string entry, + out float angle, out int direction, out string reason) + { + var sw = _steerWheels[wheelIndex]; + var forward = BuildWheelAngleCandidate(sw, targetAngle, 1); + var reverse = BuildWheelAngleCandidate(sw, targetAngle + 180, -1); + + WheelAngleCandidate? selected = null; + + if (PreferMinimumSteeringTravel && + forward.Valid && + reverse.Valid) + { + var actualAngle = sw.ReadAngle(); + + if (!float.IsNaN(actualAngle) && + !float.IsInfinity(actualAngle)) + { + // 将反馈角映射到与候选角相同的机械限位区间。 + // 这里比较的是受限机械舵角，不能使用圆周最短角度差。 + var actual = BuildWheelAngleCandidate( + sw, + actualAngle, + 1); + var forwardError = Math.Abs( + forward.NormalizedAngle - + actual.NormalizedAngle); + var reverseError = Math.Abs( + reverse.NormalizedAngle - + actual.NormalizedAngle); + + // 迟滞避免两个方案转动量接近时频繁翻转轮速方向。 + const float switchHysteresisDegrees = 5f; + + if (reverseError + + switchHysteresisDegrees < + forwardError) + { + selected = reverse; + } + else if (forwardError + + switchHysteresisDegrees < + reverseError) + { + selected = forward; + } + } + } + + // 未启用最小转舵策略、反馈无效或两个候选差异落在迟滞区时， + // 保持旧版轮速方向，避免破坏其他车型的既有行为。 + if (_wheelDirs != null && wheelIndex < _wheelDirs.Count) + { + if (selected == null && + _wheelDirs[wheelIndex] < 0 && + reverse.Valid) + { + selected = reverse; + } + else if (selected == null && + _wheelDirs[wheelIndex] >= 0 && + forward.Valid) + { + selected = forward; + } + } + + if (selected == null) + { + if (forward.Valid) selected = forward; + else if (reverse.Valid) selected = reverse; + } + + if (selected != null) + { + angle = selected.Value.NormalizedAngle; + direction = selected.Value.Direction; + reason = ""; + return true; + } + + angle = 0; + direction = 0; + var marginRequired = Math.Max(0, sw.AngleLimitMarginDeg); + reason = + $"{entry} infeasible wheel={wheelIndex} target={CommonMath.RoundTh(targetAngle):F1} " + + $"reverse={CommonMath.RoundTh(targetAngle + 180):F1} " + + $"limit=[{forward.Lower:F1},{forward.Upper:F1}] requiredMargin={marginRequired:F1} " + + $"forward(norm={forward.NormalizedAngle:F1},margin={forward.Margin:F1},valid={forward.Valid}) " + + $"reverse(norm={reverse.NormalizedAngle:F1},margin={reverse.Margin:F1},valid={reverse.Valid})"; + return false; + } + + private bool FailMotionDecomposition(string entry, string reason, TimeSpan? deltaTime) + { + LastMotionDecomposeFailureReason = $"{entry}: {reason}"; + Console.WriteLine(LastMotionDecomposeFailureReason); + DLog.Log(LastMotionDecomposeFailureReason, "MultiWheelMotionDecompose"); + Hedingben.ToastText($"Motion decompose failed: {reason}", "MultiWheelChassis-decompose"); + RampStop(deltaTime); + return false; + } + + private Vector2 _rotCenter; + + public Vector2 GetRotCenter() + { + return _rotCenter; + } + + public void SetOriginBias(float x, float y, float th) + { + _originBiasX = x; + _originBiasY = y; + _originBiasTh = th; + + foreach (var sw in _steerWheels) + { + sw.Position = CommonMath.Transform2D(new Vector2(_originBiasX, _originBiasY), _originBiasTh, + sw.PhysicalPosition); + sw.ZeroDirection = _originBiasTh; + } + + CalculateAxes(); + } + + public Visualizer SendMotionVisualizer = null; + + /// + /// frontTh和rearTh是基于当前行进方向，前后轴应该打的角度。 + /// + /// 单位m/s。 + /// 角度制。基于当前行进方向。 + /// 角度制。基于当前行进方向。 + /// 上次下发SendMotion到本次的间隔时间。 + /// 用于多车联动，大于0时生效。 + /// 用于多车联动，localControlRadius大于0时生效。 + /// 用于多车联动，localControlRadius大于0时生效。 + /// 用于多车联动，localControlRadius大于0时生效。 + /// + public bool SendMotion(float speed, float frontTh, float rearTh, TimeSpan? deltaTime = null, + float localControlRadius = 0, float localCompensateX = 0f, float localCompensateY = 0, float localCompensateTh = 0) + { + if (!Valid) return FailMotionDecomposition("SendMotion", "invalid chassis", deltaTime); + LastMotionDecomposeFailureReason = ""; + + var compInputSpeed = speed; + var compInputFrontTh = frontTh; + var compInputRearTh = rearTh; + var compMotionTh = 0f; + var compAlong = 0f; + var compSide = 0f; + var compDSpeed = 0f; + var compDTh = 0f; + + if (localControlRadius > 0) + { + var motionTh = AverageAngle(frontTh, rearTh); + var radMotionTh = motionTh / 180f * Math.PI; + var alongComp = (float)(localCompensateX * Math.Cos(radMotionTh) + + localCompensateY * Math.Sin(radMotionTh)); + var sideComp = (float)(-localCompensateX * Math.Sin(radMotionTh) + + localCompensateY * Math.Cos(radMotionTh)); + var dSpeed = alongComp / 1000f; + speed += dSpeed; + + var speedSign = Math.Abs(compInputSpeed) > 1e-4f ? Math.Sign(compInputSpeed) : 1; + var steerBaseSpeed = Math.Max(Math.Abs(speed), 0.05f); + var dTh = (float)(Math.Atan2(sideComp / 1000f, steerBaseSpeed) / Math.PI * 180f) * speedSign; + compMotionTh = motionTh; + compAlong = alongComp; + compSide = sideComp; + compDSpeed = dSpeed; + compDTh = dTh; + Hedingben.ToastText($"s:{compInputSpeed:F2} d:{dSpeed:F3}", "MultiWheelChassis-dSpeed"); + Hedingben.ToastText($"F:{frontTh:F1} R:{rearTh:F1} d:{dTh:F1}", "MultiWheelChassis-dTh"); + frontTh += dTh; + rearTh += dTh; + + if (Math.Abs(localCompensateX) > 1e-3f || Math.Abs(localCompensateY) > 1e-3f || + Math.Abs(localCompensateTh) > 1e-3f) + DLog.Log( + $"SendMotionComp in speed:{compInputSpeed:F3} fTh:{compInputFrontTh:F1} rTh:{compInputRearTh:F1} " + + $"motionTh:{motionTh:F1} biasTh:{_originBiasTh:F1} comp({localCompensateX:F1},{localCompensateY:F1},{localCompensateTh:F2}) " + + $"along:{alongComp:F1} side:{sideComp:F1} dSpeed:{dSpeed:F3} dTh:{dTh:F1} " + + $"out speed:{speed:F3} fTh:{frontTh:F1} rTh:{rearTh:F1}", + "MultiWheelMotionComp"); + } + + float scaleFactor = 1.0f; + float currentFrontTh = frontTh; + float currentRearTh = rearTh; + bool needAdjust = true; + const float minScaleFactor = 0.1f; + var finalAngles = new List(); + + var axisDiffFlag = false; + var myRotCenter = new Vector2(); + var lastInvalidReason = ""; + + float AverageAngle(float left, float right) + { + var leftRad = left / 180f * Math.PI; + var rightRad = right / 180f * Math.PI; + var x = Math.Cos(leftRad) + Math.Cos(rightRad); + var y = Math.Sin(leftRad) + Math.Sin(rightRad); + if (Math.Abs(x) < 1e-6 && Math.Abs(y) < 1e-6) + return (float)CommonMath.RoundTh(left); + return (float)CommonMath.RoundTh(Math.Atan2(y, x) / Math.PI * 180f); + } + + float Interpolate(float left, float current, float right) + { + if (current <= left) return 0; + if (current >= right) return 1; + return (current - left) / (right - left); + } + + while (needAdjust && scaleFactor > minScaleFactor) + { + needAdjust = false; + var tmpAngles = new List(); + + // ... 现有的插值函数和旋转中心计算代码 ... + var thDiff = CommonMath.ThDiff(currentFrontTh, currentRearTh); + axisDiffFlag = Math.Abs(thDiff) > ThConsistentThreshold; + + var sign = -1; + if (axisDiffFlag) + { + // Vector2 pFront = new(_frontBase, 0), pRear = new(_rearBase, 0), + Vector2 pFront = new(ControlPointRadius, 0), pRear = new(-ControlPointRadius, 0), + normFront = CommonMath.Transform2D(pFront, currentFrontTh + 90, Vector2.UnitX), + normRear = CommonMath.Transform2D(pRear, currentRearTh + 90, Vector2.UnitX); + var (intersect, center) = + CommonMath.TwoLinesIntersection(pFront, normFront, pRear, normRear); + if (!intersect) + return FailMotionDecomposition("SendMotion", + $"front/rear axes do not intersect frontTh={currentFrontTh:F1} rearTh={currentRearTh:F1}", + deltaTime); + myRotCenter = center; + sign = myRotCenter.Y > 1 ? 1 : -1; + + if (localControlRadius > 0 && localCompensateTh != 0) + { + void VisPointing(Color color, Vector2 point, float th, int width) + { + var localFrontPointing = CommonMath.Transform2D(point, th, new Vector2(200, 0)); + SendMotionVisualizer?.DrawLine(color, point, localFrontPointing, width: width); + } + + var localPFront = CommonMath.Transform2D(new Vector2(_originBiasX, _originBiasY), _originBiasTh, + new Vector2(localControlRadius, 0)); + var localPRear = CommonMath.Transform2D(new Vector2(_originBiasX, _originBiasY), _originBiasTh, + new Vector2(-localControlRadius, 0)); + // SendMotionVisualizer?.DrawLine(Color.DarkRed, myRotCenter, localPFront); + // SendMotionVisualizer?.DrawLine(Color.DarkRed, myRotCenter, localPRear); + + // 计算_rotCenter在"localPRear指向localPFront的向量"的左侧还是右侧 + // 使用叉积判断：叉积 > 0 表示在左侧，< 0 表示在右侧 + var vecRearToFront = new Vector2(localPFront.X - localPRear.X, localPFront.Y - localPRear.Y); + var vecRearToCenter = new Vector2(myRotCenter.X - localPRear.X, myRotCenter.Y - localPRear.Y); + var crossProduct = vecRearToFront.X * vecRearToCenter.Y - vecRearToFront.Y * vecRearToCenter.X; + var side = crossProduct > 0 ? 1 : -1; + + var localFrontTh = + (float)(Math.Atan2(localPFront.Y - myRotCenter.Y, localPFront.X - myRotCenter.X) / Math.PI * + 180) + 90 * side; + var localRearTh = + (float)(Math.Atan2(localPRear.Y - myRotCenter.Y, localPRear.X - myRotCenter.X) / Math.PI * + 180) + 90 * side; + var sameDirectionError = + Math.Abs(CommonMath.ThDiff(localFrontTh, currentFrontTh)) + + Math.Abs(CommonMath.ThDiff(localRearTh, currentRearTh)); + var reverseDirectionError = + Math.Abs(CommonMath.ThDiff(localFrontTh + 180, currentFrontTh)) + + Math.Abs(CommonMath.ThDiff(localRearTh + 180, currentRearTh)); + if (reverseDirectionError < sameDirectionError) + { + localFrontTh = (float)CommonMath.RoundTh(localFrontTh + 180); + localRearTh = (float)CommonMath.RoundTh(localRearTh + 180); + } + // VisPointing(Color.DarkRed, localPFront, localFrontTh, 1); + // VisPointing(Color.DarkRed, localPRear, localRearTh, 1); + + var newLocalFrontTh = localFrontTh + localCompensateTh * Math.Sign(speed); + var newLocalRearTh = localRearTh - localCompensateTh * Math.Sign(speed); + // VisPointing(Color.Red, localPFront, newLocalFrontTh, 2); + // VisPointing(Color.Red, localPRear, newLocalRearTh, 2); + + var localThDiff = CommonMath.ThDiff(newLocalFrontTh, newLocalRearTh); + axisDiffFlag = Math.Abs(localThDiff) > ThConsistentThreshold; + + if (axisDiffFlag) + { + var localNormFront = CommonMath.Transform2D(localPFront, newLocalFrontTh + 90, Vector2.UnitX); + var localNormRear = CommonMath.Transform2D(localPRear, newLocalRearTh + 90, Vector2.UnitX); + var (localIntersect, localCenter) = + CommonMath.TwoLinesIntersection(localPFront, localNormFront, localPRear, localNormRear); + if (!localIntersect) + return FailMotionDecomposition("SendMotion", + $"compensated axes do not intersect frontTh={newLocalFrontTh:F1} rearTh={newLocalRearTh:F1}", + deltaTime); + myRotCenter = localCenter; + sign = myRotCenter.Y > 1 ? 1 : -1; + + // SendMotionVisualizer?.DrawLine(Color.Red, myRotCenter, localPFront, width: 2); + // SendMotionVisualizer?.DrawLine(Color.Red, myRotCenter, localPRear, width: 2); + } + else + { + currentFrontTh = newLocalFrontTh; + currentRearTh = newLocalRearTh; + thDiff = CommonMath.ThDiff(currentFrontTh, currentRearTh); + } + } + } + + _rotCenter = myRotCenter; + Hedingben.ToastText($"fTh:{frontTh:0.0}, rTh:{rearTh:0.0}, DA:{_originBiasTh:0.00}, " + + $"rc:({_rotCenter.X:0.0},{_rotCenter.Y:0.0}), _rb:{_rearBase}, _fb:{_frontBase}", "rotCenter"); + var r0 = _rotCenter.Length(); + + // 计算所有轮子的角度 + bool anyWheelInvalid = false; + for (var i = 0; i < _steerWheels.Count; i++) + { + var sw = _steerWheels[i]; + float th; + + if (axisDiffFlag) + { + var sTh = (float)(Math.Atan2(sw.Position.Y - _rotCenter.Y, sw.Position.X - _rotCenter.X) / + Math.PI * 180) + sign * 90; + th = CommonMath.ThDiff(sTh, sw.ZeroDirection); + var r1 = Vector2.Distance(sw.Position, _rotCenter); + _tmpSpeeds[i] = r1 / (r0 + 0.00001f); + } + else + { + var axisTh = currentRearTh + thDiff * Interpolate(_rearBase, _wheelBases[i], _frontBase); + _tmpSpeeds[i] = 1; + th = CommonMath.ThDiff(axisTh, sw.ZeroDirection); + } + + if (!TryResolveWheelAngle(i, th, "SendMotion", out var resolvedTh, out var resolvedDir, + out var resolveReason)) + { + anyWheelInvalid = true; + needAdjust = true; + lastInvalidReason = resolveReason; + break; + } + + _wheelDirs[i] = resolvedDir; + tmpAngles.Add(resolvedTh); + } + + if (needAdjust && anyWheelInvalid) + { + return FailMotionDecomposition("SendMotion", lastInvalidReason, deltaTime); + } + + finalAngles = tmpAngles; + break; + } + + if (finalAngles.Count == _steerWheels.Count) + { + for (var i = 0; i < _steerWheels.Count; i++) + { + SendTh(i, finalAngles[i]); + } + } + else + { + return FailMotionDecomposition("SendMotion", + $"unable to find valid wheel angles frontTh={frontTh:F1} rearTh={rearTh:F1}", + deltaTime); + } + + // // todo: find better way to limit max speed + // var totalTurn = Math.Abs(frontTh) + Math.Abs(rearTh); + // var turnThresholdSpeed = CalculateTurningSpeedDecayFac(totalTurn) * MaxSpeed; + for (int i = 0; i < _steerWheels.Count; i++) + { + _debugSpeeds[i] = _tmpSpeeds[i] * speed * _wheelDirs[i]; + } + var speedBeforeAlignGate = speed; + + if (!GoingWheelAligned) + { + Hedingben.ToastText("Wheel Not Aligned", "MultiWheelChassis-SendMotion-notAligned"); + speed = 0; + var aligned = _steerWheels.Select((sw, i) => (sw, i)).All(ww => + Math.Abs(CommonMath.ThDiff(ww.sw.ReadAngle(), _sendAngle[ww.i])) < 1); + GoingWheelAligned = aligned; + } + + for (var i = 0; i < _steerWheels.Count; i++) + { + AccumulateSpeed(i, _tmpSpeeds[i] * speed * _wheelDirs[i], axisDiffFlag, _rotCenter, deltaTime); + } + + if ((DateTime.Now - _sendMotionDetailLastLog).TotalMilliseconds >= 200) + { + _sendMotionDetailLastLog = DateTime.Now; + var sb = new StringBuilder(); + sb.Append($"SendMotionDetail speed:{speed:F3} speedBeforeGate:{speedBeforeAlignGate:F3} fTh:{frontTh:F1} rTh:{rearTh:F1} " + + $"curF:{currentFrontTh:F1} curR:{currentRearTh:F1} " + + $"originBias:({_originBiasX:F1},{_originBiasY:F1},{_originBiasTh:F1}) " + + $"in(speed:{compInputSpeed:F3},f:{compInputFrontTh:F1},r:{compInputRearTh:F1}) " + + $"comp({localCompensateX:F1},{localCompensateY:F1},{localCompensateTh:F2}) " + + $"motionTh:{compMotionTh:F1} along:{compAlong:F1} side:{compSide:F1} dSpeed:{compDSpeed:F3} dTh:{compDTh:F1} " + + $"axisDiff:{axisDiffFlag} rotCenter:({_rotCenter.X:F1},{_rotCenter.Y:F1}) goingAligned:{GoingWheelAligned}"); + for (var i = 0; i < _steerWheels.Count; i++) + { + var sw = _steerWheels[i]; + var readTh = sw.ReadAngle(); + var targetTh = _sendAngle[i]; + var errTh = CommonMath.ThDiff(readTh, targetTh); + sb.Append($" | w{i} tgt:{targetTh:F1} read:{readTh:F1} err:{errTh:F1} " + + $"zero:{sw.ZeroDirection:F1} rel:{sw.GetAngleRelativeToChassis():F1} " + + $"dir:{_wheelDirs[i]} tmp:{_tmpSpeeds[i]:F2} targetV:{_targetSpeeds[i]:F3} sendV:{_sendSpeeds[i]:F3} " + + $"lim:[{sw.AngleLowerLimit:F1},{sw.AngleUpperLimit:F1}]"); + if (sw is DiffSteerWheel dsw) + { + sb.Append($" diffSend(L:{dsw.GetLeftSendSpeed():F3},R:{dsw.GetRightSendSpeed():F3})"); + } + else + { + sb.Append($" wheelSend:{sw.GetSendSpeed():F3}"); + } + } + DLog.Log(sb.ToString(), "MultiWheelMotionDetail"); + } + + LastMoveTime = DateTime.Now; + LastMotionDecomposeFailureReason = ""; + return true; + } + + /// + /// 停车并将四个舵轮转到绕当前坐标原点自转所需的切线方向。 + /// 只下发舵角，不下发驱动速度。 + /// + public bool PrepareRotateWheels(float alignmentToleranceDegrees = 2.0f) + { + if (!Valid) + return FailMotionDecomposition( + "PrepareRotateWheels", + "invalid chassis", + null); + + if (float.IsNaN(alignmentToleranceDegrees) || + float.IsInfinity(alignmentToleranceDegrees) || + alignmentToleranceDegrees < 0.0f) + throw new ArgumentOutOfRangeException( + nameof(alignmentToleranceDegrees), + "自转舵轮到位容差必须是非负有限值。"); + + if (_steerWheels.Count == 0) + return FailMotionDecomposition( + "PrepareRotateWheels", + "no steer wheels", + null); + + // 模式切换期间必须保持驱动轮停止。 + PredefinedDriveStop(); + + var targetAngles = new float[_steerWheels.Count]; + var directions = new int[_steerWheels.Count]; + + // 先完成全部舵角解算，再统一下发，避免只转动部分舵轮。 + for (var i = 0; i < _steerWheels.Count; i++) + { + var wheel = _steerWheels[i]; + var px = (double)wheel.Position.X; + var py = (double)wheel.Position.Y; + + // 逆时针绕原点旋转时，该舵轮的切向方向为(-py, px)。 + var tangentDegrees = + (float)(Math.Atan2(px, -py) / + Math.PI * 180.0); + tangentDegrees = CommonMath.ThDiff( + tangentDegrees, + wheel.ZeroDirection); + + if (!TryResolveWheelAngle( + i, + tangentDegrees, + "PrepareRotateWheels", + out targetAngles[i], + out directions[i], + out var reason)) + return FailMotionDecomposition( + "PrepareRotateWheels", + reason, + null); + } + + for (var i = 0; i < _steerWheels.Count; i++) + { + _wheelDirs[i] = directions[i]; + SendTh(i, targetAngles[i]); + } + + var allAligned = true; + for (var i = 0; i < _steerWheels.Count; i++) + { + var actualAngle = _steerWheels[i].ReadAngle(); + var angleError = targetAngles[i] - actualAngle; + + // 这里比较受机械限位约束的真实舵角，不能使用圆周最短角度差。 + if (float.IsNaN(actualAngle) || + float.IsInfinity(actualAngle) || + Math.Abs(angleError) > alignmentToleranceDegrees) + allAligned = false; + } + + LastRotateAligned = allAligned; + LastMotionDecomposeFailureReason = ""; + return true; + } + + /// + /// 将PrepareRotateWheels已经确认到位的舵角和轮速方向， + /// 原样交接给SendXYThSpeed，作为一段XYTh运动的初始状态。 + /// 该方法不会调用ResetMotionState，因此不会重新选择等价舵角。 + /// + public bool AdoptPreparedRotateWheelsForXYTh( + float alignmentToleranceDegrees = 2.0f) + { + if (!Valid) + return FailMotionDecomposition( + "AdoptPreparedRotateWheelsForXYTh", + "invalid chassis", + null); + + if (float.IsNaN(alignmentToleranceDegrees) || + float.IsInfinity(alignmentToleranceDegrees) || + alignmentToleranceDegrees < 0.0f) + throw new ArgumentOutOfRangeException( + nameof(alignmentToleranceDegrees), + "自转舵轮交接容差必须是非负有限值。"); + + if (!LastRotateAligned) + return FailMotionDecomposition( + "AdoptPreparedRotateWheelsForXYTh", + "rotate wheels have not been prepared and aligned", + null); + + for (var i = 0; i < _steerWheels.Count; i++) + { + var actualAngle = + _steerWheels[i].ReadAngle(); + + if (float.IsNaN(actualAngle) || + float.IsInfinity(actualAngle)) + { + LastRotateAligned = false; + return FailMotionDecomposition( + "AdoptPreparedRotateWheelsForXYTh", + $"wheel {i} angle feedback is invalid: {actualAngle}", + null); + } + + // 比较受机械限位约束的实际舵角，不使用圆周最短角。 + var angleError = + _sendAngle[i] - actualAngle; + if (Math.Abs(angleError) > + alignmentToleranceDegrees) + { + LastRotateAligned = false; + return FailMotionDecomposition( + "AdoptPreparedRotateWheelsForXYTh", + $"wheel {i} is no longer aligned: " + + $"target={_sendAngle[i]:F1}, actual={actualAngle:F1}, " + + $"error={angleError:F1}", + null); + } + } + + // 直接继承PrepareRotateWheels写入的_wheelDirs和_sendAngle。 + // 下一次SendXYThSpeed调用看到XYThActive=true时不会重置这些状态。 + XYThActive = true; + _xyThWheelsAligned = true; + GoingActive = false; + RotatingActive = false; + LastMoveTime = DateTime.Now; + LastMotionDecomposeFailureReason = ""; + return true; + } + + /// + /// 绕"已被 SetOriginBias 偏置到车队中心的原点"做原地旋转，可叠加一个车体系小幅纠偏旋量。 + /// + /// 绕车队中心角速度(deg/s，逆时针为正)。 + /// 车体系纵向(前+)修正速度(mm/s)，多车联动维持队形用。 + /// 车体系横向(左+)修正速度(mm/s)。 + /// 绕本车几何中心附加角速度(deg/s)，修正朝向偏差。 + public bool SendRotateMotion(float rotSpeed, TimeSpan? deltaTime = null, + float localCompensateX = 0f, float localCompensateY = 0f, float localCompensateTh = 0f) + { + if (!Valid) return FailMotionDecomposition("SendRotateMotion", "invalid chassis", deltaTime); + LastMotionDecomposeFailureReason = ""; + if (Math.Abs(rotSpeed) < 1e-6f && + Math.Abs(localCompensateX) < 1e-6f && + Math.Abs(localCompensateY) < 1e-6f && + Math.Abs(localCompensateTh) < 1e-6f) + { + LastRotateAligned = true; + RampStop(deltaTime); + return true; + } + + var ths = new float[_steerWheels.Count]; + var dirs = new int[_steerWheels.Count]; + // 每轮合速度大小(m/s)，含"绕队心旋转 + 车体平移纠偏 + 绕本车中心微转纠偏"三项矢量和。 + var speedMags = new float[_steerWheels.Count]; + var allWheelAligned = true; + + // 把车体系纠偏旋量换算到 sw.Position 所在的偏置帧 F(原点=车队中心, 朝向随 _originBiasTh)。 + // body->F 旋转为 R(_originBiasTh)，与 SetOriginBias 里 Transform2D 的约定一致。 + var radBias = _originBiasTh / 180.0 * Math.PI; + var cosB = Math.Cos(radBias); + var sinB = Math.Sin(radBias); + var compVxF = localCompensateX * cosB - localCompensateY * sinB; // mm/s，F帧 + var compVyF = localCompensateX * sinB + localCompensateY * cosB; + var rotRad = rotSpeed / 180.0 * Math.PI; // 绕队心(F原点)角速度 rad/s + var compRad = localCompensateTh / 180.0 * Math.PI; // 绕本车几何中心附加角速度 rad/s + var biasX = (double)_originBiasX; // 本车几何中心在 F 中的位置 + var biasY = (double)_originBiasY; + + for (var i = 0; i < _steerWheels.Count; i++) + { + var sw = _steerWheels[i]; + var px = (double)sw.Position.X; + var py = (double)sw.Position.Y; + // 合成轮速矢量(mm/s, F帧)：v = ω_rot ẑ×p + [R(bias)·v_comp + ω_comp ẑ×(p - 本车中心)] + // 旋转切向项与纠偏项分开算，便于把纠偏钳到旋转切向的一定比例。 + var vxRot = -rotRad * py; + var vyRot = rotRad * px; + var vxComp = compVxF - compRad * (py - biasY); + var vyComp = compVyF + compRad * (px - biasX); + // 纠偏钳位：|纠偏| ≤ RotateCompTangentFrac × |旋转切向|，限制合矢量相对纯切向的最大偏角 + // (frac=0.5 → 偏角≤26.6°)，避免减速末段纠偏盖过旋转切向导致舵轮 180° 乱翻。 + if (RotateCompTangentFrac >= 0f) + { + var rotMag = Math.Sqrt(vxRot * vxRot + vyRot * vyRot); + var compMag = Math.Sqrt(vxComp * vxComp + vyComp * vyComp); + var compLimit = RotateCompTangentFrac * rotMag; + if (compMag > compLimit && compMag > 1e-6) + { + var s = compLimit / compMag; + vxComp *= s; + vyComp *= s; + } + } + var vx = vxRot + vxComp; + var vy = vyRot + vyComp; + var vmag = Math.Sqrt(vx * vx + vy * vy); + speedMags[i] = (float)(vmag / 1000.0); + + // 轮子应指向的运动方向(F帧)。纠偏后合速度≈0 时退化为纯旋转切向，避免 atan2 抖动。 + var angleF = vmag < 1e-6 + ? Math.Atan2(px, -py) / Math.PI * 180 + : Math.Atan2(vy, vx) / Math.PI * 180; + var tangent = CommonMath.ThDiff((float)angleF, sw.ZeroDirection); + + if (!TryResolveWheelAngle(i, tangent, "SendRotateMotion", out ths[i], out dirs[i], + out var resolveReason)) + return FailMotionDecomposition("SendRotateMotion", resolveReason, deltaTime); + _wheelDirs[i] = dirs[i]; + } + + for (var i = 0; i < _steerWheels.Count; ++i) + SendTh(i, ths[i]); + var slowFac = 1f; + var maxDth = 0f; + for (var i = 0; i < _steerWheels.Count; ++i) + { + var actualAngle = _steerWheels[i].ReadAngle(); + var dth = Math.Abs(ths[i] - actualAngle); + maxDth = Math.Max(maxDth, dth); + slowFac = Math.Min( + slowFac, + CommonMath.gaussmf( + dth, + Math.Max(SteeringAlignmentSigmaDegrees, 0.1f), + 0)); + if (float.IsNaN(actualAngle) || + float.IsInfinity(actualAngle) || + dth > 2.0f) + allWheelAligned = false; + } + LastRotateAligned = allWheelAligned; // 供上层做积分抗饱和 + + // 对齐门控：未对齐则整体停车；对齐后用 slowFac 平滑提速。旋转与纠偏被同等缩放，保持几何一致。 + var gateFac = allWheelAligned ? slowFac : 0f; + for (var i = 0; i < _steerWheels.Count; ++i) + AccumulateSpeed(i, speedMags[i] * gateFac * dirs[i], true, Vector2.Zero, deltaTime); + + // === 原地旋转诊断（节流 ~300ms）=== + // 关注 allWheelAligned（false 说明舵轮追不上目标角）、dirs/半径，以及叠加的纠偏旋量 comp(vx,vy,om)。 + if ((DateTime.Now - _rotDbgLast).TotalMilliseconds >= 300) + { + _rotDbgLast = DateTime.Now; + var sb = new StringBuilder(); + sb.Append($"SendRotateMotion in:{rotSpeed:F1} aligned:{allWheelAligned} maxDth:{maxDth:F1} slowFac:{slowFac:F2} gate:{gateFac:F2} biasTh:{_originBiasTh:F1} comp(vx:{localCompensateX:F1} vy:{localCompensateY:F1} om:{localCompensateTh:F1})"); + for (var i = 0; i < _steerWheels.Count; ++i) + { + var sw = _steerWheels[i]; + sb.Append($" | w{i} tgt:{ths[i]:F1} read:{sw.ReadAngle():F1} dth:{Math.Abs(CommonMath.ThDiff(ths[i], sw.ReadAngle())):F1} dir:{dirs[i]} R:{sw.Position.Length():F0} v:{speedMags[i] * gateFac * dirs[i]:F3} send:{sw.GetSendSpeed():F2}"); + } + DLog.Log(sb.ToString(), "RotateDbg"); + } + + LastMoveTime = DateTime.Now; + LastMotionDecomposeFailureReason = ""; + return true; + } + + private DateTime _rotDbgLast = DateTime.MinValue; + + public override float CalculateTurningSpeedDecayFac(float turn) + { + return 1 - Math.Min(turn, MaxTurnThreshold) / MaxTurnThreshold * MinTurnSpeedFac; + } + + [Obsolete] + public List GetSteerWheels() + { + return _steerWheels.ToList(); + } + + private void SendTh(int i, float targetTh) + { + _steerWheels[i].WriteAngle(targetTh); + _sendAngle[i] = targetTh; + } + + private (int direction, float rangeFront, float rangeRear) DetermineWheelDirection(SteerWheel wheel) + { + var lower = CommonMath.RoundTh(wheel.AngleLowerLimit + wheel.ZeroDirection); + var upper = CommonMath.RoundTh(wheel.AngleUpperLimit + wheel.ZeroDirection); + while (upper < lower) upper += 360; + + var frontDir = 0; + var rearDir = 180; + + // 计算前向和后向的可用范围 + var rangeFront = Math.Min(upper - frontDir, frontDir - lower); + var rangeRear = Math.Min(upper - rearDir, rearDir - lower); + + // 选择范围更大的方向 + if (rangeFront >= rangeRear && rangeFront > 0) + { + return (1, rangeFront, rangeRear); + } + else if (rangeRear > rangeFront && rangeRear > 0) + { + return (-1, rangeFront, rangeRear); + } + + // 如果两个方向都不可行 + return (0, rangeFront, rangeRear); + } + + private void AccumulateSpeed(int i, float v, bool axisDiff, Vector2 rotCenter, TimeSpan? deltaTime = null) + { + _targetSpeeds[i] = v; + var speedSign = Math.Sign(_targetSpeeds[i] - _sendSpeeds[i]); + var acc = Math.Abs(_targetSpeeds[i]) > Math.Abs(_sendSpeeds[i]) ? AccPerSecond : DeAccPerSecond; + _sendSpeeds[i] += speedSign * Math.Min(Math.Abs(_targetSpeeds[i] - _sendSpeeds[i]), + acc * (float)(deltaTime ?? DateTime.Now - LastMoveTime).TotalSeconds); + if (_steerWheels[i] is DiffSteerWheel dsw) + { + if (axisDiff) + { + var wheelRadius = Vector2.Distance(rotCenter, dsw.Position); + var wheelDir = _sendAngle[i]; + + var left = CommonMath.Transform2D(dsw.Position, wheelDir + 90, new Vector2(dsw.WheelDistance, 0)); + var right = CommonMath.Transform2D(dsw.Position, wheelDir - 90, new Vector2(dsw.WheelDistance, 0)); + dsw.WriteLeftSpeed(_sendSpeeds[i] / wheelRadius * Vector2.Distance(rotCenter, left)); + dsw.WriteRightSpeed(_sendSpeeds[i] / wheelRadius * Vector2.Distance(rotCenter, right)); + } + else + { + dsw.WriteLeftSpeed(_sendSpeeds[i]); + dsw.WriteRightSpeed(_sendSpeeds[i]); + } + } + else + { + _steerWheels[i].WriteSpeed(_sendSpeeds[i]); + } + + if (Debug) + Console.WriteLine($"Ackermann wheel{i}: target:{_targetSpeeds[i]:0.00},send:{_sendSpeeds[i]:0.0}"); + } + + private void CalculateAxes() + { + var axes = _steerWheels + .Select(sw => (sw, Vector2.Dot(sw.Position, new Vector2(1, 0)))) + .OrderByDescending(ax => ax.Item2).ToList(); + _wheelBases = axes.Select(ax => ax.Item2).ToList(); + _steerWheels = axes.Select(ax => ax.sw).ToList(); + GeometricControlPoints = new List() + { + new (new Vector2(ControlPointRadius, 0)), + new (new Vector2(-ControlPointRadius, 0)) + }; + _frontBase = _wheelBases.First(); + _rearBase = _wheelBases.Last(); + } + + private List _steerWheels = new(); + private float _frontBase, _rearBase; + private List _wheelBases; + private List _sendSpeeds; + private List _targetSpeeds; + private List _sendAngle; + private List _debugSpeeds; + private DateTime _sendMotionDetailLastLog = DateTime.MinValue; + private DateTime _geometricComputeLastLog = DateTime.MinValue; + + private List _tmpSpeeds; + // add TimeStamp, prevent the wheel from swaying. + // for example, target angle is 90, current wheel is around 0. if no TimeStamp, + // wheel will swing between 90 and -90. + private List _wheelDirs; + + // call this before a new motion sequence happens + private void ResetMotionState() + { + LastMoveTime = DateTime.Now; + GoingWheelAligned = false; + _wheelDirs = Enumerable.Repeat(1, _steerWheels.Count).ToList(); + } + + public void AddTestFunction() + { + + } + /// + /// 得到相对舵轮在车体坐标系下的分解速度 + /// + /// + /// + /// + /// 单位为°/s + /// + private Vector2 VectorVelocity(Vector2 pos, float vx, float vy, float vth, int i) + { + var vRotX = -vth / 180 * (float)Math.PI * pos.Y / 1000; + var vRotY = vth / 180 * (float)Math.PI * pos.X / 1000; + return new Vector2(vx + vRotX, vy + vRotY); + } + /// + /// 获得车轮应该打的角度和速度 + /// + /// + /// + /// + /// 单位为°/s + /// + private (float angle, float speed) AngleAndSpeed(Vector2 pos, float vx, float vy, float vth, int i) + { + var v = VectorVelocity(pos, vx, vy, vth, i); + return ((float)(Math.Atan2(v.Y, v.X) / Math.PI * 180), v.Length()); + } + + /// + /// 原地旋转时舵角误差对应的速度衰减宽度，单位为度。 + /// + public float SteeringAlignmentSigmaDegrees { get; set; } = 8f; + /// + /// 单轮速度低于此值时认为其运动方向无意义，单位为m/s。 + /// + public float WheelDirectionDeadbandMetersPerSecond { get; set; } = 0.005f; + private bool XYThActive = false; + private bool _xyThWheelsAligned = false; + private DateTime _xyThDiagnosticsLastTime = DateTime.MinValue; + + /// + /// 下发车体二维速度，并根据舵轮机械角度误差进行高斯降速。 + /// 一段运动开始时必须先等待全部舵轮到位；运动过程中舵角误差越大， + /// 四轮驱动速度的统一缩放比例越小，适合作为默认安全接口。 + /// vx、vy单位为m/s，vth单位为°/s。 + /// + public bool SendXYThSpeed( + float vx, + float vy, + float vth, + TimeSpan? deltaTime = null) + { + const string operationName = "SendXYThSpeed"; + + if (!Valid) + return FailMotionDecomposition( + operationName, + "invalid chassis", + deltaTime); + + if (Math.Abs(vx) < 1e-6f && Math.Abs(vy) < 1e-6f && Math.Abs(vth) < 1e-6f) + { + RampStop(deltaTime); + XYThActive = false; + _xyThWheelsAligned = false; + GoingActive = false; + RotatingActive = false; + LastMotionDecomposeFailureReason = ""; + return true; + } + + if (!XYThActive) + { + ResetMotionState(); + _xyThWheelsAligned = false; + } + XYThActive = true; + GoingActive = false; + RotatingActive = false; + + float[] sendSpeed = new float[_steerWheels.Count]; + var allWheelsAligned = true; + var maximumAngleError = 0f; + var alignmentSpeedScale = 1f; + const float initialAlignmentToleranceDegrees = 2f; + var writeDiagnostics = + Debug && + (DateTime.Now - _xyThDiagnosticsLastTime) + .TotalMilliseconds >= 250.0; + + for (var i = 0; i < _steerWheels.Count; i++) + { + var sw = _steerWheels[i]; + var (angle, speed) = AngleAndSpeed(sw.Position, vx, vy, vth, i); + // 单轮合成速度接近零时，运动方向没有物理意义。 + // 此时不重新计算和下发舵角，保持上一目标舵角，轮速降为零。 + var directionDeadband = Math.Max( + WheelDirectionDeadbandMetersPerSecond, + 0f); + + if (speed < directionDeadband) + { + sendSpeed[i] = 0f; + + if (writeDiagnostics) + { + Hedingben.ToastText( + $"hold-angle speed:{speed:F4} deadband:{directionDeadband:F4}", + $"{operationName}-{i}"); + } + + continue; + } + var actualTh = sw.ReadAngle(); + + if (float.IsNaN(actualTh) || + float.IsInfinity(actualTh)) + { + return FailMotionDecomposition( + operationName, + $"wheel {i} angle feedback is invalid: {actualTh}", + deltaTime); + } + + if (!TryResolveWheelAngle(i, CommonMath.ThDiff(angle, sw.ZeroDirection), operationName, + out var useAngle, out var dir, out var resolveReason)) + return FailMotionDecomposition(operationName, resolveReason, deltaTime); + speed *= dir; + _wheelDirs[i] = dir; + + sendSpeed[i] = speed; + SendTh(i, useAngle); + + // 这里比较受机械限位约束的实际舵角，不使用圆周最短角。 + var angleError = + Math.Abs(_sendAngle[i] - actualTh); + maximumAngleError = + Math.Max(maximumAngleError, angleError); + + alignmentSpeedScale = Math.Min( + alignmentSpeedScale, + CommonMath.gaussmf( + angleError, + Math.Max( + SteeringAlignmentSigmaDegrees, + 0.1f), + 0)); + if (angleError > + initialAlignmentToleranceDegrees) + { + allWheelsAligned = false; + } + + if (writeDiagnostics) + { + Hedingben.ToastText( + $"ready:{_xyThWheelsAligned} " + + $"err:{angleError:F1} scale:{alignmentSpeedScale:F2} " + + $"s:{speed:F3} th:{_sendAngle[i]:F1} actualTh:{actualTh:F1}", + $"{operationName}-{i}"); + } + } + + // 一段XYTh运动刚开始时必须等待全部舵轮到位。 + if (!_xyThWheelsAligned && + allWheelsAligned) + { + _xyThWheelsAligned = true; + } + + var driveScale = _xyThWheelsAligned + ? alignmentSpeedScale + : 0f; + for (var i = 0; i < _steerWheels.Count; i++) + AccumulateSpeed( + i, + driveScale * + sendSpeed[i], + false, + new Vector2(0f, 0f), + deltaTime); + + if (writeDiagnostics) + { + _xyThDiagnosticsLastTime = DateTime.Now; + Hedingben.ToastText( + $"ready:{_xyThWheelsAligned} " + + $"maxErr:{maximumAngleError:F1} scale:{driveScale:F2} " + + $"cmd:({vx:F3},{vy:F3},{vth:F1})", + $"{operationName}-alignment"); + } + //todo 计算rotCenter填入 + LastMoveTime = DateTime.Now; + LastMotionDecomposeFailureReason = ""; + return true; + } + + public override CarSpeed GetCarSpeed(bool isActual = false) + { + List vx = new List(); + List vy = new List(); + List vth = new List(); + for (var i = 0; i < _steerWheels.Count; ++i) + { + var sw1 = _steerWheels[i]; + Vector2 a = sw1.Position / 1000; + //var tha = _sendAngle[i] / 180 * (float)Math.PI; + var tha = isActual ? sw1.ReadAngle() / 180 * (float)Math.PI : _sendAngle[i] / 180 * (float)Math.PI; + //var speeda = _sendSpeeds[i]; + //var speeda = isActual ? sw1.ReadSpeed() : _sendSpeeds[i]; + var speeda = isActual ? sw1.ReadSpeed() : _debugSpeeds[i]; + Vector2 va = new Vector2(speeda * (float)Math.Cos(tha), speeda * (float)Math.Sin(tha)); + // painter.DrawLine(Color.Green, sw1.Position, + // LessMath.Transform2D(sw1.Position, sw1.ReadAngle(), new Vector2(200, 0)), endArrow: true); + for (var j = i + 1; j < _steerWheels.Count; ++j) + { + var sw2 = _steerWheels[j]; + Vector2 b = sw2.Position / 1000; + //var thb = _sendAngle[j] / 180 * (float)Math.PI; + var thb = isActual ? sw2.ReadAngle() / 180 * (float)Math.PI : _sendAngle[j] / 180 * (float)Math.PI; + //var speedb = _sendSpeeds[j]; + //var speedb = isActual ? sw2.ReadSpeed() : _sendSpeeds[j]; + var speedb = isActual ? sw2.ReadSpeed() : _debugSpeeds[j]; + Vector2 vb = new Vector2(speedb * (float)Math.Cos(thb), speedb * (float)Math.Sin(thb)); + var (tempvx, tempvy, tempvth) = CenterVelocityFromPoints(a, va, b, vb); + vx.Add(tempvx); + vy.Add(tempvy); + vth.Add(tempvth); + } + } + return new CarSpeed() + { Vx = vx.Average(), Vy = vy.Average(), Vw = (float)(vth.Average() / Math.PI * 180f) }; + } + + private static (float, float, float) CenterVelocityFromPoints(Vector2 a, Vector2 va, Vector2 b, Vector2 vb) + { + float vth_x = 0, vth_y = 0, vth = 0, vx = 0, vy = 0; + var eps = 0.0000001; + if (Math.Abs(a.Y - b.Y) > eps) + { + vth_x = (va.X - vb.X) / (b.Y - a.Y); + } + + if (Math.Abs(a.X - b.X) > eps) + { + vth_y = (va.Y - vb.Y) / (a.X - b.X); + } + vth = vth_x == 0 ? vth_y : vth_x; + vx = va.X + vth * a.Y; + vy = va.Y - vth * a.X; + return (vx, vy, vth); + } + } +} diff --git a/CommonUsage-MultiVehicleSync/commonusage/Chassis/SingleSteerChassis.cs b/CommonUsage-MultiVehicleSync/commonusage/Chassis/SingleSteerChassis.cs new file mode 100644 index 0000000..cb0df3e --- /dev/null +++ b/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() + { + new (_steerWheel.Position), + new (Vector2.Zero) + }; + Valid = true; + } + + public override void AfterDirectionChanged() + { + if (Math.Abs(CommonMath.ThDiff(0, _originBiasTh)) > 90) + { + GeometricControlPoints = new List() + { + new (-_steerWheel.Position), + }; + _direction = -1; + } + else + { + GeometricControlPoints = new List() + { + 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 + } +} diff --git a/CommonUsage-MultiVehicleSync/commonusage/Chassis/SteerWheel.cs b/CommonUsage-MultiVehicleSync/commonusage/Chassis/SteerWheel.cs new file mode 100644 index 0000000..00bc7c4 --- /dev/null +++ b/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 speedWriter, + Func speedReader, Action angleWriter, Func 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 _angleWriter; + [JsonIgnore] private readonly Func _angleReader; + + private float _angleLowerLimit = -90, _angleUpperLimit = 90; + private float _centerDistance; + + public float _sendAngle = 0f; + } +} diff --git a/CommonUsage-MultiVehicleSync/commonusage/Chassis/Wheel.cs b/CommonUsage-MultiVehicleSync/commonusage/Chassis/Wheel.cs new file mode 100644 index 0000000..dea87e1 --- /dev/null +++ b/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 speedWriter, Func 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 SpeedWriter; + [JsonIgnore] public readonly Func SpeedReader; + + public float _sendSpeed; + } + + public class GeometricControlPoint + { + public GeometricControlPoint(Vector2 position) + { + Position = position; + } + + public Vector2 Position; + public float Theta; + } +} diff --git a/CommonUsage-MultiVehicleSync/commonusage/CommonUsage.csproj b/CommonUsage-MultiVehicleSync/commonusage/CommonUsage.csproj new file mode 100644 index 0000000..ef7f5b8 --- /dev/null +++ b/CommonUsage-MultiVehicleSync/commonusage/CommonUsage.csproj @@ -0,0 +1,50 @@ + + + + netstandard2.0 + CommonUsage + CommonUsage + + + + latest + True + + + + embedded + + + + embedded + + + + + + + + + + + + + + + + + + ..\..\MedullaAdapter\ref\RefFundamentalLib.dll + + + + + + + + + + diff --git a/CommonUsage-MultiVehicleSync/commonusage/CommonUsageSln.sln b/CommonUsage-MultiVehicleSync/commonusage/CommonUsageSln.sln new file mode 100644 index 0000000..227bf56 --- /dev/null +++ b/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 diff --git a/CommonUsage-MultiVehicleSync/commonusage/Deps/RefFundamentalLib.dll b/CommonUsage-MultiVehicleSync/commonusage/Deps/RefFundamentalLib.dll new file mode 100644 index 0000000..8ada7ed Binary files /dev/null and b/CommonUsage-MultiVehicleSync/commonusage/Deps/RefFundamentalLib.dll differ diff --git a/CommonUsage-MultiVehicleSync/commonusage/Geometries/AbstractGeometry.cs b/CommonUsage-MultiVehicleSync/commonusage/Geometries/AbstractGeometry.cs new file mode 100644 index 0000000..28ed7b9 --- /dev/null +++ b/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 +{ + /// + /// 便于直接创建几何形状并求几何形状的切点、切线等。 + /// + 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 processDot, Action processLine, + bool visExtendedPart = false); + + public VisualizeOption VisualizeOption; + + /// + /// 查询指定位置的曲率。 + /// + /// 从起点到查询位置的距离。 + /// + 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; + } +} diff --git a/CommonUsage-MultiVehicleSync/commonusage/Geometries/BezierCurve.cs b/CommonUsage-MultiVehicleSync/commonusage/Geometries/BezierCurve.cs new file mode 100644 index 0000000..c4c8bbe --- /dev/null +++ b/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 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 processDot, Action 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> dict, List 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 ControlPoints => _controlPoints; + + public int Resolution => _resolution; + /// + /// 仅用于simple显示路径方向 + /// + public int Direction = 1; + public int Order => _order; + + // public List 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(); + 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>>(); + for (var i = 0; i < _order; i++) + { + var size = allPoints.Count; + var morePoints = new List>(); + for (var j = 0; j < _order - i; j++) + { + var points = new List(); + 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> dict, float gSize) + { + dict = new Dictionary>(); + 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 _controlPoints; + private List _bezierPoints; + private List>> _tangentInfo; + private List _tangents; + private List _remainDistances; + private List _sumDistances; + private List _curvatures; + private Dictionary> _pointsMappingSmall; + private Dictionary> _pointsMappingBig; + private float _minX, _minY; + private float _length; + } +} diff --git a/CommonUsage-MultiVehicleSync/commonusage/Geometries/CircularArc.cs b/CommonUsage-MultiVehicleSync/commonusage/Geometries/CircularArc.cs new file mode 100644 index 0000000..2126385 --- /dev/null +++ b/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 + { + /// + /// 以center为圆心、radius为半径，从angleStart逆时针转到angleEnd所构成的圆弧。direction表示圆弧走向。 + /// + /// + /// + /// + /// + /// 表示圆弧走向，1为angleStart到angleEnd，-1为angleEnd到angleStart + 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 processDot, Action 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(); + } +} diff --git a/CommonUsage-MultiVehicleSync/commonusage/Geometries/NurbsCurve.cs b/CommonUsage-MultiVehicleSync/commonusage/Geometries/NurbsCurve.cs new file mode 100644 index 0000000..aa45227 --- /dev/null +++ b/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 controlPoints, List weights, List knotVector, int frame = 100) + { + _controlPoints = controlPoints; + _weights = weights; + _knotVector = knotVector; + _frame = frame; + InitializeNurbs(); + } + + public override void Visualize(Action processDot, Action 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> dict, List 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 ControlPoints => _controlPoints; + public List Weights => _weights; + public List 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> allpoints = new List>(); + List nurbsCurvePoints = new List(); + float delta = 1.0f / Frame; + + for (float t = 0; t <= 1; t += delta) + { + var (point, tangent) = DeBoorAlgorithm(t); + var points = new List + { + 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 _controlPoints; + private List _weights; + private List _knotVector; + private int _frame; + private List _nurbsPoints; + private List>> _tangentPoints; + private List _curvatures; + private List _sumDistances; + private List _remainDistances; + private float _minX, _minY; + private float _length; + private Dictionary> _pointsMappingSmall; + private Dictionary> _pointsMappingBig; + private List _tangents; + + } +} diff --git a/CommonUsage-MultiVehicleSync/commonusage/Geometries/Vectoriel.cs b/CommonUsage-MultiVehicleSync/commonusage/Geometries/Vectoriel.cs new file mode 100644 index 0000000..86b72a9 --- /dev/null +++ b/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 +{ + /// + /// MDCS数学类：向量。 + /// + 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); + } + + /// + /// 通过笛卡尔坐标系X和Y值构建向量。 + /// + /// + /// + /// + public static Vectoriel FromXY(float x, float y) + { + return new Vectoriel(new Vector2(x, y)); + } + + /// + /// 通过极坐标系的角度和距离值构建向量。 + /// + /// + /// + /// + 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; + } +} diff --git a/CommonUsage-MultiVehicleSync/commonusage/Mathematics/CommonMath.cs b/CommonUsage-MultiVehicleSync/commonusage/Mathematics/CommonMath.cs new file mode 100644 index 0000000..df49d5b --- /dev/null +++ b/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; + using D3 = Tuple; + + public class CommonMath + { + public class PrimeEnumerator + { + public PrimeEnumerator(List items, Func 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 _items; + private readonly Func _process; + + private IEnumerable Get() + { + for (var i = 0; i < _n; ++i) + { + var id = (i * _a + _b) % _n; + yield return _process(_items[id]); + } + } + } + + private static IEnumerable> GetPermutationsInternal(IEnumerable 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 })); + } + + /// + /// 得到一组数据的所有排列。 + /// + /// 元素数据类型 + /// 所有待选元素 + /// 所选出的元素数量 + /// + public static List> GetPermutations(List 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 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 GetConvexHull(List points) + { + if (points == null) + return null; + + if (points.Count() <= 1) + return points; + + int n = points.Count(), k = 0; + List H = new List(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 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; + } + + /// + /// 求点p到两点式直线p1p2的距离 + /// + /// 点p的x坐标 + /// 点p的y坐标 + /// 直线点p1的x坐标 + /// 直线点p1的y坐标 + /// 直线点p2的x坐标 + /// 直线点p2的y坐标 + /// + 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); + } + + /// + /// 两条两点式直线间的夹角 + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// 角度制 + 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); + } + + /// + /// 两向量间夹角 + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// 角度制 + 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; + } + + /// + /// 两向量间夹角. + /// + /// + /// + /// 角度制 + 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; + } + + /// + /// 求点到直线的垂足 + /// + /// + /// + /// + /// + /// + /// + /// + 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; + } + + + /// + /// 最小二乘法拟合直线，得到两点式。 + /// + /// 待拟合的点集，应至少有2个点。 + /// 检查是否所有点距直线的距离均小于maxDist2Line，若为-1则不检查。 + /// 返回两点式的两个端点坐标。若坐标为全0，则拟合失败。 + public static (bool, Vector2, Vector2) FitLineSegment(List 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 + { + public int numIteration = 3; + public int itemNumThreshold = 10; + + public Func inRange; + public Func, T> average; + private readonly List _inputData; + private Dictionary> _clusters = new Dictionary>(); + + public Clustering(List data, Func inRange, Func, T> average) + { + _inputData = data; + this.inRange = inRange; + this.average = average; + } + + public Dictionary> GetClusters() + { + var tmp = new List<(T center, List items)>(); + + for (var iter = 0; iter < numIteration; iter++) + { + tmp = tmp.Where(cluster => cluster.items.Count > itemNumThreshold) + .Select(cluster => (average(cluster.items), new List())).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() { 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; + } + + /// + /// 将角度转化至-180到180度的范围内。 + /// + /// + /// + public static double RoundTh(double th) + { + return th - System.Math.Round(th / 360) * 360; + } + + /// + /// 将角度转化至-180到180度的范围内。 + /// + /// + /// + public static float RoundTh(float th) + { + return th - (float)System.Math.Round(th / 360f) * 360f; + } + } +} diff --git a/CommonUsage-MultiVehicleSync/commonusage/Mathematics/LineSegment.cs b/CommonUsage-MultiVehicleSync/commonusage/Mathematics/LineSegment.cs new file mode 100644 index 0000000..87e0405 --- /dev/null +++ b/CommonUsage-MultiVehicleSync/commonusage/Mathematics/LineSegment.cs @@ -0,0 +1,161 @@ +using System; +using System.Numerics; + +namespace CommonUsage.Mathematics +{ + /// + /// 表示一条线段。 + /// + public class LineSegment + { + /// + /// 默认构造函数。所有坐标初始化为0。 + /// + public LineSegment() + { + + } + + /// + /// 使用两个端点初始化一段2D线段。 + /// + /// 线段起点。 + /// 线段终点。 + public LineSegment(Vector2 src, Vector2 dst) + { + Src = src; + Dst = dst; + } + + /// + /// 使用两个端点初始化一段3D线段。 + /// + /// 线段起点。 + /// 线段终点。 + public LineSegment(Vector3 src, Vector3 dst) + { + Src3D = src; + Dst3D = dst; + } + + /// + /// 使用两个端点初始化一条线段，2D。 + /// + /// 线段起点x坐标。 + /// 线段起点y坐标。 + /// 线段终点x坐标。 + /// 线段终点y坐标。 + public LineSegment(double x1, double y1, double x2, double y2) + { + Src = new Vector2((float)x1, (float)y1); + Dst = new Vector2((float)x2, (float)y2); + } + + /// + /// 使用两个端点初始化一条线段，2D。 + /// + /// 线段起点x坐标。 + /// 线段起点y坐标。 + /// 线段起点y坐标。 + /// 线段终点x坐标。 + /// 线段终点y坐标。 + /// 线段终点y坐标。 + 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); + } + + /// + /// 返回线段长度，2D。 + /// + /// + public double Length() + { + return Vector2.Distance(Src, Dst); + } + + /// + /// 返回线段长度，3D。 + /// + /// + public double Length3D() + { + return Vector3.Distance(Src3D, Dst3D); + } + + /// + /// 返回线段与x轴正方向夹角度数，角度制。 + /// + /// + public double Angle() + { + return Math.Atan2(Dst.Y - Src.Y, Dst.X - Src.X) / Math.PI * 180; + } + + /// + /// 返回一段方向相反的线段。 + /// + /// + public LineSegment Reverse() + { + return new LineSegment(Dst3D, Src3D); + } + + /// + /// 线段2D起点。 + /// + public Vector2 Src + { + get => new(_srcX, _srcY); + set + { + _srcX = value.X; + _srcY = value.Y; + } + } + + /// + /// 线段2D终点。 + /// + public Vector2 Dst + { + get => new(_dstX, _dstY); + set + { + _dstX = value.X; + _dstY = value.Y; + } + } + + /// + /// 线段3D起点。 + /// + public Vector3 Src3D + { + get => new(_srcX, _srcY, _srcZ); + set + { + _srcX = value.X; + _srcY = value.Y; + _srcZ = value.Z; + } + } + + /// + /// 线段3D终点。 + /// + 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; + } +} diff --git a/CommonUsage-MultiVehicleSync/commonusage/Properties/launchSettings.json b/CommonUsage-MultiVehicleSync/commonusage/Properties/launchSettings.json new file mode 100644 index 0000000..53c176e --- /dev/null +++ b/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\\" + } + } +} \ No newline at end of file diff --git a/CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/CommunicationProtocolFactory.cs b/CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/CommunicationProtocolFactory.cs new file mode 100644 index 0000000..3376c97 --- /dev/null +++ b/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") + }; + } + } +} diff --git a/CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/HTTPCommunication.cs b/CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/HTTPCommunication.cs new file mode 100644 index 0000000..5ff34a8 --- /dev/null +++ b/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 orderReceived) + { + PicoHttpServer.AddPostTextHandler("/order", new { }, (_, str) => + { + var order = JsonConvert.DeserializeObject(str); + orderReceived(order); + return ""; + }); + } + + public void SetUpInstanActionListener(Action onInstanceActionReceived) + { + PicoHttpServer.AddPostTextHandler("/instanceAction", new { }, (_, str) => + { + var instanceAction = JsonConvert.DeserializeObject(str); + onInstanceActionReceived(instanceAction); + return ""; + }); + } + + public void SetUpImmediateCommandListener(Action onChangeCarFieldsReceived) + { + throw new NotImplementedException(); + } + + public async Task SendMessageAsync(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 msg, string topic) + { + throw new NotImplementedException(); + } + + public void SetupTestListener(Action testMsg) + { + throw new NotImplementedException(); + } + public async Task PublishFactSheet(factsheetMessage message) + { + throw new NotImplementedException(); + } + } +} diff --git a/CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/IVDACommunicationProtocol.cs b/CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/IVDACommunicationProtocol.cs new file mode 100644 index 0000000..f8e998f --- /dev/null +++ b/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 orderReceived); + void SetUpInstanActionListener(Action onInstanceActionReceived); + void SetUpImmediateCommandListener(Action onChangeCarFieldsReceived); + Task SendMessageAsync(T message, string topic); + Task SendVisualizationMessageAsync(T message, string topic); + } +} diff --git a/CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/MQTTCommunication.cs b/CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/MQTTCommunication.cs new file mode 100644 index 0000000..133fd46 --- /dev/null +++ b/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 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(payload); + Console.WriteLine($"Change car field: {payload}"); + onChangeCarFieldsReceived(payload); + } + }; + } + + + public void SetUpInstanActionListener(Action 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(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 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(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 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 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}"); + } + } + } + } +} diff --git a/CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/Messages/connectionMessage.cs b/CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/Messages/connectionMessage.cs new file mode 100644 index 0000000..a4f5c70 --- /dev/null +++ b/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'} + + } + +} diff --git a/CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/Messages/errorMessage.cs b/CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/Messages/errorMessage.cs new file mode 100644 index 0000000..9888fe2 --- /dev/null +++ b/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 + } + +} diff --git a/CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/Messages/factsheetMessage.cs b/CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/Messages/factsheetMessage.cs new file mode 100644 index 0000000..bcbd349 --- /dev/null +++ b/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; + } + +} diff --git a/CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/Messages/instanceAction.cs b/CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/Messages/instanceAction.cs new file mode 100644 index 0000000..e5c5f8d --- /dev/null +++ b/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 actions { get; set; } + } +} diff --git a/CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/Messages/orderMessage.cs b/CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/Messages/orderMessage.cs new file mode 100644 index 0000000..68516c7 --- /dev/null +++ b/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; } + } +} + diff --git a/CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/Messages/stateMessage.cs b/CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/Messages/stateMessage.cs new file mode 100644 index 0000000..44b0438 --- /dev/null +++ b/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 +{ + /// + /// 6.10 Topic: "state" (from AGV to master control) + /// todo: complete all fields required by VDA5050 + /// + public class stateMessage + { + public uint headerId; + public string timestamp = ""; + public string version = ""; + public string manufacturer = ""; + public string serialNumber = ""; + /// + /// 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. + /// + public string orderId = ""; + + /// + /// Order update identification to identify, that an order update has been accepted by the AGV. + /// "0" if no previous orderUpdateId is available. + /// + public uint orderUpdatedId = 0; + + public string lastNodeId; + + public uint lastNodeSequenceId; + + /// + /// Array of nodeState objects that need to be traversed for fulfilling the order (empty array if idle) + /// + public nodeState[] nodeStates = []; + + /// + /// Array of edgeState objects that need to be traversed for fulfilling the order (empty array if idle) + /// + 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(); + public string operatingMode = ""; + public List errors { get; set; } = new List(); // Array of errorState objects + + public info[] information = []; + public safetyState safetyState; + + } +} diff --git a/CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/Messages/visualizationMessage.cs b/CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/Messages/visualizationMessage.cs new file mode 100644 index 0000000..c895311 --- /dev/null +++ b/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; + } +} diff --git a/CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/Objects/action.cs b/CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/Objects/action.cs new file mode 100644 index 0000000..e4212b4 --- /dev/null +++ b/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; } + + } +} diff --git a/CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/Objects/actionState.cs b/CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/Objects/actionState.cs new file mode 100644 index 0000000..fe454e2 --- /dev/null +++ b/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 + } + + } + +} \ No newline at end of file diff --git a/CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/Objects/agvGeometry.cs b/CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/Objects/agvGeometry.cs new file mode 100644 index 0000000..d96e7b7 --- /dev/null +++ b/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 wheelDefinitions { get; set; } = new List(); + + // 2D Envelopes + public List envelopes2D { get; set; } = new List(); + + // 3D Envelopes + public List envelopes3D { get; set; } = new List(); + + 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 polygonPoints { get; set; } = new List(); + 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; } + } + } + +} + diff --git a/CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/Objects/agvPosition.cs b/CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/Objects/agvPosition.cs new file mode 100644 index 0000000..8659cd3 --- /dev/null +++ b/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 = ""; + } +} diff --git a/CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/Objects/batteryState.cs b/CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/Objects/batteryState.cs new file mode 100644 index 0000000..1d3392c --- /dev/null +++ b/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; + } +} diff --git a/CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/Objects/boundingBoxReference.cs b/CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/Objects/boundingBoxReference.cs new file mode 100644 index 0000000..be48032 --- /dev/null +++ b/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 + } +} diff --git a/CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/Objects/controlPoint.cs b/CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/Objects/controlPoint.cs new file mode 100644 index 0000000..03300b8 --- /dev/null +++ b/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; + } + } +} diff --git a/CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/Objects/edge.cs b/CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/Objects/edge.cs new file mode 100644 index 0000000..e207eb5 --- /dev/null +++ b/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 controlPoints; + // + // public List weights; + + public action[] action = []; + } +} diff --git a/CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/Objects/edgeState.cs b/CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/Objects/edgeState.cs new file mode 100644 index 0000000..e5d8af3 --- /dev/null +++ b/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; + } +} diff --git a/CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/Objects/errorReference.cs b/CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/Objects/errorReference.cs new file mode 100644 index 0000000..f44d5c7 --- /dev/null +++ b/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 + } +} diff --git a/CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/Objects/errorState.cs b/CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/Objects/errorState.cs new file mode 100644 index 0000000..227ed80 --- /dev/null +++ b/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 errorReferences { get; set; } = new List(); // 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 + } +} diff --git a/CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/Objects/info.cs b/CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/Objects/info.cs new file mode 100644 index 0000000..a638ec8 --- /dev/null +++ b/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 infoReferences { get; set; } = new List(); // 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 + } + } +} diff --git a/CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/Objects/load.cs b/CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/Objects/load.cs new file mode 100644 index 0000000..91530d0 --- /dev/null +++ b/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 ∞) + } +} diff --git a/CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/Objects/loadDimensions.cs b/CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/Objects/loadDimensions.cs new file mode 100644 index 0000000..90469c6 --- /dev/null +++ b/CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/Objects/loadDimensions.cs @@ -0,0 +1,13 @@ +using System; +using System.Collections.Generic; +using System.Text; + +namespace CommonUsage.Protocols.VDA5050.Objects +{ + public class loadDimensions + { + public double Length { get; set; } // Length of the bounding box + public double Width { get; set; } // Width of the bounding box + public double Height { get; set; } // Height of the bounding box (optional) + } +} diff --git a/CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/Objects/node.cs b/CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/Objects/node.cs new file mode 100644 index 0000000..c6617cf --- /dev/null +++ b/CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/Objects/node.cs @@ -0,0 +1,17 @@ +using System; +using System.Collections.Generic; +using System.Text; + +namespace CommonUsage.Protocols.VDA5050.Objects +{ + public class node : sequenceItem + { + public string nodeId; + + public string nodeDescription; + + public nodePosition nodePosition; + + public action[] actions = []; + } +} diff --git a/CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/Objects/nodePosition.cs b/CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/Objects/nodePosition.cs new file mode 100644 index 0000000..a1c362a --- /dev/null +++ b/CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/Objects/nodePosition.cs @@ -0,0 +1,36 @@ +using System; +using System.Collections.Generic; +using System.Text; + +namespace CommonUsage.Protocols.VDA5050.Objects +{ + /// + /// Defines the position on a map in a global project-specific world coordinate system. + /// Each floor has its own map. + /// All maps shall use the same project-specific global origin. + /// + public class nodePosition + { + /// + /// X-position on the map in reference to the map coordinate system. + /// Precision is up to the specific implementation. + /// + public double x; + + /// + /// Y-position on the map in reference to the map coordinate system. + /// Precision is up to the specific implementation. + /// + public double y; + + /// + /// Range: [-Pi ... Pi] + /// Absolute orientation of the AGV on the node. + /// Optional: vehicle can plan the path by itself. If defined, the AGV has to assume the theta angle on this node. + /// If previous edge disallows rotation, the AGV shall rotate on the node. + /// If following edge has a differing orientation defined but disallows rotation, + /// the AGV is to rotate on the node to the edges desired rotation before entering the edge. + /// + public double theta; + } +} diff --git a/CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/Objects/nodeState.cs b/CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/Objects/nodeState.cs new file mode 100644 index 0000000..2a67bdf --- /dev/null +++ b/CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/Objects/nodeState.cs @@ -0,0 +1,26 @@ +using System; +using System.Collections.Generic; +using System.Text; + +namespace CommonUsage.Protocols.VDA5050.Objects +{ + public class nodeState : sequenceItem + { + /// + /// Unique node identification. + /// + public string nodeId; + + /// + /// Additional information on the node. + /// + public string nodeDescription; + + /// + /// Node position. + /// The object is defined in 6.6 Topic: "order" (from master control to AGV) + /// Optional: Master control has this information. Can be sent additionally, e.g., for debugging purposes. + /// + public nodePosition nodePosition; + } +} diff --git a/CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/Objects/physicalParameters.cs b/CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/Objects/physicalParameters.cs new file mode 100644 index 0000000..3b4e0ec --- /dev/null +++ b/CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/Objects/physicalParameters.cs @@ -0,0 +1,20 @@ +using System; +using System.Collections.Generic; +using System.Text; + +namespace CommonUsage.Protocols.VDA5050.Objects +{ + public class physicalParameters + { + public double speedMin; + public double speedMax; + public double angularSpeedMin; + public double angularSpeedMax; + public double accelerationMax; + public double decelerationMax; + public double heightMin; + public double heightMax; + public double width; + public double length; + } +} diff --git a/CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/Objects/protocolFeatures.cs b/CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/Objects/protocolFeatures.cs new file mode 100644 index 0000000..bcead23 --- /dev/null +++ b/CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/Objects/protocolFeatures.cs @@ -0,0 +1,10 @@ +using System; +using System.Collections.Generic; +using System.Text; + +namespace CommonUsage.Protocols.VDA5050.Objects +{ + public class protocolFeatures + { + } +} diff --git a/CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/Objects/protocolLimits.cs b/CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/Objects/protocolLimits.cs new file mode 100644 index 0000000..64c7df7 --- /dev/null +++ b/CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/Objects/protocolLimits.cs @@ -0,0 +1,10 @@ +using System; +using System.Collections.Generic; +using System.Text; + +namespace CommonUsage.Protocols.VDA5050.Objects +{ + public class protocolLimits + { + } +} diff --git a/CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/Objects/safetyState.cs b/CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/Objects/safetyState.cs new file mode 100644 index 0000000..5f67a98 --- /dev/null +++ b/CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/Objects/safetyState.cs @@ -0,0 +1,20 @@ +using System; +using System.Collections.Generic; +using System.Text; + +namespace CommonUsage.Protocols.VDA5050.Objects +{ + public class safetyState + { + public eStopEnum eStop { get; set; } // Emergency stop status + public bool fieldViolation { get; set; } // "true" if a safety field is violated, "false" otherwise + + public enum eStopEnum + { + AUTOACK, // Auto-acknowledged emergency stop (e.g., triggered by a bumper) + MANUAL, // Manually confirmed emergency stop + REMOTE, // Remote-confirmed emergency stop + NONE // No emergency stop activated + } + } +} diff --git a/CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/Objects/sequenceItem.cs b/CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/Objects/sequenceItem.cs new file mode 100644 index 0000000..b77f535 --- /dev/null +++ b/CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/Objects/sequenceItem.cs @@ -0,0 +1,13 @@ +using System; +using System.Collections.Generic; +using System.Text; + +namespace CommonUsage.Protocols.VDA5050.Objects +{ + public class sequenceItem + { + public uint sequenceId; + + public bool released; + } +} diff --git a/CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/Objects/trajectory.cs b/CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/Objects/trajectory.cs new file mode 100644 index 0000000..1b792c4 --- /dev/null +++ b/CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/Objects/trajectory.cs @@ -0,0 +1,15 @@ +using System; +using System.Collections.Generic; +using System.Text; + +namespace CommonUsage.Protocols.VDA5050.Objects +{ + public class trajectory + { + public float degree; + + public float[] knotVector; + + public controlPoint[] controlPoints; + } +} diff --git a/CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/Objects/typeSpecification.cs b/CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/Objects/typeSpecification.cs new file mode 100644 index 0000000..1f8d6e9 --- /dev/null +++ b/CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/Objects/typeSpecification.cs @@ -0,0 +1,15 @@ +using System; +using System.Collections.Generic; +using System.Text; + +namespace CommonUsage.Protocols.VDA5050.Objects +{ + public class typeSpecification + { + public string agvKinemantic = ""; + public string agvClass = ""; + public double maxLoadMass; + public string[] localizationTypes; // Simplified description of localization type (e.g., NATURAL, REFLECTOR, RFID, DMC, GRID) + public string[] navigationTypes; // Path planning types (e.g., 'AUTONOMOUS', 'VIRTUAL_LINE_GUIDED') + } +} diff --git a/CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/Objects/velocity.cs b/CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/Objects/velocity.cs new file mode 100644 index 0000000..d774ba1 --- /dev/null +++ b/CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/Objects/velocity.cs @@ -0,0 +1,15 @@ +using System; +using System.Collections.Generic; +using System.Text; + +namespace CommonUsage.Protocols.VDA5050.Objects +{ + public class velocity + { + public double vx; + + public double vy; + + public double omega; + } +} diff --git a/CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/VDA5050Basic.cs b/CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/VDA5050Basic.cs new file mode 100644 index 0000000..029348c --- /dev/null +++ b/CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/VDA5050Basic.cs @@ -0,0 +1,118 @@ +//using CommonUsage.Protocols.VDA5050.Messages; +//using System; +//using System.Collections.Generic; +//using System.Net.Http.Headers; +//using System.Runtime.CompilerServices; +//using System.Text; +//using System.Threading; +//using System.Threading.Tasks; +//using CommonUsage.Protocols.VDA5050.Objects; +//using ClumsyCore.Utilities; +//using System.Linq; +//using ClumsyCore.Pilot; +//using System.Numerics; + +//namespace CommonUsage.Protocols.VDA5050 +//{ +// public abstract class VDA5050Basic +// { +// protected static IVDACommunicationProtocol _communicationProtocol; + +// public void Enable(IVDACommunicationProtocol protocol) +// { +// _communicationProtocol = protocol; +// Task.Run(() => ManageConnection()); +// StartVisualizationLoop(); +// } +// private void StartVisualizationLoop() +// { +// Console.WriteLine("Visualization in CommonUsage"); +// new Thread(async () => +// { +// while (true) +// { +// var position = GetAGVPosition(); +// if (position != null) +// { +// var msg = new stateMessage() +// { +// serialNumber = "test-01", +// agvPosition = new() +// { +// x = position.Value.X, +// y = position.Value.Y, +// theta = position.Value.Theta, +// positionInitialized = true +// } + +// }; +// await _communicationProtocol.SendMessageAsync(msg, "vda5050/frldAGV/visualization"); +// } + +// Thread.Sleep(100); +// } +// }) +// { Name = "VDA5050TopicVisualization" }.Start(); +// } + +// private void ManageConnection() +// { +// while (true) +// { +// var status = CheckConnectionStatus() ? "ONLINE" : "OFFLINE"; +// _communicationProtocol.PublishConnectionStatus(status); +// Thread.Sleep(1000); +// } +// } + +// protected List OrganizeReceivedSequence(orderMessage order) +// { +// List receivedSequence = new(); + +// int ii = 0, jj = 0; +// while (true) +// { +// var edge = order.edges[ii]; +// var node = order.nodes[jj]; +// var takeEdge = edge.sequenceId < node.sequenceId; + +// if (takeEdge) +// { +// receivedSequence.Add(edge); +// ii++; +// if (ii == order.edges.Length) break; +// } +// else +// { +// receivedSequence.Add(node); +// jj++; +// if (jj == order.nodes.Length) break; +// } +// } +// for (var i = ii; i < order.edges.Length; ++i) receivedSequence.Add(order.edges[i]); +// for (var j = jj; j < order.nodes.Length; ++j) receivedSequence.Add(order.nodes[j]); + +// for (var i = 1; i < receivedSequence.Count; i++) +// { +// if (receivedSequence[i - 1].sequenceId + 1 != receivedSequence[i].sequenceId) +// throw new Exception("stateMessage not continuous!"); +// } + +// return receivedSequence; +// } + +// public virtual bool CheckConnectionStatus() +// { +// return false; +// } + +// protected abstract Vector3? GetAGVPosition(); +// public struct Vector3 +// { +// public double X; +// public double Y; +// public double Theta; +// } +// } + +//} diff --git a/CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/VDA5050Helper.cs b/CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/VDA5050Helper.cs new file mode 100644 index 0000000..9f5217f --- /dev/null +++ b/CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/VDA5050Helper.cs @@ -0,0 +1,11 @@ +using System; +using System.Collections.Generic; +using System.Text; + +namespace CommonUsage.Protocols.VDA5050 +{ + public class VDA5050Helper + { + + } +} diff --git a/CommonUsage-MultiVehicleSync/commonusage/Visualizer.cs b/CommonUsage-MultiVehicleSync/commonusage/Visualizer.cs new file mode 100644 index 0000000..f17c27d --- /dev/null +++ b/CommonUsage-MultiVehicleSync/commonusage/Visualizer.cs @@ -0,0 +1,28 @@ +using System; +using System.Collections.Generic; +using System.Drawing; +using System.Numerics; +using System.Text; + +namespace CommonUsage +{ + public class Visualizer + { + public Action LineAction; + + public Action TextAction; + + public Action Clear; + + public void DrawLine(Color color, Vector2 src, Vector2 dst, bool startArrow = false, bool endArrow = false, + int width = 1) + { + LineAction?.Invoke(color, src, dst, startArrow, endArrow, width); + } + + public void DrawText(Color color, string text, Vector2 pos) + { + TextAction?.Invoke(color, text, pos); + } + } +} diff --git a/MedullaAdapter/AlarmRoutine.cs b/MedullaAdapter/AlarmRoutine.cs new file mode 100644 index 0000000..990408d --- /dev/null +++ b/MedullaAdapter/AlarmRoutine.cs @@ -0,0 +1,15 @@ +// 驱动器、急停、夹臂等安全报警 +using MDCSToolBox.Medulla.Chassis.MultiWheel; + +namespace MedullaAdapter +{ + public class AlarmRoutine : MultiWheelAlarmRoutine + { + // M层单车安全：汇总停车机器人夹臂等自定义报警状态。 + public override void SetOtherAlarms() + { + AddAlarm("左夹臂驱动报警", 2, () => cart.LeftArmErrorCode != 0); + AddAlarm("右夹臂驱动报警", 2, () => cart.RightArmErrorCode != 0); + } + } +} \ No newline at end of file diff --git a/MedullaAdapter/DiverCartDefinition.cs b/MedullaAdapter/DiverCartDefinition.cs new file mode 100644 index 0000000..49e7c2d --- /dev/null +++ b/MedullaAdapter/DiverCartDefinition.cs @@ -0,0 +1,532 @@ +// 定义车型、上下层IO、参数和MCU初始化 +using CartActivator; +using MCUSerialBridgeCLR; +using MDCSToolBox.Medulla.Chassis.MultiWheel; +using Medulla.Types; +using System; +using System.Collections.Generic; +using System.Threading; +using MyParking.Shared; + +namespace MedullaAdapter +{ + [UseLadderLogic(logic = typeof(AlarmRoutine), scanInterval = 50)] + [UseLadderLogic(logic = typeof(MotorRoutine), scanInterval = 50)] + [UseLadderLogic(logic = typeof(MCURoutine), scanInterval = 20)] + [UseManualController(manualController = typeof(Remote))] + public class DiverCartDefinition : MultiWheelCartDefinition + { + #region 基本成员 + public MCUSerialBridge Bridge; + + internal enum ManualControlMode + { + Normal = 0, // 正常模式 + Crab = 1, // 螃蟹模式 + Spin = 2, // 自旋模式 + } + internal ManualControlMode TransmitterControlMode = ManualControlMode.Normal; + internal DateTime TransmitterLastTime = DateTime.Now; // 物理遥控器计算两次实体遥控器指令之间的时间间隔 + private ManualControlMode? _pendingManualMode; + private ManualControlMode? _activeManualMode; + #endregion + + #region AsUpperIO + [AsUpperIO(desc = "从C上复位")] public bool ResetFromC; + [AsUpperIO(desc = "从C将驱动轮下使能")] public bool DisableFromC; + [AsUpperIO(desc = "左夹臂下发速度", timeOutReset = true)] public float SpeedLeftArm; + [AsUpperIO(desc = "右夹臂下发速度", timeOutReset = true)] public float SpeedRightArm; + [AsUpperIO(desc = "夹臂不同步报警")] public bool ClampOutOfSync; + #endregion + + #region AsLowerIO + [AsLowerIO(desc = "左前左轮实际位置")] public float LFLActualPos; + [AsLowerIO(desc = "左前右轮实际位置")] public float LFRActualPos; + [AsLowerIO(desc = "右前左轮实际位置")] public float RFLActualPos; + [AsLowerIO(desc = "右前右轮实际位置")] public float RFRActualPos; + [AsLowerIO(desc = "左后左轮实际位置")] public float LRLActualPos; + [AsLowerIO(desc = "左后右轮实际位置")] public float LRRActualPos; + [AsLowerIO(desc = "右后左轮实际位置")] public float RRLActualPos; + [AsLowerIO(desc = "右后右轮实际位置")] public float RRRActualPos; + [AsLowerIO(desc = "左夹臂实际速度")] public float ActualSpeedLeftArm; + [AsLowerIO(desc = "右夹臂实际速度")] public float ActualSpeedRightArm; + [AsLowerIO(desc = "左夹臂状态字")] public int LeftArmStateCode; + [AsLowerIO(desc = "右夹臂状态字")] public int RightArmStateCode; + [AsLowerIO(desc = "左夹臂错误字")] public int LeftArmErrorCode; + [AsLowerIO(desc = "右夹臂错误字")] public int RightArmErrorCode; + [AsLowerIO(desc = "左夹臂电流")] public float LeftArmElectric; + [AsLowerIO(desc = "右夹臂电流")] public float RightArmElectric; + [AsLowerIO(desc = "左夹臂实际位置")] public float ActualPosLeftArm; + [AsLowerIO(desc = "右夹臂实际位置")] public float ActualPosRightArm; + [AsLowerIO(desc = "驱动轮使能状态")] public bool WheelAbleState = true; + [AsLowerIO(desc = "电池健康状态")] public float SOH; + [AsInitParam(desc = "车号")][AsLowerIO] public int CarNum = 1; + #endregion + + #region 初始参数 + [AsInitParam(desc = "MCU端口号")] public string MCUPort = "COM4"; + [AsInitParam(desc = "遥控器速度上限")] public float TransmitterSpeedUpperLimit = 1.0f; + [AsInitParam(desc = "遥控器速度下限")] public float TransmitterSpeedLowerLimit = 0.0f; + [AsInitParam(desc = "手动控制夹臂速度系数")] public float ManualArmSpeedFac = 1.0f; + [AsInitParam(desc = "遥控转弯舵角同步限速宽度，单位为度")] + public float ManualSteeringAlignmentSigmaDegrees = 8.0f; + [AsInitParam(desc = "自转最大角速度，单位deg/s")] + public float MaxSpinAngularSpeedDegreesPerSecond = 30f; + [AsInitParam(desc = "轮速诊断日志相对目录")] + public string WheelSpeedDiagnosticDirectory = + @"logs\wheel-speed"; + [AsInitParam(desc = "左夹臂低限位")][AsLowerIO] public int LeftArmLowerPos = -10000; + [AsInitParam(desc = "左夹臂高限位")][AsLowerIO] public int LeftArmUpperPos = 5927610; + [AsInitParam(desc = "右夹臂低限位")][AsLowerIO] public int RightArmLowerPos = -17295; + [AsInitParam(desc = "右夹臂高限位")][AsLowerIO] public int RightArmUpperPos = 5927610; + + + #endregion + + #region 监控参数 + [IOObjectMonitor(desc = "从M上复位")] public bool ResetFromM; + [IOObjectMonitor(desc = "从M将驱动轮下使能")] public bool DisableFromM; + [IOObjectMonitor(desc = "左前左轮PID修正后速度")] public float SpeedLFL; + [IOObjectMonitor(desc = "左前右轮PID修正后速度")] public float SpeedLFR; + [IOObjectMonitor(desc = "右前左轮PID修正后速度")] public float SpeedRFL; + [IOObjectMonitor(desc = "右前右轮PID修正后速度")] public float SpeedRFR; + [IOObjectMonitor(desc = "左后左轮PID修正后速度")] public float SpeedLRL; + [IOObjectMonitor(desc = "左后右轮PID修正后速度")] public float SpeedLRR; + [IOObjectMonitor(desc = "右后左轮PID修正后速度")] public float SpeedRRL; + [IOObjectMonitor(desc = "右后右轮PID修正后速度")] public float SpeedRRR; + [IOObjectMonitor(desc = "左前舵轮转向PID输出")] public float DiffSteerOutputLeftFront; + [IOObjectMonitor(desc = "左后舵轮转向PID输出")] public float DiffSteerOutputLeftRear; + [IOObjectMonitor(desc = "右前舵轮转向PID输出")] public float DiffSteerOutputRightFront; + [IOObjectMonitor(desc = "右后舵轮转向PID输出")] public float DiffSteerOutputRightRear; + [IOObjectMonitor(desc = "灯光模式")] public int LightMode = 0; + [IOObjectMonitor(desc = "实体遥控器当前速度倍率")] public float TransmitterSpeed = 0.3f; + [IOObjectMonitor(desc = "轮速诊断记录已启用")] + public bool WheelSpeedDiagnosticEnabled; + [IOObjectMonitor(desc = "轮速诊断记录状态")] + public string WheelSpeedDiagnosticStatus = "未启动"; + [IOObjectMonitor(desc = "左前左驱动器远程帧701")] public byte LFLRemoteCode = 0; + [IOObjectMonitor(desc = "左前右驱动器远程帧702")] public byte LFRRemoteCode = 0; + [IOObjectMonitor(desc = "右前左驱动器远程帧703")] public byte RFLRemoteCode = 0; + [IOObjectMonitor(desc = "右前右驱动器远程帧704")] public byte RFRRemoteCode = 0; + [IOObjectMonitor(desc = "左后左驱动器远程帧705")] public byte LRLRemoteCode = 0; + [IOObjectMonitor(desc = "左后右驱动器远程帧706")] public byte LRRRemoteCode = 0; + [IOObjectMonitor(desc = "右后左驱动器远程帧707")] public byte RRLRemoteCode = 0; + [IOObjectMonitor(desc = "右后右驱动器远程帧708")] public byte RRRRemoteCode = 0; + [IOObjectMonitor(desc = "左夹臂驱动器远程帧709")] public byte LArmRemoteCode = 0; + [IOObjectMonitor(desc = "右夹臂驱动器远程帧70A")] public byte RArmRemoteCode = 0; + #endregion + + #region 操作按钮 + // M层单车硬件：向驱动轮发送复位请求。 + [IOObjectUtility] + public void WheelReset() + { + ResetFromM = true; + } + // M层单车硬件：向驱动轮发送下使能请求。 + [IOObjectUtility] + public void WheelDisable() + { + DisableFromM = true; + } + + // M层诊断：请求开始保存CAN轮速事件和底盘周期快照。 + [IOObjectUtility] + public void StartWheelSpeedDiagnostic() + { + WheelSpeedDiagnosticEnabled = true; + WheelSpeedDiagnosticStatus = "等待创建日志文件"; + } + + // M层诊断：请求停止轮速记录并刷新CSV文件。 + [IOObjectUtility] + public void StopWheelSpeedDiagnostic() + { + WheelSpeedDiagnosticEnabled = false; + WheelSpeedDiagnosticStatus = "等待停止并刷新日志"; + } + #endregion + + public override void CommunicationInit() + { + if (GhostMode) return; + State = -1; + Bridge = new MCUSerialBridge(); + //Step1:打开指定串口连接 + var err = Bridge.Open(MCUPort, 1000000u); + if (err != MCUSerialBridgeError.OK) + { + Console.WriteLine($"MCU Open FAILED: {err.ToDescription()}"); + return; + } + else + { + Console.WriteLine("MCU Open OK"); + } + //Step2:远程复位MCU + err = Bridge.Reset(); + if (err != MCUSerialBridgeError.OK) + { + Console.WriteLine($"MCU Reset FAILED: {err.ToDescription()}"); + return; + } + else + { + Thread.Sleep(500); + Console.WriteLine("MCU Reset OK"); + } + //Step3:获取MCU版本号 + err = Bridge.GetVersion(out var version, 100); + if (err != MCUSerialBridgeError.OK) + { + Console.WriteLine($"MCU GetVersion FAILED: {err.ToDescription()}"); + return; + } + else + { + Console.WriteLine($"MCU GetVersion OK: {version}"); + } + //Step4:获取MCU状态 + err = Bridge.GetState(out var state, 100); + if (err != MCUSerialBridgeError.OK) + { + Console.WriteLine($"MCU GetState FAILED: {err.ToDescription()}"); + return; + } + else + { + Console.WriteLine($"MCU GetState OK: {state}"); + } + //Step5:串口/CAN配置 + try + { + var ports = new List(); + for (int i = 0; i < 1; i++) + ports.Add(new CANPortConfig(500000, 10)); + for (int i = 0; i < 3; i++) + ports.Add(new SerialPortConfig(9600, 10)); + Console.WriteLine("=== Port Configuration ==="); + for (int i = 0; i < ports.Count; i++) + { + if (ports[i] is SerialPortConfig s) + Console.WriteLine($"Port {i}: Serial, Baud={s.Baud}, ReceiveFrameMs={s.ReceiveFrameMs}"); + else if (ports[i] is CANPortConfig c) + Console.WriteLine($"Port {i}: CAN, Baud={c.Baud}, RetryTimeMs={c.RetryTimeMs}"); + } + + var ret = Bridge.Configure(ports, 200); + if (ret != MCUSerialBridgeError.OK) + { + Console.WriteLine($"MCU Configure FAILED: {ret.ToDescription()}"); + return; + } + else + { + Console.WriteLine("MCU Configure OK"); + } + Console.WriteLine("MCU Configure {0}", ret == MCUSerialBridgeError.OK ? "OK" : $"FAILED: 0x{(uint)ret:X8}"); + } + catch (Exception ex) + { + Console.WriteLine($"Configure Exception: {ex.Message}"); + return; + } + State = 0; + } + + internal void ManualControl( + ManualControlMode mode, + float x, + float y, + float frontDirection, + float speedThreshold, + TimeSpan? interval = null) + { + if (Chassis == null) return; + + var adapter = GetChassisAdapter(); + if (adapter == null) return; + + // 模式变化时先停车并下发舵轮准备角度； + // 在实际舵角到位之前，不开放驱动速度。 + if (!EnsureManualModeReady(mode, interval)) + { + adapter.StopImmediately(); + return; + } + + var speed = speedThreshold * y; + var normalizedSteering = + (float)Math.Pow( + Math.Abs(x), + ManualThetaPow) * + Math.Sign(x); + var steeringDegrees = + -normalizedSteering * MaxManualTheta; + var frontTh = steeringDegrees; + var rearTh = -steeringDegrees; + ManualMode = (int)mode; + + switch (mode) + { + case ManualControlMode.Normal: + // 普通模式统一使用车体速度命令： + // X向前，行驶中连续改变角速度时舵轮边转、车辆边走。 + // SendBodyCommand( + // vx: speed, + // vy: 0.0, + // omegaRadiansPerSecond: omega, + // interval); + Chassis.SendMotion( + speed, + frontTh, + rearTh, + interval); + break; + case ManualControlMode.Crab: + // 舵轮机械范围为[-120°,120°]。 + // 蟹行后虚拟轴距由原车宽度决定，比正常模式轴距短。 + // 按几何比例缩小转角，使相同摇杆输入获得接近一致的曲率。 + var normalSteeringRadians = + AngleMath.DegreesToRadians(steeringDegrees); + var geometryRatio = + adapter.HalfTrackWidthMeters / + adapter.HalfWheelBaseMeters; + + // +90°运动坐标系已经把虚拟左侧映射为车体后方， + // 此处保持普通模式的转向符号，避免再次取反导致左右颠倒。 + var crabSteeringRadians = + Math.Atan( + geometryRatio * + Math.Tan( + normalSteeringRadians)); + + // 蟹行转角最终限制为±30°，为±120°机械舵角保留余量。 + var maximumCrabSteeringRadians = + AngleMath.DegreesToRadians(30.0); + crabSteeringRadians = Math.Max( + -maximumCrabSteeringRadians, + Math.Min( + maximumCrabSteeringRadians, + crabSteeringRadians)); + + // 将车体左侧作为虚拟阿克曼车头，并在该运动坐标系中 + // 复用与普通模式相同的SendMotion前后控制点解算。 + if (!adapter.SendVirtualAckermannMotion( + motionDirectionRadians: + Math.PI / 2.0, + speedMetersPerSecond: + speed, + steeringRadians: + crabSteeringRadians, + interval)) + { + adapter.StopImmediately(); + + Console.WriteLine( + "蟹行SendMotion命令分解失败，车辆已经停车：" + + adapter.LastFailureReason); + } + break; + case ManualControlMode.Spin: + // 摇杆处于中位时只清零驱动速度，保持已经准备好的 + // 自转舵角；下次推动摇杆时仍会重新检查实际舵角。 + if (Math.Abs(speed) < 1e-6f) + { + adapter + .StopXYThDrivePreserveSteeringState(); + break; + } + + // 自转时speed表示最外侧舵轮中心的目标切向速度， + // 根据v=omega*r换算为SendXYThSpeed需要的角速度。 + var requestedSpinOmegaRadiansPerSecond = + speed / + adapter.MaximumWheelRadiusMeters; + + // 对半径换算结果做正负对称限幅，防止遥控速度参数误设后自转过快。 + var maximumSpinOmegaRadiansPerSecond = + AngleMath.DegreesToRadians( + Math.Max( + 0f, + MaxSpinAngularSpeedDegreesPerSecond)); + + var spinOmegaRadiansPerSecond = + Math.Max( + -maximumSpinOmegaRadiansPerSecond, + Math.Min( + maximumSpinOmegaRadiansPerSecond, + requestedSpinOmegaRadiansPerSecond)); + + // 普通安全版SendXYThSpeed只下发角速度， + // 四轮实际舵角未到位时不会开放驱动速度。 + if (!adapter.Send( + new ChassisCommand( + CarNum, + new Twist2D( + 0.0, + 0.0, + spinOmegaRadiansPerSecond)), + interval)) + { + adapter.StopImmediately(); + + Console.WriteLine( + "SendXYThSpeed原地自转命令分解失败，车辆已经停车：" + + adapter.LastFailureReason); + } + break; + default: + ManualMode = -1; + adapter.StopImmediately(); + break; + } + } + + // 停车后切换模式：先预转舵轮，实际角度到位后才允许发送运动命令。 + private bool EnsureManualModeReady( + ManualControlMode mode, + TimeSpan? interval) + { + var adapter = GetChassisAdapter(); + if (adapter == null) + return false; + + // 当前模式已经完成准备，可以直接接受运动命令。 + if (_activeManualMode == mode && + _pendingManualMode == null) + { + return true; + } + + // 第一次收到新模式时，停车并下发一次舵轮准备姿态。 + if (_pendingManualMode != mode) + { + adapter.StopImmediately(); + // 所有模式的准备角度均按真实机械舵角表达； + // 先退出上一模式的虚拟运动坐标系，再执行预对齐。 + adapter.ResetToBodyFrame(); + _activeManualMode = null; + + var preparationAccepted = mode switch + { + ManualControlMode.Normal => + adapter.PrepareParallelDirection(0.0), + + ManualControlMode.Crab => + adapter.PrepareParallelDirection( + Math.PI / 2.0), + + ManualControlMode.Spin => + adapter.PrepareSpin(interval), + + _ => false + }; + + if (!preparationAccepted) + { + _pendingManualMode = null; + return false; + } + + _pendingManualMode = mode; + return false; + } + + // 后续控制周期保持停车，并读取实际舵角判断是否到位。 + adapter.StopImmediately(); + + var toleranceRadians = + AngleMath.DegreesToRadians(2.0); + + bool aligned; + + if (mode == ManualControlMode.Spin) + { + // 自转的四个舵轮目标角不同，等待期间持续刷新其目标。 + var preparationAccepted = + adapter.PrepareSpin(interval); + + aligned = + preparationAccepted && + adapter.AreSpinWheelsAligned; + } + else + { + var targetDirection = mode == + ManualControlMode.Crab + ? Math.PI / 2.0 + : 0.0; + + aligned = + adapter.AreParallelWheelsAligned( + targetDirection, + toleranceRadians); + } + + if (!aligned) + return false; + + if (mode == ManualControlMode.Spin) + { + // 四轮实际舵角确认到位后只交接一次，保留PrepareSpin + // 选定的机械舵角和轮速方向，避免首条XYTh命令重新选角。 + if (!adapter.AdoptPreparedSpinForXYTh( + toleranceRadians)) + { + return false; + } + } + // 蟹行轮子在真实车体系中到达机械+90°后， + // 再将车体左侧激活为SendMotion的虚拟X正方向。 + else if (mode == ManualControlMode.Crab) + { + adapter.ActivateMotionFrame( + Math.PI / 2.0); + } + else + { + adapter.ResetToBodyFrame(); + } + + _activeManualMode = mode; + _pendingManualMode = null; + return true; + } + + private MultiWheelChassisAdapter _chassisAdapter; + + private MultiWheelChassisAdapter GetChassisAdapter() + { + if (Chassis == null) + return null; + + if (_chassisAdapter == null || + _chassisAdapter.VehicleId != CarNum) + { + _chassisAdapter = + new MultiWheelChassisAdapter(Chassis, CarNum); + } + + _chassisAdapter.SteeringAlignmentSigmaDegrees = + Math.Max( + ManualSteeringAlignmentSigmaDegrees, + 0.1f); + + return _chassisAdapter; + } + + #region MCURoutine兼容参数（暂保留原硬件协议） + + // 保存MCU读取到的原始输入字节，供M层监控和硬件排查使用。 + [AsLowerIO(desc = "MCU原始输入字节")] + public float test; + + // 保留原MCU灯光分支；单车默认值-1表示使用本车LightMode。 + [AsUpperIO(desc = "多车灯光同步兼容值，-1使用本车灯光")] + public int MultiVehicleLightSync = -1; + + #endregion + + } +} diff --git a/MedullaAdapter/MCURoutine.cs b/MedullaAdapter/MCURoutine.cs new file mode 100644 index 0000000..e75ffb6 --- /dev/null +++ b/MedullaAdapter/MCURoutine.cs @@ -0,0 +1,1028 @@ +// 实际CAN协议、反馈解析、IO、电池、急停 +using CartActivator; +using FundamentalLib; +using MCUSerialBridgeCLR; +using System; +using System.Collections.Generic; +using System.IO; + +namespace MedullaAdapter +{ + public class MCURoutine : LadderLogic + { + private int _lastIteration = 0; + private int _count = 0; + private int _operationTime = 0; + private bool _driversDisabled; + private bool _canCallbackRegistered; + private bool _serialCallbackRegistered; + private byte _resetCode = 0x86; + private byte _enableCode1 = 0x06; + private byte _enableCode2 = 0x07; + private byte _enableCode3 = 0x0F; + private bool io_bit0 = false;//继电器 + private bool io_bit1 = false;//抱闸 + private bool io_bit2 = false;//红灯 + private bool io_bit3 = false;//绿灯 + private bool io_bit4 = false;//黄灯 + private const byte BatteryPortIndex = 3; + private static readonly byte[] BatteryRequest = BuildBatteryRequest(); + private readonly WheelSpeedDiagnosticLogger + _wheelSpeedLogger = + new WheelSpeedDiagnosticLogger(); + + // M层单车底盘：将车轮线速度换算为驱动电机转速。 + private static float ConvertMps2Rpm(float mps) + { + return (float)(mps / (Math.PI * 85f) * 10.5f * 60f * 1000f); + } + // M层单车底盘：将驱动电机转速换算为车轮线速度。 + private static float ConvertRpm2Mps(float rpm) + { + return (float)(rpm / 10.5f / 60f * Math.PI * 85f / 1000f); + } + // M层单车底盘：将夹臂或执行器转数换算为毫米位移。 + private float ConvertR2MM(float r) + { + return (float)(r / 10.5f * Math.PI * 85); + } + // M层CAN解析：从驱动器报文中解码有符号转速。 + private static float DecodeRpmFromPayload(byte[] payload, int offset = 4) + { + return BitConverter.ToInt32(payload, offset) * 1875f / 512f / 10000f; + } + // M层硬件主循环：交换IO、发送轮组指令并更新车辆反馈状态。 + public override void Operation(int iteration) + { + UpdateWheelSpeedDiagnosticState(); + + if (_lastIteration != iteration) + { + _lastIteration = iteration; + _count = 0; + } + else + { + _count++; + } + if (_count >= 15) + { + cart.AlarmLevel = 2; + Console.WriteLine("mcu lost connection \n"); + } + + if (_count >= 30) + { + return; + } + if (cart?.Bridge == null) + { + return; + } + //注册CAN回调 + EnsureCanCallbacksRegistered(); + //注册串口回调 + EnsureSerialCallbacksRegistered(); + PollBatterySerial(iteration); + + #region io部分 + var ioReadBuffer = new byte[4]; + var readInputErr = cart.Bridge.ReadInput(out ioReadBuffer, 20); + if (readInputErr == MCUSerialBridgeError.OK) + { + cart.test = ioReadBuffer[0]; + cart.Start = (ioReadBuffer[0] & (1 << 0)) != 0; + cart.ResetPressed = (ioReadBuffer[0] & (1 << 1)) != 0; + cart.ChassisMode = ((ioReadBuffer[0] & (1 << 2)) == 0) ? 0 : 1; + cart.BrakeEnable = (ioReadBuffer[0] & (1 << 3)) != 0; + if ((ioReadBuffer[0] & (1 << 4)) == 0) + { + cart.EmergencyPressed = 1; + } + } + else + { + Console.WriteLine("IO Read FAILED"); + } + + var ioWriteBuffer = new byte[4]; + io_bit0 = cart.ChargePort ? true : false; + io_bit1 = cart.EmergencyPressed == 1 ? false : true; + if (cart.MultiVehicleLightSync == 0) + { + io_bit2 = false; + io_bit3 = false; + io_bit4 = false; + } + else if (cart.MultiVehicleLightSync == 1) + { + io_bit2 = false; + io_bit3 = true; + io_bit4 = false; + } + else + { + if (cart.LightMode == 2) + { + io_bit2 = true;//红 + io_bit3 = false;//绿 + io_bit4 = false;//黄 + } + else if (cart.LightMode == 3) + { + io_bit2 = false; + io_bit3 = false; + io_bit4 = true; + } + else if (cart.LightMode == 1) + { + io_bit2 = false; + io_bit3 = true; + io_bit4 = false; + } + else + { + io_bit2 = false; + io_bit3 = false; + io_bit4 = false; + } + } + + ioWriteBuffer[0] = (byte)((io_bit0 ? 1 << 0 : 0) | //继电器 + (io_bit1 ? 1 << 1 : 0) | //抱闸 + (io_bit2 ? 1 << 2 : 0) | //红灯 + (io_bit3 ? 1 << 3 : 0) | //绿灯 + (io_bit4 ? 1 << 4 : 0)); //黄灯 + if (cart.Bridge.WriteOutput(ioWriteBuffer, 20) != MCUSerialBridgeError.OK) + { + Console.WriteLine("IO Write FAILED"); + } + #endregion + + #region 驱动部分 + cart.ActualSpeedLeftFront = (cart.ActualSpeedLeftFrontLeft + cart.ActualSpeedLeftFrontRight) / 2; + cart.ActualSpeedLeftRear = (cart.ActualSpeedLeftRearLeft + cart.ActualSpeedLeftRearRight) / 2; + cart.ActualSpeedRightFront = (cart.ActualSpeedRightFrontLeft + cart.ActualSpeedRightFrontRight) / 2; + cart.ActualSpeedRightRear = (cart.ActualSpeedRightRearLeft + cart.ActualSpeedRightRearRight) / 2; + _wheelSpeedLogger.RecordSnapshot(cart); + // M层CAN辅助：封装本周期驱动器CAN发送参数。 + MCUSerialBridgeError SendCan(byte port, ushort standardId, byte[] payload, bool RTR = false, uint timeout = 2) + { + var canSend = new CANMessage + { + ID = standardId, + RTR = RTR, + DLC = !RTR ? (byte)8 : (byte)0, + Payload = payload ?? new byte[] { } + }; + return cart.Bridge.WriteCAN(port, canSend, timeout); + } + + var resetPayload = new byte[8] { 0xFD, _resetCode, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 }; + var enable1Payload = new byte[8] { 0xFD, _enableCode1, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 }; + var enable2Payload = new byte[8] { 0xFD, _enableCode2, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 }; + var enable3Payload = new byte[8] { 0xFD, _enableCode3, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 }; + + Hedingben.ToastText($"_operationTime:{_operationTime}", "OperationTime"); + if (_operationTime == 0) + { + SendCan(0, 0x00, new byte[] { 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 }); + _operationTime++; + return; + } + else if (_operationTime == 1) + { + if (cart.LeftFrontLeftErrorCode != 0) + { + SendCan(0, 0x201, enable1Payload); + SendCan(0, 0x201, resetPayload); + } + if (cart.LeftFrontRightErrorCode != 0) + { + SendCan(0, 0x202, enable1Payload); + SendCan(0, 0x202, resetPayload); + } + if (cart.RightFrontLeftErrorCode != 0) + { + SendCan(0, 0x203, enable1Payload); + SendCan(0, 0x203, resetPayload); + } + if (cart.RightFrontRightErrorCode != 0) + { + SendCan(0, 0x204, enable1Payload); + SendCan(0, 0x204, resetPayload); + } + if (cart.LeftRearLeftErrorCode != 0) + { + SendCan(0, 0x205, enable1Payload); + SendCan(0, 0x205, resetPayload); + } + if (cart.LeftRearRightErrorCode != 0) + { + SendCan(0, 0x206, enable1Payload); + SendCan(0, 0x206, resetPayload); + } + if (cart.RightRearLeftErrorCode != 0) + { + SendCan(0, 0x207, enable1Payload); + SendCan(0, 0x207, resetPayload); + } + if (cart.RightRearRightErrorCode != 0) + { + SendCan(0, 0x208, enable1Payload); + SendCan(0, 0x208, resetPayload); + } + if (cart.LeftArmErrorCode != 0) + { + SendCan(0, 0x209, enable1Payload); + SendCan(0, 0x209, resetPayload); + } + if (cart.RightArmErrorCode != 0) + { + SendCan(0, 0x20A, enable1Payload); + SendCan(0, 0x20A, resetPayload); + } + _operationTime++; + return; + } + else if (_operationTime == 2) + { + //SendNodeGuardRequests(SendCan); + //if (AreAllNodesOperational()) + //{ + // _operationTime++; + //} + _operationTime++; + return; + } + //检查错误是否被消除 若没被消除就得重新返回上一步发复位 若消除了就继续往下 + else if (_operationTime == 3) + { + if (cart.LeftFrontLeftErrorCode != 0 || cart.LeftFrontRightErrorCode != 0 || + cart.LeftRearLeftErrorCode != 0 || cart.LeftRearRightErrorCode != 0 + || cart.RightFrontLeftErrorCode != 0 || cart.RightFrontRightErrorCode != 0 || + cart.RightRearLeftErrorCode != 0 || cart.RightRearRightErrorCode != 0 || + cart.LeftArmErrorCode != 0 || cart.RightArmErrorCode != 0) + { + _operationTime = 1; + return; + } + else + { + _operationTime++; + return; + } + } + + // 没有错误 没有节点保护 就发06 07 0F使能 + else if (_operationTime == 4) + { + SendCan(0, 0x201, enable1Payload); + SendCan(0, 0x202, enable1Payload); + SendCan(0, 0x203, enable1Payload); + SendCan(0, 0x204, enable1Payload); + SendCan(0, 0x205, enable1Payload); + SendCan(0, 0x206, enable1Payload); + SendCan(0, 0x207, enable1Payload); + SendCan(0, 0x208, enable1Payload); + SendCan(0, 0x209, enable1Payload); + SendCan(0, 0x20A, enable1Payload); + _operationTime++; + return; + } + else if (_operationTime == 5) + { + SendCan(0, 0x201, enable2Payload); + SendCan(0, 0x202, enable2Payload); + SendCan(0, 0x203, enable2Payload); + SendCan(0, 0x204, enable2Payload); + SendCan(0, 0x205, enable2Payload); + SendCan(0, 0x206, enable2Payload); + SendCan(0, 0x207, enable2Payload); + SendCan(0, 0x208, enable2Payload); + SendCan(0, 0x209, enable2Payload); + SendCan(0, 0x20A, enable2Payload); + _operationTime++; + return; + } + else if (_operationTime == 6) + { + SendCan(0, 0x201, enable3Payload); + SendCan(0, 0x202, enable3Payload); + SendCan(0, 0x203, enable3Payload); + SendCan(0, 0x204, enable3Payload); + SendCan(0, 0x205, enable3Payload); + SendCan(0, 0x206, enable3Payload); + SendCan(0, 0x207, enable3Payload); + SendCan(0, 0x208, enable3Payload); + SendCan(0, 0x209, enable3Payload); + SendCan(0, 0x20A, enable3Payload); + if (cart.LeftFrontLeftErrorCode != 0 || cart.LeftFrontRightErrorCode != 0 || + cart.LeftRearLeftErrorCode != 0 || cart.LeftRearRightErrorCode != 0 + || cart.RightFrontLeftErrorCode != 0 || cart.RightFrontRightErrorCode != 0 || + cart.RightRearLeftErrorCode != 0 || cart.RightRearRightErrorCode != 0 || + cart.LeftArmErrorCode != 0 || cart.RightArmErrorCode != 0) + { + _operationTime = 1; + return; + } + else + { + _operationTime++; + return; + } + } + + if (cart.ResetPressed || cart.ResetFromM || cart.ResetFromC) + { + cart.AlarmLevel = -1; + cart.EmergencyPressed = 0; + _count = 0; + _operationTime = 0; + _driversDisabled = false; + cart.WheelAbleState = true; + cart.ResetFromM = false; + } + + if (cart.DisableFromM || cart.DisableFromC) + { + _driversDisabled = true; + cart.WheelAbleState = false; + cart.DisableFromM = false; + } + + var lfl = Math.Sign(cart.SpeedLFL) * Math.Min(Math.Max(0, cart.SendThresSpeed), Math.Abs(cart.SpeedLFL)); + var lfr = Math.Sign(cart.SpeedLFR) * Math.Min(Math.Max(0, cart.SendThresSpeed), Math.Abs(cart.SpeedLFR)); + var rfl = Math.Sign(cart.SpeedRFL) * Math.Min(Math.Max(0, cart.SendThresSpeed), Math.Abs(cart.SpeedRFL)); + var rfr = Math.Sign(cart.SpeedRFR) * Math.Min(Math.Max(0, cart.SendThresSpeed), Math.Abs(cart.SpeedRFR)); + var lrl = Math.Sign(cart.SpeedLRL) * Math.Min(Math.Max(0, cart.SendThresSpeed), Math.Abs(cart.SpeedLRL)); + var lrr = Math.Sign(cart.SpeedLRR) * Math.Min(Math.Max(0, cart.SendThresSpeed), Math.Abs(cart.SpeedLRR)); + var rrl = Math.Sign(cart.SpeedRRL) * Math.Min(Math.Max(0, cart.SendThresSpeed), Math.Abs(cart.SpeedRRL)); + var rrr = Math.Sign(cart.SpeedRRR) * Math.Min(Math.Max(0, cart.SendThresSpeed), Math.Abs(cart.SpeedRRR)); + + var v1 = ConvertMps2Rpm(lfl); + var v2 = ConvertMps2Rpm(-lfr); + var v3 = ConvertMps2Rpm(rfl); + var v4 = ConvertMps2Rpm(-rfr); + var v5 = ConvertMps2Rpm(lrl); + var v6 = ConvertMps2Rpm(-lrr); + var v7 = ConvertMps2Rpm(rrl); + var v8 = ConvertMps2Rpm(-rrr); + var v9 = ConvertMps2Rpm(cart.SpeedLeftArm); + var v10 = ConvertMps2Rpm(cart.SpeedRightArm); + if (cart.AlarmLevel == 2 || cart.WaitStart || cart.PreparaStart) + { + v1 = v2 = v3 = v4 = v5 = v6 = v7 = v8 = v9 = v10 = 0; + } + if ((cart.ActualPosLeftArm <= cart.LeftArmLowerPos && cart.SpeedLeftArm < 0) || (cart.ActualPosLeftArm >= cart.LeftArmUpperPos && cart.SpeedLeftArm > 0)) + { + v9 = 0; + } + if ((cart.ActualPosRightArm <= cart.RightArmLowerPos && cart.SpeedRightArm < 0) || (cart.ActualPosRightArm >= cart.RightArmUpperPos && cart.SpeedRightArm > 0)) + { + v10 = 0; + } + + var sendLFL = BitConverter.GetBytes((int)Math.Round(v1 * 512f * 10000f / 1875f)); + var sendLFR = BitConverter.GetBytes((int)Math.Round(v2 * 512f * 10000f / 1875f)); + var sendRFL = BitConverter.GetBytes((int)Math.Round(v3 * 512f * 10000f / 1875f)); + var sendRFR = BitConverter.GetBytes((int)Math.Round(v4 * 512f * 10000f / 1875f)); + var sendLRL = BitConverter.GetBytes((int)Math.Round(v5 * 512f * 10000f / 1875f)); + var sendLRR = BitConverter.GetBytes((int)Math.Round(v6 * 512f * 10000f / 1875f)); + var sendRRL = BitConverter.GetBytes((int)Math.Round(v7 * 512f * 10000f / 1875f)); + var sendRRR = BitConverter.GetBytes((int)Math.Round(v8 * 512f * 10000f / 1875f)); + var sendLArm = BitConverter.GetBytes((int)Math.Round(v9 * 512f * 10000f / 1875f)); + var sendRArm = BitConverter.GetBytes((int)Math.Round(v10 * 512f * 10000f / 1875f)); + + if (iteration % 2 == 0) + { + //SendNodeGuardRequests(SendCan); + } + + if (_driversDisabled) + { + SendCan(0, 0x201, enable1Payload); + SendCan(0, 0x202, enable1Payload); + SendCan(0, 0x203, enable1Payload); + SendCan(0, 0x204, enable1Payload); + SendCan(0, 0x205, enable1Payload); + SendCan(0, 0x206, enable1Payload); + SendCan(0, 0x207, enable1Payload); + SendCan(0, 0x208, enable1Payload); + } + else + { + SendCan(0, 0x201, new byte[] { 0xFD, _enableCode3, 0x00, sendLFL[0], sendLFL[1], sendLFL[2], sendLFL[3], 0x00 }); + SendCan(0, 0x202, new byte[] { 0xFD, _enableCode3, 0x00, sendLFR[0], sendLFR[1], sendLFR[2], sendLFR[3], 0x00 }); + SendCan(0, 0x203, new byte[] { 0xFD, _enableCode3, 0x00, sendRFL[0], sendRFL[1], sendRFL[2], sendRFL[3], 0x00 }); + SendCan(0, 0x204, new byte[] { 0xFD, _enableCode3, 0x00, sendRFR[0], sendRFR[1], sendRFR[2], sendRFR[3], 0x00 }); + SendCan(0, 0x205, new byte[] { 0xFD, _enableCode3, 0x00, sendLRL[0], sendLRL[1], sendLRL[2], sendLRL[3], 0x00 }); + SendCan(0, 0x206, new byte[] { 0xFD, _enableCode3, 0x00, sendLRR[0], sendLRR[1], sendLRR[2], sendLRR[3], 0x00 }); + SendCan(0, 0x207, new byte[] { 0xFD, _enableCode3, 0x00, sendRRL[0], sendRRL[1], sendRRL[2], sendRRL[3], 0x00 }); + SendCan(0, 0x208, new byte[] { 0xFD, _enableCode3, 0x00, sendRRR[0], sendRRR[1], sendRRR[2], sendRRR[3], 0x00 }); + } + SendCan(0, 0x209, new byte[] { 0xFD, _enableCode3, 0x00, sendLArm[0], sendLArm[1], sendLArm[2], sendLArm[3], 0x00 }); + SendCan(0, 0x20A, new byte[] { 0xFD, _enableCode3, 0x00, sendRArm[0], sendRArm[1], sendRArm[2], sendRArm[3], 0x00 }); + + if (cart.LeftFrontLeftErrorCode != 0 || cart.LeftFrontRightErrorCode != 0 || + cart.LeftRearLeftErrorCode != 0 || cart.LeftRearRightErrorCode != 0 + || cart.RightFrontLeftErrorCode != 0 || cart.RightFrontRightErrorCode != 0 || + cart.RightRearLeftErrorCode != 0 || cart.RightRearRightErrorCode != 0) + { + //_errorCount++; + } + + #endregion + + } + + // M层诊断：根据界面开关创建或关闭本次轮速CSV记录。 + private void UpdateWheelSpeedDiagnosticState() + { + if (cart == null) + return; + + if (cart.WheelSpeedDiagnosticEnabled) + { + if (_wheelSpeedLogger.IsRunning) + return; + + try + { + var configuredDirectory = + string.IsNullOrWhiteSpace( + cart.WheelSpeedDiagnosticDirectory) + ? @"logs\wheel-speed" + : cart.WheelSpeedDiagnosticDirectory; + + var logDirectory = + Path.IsPathRooted(configuredDirectory) + ? configuredDirectory + : Path.Combine( + AppContext.BaseDirectory, + configuredDirectory); + + _wheelSpeedLogger.Start( + logDirectory, + cart.CarNum); + + cart.WheelSpeedDiagnosticStatus = + "记录中：" + + _wheelSpeedLogger.SnapshotLogPath; + } + catch (Exception ex) + { + cart.WheelSpeedDiagnosticEnabled = + false; + cart.WheelSpeedDiagnosticStatus = + "启动失败：" + ex.Message; + + Console.WriteLine( + "轮速诊断启动失败：" + + ex.Message); + } + + return; + } + + if (!_wheelSpeedLogger.IsRunning) + return; + + try + { + var snapshotPath = + _wheelSpeedLogger.SnapshotLogPath; + + _wheelSpeedLogger.Stop(); + + cart.WheelSpeedDiagnosticStatus = + "已保存：" + snapshotPath; + } + catch (Exception ex) + { + cart.WheelSpeedDiagnosticStatus = + "停止失败：" + ex.Message; + + Console.WriteLine( + "轮速诊断停止失败：" + + ex.Message); + } + } + + // M层CAN安全：判断单个驱动节点是否处于可运行状态。 + private static bool IsNodeOperational(byte remoteCode) + { + return remoteCode == 5 || remoteCode == 133; + } + + // M层CAN安全：检查全部车轮和夹臂节点是否已上线。 + private bool AreAllNodesOperational() + { + return IsNodeOperational(cart.LFLRemoteCode) + && IsNodeOperational(cart.LFRRemoteCode) + && IsNodeOperational(cart.RFLRemoteCode) + && IsNodeOperational(cart.RFRRemoteCode) + && IsNodeOperational(cart.LRLRemoteCode) + && IsNodeOperational(cart.LRRRemoteCode) + && IsNodeOperational(cart.RRLRemoteCode) + && IsNodeOperational(cart.RRRRemoteCode) + && IsNodeOperational(cart.LArmRemoteCode) + && IsNodeOperational(cart.RArmRemoteCode); + } + + // M层CAN维护：轮询所有驱动节点的在线状态。 + private static void SendNodeGuardRequests(Func sendCan) + { + sendCan(0, 0x709, Array.Empty(), true, 2); + sendCan(0, 0x70A, Array.Empty(), true, 2); + sendCan(0, 0x701, Array.Empty(), true, 2); + sendCan(0, 0x702, Array.Empty(), true, 2); + sendCan(0, 0x703, Array.Empty(), true, 2); + sendCan(0, 0x704, Array.Empty(), true, 2); + sendCan(0, 0x705, Array.Empty(), true, 2); + sendCan(0, 0x706, Array.Empty(), true, 2); + sendCan(0, 0x707, Array.Empty(), true, 2); + sendCan(0, 0x708, Array.Empty(), true, 2); + } + + // M层CAN通信：按需注册驱动器反馈报文回调。 + private void EnsureCanCallbacksRegistered() + { + if (_canCallbackRegistered) + { + return; + } + if (cart?.Bridge == null) + { + return; + } + var dispatch = BuildCanDispatchTable(); + + var err0 = cart.Bridge.RegisterCANPortCallback(0, msg => + { + if (msg == null) return; + if (dispatch.TryGetValue(msg.ID, out var handler)) + { + handler(msg); + } + }); + // _canCallbackRegistered = true; + _canCallbackRegistered = + err0 == MCUSerialBridgeError.OK; + } + + // M层串口通信：按需注册电池等串口设备回调。 + private void EnsureSerialCallbacksRegistered() + { + if (_serialCallbackRegistered) + { + return; + } + if (cart?.Bridge == null) + { + return; + } + var err1 = cart.Bridge.RegisterSerialPortCallback(1, msg => + { + if (msg == null) return; + Hedingben.ToastText($"Callback Serial 1 Callback Received: {BitConverter.ToString(msg)}", "Serial 1"); + }); + Hedingben.ToastText($"Register Serial 1: {err1}", "Register Serial"); + _serialCallbackRegistered = err1 == MCUSerialBridgeError.OK; + } + + // M层单车电源：周期发送Modbus电池状态查询。 + private void PollBatterySerial(int iteration) + { + if (cart?.Bridge == null) return; + if (iteration % 20 != 0) return; + + var writeErr = cart.Bridge.WriteSerial(BatteryPortIndex, BatteryRequest, 60); + if (writeErr != MCUSerialBridgeError.OK) + { + Hedingben.ToastText($"Battery write err: {writeErr}", "Serial 2"); + return; + } + + var readErr = cart.Bridge.ReadSerial(BatteryPortIndex, out var msg, 40); + if (readErr == MCUSerialBridgeError.NoData) return; + if (readErr != MCUSerialBridgeError.OK) + { + Hedingben.ToastText($"Battery read err: {readErr}", "Serial 2"); + return; + } + if (msg == null || msg.Length == 0) return; + + Hedingben.ToastText($"Serial 2 RX: {BitConverter.ToString(msg)}", "Serial 2"); + TryUpdateBatteryData(msg); + } + + // M层单车电源：校验并解析电池Modbus响应。 + private void TryUpdateBatteryData(byte[] msg) + { + // Modbus RTU response: [id,03,08,data(8),crc(2)] + if (msg.Length < 13 || msg[0] != 0x01 || msg[1] != 0x03 || msg[2] < 8) return; + ushort calc = ComputeModbusCrc(msg, msg.Length - 2); + ushort recv = (ushort)(msg[msg.Length - 2] | (msg[msg.Length - 1] << 8)); + if (calc != recv) return; + + cart.Voltage = ReadInt16BE(msg, 3) * 0.1f; + cart.Soc = ReadUInt16BE(msg, 5); + cart.SOH = ReadUInt16BE(msg, 7); + cart.ElectricCurrent = -ReadInt16BE(msg, 9) * 0.1f; + } + + // M层单车电源：构造读取电池寄存器的Modbus请求。 + private static byte[] BuildBatteryRequest() + { + // 01 03 00 03 00 04 CRC (读取寄存器 03~06) + byte[] req = new byte[] { 0x01, 0x03, 0x00, 0x03, 0x00, 0x04, 0x00, 0x00 }; + ushort crc = ComputeModbusCrc(req, 6); + req[6] = (byte)(crc & 0xFF); + req[7] = (byte)((crc >> 8) & 0xFF); + return req; + } + + // M层协议辅助：计算Modbus RTU的CRC16校验值。 + private static ushort ComputeModbusCrc(byte[] data, int length) + { + ushort crc = 0xFFFF; + for (int i = 0; i < length; i++) + { + crc ^= data[i]; + for (int j = 0; j < 8; j++) + { + bool lsb = (crc & 0x0001) != 0; + crc >>= 1; + if (lsb) crc ^= 0xA001; + } + } + return crc; + } + + // M层协议辅助：按大端序读取有符号16位数值。 + private static short ReadInt16BE(byte[] data, int offset) + { + return (short)((data[offset] << 8) | data[offset + 1]); + } + + // M层协议辅助：按大端序读取无符号16位数值。 + private static ushort ReadUInt16BE(byte[] data, int offset) + { + return (ushort)((data[offset] << 8) | data[offset + 1]); + } + + // M层CAN解析：建立驱动器报文标识符到处理函数的分发表。 + private Dictionary> BuildCanDispatchTable() + { + return new Dictionary> + { + // 速度/位置反馈 0x281~0x28A + [0x281] = (msg) => + { + //Console.WriteLine("Received 0x281 CAN Message"); + var payload = msg.Payload; + if (payload == null || payload.Length < 8) return; + var rpm = DecodeRpmFromPayload(payload); + var speed = ConvertRpm2Mps(rpm); + cart.ActualSpeedLeftFrontLeft = speed; + _wheelSpeedLogger.RecordCanFeedback( + 0x281, + "LFL", + rpm, + speed); + cart.LFLActualPos = ConvertR2MM(BitConverter.ToInt32(payload, 0) / 10000f); + }, + [0x282] = (msg) => + { + //Console.WriteLine("Received 0x282 CAN Message"); + var payload = msg.Payload; + if (payload == null || payload.Length < 8) return; + var rpm = DecodeRpmFromPayload(payload); + var speed = -ConvertRpm2Mps(rpm); + cart.ActualSpeedLeftFrontRight = speed; + _wheelSpeedLogger.RecordCanFeedback( + 0x282, + "LFR", + rpm, + speed); + cart.LFRActualPos = ConvertR2MM(-BitConverter.ToInt32(payload, 0) / 10000f); + }, + [0x283] = (msg) => + { + //Console.WriteLine("Received 0x283 CAN Message"); + var payload = msg.Payload; + if (payload == null || payload.Length < 8) return; + var rpm = DecodeRpmFromPayload(payload); + var speed = ConvertRpm2Mps(rpm); + cart.ActualSpeedRightFrontLeft = speed; + _wheelSpeedLogger.RecordCanFeedback( + 0x283, + "RFL", + rpm, + speed); + cart.RFLActualPos = ConvertR2MM(BitConverter.ToInt32(payload, 0) / 10000f); + }, + [0x284] = (msg) => + { + //Console.WriteLine("Received 0x284 CAN Message"); + var payload = msg.Payload; + if (payload == null || payload.Length < 8) return; + var rpm = DecodeRpmFromPayload(payload); + var speed = -ConvertRpm2Mps(rpm); + cart.ActualSpeedRightFrontRight = speed; + _wheelSpeedLogger.RecordCanFeedback( + 0x284, + "RFR", + rpm, + speed); + cart.RFRActualPos = ConvertR2MM(-BitConverter.ToInt32(payload, 0) / 10000f); + }, + [0x285] = (msg) => + { + //Console.WriteLine("Received 0x285 CAN Message"); + var payload = msg.Payload; + if (payload == null || payload.Length < 8) return; + var rpm = DecodeRpmFromPayload(payload); + var speed = ConvertRpm2Mps(rpm); + cart.ActualSpeedLeftRearLeft = speed; + _wheelSpeedLogger.RecordCanFeedback( + 0x285, + "LRL", + rpm, + speed); + cart.LRLActualPos = ConvertR2MM(BitConverter.ToInt32(payload, 0) / 10000f); + }, + [0x286] = (msg) => + { + //Console.WriteLine("Received 0x286 CAN Message"); + var payload = msg.Payload; + if (payload == null || payload.Length < 8) return; + var rpm = DecodeRpmFromPayload(payload); + var speed = -ConvertRpm2Mps(rpm); + cart.ActualSpeedLeftRearRight = speed; + _wheelSpeedLogger.RecordCanFeedback( + 0x286, + "LRR", + rpm, + speed); + cart.LRRActualPos = ConvertR2MM(-BitConverter.ToInt32(payload, 0) / 10000f); + }, + [0x287] = (msg) => + { + //Console.WriteLine("Received 0x287 CAN Message"); + var payload = msg.Payload; + if (payload == null || payload.Length < 8) return; + var rpm = DecodeRpmFromPayload(payload); + var speed = ConvertRpm2Mps(rpm); + cart.ActualSpeedRightRearLeft = speed; + _wheelSpeedLogger.RecordCanFeedback( + 0x287, + "RRL", + rpm, + speed); + cart.RRLActualPos = ConvertR2MM(BitConverter.ToInt32(payload, 0) / 10000f); + }, + [0x288] = (msg) => + { + //Console.WriteLine("Received 0x288 CAN Message"); + var payload = msg.Payload; + if (payload == null || payload.Length < 8) return; + var rpm = DecodeRpmFromPayload(payload); + var speed = -ConvertRpm2Mps(rpm); + cart.ActualSpeedRightRearRight = speed; + _wheelSpeedLogger.RecordCanFeedback( + 0x288, + "RRR", + rpm, + speed); + cart.RRRActualPos = ConvertR2MM(-BitConverter.ToInt32(payload, 0) / 10000f); + }, + [0x289] = (msg) => + { + //Console.WriteLine("Received 0x289 CAN Message"); + var payload = msg.Payload; + if (payload == null || payload.Length < 8) return; + var rpm = DecodeRpmFromPayload(payload); + cart.ActualSpeedLeftArm = ConvertRpm2Mps(rpm); + cart.ActualPosLeftArm = ConvertR2MM(BitConverter.ToInt32(payload, 0) / 10000f); + }, + [0x28A] = (msg) => + { + //Console.WriteLine("Received 0x28A CAN Message"); + var payload = msg.Payload; + if (payload == null || payload.Length < 8) return; + var rpm = DecodeRpmFromPayload(payload); + cart.ActualSpeedRightArm = ConvertRpm2Mps(rpm); + cart.ActualPosRightArm = ConvertR2MM(BitConverter.ToInt32(payload, 0) / 10000f); + }, + // 状态/错误码 0x181~0x18A + [0x181] = (msg) => + { + //Console.WriteLine("Received 0x181 CAN Message"); + var payload = msg.Payload; + if (payload == null || payload.Length < 4) return; + cart.LeftFrontLeftStateCode = BitConverter.ToInt16(payload, 0); + cart.LeftFrontLeftErrorCode = BitConverter.ToInt16(payload, 2); + cart.LeftFrontLeftElectric = BitConverter.ToInt16(payload, 5) * 0.001f; + }, + [0x182] = (msg) => + { + //Console.WriteLine("Received 0x182 CAN Message"); + var payload = msg.Payload; + if (payload == null || payload.Length < 4) return; + cart.LeftFrontRightStateCode = BitConverter.ToInt16(payload, 0); + cart.LeftFrontRightErrorCode = BitConverter.ToInt16(payload, 2); + cart.LeftFrontRightElectric = BitConverter.ToInt16(payload, 5) * 0.001f; + }, + [0x183] = (msg) => + { + //Console.WriteLine("Received 0x183 CAN Message"); + var payload = msg.Payload; + if (payload == null || payload.Length < 4) return; + cart.RightFrontLeftStateCode = BitConverter.ToInt16(payload, 0); + cart.RightFrontLeftErrorCode = BitConverter.ToInt16(payload, 2); + cart.RightFrontLeftElectric = BitConverter.ToInt16(payload, 5) * 0.001f; + }, + [0x184] = (msg) => + { + //Console.WriteLine("Received 0x184 CAN Message"); + var payload = msg.Payload; + if (payload == null || payload.Length < 4) return; + cart.RightFrontRightStateCode = BitConverter.ToInt16(payload, 0); + cart.RightFrontRightErrorCode = BitConverter.ToInt16(payload, 2); + cart.RightFrontRightElectric = BitConverter.ToInt16(payload, 5) * 0.001f; + }, + [0x185] = (msg) => + { + //Console.WriteLine("Received 0x185 CAN Message"); + var payload = msg.Payload; + if (payload == null || payload.Length < 4) return; + cart.LeftRearLeftStateCode = BitConverter.ToInt16(payload, 0); + cart.LeftRearLeftErrorCode = BitConverter.ToInt16(payload, 2); + cart.LeftRearLeftElectric = BitConverter.ToInt16(payload, 5) * 0.001f; + }, + [0x186] = (msg) => + { + //Console.WriteLine("Received 0x186 CAN Message"); + var payload = msg.Payload; + if (payload == null || payload.Length < 4) return; + cart.LeftRearRightStateCode = BitConverter.ToInt16(payload, 0); + cart.LeftRearRightErrorCode = BitConverter.ToInt16(payload, 2); + cart.LeftRearRightElectric = BitConverter.ToInt16(payload, 5) * 0.001f; + }, + [0x187] = (msg) => + { + //Console.WriteLine("Received 0x187 CAN Message"); + var payload = msg.Payload; + if (payload == null || payload.Length < 4) return; + cart.RightRearLeftStateCode = BitConverter.ToInt16(payload, 0); + cart.RightRearLeftErrorCode = BitConverter.ToInt16(payload, 2); + cart.RightRearLeftElectric = BitConverter.ToInt16(payload, 5) * 0.001f; + }, + [0x188] = (msg) => + { + //Console.WriteLine("Received 0x188 CAN Message"); + var payload = msg.Payload; + if (payload == null || payload.Length < 4) return; + cart.RightRearRightStateCode = BitConverter.ToInt16(payload, 0); + cart.RightRearRightErrorCode = BitConverter.ToInt16(payload, 2); + cart.RightRearRightElectric = BitConverter.ToInt16(payload, 5) * 0.001f; + }, + [0x189] = (msg) => + { + //Console.WriteLine("Received 0x189 CAN Message"); + var payload = msg.Payload; + if (payload == null || payload.Length < 4) return; + cart.LeftArmStateCode = BitConverter.ToInt16(payload, 0); + cart.LeftArmErrorCode = BitConverter.ToInt16(payload, 2); + cart.LeftArmElectric = BitConverter.ToInt16(payload, 5) * 0.001f; + }, + [0x18A] = (msg) => + { + //Console.WriteLine("Received 0x18A CAN Message"); + var payload = msg.Payload; + if (payload == null || payload.Length < 4) return; + cart.RightArmStateCode = BitConverter.ToInt16(payload, 0); + cart.RightArmErrorCode = BitConverter.ToInt16(payload, 2); + cart.RightArmElectric = BitConverter.ToInt16(payload, 5) * 0.001f; + }, + + // 舵角 0x18B~0x18E + [0x18B] = (msg) => + { + //Console.WriteLine("Received 0x18B CAN Message"); + var payload = msg.Payload; + if (payload == null || payload.Length < 4) return; + cart.ActualThLeftFront = BitConverter.ToInt32(payload, 0); + cart.ActualThLeftFront = cart.ActualThLeftFront >= 16384 + ? (cart.ActualThLeftFront - 98303) / 4096f / 5f * 360 - cart.ThBiasLeftFront + : cart.ActualThLeftFront / 4096f / 5f * 360 - cart.ThBiasLeftFront; + }, + [0x18C] = (msg) => + { + var payload = msg.Payload; + if (payload == null || payload.Length < 4) return; + var raw = BitConverter.ToInt32(payload, 0); + cart.ActualThRightFront = raw >= 16384 + ? (raw - 98303) / 4096f / 5f * 360 - cart.ThBiasRightFront + : raw / 4096f / 5f * 360 - cart.ThBiasRightFront; + //DLog.Log($"RF raw=0x{raw:X8}({raw}) angle={cart.ActualThRightFront:F2}", "0x18C"); + }, + [0x18D] = (msg) => + { + //Console.WriteLine($"Received 0x18D CAN Message {DateTime.Now:yyyy-MM-dd HH:mm:ss.ffffff}"); + var payload = msg.Payload; + if (payload == null || payload.Length < 4) return; + cart.ActualThLeftRear = BitConverter.ToInt32(payload, 0); + cart.ActualThLeftRear = cart.ActualThLeftRear >= 16384 + ? (cart.ActualThLeftRear - 98303) / 4096f / 5f * 360 - cart.ThBiasLeftRear + : cart.ActualThLeftRear / 4096f / 5f * 360 - cart.ThBiasLeftRear; + }, + [0x18E] = (msg) => + { + //Console.WriteLine("Received 0x18E CAN Message"); + var payload = msg.Payload; + if (payload == null || payload.Length < 4) return; + cart.ActualThRightRear = BitConverter.ToInt32(payload, 0); + cart.ActualThRightRear = cart.ActualThRightRear >= 16384 + ? (cart.ActualThRightRear - 98303) / 4096f / 5f * 360 - cart.ThBiasRightRear + : cart.ActualThRightRear / 4096f / 5f * 360 - cart.ThBiasRightRear; + }, + + // 远程帧 + [0x701] = (msg) => + { + //Console.WriteLine("Received 0x701 CAN Message"); + var payload = msg.Payload; + if (payload == null || payload.Length < 1) return; + cart.LFLRemoteCode = payload[0]; + }, + [0x702] = (msg) => + { + //Console.WriteLine("Received 0x702 CAN Message"); + var payload = msg.Payload; + if (payload == null || payload.Length < 1) return; + cart.LFRRemoteCode = payload[0]; + }, + [0x703] = (msg) => + { + //Console.WriteLine("Received 0x703 CAN Message"); + var payload = msg.Payload; + if (payload == null || payload.Length < 1) return; + cart.RFLRemoteCode = payload[0]; + }, + [0x704] = (msg) => + { + //Console.WriteLine("Received 0x704 CAN Message"); + var payload = msg.Payload; + if (payload == null || payload.Length < 1) return; + cart.RFRRemoteCode = payload[0]; + }, + [0x705] = (msg) => + { + //Console.WriteLine("Received 0x705 CAN Message"); + var payload = msg.Payload; + if (payload == null || payload.Length < 1) return; + cart.LRLRemoteCode = payload[0]; + }, + [0x706] = (msg) => + { + //Console.WriteLine("Received 0x706 CAN Message"); + var payload = msg.Payload; + if (payload == null || payload.Length < 1) return; + cart.LRRRemoteCode = payload[0]; + }, + [0x707] = (msg) => + { + //Console.WriteLine("Received 0x707 CAN Message"); + var payload = msg.Payload; + if (payload == null || payload.Length < 1) return; + cart.RRLRemoteCode = payload[0]; + }, + [0x708] = (msg) => + { + //Console.WriteLine("Received 0x708 CAN Message"); + var payload = msg.Payload; + if (payload == null || payload.Length < 1) return; + cart.RRRRemoteCode = payload[0]; + }, + [0x709] = (msg) => + { + //Console.WriteLine("Received 0x709 CAN Message"); + var payload = msg.Payload; + if (payload == null || payload.Length < 1) return; + cart.LArmRemoteCode = payload[0]; + }, + [0x70A] = (msg) => + { + //Console.WriteLine("Received 0x70A CAN Message"); + var payload = msg.Payload; + if (payload == null || payload.Length < 1) return; + cart.RArmRemoteCode = payload[0]; + }, + }; + } + + } +} diff --git a/MedullaAdapter/MCUSerialBridgeCLR.cs b/MedullaAdapter/MCUSerialBridgeCLR.cs new file mode 100644 index 0000000..68f12fd --- /dev/null +++ b/MedullaAdapter/MCUSerialBridgeCLR.cs @@ -0,0 +1,928 @@ +// C#调用MCU通信桥 + +using System; +using System.Collections.Generic; +using System.Linq; +using System.Runtime.InteropServices; + +namespace MCUSerialBridgeCLR +{ + /// + /// 辅助方法与内部 C 结构体封装，用于 P/Invoke 交互 + /// + internal static class PortStructHelper + { + /// + /// 将任意结构体序列化为字节数组 + /// + /// 结构体类型 + /// 要序列化的结构体 + /// 返回结构体对应的字节数组 + /// 使用 Marshal 分配内存并复制内容 + public static byte[] StructToBytes(T str) + where T : struct + { + int size = Marshal.SizeOf(); + byte[] arr = new byte[size]; + IntPtr ptr = Marshal.AllocHGlobal(size); + try + { + Marshal.StructureToPtr(str, ptr, false); + Marshal.Copy(ptr, arr, 0, size); + } + finally + { + Marshal.FreeHGlobal(ptr); + } + return arr; + } + + /// + /// 串口端口配置的原生结构体（与 MCU C 层对应） + /// + [StructLayout(LayoutKind.Sequential, Pack = 1)] + public struct SerialPortConfigC + { + /// 端口类型（0x01 = Serial） + public byte port_type; + + /// 波特率 + public uint baud; + + /// 接收帧时间间隔 + public uint receive_frame_ms; + + /// 保留字节，填 0 + [MarshalAs(UnmanagedType.ByValArray, SizeConst = 7)] + public byte[] reserved; + } + + /// + /// CAN 端口配置的原生结构体（与 MCU C 层对应） + /// + [StructLayout(LayoutKind.Sequential, Pack = 1)] + public struct CANPortConfigC + { + /// 端口类型（0x02 = CAN） + public byte port_type; + + /// 波特率 + public uint baud; + + /// 最大重发时间 + public uint retry_time_ms; + + /// 保留字节，填 0 + [MarshalAs(UnmanagedType.ByValArray, SizeConst = 7)] + public byte[] reserved; + } + } + + /// + /// MCU 固件版本信息结构体 + /// + [StructLayout(LayoutKind.Sequential, CharSet = CharSet.Ansi)] + public struct VersionInfo + { + /// 产品型号 + [MarshalAs(UnmanagedType.ByValTStr, SizeConst = 16)] + public string ProductionName; + + /// Git 标签 + [MarshalAs(UnmanagedType.ByValTStr, SizeConst = 8)] + public string GitTag; + + /// Git commit 哈希值 + [MarshalAs(UnmanagedType.ByValTStr, SizeConst = 8)] + public string GitCommit; + + /// 编译时间（字符串） + [MarshalAs(UnmanagedType.ByValTStr, SizeConst = 24)] + public string BuildTime; + + /// + /// 转换为可读字符串 + /// + /// 返回包含产品、Tag、Commit、BuildTime 的字符串 + // M层MCU适配：格式化固件版本信息便于日志显示。 + public override string ToString() + { + return $"Product: {ProductionName}, Tag: {GitTag}, Commit: {GitCommit}, Built: {BuildTime}"; + } + } + + /// + /// MCU 当前运行状态 + /// + [StructLayout(LayoutKind.Explicit)] + public struct MCUState + { + /// 原始 32 位状态值 + [FieldOffset(0)] + public uint RawValue; + + /// 状态子字段 0 + [FieldOffset(0)] + public byte Substate0; + + /// 状态子字段 1 + [FieldOffset(1)] + public byte Substate1; + + /// 状态子字段 2 + [FieldOffset(2)] + public byte Substate2; + + /// 高字节模式标志 + [FieldOffset(3)] + public byte Mode; + + /// 是否处于 Bridge 模式 + public bool IsBridge => (Mode & 0x80) == 0; + + /// 是否处于 DIVER 模式 + public bool IsDIVER => (Mode & 0x80) != 0; + + /// + /// 返回可读的状态字符串 + /// + /// 例如 "Bridge: Running" 或 "DIVER: Error" + // M层MCU适配：格式化MCU运行状态便于日志显示。 + public override string ToString() + { + string modeStr = IsBridge ? "Bridge" : "DIVER"; + uint substate = (uint)(Substate0 | (Substate1 << 8) | (Substate2 << 16)); + + string subStr = substate switch + { + 0x00000000 => "Idle", + 0x0000000F => "Running", + 0x000000FF => "Error", + 0x00000001 => "Configured", // DIVER specific + 0x8000000F => "Running", + 0x800000FF => "Error", + _ => $"Unknown (0x{substate:X6})", + }; + + return $"{modeStr}: {subStr}"; + } + } + + /// + /// 抽象端口配置基类 + /// + public abstract class PortConfig + { + /// 端口类型（由子类实现） + public abstract byte PortType { get; } + + /// 序列化端口配置为字节数组（供 P/Invoke 使用） + /// 返回固定长度字节数组（16 bytes） + // M层MCU适配：将端口配置序列化为原生接口字节。 + public abstract byte[] ToBytes(); + } + + /// + /// 串口配置 + /// + /// + /// 构造函数 + /// + /// 波特率 + /// 接收帧间隔 + // M层MCU适配：创建串口通信参数配置。 + public class SerialPortConfig(uint baud, uint receiveFrameMs) : PortConfig + { + /// Serial 类型 + public override byte PortType => 0x01; + + /// 波特率 + public uint Baud { get; set; } = baud; + + /// 接收帧间隔 + public uint ReceiveFrameMs { get; set; } = receiveFrameMs; + + /// + /// 转换为字节数组 + /// + /// 16 字节数组 + // M层MCU适配：序列化串口波特率和组帧时间。 + public override byte[] ToBytes() + { + var c = new PortStructHelper.SerialPortConfigC + { + port_type = PortType, + baud = Baud, + receive_frame_ms = ReceiveFrameMs, + reserved = new byte[7], + }; + return PortStructHelper.StructToBytes(c); + } + } + + /// + /// CAN 端口配置 + /// + /// + /// 构造函数 + /// + /// 波特率 + /// 重发间隔 + // M层MCU适配：创建CAN通信参数配置。 + public class CANPortConfig(uint baud, uint retryTimeMs) : PortConfig + { + /// CAN 类型 + public override byte PortType => 0x02; + + /// 波特率 + public uint Baud { get; set; } = baud; + + /// 重发间隔 + public uint RetryTimeMs { get; set; } = retryTimeMs; + + /// + /// 转换为字节数组 + /// + /// 16 字节数组 + // M层MCU适配：序列化CAN波特率和重试时间。 + public override byte[] ToBytes() + { + var c = new PortStructHelper.CANPortConfigC + { + port_type = PortType, + baud = Baud, + retry_time_ms = RetryTimeMs, + reserved = new byte[7], + }; + return PortStructHelper.StructToBytes(c); + } + } + + /// + /// CAN 帧结构（标准帧 11-bit ID + 1-bit RTR + 4-bit DLC + Payload） + /// + public class CANMessage + { + /// 标准帧 ID（0~0x7FF，11 位） + public ushort ID { get; set; } + + /// 远程帧标志：false = 数据帧，true = 远程帧 + public bool RTR { get; set; } + + /// 数据长度码：0~8 + public byte DLC { get; set; } + + /// 数据负载，长度必须严格等于 DLC（DLC=0 时可为 null） + public byte[] Payload { get; set; } + + /// + /// 序列化为 MCU 协议字节流 + /// + /// 返回字节数组：2 bytes header + Payload + /// 如果 DLC > 8 + /// 如果 Payload 长度 != DLC + // M层CAN适配：将标准或扩展CAN帧序列化为原生布局。 + public byte[] ToBytes() + { + if (DLC > 8) + throw new ArgumentOutOfRangeException(nameof(DLC), "DLC must be 0-8"); + if (DLC > 0 && (Payload == null || Payload.Length != DLC)) + throw new ArgumentException("Payload length must equal DLC"); + + // 构造 2 字节 header + ushort header = 0; + header |= (ushort)(ID & 0x7FF); // bits 0-10 + if (RTR) + header |= (1 << 11); // bit 11 + header |= (ushort)((DLC & 0xF) << 12); // bits 12-15 + + byte[] result = new byte[2 + DLC]; + byte[] headerBytes = BitConverter.GetBytes(header); // 小端序 + result[0] = headerBytes[0]; + result[1] = headerBytes[1]; + + if (DLC > 0) + Buffer.BlockCopy(Payload, 0, result, 2, DLC); + return result; + } + + /// + /// 反序列化 MCU 协议字节流为 CANMessage + /// + /// 原始字节数组 + /// 实际有效长度 + /// CANMessage 实例 + /// 数据长度错误 + // M层CAN适配：从原生缓冲区还原CAN消息。 + public static CANMessage FromBytes(byte[] data, uint length) + { + if (data == null || length > data.Length || length < 2) + throw new ArgumentException("Data must be at least 2 bytes"); + + ushort header = BitConverter.ToUInt16(data, 0); + + var msg = new CANMessage + { + ID = (ushort)(header & 0x7FF), + RTR = (header & (1 << 11)) != 0, + DLC = (byte)((header >> 12) & 0xF), + }; + + if (msg.DLC > 0) + { + if (length < 2 + msg.DLC) + throw new ArgumentException("Data length less than DLC"); + msg.Payload = new byte[msg.DLC]; + Buffer.BlockCopy(data, 2, msg.Payload, 0, msg.DLC); + } + else + { + msg.Payload = Array.Empty(); + } + + return msg; + } + + // M层CAN诊断：格式化CAN标识符和数据内容。 + public override string ToString() + { + string payloadStr = + (Payload == null || Payload.Length == 0) + ? "[]" + : "0x[" + string.Join(" ", Payload.Select(b => $"{b:X2}")) + "]"; + + return $"CANMessage(ID=0x{ID:X3}, RTR={RTR}, DLC={DLC}, Payload={payloadStr})"; + } + } + + /// + /// 内部 P/Invoke 层，直接映射 C DLL 函数 + /// + internal static class MCUSerialBridgeCoreAPI + { + private const string DLL = @"mcu_serial_bridge.dll"; + + [DllImport(DLL, CallingConvention = CallingConvention.Cdecl)] + // M层原生接口：打开MCU串口桥设备。 + internal static extern MCUSerialBridgeError msb_open( + out IntPtr handle, + [MarshalAs(UnmanagedType.LPStr)] string port, + uint baud + ); + + [DllImport(DLL, CallingConvention = CallingConvention.Cdecl)] + // M层原生接口：关闭MCU串口桥设备。 + internal static extern MCUSerialBridgeError msb_close(IntPtr handle); + + [DllImport(DLL, CallingConvention = CallingConvention.Cdecl)] + // M层原生接口：复位MCU串口桥。 + internal static extern MCUSerialBridgeError msb_reset(IntPtr handle, uint timeout_ms); + + [DllImport(DLL, CallingConvention = CallingConvention.Cdecl)] + // M层原生接口：读取MCU固件版本。 + public static extern MCUSerialBridgeError msb_version( + IntPtr handle, + out VersionInfo version, + uint timeout_ms + ); + + [DllImport(DLL, CallingConvention = CallingConvention.Cdecl)] + // M层原生接口：读取MCU当前运行状态。 + public static extern MCUSerialBridgeError mcu_state( + IntPtr handle, + out MCUState state, + uint timeout + ); + + [DllImport(DLL, CallingConvention = CallingConvention.Cdecl)] + // M层原生接口：下发串口桥端口配置。 + internal static extern MCUSerialBridgeError msb_configure( + IntPtr handle, + uint num_ports, + IntPtr ports, + uint timeout + ); + + [DllImport(DLL, CallingConvention = CallingConvention.Cdecl)] + // M层原生接口：读取MCU数字输入。 + internal static extern MCUSerialBridgeError msb_read_input( + IntPtr handle, + [Out] byte[] inputs, + uint timeout_ms + ); + + [DllImport(DLL, CallingConvention = CallingConvention.Cdecl)] + // M层原生接口：写入MCU数字输出。 + internal static extern MCUSerialBridgeError msb_write_output( + IntPtr handle, + [In] byte[] outputs, + uint timeout_ms + ); + + [DllImport(DLL, CallingConvention = CallingConvention.Cdecl)] + // M层原生接口：从指定串口或CAN端口读取数据。 + internal static extern MCUSerialBridgeError msb_read_port( + IntPtr handle, + byte port_index, + [Out] byte[] dst_data, + uint dst_capacity, + out uint out_length, + uint timeout_ms + ); + + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + // M层硬件桥接：定义串口或CAN端口收到原生数据时的回调签名。 + internal delegate void msb_on_port_data_callback_function_t( + IntPtr dst_data, + uint dst_data_size, + IntPtr user_ctx + ); + + [DllImport(DLL, CallingConvention = CallingConvention.Cdecl)] + // M层原生接口：注册端口数据到达回调。 + internal static extern MCUSerialBridgeError msb_register_port_data_callback( + IntPtr handle, + byte port_index, + msb_on_port_data_callback_function_t callback, + IntPtr user_ctx + ); + + [DllImport(DLL, CallingConvention = CallingConvention.Cdecl)] + // M层原生接口：向指定串口或CAN端口写入数据。 + internal static extern MCUSerialBridgeError msb_write_port( + IntPtr handle, + byte port_index, + [In] byte[] src_data, + uint src_data_len, + uint timeout_ms + ); + } + + /// + /// MCU 串口/端口操作托管封装类 + /// 实现 IDisposable 管理底层句柄生命周期 + /// + public class MCUSerialBridge : IDisposable + { + public static uint MaxPortNumber = 16; + + private IntPtr nativeHandle = IntPtr.Zero; + + /// 判断是否已打开 + public bool IsOpen => nativeHandle != IntPtr.Zero; + + /// 构造函数，初始化对象 + // M层MCU适配：创建串口桥包装器并固定原生回调委托。 + public MCUSerialBridge() + { + nativeHandle = IntPtr.Zero; + } + + /// 析构函数 + // M层MCU适配：对象回收时兜底释放原生串口桥句柄。 + ~MCUSerialBridge() + { + Dispose(false); + } + + /// 显式释放资源 + // M层MCU适配：释放串口桥句柄和非托管资源。 + public void Dispose() + { + Dispose(true); + GC.SuppressFinalize(this); + } + + /// 内部释放资源方法 + /// true 表示手动释放，false 表示析构释放 + // M层MCU适配：按托管或终结路径关闭原生句柄。 + private void Dispose(bool disposing) + { + if (nativeHandle != IntPtr.Zero) + { + MCUSerialBridgeCoreAPI.msb_close(nativeHandle); + nativeHandle = IntPtr.Zero; + } + } + + /// 打开串口 + /// 串口名，如 "COM3" + /// 波特率 + /// 错误码 + // M层单车通信：按端口名和波特率连接MCU串口桥。 + public MCUSerialBridgeError Open(string portName, uint baud) + { + return MCUSerialBridgeCoreAPI.msb_open(out nativeHandle, portName, baud); + } + + /// 关闭串口 + /// 错误码 + // M层单车通信：关闭当前MCU串口桥连接。 + public MCUSerialBridgeError Close() + { + if (nativeHandle == IntPtr.Zero) + return MCUSerialBridgeError.Win_HandleNotFound; + + MCUSerialBridgeError error = MCUSerialBridgeCoreAPI.msb_close(nativeHandle); + nativeHandle = IntPtr.Zero; + return error; + } + + /// MCU 复位 + /// 错误码 + // M层单车通信：请求MCU复位并等待结果。 + public MCUSerialBridgeError Reset(uint timeout = 200) + { + if (nativeHandle == IntPtr.Zero) + return MCUSerialBridgeError.Win_HandleNotFound; + + return MCUSerialBridgeCoreAPI.msb_reset(nativeHandle, timeout); + } + + /// 获取固件版本 + /// 输出版本信息 + /// 超时时间（ms） + /// 错误码 + // M层MCU诊断：读取串口桥固件版本。 + public MCUSerialBridgeError GetVersion(out VersionInfo version, uint timeout = 200) + { + version = new VersionInfo(); + if (nativeHandle == IntPtr.Zero) + return MCUSerialBridgeError.Win_HandleNotFound; + + return MCUSerialBridgeCoreAPI.msb_version(nativeHandle, out version, timeout); + } + + /// 获取 MCU 当前状态 + /// 输出状态 + /// 超时时间（ms） + /// 错误码 + // M层MCU诊断：读取串口桥运行状态。 + public MCUSerialBridgeError GetState(out MCUState state, uint timeout = 200) + { + state = new MCUState(); + if (nativeHandle == IntPtr.Zero) + return MCUSerialBridgeError.Win_HandleNotFound; + + return MCUSerialBridgeCoreAPI.mcu_state(nativeHandle, out state, timeout); + } + + /// 配置端口 + /// 端口集合 + /// 超时时间（ms） + /// 错误码 + // M层MCU适配：批量配置CAN和串口通道参数。 + public MCUSerialBridgeError Configure(IEnumerable ports, uint timeout = 200) + { + if (nativeHandle == IntPtr.Zero) + return MCUSerialBridgeError.Win_HandleNotFound; + + if (ports == null) + return MCUSerialBridgeError.Win_InvalidParam; + + // 转换成数组 + PortConfig[] portArray = ports as PortConfig[] ?? ports.ToArray(); + int count = portArray.Length; + + // 分配连续原生内存 + int structSize = 16; // 每个 PortConfig 固定 16 字节 + IntPtr nativePorts = Marshal.AllocHGlobal(structSize * count); + + try + { + for (int i = 0; i < count; i++) + { + byte[] bytes = portArray[i].ToBytes(); + if (bytes.Length != structSize) + return MCUSerialBridgeError.Win_InvalidParam; + + Marshal.Copy(bytes, 0, nativePorts + i * structSize, structSize); + } + + // 调用底层 API + return MCUSerialBridgeCoreAPI.msb_configure( + nativeHandle, + (uint)count, + nativePorts, + timeout + ); + } + catch + { + return MCUSerialBridgeError.Win_InvalidParam; + } + finally + { + Marshal.FreeHGlobal(nativePorts); + } + } + + /// 读取输入（4 字节） + /// 输出数组 + /// 超时（ms） + /// 错误码 + // M层单车IO：读取MCU数字输入状态。 + public MCUSerialBridgeError ReadInput(out byte[] inputs, uint timeout = 100) + { + inputs = new byte[4]; + if (nativeHandle == IntPtr.Zero) + return MCUSerialBridgeError.Win_HandleNotFound; + + return MCUSerialBridgeCoreAPI.msb_read_input(nativeHandle, inputs, timeout); + } + + /// 写输出（4 字节） + /// 数据数组 + /// 超时（ms） + /// 错误码 + // M层单车IO：写入继电器、灯光等数字输出状态。 + public MCUSerialBridgeError WriteOutput(byte[] outputs, uint timeout = 100) + { + if (nativeHandle == IntPtr.Zero) + return MCUSerialBridgeError.Win_HandleNotFound; + + return MCUSerialBridgeCoreAPI.msb_write_output(nativeHandle, outputs, timeout); + } + + /// + /// 读取 Serial 端口的一帧数据。 + /// Serial 上报的数据按帧入队；本接口每次调用最多读取一帧。 + /// 注意，如果不及时调用该接口，数据可能会丢失。 + /// + /// Serial 端口索引 + /// 接收到的数据 + /// + /// 超时时间（毫秒） + /// - 0：不等待，有数据立即返回，没有数据立即返回 MSB_Error_NoData + /// - >0：若当前无数据，最多等待 timeout，期间有新帧到达则立即返回 + /// + /// + /// 如果已经注册回调，本函数将始终返回 MSB_Error_NoData + /// + /// + /// 错误码 MCUSerialBridgeError + /// - OK 成功读取一帧 + /// - NoData 当前无可读数据（仅在 timeout == 0 或等待超时） + /// - Win_InvalidParam 参数错误 + /// + // M层串口通信：同步读取指定MCU串口的数据。 + public MCUSerialBridgeError ReadSerial(byte portIndex, out byte[] buffer, uint timeout) + { + buffer = Array.Empty(); + + if (nativeHandle == IntPtr.Zero) + return MCUSerialBridgeError.Win_HandleNotFound; + + const int MAX_PORT_FRAME = 2048; + byte[] tmp = new byte[MAX_PORT_FRAME]; + + var err = MCUSerialBridgeCoreAPI.msb_read_port( + nativeHandle, + portIndex, + tmp, + (uint)tmp.Length, + out uint outLen, + timeout + ); + + if (err != MCUSerialBridgeError.OK) + return err; + + buffer = new byte[outLen]; + Buffer.BlockCopy(tmp, 0, buffer, 0, (int)outLen); + + return MCUSerialBridgeError.OK; + } + + /// + /// 写 Serial 端口数据。 + /// 注意：对于单个Serial端口，不支持多线程并行发送，不要在上一条数据没有发送完成之前调用该函数，否则有可能导致数据错误。 + /// 注意：超时时间一定要大于波特率和数据长度综合得出的帧时间 + /// + /// Serial 端口索引 + /// 待发送数据 + /// 超时时间（毫秒） + /// + /// 错误码 MCUSerialBridgeError + /// - OK 成功发送 + /// - 其他错误请查看 MCUSerialBridgeError + /// + // M层串口通信：向指定MCU串口发送数据。 + public MCUSerialBridgeError WriteSerial(byte portIndex, byte[] data, uint timeout) + { + if (nativeHandle == IntPtr.Zero) + return MCUSerialBridgeError.Win_HandleNotFound; + + if (data == null || data.Length == 0) + return MCUSerialBridgeError.Win_InvalidParam; + + return MCUSerialBridgeCoreAPI.msb_write_port( + nativeHandle, + portIndex, + data, + (uint)data.Length, + timeout + ); + } + + /// + /// 读取 CAN 端口的一帧数据。 + /// CAN 上报的数据按帧入队；本接口每次调用最多读取一帧。 + /// 注意，如果不及时调用该接口，数据可能会丢失。 + /// + /// CAN 端口索引 + /// 输出 CAN 消息对象 + /// 超时时间（毫秒） + /// + /// 如果已经注册回调，本函数将始终返回 MSB_Error_NoData + /// + /// + /// 错误码 MCUSerialBridgeError + /// - OK 成功读取一帧 + /// - NoData 当前无可读数据（仅在 timeout == 0 或等待超时） + /// - Win_InvalidParam 参数错误 + /// - CAN_DataError CAN数据错误 + /// - Win_HandleNotFound 句柄无效 + /// + // M层CAN通信：同步读取指定CAN通道的一帧消息。 + public MCUSerialBridgeError ReadCAN(byte portIndex, out CANMessage message, uint timeout) + { + message = null; + + if (nativeHandle == IntPtr.Zero) + return MCUSerialBridgeError.Win_HandleNotFound; + + const int MAX_FRAME = 16; + byte[] tmp = new byte[MAX_FRAME]; + + var err = MCUSerialBridgeCoreAPI.msb_read_port( + nativeHandle, + portIndex, + tmp, + (uint)tmp.Length, + out uint outLen, + timeout + ); + if (err != MCUSerialBridgeError.OK) + return err; + + try + { + message = CANMessage.FromBytes(tmp, outLen); + } + catch + { + return MCUSerialBridgeError.CAN_DataError; + } + return MCUSerialBridgeError.OK; + } + + /// + /// 写 CAN 端口数据。 + /// 注意：CAN 支持多线程发送，最多可同时发送 16 个消息。 + /// 但是，多个CAN消息会进入排队队列等待发送，如果消息太多，可能引起超时。 + /// + /// CAN 端口索引 + /// 待发送 CAN 消息对象 + /// 超时时间（毫秒），默认 500ms + /// + /// 错误码 MCUSerialBridgeError + /// - OK 成功发送 + /// - Win_InvalidParam 参数错误 + /// - CAN_DataError CAN 数据错误 + /// - Win_HandleNotFound 句柄无效 + /// + // M层CAN通信：向指定CAN通道发送一帧消息。 + public MCUSerialBridgeError WriteCAN(byte portIndex, CANMessage message, uint timeout) + { + if (nativeHandle == IntPtr.Zero) + return MCUSerialBridgeError.Win_HandleNotFound; + if (message == null) + return MCUSerialBridgeError.Win_InvalidParam; + try + { + byte[] buffer = message.ToBytes(); + return MCUSerialBridgeCoreAPI.msb_write_port( + nativeHandle, + portIndex, + buffer, + (uint)buffer.Length, + timeout + ); + } + catch + { + return MCUSerialBridgeError.CAN_DataError; + } + } + + private readonly Dictionary< + byte, + MCUSerialBridgeCoreAPI.msb_on_port_data_callback_function_t + > _portCallbacks = []; + + /// + /// 注册指定端口(Serial)的回调函数 + /// + /// 端口索引 + /// 接收数据回调，byte[] 为接收到的原始数据 + /// 错误码 + /// + /// 注意事项： + /// 1. 回调会在底层 C 层线程中直接调用，请**不要在回调内阻塞**，例如等待 I/O 或 Sleep。 + /// 2. 回调内**不能调用 WriteSerial/WriteCAN 等发送函数**，否则可能导致死锁或丢帧。 + /// 3. 回调内只能做轻量级操作，例如简单解析、统计或打标记。 + /// 4. 若需要复杂处理（例如长时间解析、解码、存储数据库等），请**将数据入队到另一个线程**，再在后台处理。 + /// 5. 数据可能随时到来，请保证回调尽快返回，避免影响后续帧接收。 + /// 6. 不要把其他类型的端口注册到这个接口，接口不对 portIndex 做类型检查。 + /// + // M层串口通信：注册指定串口的异步接收回调。 + public MCUSerialBridgeError RegisterSerialPortCallback( + byte portIndex, + Action callback + ) + { + if (callback == null) + return MCUSerialBridgeError.Win_InvalidParam; + + if (portIndex > MaxPortNumber) + return MCUSerialBridgeError.Config_PortNumOver; + + // 包装 C# 回调为 P/Invoke 委托 + // M层串口回调：复制原生缓存并转交托管回调处理。 + void del(IntPtr dst_data, uint dst_data_size, IntPtr user_ctx) + { + byte[] data = new byte[dst_data_size]; + Marshal.Copy(dst_data, data, 0, (int)dst_data_size); + callback(data); + } + + // 保存引用，防止 GC 回收 + _portCallbacks[portIndex] = del; + + // 调用 C 层注册 + return MCUSerialBridgeCoreAPI.msb_register_port_data_callback( + nativeHandle, + portIndex, + _portCallbacks[portIndex], + IntPtr.Zero + ); + } + + /// + /// 注册指定端口(CAN)的回调函数 + /// + /// 端口索引 + /// 接收数据回调，CANMessage 为接收到的原始数据 + /// 错误码 + /// + /// 注意事项： + /// 1. 回调会在底层 C 层线程中直接调用，请**不要在回调内阻塞**，例如等待 I/O 或 Sleep。 + /// 2. 回调内**不能调用 WriteSerial/WriteCAN 等发送函数**，否则可能导致死锁或丢帧。 + /// 3. 回调内只能做轻量级操作，例如简单解析、统计或打标记。 + /// 4. 若需要复杂处理（例如长时间解析、解码、存储数据库等），请**将数据入队到另一个线程**，再在后台处理。 + /// 5. 数据可能随时到来，请保证回调尽快返回，避免影响后续帧接收。 + /// 6. 不要把其他类型的端口注册到这个接口，接口不对 portIndex 做类型检查。 + /// + // M层CAN通信：注册指定CAN通道的异步接收回调。 + public MCUSerialBridgeError RegisterCANPortCallback( + byte portIndex, + Action callback + ) + { + if (callback == null) + return MCUSerialBridgeError.Win_InvalidParam; + + if (portIndex > MaxPortNumber) + return MCUSerialBridgeError.Config_PortNumOver; + + // 包装 C# 回调为 P/Invoke 委托 + // M层CAN回调：还原原生CAN帧并转交托管回调处理。 + void del(IntPtr dst_data, uint dst_data_size, IntPtr user_ctx) + { + try + { + byte[] data = new byte[dst_data_size]; + Marshal.Copy(dst_data, data, 0, (int)dst_data_size); + CANMessage msg = CANMessage.FromBytes(data, dst_data_size); + callback(msg); + } + catch + { + // 解析失败直接忽略，保证回调不会抛异常阻塞 C 层线程 + } + } + + // 保存引用，防止 GC 回收 + _portCallbacks[portIndex] = del; + + // 调用 C 层注册 + return MCUSerialBridgeCoreAPI.msb_register_port_data_callback( + nativeHandle, + portIndex, + _portCallbacks[portIndex], + IntPtr.Zero + ); + } + } +} diff --git a/MedullaAdapter/MCUSerialBridgeError.cs b/MedullaAdapter/MCUSerialBridgeError.cs new file mode 100644 index 0000000..1c86e73 --- /dev/null +++ b/MedullaAdapter/MCUSerialBridgeError.cs @@ -0,0 +1,98 @@ +// MCU通信错误码 + +namespace MCUSerialBridgeCLR +{ + public enum MCUSerialBridgeError : uint + { + OK = 0x00000000, // Success + NoData = 0x00000001, // Port no new data + Win_Unknown = 0x80000001, // Unknown Windows error + Win_InvalidParam = 0x80000002, // Invalid parameter + Win_AllocFail = 0x80000003, // Memory allocation failed + Win_HandleNotFound = 0x80000004, // Handle not found + Win_ResourceBusy = 0x80000005, // Resource busy + Win_BufferFull = 0x80000006, // Buffer is full + Win_UserBufferTooSmall = 0x80000007, // Buffer is too small + Win_CannotOpenPort = 0x80000010, // Cannot open port + Win_CannotGetCommState = 0x80000011, // Cannot get comm state + Win_CannotSetCommState = 0x80000012, // Cannot set comm state + Win_CannotCreateThread = 0x80000013, // Cannot create thread + Proto_Invalid = 0xE0000001, // Protocol invalid + Proto_Checksum = 0xE0000002, // CRC check failed + Proto_Timeout = 0xE0000003, // Protocol timeout + Proto_FrameTooLong = 0xE0000004, // Frame too long + Proto_UnknownCommand = 0xE0000005, // Unknown command + Proto_InvalidPayload = 0xE0000006, // Invalid payload + State_NotRunning = 0xF0000000, // Not running, configure + State_Running = 0xF0000001, // Can not configure, already running + Config_PortNumOver = 0xC0000000, // Port number over + Config_SerialNumOver = 0xC0000001, // Serial port number over + Config_CANNumOver = 0xC0000002, // CAN number over + Config_UnknownPortType = 0xC0000010, // Unknown Port Type + Serial_OpenFail = 0x01000001, // Serial open failed + Serial_NotOpen = 0x01000002, // Serial not open + Serial_ReadFail = 0x01000003, // Serial read failed + Serial_WriteFail = 0x01000004, // Serial write failed + Serial_Busy = 0x01000005, // Serial is busy + CAN_DataError = 0x02000000, // CAN data error + CAN_SendFail = 0x02000001, // CAN send failed + CAN_RecvFail = 0x02000002, // CAN receive failed + CAN_BufferFull = 0x02000003, // CAN buffer full + CAN_NotInit = 0x02000004, // CAN not initialized + Port_WriteBusy = 0x10000001, // Port is busy now + MCU_Unknown = 0x00010001, // MCU unknown error + MCU_IOSizeError = 0x00010002, // IO Should be 4 bytes + MCU_OverTemperature = 0x00010010, // MCU over temperature + } + + public static class MCUSerialBridgeErrorExtensions + { + // M层MCU适配：把串口桥错误码转换为便于诊断的说明。 + public static string ToDescription(this MCUSerialBridgeError err) + { + return err switch + { + MCUSerialBridgeError.OK => "OK|Success", + MCUSerialBridgeError.NoData => "NoData|Port no new data", + MCUSerialBridgeError.Win_Unknown => "Win_Unknown|Unknown Windows error", + MCUSerialBridgeError.Win_InvalidParam => "Win_InvalidParam|Invalid parameter", + MCUSerialBridgeError.Win_AllocFail => "Win_AllocFail|Memory allocation failed", + MCUSerialBridgeError.Win_HandleNotFound => "Win_HandleNotFound|Handle not found", + MCUSerialBridgeError.Win_ResourceBusy => "Win_ResourceBusy|Resource busy", + MCUSerialBridgeError.Win_BufferFull => "Win_BufferFull|Buffer is full", + MCUSerialBridgeError.Win_UserBufferTooSmall => "Win_UserBufferTooSmall|Buffer is too small", + MCUSerialBridgeError.Win_CannotOpenPort => "Win_CannotOpenPort|Cannot open port", + MCUSerialBridgeError.Win_CannotGetCommState => "Win_CannotGetCommState|Cannot get comm state", + MCUSerialBridgeError.Win_CannotSetCommState => "Win_CannotSetCommState|Cannot set comm state", + MCUSerialBridgeError.Win_CannotCreateThread => "Win_CannotCreateThread|Cannot create thread", + MCUSerialBridgeError.Proto_Invalid => "Proto_Invalid|Protocol invalid", + MCUSerialBridgeError.Proto_Checksum => "Proto_Checksum|CRC check failed", + MCUSerialBridgeError.Proto_Timeout => "Proto_Timeout|Protocol timeout", + MCUSerialBridgeError.Proto_FrameTooLong => "Proto_FrameTooLong|Frame too long", + MCUSerialBridgeError.Proto_UnknownCommand => "Proto_UnknownCommand|Unknown command", + MCUSerialBridgeError.Proto_InvalidPayload => "Proto_InvalidPayload|Invalid payload", + MCUSerialBridgeError.State_NotRunning => "State_NotRunning|Not running, configure", + MCUSerialBridgeError.State_Running => "State_Running|Can not configure, already running", + MCUSerialBridgeError.Config_PortNumOver => "Config_PortNumOver|Port number over", + MCUSerialBridgeError.Config_SerialNumOver => "Config_SerialNumOver|Serial port number over", + MCUSerialBridgeError.Config_CANNumOver => "Config_CANNumOver|CAN number over", + MCUSerialBridgeError.Config_UnknownPortType => "Config_UnknownPortType|Unknown Port Type", + MCUSerialBridgeError.Serial_OpenFail => "Serial_OpenFail|Serial open failed", + MCUSerialBridgeError.Serial_NotOpen => "Serial_NotOpen|Serial not open", + MCUSerialBridgeError.Serial_ReadFail => "Serial_ReadFail|Serial read failed", + MCUSerialBridgeError.Serial_WriteFail => "Serial_WriteFail|Serial write failed", + MCUSerialBridgeError.Serial_Busy => "Serial_Busy|Serial is busy", + MCUSerialBridgeError.CAN_DataError => "CAN_DataError|CAN data error", + MCUSerialBridgeError.CAN_SendFail => "CAN_SendFail|CAN send failed", + MCUSerialBridgeError.CAN_RecvFail => "CAN_RecvFail|CAN receive failed", + MCUSerialBridgeError.CAN_BufferFull => "CAN_BufferFull|CAN buffer full", + MCUSerialBridgeError.CAN_NotInit => "CAN_NotInit|CAN not initialized", + MCUSerialBridgeError.Port_WriteBusy => "Port_WriteBusy|Port is busy now", + MCUSerialBridgeError.MCU_Unknown => "MCU_Unknown|MCU unknown error", + MCUSerialBridgeError.MCU_IOSizeError => "MCU_IOSizeError|IO Should be 4 bytes", + MCUSerialBridgeError.MCU_OverTemperature => "MCU_OverTemperature|MCU over temperature", + _ => "Unknown Error", + }; + } + } +} diff --git a/MedullaAdapter/MedullaAdapter.csproj b/MedullaAdapter/MedullaAdapter.csproj new file mode 100644 index 0000000..e374356 --- /dev/null +++ b/MedullaAdapter/MedullaAdapter.csproj @@ -0,0 +1,58 @@ + + + + net8.0 + enable + disable + AnyCPU + + MedullaAdapter + + false + build\Medulla\plugins\ + + + + + ref\RefCartActivator.dll + false + + + ref\RefMedullaCore.dll + false + + + + ref\RefFundamentalLib.dll + false + + + + ref\MDCSToolBox.dll + false + + + + + ..\ref\CommonUsage.dll + + + + + + + + + + + + + + diff --git a/MedullaAdapter/MotorRoutine.cs b/MedullaAdapter/MotorRoutine.cs new file mode 100644 index 0000000..5ca2cea --- /dev/null +++ b/MedullaAdapter/MotorRoutine.cs @@ -0,0 +1,364 @@ +// 计算8个驱动电机的目标速度和舵角PID +using CartActivator; +using FundamentalLib; +using MDCSToolBox.Commons; +using System; +using static MDCSToolBox.Medulla.Chassis.BasicCartDefinition; + +namespace MedullaAdapter +{ + public class MotorRoutine : LadderLogic + { + private bool _wasTransmitterControlling; + private DateTime _lastMoveTime = DateTime.Now; + public override void Operation(int iteration) + { + if (!cart.GhostMode && cart.State == -1) return; + // SA稳定打开后，使能实体遥控器。 + TriggerOnce( + cart.TransmitterConnected && cart.Transmitter_SA, + 300, + () => + { + cart.TransmitterControlEnable = true; + cart.TransmitterLastTime = DateTime.Now; + }); + // 遥控器断连或SA关闭时，立即撤销遥控使能。 + if (!cart.TransmitterConnected || !cart.Transmitter_SA) + cart.TransmitterControlEnable = false; + var transmitterSelected = + cart.TransmitterConnected && + cart.TransmitterControlEnable && + cart.Transmitter_SA && + cart.Transmitter_SC == cart.CarNum; + var transmitterControlling = + transmitterSelected && + CartDefinition.testPriority(5, "TransmitterMode"); + if (transmitterControlling) + { + TransmitterChassisControl(); + cart.TransmitterLastTime = DateTime.Now; + cart.CarStatu = "实体遥控器控制"; + } + else + { + // 只在遥控器刚刚退出时发送一次停车， + // 不能每周期停车，否则会覆盖C层轨迹控制。 + if (_wasTransmitterControlling) + { + cart.ManualControl( + cart.TransmitterControlMode, + 0, 0, 0, + cart.TransmitterSpeed, + DateTime.Now - cart.TransmitterLastTime); + + StopClampArms(); + cart.TransmitterLastTime = DateTime.Now; + } + + cart.CarStatu = "正常运行"; + } + _wasTransmitterControlling = transmitterControlling; + // 当前是否由C层控制。 + cart.ClumsyControl = CartDefinition.currentPriority == 0; + // 计算四个舵轮PID和8个驱动电机最终速度。 + UpdateDiffSteerWheelSpeeds(); + // 平滑更新硬件速度限制。 + UpdateSendSpeedLimit(); + // 更新红黄绿灯状态。 + UpdateLightMode(); + } + // 物理遥控器设置 + public void TransmitterChassisControl() + { + var interval = DateTime.Now - cart.TransmitterLastTime; + switch (cart.Transmitter_SB) + { + case TransmitterState.Mode0: + cart.TransmitterControlMode = + DiverCartDefinition.ManualControlMode.Normal; + break; + case TransmitterState.Mode1: + cart.TransmitterControlMode = + DiverCartDefinition.ManualControlMode.Crab; + break; + case TransmitterState.Mode2: + cart.TransmitterControlMode = + DiverCartDefinition.ManualControlMode.Spin; + break; + default: + cart.ManualControl( + cart.TransmitterControlMode, + 0, 0, 0, + cart.TransmitterSpeed, + interval); + StopClampArms(); + return; + } + // SA关闭后立即停车。 + if (!cart.Transmitter_SA) + { + cart.ManualControl( + cart.TransmitterControlMode, + 0, 0, 0, + cart.TransmitterSpeed, + interval); + StopClampArms(); + return; + } + // 限制实体遥控器的最大速度。 + cart.TransmitterSpeed = Math.Max( + cart.TransmitterSpeedLowerLimit, + Math.Min( + cart.TransmitterSpeed, + cart.TransmitterSpeedUpperLimit)); + // SD的Mode0作为底盘驾驶档。 + if (cart.Transmitter_SD == TransmitterState.Mode0) + { + // 底盘驾驶档不允许保留上一周期的夹臂速度。 + StopClampArms(); + + cart.ManualControl( + cart.TransmitterControlMode, + cart.TransmitterLeftJoystickValX, + cart.TransmitterRightJoystickValY, + 0, + cart.TransmitterSpeed, + interval); + + return; + } + if (cart.Transmitter_SD == TransmitterState.Mode1) + { + // 切换到夹臂档时，先确保底盘停止。 + cart.ManualControl( + cart.TransmitterControlMode, + 0, 0, 0, + cart.TransmitterSpeed, + interval); + + var armSpeed = + cart.TransmitterRightJoystickValX * + cart.ManualArmSpeedFac; + + cart.SpeedLeftArm = armSpeed; + cart.SpeedRightArm = armSpeed; + return; + } + // 非驾驶档必须主动停车，防止上一条运动指令残留。 + cart.ManualControl( + cart.TransmitterControlMode, + 0, 0, 0, + cart.TransmitterSpeed, + interval); + StopClampArms(); + } + + // M层单车夹臂安全：清除物理遥控器留下的左右夹臂速度命令。 + private void StopClampArms() + { + cart.SpeedLeftArm = 0; + cart.SpeedRightArm = 0; + } + + // M层单车底盘：根据四个舵轮的目标角度和实际角度修正8个驱动电机速度。 + private void UpdateDiffSteerWheelSpeeds() + { + if (cart.LeftFrontPid == null || + cart.LeftRearPid == null || + cart.RightFrontPid == null || + cart.RightRearPid == null) + { + cart.SpeedLFL = 0; + cart.SpeedLFR = 0; + cart.SpeedRFL = 0; + cart.SpeedRFR = 0; + cart.SpeedLRL = 0; + cart.SpeedLRR = 0; + cart.SpeedRRL = 0; + cart.SpeedRRR = 0; + return; + } + + // 更新左前舵轮PID参数。 + cart.LeftFrontPid.ChangeParameters( + cart.DiffSteerKp, + cart.DiffSteerKi, + cart.DiffSteerKd, + cart.DiffSteerMaxI, + cart.DiffSteerDeadZone, + cart.DiffSteerThresh, + cart.DiffSteerSpeedAcc); + + // 更新左后舵轮PID参数。 + cart.LeftRearPid.ChangeParameters( + cart.DiffSteerKp, + cart.DiffSteerKi, + cart.DiffSteerKd, + cart.DiffSteerMaxI, + cart.DiffSteerDeadZone, + cart.DiffSteerThresh, + cart.DiffSteerSpeedAcc); + + // 更新右前舵轮PID参数。 + cart.RightFrontPid.ChangeParameters( + cart.DiffSteerKp, + cart.DiffSteerKi, + cart.DiffSteerKd, + cart.DiffSteerMaxI, + cart.DiffSteerDeadZone, + cart.DiffSteerThresh, + cart.DiffSteerSpeedAcc); + + // 更新右后舵轮PID参数。 + cart.RightRearPid.ChangeParameters( + cart.DiffSteerKp, + cart.DiffSteerKi, + cart.DiffSteerKd, + cart.DiffSteerMaxI, + cart.DiffSteerDeadZone, + cart.DiffSteerThresh, + cart.DiffSteerSpeedAcc); + + // 根据实际舵角计算四条腿的差速修正量。 + var diffLf = cart.LeftFrontPid.GetResponse( + cart.ThLeftFront, false, false, "LF"); + + var diffLr = cart.LeftRearPid.GetResponse( + cart.ThLeftRear, false, false, "LR"); + + var diffRf = cart.RightFrontPid.GetResponse( + cart.ThRightFront, false, false, "RF"); + + var diffRr = cart.RightRearPid.GetResponse( + cart.ThRightRear, false, false, "RR"); + + // 保存四个转向PID的本周期修正量，供M层监控和舵轮响应CSV记录使用。 + cart.DiffSteerOutputLeftFront = diffLf; + cart.DiffSteerOutputLeftRear = diffLr; + cart.DiffSteerOutputRightFront = diffRf; + cart.DiffSteerOutputRightRear = diffRr; + + // 左前腿：左右电机施加方向相反的PID修正量。 + cart.SpeedLFL = cart.SpeedLeftFrontLeft - diffLf; + cart.SpeedLFR = cart.SpeedLeftFrontRight + diffLf; + + // 左后腿。 + cart.SpeedLRL = cart.SpeedLeftRearLeft - diffLr; + cart.SpeedLRR = cart.SpeedLeftRearRight + diffLr; + + // 右前腿。 + cart.SpeedRFL = cart.SpeedRightFrontLeft - diffRf; + cart.SpeedRFR = cart.SpeedRightFrontRight + diffRf; + + // 右后腿。 + cart.SpeedRRL = cart.SpeedRightRearLeft - diffRr; + cart.SpeedRRR = cart.SpeedRightRearRight + diffRr; + } + + // M层单车限速：按照加速度和减速度平滑更新实际下发速度上限。 + private void UpdateSendSpeedLimit() + { + if (cart.Chassis == null) + { + cart.SendThresSpeed = 0; + return; + } + + var now = DateTime.Now; + var elapsedSeconds = (float)(now - _lastMoveTime).TotalSeconds; + _lastMoveTime = now; + + // 防止调试暂停或线程卡顿后，一次产生过大的速度跳变。 + elapsedSeconds = Math.Clamp(elapsedSeconds, 0f, 0.2f); + + // 限速值不允许小于零。 + var targetLimit = Math.Max(0f, cart.ThresSpeed); + var currentLimit = Math.Max(0f, cart.SendThresSpeed); + + // 增大速度上限时用加速度，减小时用减速度。 + var speedChangingRate = + targetLimit > currentLimit + ? cart.Chassis.AccPerSecond + : cart.Chassis.DeAccPerSecond; + + speedChangingRate = Math.Max(0f, speedChangingRate); + + var maxChange = speedChangingRate * elapsedSeconds; + var speedDifference = targetLimit - currentLimit; + + if (Math.Abs(speedDifference) <= maxChange) + { + currentLimit = targetLimit; + } + else + { + currentLimit += Math.Sign(speedDifference) * maxChange; + } + + // 计算当前8个电机目标速度中的最大绝对值。 + var wheelMaxSpeed = 0f; + + wheelMaxSpeed = Math.Max(wheelMaxSpeed, Math.Abs(cart.SpeedLFL)); + wheelMaxSpeed = Math.Max(wheelMaxSpeed, Math.Abs(cart.SpeedLFR)); + wheelMaxSpeed = Math.Max(wheelMaxSpeed, Math.Abs(cart.SpeedRFL)); + wheelMaxSpeed = Math.Max(wheelMaxSpeed, Math.Abs(cart.SpeedRFR)); + wheelMaxSpeed = Math.Max(wheelMaxSpeed, Math.Abs(cart.SpeedLRL)); + wheelMaxSpeed = Math.Max(wheelMaxSpeed, Math.Abs(cart.SpeedLRR)); + wheelMaxSpeed = Math.Max(wheelMaxSpeed, Math.Abs(cart.SpeedRRL)); + wheelMaxSpeed = Math.Max(wheelMaxSpeed, Math.Abs(cart.SpeedRRR)); + + // 没有必要让平滑限速值高于当前所有车轮需要的速度。 + if (targetLimit < wheelMaxSpeed && + currentLimit > wheelMaxSpeed) + { + currentLimit = wheelMaxSpeed; + } + + cart.SendThresSpeed = currentLimit; + } + + // M层单车灯光：根据报警、驱动器、限速和电量状态生成红黄绿灯模式。 + private void UpdateLightMode() + { + // 二级报警：红灯常亮。 + if (cart.AlarmLevel == 2) + { + cart.LightMode = 2; + return; + } + + // 一级报警：黄灯常亮。 + if (cart.AlarmLevel == 1) + { + cart.LightMode = 3; + return; + } + + // 驱动轮未使能：黄灯闪烁。 + if (!cart.WheelAbleState) + { + FlipFlop(ref cart.LightMode, 500, 0, 3); + return; + } + + // C层正在进行限速：黄灯常亮。 + if (Math.Abs(cart.ThresSpeed - 1f) > 0.001f) + { + cart.LightMode = 3; + return; + } + + // 电量低于预警值：黄灯常亮。 + if (cart.Soc <= cart.LowBatteryAlarmThreshold) + { + cart.LightMode = 3; + return; + } + + // 正常运行：绿灯闪烁。 + FlipFlop(ref cart.LightMode, 500, 0, 1); + } + + } +} diff --git a/MedullaAdapter/Remote.cs b/MedullaAdapter/Remote.cs new file mode 100644 index 0000000..85ceb6e --- /dev/null +++ b/MedullaAdapter/Remote.cs @@ -0,0 +1,86 @@ +// Medulla虚拟遥控器和夹臂控制 +using MDCSToolBox.Medulla.Chassis.MultiWheel; +using CartActivator; + +namespace MedullaAdapter +{ + // M层单车虚拟遥控器：使用父类提供的底盘控制界面。 + public class Remote : MultiWheelRemote + { + // 将父类虚拟遥控器界面输入统一转发到本车的ManualControl。 + public override void ChassisOperation() + { + if (MultiVehicleMode.on) + { + MultiVehicleModeChassisLogic(); + statusText = "单车版本不支持多车联动遥控"; + return; + } + + // 当前单车适配层只定义Normal、Crab和Spin三种模式。 + // 禁止这些旧按钮绕过适配层直接修改底盘坐标偏置。 + if (AckermannMode.on || + SwayMode.on || + XYThMode.on) + { + cart.Chassis?.PredefinedDriveStop(); + statusText = "当前单车版本暂不支持阿克曼、斜行或全向模式"; + return; + } + + var mode = SpinMode.on + ? DiverCartDefinition.ManualControlMode.Spin + : CrabMode.on + ? DiverCartDefinition.ManualControlMode.Crab + : DiverCartDefinition.ManualControlMode.Normal; + + cart.ManualControl( + mode, + SpeedPad.x, + SpeedPad.y, + FrontDirection.dval * 180, + SpeedThreshold.val); + + statusText = + $"{mode}, x={SpeedPad.x:0.00}, " + + $"y={SpeedPad.y:0.00}, " + + $"speed={SpeedThreshold.val:0.00}"; + } + [AsControlItem(name = "夹抱速度", LayoutRow = 0, LayoutCol = 4)] + public Throttle ArmSpeed; + + [AsControlItem(name = "夹抱打开", LayoutRow = 1, LayoutCol = 0)] + public Button Open; + + [AsControlItem(name = "夹抱关闭", LayoutRow = 1, LayoutCol = 2)] + public Button Close; + // M层虚拟遥控器：控制左右夹臂同步打开或关闭。 + public override void CustomOperation() + { + if (Open.pressed) + { + cart.SpeedLeftArm = + -ArmSpeed.val * cart.ManualArmSpeedFac; + cart.SpeedRightArm = + -ArmSpeed.val * cart.ManualArmSpeedFac; + } + else if (Close.pressed) + { + cart.SpeedLeftArm = + ArmSpeed.val * cart.ManualArmSpeedFac; + cart.SpeedRightArm = + ArmSpeed.val * cart.ManualArmSpeedFac; + } + else + { + cart.SpeedLeftArm = 0; + cart.SpeedRightArm = 0; + } + } + // 单车不支持多车联动，误打开多车开关时主动停车。 + public override void MultiVehicleModeChassisLogic() + { + cart.Chassis?.PredefinedDriveStop(); + } + } +} diff --git a/MedullaAdapter/WheelSpeedDiagnosticLogger.cs b/MedullaAdapter/WheelSpeedDiagnosticLogger.cs new file mode 100644 index 0000000..b6b8218 --- /dev/null +++ b/MedullaAdapter/WheelSpeedDiagnosticLogger.cs @@ -0,0 +1,381 @@ +using System; +using System.Collections.Concurrent; +using System.Diagnostics; +using System.Globalization; +using System.IO; +using System.Text; +using System.Threading; + +namespace MedullaAdapter +{ + /// + /// 在后台保存驱动器CAN速度事件和底盘周期快照，避免文件IO阻塞CAN回调。 + /// + internal sealed class WheelSpeedDiagnosticLogger : IDisposable + { + private readonly struct LogRecord + { + public LogRecord(bool isCanEvent, string line) + { + IsCanEvent = isCanEvent; + Line = line; + } + + public bool IsCanEvent { get; } + + public string Line { get; } + } + + private const int MaximumQueuedRecords = 100000; + private const double SnapshotIntervalMilliseconds = 20.0; + private readonly ConcurrentQueue _records = new(); + private readonly AutoResetEvent _recordsAvailable = new(false); + private readonly object _lifecycleLock = new(); + private Stopwatch _stopwatch; + private Thread _writerThread; + private StreamWriter _canWriter; + private StreamWriter _snapshotWriter; + private volatile bool _isRunning; + private int _queuedRecordCount; + private long _receiveSequence; + private long _droppedRecordCount; + private double _lastSnapshotMilliseconds = double.NegativeInfinity; + + public bool IsRunning => _isRunning; + + public string CanLogPath { get; private set; } = ""; + + public string SnapshotLogPath { get; private set; } = ""; + + /// + /// 创建本次诊断的两个CSV文件并启动后台写入线程。 + /// + public void Start(string directory, int carNumber) + { + lock (_lifecycleLock) + { + if (_isRunning) + return; + + if (string.IsNullOrWhiteSpace(directory)) + throw new ArgumentException( + "轮速诊断目录不能为空。", + nameof(directory)); + + Directory.CreateDirectory(directory); + + var filePrefix = + $"{DateTime.Now:yyyyMMdd_HHmmss_fff}_Car{carNumber}"; + + CanLogPath = Path.Combine( + directory, + $"{filePrefix}_can.csv"); + + SnapshotLogPath = Path.Combine( + directory, + $"{filePrefix}_snapshot.csv"); + + _canWriter = CreateWriter(CanLogPath); + _snapshotWriter = CreateWriter(SnapshotLogPath); + + _canWriter.WriteLine( + "ElapsedMs,ReceiveSequence,CanId,MotorName,RawRpm,SpeedMps"); + + _snapshotWriter.WriteLine( + "ElapsedMs,CarNum,ManualControlMode,ManualMode,SendThresSpeed," + + "DiffSteerKp,DiffSteerKi,DiffSteerKd,DiffSteerMaxI,DiffSteerDeadZone,DiffSteerThresh,DiffSteerSpeedAcc," + + "PidOutLeftFront,PidOutLeftRear,PidOutRightFront,PidOutRightRear," + + "CmdLFL,CmdLFR,CmdLRL,CmdLRR,CmdRFL,CmdRFR,CmdRRL,CmdRRR," + + "PidLFL,PidLFR,PidLRL,PidLRR,PidRFL,PidRFR,PidRRL,PidRRR," + + "ActualLFL,ActualLFR,ActualLRL,ActualLRR,ActualRFL,ActualRFR,ActualRRL,ActualRRR," + + "ActualLeftFront,ActualLeftRear,ActualRightFront,ActualRightRear," + + "TargetThLeftFront,TargetThLeftRear,TargetThRightFront,TargetThRightRear," + + "ActualThLeftFront,ActualThLeftRear,ActualThRightFront,ActualThRightRear," + + "ErrorThLeftFront,ErrorThLeftRear,ErrorThRightFront,ErrorThRightRear"); + + while (_records.TryDequeue(out _)) + { + } + + _queuedRecordCount = 0; + _receiveSequence = 0; + _droppedRecordCount = 0; + _lastSnapshotMilliseconds = + double.NegativeInfinity; + _stopwatch = Stopwatch.StartNew(); + _isRunning = true; + + _writerThread = new Thread(WriterLoop) + { + IsBackground = true, + Name = "WheelSpeedDiagnosticWriter" + }; + _writerThread.Start(); + } + } + + /// + /// 停止记录并等待队列中的诊断数据写入磁盘。 + /// + public void Stop() + { + Thread writerThread; + + lock (_lifecycleLock) + { + if (!_isRunning && + _writerThread == null) + return; + + _isRunning = false; + writerThread = _writerThread; + _recordsAvailable.Set(); + } + + writerThread?.Join(3000); + + lock (_lifecycleLock) + { + _canWriter?.Flush(); + _snapshotWriter?.Flush(); + _canWriter?.Dispose(); + _snapshotWriter?.Dispose(); + _canWriter = null; + _snapshotWriter = null; + _writerThread = null; + _stopwatch?.Stop(); + } + } + + /// + /// 将一帧驱动器速度反馈加入内存队列，不在CAN回调中执行文件写入。 + /// + public void RecordCanFeedback( + ushort canId, + string motorName, + float rawRpm, + float speedMetersPerSecond) + { + if (!_isRunning) + return; + + var elapsedMilliseconds = + _stopwatch.Elapsed.TotalMilliseconds; + var receiveSequence = + Interlocked.Increment( + ref _receiveSequence); + + var line = string.Join( + ",", + Format(elapsedMilliseconds), + receiveSequence.ToString( + CultureInfo.InvariantCulture), + $"0x{canId:X3}", + motorName, + Format(rawRpm), + Format(speedMetersPerSecond)); + + Enqueue(new LogRecord( + isCanEvent: true, + line)); + } + + /// + /// 按最多50Hz记录一帧控制命令、PID输出、CAN反馈和舵角快照。 + /// + public void RecordSnapshot( + DiverCartDefinition cart) + { + if (!_isRunning || cart == null) + return; + + var elapsedMilliseconds = + _stopwatch.Elapsed.TotalMilliseconds; + + if (elapsedMilliseconds - + _lastSnapshotMilliseconds < + SnapshotIntervalMilliseconds) + { + return; + } + + _lastSnapshotMilliseconds = + elapsedMilliseconds; + + var line = string.Join( + ",", + Format(elapsedMilliseconds), + cart.CarNum.ToString( + CultureInfo.InvariantCulture), + Format((int)cart.TransmitterControlMode), + Format(cart.ManualMode), + Format(cart.SendThresSpeed), + Format(cart.DiffSteerKp), + Format(cart.DiffSteerKi), + Format(cart.DiffSteerKd), + Format(cart.DiffSteerMaxI), + Format(cart.DiffSteerDeadZone), + Format(cart.DiffSteerThresh), + Format(cart.DiffSteerSpeedAcc), + Format(cart.DiffSteerOutputLeftFront), + Format(cart.DiffSteerOutputLeftRear), + Format(cart.DiffSteerOutputRightFront), + Format(cart.DiffSteerOutputRightRear), + Format(cart.SpeedLeftFrontLeft), + Format(cart.SpeedLeftFrontRight), + Format(cart.SpeedLeftRearLeft), + Format(cart.SpeedLeftRearRight), + Format(cart.SpeedRightFrontLeft), + Format(cart.SpeedRightFrontRight), + Format(cart.SpeedRightRearLeft), + Format(cart.SpeedRightRearRight), + Format(cart.SpeedLFL), + Format(cart.SpeedLFR), + Format(cart.SpeedLRL), + Format(cart.SpeedLRR), + Format(cart.SpeedRFL), + Format(cart.SpeedRFR), + Format(cart.SpeedRRL), + Format(cart.SpeedRRR), + Format(cart.ActualSpeedLeftFrontLeft), + Format(cart.ActualSpeedLeftFrontRight), + Format(cart.ActualSpeedLeftRearLeft), + Format(cart.ActualSpeedLeftRearRight), + Format(cart.ActualSpeedRightFrontLeft), + Format(cart.ActualSpeedRightFrontRight), + Format(cart.ActualSpeedRightRearLeft), + Format(cart.ActualSpeedRightRearRight), + Format(cart.ActualSpeedLeftFront), + Format(cart.ActualSpeedLeftRear), + Format(cart.ActualSpeedRightFront), + Format(cart.ActualSpeedRightRear), + Format(cart.ThLeftFront), + Format(cart.ThLeftRear), + Format(cart.ThRightFront), + Format(cart.ThRightRear), + Format(cart.ActualThLeftFront), + Format(cart.ActualThLeftRear), + Format(cart.ActualThRightFront), + Format(cart.ActualThRightRear), + Format(cart.ThLeftFront - cart.ActualThLeftFront), + Format(cart.ThLeftRear - cart.ActualThLeftRear), + Format(cart.ThRightFront - cart.ActualThRightFront), + Format(cart.ThRightRear - cart.ActualThRightRear)); + + Enqueue(new LogRecord( + isCanEvent: false, + line)); + } + + private static StreamWriter CreateWriter( + string path) + { + return new StreamWriter( + path, + append: false, + new UTF8Encoding( + encoderShouldEmitUTF8Identifier: true), + bufferSize: 64 * 1024); + } + + private void Enqueue(LogRecord record) + { + var queuedCount = + Interlocked.Increment( + ref _queuedRecordCount); + + if (queuedCount > + MaximumQueuedRecords) + { + Interlocked.Decrement( + ref _queuedRecordCount); + Interlocked.Increment( + ref _droppedRecordCount); + return; + } + + _records.Enqueue(record); + _recordsAvailable.Set(); + } + + private void WriterLoop() + { + var lastFlushTime = DateTime.UtcNow; + + try + { + while (_isRunning || + !_records.IsEmpty) + { + var wroteAnyRecord = false; + + while (_records.TryDequeue( + out var record)) + { + Interlocked.Decrement( + ref _queuedRecordCount); + + if (record.IsCanEvent) + _canWriter.WriteLine(record.Line); + else + _snapshotWriter.WriteLine(record.Line); + + wroteAnyRecord = true; + } + + var shouldFlush = + wroteAnyRecord && + (DateTime.UtcNow - + lastFlushTime) + .TotalMilliseconds >= 500.0; + + if (shouldFlush) + { + _canWriter.Flush(); + _snapshotWriter.Flush(); + lastFlushTime = DateTime.UtcNow; + } + + if (!wroteAnyRecord) + _recordsAvailable.WaitOne(100); + } + + var dropped = + Interlocked.Read( + ref _droppedRecordCount); + + if (dropped > 0) + { + _canWriter.WriteLine( + $"# DroppedRecords={dropped}"); + _snapshotWriter.WriteLine( + $"# DroppedRecords={dropped}"); + } + + _canWriter.Flush(); + _snapshotWriter.Flush(); + } + catch (Exception ex) + { + // 后台日志失败不能终止车辆控制线程。 + Console.WriteLine( + "轮速诊断后台写入失败：" + + ex.Message); + } + } + + private static string Format( + double value) + { + return value.ToString( + "0.######", + CultureInfo.InvariantCulture); + } + + public void Dispose() + { + Stop(); + _recordsAvailable.Dispose(); + } + } +} diff --git a/MedullaAdapter/build/Medulla/plugins/CommonUsage.dll b/MedullaAdapter/build/Medulla/plugins/CommonUsage.dll new file mode 100644 index 0000000..9079f92 Binary files /dev/null and b/MedullaAdapter/build/Medulla/plugins/CommonUsage.dll differ diff --git a/MedullaAdapter/build/Medulla/plugins/MedullaAdapter.deps.json b/MedullaAdapter/build/Medulla/plugins/MedullaAdapter.deps.json new file mode 100644 index 0000000..c1ccd7d --- /dev/null +++ b/MedullaAdapter/build/Medulla/plugins/MedullaAdapter.deps.json @@ -0,0 +1,39 @@ +{ + "runtimeTarget": { + "name": ".NETCoreApp,Version=v8.0", + "signature": "" + }, + "compilationOptions": {}, + "targets": { + ".NETCoreApp,Version=v8.0": { + "MedullaAdapter/1.0.0": { + "dependencies": { + "CommonUsage": "1.0.0.0" + }, + "runtime": { + "MedullaAdapter.dll": {} + } + }, + "CommonUsage/1.0.0.0": { + "runtime": { + "CommonUsage.dll": { + "assemblyVersion": "1.0.0.0", + "fileVersion": "1.0.0.0" + } + } + } + } + }, + "libraries": { + "MedullaAdapter/1.0.0": { + "type": "project", + "serviceable": false, + "sha512": "" + }, + "CommonUsage/1.0.0.0": { + "type": "reference", + "serviceable": false, + "sha512": "" + } + } +} \ No newline at end of file diff --git a/MedullaAdapter/build/Medulla/plugins/MedullaAdapter.dll b/MedullaAdapter/build/Medulla/plugins/MedullaAdapter.dll new file mode 100644 index 0000000..7a45380 Binary files /dev/null and b/MedullaAdapter/build/Medulla/plugins/MedullaAdapter.dll differ diff --git a/MedullaAdapter/build/Medulla/plugins/MedullaAdapter.pdb b/MedullaAdapter/build/Medulla/plugins/MedullaAdapter.pdb new file mode 100644 index 0000000..61f92d4 Binary files /dev/null and b/MedullaAdapter/build/Medulla/plugins/MedullaAdapter.pdb differ diff --git a/MedullaAdapter/ref/CycleGUI.dll b/MedullaAdapter/ref/CycleGUI.dll new file mode 100644 index 0000000..92e6ca7 Binary files /dev/null and b/MedullaAdapter/ref/CycleGUI.dll differ diff --git a/MedullaAdapter/ref/MDCSToolBox.dll b/MedullaAdapter/ref/MDCSToolBox.dll new file mode 100644 index 0000000..d9cc740 Binary files /dev/null and b/MedullaAdapter/ref/MDCSToolBox.dll differ diff --git a/MedullaAdapter/ref/RefCartActivator.dll b/MedullaAdapter/ref/RefCartActivator.dll new file mode 100644 index 0000000..2755d6e Binary files /dev/null and b/MedullaAdapter/ref/RefCartActivator.dll differ diff --git a/MedullaAdapter/ref/RefFundamentalLib.dll b/MedullaAdapter/ref/RefFundamentalLib.dll new file mode 100644 index 0000000..cdebe2e Binary files /dev/null and b/MedullaAdapter/ref/RefFundamentalLib.dll differ diff --git a/MedullaAdapter/ref/RefMedullaCore.dll b/MedullaAdapter/ref/RefMedullaCore.dll new file mode 100644 index 0000000..99a9027 Binary files /dev/null and b/MedullaAdapter/ref/RefMedullaCore.dll differ diff --git a/MultiWheelC/AGV.cs b/MultiWheelC/AGV.cs new file mode 100644 index 0000000..6592668 --- /dev/null +++ b/MultiWheelC/AGV.cs @@ -0,0 +1,21 @@ +using ClumsyCore; +using MDCSToolBox.Clumsy.AgvInterfaces; +using MDCSToolBox.Clumsy.MotionControllers; + +namespace MultiWheelC +{ + public class AGV : MultiWheelInterface + { + public override AbstractGeometricController GetController() + => new ChassisController().Get(); + public override MultiWheelMagTracker GetMagController() + => new MultiWheelMagTracker(); + public override NaiveMagnetController GetNaiveMagnetController() + => new NaiveMagnetController(); + + public void Sleep(float seconds) + { + new DriveTask(new Sleep { Second = seconds }.Get()).Wait(); + } + } +} diff --git a/MultiWheelC/ChassisController.cs b/MultiWheelC/ChassisController.cs new file mode 100644 index 0000000..1237da3 --- /dev/null +++ b/MultiWheelC/ChassisController.cs @@ -0,0 +1,45 @@ +using ClumsyCore; +using ClumsyCore.Pilot; +using MDCSToolBox.Clumsy.MotionControllers; +using MDCSToolBox.Clumsy.Movements; +using MDCSToolBox.Clumsy.Pilot; + +namespace MultiWheelC; + +public class ChassisController : MovementDefinition +{ + public float BaseSpeed = Configuration.conf.basicSpeed; + + // 创建单车几何跟踪控制器（直接控本车底盘，不走多车 Auto 通道） + public override MultiWheelGeometricController Get() + { + return new MultiWheelGeometricController + { + Chassis = BasicPilotBase.Chassis, + BaseSpeed = BaseSpeed, + SlowDistance = PilotDefinition.Conf.SlowDistance, + SlowingPow = PilotDefinition.Conf.SlowingPow, + FinishDistance = PilotDefinition.Conf.FinishDistance, + FinishSpeed = PilotDefinition.Conf.FinishSpeed, + FirstThAccuracy = PilotDefinition.Conf.FirstThAccuracy, + FirstRotateSpeedFac = PilotDefinition.Conf.FirstRotateSpeedFac, + FirstRotateMaxSpeed = PilotDefinition.Conf.FirstRotateMaxSpeed, + NotContinuousAngle = PilotDefinition.Conf.NotContinuousAngle, + DebugMode = PilotDefinition.Conf.MotionDebugPrint, + DebugCurvature = PilotDefinition.Conf.DebugCurvature, + PowerSteeringLookAhead = PilotDefinition.Conf.PowerSteeringLookAhead, + SpeedLookAhead = PilotDefinition.Conf.SpeedLookAhead, + SpeedLookAheadCurveDiff = PilotDefinition.Conf.SpeedLookAheadCurveDiff, + SpeedLookBackCurveDiff = PilotDefinition.Conf.SpeedLookBackCurveDiff, + SpeedLimitCurveDiffMin = PilotDefinition.Conf.SpeedLimitCurveDiffMin, + SpeedLimitCurveMin = PilotDefinition.Conf.SpeedLimitCurveMin, + MaxRotateSpeed = PilotDefinition.Conf.MaxRotateSpeedCurveLimit, + MaxRotateAcc = PilotDefinition.Conf.MaxRotateAccCurveLimit, + GcpThetaThreshold = PilotDefinition.Conf.GcpThetaThreshold, + DthLinearFac = PilotDefinition.Conf.DthLinearFac, + DthLinearThreshold = PilotDefinition.Conf.DthLinearThreshold, + BiasFac = PilotDefinition.Conf.BiasFac, + BiasThreshold = PilotDefinition.Conf.BiasThreshold, + }; + } +} \ No newline at end of file diff --git a/MultiWheelC/CrabMotionFrameTracker.cs b/MultiWheelC/CrabMotionFrameTracker.cs new file mode 100644 index 0000000..2e998e3 --- /dev/null +++ b/MultiWheelC/CrabMotionFrameTracker.cs @@ -0,0 +1,817 @@ +using ClumsyCore; +using ClumsyCore.DTools; +using ClumsyCore.Interfaces; +using ClumsyCore.Pilot; +using CommonUsage.Chassis; +using MyParking.Shared; +using System; +using System.Collections.Generic; +using System.Numerics; + +namespace MultiWheelC +{ + // C层单车测试：在可配置的运动坐标系中统一跟踪直线、圆弧或S型曲线。 + public sealed class CrabMotionFrameTracker : MovementDefinition + { + public enum ReferencePathKind + { + Straight = 0, + LeftArc = 1, + SCurve = 2 + } + + public enum ChassisCommandBackend + { + SendXYThSpeed = 0, + SendMotion = 1 + } + + public ReferencePathKind PathKind; + public ChassisCommandBackend CommandBackend = + ChassisCommandBackend.SendMotion; + public Vector2 StartPosition; + public double InitialBodyYawRadians; + public float LengthMillimeters = 4000f; + public float RadiusMillimeters = 2000f; + public float SCurveLateralOffsetMillimeters = 400f; + public double ArcSweepRadians = Math.PI / 2.0; + public float CruiseSpeed = 0.2f; + public float SlowDistanceMillimeters = 600f; + public float FinishDistanceMillimeters = 30f; + public float MinimumSpeed = 0.04f; + public double LateralGainPerSecond = 0.8; + public double MaximumLateralCorrection = 0.12; + public double HeadingGainPerSecond = 1.5; + public double MaximumAngularSpeedRadiansPerSecond = + AngleMath.DegreesToRadians(30.0); + public double MaximumVirtualSteeringRadians = + AngleMath.DegreesToRadians(30.0); + public float WheelAlignmentToleranceDegrees = 2f; + public float WheelAlignmentStableSeconds = 0.3f; + public float WheelAlignmentTimeoutSeconds = 10f; + public float TrackingTimeoutSeconds = 60f; + public Action CommandObserver; + + // 运动坐标系相对车体坐标系的朝向：普通模式为0，蟹行为π/2。 + public double MotionFrameYawInBodyRadians = Math.PI / 2.0; + private double _lastSCurveProgress; + + public override IEnumerable Get() + { + ValidateParameters(); + + var chassis = + PilotDefinition.Chassis as MultiWheelChassis; + if (chassis == null) + throw new InvalidOperationException( + "当前底盘不是MultiWheelChassis，无法执行运动坐标系轨迹测试。"); + + var adapter = new MultiWheelChassisAdapter( + chassis, + PilotDefinition.Self.CarNum); + adapter.ResetToBodyFrame(); + + var lastCommandTime = DateTime.Now; + + try + { + // 模式切换阶段只转舵轮，驱动速度始终保持为零。 + var alignmentStarted = DateTime.Now; + DateTime? stableSince = null; + while (true) + { + if (!adapter.PrepareParallelDirection( + MotionFrameYawInBodyRadians)) + throw new InvalidOperationException( + "无法生成运动坐标系对应的舵轮准备姿态。"); + + var aligned = + adapter.AreParallelWheelsAligned( + MotionFrameYawInBodyRadians, + AngleMath.DegreesToRadians( + WheelAlignmentToleranceDegrees)); + + if (aligned) + { + if (stableSince == null) + stableSince = DateTime.Now; + + if ((DateTime.Now - stableSince.Value) + .TotalSeconds >= + WheelAlignmentStableSeconds) + break; + } + else + { + stableSince = null; + } + + if ((DateTime.Now - alignmentStarted) + .TotalSeconds > + WheelAlignmentTimeoutSeconds) + throw new TimeoutException( + "舵轮在限定时间内未稳定到达运动坐标系初始方向。"); + + yield return true; + } + + if (CommandBackend == + ChassisCommandBackend.SendMotion) + { + // 舵轮已按真实机械角度完成预对齐； + // 现在由Shared适配层激活SendMotion虚拟运动坐标系。 + adapter.ActivateMotionFrame( + MotionFrameYawInBodyRadians); + } + + var trackingStarted = DateTime.Now; + while (true) + { + if ((DateTime.Now - trackingStarted) + .TotalSeconds > + TrackingTimeoutSeconds) + throw new TimeoutException( + "蟹行轨迹在限定时间内未完成。"); + + var location = + DetourInterface.getCartLocation(); + if (!IsFinite(location.x) || + !IsFinite(location.y) || + !IsFinite(location.th)) + throw new InvalidOperationException( + "蟹行轨迹测试期间Detour位姿无效。"); + + var currentPosition = new Vector2( + (float)location.x, + (float)location.y); + var currentBodyYaw = + AngleMath.DegreesToRadians(location.th); + + CalculateReference( + currentPosition, + out var tangentYaw, + out var referencePoint, + out var remainingMillimeters, + out var referenceCurvature); + + if (remainingMillimeters <= + FinishDistanceMillimeters) + break; + + var speed = + CalculateSpeed(remainingMillimeters); + var tangent = new Vector2( + (float)Math.Cos(tangentYaw), + (float)Math.Sin(tangentYaw)); + var leftNormal = new Vector2( + -tangent.Y, + tangent.X); + var positionError = + currentPosition - referencePoint; + var lateralErrorMeters = + Vector2.Dot( + positionError, + leftNormal) / 1000.0; + var normalCorrection = + Limit( + -LateralGainPerSecond * + lateralErrorMeters, + MaximumLateralCorrection); + + // 先在世界坐标中组合切向速度与横向纠偏速度。 + var worldVx = + tangent.X * speed + + leftNormal.X * (float)normalCorrection; + var worldVy = + tangent.Y * speed + + leftNormal.Y * (float)normalCorrection; + + // 将世界速度表达为当前蟹行运动坐标系速度。 + var motionYaw = + currentBodyYaw + + MotionFrameYawInBodyRadians; + var motionCos = Math.Cos(motionYaw); + var motionSin = Math.Sin(motionYaw); + var vxInMotion = + motionCos * worldVx + + motionSin * worldVy; + var vyInMotion = + -motionSin * worldVx + + motionCos * worldVy; + + var desiredBodyYaw = + tangentYaw - + MotionFrameYawInBodyRadians; + var headingError = + AngleMath.ShortestDifferenceRadians( + desiredBodyYaw, + currentBodyYaw); + var omega = + speed * referenceCurvature + + HeadingGainPerSecond * headingError; + omega = Limit( + omega, + MaximumAngularSpeedRadiansPerSecond); + + var now = DateTime.Now; + var interval = now - lastCommandTime; + lastCommandTime = now; + + bool commandAccepted; + Twist2D bodyTwist; + if (CommandBackend == + ChassisCommandBackend.SendMotion) + { + // 运动坐标系相对车体系旋转+90°： + // 运动系正向速度会转换成车体系+Y速度。 + bodyTwist = + FrameTransform2D + .TransformTwistAtSamePoint( + new Pose2D( + 0.0, + 0.0, + MotionFrameYawInBodyRadians), + new Twist2D( + vxInMotion, + vyInMotion, + omega)); + + // 将运动坐标系原点和前后几何控制点处的速度， + // 转换为SendMotion需要的前后轴方向。 + var controlPointRadiusMeters = + Math.Max( + chassis.ControlPointRadius / + 1000.0, + 0.001); + var frontVelocityY = + vyInMotion + + omega * + controlPointRadiusMeters; + var rearVelocityY = + vyInMotion - + omega * + controlPointRadiusMeters; + var frontSteeringRadians = + Math.Atan2( + frontVelocityY, + vxInMotion); + var rearSteeringRadians = + Math.Atan2( + rearVelocityY, + vxInMotion); + + // 蟹行测试绕过M层ManualControl并直接调用SendMotion， + // 因此需要在C层同步应用蟹行虚拟几何比例和转向符号。 + if (IsCrabMotionFrame()) + { + var geometryRatio = + adapter.HalfTrackWidthMeters / + adapter.HalfWheelBaseMeters; + + frontSteeringRadians = + ConvertToCrabSteering( + frontSteeringRadians, + geometryRatio); + rearSteeringRadians = + ConvertToCrabSteering( + rearSteeringRadians, + geometryRatio); + } + + var frontThetaDegrees = + (float)AngleMath.RadiansToDegrees( + frontSteeringRadians); + var rearThetaDegrees = + (float)AngleMath.RadiansToDegrees( + rearSteeringRadians); + var motionSpeed = + (float)Math.Sqrt( + vxInMotion * vxInMotion + + vyInMotion * vyInMotion); + + commandAccepted = + chassis.SendMotion( + motionSpeed, + frontThetaDegrees, + rearThetaDegrees, + interval); + } + else if (CommandBackend == + ChassisCommandBackend + .SendXYThSpeed) + { + // 安全XYTh后端根据舵角误差统一压低驱动轮速。 + bodyTwist = + FrameTransform2D + .TransformTwistAtSamePoint( + new Pose2D( + 0.0, + 0.0, + MotionFrameYawInBodyRadians), + new Twist2D( + vxInMotion, + vyInMotion, + omega)); + var command = new ChassisCommand( + PilotDefinition.Self.CarNum, + bodyTwist); + commandAccepted = + adapter.Send( + command, + interval); + } + else + { + throw new InvalidOperationException( + $"不支持的底盘命令后端：{CommandBackend}。"); + } + + if (!commandAccepted) + throw new InvalidOperationException( + "运动坐标系轨迹底盘解算失败：" + + chassis + .LastMotionDecomposeFailureReason); + + CommandObserver?.Invoke( + (float)bodyTwist.VxMetersPerSecond, + (float)bodyTwist.VyMetersPerSecond, + (float)bodyTwist + .OmegaRadiansPerSecond); + + yield return true; + } + } + finally + { + adapter.StopImmediately(); + if (CommandBackend == + ChassisCommandBackend.SendMotion) + { + // 测试退出后恢复真实车体坐标系，避免影响后续测试。 + adapter.ResetToBodyFrame(); + } + CommandObserver?.Invoke(0f, 0f, 0f); + } + + yield return false; + } + + // 判断当前运动坐标系是否为车体左侧朝前的蟹行坐标系。 + private bool IsCrabMotionFrame() + { + return Math.Abs( + AngleMath.ShortestDifferenceRadians( + Math.PI / 2.0, + MotionFrameYawInBodyRadians)) < + 1e-6; + } + + // 按车体几何比例缩小蟹行转角。 + // +90°运动坐标系已经完成方向映射，此处不能再次反号。 + private double ConvertToCrabSteering( + double normalSteeringRadians, + double geometryRatio) + { + var crabSteeringRadians = + Math.Atan( + geometryRatio * + Math.Tan( + normalSteeringRadians)); + + return Limit( + crabSteeringRadians, + MaximumVirtualSteeringRadians); + } + + // 计算当前点在直线或圆弧上的参考点、切线和剩余距离。 + private void CalculateReference( + Vector2 currentPosition, + out double tangentYaw, + out Vector2 referencePoint, + out float remainingMillimeters, + out double curvaturePerMeter) + { + var initialMotionYaw = + InitialBodyYawRadians + + MotionFrameYawInBodyRadians; + + if (PathKind == ReferencePathKind.Straight) + { + var tangent = new Vector2( + (float)Math.Cos(initialMotionYaw), + (float)Math.Sin(initialMotionYaw)); + var relative = currentPosition - StartPosition; + var progress = + Vector2.Dot(relative, tangent); + var clampedProgress = + Math.Max( + 0f, + Math.Min(progress, LengthMillimeters)); + + tangentYaw = initialMotionYaw; + referencePoint = + StartPosition + + tangent * clampedProgress; + remainingMillimeters = + Math.Max( + 0f, + LengthMillimeters - progress); + curvaturePerMeter = 0.0; + return; + } + + if (PathKind == ReferencePathKind.SCurve) + { + CalculateSCurveReference( + currentPosition, + initialMotionYaw, + out tangentYaw, + out referencePoint, + out remainingMillimeters, + out curvaturePerMeter); + return; + } + + var center = GetArcCenter(); + var startRadialYaw = + initialMotionYaw - Math.PI / 2.0; + var radial = currentPosition - center; + var currentRadialYaw = + Math.Atan2(radial.Y, radial.X); + var progressRadians = + AngleMath.NormalizeRadians( + currentRadialYaw - startRadialYaw); + + // 测试圆弧只有+90°，起点附近的轻微负噪声按0处理。 + if (progressRadians < 0.0) + progressRadians = 0.0; + + var clampedProgressRadians = + Math.Min( + progressRadians, + ArcSweepRadians); + var referenceRadialYaw = + startRadialYaw + + clampedProgressRadians; + referencePoint = center + new Vector2( + RadiusMillimeters * + (float)Math.Cos(referenceRadialYaw), + RadiusMillimeters * + (float)Math.Sin(referenceRadialYaw)); + tangentYaw = + referenceRadialYaw + Math.PI / 2.0; + remainingMillimeters = + (float)Math.Max( + 0.0, + (ArcSweepRadians - progressRadians) * + RadiusMillimeters); + curvaturePerMeter = + 1000.0 / RadiusMillimeters; + } + + // 通过离散最近点和解析导数计算两段三次贝塞尔S曲线的参考状态。 + private void CalculateSCurveReference( + Vector2 currentPosition, + double initialMotionYaw, + out double tangentYaw, + out Vector2 referencePoint, + out float remainingMillimeters, + out double curvaturePerMeter) + { + const int nearestPointSamples = 200; + var searchStart = + Math.Max( + 0.0, + _lastSCurveProgress - 0.02); + var bestProgress = _lastSCurveProgress; + var bestDistanceSquared = double.MaxValue; + + for (var i = 0; + i <= nearestPointSamples; + i++) + { + var progress = + searchStart + + (1.0 - searchStart) * + i / nearestPointSamples; + EvaluateSCurve( + progress, + out var localPoint, + out _, + out _); + var worldPoint = + LocalPathPointToWorld( + localPoint, + initialMotionYaw); + var distanceSquared = + Vector2.DistanceSquared( + currentPosition, + worldPoint); + + if (distanceSquared < + bestDistanceSquared) + { + bestDistanceSquared = + distanceSquared; + bestProgress = progress; + } + } + + // 轨迹进度不允许因定位噪声倒退，防止控制目标跳回上一段曲线。 + _lastSCurveProgress = + Math.Max( + _lastSCurveProgress, + bestProgress); + EvaluateSCurve( + _lastSCurveProgress, + out var bestLocalPoint, + out var firstDerivative, + out var secondDerivative); + referencePoint = + LocalPathPointToWorld( + bestLocalPoint, + initialMotionYaw); + tangentYaw = + initialMotionYaw + + Math.Atan2( + firstDerivative.Y, + firstDerivative.X); + + var derivativeMagnitude = + Math.Sqrt( + firstDerivative.X * + firstDerivative.X + + firstDerivative.Y * + firstDerivative.Y); + if (derivativeMagnitude < 1e-6) + { + curvaturePerMeter = 0.0; + } + else + { + // 导数单位为mm，乘1000后将曲率从1/mm转换成1/m。 + curvaturePerMeter = + (firstDerivative.X * + secondDerivative.Y - + firstDerivative.Y * + secondDerivative.X) * + 1000.0 / + Math.Pow( + derivativeMagnitude, + 3.0); + } + + remainingMillimeters = + ApproximateSCurveRemainingLength( + _lastSCurveProgress); + } + + // 计算与普通4m S型测试完全一致的三段三次贝塞尔完整S曲线。 + private void EvaluateSCurve( + double progress, + out Vector2 point, + out Vector2 firstDerivative, + out Vector2 secondDerivative) + { + progress = + Math.Max( + 0.0, + Math.Min(progress, 1.0)); + + Vector2 p0; + Vector2 p1; + Vector2 p2; + Vector2 p3; + double t; + + if (progress <= 0.25) + { + t = progress * 4.0; + p0 = new Vector2(0f, 0f); + p1 = new Vector2( + LengthMillimeters / 12f, + 0f); + p2 = new Vector2( + LengthMillimeters / 6f, + SCurveLateralOffsetMillimeters); + p3 = new Vector2( + LengthMillimeters * 0.25f, + SCurveLateralOffsetMillimeters); + } + else if (progress <= 0.75) + { + t = (progress - 0.25) * 2.0; + p0 = new Vector2( + LengthMillimeters * 0.25f, + SCurveLateralOffsetMillimeters); + p1 = new Vector2( + LengthMillimeters / 3f, + SCurveLateralOffsetMillimeters); + p2 = new Vector2( + LengthMillimeters * 2f / 3f, + -SCurveLateralOffsetMillimeters); + p3 = new Vector2( + LengthMillimeters * 0.75f, + -SCurveLateralOffsetMillimeters); + } + else + { + t = (progress - 0.75) * 4.0; + p0 = new Vector2( + LengthMillimeters * 0.75f, + -SCurveLateralOffsetMillimeters); + p1 = new Vector2( + LengthMillimeters * 5f / 6f, + -SCurveLateralOffsetMillimeters); + p2 = new Vector2( + LengthMillimeters * 11f / 12f, + 0f); + p3 = new Vector2( + LengthMillimeters, + 0f); + } + + var oneMinusT = 1.0 - t; + point = + p0 * (float)( + oneMinusT * + oneMinusT * + oneMinusT) + + p1 * (float)( + 3.0 * + oneMinusT * + oneMinusT * + t) + + p2 * (float)( + 3.0 * + oneMinusT * + t * + t) + + p3 * (float)(t * t * t); + firstDerivative = + (p1 - p0) * + (float)( + 3.0 * + oneMinusT * + oneMinusT) + + (p2 - p1) * + (float)( + 6.0 * + oneMinusT * + t) + + (p3 - p2) * + (float)(3.0 * t * t); + secondDerivative = + (p2 - 2f * p1 + p0) * + (float)(6.0 * oneMinusT) + + (p3 - 2f * p2 + p1) * + (float)(6.0 * t); + } + + // 通过分段采样估算从当前S曲线进度到终点的实际弧长。 + private float ApproximateSCurveRemainingLength( + double startProgress) + { + const int lengthSamples = 100; + EvaluateSCurve( + startProgress, + out var previousPoint, + out _, + out _); + var length = 0f; + + for (var i = 1; + i <= lengthSamples; + i++) + { + var progress = + startProgress + + (1.0 - startProgress) * + i / lengthSamples; + EvaluateSCurve( + progress, + out var point, + out _, + out _); + length += + Vector2.Distance( + previousPoint, + point); + previousPoint = point; + } + + return length; + } + + // 将以初始蟹行方向为X轴的局部路径点转换到Detour世界坐标。 + private Vector2 LocalPathPointToWorld( + Vector2 localPoint, + double initialMotionYaw) + { + var cos = + (float)Math.Cos(initialMotionYaw); + var sin = + (float)Math.Sin(initialMotionYaw); + + return StartPosition + new Vector2( + localPoint.X * cos - + localPoint.Y * sin, + localPoint.X * sin + + localPoint.Y * cos); + } + + // 获取蟹行左转圆弧圆心；它位于初始运动方向的左侧。 + public Vector2 GetArcCenter() + { + var initialMotionYaw = + InitialBodyYawRadians + + MotionFrameYawInBodyRadians; + return StartPosition + new Vector2( + -RadiusMillimeters * + (float)Math.Sin(initialMotionYaw), + RadiusMillimeters * + (float)Math.Cos(initialMotionYaw)); + } + + // 获取圆弧测试的理论终点。 + public Vector2 GetArcDestination() + { + var initialMotionYaw = + InitialBodyYawRadians + + MotionFrameYawInBodyRadians; + var startRadialYaw = + initialMotionYaw - Math.PI / 2.0; + var endRadialYaw = + startRadialYaw + ArcSweepRadians; + var center = GetArcCenter(); + + return center + new Vector2( + RadiusMillimeters * + (float)Math.Cos(endRadialYaw), + RadiusMillimeters * + (float)Math.Sin(endRadialYaw)); + } + + // 根据剩余路径长度生成终点减速速度。 + private float CalculateSpeed( + float remainingMillimeters) + { + if (remainingMillimeters >= + SlowDistanceMillimeters) + return CruiseSpeed; + + var ratio = + remainingMillimeters / + Math.Max( + SlowDistanceMillimeters, + 1f); + return Math.Max( + MinimumSpeed, + CruiseSpeed * ratio); + } + + private void ValidateParameters() + { + if (CruiseSpeed <= 0f || + !IsFinite(CruiseSpeed) || + LengthMillimeters <= 0f || + !IsFinite(LengthMillimeters) || + RadiusMillimeters <= 0f || + !IsFinite(RadiusMillimeters) || + SCurveLateralOffsetMillimeters <= 0f || + !IsFinite( + SCurveLateralOffsetMillimeters) || + ArcSweepRadians <= 0.0 || + !IsFinite(ArcSweepRadians) || + SlowDistanceMillimeters <= 0f || + !IsFinite(SlowDistanceMillimeters) || + FinishDistanceMillimeters < 0f || + !IsFinite(FinishDistanceMillimeters) || + TrackingTimeoutSeconds <= 0f || + !IsFinite(TrackingTimeoutSeconds) || + MaximumVirtualSteeringRadians <= 0.0 || + MaximumVirtualSteeringRadians >= + Math.PI / 2.0 || + !IsFinite( + MaximumVirtualSteeringRadians)) + throw new ArgumentOutOfRangeException( + "蟹行轨迹测试参数无效。"); + } + + private static double Limit( + double value, + double absoluteLimit) + { + return Math.Max( + -absoluteLimit, + Math.Min(value, absoluteLimit)); + } + + private static bool IsFinite(double value) + { + return + !double.IsNaN(value) && + !double.IsInfinity(value); + } + } +} diff --git a/MultiWheelC/MovementTests.cs b/MultiWheelC/MovementTests.cs new file mode 100644 index 0000000..2d61af6 --- /dev/null +++ b/MultiWheelC/MovementTests.cs @@ -0,0 +1,949 @@ +using ClumsyCore; +using ClumsyCore.DTools; +using ClumsyCore.Interfaces; +using ClumsyCore.Pilot; +using CommonUsage.Chassis; +using MDCSToolBox.Commons.Controllers; +using MDCSToolBox.Clumsy.Tracks; +using MyParking.Shared; +using System; +using System.Collections.Generic; +using System.Numerics; +using System.Threading; + +namespace MultiWheelC +{ + internal static class MovementTestPreparation + { + // 在测试正式开始前，将四个舵轮稳定回正到车体前向。 + public static bool AlignWheelsForward( + ref DriveTask activeTask) + { + var preparation = new PrepareWheelsForward(); + var task = new DriveTask(preparation.Get()); + activeTask = task; + + try + { + task.Wait(); + return preparation.Completed; + } + catch (Exception ex) + { + Console.WriteLine( + $"测试前舵轮回正失败：{ex.Message}"); + return false; + } + finally + { + task.Stop(); + if (ReferenceEquals(activeTask, task)) + activeTask = null; + } + } + + // 只读取实际舵角，检查四个舵轮是否已与车头方向一致。 + public static bool AreWheelsForward( + float toleranceDegrees = 2f) + { + var chassis = + PilotDefinition.Chassis as MultiWheelChassis; + if (chassis == null) + { + Console.WriteLine( + "当前底盘不是MultiWheelChassis，无法检查舵轮方向。"); + return false; + } + + try + { + var adapter = new MultiWheelChassisAdapter( + chassis, + PilotDefinition.Self.CarNum); + var toleranceRadians = + AngleMath.DegreesToRadians(toleranceDegrees); + + if (adapter.AreParallelWheelsAligned( + 0.0, + toleranceRadians)) + { + return true; + } + + Console.WriteLine( + "四个舵轮尚未与车头方向一致，请先执行“准备：四个舵轮与车头方向一致”。"); + return false; + } + catch (Exception ex) + { + Console.WriteLine( + $"检查舵轮方向失败：{ex.Message}"); + return false; + } + } + } + + [MovementTest(name = "准备：四个舵轮与车头方向一致")] + public class AlignWheelsForwardTest : MovementTest + { + private DriveTask _task; + + // 单独将四个舵轮转到车体前向0°并等待实际反馈稳定到位。 + public override void Test() + { + MovementTestPreparation.AlignWheelsForward( + ref _task); + } + + // 停止正在执行的舵轮回正任务并清零底盘运动命令。 + public override void TestStop() + { + _task?.Stop(); + _task = null; + } + } + + [MovementTest(name = "SendMotion：连续前进4m")] + public class TestForward4m : MovementTest + { + public float DistanceMillimeters = 4000f; // 测试距离，单位mm。 + public float CruiseSpeed = 0.3f; // 巡航速度上限，单位m/s。 + public int TrialNumber = 1; // 重复实验编号。 + private DriveTask _task; + private TrackingExperimentRecorder _recorder; + // 从当前Detour位置沿车头方向生成4m连续直线并记录测试数据。 + public override void Test() + { + if (!MovementTestPreparation.AreWheelsForward()) + { + return; + } + + var location = DetourInterface.getCartLocation(); + if (double.IsNaN(location.x) || + double.IsInfinity(location.x) || + double.IsNaN(location.y) || + double.IsInfinity(location.y) || + double.IsNaN(location.th) || + double.IsInfinity(location.th)) + { + Console.WriteLine( + "Detour当前位姿无效，取消连续前进4m测试。"); + return; + } + var source = new Vector2((float)location.x, (float)location.y); + // Detour航向单位是度，三角函数需要弧度。 + var headingRadians = + AngleMath.DegreesToRadians(location.th); + var destination = new Vector2( + source.X + DistanceMillimeters * (float)Math.Cos(headingRadians), + source.Y + DistanceMillimeters * (float)Math.Sin(headingRadians)); + _recorder = + new TrackingExperimentRecorder( + controllerName: "LegacyGeometricController", + trajectoryName: "LegacyStraight4m", + trialNumber: TrialNumber, + referenceStart: source, + referenceEnd: destination, + referenceSpeed: CruiseSpeed); + _recorder.Start(); + try + { + _task = new DriveTask( + new DstTracker + { + Src = source, + Dst = destination, + CarDirectionBias = 0f, + MaxSpeed = CruiseSpeed + }.Get()); + _task.Wait(); + // 保留少量停车后数据，便于观察速度是否回到零。 + Thread.Sleep(300); + } + finally + { + _task?.Stop(); + _recorder?.UpdateCommand(0f, 0f); + _recorder?.StopAndSave(); + _task = null; + _recorder = null; + } + } + + public override void TestStop() + { + _task?.Stop(); + _recorder?.UpdateCommand(0f, 0f); + _recorder?.StopAndSave(); + } + } + + public abstract class InPlaceRotateTestBase : MovementTest + { + public float RelativeAngleDegrees; // 相对当前航向的旋转角度，逆时针为正。 + public float MaxAngularSpeedDegreesPerSecond = 20f; // PID输出的最大角速度。 + public int TrialNumber = 1; // 重复实验编号。 + + private DriveTask _task; + private TrackingExperimentRecorder _recorder; + private readonly string _trajectoryName; + + protected InPlaceRotateTestBase( + float relativeAngleDegrees, + string trajectoryName) + { + RelativeAngleDegrees = + relativeAngleDegrees; + _trajectoryName = + trajectoryName; + } + + // 从当前Detour航向开始，原地相对旋转指定角度并记录实验数据。 + public override void Test() + { + if (float.IsNaN(RelativeAngleDegrees) || + float.IsInfinity(RelativeAngleDegrees) || + float.IsNaN(MaxAngularSpeedDegreesPerSecond) || + float.IsInfinity(MaxAngularSpeedDegreesPerSecond) || + MaxAngularSpeedDegreesPerSecond <= 0f) + { + Console.WriteLine("原地旋转测试参数无效。"); + return; + } + + var location = DetourInterface.getCartLocation(); + if (double.IsNaN(location.x) || + double.IsInfinity(location.x) || + double.IsNaN(location.y) || + double.IsInfinity(location.y) || + double.IsNaN(location.th) || + double.IsInfinity(location.th)) + { + Console.WriteLine( + "Detour当前位姿无效，取消原地旋转测试。"); + return; + } + + var rotationCenter = + new Vector2((float)location.x, (float)location.y); + var targetWorldAngle = + (float)AngleMath.NormalizeDegrees( + location.th + RelativeAngleDegrees); + + _recorder = new TrackingExperimentRecorder( + controllerName: "InPlaceRotatePID", + trajectoryName: _trajectoryName, + trialNumber: TrialNumber, + referenceStart: rotationCenter, + referenceEnd: rotationCenter, + referenceSpeed: 0f, + referenceAngularSpeed: + (float)AngleMath.DegreesToRadians( + MaxAngularSpeedDegreesPerSecond)); + _recorder.Start(); + + try + { + _task = new DriveTask( + new MultiWheelRotateInPlace + { + // MultiWheelRotateInPlace接收世界坐标系绝对航向。 + AngleTarget = targetWorldAngle, + PidparamsRead = () => new PIDParams + { + Kp = + PilotDefinition.Conf.InPlaceRotateKp, + Ki = + PilotDefinition.Conf.InPlaceRotateKi, + Kd = + PilotDefinition.Conf.InPlaceRotateKd, + DeadZone = + PilotDefinition.Conf + .InPlaceRotateArriveDeg, + SpeedAccPerSec = + PilotDefinition.Conf.InPlaceRotateAcc, + OutputUpperThreshold = + MaxAngularSpeedDegreesPerSecond, + MaxI = + PilotDefinition.Conf.InPlaceRotateMaxI + }, + CommandAngularSpeedObserver = + commandAngularSpeed => + _recorder?.UpdateCommand( + 0f, + (float)AngleMath.DegreesToRadians( + commandAngularSpeed)) + }.Get()); + + _task.Wait(); + + // 保留少量停止后的样本，用于观察角速度是否回到零。 + Thread.Sleep(300); + } + finally + { + _task?.Stop(); + _recorder?.UpdateCommand(0f, 0f); + _recorder?.StopAndSave(); + _task = null; + _recorder = null; + } + } + + // 停止原地旋转并保存当前已经采集的实验数据。 + public override void TestStop() + { + _task?.Stop(); + _recorder?.UpdateCommand(0f, 0f); + _recorder?.StopAndSave(); + } + + } + + [MovementTest(name = "SendXYThSpeed：原地自转90°")] + public sealed class TestRotate90 : + InPlaceRotateTestBase + { + public TestRotate90() + : base(90f, "Rotate90") + { + } + } + + [MovementTest(name = "SendXYThSpeed：原地自转180°")] + public sealed class TestRotate180 : + InPlaceRotateTestBase + { + public TestRotate180() + : base(180f, "Rotate180") + { + } + } + + [MovementTest(name = "SendMotion：左转90°半径2m圆弧")] + public class TestArcMovement : MovementTest + { + public float RadiusMillimeters = 2000f; // 左转圆的半径，单位mm。 + public float CruiseSpeed = 0.3f; // 圆周运动速度上限，单位m/s。 + public int TrialNumber = 1; // 重复实验编号。 + + private DriveTask _task; + private TrackingExperimentRecorder _recorder; + + // 从当前位姿开始，沿半径2m的圆弧向左转弯90°。 + public override void Test() + { + if (float.IsNaN(RadiusMillimeters) || + float.IsInfinity(RadiusMillimeters) || + RadiusMillimeters <= 0f || + float.IsNaN(CruiseSpeed) || + float.IsInfinity(CruiseSpeed) || + CruiseSpeed <= 0f) + { + Console.WriteLine("圆弧运动测试参数无效。"); + return; + } + + if (!MovementTestPreparation.AreWheelsForward()) + { + return; + } + + var location = DetourInterface.getCartLocation(); + if (double.IsNaN(location.x) || + double.IsInfinity(location.x) || + double.IsNaN(location.y) || + double.IsInfinity(location.y) || + double.IsNaN(location.th) || + double.IsInfinity(location.th)) + { + Console.WriteLine( + "Detour当前位姿无效，取消圆弧运动测试。"); + return; + } + + var source = + new Vector2((float)location.x, (float)location.y); + var headingRadians = + AngleMath.DegreesToRadians(location.th); + + // 根据世界航向求车体左法向，左转圆心位于车辆左侧。 + var center = new Vector2( + source.X - + RadiusMillimeters * + (float)Math.Sin(headingRadians), + source.Y + + RadiusMillimeters * + (float)Math.Cos(headingRadians)); + + // 从圆心指向车辆起点的极角，比车辆切线航向小90°。 + var startRadialAngleDegrees = + (float)location.th - 90f; + + var controller = new ChassisController + { + BaseSpeed = CruiseSpeed + }.Get(); + controller.FinishSpeed = 0f; + + var arc = new CircularArcTrack( + center, + RadiusMillimeters, + startRadialAngleDegrees, + startRadialAngleDegrees + 90f, + direction: 1) + { + Speed = CruiseSpeed, + CarDirectionBias = 0f + }; + + // 左转90°后，圆心到终点的径向方向等于起始车头方向。 + var destination = center + new Vector2( + RadiusMillimeters * + (float)Math.Cos(headingRadians), + RadiusMillimeters * + (float)Math.Sin(headingRadians)); + + if (!controller.AddTrack(arc, "LeftArc90Degrees")) + { + Console.WriteLine( + "左转90°圆弧轨迹添加失败，取消测试。"); + return; + } + + _recorder = new TrackingExperimentRecorder( + controllerName: "LegacyGeometricController", + trajectoryName: + $"LegacyLeftArc90_R{RadiusMillimeters:0}mm", + trialNumber: TrialNumber, + referenceStart: source, + referenceEnd: destination, + referenceSpeed: CruiseSpeed); + _recorder.Start(); + + try + { + _task = new DriveTask(controller.Track()); + _task.Wait(); + + // 保留少量停车后的样本，用于观察速度是否回到零。 + Thread.Sleep(300); + } + finally + { + _task?.Stop(); + _recorder?.UpdateCommand(0f, 0f); + _recorder?.StopAndSave(); + _task = null; + _recorder = null; + } + } + + // 停止圆弧运动并保存当前已经采集的实验数据。 + public override void TestStop() + { + _task?.Stop(); + _recorder?.UpdateCommand(0f, 0f); + _recorder?.StopAndSave(); + } + } + + #region 蟹行运动测试 + [MovementTest(name = "SendMotion：蟹行直线4m")] + public class TestCrabForward4m : MovementTest + { + public float DistanceMillimeters = 4000f; + public float CruiseSpeed = 0.2f; + public int TrialNumber = 1; + + private DriveTask _task; + private TrackingExperimentRecorder _recorder; + + // 将车体左侧作为运动前向，沿直线蟹行4m并记录Detour实验数据。 + public override void Test() + { + if (!TryReadStartPose( + out var source, + out var bodyYawRadians)) + return; + + var motionYaw = + bodyYawRadians + Math.PI / 2.0; + var destination = new Vector2( + source.X + + DistanceMillimeters * + (float)Math.Cos(motionYaw), + source.Y + + DistanceMillimeters * + (float)Math.Sin(motionYaw)); + + var tracker = new CrabMotionFrameTracker + { + CommandBackend = + CrabMotionFrameTracker + .ChassisCommandBackend + .SendMotion, + PathKind = + CrabMotionFrameTracker + .ReferencePathKind.Straight, + StartPosition = source, + InitialBodyYawRadians = + bodyYawRadians, + LengthMillimeters = + DistanceMillimeters, + CruiseSpeed = CruiseSpeed + }; + + _recorder = new TrackingExperimentRecorder( + controllerName: + "CrabSendMotionTracker", + trajectoryName: + "CrabStraight4m", + trialNumber: TrialNumber, + referenceStart: source, + referenceEnd: destination, + referenceSpeed: CruiseSpeed, + referenceMotionFrameYawDegrees: 90f); + tracker.CommandObserver = + (vx, vy, omega) => + _recorder?.UpdateBodyCommand( + vx, + vy, + omega); + _recorder.Start(); + + try + { + _task = new DriveTask(tracker.Get()); + _task.Wait(); + Thread.Sleep(300); + } + finally + { + _task?.Stop(); + _recorder?.UpdateBodyCommand( + 0f, + 0f, + 0f); + _recorder?.StopAndSave(); + _task = null; + _recorder = null; + } + } + + public override void TestStop() + { + _task?.Stop(); + _recorder?.UpdateBodyCommand( + 0f, + 0f, + 0f); + _recorder?.StopAndSave(); + } + + // 读取并校验测试开始时的Detour世界位姿。 + private static bool TryReadStartPose( + out Vector2 source, + out double bodyYawRadians) + { + var location = + DetourInterface.getCartLocation(); + if (double.IsNaN(location.x) || + double.IsInfinity(location.x) || + double.IsNaN(location.y) || + double.IsInfinity(location.y) || + double.IsNaN(location.th) || + double.IsInfinity(location.th)) + { + Console.WriteLine( + "Detour当前位姿无效，取消蟹行直线测试。"); + source = Vector2.Zero; + bodyYawRadians = 0.0; + return false; + } + + source = new Vector2( + (float)location.x, + (float)location.y); + bodyYawRadians = + AngleMath.DegreesToRadians(location.th); + return true; + } + } + + [MovementTest(name = "SendMotion：蟹行左转90°半径2m圆弧")] + public class TestCrabLeftArc90 : MovementTest + { + public float RadiusMillimeters = 2000f; + public float CruiseSpeed = 0.2f; + public int TrialNumber = 1; + + private DriveTask _task; + private TrackingExperimentRecorder _recorder; + + // 将车体左侧作为运动前向，沿半径2m的左转圆弧运动90°。 + public override void Test() + { + var location = + DetourInterface.getCartLocation(); + if (double.IsNaN(location.x) || + double.IsInfinity(location.x) || + double.IsNaN(location.y) || + double.IsInfinity(location.y) || + double.IsNaN(location.th) || + double.IsInfinity(location.th)) + { + Console.WriteLine( + "Detour当前位姿无效，取消蟹行圆弧测试。"); + return; + } + + var source = new Vector2( + (float)location.x, + (float)location.y); + var bodyYawRadians = + AngleMath.DegreesToRadians(location.th); + var tracker = new CrabMotionFrameTracker + { + CommandBackend = + CrabMotionFrameTracker + .ChassisCommandBackend + .SendMotion, + PathKind = + CrabMotionFrameTracker + .ReferencePathKind.LeftArc, + StartPosition = source, + InitialBodyYawRadians = + bodyYawRadians, + RadiusMillimeters = + RadiusMillimeters, + ArcSweepRadians = Math.PI / 2.0, + CruiseSpeed = CruiseSpeed + }; + var destination = + tracker.GetArcDestination(); + + _recorder = new TrackingExperimentRecorder( + controllerName: + "CrabSendMotionTracker", + trajectoryName: + $"CrabLeftArc90_R{RadiusMillimeters:0}mm", + trialNumber: TrialNumber, + referenceStart: source, + referenceEnd: destination, + referenceSpeed: CruiseSpeed, + referenceMotionFrameYawDegrees: 90f); + tracker.CommandObserver = + (vx, vy, omega) => + _recorder?.UpdateBodyCommand( + vx, + vy, + omega); + _recorder.Start(); + + try + { + _task = new DriveTask(tracker.Get()); + _task.Wait(); + Thread.Sleep(300); + } + finally + { + _task?.Stop(); + _recorder?.UpdateBodyCommand( + 0f, + 0f, + 0f); + _recorder?.StopAndSave(); + _task = null; + _recorder = null; + } + } + + public override void TestStop() + { + _task?.Stop(); + _recorder?.UpdateBodyCommand( + 0f, + 0f, + 0f); + _recorder?.StopAndSave(); + } + } + + [MovementTest(name = "SendMotion：4m S型曲线")] + public class TestSCurve4m : MovementTest + { + public float LengthMillimeters = 4000f; // S型曲线纵向长度，单位mm。 + public float LateralOffsetMillimeters = 400f; // S型曲线左右两侧的最大偏移，单位mm。 + public float CruiseSpeed = 0.3f; // 首次实车测试建议使用0.3m/s。 + public int TrialNumber = 1; // 重复实验编号。 + + private DriveTask _task; + private TrackingExperimentRecorder _recorder; + + // 从当前Detour位姿开始，沿车头方向跟踪先左偏、再右偏并最终回中的完整S型曲线。 + public override void Test() + { + if (float.IsNaN(LengthMillimeters) || + float.IsInfinity(LengthMillimeters) || + LengthMillimeters <= 0f || + float.IsNaN(LateralOffsetMillimeters) || + float.IsInfinity(LateralOffsetMillimeters) || + LateralOffsetMillimeters <= 0f || + float.IsNaN(CruiseSpeed) || + float.IsInfinity(CruiseSpeed) || + CruiseSpeed <= 0f) + { + Console.WriteLine("S型曲线测试参数无效。"); + return; + } + + if (!MovementTestPreparation.AreWheelsForward()) + return; + + var location = DetourInterface.getCartLocation(); + if (double.IsNaN(location.x) || + double.IsInfinity(location.x) || + double.IsNaN(location.y) || + double.IsInfinity(location.y) || + double.IsNaN(location.th) || + double.IsInfinity(location.th)) + { + Console.WriteLine( + "Detour当前位姿无效，取消4m S型曲线测试。"); + return; + } + + var source = + new Vector2((float)location.x, (float)location.y); + var headingRadians = + AngleMath.DegreesToRadians(location.th); + var length = LengthMillimeters; + var offset = LateralOffsetMillimeters; + + // 三段三次贝塞尔依次经过左侧峰值、中心线和右侧峰值， + // 起点、两个峰值和终点的切线均沿初始前向，连接处没有折角。 + var firstControlPoints = new List + { + LocalToWorld(source, headingRadians, 0f, 0f), + LocalToWorld( + source, headingRadians, + length / 12f, 0f), + LocalToWorld( + source, headingRadians, + length / 6f, offset), + LocalToWorld( + source, headingRadians, + length * 0.25f, offset) + }; + var secondControlPoints = new List + { + LocalToWorld( + source, headingRadians, + length * 0.25f, offset), + LocalToWorld( + source, headingRadians, + length / 3f, offset), + LocalToWorld( + source, headingRadians, + length * 2f / 3f, -offset), + LocalToWorld( + source, headingRadians, + length * 0.75f, -offset) + }; + var thirdControlPoints = new List + { + LocalToWorld( + source, headingRadians, + length * 0.75f, -offset), + LocalToWorld( + source, headingRadians, + length * 5f / 6f, -offset), + LocalToWorld( + source, headingRadians, + length * 11f / 12f, 0f), + LocalToWorld( + source, headingRadians, + length, 0f) + }; + + var firstTrack = new BezierTrack(firstControlPoints) + { + Speed = CruiseSpeed, + CarDirectionBias = 0f + }; + var secondTrack = new BezierTrack(secondControlPoints) + { + Speed = CruiseSpeed, + CarDirectionBias = 0f + }; + var thirdTrack = new BezierTrack(thirdControlPoints) + { + Speed = CruiseSpeed, + CarDirectionBias = 0f + }; + + var controller = new ChassisController + { + BaseSpeed = CruiseSpeed + }.Get(); + controller.FinishSpeed = 0f; + + if (!controller.AddTrack( + firstTrack, + "SCurve4m-Part1") || + !controller.AddTrack( + secondTrack, + "SCurve4m-Part2") || + !controller.AddTrack( + thirdTrack, + "SCurve4m-Part3")) + { + Console.WriteLine( + "4m S型曲线轨迹添加失败，取消测试。"); + return; + } + + var destination = + LocalToWorld( + source, + headingRadians, + length, + 0f); + _recorder = new TrackingExperimentRecorder( + controllerName: "LegacyGeometricController", + trajectoryName: + $"LegacySCurve4m_A{LateralOffsetMillimeters:0}mm", + trialNumber: TrialNumber, + referenceStart: source, + referenceEnd: destination, + referenceSpeed: CruiseSpeed); + _recorder.Start(); + + try + { + _task = new DriveTask(controller.Track()); + _task.Wait(); + + // 保留少量停止后的数据，用于观察速度是否回到零。 + Thread.Sleep(300); + } + finally + { + _task?.Stop(); + _recorder?.UpdateCommand(0f, 0f); + _recorder?.StopAndSave(); + _task = null; + _recorder = null; + } + } + + // 停止S型曲线测试并保存当前已经采集的数据。 + public override void TestStop() + { + _task?.Stop(); + _recorder?.UpdateCommand(0f, 0f); + _recorder?.StopAndSave(); + } + + // 将车体起点局部坐标转换为Detour世界坐标，X向前、Y向左。 + private static Vector2 LocalToWorld( + Vector2 origin, + double headingRadians, + float localX, + float localY) + { + var cos = (float)Math.Cos(headingRadians); + var sin = (float)Math.Sin(headingRadians); + + return new Vector2( + origin.X + localX * cos - localY * sin, + origin.Y + localX * sin + localY * cos); + } + } + + #endregion + + #region 夹臂运动测试 + public abstract class ClampMovementTestBase : MovementTest + { + public float TimeoutSeconds = 30f; // 动作超时时间，单位s。 + + private DriveTask _task; + protected abstract bool Close { get; } + + // 根据派生测试类型驱动左右夹臂同步夹紧或打开。 + public override void Test() + { + var leftTarget = Close + ? PilotDefinition.Self.LeftArmUpperPos + : PilotDefinition.Self.LeftArmLowerPos; + var rightTarget = Close + ? PilotDefinition.Self.RightArmUpperPos + : PilotDefinition.Self.RightArmLowerPos; + + if (float.IsNaN(leftTarget) || + float.IsInfinity(leftTarget) || + float.IsNaN(rightTarget) || + float.IsInfinity(rightTarget)) + { + Console.WriteLine( + "夹臂目标位置无效，取消夹臂运动测试。"); + return; + } + + // 防止重复点击时上一项夹臂任务仍在运行。 + TestStop(); + Console.WriteLine( + $"开始夹臂{(Close ? "夹紧" : "打开")}测试：" + + $"左目标={leftTarget}，右目标={rightTarget}"); + + var task = new DriveTask( + new ClampToTarget + { + LeftClampTarget = leftTarget, + RightClampTarget = rightTarget, + TimeoutSeconds = TimeoutSeconds + }.Get()); + _task = task; + + try + { + task.Wait(); + } + finally + { + PilotDefinition.Self.SpeedLeftArm = 0f; + PilotDefinition.Self.SpeedRightArm = 0f; + if (ReferenceEquals(_task, task)) + _task = null; + } + } + + // 停止夹臂任务并立即清零左右夹臂下发速度。 + public override void TestStop() + { + _task?.Stop(); + _task = null; + PilotDefinition.Self.SpeedLeftArm = 0f; + PilotDefinition.Self.SpeedRightArm = 0f; + } + } + + [MovementTest(name = "夹臂关闭测试")] + public sealed class TestClampCloseMovement + : ClampMovementTestBase + { + protected override bool Close => false; + } + + [MovementTest(name = "夹臂启动测试")] + public sealed class TestClampOpenMovement + : ClampMovementTestBase + { + protected override bool Close => true; + } + #endregion +} diff --git a/MultiWheelC/Movements.cs b/MultiWheelC/Movements.cs new file mode 100644 index 0000000..09d756d --- /dev/null +++ b/MultiWheelC/Movements.cs @@ -0,0 +1,629 @@ +using ClumsyCore; +using ClumsyCore.DTools; +using ClumsyCore.Interfaces; +using ClumsyCore.Pilot; +using CommonUsage.Chassis; +using MDCSToolBox.Clumsy.Movements; +using MDCSToolBox.Clumsy.Tracks; +using MDCSToolBox.Commons.Controllers; +using System; +using System.Collections.Generic; +using System.Drawing; +using System.Numerics; +using System.Threading; +using FundamentalLib; +using MyParking.Shared; + +namespace MultiWheelC +{ + // C层测试准备：停车并等待四个舵轮稳定回到车体前向0°。 + public class PrepareWheelsForward : MovementDefinition + { + public float ToleranceDegrees = 2f; + public float StableSeconds = 0.3f; + public float TimeoutSeconds = 10f; + public bool Completed { get; private set; } + + public override IEnumerable Get() + { + var chassis = + PilotDefinition.Chassis as MultiWheelChassis; + if (chassis == null) + { + throw new InvalidOperationException( + "当前底盘不是MultiWheelChassis，无法执行舵轮回正。"); + } + + var adapter = new MultiWheelChassisAdapter( + chassis, + PilotDefinition.Self.CarNum); + adapter.ResetToBodyFrame(); + var toleranceRadians = + AngleMath.DegreesToRadians(ToleranceDegrees); + 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 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 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 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 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.PredefinedDriveStop(); + } + } + + } + //直线行走基于轮里程 + // 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 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 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 LeaveSrcFunction = null; + public Painter painter = UI.GetPainter("Line", false); + // C层单车轨迹：执行早期版本的两点直线跟踪动作。 + public override IEnumerable Get() + { + var curpose = DetourInterface.getCartLocation(); + Console.WriteLine($"curpose.th:{curpose.th}"); + var src = new Vector2((float)curpose.x, (float)curpose.y); + var headingRadians = + AngleMath.DegreesToRadians(curpose.th); + 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 + { + /// + /// 旋转目标角度 + /// + public float AngleTarget; + + public Func ThetaReader = () => (float)DetourInterface.getCartLocation().th; + + public MultiWheelChassis Chassis = (MultiWheelChassis)PilotDefinition.Chassis; + + public Func PidparamsRead = () => new PIDParams() { }; + + public PIDController thPid; + + // 将本周期PID角速度输出提供给实验记录器，单位deg/s。 + public Action CommandAngularSpeedObserver; + + // 自转前舵轮实际角度允许误差，单位deg。 + public float WheelAlignmentToleranceDegrees = 2f; + + // 自转舵轮连续保持到位的时间，单位s。 + public float WheelAlignmentStableSeconds = 0.3f; + + // 自转舵轮准备超时时间，单位s。 + public float WheelAlignmentTimeoutSeconds = 10f; + + // 先准备自转舵角，再通过安全版SendXYThSpeed闭环旋转到目标角度。 + public override IEnumerable Get() + { + if (Chassis == null) + throw new InvalidOperationException( + "当前底盘不是MultiWheelChassis，无法执行原地自转。"); + + var adapter = new MultiWheelChassisAdapter( + Chassis, + PilotDefinition.Self.CarNum); + adapter.ResetToBodyFrame(); + + try + { + var alignmentStarted = DateTime.Now; + DateTime? alignedSince = null; + while (true) + { + if (!adapter.PrepareSpin()) + throw new InvalidOperationException( + "无法生成原地自转舵轮目标：" + + adapter.LastFailureReason); + + if (adapter.AreSpinWheelsAligned) + { + if (alignedSince == null) + alignedSince = DateTime.Now; + + if ((DateTime.Now - alignedSince.Value) + .TotalSeconds >= + WheelAlignmentStableSeconds) + break; + } + else + { + alignedSince = null; + } + + if ((DateTime.Now - alignmentStarted) + .TotalSeconds > + WheelAlignmentTimeoutSeconds) + throw new TimeoutException( + "原地自转舵轮在限定时间内未稳定到位。"); + + yield return true; + } + + var targetAngle = + (float)AngleMath.NormalizeDegrees(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); + var lastCommandTime = DateTime.Now; + + while (true) + { + var s = thPid.GetResponse(targetAngle, true); + Console.WriteLine($"s:{s} AngleTarget:{AngleTarget}"); + CommandAngularSpeedObserver?.Invoke(s); + var now = DateTime.Now; + var interval = now - lastCommandTime; + lastCommandTime = now; + + // PID输出s为deg/s，Shared命令统一使用rad/s。 + // adapter.Send最终调用普通安全版SendXYThSpeed。 + var omegaRadiansPerSecond = + (float)AngleMath.DegreesToRadians(s); + if (!adapter.Send( + new ChassisCommand( + PilotDefinition.Self.CarNum, + new Twist2D( + 0.0, + 0.0, + omegaRadiansPerSecond)), + interval)) + { + throw new InvalidOperationException( + "安全XYTh原地旋转底盘解算失败：" + + adapter.LastFailureReason); + } + if (thPid.IsArrived()) break; + yield return true; + } + + Console.WriteLine($"final rotate to {targetAngle}"); + } + finally + { + CommandAngularSpeedObserver?.Invoke(0f); + adapter.StopImmediately(); + } + } + } + #endregion + + #region 夹臂运动 + public class ClampToTarget : MovementDefinition + { + public float LeftClampTarget; + public float RightClampTarget; + public float MaxClampSpeed = PilotDefinition.Conf.MaxClampSpeed; + public float ClampKp = PilotDefinition.Conf.ClampControlKp; + public float ClampKi = PilotDefinition.Conf.ClampControlKi; + public float ClampKd = PilotDefinition.Conf.ClampControlKd; + public float ClampMaxI = PilotDefinition.Conf.ClampControlMaxI; + public float ClampSpeedAcc = PilotDefinition.Conf.ClampControlSpeedAcc; + public float ClampDeadZone = PilotDefinition.Conf.ClampControlDeadZone; + public float TimeoutSeconds = 30f; + private PIDController leftpid, rightpid; + + // C层单车业务：驱动左右夹臂运动到夹紧或松开目标。 + public override IEnumerable Get() + { + try + { + leftpid = new PIDController( + () => PilotDefinition.Self.ActualPosLeftArm, + ClampKp, ClampKi, ClampKd, ClampMaxI, + ClampDeadZone, MaxClampSpeed) + { + SpeedAccPerSec = ClampSpeedAcc + }; + + rightpid = new PIDController( + () => PilotDefinition.Self.ActualPosRightArm, + ClampKp, ClampKi, ClampKd, ClampMaxI, + ClampDeadZone, MaxClampSpeed) + { + SpeedAccPerSec = ClampSpeedAcc + }; + + var startTime = DateTime.UtcNow; + while (true) + { + if (TimeoutSeconds > 0f && + (DateTime.UtcNow - startTime).TotalSeconds > + TimeoutSeconds) + { + Console.WriteLine( + $"夹臂运动超时({TimeoutSeconds:F1}s)，" + + "停止左右夹臂。"); + yield break; + } + + var leftspeed = + leftpid.GetResponse(LeftClampTarget); + var rightspeed = + rightpid.GetResponse(RightClampTarget); + Console.WriteLine( + $"left arm speed:{leftspeed} " + + $"right arm speed:{rightspeed}"); + + PilotDefinition.Self.SpeedLeftArm = leftspeed; + PilotDefinition.Self.SpeedRightArm = rightspeed; + + var leftArrived = leftpid.IsArrived(); + var rightArrived = rightpid.IsArrived(); + if (leftArrived) + PilotDefinition.Self.SpeedLeftArm = 0f; + if (rightArrived) + PilotDefinition.Self.SpeedRightArm = 0f; + + if (leftArrived && rightArrived) + break; + + yield return true; + } + + Console.WriteLine( + $"left clamp to target:{LeftClampTarget} " + + $"right clamp to target:{RightClampTarget}"); + } + finally + { + PilotDefinition.Self.SpeedLeftArm = 0f; + PilotDefinition.Self.SpeedRightArm = 0f; + } + } + } + #endregion +} diff --git a/MultiWheelC/MultiWheelC.csproj b/MultiWheelC/MultiWheelC.csproj new file mode 100644 index 0000000..4ebf4e4 --- /dev/null +++ b/MultiWheelC/MultiWheelC.csproj @@ -0,0 +1,43 @@ + + + + netstandard2.0 + 10 + MultiWheelC + MultiWheelC + false + build\Clumsy\ + + + + + + + + + + + + + + ref\LessokajiWeaverUtilities.dll + + + ref\MDCSToolBox.dll + + + ref\RefClumsyCore.dll + + + ref\RefClumsyDance.dll + + + ref\RefFundamentalLib.dll + + + ..\ref\CommonUsage.dll + + + + diff --git a/MultiWheelC/PilotConfig.cs b/MultiWheelC/PilotConfig.cs new file mode 100644 index 0000000..810f2f2 --- /dev/null +++ b/MultiWheelC/PilotConfig.cs @@ -0,0 +1,335 @@ +using ClumsyCore; +using MDCSToolBox.Clumsy.Pilot.MultiWheel; +using Newtonsoft.Json; + +namespace MultiWheelC; + +public class PilotConfig : MultiWheelPilotConfig +{ + #region 单车-轨迹跟踪 LineTracking + + [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 DstTrackerMaxSpeed = 0.3f; + #endregion + + #region 单车-原地旋转 暂时没用上 + [FieldMember(desc = "原地旋转：目标朝向(世界坐标系, deg)")] + public float InPlaceRotateTargetWorldDeg = 90f; + + [FieldMember(desc = "原地旋转：旋转角速度(deg/s)")] + public float InPlaceRotateSpeed = 30f; + + [FieldMember(desc = "原地旋转：到位角度精度(deg)")] + public float InPlaceRotateArriveDeg = 1f; + + [FieldMember(desc = "原地旋转：起转前舵轮对齐精度(deg)")] + public float InPlaceRotateWheelAlignDeg = 2f; + + [FieldMember(desc = "原地旋转：旋转过程中舵轮偏差重对齐阈值(deg)")] + public float InPlaceRotateActiveWheelAlignDeg = 10f; + + #endregion + + #region 单车-临时 + [FieldMember(desc = "原地旋转Kp")] + public float InPlaceRotateKp = 0.2f; + // public float InPlaceRotateKp = 0.2f; + + [FieldMember(desc = "原地旋转Ki")] + public float InPlaceRotateKi = 0.01f; + // public float InPlaceRotateKi = 0.01f; + + [FieldMember(desc = "原地旋转Kd")] + public float InPlaceRotateKd = 0f; + + [FieldMember(desc = "原地旋转积分限幅")] + public float InPlaceRotateMaxI = 0.01f; + + [FieldMember(desc = "原地旋转最大角速度(deg/s)")] + public float InPlaceRotateMaxSpeed = 30f; + + [FieldMember(desc = "原地旋转角加速度(deg/s²)")] + public float InPlaceRotateAcc = 30f; + + [FieldMember(desc = "原地旋转超时(s)")] + public float InPlaceRotateTimeoutSec = 15f; + #endregion + + + + + #region 单车-钻车与夹抱 + [FieldMember(desc = "2腿检测：雷达名（逗号分隔可多个）")] + public string TwoLegLidarName = "rear_left_lidar_1,rear_right_lidar_1"; + + [FieldMember(desc = "2腿检测：初始猜测X(mm, 车体坐标系)")] + public float TwoLegGuessX = 2000f; + + [FieldMember(desc = "2腿检测：两腿间距(mm)")] + public float TwoLegWidth = 800f; + + [FieldMember(desc = "2腿检测：两腿间距允许误差(mm)")] + public float TwoLegWidthErr = 100f; + + [FieldMember(desc = "2腿检测：聚类点间距(mm)")] + public float TwoLegBlobDist = 100f; + + [FieldMember(desc = "2腿检测：聚类尺寸(mm)")] + public float TwoLegBlobSize = 200f; + + [FieldMember(desc = "2腿检测：聚类最小点数")] + public int TwoLegBlobPtCount = 5; + + [FieldMember(desc = "2腿检测：聚类 padding")] + public int TwoLegPadding = 5; + + [FieldMember(desc = "2腿检测：腿柱搜索范围")] + public int TwoLegPillarFindingScope = 20; + + [FieldMember(desc = "2腿检测：方向符号(±1)")] + public int TwoLegSgnDir = 1; + + [FieldMember(desc = "2腿检测：中心X偏移(mm)")] + public float TwoLegCenterChangeX = 0f; + + [FieldMember(desc = "2腿检测：输出X补偿(mm)")] + public float TwoLegOutputBiasX = 0f; + + [FieldMember(desc = "2腿检测：输出Y补偿(mm)")] + public float TwoLegOutputBiasY = 0f; + + [FieldMember(desc = "2腿检测：ROI滤波框长(mm)")] + public float TwoLegFilterLength = 1800f; + + [FieldMember(desc = "2腿检测：ROI滤波框宽(mm)")] + public float TwoLegFilterWidth = 600f; + + [FieldMember(desc = "轮胎识别:识别框长")] public float TireFilterLength = 1800f; + [FieldMember(desc = "轮胎识别:识别框宽")] public float TireFilterWidth = 600f; + [FieldMember(desc = "轮胎识别:轮胎间距")] public float TireTwoLegWidth = 800f; + [FieldMember(desc = "轮胎识别:轮胎识别允许误差")] public float TireTwoLegWidthErr = 100f; + [FieldMember(desc = "轮胎识别:轮胎聚类最小点云数")] public int TireTwoLegBlobPtCount = 15; + + [FieldMember(desc = "轮胎识别:前雷达参数")] public float TireFrontTwoLegBlobDist = 100f; + [FieldMember(desc = "轮胎识别:前雷达参数")] public float TireFrontTwoLegBlobSize = 200f; + [FieldMember(desc = "轮胎识别:前雷达参数")] public int TireFrontPadding = 5; + [FieldMember(desc = "轮胎识别:前雷达参数")] public int TireFrontTwoLegPillarFindingScope = 20; + [FieldMember(desc = "轮胎识别:前雷达参数")] public int TireFrontTwoLegSgnDir = 1; + [FieldMember(desc = "轮胎识别:前雷达参数")] public float TireFrontTwoLegCenterChangeX = 0; + [FieldMember(desc = "轮胎识别:后雷达参数")] public float TireBackTwoLegBlobDist = 100f; + [FieldMember(desc = "轮胎识别:后雷达参数")] public float TireBackTwoLegBlobSize = 200f; + [FieldMember(desc = "轮胎识别:后雷达参数")] public int TireBackPadding = 5; + [FieldMember(desc = "轮胎识别:后雷达参数")] public int TireBackTwoLegPillarFindingScope = 20; + [FieldMember(desc = "轮胎识别:后雷达参数")] public int TireBackTwoLegSgnDir = 1; + [FieldMember(desc = "轮胎识别:后雷达参数")] public float TireBackTwoLegCenterChangeX = 0; + + [FieldMember(desc = "轮胎跟踪:切换至盲走距离")] public float TireFollowingWalkBlindSwitchingDistance = 1200f; + [FieldMember(desc = "轮胎跟踪:识别第一对轮胎的初始距离")] public float TireFollowingStage1GuessX = 2000f; + [FieldMember(desc = "轮胎跟踪:识别第二对轮胎的初始距离")] public float TireFollowingStage2GuessX = 2475f; + [FieldMember(desc = "轮胎跟踪:盲走停止距离")] public float TireFollowingWalkBlindFinishDistance = 10f; + [FieldMember(desc = "轮胎跟踪:减速距离")] public float TireFollowingSlowDistance = 200f; + [FieldMember(desc = "轮胎跟踪:最大速度")] public float TireFollowingMaxSpeed = 0.2f; + [FieldMember(desc = "轮胎跟踪:前雷达识别路径偏移X")] public float TireFollowingFrontLidarPathTransformationX = 253f; + [FieldMember(desc = "轮胎跟踪:前雷达识别路径偏移Y")] public float TireFollowingFrontLidarPathTransformationY = 13f; + [FieldMember(desc = "轮胎跟踪:前雷达识别路径偏移Th")] public float TireFollowingFrontLidarWalkBlindTh = -1f; + [FieldMember(desc = "轮胎跟踪:后雷达识别路径偏移X")] public float TireFollowingBackLidarPathTransformationX = 148f; + [FieldMember(desc = "轮胎跟踪:后雷达识别路径偏移Y")] public float TireFollowingBackLidarPathTransformationY = 2f; + [FieldMember(desc = "轮胎跟踪:后雷达识别路径偏移Th")] public float TireFollowingBackLidarWalkBlindTh = 0f; + [FieldMember(desc = "轮胎跟踪:离车时后雷达识别路径偏移X")] public float TireFollowingLeaveCarBackLidarPathTransformationX = 1500f; + [FieldMember(desc = "轮胎跟踪:离车时切换至盲走距离")] public float TireFollowingLeaveCarWalkBlindSwitchingDistance = 1200f; + + [FieldMember(desc = "轮胎跟踪:测试钻轮胎数量")] public int TireFollowingTireNum = 1; + [FieldMember(desc = "轮胎跟踪:过近距离")] public float TireFollowingCloseDistance = 1400; + [FieldMember(desc = "轮胎跟踪:距离过近角度忽略阈值")] public float TireFollowingAngleIgnoreThr = 0.2f; + [FieldMember(desc = "轮胎跟踪:Y最大平均数")] public int TireFollowingYAverageFrameCount = 5; + + [FieldMember(desc = "轮胎跟踪:释放锁点距离")] public float TireFollowingReleaseDistance = 1600; + + [FieldMember(desc = "轮胎跟踪:角度调整kp")] public float TireFollowingThkp = 0.05f; + [FieldMember(desc = "轮胎跟踪:角度调整ki")] public float TireFollowingThki = 0.01f; + [FieldMember(desc = "轮胎跟踪:角度调整kd")] public float TireFollowingThkd = 0f; + [FieldMember(desc = "轮胎跟踪:角度调整SpeedAcc")] public float TireFollowingThSpeedAccPerSec = 1f; + [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 + + + +} diff --git a/MultiWheelC/PilotDefinition.cs b/MultiWheelC/PilotDefinition.cs new file mode 100644 index 0000000..188fa94 --- /dev/null +++ b/MultiWheelC/PilotDefinition.cs @@ -0,0 +1,44 @@ +using ClumsyCore; +using ClumsyCore.Interfaces; +using ClumsyCore.Pilot; +using FundamentalLib; +using MDCSToolBox.Clumsy.Pilot.MultiWheel; +namespace MultiWheelC; + +public class PilotDefinition : MultiWheelPilotDefinition +{ + public new float CarLength = 1472f; + public new float CarWidth = 948f; + [AsLowerIO(desc = "车号")] public int CarNum = 1; + public override void StandardInit() { } + + #region 夹臂变量 + [AsUpperIO(desc = "左夹臂下发速度")] public float SpeedLeftArm; + + [AsUpperIO(desc = "右夹臂下发速度")] public float SpeedRightArm; + + [AsLowerIO(desc = "左夹臂实际位置")] public float ActualPosLeftArm; + + [AsLowerIO(desc = "右夹臂实际位置")] public float ActualPosRightArm; + + [AsUpperIO(desc = "夹臂不同步报警")] public bool ClampOutOfSync = false; + #endregion + + [AsLowerIO(desc = "左前左轮实际位置")] public float LFLActualPos; + [AsLowerIO(desc = "左前右轮实际位置")] public float LFRActualPos; + [AsLowerIO(desc = "右前左轮实际位置")] public float RFLActualPos; + [AsLowerIO(desc = "右前右轮实际位置")] public float RFRActualPos; + [AsLowerIO(desc = "左后左轮实际位置")] public float LRLActualPos; + [AsLowerIO(desc = "左后右轮实际位置")] public float LRRActualPos; + [AsLowerIO(desc = "右后左轮实际位置")] public float RRLActualPos; + [AsLowerIO(desc = "右后右轮实际位置")] public float RRRActualPos; + + [AsLowerIO(desc = "左夹臂低限位")] public float LeftArmLowerPos; + [AsLowerIO(desc = "左夹臂高限位")] public float LeftArmUpperPos; + [AsLowerIO(desc = "右夹臂低限位")] public float RightArmLowerPos; + [AsLowerIO(desc = "右夹臂高限位")] public float RightArmUpperPos; + + [AsUpperIO(desc = "从C往驱动器下使能")] public bool DisableFromC = false; + [AsUpperIO(desc = "从C上复位")] public bool ResetFromC = false; + [AsLowerIO(desc = "驱动轮使能状态")] public bool WheelAbleState = true; +} diff --git a/MultiWheelC/TrackingExperimentRecorder.cs b/MultiWheelC/TrackingExperimentRecorder.cs new file mode 100644 index 0000000..7d15f46 --- /dev/null +++ b/MultiWheelC/TrackingExperimentRecorder.cs @@ -0,0 +1,415 @@ +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; +using MyParking.Shared; + +namespace MultiWheelC +{ + // C层实验数据：保存一个采样时刻的定位与控制命令。 + public sealed class TrackingSample + { + public double ElapsedSeconds; + + // Detour位置单位为mm，航向单位为deg。 + public double DetourX; + public double DetourY; + public double DetourTheta; + + // 车体速度单位为m/s，角速度统一使用rad/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 float _referenceAngularSpeed; + private readonly float _referenceMotionFrameYawDegrees; + private readonly int _sampleIntervalMs; + + private readonly List _samples = + new List(); + + 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, + float referenceAngularSpeed = 0f, + int sampleIntervalMs = 50, + float referenceMotionFrameYawDegrees = 0f) + { + 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; + _referenceAngularSpeed = referenceAngularSpeed; + _referenceMotionFrameYawDegrees = + referenceMotionFrameYawDegrees; + _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; + // CommonUsage.GetCarSpeed().Vw的单位为deg/s， + // 记录器内部统一转换为rad/s。 + commandAngularSpeed = + (float)AngleMath.DegreesToRadians( + 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 snapshot; + + lock (_sampleSyncRoot) + { + snapshot = + new List(_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," + + // 保留旧列（deg/s）供历史Python脚本兼容。 + "CommandAngularSpeed," + + "CommandAngularSpeedRadPerSecond," + + "CommandVx," + + "CommandVy," + + "ReferenceStartX," + + "ReferenceStartY," + + "ReferenceEndX," + + "ReferenceEndY," + + "ReferenceSpeed," + + "ReferenceAngularSpeedRadPerSecond," + + "ReferenceMotionFrameYawDegrees"); + + 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( + AngleMath.RadiansToDegrees( + sample.CommandAngularSpeed)), + Format(sample.CommandAngularSpeed), + Format(sample.CommandVx), + Format(sample.CommandVy), + Format(_referenceStart.X), + Format(_referenceStart.Y), + Format(_referenceEnd.X), + Format(_referenceEnd.Y), + Format(_referenceSpeed), + Format(_referenceAngularSpeed), + Format(_referenceMotionFrameYawDegrees))); + } + } + } + + // 将文件名中的非法字符替换为下划线。 + 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("\"", "\"\"") + + "\""; + } + } +} diff --git a/MultiWheelC/build/Clumsy/CommonUsage.dll b/MultiWheelC/build/Clumsy/CommonUsage.dll new file mode 100644 index 0000000..9079f92 Binary files /dev/null and b/MultiWheelC/build/Clumsy/CommonUsage.dll differ diff --git a/MultiWheelC/build/Clumsy/LessokajiWeaverUtilities.dll b/MultiWheelC/build/Clumsy/LessokajiWeaverUtilities.dll new file mode 100644 index 0000000..7ca4e8f Binary files /dev/null and b/MultiWheelC/build/Clumsy/LessokajiWeaverUtilities.dll differ diff --git a/MultiWheelC/build/Clumsy/MDCSToolBox.dll b/MultiWheelC/build/Clumsy/MDCSToolBox.dll new file mode 100644 index 0000000..091a9bb Binary files /dev/null and b/MultiWheelC/build/Clumsy/MDCSToolBox.dll differ diff --git a/MultiWheelC/build/Clumsy/MultiWheelC.deps.json b/MultiWheelC/build/Clumsy/MultiWheelC.deps.json new file mode 100644 index 0000000..aa63784 --- /dev/null +++ b/MultiWheelC/build/Clumsy/MultiWheelC.deps.json @@ -0,0 +1,163 @@ +{ + "runtimeTarget": { + "name": ".NETStandard,Version=v2.0/", + "signature": "" + }, + "compilationOptions": {}, + "targets": { + ".NETStandard,Version=v2.0": {}, + ".NETStandard,Version=v2.0/": { + "MultiWheelC/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": { + "MultiWheelC.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": { + "MultiWheelC/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": "" + } + } +} \ No newline at end of file diff --git a/MultiWheelC/build/Clumsy/MultiWheelC.dll b/MultiWheelC/build/Clumsy/MultiWheelC.dll new file mode 100644 index 0000000..d8744ae Binary files /dev/null and b/MultiWheelC/build/Clumsy/MultiWheelC.dll differ diff --git a/MultiWheelC/build/Clumsy/MultiWheelC.pdb b/MultiWheelC/build/Clumsy/MultiWheelC.pdb new file mode 100644 index 0000000..5fff990 Binary files /dev/null and b/MultiWheelC/build/Clumsy/MultiWheelC.pdb differ diff --git a/MultiWheelC/build/Clumsy/RefClumsyCore.dll b/MultiWheelC/build/Clumsy/RefClumsyCore.dll new file mode 100644 index 0000000..ed36470 Binary files /dev/null and b/MultiWheelC/build/Clumsy/RefClumsyCore.dll differ diff --git a/MultiWheelC/build/Clumsy/RefClumsyDance.dll b/MultiWheelC/build/Clumsy/RefClumsyDance.dll new file mode 100644 index 0000000..7e191a1 Binary files /dev/null and b/MultiWheelC/build/Clumsy/RefClumsyDance.dll differ diff --git a/MultiWheelC/build/Clumsy/RefFundamentalLib.dll b/MultiWheelC/build/Clumsy/RefFundamentalLib.dll new file mode 100644 index 0000000..2b8ba30 Binary files /dev/null and b/MultiWheelC/build/Clumsy/RefFundamentalLib.dll differ diff --git a/MultiWheelC/ref/LessokajiWeaverUtilities.dll b/MultiWheelC/ref/LessokajiWeaverUtilities.dll new file mode 100644 index 0000000..7ca4e8f Binary files /dev/null and b/MultiWheelC/ref/LessokajiWeaverUtilities.dll differ diff --git a/MultiWheelC/ref/MDCSToolBox.dll b/MultiWheelC/ref/MDCSToolBox.dll new file mode 100644 index 0000000..091a9bb Binary files /dev/null and b/MultiWheelC/ref/MDCSToolBox.dll differ diff --git a/MultiWheelC/ref/RefClumsyCore.dll b/MultiWheelC/ref/RefClumsyCore.dll new file mode 100644 index 0000000..ed36470 Binary files /dev/null and b/MultiWheelC/ref/RefClumsyCore.dll differ diff --git a/MultiWheelC/ref/RefClumsyDance.dll b/MultiWheelC/ref/RefClumsyDance.dll new file mode 100644 index 0000000..7e191a1 Binary files /dev/null and b/MultiWheelC/ref/RefClumsyDance.dll differ diff --git a/MultiWheelC/ref/RefFundamentalLib.dll b/MultiWheelC/ref/RefFundamentalLib.dll new file mode 100644 index 0000000..2b8ba30 Binary files /dev/null and b/MultiWheelC/ref/RefFundamentalLib.dll differ diff --git a/README.md b/README.md new file mode 100644 index 0000000..65cc54c --- /dev/null +++ b/README.md @@ -0,0 +1,264 @@ +# MyParking 停车机器人 + +[简体中文](README.md) | [English](README_en.md) + +## 重写路线与当前状态 + +本仓库是停车机器人控制软件的重写版本，研发顺序保持为： + +1. 先实现单台停车机器人小车的基本功能； +2. 在单车闭环稳定后逐步增加停车作业功能； +3. 基于仿真、台架和实车数据优化轨迹跟踪方法； +4. 最后再考虑多车通信、编队和协同控制。 + +当前工作仍以**单车**为主，已经从基础框架搭建进入底盘联调、功能补充和跟踪实验阶段。多车配置仍位于 `PilotConfig.cs` 的 `#if false` 区域，`Shared/FleetKinematics.cs` 仍是占位文件，不能视为多车能力已经实现。 + +| 阶段 | 当前状态 | 说明 | +| --- | --- | --- | +| 1. 单车基本功能 | 联调中 | 已接入运动控制、MCU 通信、轮组反馈、急停 IO、电池、灯光、遥控和诊断代码，仍需持续实车验证 | +| 2. 增加停车功能 | 部分开展 | 已提供夹臂控制、限位、报警和测试入口；轮胎识别、钻车和完整停车流程尚未实现 | +| 3. 优化跟踪方法 | 已启动 | 已加入直线、圆弧、S 型、蟹行测试、实验 CSV 记录和 Python 绘图工具 | +| 4. 多车场景 | 暂不实施 | 多车参数和预研内容未参与当前编译，当前版本不提供多车联动 | + +## 项目简介 + +MyParking 是一个面向多轮停车机器人底盘的 C# 工程，覆盖上层运动动作、共享运动学、底层硬件适配、离线 Web 仿真和实验数据分析。 + +核心代码分为： + +- `ClumsyPilot`：Clumsy 上层动作、轨迹跟踪和人工测试； +- `MedullaAdapter`：Medulla 下层 MCU、CAN、串口、轮组、夹臂、遥控和报警适配； +- `Shared`：统一的二维坐标、底盘命令、坐标变换和多轮底盘适配； +- `CommonUsage-MultiVehicleSync/commonusage`：仓库内的 `CommonUsage` 底盘公共库源码； +- `Simulation`：基于 ASP.NET Core 的单车 Web 仿真器； +- `data_process`：轨迹实验 CSV 的 Python 分析工具。 + +仓库中没有 ROS/ROS 2 或 Docker 配置。 + +## 当前已接入能力 + +| 模块 | 当前代码能力 | +| --- | --- | +| 单车运动 | 直线、圆弧、S 型轨迹，前进、蟹行和原地旋转 | +| 底盘命令 | `SendMotion`、`SendXYThSpeed` 和虚拟阿克曼测试后端 | +| 模式切换 | 正常、蟹行、自转模式；切换时先停车、预转舵轮并等待到位 | +| 跟踪控制 | 终点跟踪、直线跟踪、基于 Detour 的直线跟踪和蟹行运动坐标系跟踪 | +| 夹臂 | 左右夹臂速度命令、位置反馈、软限位、驱动报警、实体/虚拟遥控和目标位置动作 | +| MCU 通信 | 串口桥打开、复位、版本/状态查询、数字 IO、CAN/串口同步收发和异步回调 | +| 驱动与反馈 | 8 个驱动电机和 4 个舵轮的命令、速度/位置/舵角反馈及远程帧状态 | +| 车辆状态 | 急停、启停、抱闸、灯光、电池 SOC/SOH 和驱动使能状态 | +| 诊断 | CAN 轮速事件与周期快照 CSV、轨迹实验 CSV、控制命令和 Detour 位姿记录 | +| 仿真 | 浏览器二维车辆显示、模式按钮、手动控制、车辆配置、复位和 REST API | + +以上表示代码和测试入口已经存在，不等同于所有工况均已完成实车验收。 + +## 软件架构 + +```text +Clumsy 宿主 + │ + ▼ +ClumsyPilot ───────────────┐ + │ │ + ▼ │ 实验 CSV +Shared / CommonUsage ├──────────► data_process + │ │ + ▼ │ +Medulla 宿主 │ + │ │ + ▼ │ +MedullaAdapter │ + │ P/Invoke │ + ▼ │ +mcu_serial_bridge.dll │ + │ │ + ▼ │ +MCU ─► CAN / Serial / IO ──┘ + +Simulation ─► Shared 数据类型 ─► 浏览器仿真界面 +``` + +`ClumsyPilot` 和 `MedullaAdapter` 生成插件类库，需要由对应宿主加载；`Simulation` 是可以独立启动的 ASP.NET Core Web 项目。 + +## 目录说明 + +```text +MyParking/ +├── ParkingRobot.sln +├── ClumsyPilot/ # 上层动作、跟踪、测试和实验记录 +├── MedullaAdapter/ # MCU、CAN、轮组、夹臂、遥控和报警 +├── Shared/ # 共享命令、坐标变换和底盘适配 +├── CommonUsage-MultiVehicleSync/ +│ └── commonusage/ # CommonUsage 公共底盘库源码 +├── Simulation/ # .NET 8 Web 仿真器 +│ ├── Commands/ # 可由特性自动发现的仿真动作 +│ ├── Core/ # 仿真车辆、舵轮、时钟和世界 +│ ├── Models/ # Web API DTO +│ └── wwwroot/ # 浏览器界面 +├── data_process/ # Python 实验绘图脚本 +├── ref/ # 两个插件共同使用的 CommonUsage.dll +├── 测试方案.txt # 单车轨迹实验方案 +├── 记录.txt # 项目调试记录 +└── 电机记录.txt # 电机调试记录 +``` + +根目录的 `ParkingRobot.sln` 当前只包含 `ClumsyPilot` 和 `MedullaAdapter`；`CommonUsage` 与 `Simulation` 需要分别构建。 + +## 开发环境与依赖 + +- Windows 开发/实机运行环境； +- Visual Studio 2022，或支持 .NET 8.0 和 .NET Standard 2.0 的 .NET SDK； +- Python 环境，用于可选的实验数据绘图； +- Clumsy/Medulla 内部框架程序集，位于各项目的 `ref` 目录； +- 实机所需的 `mcu_serial_bridge.dll`，当前仓库中未包含该文件； +- 能够加载 `ClumsyPilot.dll` 和 `MedullaAdapter.dll` 的匹配版本宿主程序，当前仓库中未包含宿主。 + +主要 NuGet/Python 依赖： + +- `ClumsyPilot`：`Newtonsoft.Json 13.0.3`、`System.Numerics.Vectors 4.6.1`； +- `CommonUsage`：`MQTTnet 4.3.7.1207`、`Newtonsoft.Json 13.0.3` 等； +- `data_process`：NumPy、pandas、Matplotlib、SciPy。 + +## 编译 + +### 1. 构建 CommonUsage + +修改公共底盘库后，先执行： + +```powershell +dotnet restore CommonUsage-MultiVehicleSync\commonusage\CommonUsage.csproj +dotnet build CommonUsage-MultiVehicleSync\commonusage\CommonUsage.csproj -c Debug +``` + +该项目的构建目标会把生成的 `CommonUsage.dll` 复制到根目录 `ref`。 + +### 2. 构建实车插件 + +```powershell +dotnet restore ParkingRobot.sln +dotnet build ParkingRobot.sln -c Debug +``` + +主要输出： + +```text +ClumsyPilot/build/Clumsy/ClumsyPilot.dll +MedullaAdapter/build/Medulla/plugins/MedullaAdapter.dll +``` + +### 3. 构建 Web 仿真器 + +```powershell +dotnet restore Simulation\MyParking.Simulation.csproj +dotnet build Simulation\MyParking.Simulation.csproj -c Debug +``` + +## 启动 Web 仿真 + +```powershell +dotnet run --project Simulation\MyParking.Simulation.csproj --launch-profile http +``` + +浏览器访问： + +```text +http://localhost:5203 +``` + +仿真界面提供正常、左蟹行、右蟹行、自转、前进、后退、左转、右转、停止和复位动作，并可修改车辆布局及手动控制输入。主要 API 包括： + +- `GET /api/vehicles` +- `GET /api/actions` +- `GET/POST /api/configuration` +- `POST /api/vehicles/{vehicleId}/commands/{command}` +- `POST /api/vehicles/{vehicleId}/manual-control` +- `POST /api/reset` + +`Simulation/Commands/MySimulationTests.cs` 给出了自定义仿真动作示例；为静态方法添加 `SimulationAction` 特性后，调度器会自动发现并在网页生成对应动作。 + +## 实车运行与 MCU 配置 + +实车插件不能通过 `dotnet run` 独立启动。需要由匹配版本的 Clumsy/Medulla 宿主加载两个 DLL。宿主版本、部署目录和完整启动步骤尚未随仓库提供，待补充。 + +当前源码中的 MCU 默认参数： + +| 参数 | 默认值 | +| --- | --- | +| MCU 端口 | `COM4` | +| MCU 连接波特率 | `1000000` | +| CAN | 1 路，`500000 bit/s`，重试时间 `10 ms` | +| 串口 | 3 路，`9600 bit/s`，接收帧时间 `10 ms` | +| 电池通信端口索引 | `3` | +| 自转最大角速度 | `30 deg/s` | +| 轮速诊断目录 | `logs\wheel-speed` | + +当前工作区存在 `chassis.json` 底盘参数样例，但源码中尚未发现自动加载该文件的入口；实车参数仍应以宿主实际配置为准。 + +实机测试前必须确认端口、车号、舵轮零位与限位、速度单位、驱动方向、夹臂限位和急停链路。建议先架空驱动轮或在隔离区域低速测试，并保留独立可靠的物理急停，不能只依赖软件停车。 + +## 单车测试入口 + +`ClumsyPilot/MovementTests.cs` 当前注册： + +- `准备：四个舵轮与车头方向一致` +- `SendMotion：连续前进4m` +- `SendXYThSpeed：原地自转90°` +- `SendXYThSpeed：原地自转180°` +- `SendMotion：左转90°半径2m圆弧` +- `SendMotion：蟹行直线4m` +- `SendMotion：蟹行左转90°半径2m圆弧` +- `SendMotion：4m S型曲线` +- `夹臂关闭测试` +- `夹臂启动测试` + +这些测试由 Clumsy 宿主的测试界面执行，并不是 `dotnet test` 自动化测试。运动测试会按配置记录实验编号、参考轨迹、Detour 位姿和控制命令。 + +## 实验数据分析 + +轨迹记录器默认把 CSV 保存到宿主程序目录下的： + +```text +TrackingExperiments/ +``` + +Medulla 的轮速诊断可通过 `StartWheelSpeedDiagnostic` / `StopWheelSpeedDiagnostic` 操作按钮控制，默认输出到： + +```text +logs/wheel-speed/ +``` + +在自行管理的 Python 环境中安装依赖： + +```powershell +python -m pip install -r data_process\requirements.txt +``` + +对一份或多份轨迹 CSV 同时生成轨迹对比、跟踪误差、速度响应和角速度命令图： + +```powershell +python data_process\run_all_plots.py "路径\实验1.csv" "路径\实验2.csv" --output-dir "路径\plots" +``` + +不传 CSV 路径时，脚本会查找 `data_process` 目录中的 CSV。默认重采样频率为 `20 Hz`，滤波窗口为 `0.55 s`，可通过 `--frequency` 和 `--window` 调整。 + +## 尚未完成或需要继续验证 + +- 雷达点云、轮胎识别、自动钻车、车辆释放和完整停车作业状态机； +- 当前运动和夹臂功能的完整实车验收、故障注入及长期稳定性测试； +- 舵轮软限位预测和自动车身重定向；`SteeringConstraintManager.cs` 当前主要是设计记录； +- 自动化单元测试和持续集成； +- 多车通信、编队、同步和安全降级；`FleetKinematics.cs` 当前仅为占位； +- 宿主版本、插件部署目录、配置文件位置和发布流程。 + +## 参与开发 + +1. 当前改动优先服务于单车闭环、停车功能和跟踪质量，不提前启用多车代码； +2. 保持上层动作、共享运动学、底层硬件协议和仿真模块的边界； +3. 新增参数时注明坐标系、单位、默认值、车型和安全范围； +4. 提交前构建受影响的项目，并记录仿真、台架或实车验证条件； +5. 修改 `CommonUsage` 后同步更新根目录 `ref/CommonUsage.dll`； +6. 分支、评审和发布流程待团队补充。 + +## 许可证 + +仓库中暂未提供许可证文件。使用和分发范围请遵循公司内部规定。 diff --git a/README_en.md b/README_en.md new file mode 100644 index 0000000..569b7a8 --- /dev/null +++ b/README_en.md @@ -0,0 +1,264 @@ +# MyParking Parking Robot + +[简体中文](README.md) | [English](README_en.md) + +## Rewrite Roadmap and Current Status + +This repository is a rewrite of the parking-robot control software. Development follows this order: + +1. Implement the basic functions of one parking robot first; +2. Add parking-operation features after the single-robot loop is stable; +3. Improve tracking with simulation, bench, and physical-vehicle data; +4. Consider multi-robot communication, formation, and coordination last. + +The current work remains focused on the **single robot** and has progressed from framework construction to chassis integration, feature development, and tracking experiments. Multi-robot settings remain inside the `#if false` section of `PilotConfig.cs`, while `Shared/FleetKinematics.cs` is still a placeholder. These files do not represent an implemented multi-robot system. + +| Stage | Current status | Notes | +| --- | --- | --- | +| 1. Basic single-robot functions | Integration in progress | Motion control, MCU communication, wheel feedback, emergency-stop I/O, battery, lights, remote control, and diagnostics are connected in code; physical validation is ongoing | +| 2. Add parking functions | Partially started | Clamp control, limits, alarms, and test entries exist; tire recognition, vehicle entry, and the complete parking workflow are not implemented | +| 3. Improve tracking | Started | Straight, arc, S-curve, and crab tests, experiment CSV recording, and Python plotting tools are available | +| 4. Multi-robot scenarios | Deferred | Multi-robot R&D settings are excluded from the build, and the current version provides no fleet coordination | + +## Overview + +MyParking is a C# project for a multi-wheel parking-robot chassis. It covers upper-layer actions, shared kinematics, lower-layer hardware adaptation, an offline Web simulator, and experiment-data analysis. + +The main components are: + +- `ClumsyPilot`: Clumsy actions, tracking, and manual tests; +- `MedullaAdapter`: Medulla MCU, CAN, serial, wheel, clamp, remote-control, and alarm adaptation; +- `Shared`: common 2D coordinates, chassis commands, frame transforms, and multi-wheel adaptation; +- `CommonUsage-MultiVehicleSync/commonusage`: in-repository source for the `CommonUsage` chassis library; +- `Simulation`: an ASP.NET Core single-robot Web simulator; +- `data_process`: Python tools for tracking-experiment CSV files. + +No ROS/ROS 2 or Docker configuration is present. + +## Currently Integrated Capabilities + +| Module | Current code capability | +| --- | --- | +| Single-robot motion | Straight, arc, and S-curve paths; forward, crab, and in-place rotation | +| Chassis commands | `SendMotion`, `SendXYThSpeed`, and a virtual-Ackermann test backend | +| Mode switching | Normal, crab, and spin modes; stop, pre-steer, and wait for wheel alignment before motion | +| Tracking | Destination tracking, line tracking, Detour-based line tracking, and crab motion-frame tracking | +| Clamp | Left/right speed commands, position feedback, soft limits, driver alarms, physical/virtual remote control, and target-position actions | +| MCU communication | Bridge open/reset, version/state queries, digital I/O, synchronous serial/CAN access, and asynchronous callbacks | +| Drive and feedback | Commands and speed/position/steering feedback for eight drive motors and four steer modules, plus remote-frame state | +| Vehicle state | Emergency stop, start/stop, brake, lights, battery SOC/SOH, and drive-enable state | +| Diagnostics | CAN wheel-speed events, periodic snapshot CSVs, tracking CSVs, command recording, and Detour pose recording | +| Simulation | Browser-based 2D display, mode actions, manual control, vehicle configuration, reset, and REST APIs | + +The presence of code and test entries does not mean every operating condition has passed physical acceptance testing. + +## Software Architecture + +```text +Clumsy host + │ + ▼ +ClumsyPilot ───────────────┐ + │ │ + ▼ │ experiment CSV +Shared / CommonUsage ├──────────► data_process + │ │ + ▼ │ +Medulla host │ + │ │ + ▼ │ +MedullaAdapter │ + │ P/Invoke │ + ▼ │ +mcu_serial_bridge.dll │ + │ │ + ▼ │ +MCU ─► CAN / Serial / IO ──┘ + +Simulation ─► Shared data types ─► browser simulator +``` + +`ClumsyPilot` and `MedullaAdapter` build as plugin libraries that require their respective hosts. `Simulation` is an independently runnable ASP.NET Core Web project. + +## Repository Layout + +```text +MyParking/ +├── ParkingRobot.sln +├── ClumsyPilot/ # Upper-layer actions, tracking, tests, and recording +├── MedullaAdapter/ # MCU, CAN, wheel, clamp, remote, and alarms +├── Shared/ # Shared commands, frame transforms, and chassis adapter +├── CommonUsage-MultiVehicleSync/ +│ └── commonusage/ # CommonUsage chassis-library source +├── Simulation/ # .NET 8 Web simulator +│ ├── Commands/ # Attribute-discovered simulation actions +│ ├── Core/ # Vehicles, steer wheels, clock, and world +│ ├── Models/ # Web API DTOs +│ └── wwwroot/ # Browser UI +├── data_process/ # Python experiment-plotting scripts +├── ref/ # CommonUsage.dll shared by both plugins +├── 测试方案.txt # Single-robot tracking experiment plan +├── 记录.txt # Project debugging notes +└── 电机记录.txt # Motor debugging notes +``` + +The root `ParkingRobot.sln` currently contains only `ClumsyPilot` and `MedullaAdapter`. Build `CommonUsage` and `Simulation` separately. + +## Development Environment and Dependencies + +- Windows development and physical-runtime environment; +- Visual Studio 2022, or a .NET SDK supporting .NET 8.0 and .NET Standard 2.0; +- A Python environment for optional experiment plotting; +- Internal Clumsy/Medulla framework assemblies under each project's `ref` directory; +- `mcu_serial_bridge.dll` for physical operation; this file is not currently in the repository; +- Compatible hosts capable of loading `ClumsyPilot.dll` and `MedullaAdapter.dll`; the hosts are not included. + +Primary NuGet/Python dependencies: + +- `ClumsyPilot`: `Newtonsoft.Json 13.0.3` and `System.Numerics.Vectors 4.6.1`; +- `CommonUsage`: `MQTTnet 4.3.7.1207`, `Newtonsoft.Json 13.0.3`, and related packages; +- `data_process`: NumPy, pandas, Matplotlib, and SciPy. + +## Build + +### 1. Build CommonUsage + +After changing the common chassis library, run: + +```powershell +dotnet restore CommonUsage-MultiVehicleSync\commonusage\CommonUsage.csproj +dotnet build CommonUsage-MultiVehicleSync\commonusage\CommonUsage.csproj -c Debug +``` + +The project includes a build target that copies the generated `CommonUsage.dll` to the root `ref` directory. + +### 2. Build Physical-Robot Plugins + +```powershell +dotnet restore ParkingRobot.sln +dotnet build ParkingRobot.sln -c Debug +``` + +Primary outputs: + +```text +ClumsyPilot/build/Clumsy/ClumsyPilot.dll +MedullaAdapter/build/Medulla/plugins/MedullaAdapter.dll +``` + +### 3. Build the Web Simulator + +```powershell +dotnet restore Simulation\MyParking.Simulation.csproj +dotnet build Simulation\MyParking.Simulation.csproj -c Debug +``` + +## Run the Web Simulator + +```powershell +dotnet run --project Simulation\MyParking.Simulation.csproj --launch-profile http +``` + +Open: + +```text +http://localhost:5203 +``` + +The UI provides normal, left-crab, right-crab, spin, forward, backward, left-turn, right-turn, stop, and reset actions. It also supports vehicle-layout configuration and manual-control input. Main APIs include: + +- `GET /api/vehicles` +- `GET /api/actions` +- `GET/POST /api/configuration` +- `POST /api/vehicles/{vehicleId}/commands/{command}` +- `POST /api/vehicles/{vehicleId}/manual-control` +- `POST /api/reset` + +`Simulation/Commands/MySimulationTests.cs` contains an example custom action. Add the `SimulationAction` attribute to a static method to have it discovered by the dispatcher and exposed in the Web UI. + +## Physical Runtime and MCU Configuration + +The physical-robot plugins cannot be started independently with `dotnet run`. Compatible Clumsy/Medulla hosts must load both DLLs. The required host versions, deployment directories, and complete startup procedure have not yet been provided. + +MCU defaults confirmed from the current source: + +| Setting | Default | +| --- | --- | +| MCU port | `COM4` | +| MCU connection baud rate | `1000000` | +| CAN | One channel at `500000 bit/s`, with a `10 ms` retry time | +| Serial | Three channels at `9600 bit/s`, with a `10 ms` receive-frame time | +| Battery port index | `3` | +| Maximum spin rate | `30 deg/s` | +| Wheel-speed diagnostic directory | `logs\wheel-speed` | + +A `chassis.json` chassis-parameter example is present in the current workspace, but no automatic loader for it was found in the source. Treat the actual host configuration as authoritative. + +Before physical testing, verify the port, vehicle ID, steering zero and limits, speed units, motor direction, clamp limits, and emergency-stop chain. Begin with lifted drive wheels or a segregated low-speed test area and retain an independent physical emergency stop; never rely on software stopping alone. + +## Single-Robot Test Entries + +`ClumsyPilot/MovementTests.cs` currently registers: + +- `准备：四个舵轮与车头方向一致` +- `SendMotion：连续前进4m` +- `SendXYThSpeed：原地自转90°` +- `SendXYThSpeed：原地自转180°` +- `SendMotion：左转90°半径2m圆弧` +- `SendMotion：蟹行直线4m` +- `SendMotion：蟹行左转90°半径2m圆弧` +- `SendMotion：4m S型曲线` +- `夹臂关闭测试` +- `夹臂启动测试` + +These are run through the Clumsy host's test interface and are not an automated `dotnet test` suite. Motion tests record the experiment number, reference path, Detour pose, and control commands according to their configuration. + +## Experiment Data Analysis + +The tracking recorder saves CSV files under the host application's: + +```text +TrackingExperiments/ +``` + +Medulla wheel-speed diagnostics can be controlled with the `StartWheelSpeedDiagnostic` and `StopWheelSpeedDiagnostic` utility buttons. Their default output directory is: + +```text +logs/wheel-speed/ +``` + +Install dependencies in a Python environment managed by your team: + +```powershell +python -m pip install -r data_process\requirements.txt +``` + +Generate trajectory comparison, tracking error, speed response, and angular-command plots for one or more CSV files: + +```powershell +python data_process\run_all_plots.py "path\trial1.csv" "path\trial2.csv" --output-dir "path\plots" +``` + +When no CSV path is supplied, the scripts search the `data_process` directory. The default resampling frequency is `20 Hz`, and the default filter window is `0.55 s`; use `--frequency` and `--window` to change them. + +## Incomplete or Pending Validation + +- Lidar point clouds, tire recognition, automatic vehicle entry, vehicle release, and the complete parking-operation state machine; +- Full physical acceptance, fault injection, and long-duration testing for current motion and clamp functions; +- Steering soft-limit prediction and automatic body reorientation; `SteeringConstraintManager.cs` currently contains mainly design notes; +- Automated unit tests and continuous integration; +- Multi-robot communication, formation, synchronization, and safety fallback; `FleetKinematics.cs` is currently only a placeholder; +- Host versions, plugin deployment directories, configuration-file locations, and the release process. + +## Contributing + +1. Prioritize single-robot closed-loop behavior, parking functions, and tracking quality; do not enable multi-robot code prematurely. +2. Preserve the boundaries between upper-layer actions, shared kinematics, hardware protocols, and simulation. +3. Document coordinate frames, units, defaults, applicable vehicle types, and safe ranges for new parameters. +4. Build every affected project before submission and record the simulation, bench, or physical-test conditions. +5. After changing `CommonUsage`, update the root `ref/CommonUsage.dll`. +6. The team still needs to document its branch, review, and release processes. + +## License + +No license file is currently included. Use and distribution must follow internal company policy. diff --git a/Shared/Chassis/MultiWheelChassisAdapter.cs b/Shared/Chassis/MultiWheelChassisAdapter.cs new file mode 100644 index 0000000..0c12308 --- /dev/null +++ b/Shared/Chassis/MultiWheelChassisAdapter.cs @@ -0,0 +1,515 @@ +// 将统一命令转换为原 Chassis API 调用 +using System; +using CommonUsage.Chassis; + +namespace MyParking.Shared +{ + /// + /// 将统一的单车车体速度命令转换为旧版MultiWheelChassis调用。 + /// 车体坐标系固定为X向前、Y向左、逆时针为正。 + /// + public sealed class MultiWheelChassisAdapter + { + #region 辅助内容 + private const double RadiansToDegrees = 180.0 / Math.PI; + private const float BiasTolerance = 0.001f; + private readonly MultiWheelChassis _chassis; + /// + /// 当前适配器对应的车辆编号。 + /// + public int VehicleId { get; } + + /// + /// Maximum distance from the body origin to a wheel center, in metres. + /// + public double MaximumWheelRadiusMeters { get; } + + /// + /// Maximum longitudinal wheel offset from the body origin, in metres. + /// For a symmetric four-wheel-steering chassis this is half the wheelbase. + /// + public double HalfWheelBaseMeters { get; } + + /// + /// 车体原点到最外侧舵轮中心的最大横向距离，单位为米。 + /// 对称四舵轮底盘中，它也是蟹行虚拟阿克曼模型的半轴距。 + /// + public double HalfTrackWidthMeters { get; } + + /// + /// Width of the steering-alignment speed gate, in degrees. + /// + public double SteeringAlignmentSigmaDegrees + { + get => _chassis.SteeringAlignmentSigmaDegrees; + set + { + if (double.IsNaN(value) || + double.IsInfinity(value) || + value <= 0.0 || + value > float.MaxValue) + { + throw new ArgumentOutOfRangeException( + nameof(value), + "Steering alignment sigma must be a positive finite value."); + } + + _chassis.SteeringAlignmentSigmaDegrees = + (float)value; + } + } + + /// + /// 检查旧底盘是否仍处于无偏置的真实车体坐标系。 + /// + private void EnsureBodyFrameIsActive() + { + EnsureMotionFrameIsActive(0.0); + } + + /// + /// 检查旧底盘当前是否处于指定的运动坐标系。 + /// motionDirectionRadians表示该运动系X轴在真实车体坐标系中的方向。 + /// + private void EnsureMotionFrameIsActive( + double motionDirectionRadians) + { + ValidateFinite( + motionDirectionRadians, + nameof(motionDirectionRadians)); + + var expectedBiasDegrees = + (float)( + -FrameTransform2D.NormalizeAngle( + motionDirectionRadians) * + RadiansToDegrees); + var bias = _chassis.GetOriginBias(); + var angleErrorDegrees = + NormalizeDegrees( + bias.Z - expectedBiasDegrees); + + if (Math.Abs(bias.X) <= BiasTolerance && + Math.Abs(bias.Y) <= BiasTolerance && + Math.Abs(angleErrorDegrees) <= + BiasTolerance) + { + return; + } + + throw new InvalidOperationException( + "MultiWheelChassis当前运动坐标系与命令不一致。" + + $"当前偏置为X={bias.X}, Y={bias.Y}, Th={bias.Z}°，" + + $"期望Th={expectedBiasDegrees}°。"); + } + + /// + /// 将角度归一化到[-180°,180°]附近。 + /// + private static float NormalizeDegrees(float degrees) + { + return (float)( + degrees - + Math.Round(degrees / 360.0) * 360.0); + } + /// + /// 检查底盘命令是否包含无效数值。 + /// + private static void ValidateTwist(Twist2D twist) + { + ValidateFinite( + twist.VxMetersPerSecond, + nameof(twist.VxMetersPerSecond)); + + ValidateFinite( + twist.VyMetersPerSecond, + nameof(twist.VyMetersPerSecond)); + + ValidateFinite( + twist.OmegaRadiansPerSecond, + nameof(twist.OmegaRadiansPerSecond)); + } + /// + /// 检查数值是否为有限值。 + /// + private static void ValidateFinite( + double value, + string parameterName) + { + if (double.IsNaN(value) || + double.IsInfinity(value)) + { + throw new ArgumentOutOfRangeException( + parameterName, + "底盘速度命令不能是NaN或无穷大。"); + } + + if (value > float.MaxValue || + value < -float.MaxValue) + { + throw new ArgumentOutOfRangeException( + parameterName, + "底盘速度命令超过float可表示范围。"); + } + } + + /// + /// 获取最近一次底盘运动分解失败原因。 + /// + public string LastFailureReason => + _chassis.LastMotionDecomposeFailureReason; + + #endregion + + /// + /// 将旧底盘的原点偏置恢复为真实单车车体坐标系。 + /// + public void ResetToBodyFrame() + { + ActivateMotionFrame(0.0); + } + + /// + /// 激活指定运动方向对应的SendMotion坐标系。 + /// 0表示真实车头，正90度表示将车体左侧作为虚拟车头。 + /// + public void ActivateMotionFrame( + double motionDirectionRadians) + { + ValidateFinite( + motionDirectionRadians, + nameof(motionDirectionRadians)); + + var biasDegrees = + (float)( + -FrameTransform2D.NormalizeAngle( + motionDirectionRadians) * + RadiansToDegrees); + var currentBias = + _chassis.GetOriginBias(); + + if (Math.Abs(currentBias.X) <= + BiasTolerance && + Math.Abs(currentBias.Y) <= + BiasTolerance && + Math.Abs( + NormalizeDegrees( + currentBias.Z - + biasDegrees)) <= + BiasTolerance) + { + return; + } + + _chassis.SetOriginBias( + x: 0.0f, + y: 0.0f, + th: biasDegrees); + } + public MultiWheelChassisAdapter(MultiWheelChassis chassis, int vehicleId) + { + _chassis = chassis ?? throw new ArgumentNullException(nameof(chassis)); + if (vehicleId <= 0) + { + throw new ArgumentOutOfRangeException( + nameof(vehicleId), + "车辆编号必须大于零。"); + } + VehicleId = vehicleId; +#pragma warning disable CS0612, CS0618 + var wheels = _chassis.GetSteerWheels(); +#pragma warning restore CS0612, CS0618 + + if (wheels.Count == 0) + { + throw new InvalidOperationException( + "MultiWheelChassis尚未完成舵轮初始化，" + + "不能创建底盘适配器。"); + } + // 禁用旧版DirectionAngle/ZeroDirection坐标偏置， + // 保证SendXYThSpeed直接使用真实车体坐标系。 + var maximumWheelRadiusMillimeters = 0.0; + var maximumLongitudinalOffsetMillimeters = 0.0; + var maximumLateralOffsetMillimeters = 0.0; + foreach (var wheel in wheels) + { + maximumWheelRadiusMillimeters = Math.Max( + maximumWheelRadiusMillimeters, + wheel.PhysicalPosition.Length()); + + maximumLongitudinalOffsetMillimeters = Math.Max( + maximumLongitudinalOffsetMillimeters, + Math.Abs(wheel.PhysicalPosition.X)); + + maximumLateralOffsetMillimeters = Math.Max( + maximumLateralOffsetMillimeters, + Math.Abs(wheel.PhysicalPosition.Y)); + } + + MaximumWheelRadiusMeters = + maximumWheelRadiusMillimeters / 1000.0; + HalfWheelBaseMeters = + maximumLongitudinalOffsetMillimeters / 1000.0; + HalfTrackWidthMeters = + maximumLateralOffsetMillimeters / 1000.0; + + if (MaximumWheelRadiusMeters <= 0.0 || + HalfWheelBaseMeters <= 0.0 || + HalfTrackWidthMeters <= 0.0) + { + throw new InvalidOperationException( + "Wheel positions cannot produce valid chassis dimensions."); + } + + // 通过反转轮速表达反向运动，避免蟹行正反切换时舵轮无意义地旋转180°。 + _chassis.PreferMinimumSteeringTravel = true; + } + + /// + /// 将车体坐标系速度命令发送给多舵轮底盘。 + /// + public bool Send(ChassisCommand command, TimeSpan? interval = null) + { + if (command.VehicleId != VehicleId) + { + throw new InvalidOperationException( + $"命令车辆编号{command.VehicleId}与适配器车辆编号" + + $"{VehicleId}不一致。"); + } + ValidateTwist(command.BodyTwist); + // 防止其他旧逻辑再次调用DirectionAngle或 + // SetOriginBias改变底盘坐标语义。 + EnsureBodyFrameIsActive(); + var vxMetersPerSecond = + (float)command.BodyTwist.VxMetersPerSecond; + var vyMetersPerSecond = + (float)command.BodyTwist.VyMetersPerSecond; + var omegaDegreesPerSecond = + (float)( + command.BodyTwist.OmegaRadiansPerSecond * + RadiansToDegrees); + var success = _chassis.SendXYThSpeed( + vxMetersPerSecond, + vyMetersPerSecond, + omegaDegreesPerSecond, + interval); + if (!success) + { + // 防止分解失败后继续执行上一条运动命令。 + _chassis.PredefinedDriveStop(); + } + return success; + } + + + /// + /// 在已经激活的运动坐标系中使用SendMotion执行虚拟阿克曼运动。 + /// 转向角均相对该运动坐标系表达；正90度运动系对应车体左侧蟹行。 + /// + public bool SendVirtualAckermannMotion( + double motionDirectionRadians, + double speedMetersPerSecond, + double steeringRadians, + TimeSpan? interval = null) + { + ValidateFinite( + motionDirectionRadians, + nameof(motionDirectionRadians)); + ValidateFinite( + speedMetersPerSecond, + nameof(speedMetersPerSecond)); + ValidateFinite( + steeringRadians, + nameof(steeringRadians)); + EnsureMotionFrameIsActive( + motionDirectionRadians); + + if (Math.Abs(steeringRadians) >= + Math.PI / 2.0) + { + throw new ArgumentOutOfRangeException( + nameof(steeringRadians), + "虚拟阿克曼转向角必须位于正负90度以内。"); + } + + var steeringDegrees = + (float)( + steeringRadians * + RadiansToDegrees); + var success = + _chassis.SendMotion( + (float)speedMetersPerSecond, + steeringDegrees, + -steeringDegrees, + interval); + + if (!success) + { + _chassis.PredefinedDriveStop(); + } + + return success; + } + + /// + /// 立即将所有驱动轮速度下发为零。 + /// + public void StopImmediately() + { + _chassis.PredefinedDriveStop(); + } + + /// + /// 清零XYTh驱动速度，但保留已经准备好的自转舵角和轮速方向。 + /// + public void StopXYThDrivePreserveSteeringState() + { + _chassis.StopXYThDrivePreserveSteeringState(); + } + + /// + /// 停车并将所有舵轮转到指定的车体角度。 + /// 只调整舵轮角度，不产生车辆线速度。 + /// + public bool PrepareParallelDirection( + double directionRadians) + { + EnsureBodyFrameIsActive(); + var targetDegrees = (float)(FrameTransform2D.NormalizeAngle(directionRadians) * + RadiansToDegrees); + +#pragma warning disable CS0612, CS0618 + var wheels = _chassis.GetSteerWheels(); +#pragma warning restore CS0612, CS0618 + + // 没有舵轮时不能认为预对齐成功。 + if (wheels.Count == 0) + { + return false; + } + + // 先检查所有舵轮能否到达目标机械角度。 + foreach (var wheel in wheels) + { + if (targetDegrees < wheel.AngleLowerLimit || + targetDegrees > wheel.AngleUpperLimit) + { + return false; + } + } + + // 模式切换前立即停止驱动轮。 + _chassis.PredefinedDriveStop(); + // 检查完成后再统一下发，避免只转动一部分舵轮。 + foreach (var wheel in wheels) + { + wheel.WriteAngle(targetDegrees); + } + + return true; + } + + /// + /// 检查所有舵轮是否已经对准给定方向。 + /// + public bool AreParallelWheelsAligned( + double directionRadians, + double toleranceRadians) + { + if (double.IsNaN(toleranceRadians) || + double.IsInfinity(toleranceRadians) || + toleranceRadians < 0.0) + { + throw new ArgumentOutOfRangeException( + nameof(toleranceRadians), + "舵轮到位容差必须是非负有限值。"); + } + + EnsureBodyFrameIsActive(); + var targetDegrees = (float)( + FrameTransform2D.NormalizeAngle(directionRadians) * + 180.0 / Math.PI); + + var toleranceDegrees = (float)( + Math.Abs(toleranceRadians) * + 180.0 / Math.PI); + +#pragma warning disable CS0612, CS0618 + var wheels = _chassis.GetSteerWheels(); +#pragma warning restore CS0612, CS0618 + + foreach (var wheel in wheels) + { + var angleErrorDegrees = targetDegrees - wheel.ReadAngle(); + + if (Math.Abs(angleErrorDegrees) > + toleranceDegrees) + { + return false; + } + } + + return true; + } + + /// + /// 停车并将舵轮预对齐到原地自转方向。 + /// 返回是否成功生成舵轮目标。 + /// + public bool PrepareSpin( + TimeSpan? interval = null) + { + EnsureBodyFrameIsActive(); + + var success = + _chassis.PrepareRotateWheels( + alignmentToleranceDegrees: 2.0f); + + if (!success) + { + _chassis.PredefinedDriveStop(); + } + return success; + } + + /// + /// 将已到位的自转舵角和轮速方向一次性交接给XYTh， + /// 防止普通SendXYThSpeed正式运动首帧重新初始化运动状态。 + /// + public bool AdoptPreparedSpinForXYTh( + double toleranceRadians = + 2.0 * Math.PI / 180.0) + { + if (double.IsNaN(toleranceRadians) || + double.IsInfinity(toleranceRadians) || + toleranceRadians < 0.0) + { + throw new ArgumentOutOfRangeException( + nameof(toleranceRadians), + "自转状态交接容差必须是非负有限值。"); + } + + EnsureBodyFrameIsActive(); + + var success = + _chassis + .AdoptPreparedRotateWheelsForXYTh( + (float)( + toleranceRadians * + RadiansToDegrees)); + + if (!success) + { + _chassis.PredefinedDriveStop(); + } + + return success; + } + /// + /// 所有舵轮是否已对齐到原地自转方向。 + /// + public bool AreSpinWheelsAligned => _chassis.LastRotateAligned; + + + + } +} diff --git a/Shared/Fleet/FleetKinematics.cs b/Shared/Fleet/FleetKinematics.cs new file mode 100644 index 0000000..46b6899 --- /dev/null +++ b/Shared/Fleet/FleetKinematics.cs @@ -0,0 +1 @@ +// 把车队整体速度分解为每辆车的局部速度 diff --git a/Shared/Mathematics/AngleMath.cs b/Shared/Mathematics/AngleMath.cs new file mode 100644 index 0000000..334fd2a --- /dev/null +++ b/Shared/Mathematics/AngleMath.cs @@ -0,0 +1,115 @@ +using System; + +namespace MyParking.Shared +{ + /// + /// 提供与坐标系无关的角度归一化、角度差和单位转换功能。 + /// + public static class AngleMath + { + public const double TwoPi = 2.0 * Math.PI; + + /// + /// 将弧度归一化到[-π, π)区间。 + /// -π包含在结果中，+π不包含在结果中，因此+π会返回-π。 + /// + public static double NormalizeRadians(double angleRadians) + { + EnsureFinite(angleRadians, nameof(angleRadians)); + + var normalized = angleRadians % TwoPi; + + if (normalized >= Math.PI) + { + normalized -= TwoPi; + } + else if (normalized < -Math.PI) + { + normalized += TwoPi; + } + + return normalized == 0.0 ? 0.0 : normalized; + } + + /// + /// 将角度归一化到[-180°, 180°)区间。 + /// -180°包含在结果中，+180°不包含在结果中，因此+180°会返回-180°。 + /// + public static double NormalizeDegrees(double angleDegrees) + { + EnsureFinite(angleDegrees, nameof(angleDegrees)); + + var normalized = angleDegrees % 360.0; + + if (normalized >= 180.0) + { + normalized -= 360.0; + } + else if (normalized < -180.0) + { + normalized += 360.0; + } + + return normalized == 0.0 ? 0.0 : normalized; + } + + /// + /// 计算从当前方向旋转到目标方向的最短有符号角度差，单位为弧度。 + /// 返回值位于[-π, π)；正值表示逆时针，负值表示顺时针。 + /// + public static double ShortestDifferenceRadians( + double targetRadians, + double currentRadians) + { + EnsureFinite(targetRadians, nameof(targetRadians)); + EnsureFinite(currentRadians, nameof(currentRadians)); + + return NormalizeRadians(targetRadians - currentRadians); + } + + /// + /// 计算从当前方向旋转到目标方向的最短有符号角度差，单位为度。 + /// 返回值位于[-180°, 180°)；正值表示逆时针，负值表示顺时针。 + /// + public static double ShortestDifferenceDegrees( + double targetDegrees, + double currentDegrees) + { + EnsureFinite(targetDegrees, nameof(targetDegrees)); + EnsureFinite(currentDegrees, nameof(currentDegrees)); + + return NormalizeDegrees(targetDegrees - currentDegrees); + } + + /// + /// 将角度从度转换为弧度，不进行归一化。 + /// + public static double DegreesToRadians(double angleDegrees) + { + EnsureFinite(angleDegrees, nameof(angleDegrees)); + return angleDegrees * Math.PI / 180.0; + } + + /// + /// 将角度从弧度转换为度，不进行归一化。 + /// + public static double RadiansToDegrees(double angleRadians) + { + EnsureFinite(angleRadians, nameof(angleRadians)); + return angleRadians * 180.0 / Math.PI; + } + + /// + /// 验证角度是可用于计算的有限数值。 + /// + private static void EnsureFinite(double angle, string parameterName) + { + if (double.IsNaN(angle) || double.IsInfinity(angle)) + { + throw new ArgumentOutOfRangeException( + parameterName, + "角度必须是有限数值。"); + } + } + } +} diff --git a/Shared/Mathematics/FrameTransform2D.cs b/Shared/Mathematics/FrameTransform2D.cs new file mode 100644 index 0000000..58e33db --- /dev/null +++ b/Shared/Mathematics/FrameTransform2D.cs @@ -0,0 +1,165 @@ +// 车体、运动、车队坐标系之间的转换 +using System; + +namespace MyParking.Shared +{ + /// + /// 提供二维刚体坐标系之间的点、向量、位姿和速度变换。 + /// 坐标系采用X向前、Y向左、逆时针为正的右手系。 + /// + public static class FrameTransform2D + { + /// + /// 将角度归一化到[-π, π)范围。 + /// + public static double NormalizeAngle(double angleRadians) + { + return AngleMath.NormalizeRadians(angleRadians); + } + + /// + /// 计算从current到target的最短角度差。 + /// 返回正值表示逆时针旋转。 + /// + public static double ShortestAngleDifference( + double targetRadians, + double currentRadians) + { + return AngleMath.ShortestDifferenceRadians( + targetRadians, + currentRadians); + } + + /// + /// 将源坐标系中的点变换到目标坐标系。 + /// sourcePoseInTarget表示源坐标系在目标坐标系中的位姿。 + /// + public static Point2D TransformPoint( + Pose2D sourcePoseInTarget, + Point2D pointInSource) + { + var cos = Math.Cos(sourcePoseInTarget.YawRadians); + var sin = Math.Sin(sourcePoseInTarget.YawRadians); + + return new Point2D( + sourcePoseInTarget.XMeters + + cos * pointInSource.XMeters - + sin * pointInSource.YMeters, + + sourcePoseInTarget.YMeters + + sin * pointInSource.XMeters + + cos * pointInSource.YMeters); + } + + /// + /// 将目标坐标系中的点反向变换到源坐标系。 + /// + public static Point2D InverseTransformPoint( + Pose2D sourcePoseInTarget, + Point2D pointInTarget) + { + var dx = pointInTarget.XMeters - sourcePoseInTarget.XMeters; + + var dy = pointInTarget.YMeters - sourcePoseInTarget.YMeters; + + var cos = Math.Cos(sourcePoseInTarget.YawRadians); + var sin = Math.Sin(sourcePoseInTarget.YawRadians); + + return new Point2D( + cos * dx + sin * dy, + -sin * dx + cos * dy); + } + + /// + /// 将源坐标系中的向量旋转到目标坐标系。 + /// 向量没有位置，因此不叠加平移量。 + /// + public static Point2D TransformVector( + Pose2D sourcePoseInTarget, + Point2D vectorInSource) + { + var cos = Math.Cos(sourcePoseInTarget.YawRadians); + var sin = Math.Sin(sourcePoseInTarget.YawRadians); + + return new Point2D( + cos * vectorInSource.XMeters - + sin * vectorInSource.YMeters, + + sin * vectorInSource.XMeters + + cos * vectorInSource.YMeters); + } + + /// + /// 组合两级坐标变换。 + /// parentFromMiddle表示middle在parent中的位姿； + /// middleFromChild表示child在middle中的位姿； + /// 返回child在parent中的位姿。 + /// + public static Pose2D Compose( + Pose2D parentFromMiddle, + Pose2D middleFromChild) + { + var childPositionInParent = TransformPoint( + parentFromMiddle, + middleFromChild.Position); + + return new Pose2D( + childPositionInParent.XMeters, + childPositionInParent.YMeters, + NormalizeAngle( + parentFromMiddle.YawRadians + + middleFromChild.YawRadians)); + } + + /// + /// 对坐标变换求逆。 + /// 输入child在parent中的位姿，返回parent在child中的位姿。 + /// + public static Pose2D Inverse(Pose2D childPoseInParent) + { + var cos = Math.Cos(childPoseInParent.YawRadians); + var sin = Math.Sin(childPoseInParent.YawRadians); + + return new Pose2D( + -cos * childPoseInParent.XMeters - + sin * childPoseInParent.YMeters, + + sin * childPoseInParent.XMeters - + cos * childPoseInParent.YMeters, + + NormalizeAngle( + -childPoseInParent.YawRadians)); + } + + + /// + /// 将源坐标系中的位姿变换到目标坐标系。 + /// + public static Pose2D TransformPose( + Pose2D sourcePoseInTarget, + Pose2D poseInSource) + { + return Compose(sourcePoseInTarget, poseInSource); + } + + /// + /// 转换同一物理参考点处的速度表达坐标系。 + /// 只旋转线速度，角速度保持不变。 + /// + public static Twist2D TransformTwistAtSamePoint( + Pose2D sourcePoseInTarget, + Twist2D twistInSource) + { + var linearVelocityInTarget = TransformVector( + sourcePoseInTarget, + new Point2D( + twistInSource.VxMetersPerSecond, + twistInSource.VyMetersPerSecond)); + + return new Twist2D( + linearVelocityInTarget.XMeters, + linearVelocityInTarget.YMeters, + twistInSource.OmegaRadiansPerSecond); + } + } +} diff --git a/Shared/Models/ChassisCommand.cs b/Shared/Models/ChassisCommand.cs new file mode 100644 index 0000000..5f073fd --- /dev/null +++ b/Shared/Models/ChassisCommand.cs @@ -0,0 +1,183 @@ +// 纯数据层：只描述坐标、速度和命令 +// 定义二维坐标、位姿、速度、车队布局和单车底盘命令。 +// Shared层统一使用SI单位：位置m、线速度m/s、角度rad、角速度rad/s。 +// 车体坐标系采用右手系：X向前、Y向左、逆时针角度和角速度为正。 +// 命名约定：XxxInYyy表示Xxx在Yyy坐标系中的表达。 + +namespace MyParking.Shared +{ + /// + /// 二维坐标点，X、Y单位均为米。 + /// + public readonly struct Point2D + { + public Point2D(double xMeters, double yMeters) + { + XMeters = xMeters; + YMeters = yMeters; + } + + public double XMeters { get; } + + public double YMeters { get; } + + public static Point2D Zero => new Point2D(0.0, 0.0); + } + + /// + /// 二维局部坐标系在父坐标系中的位姿。 + /// 位置单位为米，朝向单位为弧度，逆时针为正。 + /// 具体父子关系由变量名称说明，例如RadarPoseInBody。 + /// + public readonly struct Pose2D + { + public Pose2D( + double xMeters, + double yMeters, + double yawRadians) + { + XMeters = xMeters; + YMeters = yMeters; + YawRadians = yawRadians; + } + + public double XMeters { get; } + + public double YMeters { get; } + + public double YawRadians { get; } + + public Point2D Position => + new Point2D(XMeters, YMeters); + + public static Pose2D Identity => + new Pose2D(0.0, 0.0, 0.0); + } + + /// + /// 二维刚体速度。 + /// 线速度单位为m/s，角速度单位为rad/s。 + /// 速度所属坐标系由持有该Twist2D的外层类型或变量名称确定。 + /// + public readonly struct Twist2D + { + public Twist2D( + double vxMetersPerSecond, + double vyMetersPerSecond, + double omegaRadiansPerSecond) + { + VxMetersPerSecond = vxMetersPerSecond; + VyMetersPerSecond = vyMetersPerSecond; + OmegaRadiansPerSecond = omegaRadiansPerSecond; + } + + public double VxMetersPerSecond { get; } + + public double VyMetersPerSecond { get; } + + public double OmegaRadiansPerSecond { get; } + + public static Twist2D Zero => + new Twist2D(0.0, 0.0, 0.0); + } + + /// + /// 发送给单辆车的车体坐标系速度命令。 + /// + public readonly struct ChassisCommand + { + public ChassisCommand( + int vehicleId, + Twist2D bodyTwist) + { + VehicleId = vehicleId; + BodyTwist = bodyTwist; + } + + public int VehicleId { get; } + + /// + /// 单车车体坐标系速度：X向前、Y向左、逆时针旋转为正。 + /// + public Twist2D BodyTwist { get; } + + /// + /// 创建指定车辆的停止命令。 + /// + public static ChassisCommand Stop(int vehicleId) + { + return new ChassisCommand( + vehicleId, + Twist2D.Zero); + } + } + + /// + /// 单辆车的车体坐标系在车队坐标系中的位姿。 + /// + public readonly struct VehicleLayout + { + public VehicleLayout( + int vehicleId, + Pose2D poseInFleet) + { + VehicleId = vehicleId; + PoseInFleet = poseInFleet; + } + + public int VehicleId { get; } + + public Pose2D PoseInFleet { get; } + } + + /// + /// 车队整体运动命令，速度分量均在车队坐标系中表达。 + /// + public readonly struct FleetMotionCommand + { + public FleetMotionCommand( + Point2D referencePointInFleet, + Twist2D twistAtReferencePoint) + { + ReferencePointInFleet = referencePointInFleet; + TwistAtReferencePoint = twistAtReferencePoint; + } + + /// + /// 速度命令对应的参考点，也可作为自定义旋转中心。 + /// + public Point2D ReferencePointInFleet { get; } + + /// + /// 参考点处的车队速度。 + /// + public Twist2D TwistAtReferencePoint { get; } + + /// + /// 创建绕指定中心原地旋转的车队命令。 + /// + public static FleetMotionCommand RotateAround( + Point2D rotationCenterInFleet, + double omegaRadiansPerSecond) + { + return new FleetMotionCommand( + rotationCenterInFleet, + new Twist2D( + 0.0, + 0.0, + omegaRadiansPerSecond)); + } + + /// + /// 创建车队停止命令。 + /// + public static FleetMotionCommand Stop() + { + return new FleetMotionCommand( + Point2D.Zero, + Twist2D.Zero); + } + } + + +} \ No newline at end of file diff --git a/build-and-package.ps1 b/build-and-package.ps1 new file mode 100644 index 0000000..f0c3b76 --- /dev/null +++ b/build-and-package.ps1 @@ -0,0 +1,93 @@ +param( + [ValidateSet("Debug", "Release")] + [string]$Configuration = "Debug" +) + +$ErrorActionPreference = "Stop" + +$projectRoot = $PSScriptRoot +$commonUsageProject = Join-Path $projectRoot "CommonUsage-MultiVehicleSync\commonusage\CommonUsage.csproj" +$medullaProject = Join-Path $projectRoot "MedullaAdapter\MedullaAdapter.csproj" +$multiWheelProject = Join-Path $projectRoot "MultiWheelC\MultiWheelC.csproj" + +$commonUsageBuildDirectory = Join-Path $projectRoot "CommonUsage-MultiVehicleSync\commonusage\bin\$Configuration\netstandard2.0" +$commonUsageBuildDll = Join-Path $commonUsageBuildDirectory "CommonUsage.dll" +$commonUsageReferenceDll = Join-Path $projectRoot "ref\CommonUsage.dll" + +$medullaBuildDirectory = Join-Path $projectRoot "MedullaAdapter\build\Medulla\plugins" +$multiWheelBuildDirectory = Join-Path $projectRoot "MultiWheelC\build\Clumsy" + +$mOutputDirectory = Join-Path $projectRoot "output\M" +$cOutputDirectory = Join-Path $projectRoot "output\C" + +function Invoke-ProjectBuild { + param( + [Parameter(Mandatory = $true)] + [string]$ProjectPath + ) + + Write-Host "Building: $ProjectPath" -ForegroundColor Cyan + & dotnet build $ProjectPath --configuration $Configuration --no-restore + + if ($LASTEXITCODE -ne 0) { + throw "Build failed: $ProjectPath (dotnet exit code: $LASTEXITCODE)" + } +} + +function Assert-BuildFile { + param( + [Parameter(Mandatory = $true)] + [string]$Path + ) + + if (-not (Test-Path -LiteralPath $Path -PathType Leaf)) { + throw "Expected build file was not found: $Path" + } +} + +function Copy-BuildFile { + param( + [Parameter(Mandatory = $true)] + [string]$Source, + + [Parameter(Mandatory = $true)] + [string]$DestinationDirectory + ) + + Assert-BuildFile -Path $Source + Copy-Item -LiteralPath $Source -Destination $DestinationDirectory -Force + Write-Host "Copied: $Source -> $DestinationDirectory" -ForegroundColor DarkGray +} + +Write-Host "Building and packaging MyParking. Configuration: $Configuration" -ForegroundColor Green + +# CommonUsage must be built first and copied to ref for downstream projects. +Invoke-ProjectBuild -ProjectPath $commonUsageProject +Assert-BuildFile -Path $commonUsageBuildDll + +$referenceDirectory = Split-Path -Parent $commonUsageReferenceDll +New-Item -ItemType Directory -Path $referenceDirectory -Force | Out-Null +Copy-Item -LiteralPath $commonUsageBuildDll -Destination $commonUsageReferenceDll -Force +Write-Host "Updated CommonUsage reference: $commonUsageReferenceDll" -ForegroundColor Green + +# Build the M and C projects against the updated ref/CommonUsage.dll. +Invoke-ProjectBuild -ProjectPath $medullaProject +Invoke-ProjectBuild -ProjectPath $multiWheelProject + +New-Item -ItemType Directory -Path $mOutputDirectory -Force | Out-Null +New-Item -ItemType Directory -Path $cOutputDirectory -Force | Out-Null + +# Package the M-layer files. +Copy-BuildFile -Source (Join-Path $medullaBuildDirectory "MedullaAdapter.dll") -DestinationDirectory $mOutputDirectory +Copy-BuildFile -Source (Join-Path $medullaBuildDirectory "MedullaAdapter.pdb") -DestinationDirectory $mOutputDirectory +Copy-BuildFile -Source (Join-Path $medullaBuildDirectory "CommonUsage.dll") -DestinationDirectory $mOutputDirectory + +# Package the C-layer files. +Copy-BuildFile -Source (Join-Path $multiWheelBuildDirectory "MultiWheelC.dll") -DestinationDirectory $cOutputDirectory +Copy-BuildFile -Source (Join-Path $multiWheelBuildDirectory "MultiWheelC.pdb") -DestinationDirectory $cOutputDirectory +Copy-BuildFile -Source (Join-Path $multiWheelBuildDirectory "CommonUsage.dll") -DestinationDirectory $cOutputDirectory + +Write-Host "" +Write-Host "Build and packaging completed." -ForegroundColor Green +Write-Host "M output: $mOutputDirectory" +Write-Host "C output: $cOutputDirectory" diff --git a/chassis参考.json b/chassis参考.json new file mode 100644 index 0000000..4a35c9f --- /dev/null +++ b/chassis参考.json @@ -0,0 +1,50 @@ +{ + "WheelConfig": { + "LeftFront": { + "Position": { + "X": 525.0, + "Y": 200.0 + }, + "WheelDistance": 85.0, + "AngleLowerLimit": -128.0, + "AngleUpperLimit": 172.0, + "IsDiffWheel": true + }, + "RightFront": { + "Position": { + "X": 525.0, + "Y": -200.0 + }, + "WheelDistance": 85.0, + "AngleLowerLimit": -128.0, + "AngleUpperLimit": 172.0, + "IsDiffWheel": true + }, + "LeftRear": { + "Position": { + "X": -525.0, + "Y": 200.0 + }, + "WheelDistance": 85.0, + "AngleLowerLimit": -126.0, + "AngleUpperLimit": 174.0, + "IsDiffWheel": true + }, + "RightRear": { + "Position": { + "X": -525.0, + "Y": -200.0 + }, + "WheelDistance": 85.0, + "AngleLowerLimit": -130.0, + "AngleUpperLimit": 170.0, + "IsDiffWheel": true + } + }, + "MinimumTurningAngleForAckermann": 60.0, + "ControlPointRadius": 500.0, + "MaxSpeed": 1.0, + "AccPerSecond": 0.3, + "DeAccPerSecond": 1.0, + "MinTurnSpeedFac": 0.25 +} \ No newline at end of file diff --git a/data_process/__pycache__/plot_angular_command.cpython-312.pyc b/data_process/__pycache__/plot_angular_command.cpython-312.pyc new file mode 100644 index 0000000..1bca082 Binary files /dev/null and b/data_process/__pycache__/plot_angular_command.cpython-312.pyc differ diff --git a/data_process/__pycache__/plot_speed_response.cpython-312.pyc b/data_process/__pycache__/plot_speed_response.cpython-312.pyc new file mode 100644 index 0000000..e3b6740 Binary files /dev/null and b/data_process/__pycache__/plot_speed_response.cpython-312.pyc differ diff --git a/data_process/__pycache__/plot_tracking_errors.cpython-312.pyc b/data_process/__pycache__/plot_tracking_errors.cpython-312.pyc new file mode 100644 index 0000000..85fc360 Binary files /dev/null and b/data_process/__pycache__/plot_tracking_errors.cpython-312.pyc differ diff --git a/data_process/__pycache__/plot_trajectory_comparison.cpython-312.pyc b/data_process/__pycache__/plot_trajectory_comparison.cpython-312.pyc new file mode 100644 index 0000000..1c493af Binary files /dev/null and b/data_process/__pycache__/plot_trajectory_comparison.cpython-312.pyc differ diff --git a/data_process/__pycache__/run_all_plots.cpython-312.pyc b/data_process/__pycache__/run_all_plots.cpython-312.pyc new file mode 100644 index 0000000..bea76f9 Binary files /dev/null and b/data_process/__pycache__/run_all_plots.cpython-312.pyc differ diff --git a/data_process/电机响应处理/README.md b/data_process/电机响应处理/README.md new file mode 100644 index 0000000..a618f83 --- /dev/null +++ b/data_process/电机响应处理/README.md @@ -0,0 +1,28 @@ +# 舵轮转向响应处理 + +此工具读取 M 层 `StartWheelSpeedDiagnostic` / `StopWheelSpeedDiagnostic` 生成的 `*_snapshot.csv`，用于分析正常、蟹行、自转模式切换时四个舵轮的响应。 + +首次使用时安装依赖： + +```powershell +pip install -r requirements.txt +``` + +在本目录运行： + +```powershell +python .\plot_steering_response.py +``` + +默认选择 `MyParking\logs\wheel-speed` 中最新的快照 CSV，并在同级 `plots` 文件夹生成： + +- `*_steering_angles.png`：四轮目标角、实际角和 ±120°机械限位； +- `*_steering_error_pid.png`：四轮转角误差与 PID 输出； +- `*_motor_command_feedback.png`：八个电机的最终命令速度与 CAN 反馈速度； +- `*_mode_speed_limit.png`：正常/蟹行/自转模式变化和 `SendThresSpeed`。 + +也可以指定一个文件或局部时间范围： + +```powershell +python .\plot_steering_response.py --input "D:\\xxx_snapshot.csv" --from-seconds 2 --to-seconds 15 +``` diff --git a/data_process/电机响应处理/__pycache__/plot_steering_response.cpython-312.pyc b/data_process/电机响应处理/__pycache__/plot_steering_response.cpython-312.pyc new file mode 100644 index 0000000..8379bcd Binary files /dev/null and b/data_process/电机响应处理/__pycache__/plot_steering_response.cpython-312.pyc differ diff --git a/data_process/电机响应处理/plot_steering_response.py b/data_process/电机响应处理/plot_steering_response.py new file mode 100644 index 0000000..eeae425 --- /dev/null +++ b/data_process/电机响应处理/plot_steering_response.py @@ -0,0 +1,238 @@ +"""可视化停车机器人四舵轮的模式切换与转向响应。 + +默认读取 MyParking/logs/wheel-speed 中最新的 *_snapshot.csv，输出四张 PNG： +1. 四个舵轮的目标角、实际角和模式切换时刻； +2. 四个舵轮的转角误差； +3. 四个转向 PID 输出； +4. 八个电机的最终命令速度与 CAN 反馈速度。 + +示例： + python plot_steering_response.py + python plot_steering_response.py --input "D:\\logs\\xxx_snapshot.csv" +""" + +from __future__ import annotations + +import argparse +from pathlib import Path + +import matplotlib.pyplot as plt +import numpy as np +import pandas as pd + + +SCRIPT_DIRECTORY = Path(__file__).resolve().parent +PROJECT_DIRECTORY = SCRIPT_DIRECTORY.parent.parent +DEFAULT_LOG_DIRECTORY = PROJECT_DIRECTORY / "logs" / "wheel-speed" + +WHEELS = ( + ("LeftFront", "左前", "tab:blue"), + ("LeftRear", "左后", "tab:orange"), + ("RightFront", "右前", "tab:green"), + ("RightRear", "右后", "tab:red"), +) + +MOTORS = ( + ("LFL", "左前左"), ("LFR", "左前右"), + ("LRL", "左后左"), ("LRR", "左后右"), + ("RFL", "右前左"), ("RFR", "右前右"), + ("RRL", "右后左"), ("RRR", "右后右"), +) + +MODE_NAMES = {0: "正常", 1: "蟹行", 2: "自转"} + + +def parse_arguments() -> argparse.Namespace: + parser = argparse.ArgumentParser(description="绘制四舵轮模式切换响应图") + parser.add_argument("--input", type=Path, help="指定 *_snapshot.csv；缺省时取最新文件") + parser.add_argument("--output", type=Path, help="图片输出目录；缺省时写入本次日志同级 plots") + parser.add_argument("--from-seconds", type=float, default=0.0, help="从第几秒开始显示") + parser.add_argument("--to-seconds", type=float, help="显示到第几秒结束") + return parser.parse_args() + + +def find_snapshot(path: Path | None) -> Path: + if path is not None: + if not path.is_file(): + raise FileNotFoundError(f"找不到快照文件：{path}") + return path + + candidates = sorted( + DEFAULT_LOG_DIRECTORY.glob("*_snapshot.csv"), + key=lambda item: item.stat().st_mtime, + reverse=True, + ) + if not candidates: + raise FileNotFoundError( + f"{DEFAULT_LOG_DIRECTORY} 中没有 *_snapshot.csv。\n" + "请先在 M 层点击 StartWheelSpeedDiagnostic，完成模式切换后点击 StopWheelSpeedDiagnostic。" + ) + return candidates[0] + + +def load_snapshot(path: Path) -> pd.DataFrame: + frame = pd.read_csv(path, comment="#") + required = {"ElapsedMs", "ManualControlMode", "SendThresSpeed"} + missing = required.difference(frame.columns) + if missing: + raise ValueError(f"CSV 缺少字段：{', '.join(sorted(missing))}。请部署最新 MedullaAdapter.dll 后重新记录。") + + frame = frame.apply(pd.to_numeric, errors="coerce") + frame = frame.dropna(subset=["ElapsedMs"]).sort_values("ElapsedMs") + if frame.empty: + raise ValueError("CSV 中没有有效数据行。") + frame["ElapsedSeconds"] = (frame["ElapsedMs"] - frame["ElapsedMs"].iloc[0]) / 1000.0 + return frame + + +def crop(frame: pd.DataFrame, start: float, end: float | None) -> pd.DataFrame: + result = frame[frame["ElapsedSeconds"] >= start] + if end is not None: + result = result[result["ElapsedSeconds"] <= end] + if result.empty: + raise ValueError("所选时间范围内没有数据。") + return result + + +def require_columns(frame: pd.DataFrame, names: list[str]) -> None: + missing = [name for name in names if name not in frame.columns] + if missing: + raise ValueError("CSV 缺少字段：" + ", ".join(missing)) + + +def add_mode_markers(axis: plt.Axes, frame: pd.DataFrame) -> None: + modes = frame["ManualControlMode"].round().astype("Int64") + changes = modes.ne(modes.shift()) + for _, row in frame.loc[changes].iterrows(): + mode = int(row["ManualControlMode"]) + axis.axvline(row["ElapsedSeconds"], color="0.55", linestyle="--", linewidth=0.8, alpha=0.75) + axis.text( + row["ElapsedSeconds"], 0.99, MODE_NAMES.get(mode, f"模式{mode}"), + transform=axis.get_xaxis_transform(), rotation=90, + va="top", ha="right", fontsize=8, color="0.35", + ) + + +def save_steering_angle_plot(frame: pd.DataFrame, output: Path, prefix: str) -> None: + required = [] + for key, _, _ in WHEELS: + required.extend([f"TargetTh{key}", f"ActualTh{key}"]) + require_columns(frame, required) + + figure, axes = plt.subplots(2, 2, figsize=(14, 8), sharex=True) + for axis, (key, label, color) in zip(axes.flat, WHEELS): + time = frame["ElapsedSeconds"] + axis.plot(time, frame[f"TargetTh{key}"], label="目标角", color=color, linewidth=1.8) + axis.plot(time, frame[f"ActualTh{key}"], label="实际角", color="0.15", linewidth=1.1) + axis.axhline(120, color="tab:red", linestyle=":", linewidth=0.8, label="机械限位 ±120°") + axis.axhline(-120, color="tab:red", linestyle=":", linewidth=0.8) + add_mode_markers(axis, frame) + axis.set_title(f"{label}舵轮") + axis.set_ylabel("转角 (deg)") + axis.grid(alpha=0.25) + axis.legend(loc="best", fontsize=8) + for axis in axes[1]: + axis.set_xlabel("时间 (s)") + figure.suptitle("四舵轮目标转角与实际转角") + figure.tight_layout() + figure.savefig(output / f"{prefix}_steering_angles.png", dpi=180) + plt.close(figure) + + +def save_error_and_pid_plot(frame: pd.DataFrame, output: Path, prefix: str) -> None: + error_columns = [f"ErrorTh{key}" for key, _, _ in WHEELS] + pid_columns = [f"PidOut{key}" for key, _, _ in WHEELS] + require_columns(frame, error_columns + pid_columns) + + figure, axes = plt.subplots(2, 1, figsize=(14, 9), sharex=True) + time = frame["ElapsedSeconds"] + for key, label, color in WHEELS: + axes[0].plot(time, frame[f"ErrorTh{key}"], label=label, color=color, linewidth=1.2) + axes[1].plot(time, frame[f"PidOut{key}"], label=label, color=color, linewidth=1.2) + axes[0].axhline(2, color="0.4", linestyle=":", linewidth=0.9, label="到位阈值 ±2°") + axes[0].axhline(-2, color="0.4", linestyle=":", linewidth=0.9) + for axis in axes: + add_mode_markers(axis, frame) + axis.grid(alpha=0.25) + axis.legend(loc="best", ncol=3, fontsize=9) + axes[0].set_ylabel("目标角 - 实际角 (deg)") + axes[1].set_ylabel("转向 PID 输出 (m/s)") + axes[1].set_xlabel("时间 (s)") + figure.suptitle("转角误差与转向 PID 输出") + figure.tight_layout() + figure.savefig(output / f"{prefix}_steering_error_pid.png", dpi=180) + plt.close(figure) + + +def save_motor_speed_plot(frame: pd.DataFrame, output: Path, prefix: str) -> None: + command_columns = [f"Pid{name}" for name, _ in MOTORS] + feedback_columns = [f"Actual{name}" for name, _ in MOTORS] + require_columns(frame, command_columns + feedback_columns) + + figure, axes = plt.subplots(4, 2, figsize=(15, 12), sharex=True) + time = frame["ElapsedSeconds"] + for axis, (name, label) in zip(axes.flat, MOTORS): + axis.plot(time, frame[f"Pid{name}"], label="最终命令", color="tab:blue", linewidth=1.2) + axis.plot(time, frame[f"Actual{name}"], label="CAN反馈", color="tab:orange", linewidth=1.0) + add_mode_markers(axis, frame) + axis.set_title(f"{label}电机 ({name})") + axis.set_ylabel("速度 (m/s)") + axis.grid(alpha=0.25) + axis.legend(loc="best", fontsize=8) + for axis in axes[-1]: + axis.set_xlabel("时间 (s)") + figure.suptitle("八个电机最终速度命令与 CAN 实际速度反馈") + figure.tight_layout() + figure.savefig(output / f"{prefix}_motor_command_feedback.png", dpi=180) + plt.close(figure) + + +def save_summary_plot(frame: pd.DataFrame, output: Path, prefix: str) -> None: + require_columns(frame, ["SendThresSpeed"]) + figure, axes = plt.subplots(2, 1, figsize=(14, 7), sharex=True) + time = frame["ElapsedSeconds"] + axes[0].step(time, frame["ManualControlMode"], where="post", color="tab:purple", linewidth=1.5) + axes[0].set_yticks([0, 1, 2], ["正常", "蟹行", "自转"]) + axes[0].set_ylabel("控制模式") + axes[0].grid(alpha=0.25) + axes[1].plot(time, frame["SendThresSpeed"], color="tab:brown", linewidth=1.4, label="SendThresSpeed") + axes[1].set_ylabel("速度限幅 (m/s)") + axes[1].set_xlabel("时间 (s)") + axes[1].grid(alpha=0.25) + axes[1].legend(loc="best") + figure.suptitle("模式切换与整车下发速度限幅") + figure.tight_layout() + figure.savefig(output / f"{prefix}_mode_speed_limit.png", dpi=180) + plt.close(figure) + + +def print_parameter_summary(frame: pd.DataFrame) -> None: + parameter_names = [ + "DiffSteerKp", "DiffSteerKi", "DiffSteerKd", "DiffSteerMaxI", + "DiffSteerDeadZone", "DiffSteerThresh", "DiffSteerSpeedAcc", + ] + if not set(parameter_names).issubset(frame.columns): + return + print("本次记录的转向 PID 参数：") + print(" " + ", ".join(f"{name}={frame[name].iloc[0]:.6g}" for name in parameter_names)) + + +def main() -> None: + arguments = parse_arguments() + snapshot_path = find_snapshot(arguments.input) + frame = crop(load_snapshot(snapshot_path), arguments.from_seconds, arguments.to_seconds) + output_directory = arguments.output or snapshot_path.parent / "plots" + output_directory.mkdir(parents=True, exist_ok=True) + prefix = snapshot_path.name.removesuffix("_snapshot.csv") + + save_steering_angle_plot(frame, output_directory, prefix) + save_error_and_pid_plot(frame, output_directory, prefix) + save_motor_speed_plot(frame, output_directory, prefix) + save_summary_plot(frame, output_directory, prefix) + print_parameter_summary(frame) + print(f"已读取：{snapshot_path}") + print(f"已生成四张图：{output_directory}") + + +if __name__ == "__main__": + main() diff --git a/data_process/电机响应处理/requirements.txt b/data_process/电机响应处理/requirements.txt new file mode 100644 index 0000000..28058ed --- /dev/null +++ b/data_process/电机响应处理/requirements.txt @@ -0,0 +1,3 @@ +matplotlib>=3.7 +numpy>=1.24 +pandas>=2.0 diff --git a/data_process/轨迹测试处理/plot_angular_command.py b/data_process/轨迹测试处理/plot_angular_command.py new file mode 100644 index 0000000..4ddcded --- /dev/null +++ b/data_process/轨迹测试处理/plot_angular_command.py @@ -0,0 +1,100 @@ +"""绘制控制器下发角速度命令曲线。""" + +from __future__ import annotations + +import argparse +from pathlib import Path + +import matplotlib +matplotlib.use("Agg") +import matplotlib.pyplot as plt +import numpy as np + +from plot_trajectory_comparison import ( + configure_matplotlib, + discover_csv_files, + load_and_resample, + output_path, + shade_localization_jump_windows, +) + + +def plot_angular_command( + csv_path: Path, + frequency_hz: float, + filter_window_seconds: float, + output_directory: str | None, + show: bool, +) -> Path: + """生成单份CSV的命令角速度曲线。""" + frame, metadata = load_and_resample( + csv_path, + frequency_hz, + filter_window_seconds, + ) + time = frame["TimeSeconds"].to_numpy(dtype=float) + angular_command = frame[ + "CommandAngularSpeedRadPerSec" + ].to_numpy(dtype=float) + maximum = float(np.max(angular_command)) + minimum = float(np.min(angular_command)) + + fig, ax = plt.subplots(figsize=(10.0, 5.5)) + ax.plot( + time, + angular_command, + color="tab:red", + linewidth=1.6, + label="CommandAngularSpeed", + ) + ax.axhline(0.0, color="black", linewidth=0.8) + shade_localization_jump_windows(ax, metadata) + ax.set_xlabel("时间 / s") + ax.set_ylabel("命令角速度 / (rad/s)") + ax.set_title( + f"角速度指令曲线\n" + f"{metadata['controller_name']} - " + f"{metadata['trajectory_name']}，" + f"范围=[{minimum:.3f}, {maximum:.3f}]rad/s" + ) + ax.grid(True, alpha=0.3) + ax.legend() + fig.tight_layout() + + destination = output_path( + csv_path, + output_directory, + "angular_command", + ) + fig.savefig(destination, dpi=300, bbox_inches="tight") + if show: + plt.show() + plt.close(fig) + return destination + + +def main() -> None: + configure_matplotlib() + parser = argparse.ArgumentParser( + description="绘制控制器下发角速度命令曲线。" + ) + parser.add_argument("files", nargs="*", help="一个或多个CSV文件") + parser.add_argument("--frequency", type=float, default=20.0) + parser.add_argument("--window", type=float, default=0.55) + parser.add_argument("--output-dir") + parser.add_argument("--show", action="store_true") + args = parser.parse_args() + + for csv_path in discover_csv_files(args.files): + destination = plot_angular_command( + csv_path, + args.frequency, + args.window, + args.output_dir, + args.show, + ) + print(f"已生成：{destination}") + + +if __name__ == "__main__": + main() diff --git a/data_process/轨迹测试处理/plot_speed_response.py b/data_process/轨迹测试处理/plot_speed_response.py new file mode 100644 index 0000000..76759ea --- /dev/null +++ b/data_process/轨迹测试处理/plot_speed_response.py @@ -0,0 +1,177 @@ +"""绘制控制器参考速度与Detour差分实际速度对比图。""" + +from __future__ import annotations + +import argparse +from pathlib import Path + +import matplotlib +matplotlib.use("Agg") +import matplotlib.pyplot as plt +import numpy as np + +from plot_trajectory_comparison import ( + configure_matplotlib, + discover_csv_files, + load_and_resample, + output_path, + segmented_savgol, + shade_localization_jump_windows, +) + + +def calculate_actual_speed_mps( + frame, + filter_window_seconds: float, +) -> np.ndarray: + """使用Savitzky-Golay求位置导数并计算Detour实际合速度。""" + time = frame["TimeSeconds"].to_numpy(dtype=float) + dt = float(np.median(np.diff(time))) + # 直接对固定频率重采样后的位置做SG求导，避免“先平滑再求导” + # 造成两次滤波和过度削弱速度峰值。 + x_mm = frame["DetourXRawMm"].to_numpy(dtype=float) + y_mm = frame["DetourYRawMm"].to_numpy(dtype=float) + vx_mm_per_second = segmented_savgol( + x_mm, + dt, + filter_window_seconds, + derivative=1, + ) + vy_mm_per_second = segmented_savgol( + y_mm, + dt, + filter_window_seconds, + derivative=1, + ) + + speed = np.hypot( + vx_mm_per_second, + vy_mm_per_second, + ) / 1000.0 + speed[ + frame["InvalidNearLocalizationJump"].to_numpy(dtype=bool) + ] = np.nan + return speed + + +def plot_speed( + csv_path: Path, + frequency_hz: float, + filter_window_seconds: float, + output_directory: str | None, + show: bool, +) -> Path: + """生成单份CSV的参考/实际速度响应图。""" + frame, metadata = load_and_resample( + csv_path, + frequency_hz, + filter_window_seconds, + ) + time = frame["TimeSeconds"].to_numpy(dtype=float) + command_speed = frame["CommandSpeedMps"].to_numpy(dtype=float) + actual_speed = calculate_actual_speed_mps( + frame, + filter_window_seconds, + ) + is_in_place_rotation = ( + str(metadata["trajectory_name"]) + .lower() + .startswith("rotate") + ) + # 原地自转CSV中的ReferenceSpeed历史上保存的是角速度上限deg/s， + # 不能作为线速度m/s使用；其参考线速度应为0。 + configured_speed = ( + 0.0 + if is_in_place_rotation + else float(metadata["reference_speed_mps"]) + ) + + moving = ( + (command_speed > max(0.02, configured_speed * 0.1)) & + np.isfinite(actual_speed) + ) + if np.any(moving): + speed_rmse = float( + np.sqrt( + np.mean( + (actual_speed[moving] - command_speed[moving]) ** 2 + ) + ) + ) + else: + speed_rmse = float("nan") + + fig, ax = plt.subplots(figsize=(10.0, 5.8)) + ax.plot( + time, + command_speed, + linewidth=1.8, + label="控制器参考/下发线速度", + ) + ax.plot( + time, + actual_speed, + linewidth=1.5, + label="Detour差分实际线速度（SG求导）", + ) + ax.axhline( + configured_speed, + linestyle=":", + linewidth=1.3, + color="tab:green", + label=( + "原地自转参考线速度 0 m/s" + if is_in_place_rotation + else f"配置巡航速度 {configured_speed:.3f} m/s" + ), + ) + shade_localization_jump_windows(ax, metadata) + ax.set_xlabel("时间 / s") + ax.set_ylabel("线速度 / (m/s)") + ax.set_title( + f"参考速度与实际速度对比\n" + f"{metadata['controller_name']} - " + f"{metadata['trajectory_name']}，" + f"运动段RMSE={speed_rmse:.4f} m/s" + ) + ax.grid(True, alpha=0.3) + ax.legend() + fig.tight_layout() + + destination = output_path( + csv_path, + output_directory, + "speed_response", + ) + fig.savefig(destination, dpi=300, bbox_inches="tight") + if show: + plt.show() + plt.close(fig) + return destination + + +def main() -> None: + configure_matplotlib() + parser = argparse.ArgumentParser( + description="绘制参考速度与Detour差分实际速度对比图。" + ) + parser.add_argument("files", nargs="*", help="一个或多个CSV文件") + parser.add_argument("--frequency", type=float, default=20.0) + parser.add_argument("--window", type=float, default=0.55) + parser.add_argument("--output-dir") + parser.add_argument("--show", action="store_true") + args = parser.parse_args() + + for csv_path in discover_csv_files(args.files): + destination = plot_speed( + csv_path, + args.frequency, + args.window, + args.output_dir, + args.show, + ) + print(f"已生成：{destination}") + + +if __name__ == "__main__": + main() diff --git a/data_process/轨迹测试处理/plot_tracking_errors.py b/data_process/轨迹测试处理/plot_tracking_errors.py new file mode 100644 index 0000000..1b77a27 --- /dev/null +++ b/data_process/轨迹测试处理/plot_tracking_errors.py @@ -0,0 +1,172 @@ +"""绘制横向误差和航向误差随时间变化图。""" + +from __future__ import annotations + +import argparse +from pathlib import Path + +import matplotlib +matplotlib.use("Agg") +import matplotlib.pyplot as plt +import numpy as np + +from plot_trajectory_comparison import ( + build_reference, + configure_matplotlib, + discover_csv_files, + load_and_resample, + output_path, + shade_localization_jump_windows, +) + + +def plot_errors( + csv_path: Path, + frequency_hz: float, + filter_window_seconds: float, + output_directory: str | None, + show: bool, +) -> Path: + """生成单份CSV的横向/航向误差图。""" + frame, metadata = load_and_resample( + csv_path, + frequency_hz, + filter_window_seconds, + ) + reference = build_reference(frame, metadata) + time = frame["TimeSeconds"].to_numpy() + lateral = np.asarray(reference["lateral_error_mm"]) + heading = np.asarray(reference["heading_error_degrees"]) + invalid = frame[ + "InvalidNearLocalizationJump" + ].to_numpy(dtype=bool) + lateral_for_statistics = lateral.copy() + heading_for_statistics = heading.copy() + lateral_for_statistics[invalid] = np.nan + heading_for_statistics[invalid] = np.nan + + lateral_rmse = float( + np.sqrt(np.nanmean(lateral_for_statistics**2)) + ) + heading_rmse = float( + np.sqrt(np.nanmean(heading_for_statistics**2)) + ) + lateral_max = float( + np.nanmax(np.abs(lateral_for_statistics)) + ) + heading_max = float( + np.nanmax(np.abs(heading_for_statistics)) + ) + is_in_place_rotation = ( + reference["kind"] == "in_place_rotation" + ) + + fig, axes = plt.subplots( + 2, + 1, + figsize=(10.0, 7.0), + sharex=True, + ) + axes[0].plot(time, lateral, linewidth=1.5) + axes[0].axhline(0.0, color="black", linewidth=0.8) + if is_in_place_rotation: + axes[0].set_ylabel("旋转中心位置漂移 / mm") + axes[0].set_title( + f"原地自转位置漂移：RMS={lateral_rmse:.2f} mm，" + f"最大值={lateral_max:.2f} mm" + ) + else: + axes[0].set_ylabel("横向误差 / mm") + axes[0].set_title( + f"横向误差：RMSE={lateral_rmse:.2f} mm，" + f"最大绝对值={lateral_max:.2f} mm" + ) + shade_localization_jump_windows(axes[0], metadata) + axes[0].grid(True, alpha=0.3) + + axes[1].plot( + time, + heading, + color="tab:orange", + linewidth=1.5, + ) + axes[1].axhline(0.0, color="black", linewidth=0.8) + axes[1].set_xlabel("时间 / s") + axes[1].set_ylabel( + "目标角度剩余误差 / °" + if is_in_place_rotation + else "航向误差 / °" + ) + axes[1].set_title( + ( + f"目标角度剩余误差：RMSE={heading_rmse:.2f}°，" + f"最大绝对值={heading_max:.2f}°" + ) + if is_in_place_rotation + else ( + f"航向误差：RMSE={heading_rmse:.2f}°，" + f"最大绝对值={heading_max:.2f}°" + ) + ) + shade_localization_jump_windows(axes[1], metadata) + axes[1].grid(True, alpha=0.3) + if metadata["localization_jump_events"]: + axes[1].legend(loc="best") + + fig.suptitle( + f"横向/航向误差随时间变化\n" + f"{metadata['controller_name']} - " + f"{metadata['trajectory_name']}" + ) + fig.tight_layout() + + destination = output_path( + csv_path, + output_directory, + "tracking_errors", + ) + fig.savefig(destination, dpi=300, bbox_inches="tight") + if show: + plt.show() + plt.close(fig) + + if is_in_place_rotation: + print( + f"{csv_path.name}: position drift RMS=" + f"{lateral_rmse:.3f} mm, " + f"target-angle error RMS={heading_rmse:.3f} deg" + ) + else: + print( + f"{csv_path.name}: lateral RMSE=" + f"{lateral_rmse:.3f} mm, " + f"heading RMSE={heading_rmse:.3f} deg" + ) + return destination + + +def main() -> None: + configure_matplotlib() + parser = argparse.ArgumentParser( + description="绘制横向误差和航向误差随时间变化图。" + ) + parser.add_argument("files", nargs="*", help="一个或多个CSV文件") + parser.add_argument("--frequency", type=float, default=20.0) + parser.add_argument("--window", type=float, default=0.55) + parser.add_argument("--output-dir") + parser.add_argument("--show", action="store_true") + args = parser.parse_args() + + for csv_path in discover_csv_files(args.files): + destination = plot_errors( + csv_path, + args.frequency, + args.window, + args.output_dir, + args.show, + ) + print(f"已生成：{destination}") + + +if __name__ == "__main__": + main() diff --git a/data_process/轨迹测试处理/plot_trajectory_comparison.py b/data_process/轨迹测试处理/plot_trajectory_comparison.py new file mode 100644 index 0000000..8f2b85c --- /dev/null +++ b/data_process/轨迹测试处理/plot_trajectory_comparison.py @@ -0,0 +1,920 @@ +"""绘制理想轨迹与Detour实际轨迹对比图。 + +不传CSV路径时，默认处理本脚本目录下的全部CSV文件。 +本文件也提供其余三个绘图脚本共用的数据预处理函数。 +""" + +from __future__ import annotations + +import argparse +import re +from pathlib import Path +from typing import Any + +import matplotlib +matplotlib.use("Agg") +import matplotlib.pyplot as plt +import numpy as np +import pandas as pd +from scipy.signal import savgol_filter + + +SCRIPT_DIR = Path(__file__).resolve().parent +REQUIRED_COLUMNS = { + "ElapsedSeconds", + "TrajectoryName", + "DetourX", + "DetourY", + "DetourTheta", + "CommandSpeed", + "CommandAngularSpeed", + "ReferenceStartX", + "ReferenceStartY", + "ReferenceEndX", + "ReferenceEndY", + "ReferenceSpeed", +} + + +def configure_matplotlib() -> None: + """配置中文字体和图片输出风格。""" + matplotlib.rcParams["font.sans-serif"] = [ + "Microsoft YaHei", + "SimHei", + "Arial Unicode MS", + "DejaVu Sans", + ] + matplotlib.rcParams["axes.unicode_minus"] = False + matplotlib.rcParams["figure.dpi"] = 120 + + +def _odd_window_length( + sample_count: int, + sample_interval: float, + window_seconds: float, + polynomial_order: int = 2, +) -> int | None: + """计算不超过数据长度的Savitzky-Golay奇数窗口。""" + requested = max( + polynomial_order + 2, + int(round(window_seconds / sample_interval)), + ) + if requested % 2 == 0: + requested += 1 + + maximum = sample_count if sample_count % 2 == 1 else sample_count - 1 + window = min(requested, maximum) + minimum = polynomial_order + 2 + if minimum % 2 == 0: + minimum += 1 + + return window if window >= minimum else None + + +def wrap_degrees(angle_degrees: np.ndarray) -> np.ndarray: + """将角度差归一化到[-180°, 180°)。""" + return (angle_degrees + 180.0) % 360.0 - 180.0 + + +def build_complete_s_curve( + start: np.ndarray, + end: np.ndarray, + offset_mm: float, + samples_per_segment: int = 120, +) -> tuple[np.ndarray, np.ndarray]: + """重建测试使用的三段三次贝塞尔完整S曲线及各点切线航向。""" + line = end - start + length = float(np.linalg.norm(line)) + if length <= 1e-6: + raise ValueError("S型曲线的起点和终点不能重合。") + + forward = line / length + left = np.array([-forward[1], forward[0]]) + controls = [ + np.array([ + [0.0, 0.0], + [length / 12.0, 0.0], + [length / 6.0, offset_mm], + [length * 0.25, offset_mm], + ]), + np.array([ + [length * 0.25, offset_mm], + [length / 3.0, offset_mm], + [length * 2.0 / 3.0, -offset_mm], + [length * 0.75, -offset_mm], + ]), + np.array([ + [length * 0.75, -offset_mm], + [length * 5.0 / 6.0, -offset_mm], + [length * 11.0 / 12.0, 0.0], + [length, 0.0], + ]), + ] + + local_parts: list[np.ndarray] = [] + derivative_parts: list[np.ndarray] = [] + for index, points in enumerate(controls): + t = np.linspace(0.0, 1.0, samples_per_segment + 1) + if index > 0: + t = t[1:] + one_minus_t = 1.0 - t + local = ( + one_minus_t[:, None] ** 3 * points[0] + + 3.0 + * one_minus_t[:, None] ** 2 + * t[:, None] + * points[1] + + 3.0 + * one_minus_t[:, None] + * t[:, None] ** 2 + * points[2] + + t[:, None] ** 3 * points[3] + ) + derivative = ( + 3.0 + * one_minus_t[:, None] ** 2 + * (points[1] - points[0]) + + 6.0 + * one_minus_t[:, None] + * t[:, None] + * (points[2] - points[1]) + + 3.0 + * t[:, None] ** 2 + * (points[3] - points[2]) + ) + local_parts.append(local) + derivative_parts.append(derivative) + + local_points = np.vstack(local_parts) + local_derivatives = np.vstack(derivative_parts) + world_points = ( + start + + local_points[:, 0, None] * forward + + local_points[:, 1, None] * left + ) + world_derivatives = ( + local_derivatives[:, 0, None] * forward + + local_derivatives[:, 1, None] * left + ) + headings = np.rad2deg( + np.arctan2(world_derivatives[:, 1], world_derivatives[:, 0]) + ) + return world_points, headings + + +def segmented_savgol( + values: np.ndarray, + sample_interval: float, + window_seconds: float, + derivative: int = 0, + polynomial_order: int = 2, +) -> np.ndarray: + """对含NaN断点的数据逐段执行SG滤波或求导。""" + values = np.asarray(values, dtype=float) + result = np.full_like(values, np.nan) + finite_indices = np.flatnonzero(np.isfinite(values)) + if finite_indices.size == 0: + return result + + breaks = np.flatnonzero(np.diff(finite_indices) > 1) + starts = np.r_[0, breaks + 1] + ends = np.r_[breaks + 1, finite_indices.size] + + for start_index, end_index in zip(starts, ends): + indices = finite_indices[start_index:end_index] + segment = values[indices] + window = _odd_window_length( + len(segment), + sample_interval, + window_seconds, + polynomial_order, + ) + if window is not None: + result[indices] = savgol_filter( + segment, + window, + polynomial_order, + deriv=derivative, + delta=sample_interval, + mode="interp", + ) + elif derivative == 0: + result[indices] = segment + elif len(segment) >= 2: + result[indices] = np.gradient(segment, sample_interval) + + return result + + +def shade_localization_jump_windows( + axis, + metadata: dict[str, Any], +) -> None: + """在时间曲线中标记不应参与车辆动力学评价的定位跳变窗口。""" + for index, (start, end) in enumerate( + metadata["jump_exclusion_windows"] + ): + axis.axvspan( + start, + end, + color="tab:red", + alpha=0.12, + label="Detour定位跳变排除窗口" if index == 0 else None, + ) + + +def load_and_resample( + csv_path: Path, + frequency_hz: float = 20.0, + filter_window_seconds: float = 0.55, +) -> tuple[pd.DataFrame, dict[str, Any]]: + """压缩Detour保持帧，检测定位跳变，再分段重采样和平滑。""" + if not np.isfinite(frequency_hz) or frequency_hz <= 0.0: + raise ValueError("重采样频率必须是正有限值。") + + raw = pd.read_csv(csv_path) + missing = REQUIRED_COLUMNS.difference(raw.columns) + if missing: + raise ValueError( + f"{csv_path.name}缺少列：{', '.join(sorted(missing))}" + ) + + numeric_columns = [ + "ElapsedSeconds", + "DetourX", + "DetourY", + "DetourTheta", + "CommandSpeed", + "CommandAngularSpeed", + "ReferenceStartX", + "ReferenceStartY", + "ReferenceEndX", + "ReferenceEndY", + "ReferenceSpeed", + ] + optional_numeric_columns = [ + "CommandAngularSpeedRadPerSecond", + "ReferenceAngularSpeedRadPerSecond", + "ReferenceMotionFrameYawDegrees", + ] + numeric_columns.extend( + column + for column in optional_numeric_columns + if column in raw.columns + ) + for column in numeric_columns: + raw[column] = pd.to_numeric(raw[column], errors="coerce") + + raw = ( + raw.dropna(subset=[ + "ElapsedSeconds", + "DetourX", + "DetourY", + "DetourTheta", + ]) + .sort_values("ElapsedSeconds") + .drop_duplicates("ElapsedSeconds", keep="last") + .reset_index(drop=True) + ) + if len(raw) < 5: + raise ValueError(f"{csv_path.name}有效数据不足5行。") + + time_raw = raw["ElapsedSeconds"].to_numpy(dtype=float) + time_raw = time_raw - time_raw[0] + raw["ElapsedSeconds"] = time_raw + duration = float(time_raw[-1]) + sample_interval = 1.0 / frequency_hz + time_uniform = np.arange( + 0.0, + duration + sample_interval * 0.5, + sample_interval, + ) + + def interpolate_command(column: str) -> np.ndarray: + values = raw[column].to_numpy(dtype=float) + return np.interp(time_uniform, time_raw, values) + + # 记录器频率高于Detour更新频率，会得到A,A,B,B形式的保持帧。 + # 速度估计前先保留真正发生位姿更新的样本。 + x_all = raw["DetourX"].to_numpy(dtype=float) + y_all = raw["DetourY"].to_numpy(dtype=float) + theta_all = raw["DetourTheta"].to_numpy(dtype=float) + position_change = np.hypot(np.diff(x_all), np.diff(y_all)) + heading_change = np.abs(wrap_degrees(np.diff(theta_all))) + update_mask = np.r_[ + True, + (position_change > 1e-6) | (heading_change > 1e-6), + ] + updates = raw.loc[update_mask].copy().reset_index(drop=True) + if len(updates) < 3: + raise ValueError(f"{csv_path.name}有效Detour更新点不足3个。") + + update_time = updates["ElapsedSeconds"].to_numpy(dtype=float) + update_x = updates["DetourX"].to_numpy(dtype=float) + update_y = updates["DetourY"].to_numpy(dtype=float) + update_theta = updates["DetourTheta"].to_numpy(dtype=float) + update_command_speed = np.abs( + updates["CommandSpeed"].to_numpy(dtype=float) + ) + if "CommandAngularSpeedRadPerSecond" in updates.columns: + update_command_angular_rad = np.abs( + updates[ + "CommandAngularSpeedRadPerSecond" + ].to_numpy(dtype=float) + ) + else: + # 旧CSV中的CommandAngularSpeed单位为deg/s。 + update_command_angular_rad = np.deg2rad( + np.abs( + updates[ + "CommandAngularSpeed" + ].to_numpy(dtype=float) + ) + ) + + # 自适应跳变阈值：正常移动允许达到参考位移的3倍并保留15mm余量； + # 低速阶段仍至少允许30mm，防止把普通定位噪声误判为跳变。 + update_dt = np.diff(update_time) + update_distance = np.hypot(np.diff(update_x), np.diff(update_y)) + expected_distance = ( + 0.5 * + (update_command_speed[1:] + update_command_speed[:-1]) * + update_dt * + 1000.0 + ) + distance_threshold = np.maximum( + 30.0, + expected_distance * 3.0 + 15.0, + ) + update_heading_delta = np.abs( + wrap_degrees(np.diff(update_theta)) + ) + expected_heading_delta = ( + 0.5 * + ( + update_command_angular_rad[1:] + + update_command_angular_rad[:-1] + ) * + update_dt * + 180.0 / np.pi + ) + heading_threshold = np.maximum( + 5.0, + expected_heading_delta * 3.0 + 2.0, + ) + jump_before_current = ( + (update_distance > distance_threshold) | + (update_heading_delta > heading_threshold) + ) + jump_at_update = np.r_[False, jump_before_current] + segment_ids = np.cumsum(jump_at_update.astype(int)) + + jump_events: list[dict[str, float]] = [] + for current_index in np.flatnonzero(jump_at_update): + previous_index = current_index - 1 + jump_events.append({ + "time_seconds": float(update_time[current_index]), + "distance_mm": float(update_distance[previous_index]), + "heading_change_degrees": + float(update_heading_delta[previous_index]), + "before_x_mm": float(update_x[previous_index]), + "before_y_mm": float(update_y[previous_index]), + "after_x_mm": float(update_x[current_index]), + "after_y_mm": float(update_y[current_index]), + }) + + # 不跨越定位跳变插值。跳变前后之间保留NaN，使轨迹图自然断线， + # 也防止SG滤波把坐标修正涂抹成车辆高速运动。 + x_resampled = np.full_like(time_uniform, np.nan) + y_resampled = np.full_like(time_uniform, np.nan) + theta_resampled = np.full_like(time_uniform, np.nan) + update_theta_unwrapped = np.rad2deg( + np.unwrap(np.deg2rad(update_theta)) + ) + maximum_segment_id = int(segment_ids[-1]) + for segment_id in range(maximum_segment_id + 1): + segment_mask = segment_ids == segment_id + segment_time = update_time[segment_mask] + if segment_time.size == 0: + continue + + interval_start = ( + 0.0 if segment_id == 0 else float(segment_time[0]) + ) + interval_end = ( + duration + if segment_id == maximum_segment_id + else float(segment_time[-1]) + ) + uniform_mask = ( + (time_uniform >= interval_start) & + (time_uniform <= interval_end) + ) + x_resampled[uniform_mask] = np.interp( + time_uniform[uniform_mask], + segment_time, + update_x[segment_mask], + ) + y_resampled[uniform_mask] = np.interp( + time_uniform[uniform_mask], + segment_time, + update_y[segment_mask], + ) + theta_resampled[uniform_mask] = np.interp( + time_uniform[uniform_mask], + segment_time, + update_theta_unwrapped[segment_mask], + ) + + x_filtered = segmented_savgol( + x_resampled, + sample_interval, + filter_window_seconds, + ) + y_filtered = segmented_savgol( + y_resampled, + sample_interval, + filter_window_seconds, + ) + theta_filtered = segmented_savgol( + theta_resampled, + sample_interval, + filter_window_seconds, + ) + + exclusion_half_width = max( + 0.30, + filter_window_seconds * 0.5, + ) + jump_exclusion_windows = [ + ( + max(0.0, event["time_seconds"] - exclusion_half_width), + min(duration, event["time_seconds"] + exclusion_half_width), + ) + for event in jump_events + ] + invalid_near_jump = np.zeros(len(time_uniform), dtype=bool) + for start, end in jump_exclusion_windows: + invalid_near_jump |= ( + (time_uniform >= start) & (time_uniform <= end) + ) + + if "CommandAngularSpeedRadPerSecond" in raw.columns: + angular_command_rad = interpolate_command( + "CommandAngularSpeedRadPerSecond" + ) + else: + angular_command_rad = np.deg2rad( + interpolate_command("CommandAngularSpeed") + ) + + frame = pd.DataFrame({ + "TimeSeconds": time_uniform, + "DetourXRawMm": x_resampled, + "DetourYRawMm": y_resampled, + "DetourXFilteredMm": x_filtered, + "DetourYFilteredMm": y_filtered, + "DetourThetaUnwrappedDeg": theta_filtered, + "DetourThetaDeg": wrap_degrees(theta_filtered), + "CommandSpeedMps": interpolate_command("CommandSpeed"), + "CommandAngularSpeedRadPerSec": + angular_command_rad, + "InvalidNearLocalizationJump": invalid_near_jump, + }) + + first = raw.iloc[0] + metadata: dict[str, Any] = { + "csv_path": csv_path, + "trajectory_name": str(first["TrajectoryName"]), + "controller_name": str(first.get("ControllerName", "")), + "trial_number": str(first.get("TrialNumber", "")), + # 蟹行轨迹的运动前向相对车体X轴逆时针偏置90°。 + # DetourTheta始终是车体航向，计算航向误差时必须扣除该偏置。 + "motion_frame_yaw_degrees": float( + first["ReferenceMotionFrameYawDegrees"] + if ( + "ReferenceMotionFrameYawDegrees" in raw.columns + and pd.notna( + first["ReferenceMotionFrameYawDegrees"] + ) + ) + else ( + 90.0 + if "crab" in ( + str(first["TrajectoryName"]) + + str(first.get("ControllerName", "")) + ).lower() + else 0.0 + ) + ), + "reference_start_mm": np.array( + [first["ReferenceStartX"], first["ReferenceStartY"]], + dtype=float, + ), + "reference_end_mm": np.array( + [first["ReferenceEndX"], first["ReferenceEndY"]], + dtype=float, + ), + "reference_speed_mps": float(first["ReferenceSpeed"]), + "reference_angular_speed_rad_per_second": float( + first.get( + "ReferenceAngularSpeedRadPerSecond", + 0.0, + ) + ), + # 圆弧构造时使用了测试开始处Detour航向，因此这里取首帧航向。 + "start_heading_degrees": float(first["DetourTheta"]), + "sample_interval_seconds": sample_interval, + "filter_window_seconds": filter_window_seconds, + "raw_sample_count": len(raw), + "detour_update_count": len(updates), + "held_sample_count": int(len(raw) - len(updates)), + "localization_jump_events": jump_events, + "jump_exclusion_windows": jump_exclusion_windows, + } + return frame, metadata + + +def build_reference( + frame: pd.DataFrame, + metadata: dict[str, Any], +) -> dict[str, np.ndarray | float | str]: + """根据CSV元数据建立直线、圆弧、完整S曲线或原地自转参考及误差。""" + trajectory_name = str(metadata["trajectory_name"]) + start = np.asarray(metadata["reference_start_mm"], dtype=float) + end = np.asarray(metadata["reference_end_mm"], dtype=float) + motion_frame_yaw_degrees = float( + metadata.get("motion_frame_yaw_degrees", 0.0) + ) + actual = frame[ + ["DetourXFilteredMm", "DetourYFilteredMm"] + ].to_numpy(dtype=float) + actual_heading = frame["DetourThetaUnwrappedDeg"].to_numpy(dtype=float) + + radius_match = re.search( + r"LeftArc(?P[0-9.]+)_R(?P[0-9.]+)mm", + trajectory_name, + flags=re.IGNORECASE, + ) + if radius_match: + radius = float(radius_match.group("radius")) + sweep_degrees = float(radius_match.group("sweep")) + start_body_heading = float(metadata["start_heading_degrees"]) + start_motion_heading = ( + start_body_heading + motion_frame_yaw_degrees + ) + heading_radians = np.deg2rad(start_motion_heading) + center = start + radius * np.array( + [-np.sin(heading_radians), np.cos(heading_radians)] + ) + start_radial_degrees = start_motion_heading - 90.0 + + radial = actual - center + distance_to_center = np.linalg.norm(radial, axis=1) + radial_angle_degrees = np.rad2deg( + np.arctan2(radial[:, 1], radial[:, 0]) + ) + radial_angle_radians = np.deg2rad(radial_angle_degrees) + reference_points = center + radius * np.column_stack([ + np.cos(radial_angle_radians), + np.sin(radial_angle_radians), + ]) + # 对逆时针圆弧，正横向误差表示车辆位于轨迹左侧（圆内侧）。 + lateral_error = radius - distance_to_center + reference_motion_heading = radial_angle_degrees + 90.0 + reference_heading = ( + reference_motion_heading - motion_frame_yaw_degrees + ) + heading_error = wrap_degrees( + actual_heading - reference_heading + ) + + plot_angles = np.deg2rad( + np.linspace( + start_radial_degrees, + start_radial_degrees + sweep_degrees, + 361, + ) + ) + ideal_plot = center + radius * np.column_stack([ + np.cos(plot_angles), + np.sin(plot_angles), + ]) + return { + "kind": "left_arc", + "ideal_plot_mm": ideal_plot, + "reference_points_mm": reference_points, + "reference_heading_degrees": reference_heading, + "reference_motion_heading_degrees": + reference_motion_heading, + "lateral_error_mm": lateral_error, + "heading_error_degrees": heading_error, + "center_mm": center, + "radius_mm": radius, + } + + s_curve_match = re.search( + r"SCurve(?P[0-9.]+)m_A(?P[0-9.]+)mm", + trajectory_name, + flags=re.IGNORECASE, + ) + if s_curve_match: + offset_mm = float(s_curve_match.group("offset")) + ideal_plot, ideal_heading = build_complete_s_curve( + start, + end, + offset_mm, + ) + delta = actual[:, np.newaxis, :] - ideal_plot[np.newaxis, :, :] + nearest_indices = np.argmin( + np.sum(delta * delta, axis=2), + axis=1, + ) + reference_points = ideal_plot[nearest_indices] + reference_motion_heading = ideal_heading[nearest_indices] + reference_heading = ( + reference_motion_heading - motion_frame_yaw_degrees + ) + heading_radians = np.deg2rad(reference_motion_heading) + left_normals = np.column_stack([ + -np.sin(heading_radians), + np.cos(heading_radians), + ]) + lateral_error = np.sum( + (actual - reference_points) * left_normals, + axis=1, + ) + heading_error = wrap_degrees( + actual_heading - reference_heading + ) + return { + "kind": "s_curve", + "ideal_plot_mm": ideal_plot, + "reference_points_mm": reference_points, + "reference_heading_degrees": reference_heading, + "reference_motion_heading_degrees": + reference_motion_heading, + "lateral_error_mm": lateral_error, + "heading_error_degrees": heading_error, + "offset_mm": offset_mm, + } + + line = end - start + length = float(np.linalg.norm(line)) + if length <= 1e-6: + rotation_match = re.search( + r"Rotate(?P[+-]?[0-9.]+)", + trajectory_name, + flags=re.IGNORECASE, + ) + if rotation_match: + relative_angle_degrees = float( + rotation_match.group("angle") + ) + target_heading_degrees = ( + float(metadata["start_heading_degrees"]) + + relative_angle_degrees + ) + reference_points = np.repeat( + start[np.newaxis, :], + len(frame), + axis=0, + ) + position_drift = np.linalg.norm( + actual - start, + axis=1, + ) + reference_heading = np.full( + len(frame), + target_heading_degrees, + ) + heading_error = wrap_degrees( + actual_heading - reference_heading + ) + ideal_plot = np.repeat( + start[np.newaxis, :], + 2, + axis=0, + ) + return { + "kind": "in_place_rotation", + "ideal_plot_mm": ideal_plot, + "reference_points_mm": reference_points, + "reference_heading_degrees": reference_heading, + # 对原地自转，该字段表示偏离初始旋转中心的距离。 + "lateral_error_mm": position_drift, + "heading_error_degrees": heading_error, + "rotation_center_mm": start, + "relative_angle_degrees": relative_angle_degrees, + "target_heading_degrees": target_heading_degrees, + } + + raise ValueError( + f"{trajectory_name}无法识别为圆弧，且参考直线长度为0。" + ) + + tangent = line / length + left_normal = np.array([-tangent[1], tangent[0]]) + displacement = actual - start + progress = np.clip(displacement @ tangent, 0.0, length) + reference_points = start + np.outer(progress, tangent) + lateral_error = (actual - reference_points) @ left_normal + reference_motion_heading_scalar = np.rad2deg( + np.arctan2(tangent[1], tangent[0]) + ) + reference_heading_scalar = ( + reference_motion_heading_scalar - + motion_frame_yaw_degrees + ) + reference_heading = np.full( + len(frame), + reference_heading_scalar, + ) + heading_error = wrap_degrees( + actual_heading - reference_heading + ) + ideal_plot = np.linspace(start, end, 361) + return { + "kind": "line", + "ideal_plot_mm": ideal_plot, + "reference_points_mm": reference_points, + "reference_heading_degrees": reference_heading, + "reference_motion_heading_degrees": np.full( + len(frame), + reference_motion_heading_scalar, + ), + "lateral_error_mm": lateral_error, + "heading_error_degrees": heading_error, + } + + +def discover_csv_files(arguments: list[str]) -> list[Path]: + """解析命令行CSV；未指定时使用脚本目录下全部CSV。""" + if arguments: + files = [Path(item).expanduser().resolve() for item in arguments] + else: + files = sorted(SCRIPT_DIR.glob("*.csv")) + if not files: + raise FileNotFoundError("没有找到可处理的CSV文件。") + return files + + +def output_path( + csv_path: Path, + output_directory: str | None, + suffix: str, +) -> Path: + """构造图片输出路径并创建目录。""" + directory = ( + Path(output_directory).expanduser().resolve() + if output_directory + else csv_path.parent / "plots" + ) + directory.mkdir(parents=True, exist_ok=True) + return directory / f"{csv_path.stem}_{suffix}.png" + + +def plot_trajectory( + csv_path: Path, + frequency_hz: float, + filter_window_seconds: float, + output_directory: str | None, + show: bool, +) -> Path: + """生成单份CSV的理想/实际轨迹对比图。""" + frame, metadata = load_and_resample( + csv_path, + frequency_hz, + filter_window_seconds, + ) + reference = build_reference(frame, metadata) + + actual_x_m = frame["DetourXFilteredMm"].to_numpy() / 1000.0 + actual_y_m = frame["DetourYFilteredMm"].to_numpy() / 1000.0 + ideal_m = np.asarray(reference["ideal_plot_mm"]) / 1000.0 + + fig, ax = plt.subplots(figsize=(8.0, 7.0)) + ax.plot( + ideal_m[:, 0], + ideal_m[:, 1], + "--", + linewidth=2.2, + label="理想轨迹", + ) + ax.plot( + actual_x_m, + actual_y_m, + linewidth=1.8, + label="Detour实际轨迹（滤波后）", + ) + if reference["kind"] == "in_place_rotation": + ax.scatter( + [ideal_m[0, 0]], + [ideal_m[0, 1]], + marker="*", + s=100, + label="理想旋转中心", + zorder=5, + ) + else: + ax.scatter( + [ideal_m[0, 0]], + [ideal_m[0, 1]], + marker="o", + s=55, + label="起点", + zorder=5, + ) + ax.scatter( + [ideal_m[-1, 0]], + [ideal_m[-1, 1]], + marker="x", + s=65, + label="终点", + zorder=5, + ) + for event_index, event in enumerate( + metadata["localization_jump_events"] + ): + before = np.array([ + event["before_x_mm"], + event["before_y_mm"], + ]) / 1000.0 + after = np.array([ + event["after_x_mm"], + event["after_y_mm"], + ]) / 1000.0 + ax.scatter( + [before[0], after[0]], + [before[1], after[1]], + marker="x", + color="tab:red", + s=55, + zorder=6, + label="Detour定位跳变前/后" + if event_index == 0 else None, + ) + ax.annotate( + f"定位跳变 {event['distance_mm']:.1f} mm\n" + f"t={event['time_seconds']:.2f} s", + xy=(after[0], after[1]), + xytext=(8, 8), + textcoords="offset points", + color="tab:red", + fontsize=9, + ) + ax.set_aspect("equal", adjustable="box") + ax.set_xlabel("世界坐标 X / m") + ax.set_ylabel("世界坐标 Y / m") + ax.set_title( + f"理想轨迹与实际轨迹对比\n" + f"{metadata['controller_name']} - " + f"{metadata['trajectory_name']}" + ) + ax.grid(True, alpha=0.3) + ax.legend() + fig.tight_layout() + + destination = output_path( + csv_path, + output_directory, + "trajectory_comparison", + ) + fig.savefig(destination, dpi=300, bbox_inches="tight") + if show: + plt.show() + plt.close(fig) + print( + f"{csv_path.name}: 原始采样{metadata['raw_sample_count']}帧，" + f"有效Detour更新{metadata['detour_update_count']}帧，" + f"保持重复{metadata['held_sample_count']}帧，" + f"定位跳变{len(metadata['localization_jump_events'])}次" + ) + return destination + + +def main() -> None: + configure_matplotlib() + parser = argparse.ArgumentParser( + description="绘制理想轨迹与Detour实际轨迹对比图。" + ) + parser.add_argument("files", nargs="*", help="一个或多个CSV文件") + parser.add_argument("--frequency", type=float, default=20.0) + parser.add_argument("--window", type=float, default=0.55) + parser.add_argument("--output-dir") + parser.add_argument("--show", action="store_true") + args = parser.parse_args() + + for csv_path in discover_csv_files(args.files): + destination = plot_trajectory( + csv_path, + args.frequency, + args.window, + args.output_dir, + args.show, + ) + print(f"已生成：{destination}") + + +if __name__ == "__main__": + main() diff --git a/data_process/轨迹测试处理/requirements.txt b/data_process/轨迹测试处理/requirements.txt new file mode 100644 index 0000000..dbae9b1 --- /dev/null +++ b/data_process/轨迹测试处理/requirements.txt @@ -0,0 +1,4 @@ +numpy>=1.26 +pandas>=2.2 +matplotlib>=3.8 +scipy>=1.12 diff --git a/data_process/轨迹测试处理/run_all_plots.py b/data_process/轨迹测试处理/run_all_plots.py new file mode 100644 index 0000000..c50e4e0 --- /dev/null +++ b/data_process/轨迹测试处理/run_all_plots.py @@ -0,0 +1,191 @@ +"""一次运行四个轨迹实验绘图脚本。""" + +from __future__ import annotations + +import argparse +import os +import subprocess +import sys +from concurrent.futures import ThreadPoolExecutor, as_completed +from pathlib import Path + + +SCRIPT_NAMES = ( + "plot_trajectory_comparison.py", + "plot_tracking_errors.py", + "plot_speed_response.py", + "plot_angular_command.py", +) + + +def build_command( + script_path: Path, + files: list[str], + frequency_hz: float, + filter_window_seconds: float, + output_directory: str | None, + show: bool, +) -> list[str]: + """为一个绘图脚本构造与统一入口一致的命令行参数。""" + command = [ + sys.executable, + str(script_path), + *files, + "--frequency", + str(frequency_hz), + "--window", + str(filter_window_seconds), + ] + + if output_directory: + command.extend(["--output-dir", output_directory]) + + if show: + command.append("--show") + + return command + + +def run_script( + script_path: Path, + files: list[str], + frequency_hz: float, + filter_window_seconds: float, + output_directory: str | None, + show: bool, +) -> tuple[str, int, str, str]: + """运行一个绘图脚本并返回名称、退出码及标准输出和错误。""" + result = subprocess.run( + build_command( + script_path, + files, + frequency_hz, + filter_window_seconds, + output_directory, + show, + ), + cwd=script_path.parent, + capture_output=True, + text=True, + encoding="utf-8", + errors="replace", + env={ + **os.environ, + "PYTHONIOENCODING": "utf-8", + }, + check=False, + ) + + return ( + script_path.name, + result.returncode, + result.stdout.strip(), + result.stderr.strip(), + ) + + +def main() -> None: + """并行执行四类实验图的生成任务。""" + parser = argparse.ArgumentParser( + description="一次生成轨迹、误差、速度响应和角速度指令四类图。" + ) + parser.add_argument( + "files", + nargs="*", + help="一个或多个CSV文件；省略时处理data_process目录下全部CSV。", + ) + parser.add_argument( + "--frequency", + type=float, + default=20.0, + help="固定重采样频率，默认20 Hz。", + ) + parser.add_argument( + "--window", + type=float, + default=0.55, + help="Savitzky-Golay滤波窗口，默认0.55 s。", + ) + parser.add_argument( + "--output-dir", + help="图片输出目录；省略时由各绘图脚本使用默认目录。", + ) + parser.add_argument( + "--show", + action="store_true", + help="生成后请求显示图片。", + ) + args = parser.parse_args() + + if args.frequency <= 0.0: + parser.error("--frequency必须大于0。") + + if args.window <= 0.0: + parser.error("--window必须大于0。") + + script_directory = Path(__file__).resolve().parent + script_paths = [ + script_directory / name + for name in SCRIPT_NAMES + ] + missing_scripts = [ + str(path) + for path in script_paths + if not path.is_file() + ] + if missing_scripts: + parser.error( + "缺少绘图脚本：" + "，".join(missing_scripts) + ) + + print("开始并行生成四类实验图……") + failures: list[str] = [] + + with ThreadPoolExecutor( + max_workers=len(script_paths) + ) as executor: + futures = [ + executor.submit( + run_script, + script_path, + args.files, + args.frequency, + args.window, + args.output_dir, + args.show, + ) + for script_path in script_paths + ] + + for future in as_completed(futures): + script_name, return_code, stdout, stderr = ( + future.result() + ) + print(f"\n[{script_name}]") + if stdout: + print(stdout) + if stderr: + print(stderr, file=sys.stderr) + + if return_code == 0: + print("执行成功。") + else: + failures.append(script_name) + print( + f"执行失败，退出码={return_code}。", + file=sys.stderr, + ) + + if failures: + print( + "\n以下脚本执行失败：" + + "，".join(failures), + file=sys.stderr, + ) + raise SystemExit(1) + + print("\n四类实验图均已生成。") + + +if __name__ == "__main__": + main() diff --git a/docs/SteeringConstraintDesign.md b/docs/SteeringConstraintDesign.md new file mode 100644 index 0000000..33a2890 --- /dev/null +++ b/docs/SteeringConstraintDesign.md @@ -0,0 +1,225 @@ +// 然后因为我这个车的舵轮是有正负120°限制的，所以有这种情况，比如我给了+90°让它进入蟹行模式，然后我在蟹行模式我去追一个相对于车左侧方向+40°的轨迹，那这时候就超过限制了，那我这是要做一个什么措施吗，比如我能想到的就是达到这个限位要么就停车打印说明到达限位，安全措施；要么就是我在追这个轨迹的过程中知道我大概可能超过限位，然后我提前停下，自转，调整为车头朝向为轨迹的方向，也就是自转调整到我车当前模式的初始姿态方向这种；你有什么好的建议吗，其他的建议吗，或者有什么可行的方法，因为实际的过程还是需要考虑这个限位的问题的 + + + + + + +// 9:49 + + + + + + +// 这个问题必须考虑，但“目标轮角超过 +120°”不一定意味着命令无法执行。对于舵轮，首先要利用“轮角反向、轮速反向”的等价关系，然后才考虑停车和车身重定向。 +// 1. +130° 不一定超过限位后就无法运动 +// 你的例子是： +// 蟹行基准方向：+90° +// 轨迹又向该方向增加：+40° +// 期望运动方向：+130° +// 舵轮限制：[-120°, +120°] +// 直接使用： +// 轮角：+130° +// 轮速：+0.3 m/s +// 确实超过了 +120°。 +// 但它有一个完全等价的表达： +// 轮角：-50° +// 轮速：-0.3 m/s +// 因为： +// (θ, v) ≡ (θ - 180°, -v) +// 即： +// (+130°, +0.3) ≡ (-50°, -0.3) +// 虽然轮子的机械朝向和转速符号变了，但轮胎与地面的速度向量完全相同。 +// 所以第一层措施应当是： +// 先尝试期望角度 +// 如果超过限位 +// 尝试角度±180° +// 同时将轮速取反 +// 如果等价角度满足限位 +// 使用等价解 +// 否则才判定无法执行 +// 你当前的 CommonUsage.dll 中已经有类似的舵角反解逻辑，SendXYThSpeed() 返回 bool 也是为了告诉上层运动分解是否成功。 +// 2. 你的±120°结构在静态上覆盖了所有运动方向 +// 因为： +// 可转范围总宽度 = 240° +// 大于 180°。 +// 只要驱动轮允许正反转，那么对于任意二维速度方向，通常都能在下面两种表达中找到一个合法值： +// (θ, +v) +// (θ ± 180°, -v) +// 因此单纯从“某一时刻目标速度方向”看，+130° 可以用 -50° 表达，不一定需要旋转车身。 +// 真正困难的是动态过程： +// 当前轮角：+90° +// 新目标等价轮角：-50° +// 需要转动：-140° +// 虽然最终角度合法，但舵轮需要转过很大的角度。在舵轮没有到位前，不能继续以正常速度驱动车辆，否则实际运动方向会与轨迹命令不一致。 +// 所以核心问题不是“有没有合法终态”，而是： +// 从当前轮角切换到合法目标轮角的过程是否安全、连续。 + +// 3. 建议采用四层处理策略 +// 第一层：等价舵角选择 +// 为每一个舵轮计算速度向量： +// Vix = Vx - ω·yi +// Viy = Vy + ω·xi +// 然后： +// θi = atan2(Viy, Vix) +// si = sqrt(Vix² + Viy²) +// 尝试候选解： +// 候选1：(θi, si) +// 候选2：(θi + 180°, -si) +// 候选3：(θi - 180°, -si) +// 过滤掉超过 [-120°, +120°] 的候选，再选择相对当前机械轮角转动最小的候选。 +// 这里计算机械转角距离时，不要简单把差值归一化成最短圆周角，因为舵轮不能无限旋转。对于有限机械区间，应当直接比较： +// Math.Abs(candidateAngle - currentMechanicalAngle) +// 第二层：软限位 +// 不要把 ±120° 当作正常工作的边界，建议设置软限位，例如： +// 物理硬限位：[-120°, +120°] +// 软件工作限位：[-105°, +105°] +// 预警区域：[105°, 120°] +// 具体余量需要实车标定，初期可以保留 10°～20°。 +// 状态可以分成： +// |θ| < 100° 正常 +// 100°～105° 接近限位，开始规划 +// 105°～120° 限速或停止切换 +// >120° 禁止下发并报警 +// 软限位的意义是给控制器留出制动、舵轮响应和误差空间。 +// 第三层：舵轮转向期间限制驱动速度 +// 如果新的合法舵角与当前舵角相差很大，例如： +// 当前：+90° +// 目标：-50° +// 差值：140° +// 不应当马上正常行驶，应进入： +// 减速 +// ↓ +// 驱动速度降为0 +// ↓ +// 舵轮转到-50° +// ↓ +// 确认所有舵轮到位 +// ↓ +// 轮速使用负值重新起步 +// 可以设置阈值： +// 舵角误差 < 2°：允许正常起步 +// 舵角误差 2°～10°：低速或保持 +// 舵角误差 > 10°：驱动速度为0 +// 实际阈值根据舵向响应速度标定。 +// 需要注意：把所有 Vx、Vy、Omega 同比例减小，只能降低运动速度，通常不会改变目标舵轮角度。因此“减速”能让切换更安全，但不一定能让超限角度重新变得合法。 +// 4. 车身重定向是更平滑的高级方案 +// 你提出的： +// 提前停车，然后自转，调整车头朝向，再继续跟踪。 + +// 这是正确而且很实用的方案，尤其适合连续轨迹跟踪。 +// 但不一定要把车头完全转到轨迹方向，应当根据当前驾驶模式选择车身朝向。 +// 假设轨迹方向相对当前车体为： +// +130° +// 如果希望继续保持蟹行模式，舵轮的优选方向是 +90°，那么只需要让车体逆时针自转： +// 130° - 90° = 40° +// 车体旋转 +40° 后： +// 世界轨迹方向不变 +// 轨迹在新车体坐标系中的方向：+90° +// 舵轮回到标准蟹行角度：+90° +// 不需要让车头旋转完整的 130°。 +// 如果准备切换成正常前进模式，优选舵角是 0°，才需要让车头基本对准轨迹： +// 130° - 0° = 130° +// 因此可以使用下面的通用公式： +// 期望车身朝向 = +// 世界轨迹切线方向 - 当前模式的优选舵轮方向 +// 其中： +// 正常模式优选角：0° +// 左蟹行优选角：+90° +// 右蟹行优选角：-90° +// 对于蟹行轨迹，车身朝向参考值为： +// yawReference = +// pathTangentWorldRadians - +// Math.PI / 2.0; +// 这样车体会逐渐调整自身朝向，使轨迹方向始终落在舵轮舒适区，而不是一直顶着 ±120° 工作。 +// 5. 不建议到达硬限位才突然自转 +// 更好的方案是向前预测。 +// 轨迹跟踪本身通常有前视点，可以提前计算未来一段轨迹对应的目标舵角： +// 当前时刻目标轮角 +// 未来0.2秒目标轮角 +// 未来0.5秒目标轮角 +// 未来1.0秒目标轮角 +// 一旦预测到未来会进入软限位，就提前触发车身重定向： +// 正常跟踪 +// ↓ +// 预测舵角接近软限位 +// ↓ +// 逐渐降低线速度 +// ↓ +// 停车 +// ↓ +// 车身原地旋转 +// ↓ +// 舵轮调整到模式初始方向 +// ↓ +// 重新捕获轨迹 +// ↓ +// 继续跟踪 +// 可以定义状态机： +// internal enum TrackingConstraintState +// { +// Tracking, +// ApproachingSteeringLimit, +// Stopping, +// ReorientingBody, +// AligningWheels, +// Resuming, +// Fault +// } +// 不要在一个控制周期内同时完成停车、车身旋转和恢复跟踪。 +// 6. LQR和限位处理应该分层 +// 以后写 LQR 时，不建议让基础 LQR 直接承担全部舵轮限位逻辑。 +// 推荐结构： +// 轨迹参考 +// ↓ +// LQR轨迹控制器 +// 输出期望 Vx、Vy、Omega +// ↓ +// 舵轮约束管理器 +// 等价角选择、软限位、预测、车身重定向 +// ↓ +// ChassisCommand +// ↓ +// MultiWheelChassisAdapter +// ↓ +// SendXYThSpeed +// 可以以后增加一个文件： +// Shared/SteeringConstraintManager.cs +// 它负责： +// 根据 Vx、Vy、Omega 预测每个舵轮目标角。 +// 选择“舵角反向、轮速反向”的等价解。 +// 检查硬限位和软限位。 +// 判断是否需要停车对齐。 +// 给出是否需要重新规划车身朝向。 +// 记录哪个舵轮导致不可执行。 +// 而 FrameTransform2D、ChassisCommand 不应该包含这些车辆机械约束。 +// 7. 当前阶段建议先做到什么 +// 现在先实现最小安全版本： +// 使用 SendXYThSpeed()，让底层优先选择等价舵角。 +// 检查 adapter.Send(command) 的返回值。 +// 返回 false 时立即停车，不能继续沿用上一条速度。 +// 打印每个舵轮的当前角度、目标角度和上下限。 +// 模式切换时先停车、预转舵轮、确认到位，再开放摇杆。 +// 先用较低速度测试 +90° → +110° → +120° → +130° 的过程。 +// 最低限度的发送逻辑应当是： +// var success = adapter.Send(command, interval); + +// if (!success) +// { +// adapter.Stop(interval); + +// Console.WriteLine( +// "底盘命令分解失败，已停车：" + +// "可能存在舵角限制或舵轮无法完成方向切换。"); +// } +// 然后再实现预测重定向。 +// 推荐的最终策略 +// 对于你的停车机器人，我建议采用： +// 优先级1：使用θ±180°、轮速反向的等价解 +// 优先级2：选择机械转角变化较小的等价解 +// 优先级3：接近软限位时降低速度 +// 优先级4：提前规划车身朝向，保持舵轮在舒适角度 +// 优先级5：无法连续切换时停车、自转、重新对齐 +// 优先级6：仍不可行时保持停车并报警 +// 所以你的 +130° 示例不应该一开始就判故障。它可以先使用 -50° + 反向轮速。如果从当前 +90° 切换到 -50° 的动态过程不安全，再停车调整。若轨迹允许改变车身姿态，更理想的办法是让车身提前旋转 +40°，从而继续以标准 +90° 蟹行姿态跟踪，而不是一直逼近机械硬限位。 \ No newline at end of file diff --git a/output/C/CommonUsage.dll b/output/C/CommonUsage.dll new file mode 100644 index 0000000..9079f92 Binary files /dev/null and b/output/C/CommonUsage.dll differ diff --git a/output/C/MultiWheelC.dll b/output/C/MultiWheelC.dll new file mode 100644 index 0000000..b9a9c4a Binary files /dev/null and b/output/C/MultiWheelC.dll differ diff --git a/output/C/MultiWheelC.pdb b/output/C/MultiWheelC.pdb new file mode 100644 index 0000000..87b88c1 Binary files /dev/null and b/output/C/MultiWheelC.pdb differ diff --git a/output/M/CommonUsage.dll b/output/M/CommonUsage.dll new file mode 100644 index 0000000..9079f92 Binary files /dev/null and b/output/M/CommonUsage.dll differ diff --git a/output/M/MedullaAdapter.dll b/output/M/MedullaAdapter.dll new file mode 100644 index 0000000..91f4b91 Binary files /dev/null and b/output/M/MedullaAdapter.dll differ diff --git a/output/M/MedullaAdapter.pdb b/output/M/MedullaAdapter.pdb new file mode 100644 index 0000000..7a9330a Binary files /dev/null and b/output/M/MedullaAdapter.pdb differ diff --git a/ref/CommonUsage.dll b/ref/CommonUsage.dll new file mode 100644 index 0000000..9079f92 Binary files /dev/null and b/ref/CommonUsage.dll differ diff --git a/测试方案.txt b/测试方案.txt new file mode 100644 index 0000000..85535c6 --- /dev/null +++ b/测试方案.txt @@ -0,0 +1,34 @@ +单个停车机器人轨迹测试方案 +### 测试对象 +单台停车机器人（+50kg负载） +### 测试曲线 +1. 直线：前进/后退2m、速度0.3m/s、起点终点静止 +2. 转弯：左转/右转组合前进/后退、转弯半径1m、曲率1.0、速度0.3m/s +3. 原地自转：±90°/±180°、角速度10°/s、20°/s、起点终点静止 + +### 评价指标 +横向误差 RMSE、最大横向误差；航向误差 RMSE、最大航向误差；速度误差 RMSE、最大速度偏差； +角速度或转角指令的变化曲线；最终位置误差、最终航向误差 + +### 展示形式 +理想轨迹与实际轨迹对比图、横向/航向误差随时间变化图、参考速度与实际速度对比图、角速度指令曲线 + +### LQR调参策略 +归一化状态/控制量：一般Q、R初始选择对应控制量最大值的平方的倒数 +或者使用Bryson’s Rule来给 Q、R 一个很好的初始猜测 +贝叶斯优化在仿真中自动调节Q、R参数 +ALQR：在线估计最新参数实时重新求解 +---------------------------------------- +不调参：学习式 + +### 杂项 +1. M层获取实际小车的速度信息与位置信息并保存、使用python可视化来量化跟踪误差 +2. 自行车模型改动、过于局限于阿克曼小车的运动学限制 + + +先用 Bryson’s Rule + 贝叶斯优化在仿真里把 Q/R 调到一个不错的基准。 +上实车时采用自适应 LQR：在线估计关键参数（尤其是轮胎刚度），实时更新 K。 +C# 实现的话： +矩阵运算继续用 Math.NET +贝叶斯优化可以调 Python 库，或者自己写简单版本 +在线参数估计（RLS）用 C# 写很轻松 \ No newline at end of file diff --git a/记录.txt b/记录.txt new file mode 100644 index 0000000..80c8e57 --- /dev/null +++ b/记录.txt @@ -0,0 +1,23 @@ +还可以把底盘的失败原因暴露出来： +/// +/// 获取最近一次底盘运动分解失败原因。 +/// +public string LastFailureReason => + _chassis.LastMotionDecomposeFailureReason; +这样调用方可以打印： +if (!adapter.Send(command)) +{ + Console.WriteLine( + $"底盘命令执行失败：{adapter.LastFailureReason}"); +} + + +需要注意，Detour 差分速度会有噪声，建议在 Python 中： +按固定频率重新采样。 +对位置做轻微滤波或使用 Savitzky–Golay 求导。 +再计算速度，避免直接逐点差分产生尖峰。 +Stanley 和 LQR 必须使用相同的滤波和采样参数。 + + +编译命令： +powershell -NoProfile -ExecutionPolicy Bypass -File .\build-and-package.ps1