Initial commit from MyParking project
This commit is contained in:
@@ -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` 强推，交给用户决定
|
||||
- 如果改动很大很杂，主动提示用户考虑拆分提交，但不强制
|
||||
@@ -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
|
||||
@@ -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.
|
||||
+93
@@ -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
|
||||
@@ -0,0 +1,2 @@
|
||||
*.cs text eol=crlf
|
||||
.gitattributes text eol=lf
|
||||
@@ -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
|
||||
@@ -0,0 +1,126 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Diagnostics;
|
||||
using System.IO;
|
||||
using System.Numerics;
|
||||
using System.Security.Cryptography.X509Certificates;
|
||||
using System.Text;
|
||||
using FundamentalLib;
|
||||
using Newtonsoft.Json;
|
||||
|
||||
namespace CommonUsage.Chassis
|
||||
{
|
||||
public abstract class AbstractChassis
|
||||
{
|
||||
protected AbstractChassis()
|
||||
{
|
||||
Valid = false;
|
||||
}
|
||||
|
||||
public abstract void Initialize();
|
||||
|
||||
public abstract void Visualize();
|
||||
|
||||
public abstract void AfterDirectionChanged();
|
||||
|
||||
/// <summary>
|
||||
/// 当前行进方向。
|
||||
/// </summary>
|
||||
[Obsolete]
|
||||
public float DirectionAngle
|
||||
{
|
||||
get => _originBiasTh;
|
||||
set
|
||||
{
|
||||
_originBiasTh = value;
|
||||
if (_originBiasTh != _lastDirectionAngle) AfterDirectionChanged();
|
||||
_lastDirectionAngle = _originBiasTh;
|
||||
}
|
||||
}
|
||||
|
||||
protected float _originBiasX = 0f, _originBiasY = 0f, _originBiasTh;
|
||||
|
||||
public Vector3 GetOriginBias()
|
||||
{
|
||||
return new Vector3(_originBiasX, _originBiasY, _originBiasTh);
|
||||
}
|
||||
|
||||
public class CarSpeed
|
||||
{
|
||||
public float Vx,Vy,Vw;
|
||||
}
|
||||
|
||||
public abstract CarSpeed GetCarSpeed(bool isActual = false);
|
||||
public List<GeometricControlPoint> GetGeometricControlPoints()
|
||||
{
|
||||
return GeometricControlPoints;
|
||||
}
|
||||
|
||||
public void ComputeWheelsGeometrically(float speed)
|
||||
{
|
||||
// 打印调用位置信息
|
||||
var stackTrace = new StackTrace(true);
|
||||
var callerFrame = stackTrace.GetFrame(1); // 获取调用者的帧
|
||||
if (callerFrame != null)
|
||||
{
|
||||
var fileName = callerFrame.GetFileName();
|
||||
var lineNumber = callerFrame.GetFileLineNumber();
|
||||
DLog.Log($"s:{speed:0.000} from {fileName} ln.{lineNumber}", $"WheelComputeCaller");
|
||||
}
|
||||
|
||||
DefineGeometricWheelComputation(speed);
|
||||
}
|
||||
|
||||
protected abstract void DefineGeometricWheelComputation(float speed);
|
||||
|
||||
public void DriveStop()
|
||||
{
|
||||
PredefinedDriveStop();
|
||||
CustomDriveStop?.Invoke();
|
||||
}
|
||||
|
||||
public abstract void PredefinedDriveStop();
|
||||
|
||||
public Action CustomDriveStop;
|
||||
|
||||
public abstract bool ComputeRotateWheels(float rotSpeed);
|
||||
|
||||
public abstract float CalculateTurningSpeedDecayFac(float turn);
|
||||
|
||||
public enum ChassisState
|
||||
{
|
||||
Standby,
|
||||
Running,
|
||||
AbnormalFeedback,
|
||||
ExceedMotionAbility,
|
||||
}
|
||||
|
||||
protected ChassisState State;
|
||||
|
||||
protected string StateDescription;
|
||||
|
||||
public (ChassisState State, string Description) GetChassisState()
|
||||
{
|
||||
return (State, StateDescription);
|
||||
}
|
||||
|
||||
public bool Debug = false;
|
||||
public float AccPerSecond = 0.2f;
|
||||
public float DeAccPerSecond = 0.2f;
|
||||
public float MaxSpeed = 1; // m/s
|
||||
public float MinTurnSpeedFac = 0.5f;
|
||||
public float MaxTurnThreshold = 90f;
|
||||
|
||||
public float GcpThetaPerSecond = 10f;
|
||||
|
||||
public DateTime LastMoveTime = DateTime.MinValue;
|
||||
|
||||
protected bool Valid = false;
|
||||
protected List<GeometricControlPoint> GeometricControlPoints = new();
|
||||
protected bool RotatingActive = false;
|
||||
protected bool GoingActive = false;
|
||||
protected bool GoingWheelAligned = false;
|
||||
|
||||
private float _lastDirectionAngle = 0;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,47 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Numerics;
|
||||
using System.Text;
|
||||
|
||||
namespace CommonUsage.Chassis
|
||||
{
|
||||
public class DiffSteerWheel:SteerWheel
|
||||
{
|
||||
public DiffSteerWheel(float wheelDistance,Vector2 position, float angleLowerLimit, float angleUpperLimit, Action<float> speedWriter,
|
||||
Func<float> speedReader, Action<float> angleWriter, Func<float> angleReader, Action<float> leftSpeedWriter, Action<float> rightSpeedWriter,
|
||||
float angleLimitMarginDeg = 15f) : base(position,
|
||||
angleLowerLimit, angleUpperLimit, speedWriter, speedReader, angleWriter, angleReader, angleLimitMarginDeg)
|
||||
{
|
||||
_leftSpeedWriter = leftSpeedWriter;
|
||||
_rightSpeedWriter = rightSpeedWriter;
|
||||
WheelDistance = wheelDistance;
|
||||
}
|
||||
|
||||
public float GetLeftSendSpeed()
|
||||
{
|
||||
return _leftSendSpeed;
|
||||
}
|
||||
|
||||
public float GetRightSendSpeed()
|
||||
{
|
||||
return _rightSendSpeed;
|
||||
}
|
||||
|
||||
public void WriteLeftSpeed(float speed)
|
||||
{
|
||||
_leftSpeedWriter(_leftSendSpeed = speed);
|
||||
}
|
||||
|
||||
public void WriteRightSpeed(float speed)
|
||||
{
|
||||
_rightSpeedWriter(_rightSendSpeed = speed);
|
||||
}
|
||||
|
||||
public float WheelDistance;
|
||||
private readonly Action<float> _leftSpeedWriter;
|
||||
private readonly Action<float> _rightSpeedWriter;
|
||||
|
||||
private float _leftSendSpeed;
|
||||
private float _rightSendSpeed;
|
||||
}
|
||||
}
|
||||
@@ -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;
|
||||
}
|
||||
}
|
||||
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,172 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Linq;
|
||||
using System.Numerics;
|
||||
using System.Text;
|
||||
using System.Diagnostics;
|
||||
using CommonUsage.Mathematics;
|
||||
using FundamentalLib;
|
||||
|
||||
namespace CommonUsage.Chassis
|
||||
{
|
||||
public class SingleSteerChassis : AbstractChassis
|
||||
{
|
||||
public void SetSteerWheel(SteerWheel wheel)
|
||||
{
|
||||
_steerWheel = wheel;
|
||||
}
|
||||
|
||||
public SteerWheel GetSteerWheel()
|
||||
{
|
||||
return _steerWheel;
|
||||
}
|
||||
|
||||
public override void Visualize()
|
||||
{
|
||||
|
||||
}
|
||||
|
||||
public override CarSpeed GetCarSpeed(bool isActual = false)
|
||||
{
|
||||
if (!isActual)
|
||||
{
|
||||
var sendAngle = _steerWheel.GetSendAngle();
|
||||
var sendAngleRad = _steerWheel.GetSendAngle() / 180f * Math.PI;
|
||||
// VSteer* Cos = v;
|
||||
var vsteer = _sendSpeed / ((Math.Cos(Math.Abs(sendAngleRad)) + 0.000001));
|
||||
var vsteerY = vsteer * Math.Sin(sendAngleRad);
|
||||
// Console.WriteLine($"{vsteer} {vsteerY} {sendAngleRad} {_sendSpeed}");
|
||||
return new CarSpeed()
|
||||
{
|
||||
Vx = (float)(_sendSpeed * Math.Cos(Math.Abs(sendAngle) / 180f * Math.PI)),
|
||||
Vy = 0,
|
||||
Vw = (float)(_sendSpeed * Math.Sin(Math.Abs(sendAngle) / 180f * Math.PI) /
|
||||
Math.Abs(_steerWheel.Position.X / 1000f) / Math.PI * 180f)
|
||||
//阿克曼
|
||||
// Vx = (float)(_sendSpeed),
|
||||
// Vy = 0,
|
||||
// Vw = (float)(vsteerY / Math.Abs(_steerWheel.Position.X / 1000f) / Math.PI * 180f)
|
||||
};
|
||||
}
|
||||
else
|
||||
{
|
||||
return new CarSpeed()
|
||||
{
|
||||
Vx = (float)(_steerWheel.ReadSpeed() *
|
||||
Math.Cos(Math.Abs(_steerWheel.ReadAngle()) / 180f * Math.PI)),
|
||||
Vy = 0,
|
||||
Vw = (float)(_steerWheel.ReadSpeed() *
|
||||
Math.Sin(Math.Abs(_steerWheel.ReadAngle()) / 180f * Math.PI) /
|
||||
Math.Abs(_steerWheel.Position.X / 1000f) / Math.PI * 180f)
|
||||
};
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
public override void Initialize()
|
||||
{
|
||||
GeometricControlPoints = new List<GeometricControlPoint>()
|
||||
{
|
||||
new (_steerWheel.Position),
|
||||
new (Vector2.Zero)
|
||||
};
|
||||
Valid = true;
|
||||
}
|
||||
|
||||
public override void AfterDirectionChanged()
|
||||
{
|
||||
if (Math.Abs(CommonMath.ThDiff(0, _originBiasTh)) > 90)
|
||||
{
|
||||
GeometricControlPoints = new List<GeometricControlPoint>()
|
||||
{
|
||||
new (-_steerWheel.Position),
|
||||
};
|
||||
_direction = -1;
|
||||
}
|
||||
else
|
||||
{
|
||||
GeometricControlPoints = new List<GeometricControlPoint>()
|
||||
{
|
||||
new (_steerWheel.Position),
|
||||
};
|
||||
_direction = 1;
|
||||
}
|
||||
}
|
||||
|
||||
public override void PredefinedDriveStop()
|
||||
{
|
||||
if (!Valid) return;
|
||||
_sendSpeed = 0;
|
||||
_steerWheel.WriteSpeed(_sendSpeed);
|
||||
GoingActive = false;
|
||||
RotatingActive = false;
|
||||
}
|
||||
|
||||
protected override void DefineGeometricWheelComputation(float speed)
|
||||
{
|
||||
var now = DateTime.Now;
|
||||
|
||||
if (!GoingActive)
|
||||
{
|
||||
LastMoveTime = now;
|
||||
GoingWheelAligned = false;
|
||||
}
|
||||
|
||||
SendSteerMotion(speed * _direction, GeometricControlPoints[0].Theta, now - LastMoveTime);
|
||||
|
||||
GoingActive = true;
|
||||
RotatingActive = false;
|
||||
}
|
||||
|
||||
public override bool ComputeRotateWheels(float rotSpeed)
|
||||
{
|
||||
if (!RotatingActive)
|
||||
{
|
||||
LastMoveTime = DateTime.Now;
|
||||
GoingWheelAligned = false;
|
||||
}
|
||||
|
||||
SendSteerMotion(rotSpeed, 90);
|
||||
|
||||
GoingActive = false;
|
||||
RotatingActive = true;
|
||||
return true;
|
||||
}
|
||||
|
||||
public void SendSteerMotion(float speed, float theta, TimeSpan? deltaTime = null)
|
||||
{
|
||||
_steerWheel.WriteAngle(theta);
|
||||
|
||||
if (!GoingWheelAligned && Math.Abs(CommonMath.ThDiff(_steerWheel.ReadAngle(), theta)) < 1)
|
||||
GoingWheelAligned = true;
|
||||
if (!GoingWheelAligned) speed = 0;
|
||||
|
||||
var turnThresholdSpeed = CalculateTurningSpeedDecayFac(Math.Abs(theta)) * MaxSpeed;
|
||||
AccumulateSpeed(Math.Min(turnThresholdSpeed, Math.Abs(speed)) * Math.Sign(speed), deltaTime);
|
||||
|
||||
LastMoveTime = DateTime.Now;
|
||||
}
|
||||
|
||||
public override float CalculateTurningSpeedDecayFac(float turn)
|
||||
{
|
||||
return 1 - Math.Min(turn, MaxTurnThreshold) / MaxTurnThreshold * MinTurnSpeedFac;
|
||||
}
|
||||
|
||||
private void AccumulateSpeed(float v, TimeSpan? deltaTime = null)
|
||||
{
|
||||
// _targetSpeed = v;
|
||||
var speedSign = Math.Sign(v - _sendSpeed);
|
||||
var acc = Math.Abs(v) > Math.Abs(_sendSpeed) ? AccPerSecond : DeAccPerSecond;
|
||||
_sendSpeed += speedSign * Math.Min(Math.Abs(v - _sendSpeed),
|
||||
acc * (float)(deltaTime ?? DateTime.Now - LastMoveTime).TotalSeconds);
|
||||
_steerWheel.WriteSpeed(_sendSpeed);
|
||||
|
||||
if (Debug)
|
||||
Console.WriteLine($"SingleSteer, target:{v:0.00},send:{_sendSpeed:0.0}");
|
||||
}
|
||||
|
||||
private SteerWheel _steerWheel;
|
||||
private float _sendSpeed;
|
||||
private int _direction = 1; // 1 forward, -1 backward
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,100 @@
|
||||
using Newtonsoft.Json;
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Numerics;
|
||||
using System.Text;
|
||||
using CommonUsage.Mathematics;
|
||||
|
||||
namespace CommonUsage.Chassis
|
||||
{
|
||||
public class SteerWheel : Wheel
|
||||
{
|
||||
public SteerWheel(Vector2 position, float angleLowerLimit, float angleUpperLimit, Action<float> speedWriter,
|
||||
Func<float> speedReader, Action<float> angleWriter, Func<float> angleReader, float angleLimitMarginDeg = 15f) : base(position, speedWriter,
|
||||
speedReader)
|
||||
{
|
||||
_angleLowerLimit = angleLowerLimit;
|
||||
_angleUpperLimit = angleUpperLimit;
|
||||
_angleWriter = angleWriter;
|
||||
_angleReader = angleReader;
|
||||
_centerDistance = position.Length();
|
||||
AngleLimitMarginDeg = angleLimitMarginDeg;
|
||||
}
|
||||
|
||||
public float AngleLimitMarginDeg = 15f;
|
||||
|
||||
public bool TrySetDirection(bool allowReverse, ref float desireDirection, ref int dir)
|
||||
{
|
||||
if (TryNormalizeAngleInLimit(desireDirection, out var normalized))
|
||||
{
|
||||
dir = 1;
|
||||
desireDirection = normalized;
|
||||
return true;
|
||||
}
|
||||
|
||||
if (!allowReverse) return false;
|
||||
|
||||
var oppositeTh = (float)CommonMath.RoundTh(desireDirection + 180);
|
||||
if (TryNormalizeAngleInLimit(oppositeTh, out normalized))
|
||||
{
|
||||
dir = -1;
|
||||
desireDirection = normalized;
|
||||
return true;
|
||||
}
|
||||
|
||||
dir = 0;
|
||||
return false;
|
||||
}
|
||||
|
||||
private bool TryNormalizeAngleInLimit(float angle, out float normalized)
|
||||
{
|
||||
var lower = CommonMath.RoundTh(_angleLowerLimit);
|
||||
var upper = CommonMath.RoundTh(_angleUpperLimit);
|
||||
while (upper < lower) upper += 360;
|
||||
|
||||
normalized = (float)CommonMath.RoundTh(angle);
|
||||
while (normalized < lower) normalized += 360;
|
||||
while (normalized > upper && normalized - 360 >= lower) normalized -= 360;
|
||||
|
||||
var margin = Math.Min(normalized - lower, upper - normalized);
|
||||
return normalized >= lower && normalized <= upper && margin >= Math.Max(0, AngleLimitMarginDeg);
|
||||
}
|
||||
|
||||
public float ReadAngle()
|
||||
{
|
||||
return _angleReader();
|
||||
}
|
||||
|
||||
public void WriteAngle(float angle)
|
||||
{
|
||||
_angleWriter.Invoke(_sendAngle = Math.Max(_angleLowerLimit, Math.Min(angle, _angleUpperLimit)));
|
||||
}
|
||||
|
||||
public float GetSendAngle()
|
||||
{
|
||||
return _sendAngle;
|
||||
}
|
||||
|
||||
public float GetAngleRelativeToChassis()
|
||||
{
|
||||
return ZeroDirection + _sendAngle;
|
||||
}
|
||||
|
||||
public float CenterDistance()
|
||||
{
|
||||
return _centerDistance;
|
||||
}
|
||||
|
||||
public float AngleLowerLimit => _angleLowerLimit;
|
||||
|
||||
public float AngleUpperLimit => _angleUpperLimit;
|
||||
|
||||
[JsonIgnore] private readonly Action<float> _angleWriter;
|
||||
[JsonIgnore] private readonly Func<float> _angleReader;
|
||||
|
||||
private float _angleLowerLimit = -90, _angleUpperLimit = 90;
|
||||
private float _centerDistance;
|
||||
|
||||
public float _sendAngle = 0f;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,56 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Numerics;
|
||||
using System.Text;
|
||||
using Newtonsoft.Json;
|
||||
|
||||
namespace CommonUsage.Chassis
|
||||
{
|
||||
public class Wheel
|
||||
{
|
||||
public Wheel(Vector2 position, Action<float> speedWriter, Func<float> speedReader)
|
||||
{
|
||||
PhysicalPosition = Position = position;
|
||||
SpeedWriter = speedWriter;
|
||||
SpeedReader = speedReader;
|
||||
}
|
||||
|
||||
public void WriteSpeed(float speed)
|
||||
{
|
||||
SpeedWriter.Invoke(_sendSpeed = speed);
|
||||
}
|
||||
|
||||
public float GetSendSpeed()
|
||||
{
|
||||
return _sendSpeed;
|
||||
}
|
||||
|
||||
public float ReadSpeed()
|
||||
{
|
||||
return SpeedReader();
|
||||
}
|
||||
|
||||
// PhysicalPosition ===(chassis transform)===> Position
|
||||
// useful in dual agv coordination
|
||||
public readonly Vector2 PhysicalPosition;
|
||||
public Vector2 Position;
|
||||
|
||||
public float ZeroDirection = 0;
|
||||
|
||||
[JsonIgnore] public readonly Action<float> SpeedWriter;
|
||||
[JsonIgnore] public readonly Func<float> SpeedReader;
|
||||
|
||||
public float _sendSpeed;
|
||||
}
|
||||
|
||||
public class GeometricControlPoint
|
||||
{
|
||||
public GeometricControlPoint(Vector2 position)
|
||||
{
|
||||
Position = position;
|
||||
}
|
||||
|
||||
public Vector2 Position;
|
||||
public float Theta;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,50 @@
|
||||
<Project Sdk="Microsoft.NET.Sdk">
|
||||
|
||||
<PropertyGroup>
|
||||
<TargetFramework>netstandard2.0</TargetFramework>
|
||||
<AssemblyName>CommonUsage</AssemblyName>
|
||||
<RootNamespace>CommonUsage</RootNamespace>
|
||||
</PropertyGroup>
|
||||
|
||||
<PropertyGroup>
|
||||
<LangVersion>latest</LangVersion>
|
||||
<AllowUnsafeBlocks>True</AllowUnsafeBlocks>
|
||||
</PropertyGroup>
|
||||
|
||||
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Debug|AnyCPU'">
|
||||
<DebugType>embedded</DebugType>
|
||||
</PropertyGroup>
|
||||
|
||||
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Release|AnyCPU'">
|
||||
<DebugType>embedded</DebugType>
|
||||
</PropertyGroup>
|
||||
|
||||
<ItemGroup>
|
||||
<Compile Remove="Hedingben.cs" />
|
||||
<Compile Remove="IPCConcurrentDictionary.cs" />
|
||||
</ItemGroup>
|
||||
|
||||
<ItemGroup>
|
||||
<PackageReference Include="MQTTnet" Version="4.3.7.1207" />
|
||||
<PackageReference Include="MQTTnet.Extensions.ManagedClient" Version="4.3.7.1207" />
|
||||
<PackageReference Include="Newtonsoft.Json" Version="13.0.3" />
|
||||
<PackageReference Include="System.Buffers" Version="4.5.1" />
|
||||
<PackageReference Include="System.Numerics.Vectors" Version="4.5.0" />
|
||||
</ItemGroup>
|
||||
|
||||
<ItemGroup>
|
||||
<Reference Include="FundamentalLib">
|
||||
<HintPath>..\..\MedullaAdapter\ref\RefFundamentalLib.dll</HintPath>
|
||||
</Reference>
|
||||
<!-- <Reference Include="ClumsyCore">
|
||||
<HintPath>..\..\MultiWheelC\ref\RefClumsyCore.dll</HintPath>
|
||||
</Reference> -->
|
||||
</ItemGroup>
|
||||
|
||||
<Target Name="CopyCommonUsageToMyParkingRef" AfterTargets="Build">
|
||||
<MakeDir Directories="..\..\ref" />
|
||||
<Copy SourceFiles="$(TargetPath)"
|
||||
DestinationFolder="..\..\ref" />
|
||||
</Target>
|
||||
|
||||
</Project>
|
||||
@@ -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
|
||||
Binary file not shown.
@@ -0,0 +1,93 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Drawing;
|
||||
using System.Numerics;
|
||||
using System.Text;
|
||||
using static CommonUsage.Geometries.CircularArc;
|
||||
|
||||
namespace CommonUsage.Geometries
|
||||
{
|
||||
/// <summary>
|
||||
/// 便于直接创建几何形状并求几何形状的切点、切线等。
|
||||
/// </summary>
|
||||
public abstract class AbstractGeometry
|
||||
{
|
||||
protected AbstractGeometry()
|
||||
{
|
||||
PaddingType = Padding.StartExtendEndExtend;
|
||||
VisualizeOption = new VisualizeOption(Color.Red, Color.Gray);
|
||||
}
|
||||
|
||||
public Padding PaddingType;
|
||||
|
||||
public abstract (Vector2 Pt, float Angle, float Bias, float Position) QueryTangentPoint(Vector2 point);
|
||||
|
||||
public abstract void Visualize(Action<VisDot> processDot, Action<VisLine> processLine,
|
||||
bool visExtendedPart = false);
|
||||
|
||||
public VisualizeOption VisualizeOption;
|
||||
|
||||
/// <summary>
|
||||
/// 查询指定位置的曲率。
|
||||
/// </summary>
|
||||
/// <param name="position">从起点到查询位置的距离。</param>
|
||||
/// <returns></returns>
|
||||
public abstract float QueryCurvature(float position);
|
||||
|
||||
public abstract float Length();
|
||||
}
|
||||
|
||||
public class VisualizeOption
|
||||
{
|
||||
public VisualizeOption(Color mainColor, Color auxiliaryColor)
|
||||
{
|
||||
MainColor = mainColor;
|
||||
AuxiliaryColor = auxiliaryColor;
|
||||
}
|
||||
|
||||
public Color MainColor;
|
||||
public Color AuxiliaryColor;
|
||||
public bool DrawAuxiliary = true;
|
||||
public bool VisualizeDirection = true;
|
||||
}
|
||||
|
||||
public enum Padding
|
||||
{
|
||||
StartLineEndLine = 0b_0001_0001,
|
||||
StartLineEndExtend = 0b_0001_0010,
|
||||
StartExtendEndLine = 0b_0010_0001,
|
||||
StartExtendEndExtend = 0b_0010_0010,
|
||||
}
|
||||
|
||||
public class VisDot
|
||||
{
|
||||
public VisDot(Vector2 point, Color color)
|
||||
{
|
||||
Point = point;
|
||||
Color = color;
|
||||
}
|
||||
|
||||
public Vector2 Point;
|
||||
public Color Color;
|
||||
}
|
||||
|
||||
public class VisLine
|
||||
{
|
||||
public VisLine(Vector2 start, Vector2 end, bool startArrow, bool endArrow, Color color, float width = 1)
|
||||
{
|
||||
Start = start;
|
||||
End = end;
|
||||
StartArrow = startArrow;
|
||||
EndArrow = endArrow;
|
||||
Color = color;
|
||||
Width = width;
|
||||
}
|
||||
|
||||
public Vector2 Start;
|
||||
public Vector2 End;
|
||||
public bool StartArrow = false;
|
||||
public bool EndArrow = false;
|
||||
public Color Color;
|
||||
public float Width;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,334 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Diagnostics;
|
||||
using System.Linq;
|
||||
using System.Numerics;
|
||||
using System.Reflection;
|
||||
using CommonUsage.Mathematics;
|
||||
|
||||
namespace CommonUsage.Geometries
|
||||
{
|
||||
public class BezierCurve : AbstractGeometry
|
||||
{
|
||||
|
||||
public BezierCurve(List<Vector2> controlPoints, int resolution = 100)
|
||||
{
|
||||
// Console.WriteLine($"BezierCurve1");
|
||||
// Console.WriteLine(string.Join(" ",controlPoints.Select(p=>$"{p.X:f2},{p.Y:f2}")));
|
||||
_controlPoints = controlPoints;
|
||||
_resolution = resolution;
|
||||
InitializeBezier();
|
||||
}
|
||||
|
||||
public override void Visualize(Action<VisDot> processDot, Action<VisLine> processLine, bool visExtendedPart = false)
|
||||
{
|
||||
if (VisualizeOption.DrawAuxiliary)
|
||||
for (var i = 0; i < _controlPoints.Count - 1; ++i)
|
||||
{
|
||||
processLine(new VisLine(_controlPoints[i], _controlPoints[i + 1],
|
||||
false, false, VisualizeOption.AuxiliaryColor));
|
||||
if (i == 0) continue;
|
||||
processDot(new VisDot(_controlPoints[i], VisualizeOption.AuxiliaryColor));
|
||||
}
|
||||
|
||||
for (var i = 0; i < _bezierPoints.Count - 1; ++i)
|
||||
{
|
||||
if (Direction == -1)
|
||||
{
|
||||
processLine(new VisLine(_bezierPoints[i + 1], _bezierPoints[i],
|
||||
false, i == (int)(_bezierPoints.Count / 2), VisualizeOption.MainColor, 2));
|
||||
}
|
||||
else
|
||||
{
|
||||
processLine(new VisLine(_bezierPoints[i], _bezierPoints[i + 1],
|
||||
false, i == (int)(_bezierPoints.Count / 2), VisualizeOption.MainColor, 2));
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
public (Vector2 Point, int Id) QueryPoint(Vector2 point)
|
||||
{
|
||||
var p = new Vector2();
|
||||
var id = -1;
|
||||
var bestDistance = float.MaxValue;
|
||||
|
||||
var hashes = _bias.Select(bb => CalculateHash(point, 100, bb.X, bb.Y)).ToList();
|
||||
|
||||
void TryQuery(Dictionary<uint, List<(Vector2 Point, int Id)>> dict, List<uint> hashList)
|
||||
{
|
||||
foreach (var hash in hashList)
|
||||
{
|
||||
if (!dict.TryGetValue(hash, out var ll)) continue;
|
||||
foreach (var (q, qId) in ll)
|
||||
{
|
||||
var d = Vector2.Distance(q, point);
|
||||
if (d < bestDistance)
|
||||
{
|
||||
p = q;
|
||||
id = qId;
|
||||
bestDistance = d;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
//map目前有bug,取消cpu占用也不严重，必要时候在优化
|
||||
// TryQuery(_pointsMappingSmall, hashes);
|
||||
//
|
||||
// if (id == -1)
|
||||
// {
|
||||
// hashes = _bias.Select(bb => CalculateHash(point, 1000, bb.X, bb.Y)).ToList();
|
||||
// TryQuery(_pointsMappingBig, hashes);
|
||||
// }
|
||||
|
||||
if (id == -1)
|
||||
{
|
||||
// todo: improve the way to find closest point if mappings fail
|
||||
(p, id) = _bezierPoints.Select((p, i) => (p, i))
|
||||
.OrderBy(pair => CommonMath.dist(pair.p.X, pair.p.Y, point.X, point.Y)).First();
|
||||
}
|
||||
|
||||
return (p, id);
|
||||
}
|
||||
|
||||
public override (Vector2 Pt, float Angle, float Bias, float Position) QueryTangentPoint(Vector2 point)
|
||||
{
|
||||
var (p, id) = QueryPoint(point);
|
||||
var tangent = _tangents[id];
|
||||
var (bias, lp, fd) = CommonMath.Project2DLine(point, p, tangent);
|
||||
var next = fd > 0 ? id + 1 : id - 1;
|
||||
if (id == 0) next = 1;
|
||||
// Console.WriteLine($"id:{id} next:{next} tangent:{tangent} _tangents.Count:{_tangents.Count}");
|
||||
if (next > 0 && next < _tangents.Count)//线性插值
|
||||
{
|
||||
var (_, _, t) = CommonMath.Project2DLine(point, _bezierPoints[id], _bezierPoints[next]);
|
||||
var partial = t / Vector2.Distance(_bezierPoints[id], _bezierPoints[next]);
|
||||
if (partial >= 0 && partial <= 1)
|
||||
{
|
||||
tangent = CommonMath.RoundTh(_tangents[id] +
|
||||
partial * CommonMath.RoundTh(_tangents[next] - _tangents[id]));
|
||||
if (CommonMath.RoundTh(_tangents[next] - _tangents[id]) > 5)
|
||||
Console.WriteLine($"bezier tangents bug, tanget: {id}:{_tangents[id]} {next}:{_tangents[next]}");
|
||||
}
|
||||
// else Console.WriteLine("bezier tangents bug");
|
||||
}
|
||||
return (lp, tangent, bias, fd + _sumDistances[id]);
|
||||
}
|
||||
|
||||
public override float Length()
|
||||
{
|
||||
return _length;
|
||||
}
|
||||
|
||||
public override float QueryCurvature(float position)
|
||||
{
|
||||
int id = _sumDistances.Count - 1;
|
||||
if (position <= 0) id = 0;
|
||||
else
|
||||
{
|
||||
for (int i = 1; i < _sumDistances.Count; i++)
|
||||
{
|
||||
if (position > _sumDistances[i - 1] && position <= _sumDistances[i])
|
||||
{
|
||||
id = i;
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
var result = _curvatures[id];
|
||||
if (id > 0 && id < _sumDistances.Count - 1)//插值
|
||||
{
|
||||
var partial = (position - _sumDistances[id - 1]) / (_sumDistances[id] - _sumDistances[id - 1]);
|
||||
if (partial >= 0 && partial <= 1) result = (1 - partial) * _curvatures[id - 1] + partial * _curvatures[id];
|
||||
else Console.WriteLine("bezier curvature bug");
|
||||
}
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
public Vector3 QueryBezierPointsById(int id)
|
||||
{
|
||||
if (id < 0 || id > Resolution)
|
||||
{
|
||||
Console.WriteLine($"QueryBezierPointsById out of range, Resolution:{Resolution},id:{id}.");
|
||||
return new Vector3(0, 0, 0);
|
||||
}
|
||||
|
||||
return new Vector3(_bezierPoints[id].X, _bezierPoints[id].Y, _tangents[id]);
|
||||
}
|
||||
|
||||
public List<Vector2> ControlPoints => _controlPoints;
|
||||
|
||||
public int Resolution => _resolution;
|
||||
/// <summary>
|
||||
/// 仅用于simple显示路径方向
|
||||
/// </summary>
|
||||
public int Direction = 1;
|
||||
public int Order => _order;
|
||||
|
||||
// public List<float> Tangents => _tangents;
|
||||
|
||||
public void UpdateControlPoint(int id, Vector2 point)
|
||||
{
|
||||
_controlPoints[id] = point;
|
||||
InitializeBezier();
|
||||
}
|
||||
|
||||
public void AddControlPoint(int id, Vector2 point)
|
||||
{
|
||||
_controlPoints.Insert(id, point);
|
||||
InitializeBezier();
|
||||
}
|
||||
|
||||
public void RemoveControlPoint(int id)
|
||||
{
|
||||
_controlPoints.RemoveAt(id);
|
||||
InitializeBezier();
|
||||
}
|
||||
|
||||
public Vector2 GetMidPoint()
|
||||
{
|
||||
return _bezierPoints[(int)Math.Ceiling(_resolution / 2d)];
|
||||
}
|
||||
|
||||
private void InitializeBezier()
|
||||
{
|
||||
_order = _controlPoints.Count - 1;
|
||||
// _bezierPoints = new List<Vector2>();
|
||||
var delta = 1.0f / _resolution;
|
||||
// for (int t = 0; t <= _resolution; t += 1)//下面循环算了，没必要先递归算一遍
|
||||
// _bezierPoints.Add(new Vector2(DeCasteljauX(_order, 0, t*delta), DeCasteljauY(_order, 0, t*delta)));
|
||||
var allPoints = new List<List<List<Vector2>>>();
|
||||
for (var i = 0; i < _order; i++)
|
||||
{
|
||||
var size = allPoints.Count;
|
||||
var morePoints = new List<List<Vector2>>();
|
||||
for (var j = 0; j < _order - i; j++)
|
||||
{
|
||||
var points = new List<Vector2>();
|
||||
for (int t = 0; t <= _resolution; t += 1)
|
||||
{
|
||||
float p0x;
|
||||
float p1x;
|
||||
float p0y;
|
||||
float p1y;
|
||||
var z = t;
|
||||
if (size > 0)
|
||||
{
|
||||
p0x = allPoints[i - 1][j][z].X;
|
||||
p1x = allPoints[i - 1][j + 1][z].X;
|
||||
p0y = allPoints[i - 1][j][z].Y;
|
||||
p1y = allPoints[i - 1][j + 1][z].Y;
|
||||
}
|
||||
else
|
||||
{
|
||||
p0x = _controlPoints[j].X;
|
||||
p1x = _controlPoints[j + 1].X;
|
||||
p0y = _controlPoints[j].Y;
|
||||
p1y = _controlPoints[j + 1].Y;
|
||||
}
|
||||
|
||||
var part = t * delta;
|
||||
points.Add(new Vector2((1 - part) * p0x + part * p1x, (1 - part) * p0y + part * p1y));
|
||||
}
|
||||
morePoints.Add(points);
|
||||
}
|
||||
allPoints.Add(morePoints);
|
||||
}
|
||||
|
||||
_bezierPoints = allPoints.Last().Last();
|
||||
_tangentInfo = allPoints;
|
||||
_tangents = Enumerable.Repeat(0f, _bezierPoints.Count).ToList();
|
||||
_curvatures = Enumerable.Repeat(0f, _bezierPoints.Count).ToList();
|
||||
var p2 = allPoints[Order - 2];
|
||||
for (var id = 0; id < _bezierPoints.Count; ++id)
|
||||
{
|
||||
_tangents[id] =
|
||||
(float)(Math.Atan2(p2[1][id].Y - p2[0][id].Y, p2[1][id].X - p2[0][id].X) / Math.PI * 180);
|
||||
if (id != 0) _curvatures[id] = (float)((CommonMath.ThDiff(_tangents[id], _tangents[id - 1]) / 180 * Math.PI)
|
||||
/ (Vector2.Distance(_bezierPoints[id], _bezierPoints[id - 1]) / 1000));
|
||||
}
|
||||
// Console.WriteLine($"{string.Join("\n", _tangents.Select((val, i) => $"{i}: {val}"))}");
|
||||
_tangents[0] = _tangents[1]; // todo: here is temporary fix
|
||||
_curvatures[0] = _curvatures[1];
|
||||
for (var id = 1; id < _bezierPoints.Count - 1; ++id)//前移0.5
|
||||
_curvatures[id] = (_curvatures[id] + _curvatures[id + 1]) / 2;
|
||||
|
||||
_remainDistances = Enumerable.Repeat(0f, _bezierPoints.Count).ToList();
|
||||
_sumDistances = Enumerable.Repeat(0f, _bezierPoints.Count).ToList();
|
||||
for (var i = _bezierPoints.Count - 2; i >= 0; --i)
|
||||
{
|
||||
_remainDistances[i] =
|
||||
_remainDistances[i + 1] + Vector2.Distance(_bezierPoints[i], _bezierPoints[i + 1]);
|
||||
}
|
||||
for (var i = 1; i < _bezierPoints.Count; ++i)
|
||||
{
|
||||
_sumDistances[i] =
|
||||
_sumDistances[i - 1] + Vector2.Distance(_bezierPoints[i], _bezierPoints[i - 1]);
|
||||
}
|
||||
_length = _sumDistances.Last();
|
||||
|
||||
_minX = _bezierPoints.Min(pp => pp.X);
|
||||
_minY = _bezierPoints.Min(pp => pp.Y);
|
||||
|
||||
var tmpList = _bezierPoints.Select((point, index) => (point, index)).ToList();
|
||||
return;
|
||||
void GenerateGridMapping(ref Dictionary<uint, List<(Vector2 Point, int Id)>> dict, float gSize)
|
||||
{
|
||||
dict = new Dictionary<uint, List<(Vector2 Point, int Id)>>();
|
||||
foreach (var (point, index) in tmpList)
|
||||
{
|
||||
var hash = CalculateHash(point, gSize);
|
||||
if (dict.TryGetValue(hash, out var ll))
|
||||
ll.Add((point, index));
|
||||
else dict[hash] = new List<(Vector2 Point, int Id)>() { (point, index) };
|
||||
}
|
||||
}
|
||||
|
||||
GenerateGridMapping(ref _pointsMappingSmall, 100);
|
||||
GenerateGridMapping(ref _pointsMappingBig, 1000);
|
||||
}
|
||||
|
||||
private uint CalculateHash(Vector2 point, float gridSize, int xBias = 0, int yBias = 0)
|
||||
{
|
||||
return (uint)(((int)((point.X - _minX) / gridSize) + xBias) << 16 + (((int)((point.Y - _minY) / gridSize) + yBias) & 0xffff));
|
||||
}
|
||||
|
||||
private readonly List<(int X, int Y)> _bias = new()
|
||||
{
|
||||
new(-1, -1), new(-1, 0), new(-1, 1),
|
||||
new(0, -1), new(0, 0), new(0, 1),
|
||||
new(1, -1), new(1, 0), new(1, 1),
|
||||
};
|
||||
|
||||
private float DeCasteljauX(int i, int j, float t)
|
||||
{
|
||||
if (i == 1)
|
||||
return (1 - t) * _controlPoints[j].X + t * _controlPoints[j + 1].X;
|
||||
return (1 - t) * DeCasteljauX(i - 1, j, t) + t * DeCasteljauX(i - 1, j + 1, t);
|
||||
}
|
||||
|
||||
private float DeCasteljauY(int i, int j, float t)
|
||||
{
|
||||
if (i == 1)
|
||||
return (1 - t) * _controlPoints[j].Y + t * _controlPoints[j + 1].Y;
|
||||
return (1 - t) * DeCasteljauY(i - 1, j, t) + t * DeCasteljauY(i - 1, j + 1, t);
|
||||
}
|
||||
|
||||
private int _order;
|
||||
private int _resolution;
|
||||
private List<Vector2> _controlPoints;
|
||||
private List<Vector2> _bezierPoints;
|
||||
private List<List<List<Vector2>>> _tangentInfo;
|
||||
private List<float> _tangents;
|
||||
private List<float> _remainDistances;
|
||||
private List<float> _sumDistances;
|
||||
private List<float> _curvatures;
|
||||
private Dictionary<uint, List<(Vector2 Point, int Id)>> _pointsMappingSmall;
|
||||
private Dictionary<uint, List<(Vector2 Point, int Id)>> _pointsMappingBig;
|
||||
private float _minX, _minY;
|
||||
private float _length;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,256 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Drawing;
|
||||
using System.Numerics;
|
||||
using System.Reflection;
|
||||
using System.Security.Cryptography;
|
||||
using System.Text;
|
||||
using CommonUsage.Mathematics;
|
||||
|
||||
namespace CommonUsage.Geometries
|
||||
{
|
||||
public class CircularArc : AbstractGeometry
|
||||
{
|
||||
/// <summary>
|
||||
/// 以center为圆心、radius为半径，从angleStart逆时针转到angleEnd所构成的圆弧。direction表示圆弧走向。
|
||||
/// </summary>
|
||||
/// <param name="center"></param>
|
||||
/// <param name="radius"></param>
|
||||
/// <param name="angleStart"></param>
|
||||
/// <param name="angleEnd"></param>
|
||||
/// <param name="direction">表示圆弧走向，1为angleStart到angleEnd，-1为angleEnd到angleStart</param>
|
||||
public CircularArc(Vector2 center, float radius, float angleStart, float angleEnd, int direction, Padding paddingType)
|
||||
{
|
||||
_center = center;
|
||||
_radius = radius;
|
||||
_angleStart = angleStart;
|
||||
_angleEnd = angleEnd;
|
||||
_direction = direction;
|
||||
PaddingType = paddingType;
|
||||
|
||||
ChangeShape();
|
||||
CalculateVisPoints();
|
||||
}
|
||||
|
||||
public override void Visualize(Action<VisDot> processDot, Action<VisLine> processLine,
|
||||
bool visExtendedPart = false)
|
||||
{
|
||||
lock (_visPoints)
|
||||
{
|
||||
if (visExtendedPart)
|
||||
{
|
||||
|
||||
}
|
||||
|
||||
for (var i = 0; i < _visPoints.Length - 1; ++i)
|
||||
{
|
||||
if ((i == 0 || i == _visPoints.Length - 2) && !visExtendedPart) continue;
|
||||
var color = Color.Red;
|
||||
if (i == 0 || i == _visPoints.Length - 2) color = Color.Gray;
|
||||
processLine(new VisLine(_visPoints[i], _visPoints[i + 1],
|
||||
false, i == (_visPoints.Length - 1) / 2, color));
|
||||
}
|
||||
|
||||
if (visExtendedPart)
|
||||
{
|
||||
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
public void SwitchSide()
|
||||
{
|
||||
(_angleStart, _angleEnd) = (_angleEnd, _angleStart);
|
||||
ChangeShape();
|
||||
CalculateVisPoints();
|
||||
}
|
||||
|
||||
public float VisAngleResolution = 1;
|
||||
|
||||
public Vector2 Center
|
||||
{
|
||||
get => _center;
|
||||
set
|
||||
{
|
||||
_center = value;
|
||||
ChangeShape();
|
||||
CalculateVisPoints();
|
||||
}
|
||||
}
|
||||
|
||||
public float Radius
|
||||
{
|
||||
get => _radius;
|
||||
set
|
||||
{
|
||||
_radius = value;
|
||||
ChangeShape();
|
||||
CalculateVisPoints();
|
||||
}
|
||||
}
|
||||
|
||||
public float AngleStart
|
||||
{
|
||||
get => _angleStart;
|
||||
set
|
||||
{
|
||||
_angleStart = value;
|
||||
ChangeShape();
|
||||
CalculateVisPoints();
|
||||
}
|
||||
}
|
||||
|
||||
public float AngleEnd
|
||||
{
|
||||
get => _angleEnd;
|
||||
set
|
||||
{
|
||||
_angleEnd = value;
|
||||
ChangeShape();
|
||||
CalculateVisPoints();
|
||||
}
|
||||
}
|
||||
|
||||
public int Direction
|
||||
{
|
||||
get => _direction;
|
||||
set
|
||||
{
|
||||
_direction = value;
|
||||
ChangeShape();
|
||||
CalculateVisPoints();
|
||||
}
|
||||
}
|
||||
|
||||
public float AngleRange => _totalTh;
|
||||
|
||||
public Vector2 PointStart => _center + new Vector2(_radius * (float)Math.Cos(_angleStart / 180 * Math.PI),
|
||||
_radius * (float)Math.Sin(_angleStart / 180 * Math.PI));
|
||||
|
||||
public Vector2 PointEnd => _center + new Vector2(_radius * (float)Math.Cos(_angleEnd / 180 * Math.PI),
|
||||
_radius * (float)Math.Sin(_angleEnd / 180 * Math.PI));
|
||||
|
||||
public Vector2 Src => _src;
|
||||
|
||||
public Vector2 Dst => _dst;
|
||||
|
||||
public float TangentSrc => _tangentSrc;
|
||||
|
||||
public float TangentDst => _tangentDst;
|
||||
|
||||
public override (Vector2 Pt, float Angle, float Bias, float Position) QueryTangentPoint(Vector2 point)
|
||||
{
|
||||
var queryTh = (float)(Math.Atan2(point.Y - _center.Y, point.X - _center.X) / Math.PI * 180);
|
||||
|
||||
var p = new Vector2();
|
||||
var tangent = 0f;
|
||||
var pd = 0f;
|
||||
var bestBias = float.MaxValue;
|
||||
|
||||
if ((((int)PaddingType >> 4) & 0x1) == 1)
|
||||
{
|
||||
var (bias1, hPnt1, fd1) = CommonMath.Project2DLine(point, _beforeStartSrc, _src);
|
||||
if (fd1 <= 1000)
|
||||
{
|
||||
p = hPnt1;
|
||||
tangent = (_direction >= 0 ? _angleStart : _angleEnd) + 90 * _direction;
|
||||
pd = fd1;
|
||||
bestBias = bias1;
|
||||
}
|
||||
}
|
||||
|
||||
if (((int)PaddingType & 0x1) == 1)
|
||||
{
|
||||
var (bias2, hPnt2, fd2) = CommonMath.Project2DLine(point, _dst, _afterEndDst);
|
||||
if (fd2 >= 0 && Math.Abs(bias2) < Math.Abs(bestBias))
|
||||
{
|
||||
p = hPnt2;
|
||||
tangent = (_direction >= 0 ? _angleEnd : _angleStart) + 90 * _direction;
|
||||
pd = _totalLen + fd2;
|
||||
bestBias = bias2;
|
||||
}
|
||||
}
|
||||
|
||||
var th1 = _direction == 1 ? CommonMath.ThDiff(queryTh, _angleStart) : CommonMath.ThDiff(_angleEnd, queryTh);
|
||||
// todo: urgent bug! should use better strategy to prevent sign problem
|
||||
if (th1 < -55) th1 += 360;
|
||||
var arcBias = (_radius - Vector2.Distance(point, _center)) * _direction;
|
||||
if (Math.Abs(arcBias) < Math.Abs(bestBias))
|
||||
{
|
||||
p = _center + _radius * new Vector2((float)Math.Cos(queryTh / 180 * Math.PI),
|
||||
(float)Math.Sin(queryTh / 180 * Math.PI));
|
||||
tangent = queryTh + 90 * _direction;
|
||||
pd = _radius * th1 / 180 * (float)Math.PI;
|
||||
bestBias = arcBias;
|
||||
}
|
||||
|
||||
return (p, tangent, bestBias, pd);
|
||||
}
|
||||
|
||||
public override float QueryCurvature(float position)
|
||||
{
|
||||
// var theta = (float)(_angleEnd - position / _radius / Math.PI * 180f + Math.PI);
|
||||
// return Vectoriel.FromAngleLen(theta, 1f / _radius);
|
||||
return 1000f / _radius * _direction;
|
||||
}
|
||||
|
||||
public override float Length()
|
||||
{
|
||||
return _totalLen;
|
||||
}
|
||||
|
||||
private void CalculateVisPoints()
|
||||
{
|
||||
lock (_visPoints)
|
||||
{
|
||||
// todo: overlapping start and end is problematic
|
||||
var ptCnt = (int)Math.Ceiling((_angleEnd + 360 - _angleStart) % 360 / VisAngleResolution);
|
||||
_visPoints = new Vector2[ptCnt + 2];
|
||||
|
||||
var starting = _angleStart;
|
||||
if (_direction == -1) starting = _angleEnd;
|
||||
_visPoints[0] = _beforeStartSrc;
|
||||
|
||||
for (var j = 0; j < ptCnt; ++j)
|
||||
{
|
||||
var th = starting + j * VisAngleResolution * _direction;
|
||||
var radAngle = (float)(th / 180f * Math.PI);
|
||||
_visPoints[j + 1] = Center + new Vector2((float)Math.Cos(radAngle), (float)Math.Sin(radAngle)) * Radius;
|
||||
}
|
||||
|
||||
_visPoints[ptCnt + 1] = _afterEndDst;
|
||||
}
|
||||
}
|
||||
|
||||
private void ChangeShape()
|
||||
{
|
||||
_totalTh = CommonMath.ThDiff(_angleEnd, _angleStart);
|
||||
if (_totalTh < 0) _totalTh += 360;
|
||||
_totalLen = _radius * _totalTh / 180 * (float)Math.PI;
|
||||
|
||||
var radAngleStart = _angleStart / 180 * Math.PI;
|
||||
var radAngleEnd = _angleEnd / 180 * Math.PI;
|
||||
double srcAngle = radAngleStart, dstAngle = radAngleEnd;
|
||||
if (_direction == -1) (srcAngle, dstAngle) = (dstAngle, srcAngle);
|
||||
_src = _center + new Vector2((float)Math.Cos(srcAngle), (float)Math.Sin(srcAngle)) * _radius;
|
||||
_dst = _center + new Vector2((float)Math.Cos(dstAngle), (float)Math.Sin(dstAngle)) * _radius;
|
||||
_beforeStartSrc = CommonMath.Transform2D(_src,
|
||||
(_direction >= 0 ? _angleStart : _angleEnd) + 90 * _direction, new Vector2(-1000, 0));
|
||||
_afterEndDst = CommonMath.Transform2D(_dst, (_direction >= 0 ? _angleEnd : _angleStart) + 90 * _direction,
|
||||
new Vector2(1000, 0));
|
||||
_tangentSrc = QueryTangentPoint(_src).Angle;
|
||||
_tangentDst = QueryTangentPoint(_dst).Angle;
|
||||
}
|
||||
|
||||
private Vector2 _center;
|
||||
private float _radius, _angleStart, _angleEnd;
|
||||
private int _direction;
|
||||
|
||||
private float _totalTh, _totalLen;
|
||||
private Vector2 _src, _dst;
|
||||
private float _tangentSrc, _tangentDst;
|
||||
private Vector2 _beforeStartSrc, _afterEndDst;
|
||||
|
||||
private Vector2[] _visPoints = Array.Empty<Vector2>();
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,349 @@
|
||||
using CommonUsage.Mathematics;
|
||||
using System.Collections.Generic;
|
||||
using System.Numerics;
|
||||
using System;
|
||||
using System.Linq;
|
||||
|
||||
namespace CommonUsage.Geometries
|
||||
{
|
||||
public class NurbsCurve : AbstractGeometry
|
||||
{
|
||||
|
||||
public NurbsCurve(List<Vector2> controlPoints, List<float> weights, List<float> knotVector, int frame = 100)
|
||||
{
|
||||
_controlPoints = controlPoints;
|
||||
_weights = weights;
|
||||
_knotVector = knotVector;
|
||||
_frame = frame;
|
||||
InitializeNurbs();
|
||||
}
|
||||
|
||||
public override void Visualize(Action<VisDot> processDot, Action<VisLine> processLine, bool visExtendedPart = false)
|
||||
{
|
||||
if (VisualizeOption.DrawAuxiliary)
|
||||
for (var i = 0; i < _controlPoints.Count - 1; ++i)
|
||||
{
|
||||
processLine(new VisLine(_controlPoints[i], _controlPoints[i + 1],
|
||||
false, false, VisualizeOption.AuxiliaryColor));
|
||||
if (i == 0) continue;
|
||||
processDot(new VisDot(_controlPoints[i], VisualizeOption.AuxiliaryColor));
|
||||
}
|
||||
|
||||
for (var i = 0; i < _nurbsPoints.Count - 1; ++i)
|
||||
{
|
||||
if (Direction == -1)
|
||||
{
|
||||
processLine(new VisLine(_nurbsPoints[i + 1], _nurbsPoints[i],
|
||||
false, i == (int)(_nurbsPoints.Count / 2), VisualizeOption.MainColor, 2));
|
||||
}
|
||||
else
|
||||
{
|
||||
processLine(new VisLine(_nurbsPoints[i], _nurbsPoints[i + 1],
|
||||
false, i == (int)(_nurbsPoints.Count / 2), VisualizeOption.MainColor, 2));
|
||||
}
|
||||
}
|
||||
}
|
||||
public (Vector2 Point, int Id) QueryPoint(Vector2 point)
|
||||
{
|
||||
var p = new Vector2();
|
||||
var id = -1;
|
||||
var bestDistance = float.MaxValue;
|
||||
|
||||
var hashes = _bias.Select(bb => CalculateHash(point, 100, bb.X, bb.Y)).ToList();
|
||||
|
||||
void TryQuery(Dictionary<uint, List<(Vector2 Point, int Id)>> dict, List<uint> hashList)
|
||||
{
|
||||
foreach (var hash in hashList)
|
||||
{
|
||||
if (!dict.TryGetValue(hash, out var ll)) continue;
|
||||
foreach (var (q, qId) in ll)
|
||||
{
|
||||
var d = Vector2.Distance(q, point);
|
||||
if (d < bestDistance)
|
||||
{
|
||||
p = q;
|
||||
id = qId;
|
||||
bestDistance = d;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (id == -1)
|
||||
{
|
||||
// todo: improve the way to find closest point if mappings fail
|
||||
(p, id) = _nurbsPoints.Select((p, i) => (p, i))
|
||||
.OrderBy(pair => CommonMath.dist(pair.p.X, pair.p.Y, point.X, point.Y)).First();
|
||||
}
|
||||
|
||||
return (p, id);
|
||||
}
|
||||
|
||||
private uint CalculateHash(Vector2 point, float gridSize, int xBias = 0, int yBias = 0)
|
||||
{
|
||||
return (uint)(((int)((point.X - _minX) / gridSize) + xBias) << 16 + (((int)((point.Y - _minY) / gridSize) + yBias) & 0xffff));
|
||||
}
|
||||
|
||||
private readonly List<(int X, int Y)> _bias = new()
|
||||
{
|
||||
new(-1, -1), new(-1, 0), new(-1, 1),
|
||||
new(0, -1), new(0, 0), new(0, 1),
|
||||
new(1, -1), new(1, 0), new(1, 1),
|
||||
};
|
||||
|
||||
public override (Vector2 Pt, float Angle, float Bias, float Position) QueryTangentPoint(Vector2 point)
|
||||
{
|
||||
var (p, id) = QueryPoint(point);
|
||||
var tangent = _tangents[id];
|
||||
var (bias, lp, fd) = CommonMath.Project2DLine(point, p, tangent);
|
||||
var next = fd > 0 ? id + 1 : id - 1;
|
||||
if (next > 0 && next < _tangents.Count)//线性插值
|
||||
{
|
||||
var (_, _, t) = CommonMath.Project2DLine(point, _nurbsPoints[id], _nurbsPoints[next]);
|
||||
var partial = t / Vector2.Distance(_nurbsPoints[id], _nurbsPoints[next]);
|
||||
if (partial >= 0 && partial <= 1)
|
||||
{
|
||||
tangent = CommonMath.RoundTh(_tangents[id] +
|
||||
partial * CommonMath.RoundTh(_tangents[next] - _tangents[id]));
|
||||
if (CommonMath.RoundTh(_tangents[next] - _tangents[id]) > 5)
|
||||
Console.WriteLine($"Nurbs tangents bug, tanget: {id}:{_tangents[id]} {next}:{_tangents[next]}");
|
||||
}
|
||||
else Console.WriteLine("Nurbs tangents bug");
|
||||
}
|
||||
return (lp, tangent, bias, fd + _sumDistances[id]);
|
||||
}
|
||||
|
||||
public override float QueryCurvature(float position)
|
||||
{
|
||||
int id = _sumDistances.Count - 1;
|
||||
if (position <= 0) id = 0;
|
||||
else
|
||||
{
|
||||
for (int i = 1; i < _sumDistances.Count; i++)
|
||||
{
|
||||
if (position > _sumDistances[i - 1] && position <= _sumDistances[i])
|
||||
{
|
||||
id = i;
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
var result = _curvatures[id];
|
||||
if (id > 0 && id < _sumDistances.Count - 1)//插值
|
||||
{
|
||||
var partial = (position - _sumDistances[id - 1]) / (_sumDistances[id] - _sumDistances[id - 1]);
|
||||
if (partial >= 0 && partial <= 1) result = (1 - partial) * _curvatures[id - 1] + partial * _curvatures[id];
|
||||
else Console.WriteLine("Nurbs curvature bug");
|
||||
}
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
public Vector3 QueryNurbsPointsById(int id)
|
||||
{
|
||||
if (id < 0 || id > Frame)
|
||||
{
|
||||
Console.WriteLine($"QueryBezierPointsById out of range, Resolution:{Frame},id:{id}.");
|
||||
return new Vector3(0, 0, 0);
|
||||
}
|
||||
|
||||
return new Vector3(_nurbsPoints[id].X, _nurbsPoints[id].Y, _tangents[id]);
|
||||
}
|
||||
public override float Length()
|
||||
{
|
||||
return _length;
|
||||
}
|
||||
|
||||
public int Order => _order;
|
||||
public List<Vector2> ControlPoints => _controlPoints;
|
||||
public List<float> Weights => _weights;
|
||||
public List<float> KnotVector => _knotVector;
|
||||
public int Frame => _frame;
|
||||
|
||||
public int Direction = 1;
|
||||
|
||||
|
||||
public void UpdateControlPoint(int id, Vector2 point)
|
||||
{
|
||||
_controlPoints[id] = point;
|
||||
InitializeNurbs();
|
||||
}
|
||||
public void UpdateNurbsWeihgts(int id, float weight)
|
||||
{
|
||||
_weights[id] = weight;
|
||||
InitializeNurbs();
|
||||
|
||||
}
|
||||
public void AddControlPoint(int id, Vector2 point)
|
||||
{
|
||||
_controlPoints.Insert(id, point);
|
||||
InitializeNurbs();
|
||||
}
|
||||
|
||||
public void RemoveControlPoint(int id)
|
||||
{
|
||||
_controlPoints.RemoveAt(id);
|
||||
InitializeNurbs();
|
||||
}
|
||||
|
||||
public Vector2 GetMidPoint()
|
||||
{
|
||||
return _nurbsPoints[(int)Math.Ceiling(_frame / 2d)];
|
||||
}
|
||||
|
||||
private void InitializeNurbs()
|
||||
{
|
||||
_order = _controlPoints.Count - 1;
|
||||
List<List<Vector2>> allpoints = new List<List<Vector2>>();
|
||||
List<Vector2> nurbsCurvePoints = new List<Vector2>();
|
||||
float delta = 1.0f / Frame;
|
||||
|
||||
for (float t = 0; t <= 1; t += delta)
|
||||
{
|
||||
var (point, tangent) = DeBoorAlgorithm(t);
|
||||
var points = new List<Vector2>
|
||||
{
|
||||
point,
|
||||
point + tangent // Tangent endpoint
|
||||
};
|
||||
allpoints.Add(points);
|
||||
nurbsCurvePoints.Add(point); // Store the curve point separately
|
||||
}
|
||||
|
||||
_nurbsPoints = nurbsCurvePoints;
|
||||
_tangents = Enumerable.Repeat(0f, _nurbsPoints.Count).ToList();
|
||||
_curvatures = Enumerable.Repeat(0f, _nurbsPoints.Count).ToList();
|
||||
|
||||
for (var id = 0; id < _nurbsPoints.Count - 1; ++id)
|
||||
{
|
||||
Vector2 p1 = _nurbsPoints[id];
|
||||
Vector2 p2 = _nurbsPoints[id + 1];
|
||||
|
||||
float tangentAngle = (float)Math.Atan2(p2.Y - p1.Y, p2.X - p1.X) * 180 / (float)Math.PI;
|
||||
_tangents[id] = tangentAngle;
|
||||
|
||||
// Calculate curvature using finite differences of tangent (second derivative approximation)
|
||||
if (id > 0)
|
||||
{
|
||||
float previousTangent = _tangents[id - 1];
|
||||
float curvature = (float)(CommonMath.ThDiff(tangentAngle, previousTangent) * Math.PI / 180) /
|
||||
(Vector2.Distance(p1, p2) / 1000);
|
||||
_curvatures[id] = curvature;
|
||||
}
|
||||
}
|
||||
|
||||
_curvatures.Insert(0, _curvatures[0]);
|
||||
for (var id = 1; id < _curvatures.Count - 1; ++id)
|
||||
{
|
||||
_curvatures[id] = (_curvatures[id] + _curvatures[id + 1]) / 2;
|
||||
}
|
||||
|
||||
_remainDistances = Enumerable.Repeat(0f, _nurbsPoints.Count).ToList();
|
||||
_sumDistances = Enumerable.Repeat(0f, _nurbsPoints.Count).ToList();
|
||||
_remainDistances[_nurbsPoints.Count - 1] = 0;
|
||||
|
||||
for (var i = _nurbsPoints.Count - 2; i >= 0; --i)
|
||||
{
|
||||
_remainDistances[i] = _remainDistances[i + 1] + Vector2.Distance(_nurbsPoints[i], _nurbsPoints[i + 1]);
|
||||
}
|
||||
|
||||
_sumDistances[0] = 0;
|
||||
for (var i = 1; i < _nurbsPoints.Count; ++i)
|
||||
{
|
||||
_sumDistances[i] = _sumDistances[i - 1] + Vector2.Distance(_nurbsPoints[i], _nurbsPoints[i - 1]);
|
||||
}
|
||||
|
||||
_length = _sumDistances.Last();
|
||||
|
||||
_minX = _nurbsPoints.Min(pp => pp.X);
|
||||
_minY = _nurbsPoints.Min(pp => pp.Y);
|
||||
|
||||
var tmpList = _nurbsPoints.Select((point, index) => (point, index)).ToList();
|
||||
// GenerateGridMapping(ref _pointsMappingSmall, 100, tmpList);
|
||||
// GenerateGridMapping(ref _pointsMappingBig, 1000, tmpList);
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
private float CalculateLength()
|
||||
{
|
||||
return _nurbsPoints.Zip(_nurbsPoints.Skip(1), Vector2.Distance).Sum();
|
||||
}
|
||||
|
||||
private (Vector2, Vector2) DeBoorAlgorithm(float t)
|
||||
{
|
||||
Vector2 numerator = Vector2.Zero;
|
||||
Vector2 tangentNumerator = Vector2.Zero;
|
||||
float denominator = 0f;
|
||||
|
||||
// Calculate the point on the curve
|
||||
for (int i = 0; i < ControlPoints.Count; ++i)
|
||||
{
|
||||
float basis = BasisFunction(i, _order, t) * Weights[i];
|
||||
numerator += basis * ControlPoints[i];
|
||||
denominator += basis;
|
||||
}
|
||||
|
||||
Vector2 point = numerator / denominator;
|
||||
|
||||
// Calculate the tangent vector using the analytical derivative
|
||||
for (int i = 0; i < ControlPoints.Count; ++i)
|
||||
{
|
||||
float basisDerivative = BasisFunctionDerivative(i, _order, t) * Weights[i];
|
||||
tangentNumerator += basisDerivative * ControlPoints[i];
|
||||
}
|
||||
Vector2 tangent = tangentNumerator / denominator;
|
||||
|
||||
return (point, tangent);
|
||||
}
|
||||
|
||||
private float BasisFunction(int i, int p, float t)
|
||||
{
|
||||
if (p == 0)
|
||||
return (KnotVector[i] <= t && t < KnotVector[i + 1]) ? 1.0f : 0.0f;
|
||||
|
||||
float denom1 = KnotVector[i + p] - KnotVector[i];
|
||||
float term1 = denom1 == 0 ? 0 : ((t - KnotVector[i]) / denom1) * BasisFunction(i, p - 1, t);
|
||||
|
||||
float denom2 = KnotVector[i + p + 1] - KnotVector[i + 1];
|
||||
float term2 = denom2 == 0 ? 0 : ((KnotVector[i + p + 1] - t) / denom2) * BasisFunction(i + 1, p - 1, t);
|
||||
|
||||
return term1 + term2;
|
||||
}
|
||||
|
||||
private float BasisFunctionDerivative(int i, int k, float t)
|
||||
{
|
||||
if (k == 0) return 0;
|
||||
|
||||
float denom1 = KnotVector[i + k] - KnotVector[i];
|
||||
float denom2 = KnotVector[i + k + 1] - KnotVector[i + 1];
|
||||
|
||||
float term1 = denom1 != 0 ? BasisFunction(i, k - 1, t) / denom1 : 0;
|
||||
float term2 = denom1 != 0 ? (t - KnotVector[i]) * BasisFunctionDerivative(i, k - 1, t) / denom1 : 0;
|
||||
|
||||
float term3 = denom2 != 0 ? -BasisFunction(i + 1, k - 1, t) / denom2 : 0;
|
||||
float term4 = denom2 != 0 ? (KnotVector[i + k + 1] - t) * BasisFunctionDerivative(i + 1, k - 1, t) / denom2 : 0;
|
||||
|
||||
return term1 + term2 + term3 + term4;
|
||||
}
|
||||
|
||||
private int _order;
|
||||
private List<Vector2> _controlPoints;
|
||||
private List<float> _weights;
|
||||
private List<float> _knotVector;
|
||||
private int _frame;
|
||||
private List<Vector2> _nurbsPoints;
|
||||
private List<List<List<Vector2>>> _tangentPoints;
|
||||
private List<float> _curvatures;
|
||||
private List<float> _sumDistances;
|
||||
private List<float> _remainDistances;
|
||||
private float _minX, _minY;
|
||||
private float _length;
|
||||
private Dictionary<uint, List<(Vector2 Point, int Id)>> _pointsMappingSmall;
|
||||
private Dictionary<uint, List<(Vector2 Point, int Id)>> _pointsMappingBig;
|
||||
private List<float> _tangents;
|
||||
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,71 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Numerics;
|
||||
using System.Text;
|
||||
|
||||
namespace CommonUsage.Geometries
|
||||
{
|
||||
/// <summary>
|
||||
/// MDCS数学类：向量。
|
||||
/// </summary>
|
||||
public class Vectoriel
|
||||
{
|
||||
public Vectoriel()
|
||||
{
|
||||
_vec2 = Vector2.Zero;
|
||||
_dir2 = Vector2.Normalize(_vec2);
|
||||
_len = _vec2.Length();
|
||||
_angle = (float)(Math.Atan2(_vec2.Y, _vec2.X) / Math.PI * 180f);
|
||||
}
|
||||
|
||||
public Vectoriel(Vector2 vec)
|
||||
{
|
||||
_vec2 = vec;
|
||||
_dir2 = Vector2.Normalize(_vec2);
|
||||
_len = _vec2.Length();
|
||||
_angle = (float)(Math.Atan2(_vec2.Y, _vec2.X) / Math.PI * 180f);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 通过笛卡尔坐标系X和Y值构建向量。
|
||||
/// </summary>
|
||||
/// <param name="x"></param>
|
||||
/// <param name="y"></param>
|
||||
/// <returns></returns>
|
||||
public static Vectoriel FromXY(float x, float y)
|
||||
{
|
||||
return new Vectoriel(new Vector2(x, y));
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 通过极坐标系的角度和距离值构建向量。
|
||||
/// </summary>
|
||||
/// <param name="angle"></param>
|
||||
/// <param name="len"></param>
|
||||
/// <returns></returns>
|
||||
public static Vectoriel FromAngleLen(float angle, float len)
|
||||
{
|
||||
var rad = angle / 180f * Math.PI;
|
||||
return new Vectoriel(new Vector2((float)Math.Cos(rad), (float)Math.Sin(rad)) * len);
|
||||
}
|
||||
|
||||
public static implicit operator Vector2(Vectoriel vec)
|
||||
{
|
||||
return vec._vec2;
|
||||
}
|
||||
|
||||
public static explicit operator Vectoriel(Vector2 vec)
|
||||
{
|
||||
return FromXY(vec.X, vec.Y);
|
||||
}
|
||||
|
||||
public Vector2 Direction => _dir2;
|
||||
|
||||
public float Length => _len;
|
||||
|
||||
public float Angle => _angle;
|
||||
|
||||
private Vector2 _vec2, _dir2;
|
||||
private float _len, _angle;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,774 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Drawing;
|
||||
using System.Linq;
|
||||
using System.Numerics;
|
||||
using System.Reflection;
|
||||
using System.Runtime.CompilerServices;
|
||||
using System.Runtime.InteropServices;
|
||||
|
||||
namespace CommonUsage.Mathematics
|
||||
{
|
||||
|
||||
using T3 = Tuple<float, float, float>;
|
||||
using D3 = Tuple<double, double, double>;
|
||||
|
||||
public class CommonMath
|
||||
{
|
||||
public class PrimeEnumerator<T>
|
||||
{
|
||||
public PrimeEnumerator(List<T> items, Func<T, bool> process)
|
||||
{
|
||||
_n = items.Count;
|
||||
_items = items;
|
||||
_process = process;
|
||||
|
||||
foreach (var pNum in _primes)
|
||||
{
|
||||
if (_n % pNum != 0)
|
||||
{
|
||||
_a = pNum;
|
||||
_b = 11;
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
public void Enumerate()
|
||||
{
|
||||
using (var enumerator = Get().GetEnumerator())
|
||||
{
|
||||
while (enumerator.MoveNext()) { }
|
||||
}
|
||||
}
|
||||
|
||||
private readonly int _n, _a, _b;
|
||||
private readonly int[] _primes = new[] { 29, 23, 19, 17, 13 };
|
||||
private List<T> _items;
|
||||
private readonly Func<T, bool> _process;
|
||||
|
||||
private IEnumerable<bool> Get()
|
||||
{
|
||||
for (var i = 0; i < _n; ++i)
|
||||
{
|
||||
var id = (i * _a + _b) % _n;
|
||||
yield return _process(_items[id]);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
private static IEnumerable<IEnumerable<T>> GetPermutationsInternal<T>(IEnumerable<T> list, int length)
|
||||
{
|
||||
if (length == 1) return list.Select(t => new T[] { t });
|
||||
|
||||
return GetPermutationsInternal(list, length - 1)
|
||||
.SelectMany(t => list.Where(e => !t.Contains(e)),
|
||||
(t1, t2) => t1.Concat(new T[] { t2 }));
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 得到一组数据的所有排列。
|
||||
/// </summary>
|
||||
/// <typeparam name="T">元素数据类型</typeparam>
|
||||
/// <param name="list">所有待选元素</param>
|
||||
/// <param name="selectNum">所选出的元素数量</param>
|
||||
/// <returns></returns>
|
||||
public static List<List<T>> GetPermutations<T>(List<T> list, int selectNum)
|
||||
{
|
||||
return GetPermutationsInternal(list, selectNum).Select(ll => ll.ToList()).ToList();
|
||||
}
|
||||
|
||||
public static (float bias, Vector2 hPnt, float d) Project2DLine(Vector2 pnt, Vector2 segSt,
|
||||
Vector2 segEnd)
|
||||
{
|
||||
var dir = Vector2.Normalize(segEnd - segSt);
|
||||
var fd = Vector2.Dot(pnt - segSt, dir);
|
||||
var hPnt = segSt + fd * dir;
|
||||
var bias = dir.X * (pnt.Y-segSt.Y) - (pnt.X-segSt.X) * dir.Y;
|
||||
return (bias, hPnt, fd);
|
||||
}
|
||||
|
||||
public static (float bias, Vector2 hPnt, float fd) Project2DLine(Vector2 pnt, Vector2 segSt, float tangent)
|
||||
{
|
||||
var dir = new Vector2((float)System.Math.Cos(tangent / 180 * System.Math.PI), (float)System.Math.Sin(tangent / 180 * System.Math.PI));
|
||||
var fd = Vector2.Dot(pnt - segSt, dir);
|
||||
var hPnt = segSt + fd * dir;
|
||||
var bias = dir.X * (pnt.Y - segSt.Y) - (pnt.X - segSt.X) * dir.Y;
|
||||
return (bias, hPnt, fd);
|
||||
}
|
||||
|
||||
public class LineEqu
|
||||
{
|
||||
public double A, B, C, ln, dAB;
|
||||
public double px1, px2, py1, py2;
|
||||
public float midX;
|
||||
public float midY;
|
||||
}
|
||||
// Fit line with PCA.
|
||||
public LineEqu CalcLine(IEnumerable<Vector2> tls)
|
||||
{
|
||||
var lidarPoint2Ds = tls as Vector2[] ?? tls.ToArray();
|
||||
float fx = lidarPoint2Ds.Average(f => f.X);
|
||||
float fy = lidarPoint2Ds.Average(f => f.Y);
|
||||
float fxx = lidarPoint2Ds.Average(f => f.X * f.X);
|
||||
float fxy = lidarPoint2Ds.Average(f => f.X * f.Y);
|
||||
float fyy = lidarPoint2Ds.Average(f => f.Y * f.Y);
|
||||
float a = fxx - fx * fx, b = fxy - fx * fy, c = fyy - fy * fy;
|
||||
double sqt = System.Math.Sqrt((a - c) * (a - c) + 4 * b * b);
|
||||
double l1 = a + c + sqt;
|
||||
double l2 = a + c - sqt;
|
||||
double dx, dy;
|
||||
if (System.Math.Abs(a - l1 / 2) > System.Math.Abs(c - l1 / 2))
|
||||
{
|
||||
dy = l1 / 2 - a; dx = b;
|
||||
}
|
||||
else
|
||||
{
|
||||
dx = l1 / 2 - c; dy = b;
|
||||
}
|
||||
double norm = System.Math.Sqrt(dx * dx + dy * dy);
|
||||
dx /= norm; dy /= norm;
|
||||
double A = dy, B = -dx, C = dx * fy - dy * fx;
|
||||
double dAB = System.Math.Sqrt(A * A + B * B);
|
||||
|
||||
return new CommonMath.LineEqu
|
||||
{
|
||||
A = A,
|
||||
B = B,
|
||||
C = C,
|
||||
ln = lidarPoint2Ds.Average(p => System.Math.Abs(p.X * A + p.Y * B + C) / dAB),
|
||||
midX = fx,
|
||||
midY = fy
|
||||
};
|
||||
}
|
||||
|
||||
public static double QuadInterp3(double[] confsF)
|
||||
{
|
||||
if (confsF[0] > confsF[1] && confsF[0] > confsF[2])
|
||||
{
|
||||
//printf("left overflow...\n");
|
||||
return -1;
|
||||
}
|
||||
|
||||
if (confsF[1] > confsF[0] && confsF[1] > confsF[2])
|
||||
{
|
||||
return (-(confsF[2] - confsF[0]) / 2.0f / (confsF[0] + confsF[2] - 2.0f * confsF[1] + 0.0001f));
|
||||
}
|
||||
if (confsF[2] > confsF[0] && confsF[2] > confsF[1])
|
||||
{
|
||||
//printf("right overflow...\n");
|
||||
return 1;
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
public static double cross(PointF O, PointF A, PointF B)
|
||||
{
|
||||
return (A.X - O.X) * (B.Y - O.Y) - (A.Y - O.Y) * (B.X - O.X);
|
||||
}
|
||||
|
||||
public static List<PointF> GetConvexHull(List<PointF> points)
|
||||
{
|
||||
if (points == null)
|
||||
return null;
|
||||
|
||||
if (points.Count() <= 1)
|
||||
return points;
|
||||
|
||||
int n = points.Count(), k = 0;
|
||||
List<PointF> H = new List<PointF>(new PointF[2 * n]);
|
||||
|
||||
points.Sort((a, b) =>
|
||||
a.X == b.X ? a.Y.CompareTo(b.Y) : a.X.CompareTo(b.X));
|
||||
|
||||
// Build lower hull
|
||||
for (int i = 0; i < n; ++i)
|
||||
{
|
||||
while (k >= 2 && cross(H[k - 2], H[k - 1], points[i]) <= 0)
|
||||
k--;
|
||||
H[k++] = points[i];
|
||||
}
|
||||
|
||||
// Build upper hull
|
||||
for (int i = n - 2, t = k + 1; i >= 0; i--)
|
||||
{
|
||||
while (k >= t && cross(H[k - 2], H[k - 1], points[i]) <= 0)
|
||||
k--;
|
||||
H[k++] = points[i];
|
||||
}
|
||||
|
||||
return H.Take(k - 1).ToList();
|
||||
}
|
||||
|
||||
public static bool IsPointInPolygon4(PointF[] polygon, PointF testPoint)
|
||||
{
|
||||
// ray casting odd even test.
|
||||
bool result = false;
|
||||
int j = polygon.Count() - 1;
|
||||
for (int i = 0; i < polygon.Count(); i++)
|
||||
{
|
||||
if (polygon[i].Y < testPoint.Y && polygon[j].Y >= testPoint.Y ||
|
||||
polygon[j].Y < testPoint.Y && polygon[i].Y >= testPoint.Y)
|
||||
{
|
||||
if (polygon[i].X + (testPoint.Y - polygon[i].Y) / (polygon[j].Y - polygon[i].Y) *
|
||||
(polygon[j].X - polygon[i].X) < testPoint.X)
|
||||
{
|
||||
result = !result;
|
||||
}
|
||||
}
|
||||
|
||||
j = i;
|
||||
}
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
public static double Exp(double val)
|
||||
{
|
||||
if (val < -20) return 0.0000001;
|
||||
if (val > 20) return 99999999999999;
|
||||
long tmp = (long)(1512775 * val + 1072632447);
|
||||
return BitConverter.Int64BitsToDouble(tmp << 32);
|
||||
}
|
||||
public static double gaussmf(double x, double sig, double c)
|
||||
{
|
||||
return Exp(-(x - c) * (x - c) / (2 * sig * sig));
|
||||
}
|
||||
|
||||
public static float Exp(float x)
|
||||
{
|
||||
if (x < -10) return 0;
|
||||
if (x > 10) return 99999999999999;
|
||||
x = 1.0f + x / 64f;
|
||||
x *= x;
|
||||
x *= x;
|
||||
x *= x;
|
||||
x *= x;
|
||||
x *= x;
|
||||
x *= x;
|
||||
return x;
|
||||
}
|
||||
public static float gaussmf(float x, float sig, float c)
|
||||
{
|
||||
return Exp(-(x - c) * (x - c) / (2 * sig * sig));
|
||||
}
|
||||
|
||||
public static D3 Transform2D(D3 src, D3 t)
|
||||
{
|
||||
var rth = src.Item3 / 180.0 * System.Math.PI;
|
||||
var p1dtx = (src.Item1 + System.Math.Cos(rth) * t.Item1 -
|
||||
System.Math.Sin(rth) * t.Item2);
|
||||
var p1dty = (src.Item2 + System.Math.Sin(rth) * t.Item1 +
|
||||
System.Math.Cos(rth) * t.Item2);
|
||||
var p1dtth = src.Item3 + t.Item3;
|
||||
return Tuple.Create(p1dtx, p1dty, p1dtth);
|
||||
}
|
||||
|
||||
public struct LngLatToXY
|
||||
{
|
||||
public double scale;
|
||||
public double rad;
|
||||
public double biasX, biasY;
|
||||
}
|
||||
|
||||
public static LngLatToXY GetTransformLngLatToXY(Vector2 lnglat1, Vector2 xy1, Vector2 lnglat2, Vector2 xy2)
|
||||
{
|
||||
var scale = (xy1 - xy2).Length() / (lnglat1 - lnglat2).Length();
|
||||
var dxy = (xy1 - xy2);
|
||||
var dlnglat = lnglat1 - lnglat2;
|
||||
var rad = System.Math.Atan2(dxy.X, dxy.Y) - System.Math.Atan2(dlnglat.X, dlnglat.Y);
|
||||
var intm = lnglat1 * scale;
|
||||
var biasX = xy1.X - (intm.X * System.Math.Cos(rad) - intm.Y * System.Math.Sin(rad));
|
||||
var biasY = xy1.Y - (intm.X * System.Math.Sin(rad) + intm.Y * System.Math.Cos(rad));
|
||||
return new LngLatToXY {rad = rad, biasX = biasX, biasY = biasY, scale = scale};
|
||||
}
|
||||
|
||||
public Vector2 TransformLngLatToXY(Vector2 lnglat, LngLatToXY t)
|
||||
{
|
||||
var intm = lnglat * (float) t.scale;
|
||||
return new Vector2((float) (intm.X * System.Math.Cos(t.rad) - intm.Y * System.Math.Sin(t.rad) + t.biasX),
|
||||
(float) (intm.X * System.Math.Sin(t.rad) + intm.Y * System.Math.Cos(t.rad) + t.biasY));
|
||||
}
|
||||
|
||||
public static D3 ReverseTransform(D3 dest, D3 t)
|
||||
{
|
||||
var rth = (dest.Item3 - t.Item3) / 180.0 * System.Math.PI;
|
||||
var nxT = (dest.Item1 - System.Math.Cos(rth) * t.Item1 +
|
||||
System.Math.Sin(rth) * t.Item2);
|
||||
var nyT = (dest.Item2 - System.Math.Sin(rth) * t.Item1 -
|
||||
System.Math.Cos(rth) * t.Item2);
|
||||
var pth = dest.Item3 - t.Item3;
|
||||
return Tuple.Create(nxT, nyT, pth);
|
||||
}
|
||||
|
||||
public static D3 SolveTransform2D(D3 src, D3 dest)
|
||||
{
|
||||
var th = dest.Item3 - src.Item3;
|
||||
th = (th - System.Math.Round((th) / 360.0f) * 360);
|
||||
var rth = src.Item3 / 180.0 * System.Math.PI;
|
||||
var x = ((dest.Item1 - src.Item1) * System.Math.Cos(rth) +
|
||||
(dest.Item2 - src.Item2) * System.Math.Sin(rth));
|
||||
var y = (-(dest.Item1 - src.Item1) * System.Math.Sin(rth) +
|
||||
(dest.Item2 - src.Item2) * System.Math.Cos(rth));
|
||||
return Tuple.Create(x, y, th);
|
||||
}
|
||||
|
||||
public static T3 Transform2D(T3 src, T3 t)
|
||||
{
|
||||
var rth = src.Item3 / 180.0 * System.Math.PI;
|
||||
var p1dtx = (float)(src.Item1 + System.Math.Cos(rth) * t.Item1 -
|
||||
System.Math.Sin(rth) * t.Item2);
|
||||
var p1dty = (float)(src.Item2 + System.Math.Sin(rth) * t.Item1 +
|
||||
System.Math.Cos(rth) * t.Item2);
|
||||
var p1dtth = src.Item3 + t.Item3;
|
||||
return Tuple.Create(p1dtx, p1dty, p1dtth);
|
||||
}
|
||||
|
||||
public static Vector2 Transform2D(Vector3 src, Vector3 t)
|
||||
{
|
||||
var tup = Transform2D(Tuple.Create(src.X, src.Y, src.Z), Tuple.Create(t.X, t.Y, t.Z));
|
||||
return new Vector2(tup.Item1, tup.Item2);
|
||||
}
|
||||
|
||||
public static Vector2 Transform2D(Vector2 srcPos, float srcTh, Vector2 dt, float dth = 0)
|
||||
{
|
||||
var tup = Transform2D(Tuple.Create(srcPos.X, srcPos.Y, srcTh), Tuple.Create(dt.X, dt.Y, dth));
|
||||
return new Vector2(tup.Item1, tup.Item2);
|
||||
}
|
||||
|
||||
public static T3 ReverseTransform(T3 dest, T3 t)
|
||||
{
|
||||
var rth = (dest.Item3 - t.Item3) / 180.0 * System.Math.PI;
|
||||
var nxT = (float)(dest.Item1 - System.Math.Cos(rth) * t.Item1 +
|
||||
System.Math.Sin(rth) * t.Item2);
|
||||
var nyT = (float)(dest.Item2 - System.Math.Sin(rth) * t.Item1 -
|
||||
System.Math.Cos(rth) * t.Item2);
|
||||
var pth = dest.Item3 - t.Item3;
|
||||
return Tuple.Create(nxT, nyT, pth);
|
||||
}
|
||||
|
||||
public static T3 SolveTransform2D(T3 src, T3 dest)
|
||||
{
|
||||
var th = dest.Item3 - src.Item3;
|
||||
th = (float)(th - System.Math.Round((th) / 360.0f) * 360);
|
||||
var rth = src.Item3 / 180.0 * System.Math.PI;
|
||||
var x = (float)((dest.Item1 - src.Item1) * System.Math.Cos(rth) +
|
||||
(dest.Item2 - src.Item2) * System.Math.Sin(rth));
|
||||
var y = (float)(-(dest.Item1 - src.Item1) * System.Math.Sin(rth) +
|
||||
(dest.Item2 - src.Item2) * System.Math.Cos(rth));
|
||||
return Tuple.Create(x, y, th);
|
||||
}
|
||||
|
||||
public static Vector2 SolveTransform2D(Vector2 srcPos, float srcTh, Vector2 dt, float dth = 0)
|
||||
{
|
||||
var tup = SolveTransform2D(Tuple.Create(srcPos.X, srcPos.Y, srcTh), Tuple.Create(dt.X, dt.Y, dth));
|
||||
return new Vector2(tup.Item1, tup.Item2);
|
||||
}
|
||||
|
||||
public static double dist(double x1, double y1, double x2, double y2)
|
||||
{
|
||||
return System.Math.Sqrt((x1 - x2) * (x1 - x2) + (y1 - y2) * (y1 - y2));
|
||||
}
|
||||
|
||||
[StructLayout(LayoutKind.Explicit)]
|
||||
private struct FloatIntUnion
|
||||
{
|
||||
[FieldOffset(0)] public float f;
|
||||
|
||||
[FieldOffset(0)] public int tmp;
|
||||
}
|
||||
|
||||
public static float Sqrt(float z)
|
||||
{
|
||||
FloatIntUnion u;
|
||||
u.tmp = 0;
|
||||
u.f = z;
|
||||
u.tmp -= 1 << 23; /* Subtract 2^m. */
|
||||
u.tmp >>= 1; /* Divide by 2. */
|
||||
u.tmp += 1 << 29; /* Add ((b + 1) / 2) * 2^m. */
|
||||
return u.f;
|
||||
}
|
||||
|
||||
public static float dist2(float x1, float y1, float x2, float y2)
|
||||
{
|
||||
return ((x1 - x2) * (x1 - x2) + (y1 - y2) * (y1 - y2));
|
||||
}
|
||||
|
||||
|
||||
public static float d2(float x1, float y1, float x2, float y2)
|
||||
{
|
||||
return (x1 - x2) * (x1 - x2) + (y1 - y2) * (y1 - y2);
|
||||
}
|
||||
|
||||
public static float ThAverage(List<float> angles)
|
||||
{
|
||||
var anchor = angles[0];
|
||||
var diff = 0f;
|
||||
foreach (var angle in angles)
|
||||
diff += ThDiff(angle, anchor);
|
||||
return RoundTh(anchor + diff / angles.Count);
|
||||
}
|
||||
|
||||
public static float ThDiff(float th1, float th2)
|
||||
{
|
||||
return (float)(th1 - th2 -
|
||||
System.Math.Round((th1 - th2) / 360.0f) * 360);
|
||||
}
|
||||
|
||||
public static double ThDiff(double th1, double th2)
|
||||
{
|
||||
return th1 - th2 -
|
||||
System.Math.Round((th1 - th2) / 360.0f) * 360;
|
||||
}
|
||||
|
||||
public static double refine(double x)
|
||||
{
|
||||
if (x < 1 && x > -1) return x;
|
||||
if (x > 1)
|
||||
return (2 / (1 + System.Math.Exp(-((x - 1) * 2))));
|
||||
return (2 / (1 + System.Math.Exp(-((x + 1) * 2)))) - 2;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 求点p到两点式直线p1p2的距离
|
||||
/// </summary>
|
||||
/// <param name="x">点p的x坐标</param>
|
||||
/// <param name="y">点p的y坐标</param>
|
||||
/// <param name="x1">直线点p1的x坐标</param>
|
||||
/// <param name="y1">直线点p1的y坐标</param>
|
||||
/// <param name="x2">直线点p2的x坐标</param>
|
||||
/// <param name="y2">直线点p2的y坐标</param>
|
||||
/// <returns></returns>
|
||||
public static double Point2LineDist(double x, double y, double x1, double y1, double x2, double y2)
|
||||
{
|
||||
double a1 = -(y1 - y2) / 10;
|
||||
double b1 = (x1 - x2) / 10;
|
||||
double c1 = (x1 * (y1 - y2) - y1 * (x1 - x2)) / 10;
|
||||
return System.Math.Abs(a1 * x + b1 * y + c1) / System.Math.Sqrt(a1 * a1 + b1 * b1);
|
||||
}
|
||||
|
||||
public static double Point2LineDist(Vector2 p, LineSegment ll)
|
||||
{
|
||||
double a1 = -(ll.Src.Y - ll.Dst.Y) / 10;
|
||||
double b1 = (ll.Src.X - ll.Dst.X) / 10;
|
||||
double c1 = (ll.Src.X * (ll.Src.Y - ll.Dst.Y) - ll.Src.Y * (ll.Src.X - ll.Dst.X)) / 10;
|
||||
return System.Math.Abs(a1 * p.X + b1 * p.Y + c1) / System.Math.Sqrt(a1 * a1 + b1 * b1);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 两条两点式直线间的夹角
|
||||
/// </summary>
|
||||
/// <param name="x1"></param>
|
||||
/// <param name="y1"></param>
|
||||
/// <param name="x2"></param>
|
||||
/// <param name="y2"></param>
|
||||
/// <param name="x3"></param>
|
||||
/// <param name="y3"></param>
|
||||
/// <param name="x4"></param>
|
||||
/// <param name="y4"></param>
|
||||
/// <returns>角度制</returns>
|
||||
public static double AngleBetweenLines(double x1, double y1, double x2, double y2, double x3, double y3,
|
||||
double x4, double y4)
|
||||
{
|
||||
var vec1 = new Vector2((float)(x2 - x1), (float)(y2 - y1));
|
||||
var vec2 = new Vector2((float)(x4 - x3), (float)(y4 - y3));
|
||||
return System.Math.Acos(System.Math.Abs(Vector2.Dot(vec1, vec2) / vec1.Length() / vec2.Length())) / System.Math.PI * 180;
|
||||
}
|
||||
|
||||
public static double AngleBetweenLines(LineSegment ls1, LineSegment ls2)
|
||||
{
|
||||
return AngleBetweenLines(ls1.Src.X, ls1.Src.Y, ls1.Dst.X, ls1.Dst.Y, ls2.Src.X, ls2.Src.Y, ls2.Dst.X,
|
||||
ls2.Dst.Y);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 两向量间夹角
|
||||
/// </summary>
|
||||
/// <param name="x1"></param>
|
||||
/// <param name="y1"></param>
|
||||
/// <param name="x2"></param>
|
||||
/// <param name="y2"></param>
|
||||
/// <param name="x3"></param>
|
||||
/// <param name="y3"></param>
|
||||
/// <param name="x4"></param>
|
||||
/// <param name="y4"></param>
|
||||
/// <returns>角度制</returns>
|
||||
public static double AngleBetweenVectors(double x1, double y1, double x2, double y2, double x3, double y3,
|
||||
double x4, double y4)
|
||||
{
|
||||
var vec1 = new Vector2((float)(x2 - x1), (float)(y2 - y1));
|
||||
var vec2 = new Vector2((float)(x4 - x3), (float)(y4 - y3));
|
||||
return System.Math.Acos(Vector2.Dot(vec1, vec2) / vec1.Length() / vec2.Length()) / System.Math.PI * 180;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 两向量间夹角.
|
||||
/// </summary>
|
||||
/// <param name="vec1"></param>
|
||||
/// <param name="vec2"></param>
|
||||
/// <returns>角度制</returns>
|
||||
public static double AngleBetweenVectors(Vector2 vec1, Vector2 vec2)
|
||||
{
|
||||
return System.Math.Acos(Vector2.Dot(vec1, vec2) / vec1.Length() / vec2.Length()) / System.Math.PI * 180;
|
||||
}
|
||||
|
||||
public static double AngleBetweenVectors(Vector3 vector1, Vector3 vector2)
|
||||
{
|
||||
float dotProduct = Vector3.Dot(vector1, vector2);
|
||||
float magnitude1 = vector1.Length();
|
||||
float magnitude2 = vector2.Length();
|
||||
float cosine = dotProduct / (magnitude1 * magnitude2);
|
||||
|
||||
return System.Math.Acos(cosine) / System.Math.PI * 180;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 求点到直线的垂足
|
||||
/// </summary>
|
||||
/// <param name="x"></param>
|
||||
/// <param name="y"></param>
|
||||
/// <param name="x1"></param>
|
||||
/// <param name="y1"></param>
|
||||
/// <param name="x2"></param>
|
||||
/// <param name="y2"></param>
|
||||
/// <returns></returns>
|
||||
public static (double, double) PerpendicularPoint(double x, double y, double x1, double y1, double x2, double y2)
|
||||
{
|
||||
double lx = x2 - x1, ly = y2 - y1, dAB = lx * lx + ly * ly;
|
||||
var u = ((x - x1) * lx + (y - y1) * ly) / dAB;
|
||||
return new(x1 + u * lx, y1 + u * ly);
|
||||
}
|
||||
|
||||
public static Vector2 PerpendicularPoint(Vector2 p, LineSegment ls)
|
||||
{
|
||||
double lx = ls.Dst.X - ls.Src.X, ly = ls.Dst.Y - ls.Src.Y, dAB = lx * lx + ly * ly;
|
||||
var u = ((p.X - ls.Src.X) * lx + (p.Y - ls.Src.Y) * ly) / dAB;
|
||||
return new Vector2((float)(ls.Src.X + u * lx), (float)(ls.Src.Y + u * ly));
|
||||
}
|
||||
|
||||
public static double PerpendicularPosition(double x, double y, double x1, double y1, double x2, double y2)
|
||||
{
|
||||
double lx = x2 - x1, ly = y2 - y1;
|
||||
var dAB = CommonMath.Sqrt((float)(lx * lx + ly * ly));
|
||||
lx /= dAB;
|
||||
ly /= dAB;
|
||||
return (x - x1) * lx + (y - y1) * ly;
|
||||
}
|
||||
|
||||
|
||||
/// <summary>
|
||||
/// 最小二乘法拟合直线，得到两点式。
|
||||
/// </summary>
|
||||
/// <param name="pts">待拟合的点集，应至少有2个点。</param>
|
||||
/// <param name="maxDist2Line">检查是否所有点距直线的距离均小于maxDist2Line，若为-1则不检查。</param>
|
||||
/// <returns>返回两点式的两个端点坐标。若坐标为全0，则拟合失败。</returns>
|
||||
public static (bool, Vector2, Vector2) FitLineSegment(List<Vector2> pts, double maxDist2Line = -1)
|
||||
{
|
||||
if (pts.Count < 2)
|
||||
{
|
||||
Console.WriteLine($"Points too Few! {pts.Count}! Cannot perform line fitting!",
|
||||
MethodBase.GetCurrentMethod()?.Name ?? "FitLine");
|
||||
return (false, Vector2.Zero, Vector2.Zero);
|
||||
};
|
||||
|
||||
// y = kx + b
|
||||
double A = 0, B = 0, C = 0, D = 0;
|
||||
foreach (var p in pts)
|
||||
{
|
||||
A += p.X * p.X;
|
||||
B += p.X;
|
||||
C += p.X * p.Y;
|
||||
D += p.Y;
|
||||
}
|
||||
|
||||
var tmp = A * pts.Count - B * B;
|
||||
var k = (C * pts.Count - B * D) / tmp;
|
||||
var b = (A * D - C * B) / tmp;
|
||||
|
||||
double x1 = 0,
|
||||
y1 = k * x1 + b,
|
||||
x2 = 1000,
|
||||
y2 = k * x2 + b;
|
||||
|
||||
double CalcDist(ref bool fail, ref Vector2 endP, ref Vector2 endQ)
|
||||
{
|
||||
double distSum = 0;
|
||||
double lx = x2 - x1, ly = y2 - y1, dAB = lx * lx + ly * ly;
|
||||
double minU = double.MaxValue, maxU = double.MinValue;
|
||||
|
||||
foreach (var p in pts)
|
||||
{
|
||||
var u = ((p.X - x1) * lx + (p.Y - y1) * ly) / dAB;
|
||||
var perp = new Vector2((float)(x1 + u * lx), (float)(y1 + u * ly));
|
||||
if (u < minU)
|
||||
{
|
||||
endP = perp;
|
||||
minU = u;
|
||||
}
|
||||
if (u > maxU)
|
||||
{
|
||||
endQ = perp;
|
||||
maxU = u;
|
||||
}
|
||||
|
||||
var curDist = dist(perp.X, perp.Y, p.X, p.Y);
|
||||
if (maxDist2Line > -1 && curDist > maxDist2Line) fail = true;
|
||||
distSum += curDist;
|
||||
}
|
||||
|
||||
return distSum;
|
||||
}
|
||||
|
||||
var kbFail = false;
|
||||
Vector2 endP1 = new Vector2(), endQ1 = new Vector2();
|
||||
double kbDist = CalcDist(ref kbFail, ref endP1, ref endQ1);
|
||||
|
||||
// x = my + n
|
||||
A = 0;
|
||||
B = 0;
|
||||
C = 0;
|
||||
D = 0;
|
||||
foreach (var p in pts)
|
||||
{
|
||||
A += p.X * p.Y;
|
||||
B += p.Y * p.Y;
|
||||
C += p.Y;
|
||||
D += p.X;
|
||||
}
|
||||
|
||||
tmp = C * C - B * pts.Count;
|
||||
var m = (C * D - A * pts.Count) / tmp;
|
||||
var n = (A * C - B * D) / tmp;
|
||||
y1 = 0;
|
||||
x1 = m * y1 + n;
|
||||
y2 = 1000;
|
||||
x2 = m * y2 + n;
|
||||
|
||||
var mnFail = false;
|
||||
Vector2 endP2 = new Vector2(), endQ2 = new Vector2();
|
||||
double mnDist = CalcDist(ref mnFail, ref endP2, ref endQ2);
|
||||
|
||||
Vector2 endP = endP1, endQ = endQ1;
|
||||
if (mnDist < kbDist)
|
||||
{
|
||||
if (mnFail) return (false, new Vector2(), new Vector2());
|
||||
endP = endP2;
|
||||
endQ = endQ2;
|
||||
}
|
||||
else if (kbFail) return (false, new Vector2(), new Vector2());
|
||||
|
||||
return (true, endP, endQ);
|
||||
}
|
||||
|
||||
[MethodImpl(MethodImplOptions.AggressiveInlining)]
|
||||
public static int toId(int x, int y, int z)
|
||||
{
|
||||
return (x * 1140671485 + 12820163) ^ (y * 134775813 + 1) ^ (z * 1103515245 + 12345);
|
||||
}
|
||||
|
||||
public class Clustering<T>
|
||||
{
|
||||
public int numIteration = 3;
|
||||
public int itemNumThreshold = 10;
|
||||
|
||||
public Func<T, T, bool> inRange;
|
||||
public Func<List<T>, T> average;
|
||||
private readonly List<T> _inputData;
|
||||
private Dictionary<T, List<T>> _clusters = new Dictionary<T, List<T>>();
|
||||
|
||||
public Clustering(List<T> data, Func<T, T, bool> inRange, Func<List<T>, T> average)
|
||||
{
|
||||
_inputData = data;
|
||||
this.inRange = inRange;
|
||||
this.average = average;
|
||||
}
|
||||
|
||||
public Dictionary<T, List<T>> GetClusters()
|
||||
{
|
||||
var tmp = new List<(T center, List<T> items)>();
|
||||
|
||||
for (var iter = 0; iter < numIteration; iter++)
|
||||
{
|
||||
tmp = tmp.Where(cluster => cluster.items.Count > itemNumThreshold)
|
||||
.Select(cluster => (average(cluster.items), new List<T>())).ToList();
|
||||
|
||||
foreach (var data in _inputData)
|
||||
{
|
||||
var added = false;
|
||||
foreach (var cluster in tmp)
|
||||
{
|
||||
if (inRange(cluster.center, data))
|
||||
{
|
||||
cluster.items.Add(data);
|
||||
added = true;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
if (!added)
|
||||
tmp.Add((data, new List<T>() { data }));
|
||||
}
|
||||
}
|
||||
|
||||
_clusters = tmp.Where(cluster => cluster.items.Count > itemNumThreshold)
|
||||
.ToDictionary(cluster => cluster.center, cluster => cluster.items);
|
||||
return _clusters;
|
||||
}
|
||||
}
|
||||
|
||||
public static (bool, Vector2) TwoLinesIntersection(Vector2 A, Vector2 B, Vector2 C, Vector2 D)
|
||||
{
|
||||
// Line AB represented as a1x + b1y = c1
|
||||
double a1 = B.Y - A.Y;
|
||||
double b1 = A.X - B.X;
|
||||
double c1 = a1 * (A.X) + b1 * (A.Y);
|
||||
|
||||
// Line CD represented as a2x + b2y = c2
|
||||
double a2 = D.Y - C.Y;
|
||||
double b2 = C.X - D.X;
|
||||
double c2 = a2 * (C.X) + b2 * (C.Y);
|
||||
|
||||
double determinant = a1 * b2 - a2 * b1;
|
||||
|
||||
if (determinant == 0)
|
||||
{
|
||||
// The lines are parallel. This is simplified
|
||||
// by returning a pair of FLT_MAX
|
||||
return new(false, new Vector2());
|
||||
}
|
||||
else
|
||||
{
|
||||
double x = (b2 * c1 - b1 * c2) / determinant;
|
||||
double y = (a1 * c2 - a2 * c1) / determinant;
|
||||
return (true, new Vector2((float)x, (float)y));
|
||||
}
|
||||
}
|
||||
|
||||
public static bool IsAtLeft(Vector2 anchor, Vector2 dest, Vector2 p)
|
||||
{
|
||||
var v1 = new Vector3(anchor - p, 0);
|
||||
var v2 = new Vector3(dest - p, 0);
|
||||
return Vector3.Cross(v1, v2).Z > 0;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 将角度转化至-180到180度的范围内。
|
||||
/// </summary>
|
||||
/// <param name="th"></param>
|
||||
/// <returns></returns>
|
||||
public static double RoundTh(double th)
|
||||
{
|
||||
return th - System.Math.Round(th / 360) * 360;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 将角度转化至-180到180度的范围内。
|
||||
/// </summary>
|
||||
/// <param name="th"></param>
|
||||
/// <returns></returns>
|
||||
public static float RoundTh(float th)
|
||||
{
|
||||
return th - (float)System.Math.Round(th / 360f) * 360f;
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,161 @@
|
||||
using System;
|
||||
using System.Numerics;
|
||||
|
||||
namespace CommonUsage.Mathematics
|
||||
{
|
||||
/// <summary>
|
||||
/// 表示一条线段。
|
||||
/// </summary>
|
||||
public class LineSegment
|
||||
{
|
||||
/// <summary>
|
||||
/// 默认构造函数。所有坐标初始化为0。
|
||||
/// </summary>
|
||||
public LineSegment()
|
||||
{
|
||||
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 使用两个端点初始化一段2D线段。
|
||||
/// </summary>
|
||||
/// <param name="src">线段起点。</param>
|
||||
/// <param name="dst">线段终点。</param>
|
||||
public LineSegment(Vector2 src, Vector2 dst)
|
||||
{
|
||||
Src = src;
|
||||
Dst = dst;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 使用两个端点初始化一段3D线段。
|
||||
/// </summary>
|
||||
/// <param name="src">线段起点。</param>
|
||||
/// <param name="dst">线段终点。</param>
|
||||
public LineSegment(Vector3 src, Vector3 dst)
|
||||
{
|
||||
Src3D = src;
|
||||
Dst3D = dst;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 使用两个端点初始化一条线段，2D。
|
||||
/// </summary>
|
||||
/// <param name="x1">线段起点x坐标。</param>
|
||||
/// <param name="y1">线段起点y坐标。</param>
|
||||
/// <param name="x2">线段终点x坐标。</param>
|
||||
/// <param name="y2">线段终点y坐标。</param>
|
||||
public LineSegment(double x1, double y1, double x2, double y2)
|
||||
{
|
||||
Src = new Vector2((float)x1, (float)y1);
|
||||
Dst = new Vector2((float)x2, (float)y2);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 使用两个端点初始化一条线段，2D。
|
||||
/// </summary>
|
||||
/// <param name="x1">线段起点x坐标。</param>
|
||||
/// <param name="y1">线段起点y坐标。</param>
|
||||
/// <param name="z1">线段起点y坐标。</param>
|
||||
/// <param name="x2">线段终点x坐标。</param>
|
||||
/// <param name="y2">线段终点y坐标。</param>
|
||||
/// <param name="z2">线段终点y坐标。</param>
|
||||
public LineSegment(double x1, double y1, double z1, double x2, double y2, double z2)
|
||||
{
|
||||
Src3D = new Vector3((float)x1, (float)y1, (float)z1);
|
||||
Dst3D = new Vector3((float)x2, (float)y2, (float)z2);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 返回线段长度，2D。
|
||||
/// </summary>
|
||||
/// <returns></returns>
|
||||
public double Length()
|
||||
{
|
||||
return Vector2.Distance(Src, Dst);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 返回线段长度，3D。
|
||||
/// </summary>
|
||||
/// <returns></returns>
|
||||
public double Length3D()
|
||||
{
|
||||
return Vector3.Distance(Src3D, Dst3D);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 返回线段与x轴正方向夹角度数，角度制。
|
||||
/// </summary>
|
||||
/// <returns></returns>
|
||||
public double Angle()
|
||||
{
|
||||
return Math.Atan2(Dst.Y - Src.Y, Dst.X - Src.X) / Math.PI * 180;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 返回一段方向相反的线段。
|
||||
/// </summary>
|
||||
/// <returns></returns>
|
||||
public LineSegment Reverse()
|
||||
{
|
||||
return new LineSegment(Dst3D, Src3D);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 线段2D起点。
|
||||
/// </summary>
|
||||
public Vector2 Src
|
||||
{
|
||||
get => new(_srcX, _srcY);
|
||||
set
|
||||
{
|
||||
_srcX = value.X;
|
||||
_srcY = value.Y;
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 线段2D终点。
|
||||
/// </summary>
|
||||
public Vector2 Dst
|
||||
{
|
||||
get => new(_dstX, _dstY);
|
||||
set
|
||||
{
|
||||
_dstX = value.X;
|
||||
_dstY = value.Y;
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 线段3D起点。
|
||||
/// </summary>
|
||||
public Vector3 Src3D
|
||||
{
|
||||
get => new(_srcX, _srcY, _srcZ);
|
||||
set
|
||||
{
|
||||
_srcX = value.X;
|
||||
_srcY = value.Y;
|
||||
_srcZ = value.Z;
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 线段3D终点。
|
||||
/// </summary>
|
||||
public Vector3 Dst3D
|
||||
{
|
||||
get => new(_dstX, _dstY, _dstZ);
|
||||
set
|
||||
{
|
||||
_dstX = value.X;
|
||||
_dstY = value.Y;
|
||||
_dstZ = value.Z;
|
||||
}
|
||||
}
|
||||
|
||||
private float _srcX, _srcY, _srcZ, _dstX, _dstY, _dstZ;
|
||||
}
|
||||
}
|
||||
@@ -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\\"
|
||||
}
|
||||
}
|
||||
}
|
||||
+19
@@ -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")
|
||||
};
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,94 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Net.Http;
|
||||
using System.Text;
|
||||
using System.Threading.Tasks;
|
||||
using CommonUsage.Protocols.VDA5050.Messages;
|
||||
using FundamentalLib;
|
||||
using Newtonsoft.Json;
|
||||
|
||||
namespace CommonUsage.Protocols.VDA5050
|
||||
{
|
||||
public class HTTPCommunication : IVDACommunicationProtocol
|
||||
{
|
||||
private readonly string _host;
|
||||
private readonly int _port;
|
||||
|
||||
public HTTPCommunication(string host, int port)
|
||||
{
|
||||
_host = host;
|
||||
_port = port;
|
||||
}
|
||||
|
||||
public async Task PublishConnectionStatus(string status)
|
||||
{
|
||||
var message = new connectionMessage()
|
||||
{
|
||||
serialNumber = "test-01",
|
||||
headerId = 1,
|
||||
timestamp = DateTime.Now,
|
||||
connectionState = status
|
||||
};
|
||||
|
||||
await SendMessageAsync(message, "vda5050/connection");
|
||||
}
|
||||
|
||||
public void SetupOrderListener(Action<orderMessage> orderReceived)
|
||||
{
|
||||
PicoHttpServer.AddPostTextHandler("/order", new { }, (_, str) =>
|
||||
{
|
||||
var order = JsonConvert.DeserializeObject<orderMessage>(str);
|
||||
orderReceived(order);
|
||||
return "";
|
||||
});
|
||||
}
|
||||
|
||||
public void SetUpInstanActionListener(Action<instanceAction> onInstanceActionReceived)
|
||||
{
|
||||
PicoHttpServer.AddPostTextHandler("/instanceAction", new { }, (_, str) =>
|
||||
{
|
||||
var instanceAction = JsonConvert.DeserializeObject<instanceAction>(str);
|
||||
onInstanceActionReceived(instanceAction);
|
||||
return "";
|
||||
});
|
||||
}
|
||||
|
||||
public void SetUpImmediateCommandListener(Action<string> onChangeCarFieldsReceived)
|
||||
{
|
||||
throw new NotImplementedException();
|
||||
}
|
||||
|
||||
public async Task SendMessageAsync<T>(T message, string topic)
|
||||
{
|
||||
try
|
||||
{
|
||||
var url = $"http://{_host}:{_port}/{topic}";
|
||||
using var client = new HttpClient();
|
||||
var response = await client.PostAsync(url, new StringContent(JsonConvert.SerializeObject(message), Encoding.UTF8, "application/json"));
|
||||
|
||||
if (!response.IsSuccessStatusCode)
|
||||
{
|
||||
Console.WriteLine($" >> Sending Message: Failed to send message. Status Code: {response.StatusCode}");
|
||||
}
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
Console.WriteLine($"Error in sending message: {ex.Message}");
|
||||
}
|
||||
}
|
||||
|
||||
public async Task SendVisualizationMessageAsync<T>(T msg, string topic)
|
||||
{
|
||||
throw new NotImplementedException();
|
||||
}
|
||||
|
||||
public void SetupTestListener(Action<string> testMsg)
|
||||
{
|
||||
throw new NotImplementedException();
|
||||
}
|
||||
public async Task PublishFactSheet(factsheetMessage message)
|
||||
{
|
||||
throw new NotImplementedException();
|
||||
}
|
||||
}
|
||||
}
|
||||
+19
@@ -0,0 +1,19 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Text;
|
||||
using System.Threading.Tasks;
|
||||
using CommonUsage.Protocols.VDA5050.Messages;
|
||||
|
||||
namespace CommonUsage.Protocols.VDA5050
|
||||
{
|
||||
public interface IVDACommunicationProtocol
|
||||
{
|
||||
Task PublishConnectionStatus(string status);
|
||||
Task PublishFactSheet(factsheetMessage msg);
|
||||
void SetupOrderListener(Action<orderMessage> orderReceived);
|
||||
void SetUpInstanActionListener(Action<instanceAction> onInstanceActionReceived);
|
||||
void SetUpImmediateCommandListener(Action<string> onChangeCarFieldsReceived);
|
||||
Task SendMessageAsync<T>(T message, string topic);
|
||||
Task SendVisualizationMessageAsync<T>(T message, string topic);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,319 @@
|
||||
using System.IO;
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Text;
|
||||
using System.Threading;
|
||||
using System.Threading.Tasks;
|
||||
using CommonUsage.Protocols.VDA5050.Messages;
|
||||
using MQTTnet;
|
||||
using MQTTnet.Client;
|
||||
using MQTTnet.Extensions.ManagedClient;
|
||||
using MQTTnet.Packets;
|
||||
using MQTTnet.Protocol;
|
||||
using MQTTnet.Server;
|
||||
using Newtonsoft.Json;
|
||||
using FundamentalLib;
|
||||
using CommonUsage.Protocols.VDA5050.Objects;
|
||||
using FundamentalLib.MiscHelpers;
|
||||
|
||||
namespace CommonUsage.Protocols.VDA5050
|
||||
{
|
||||
public class MQTTCommunication : IVDACommunicationProtocol
|
||||
{
|
||||
private readonly string _host;
|
||||
private readonly int _port;
|
||||
private readonly string _orderTopic = "vda5050/frldAGV/order";
|
||||
private readonly string _connectionTopic = "vda5050/frldAGV/connection";
|
||||
private readonly string _instanceAction = "vda5050/frldAGV/instantActions";
|
||||
private readonly string _factsheet = "vda5050/frldAGV/factsheet";
|
||||
private readonly string _changeCarFields = "vda5050/frldAGV/changeCarFields";
|
||||
|
||||
private IManagedMqttClient _client;
|
||||
private IManagedMqttClient _visualizationClient;
|
||||
|
||||
public MQTTCommunication(string host, int port)
|
||||
{
|
||||
_host = host;
|
||||
_port = port;
|
||||
InitializeClient();
|
||||
InitializeVisualizationClient();
|
||||
}
|
||||
|
||||
private void InitializeClient()
|
||||
{
|
||||
var willMessage = new connectionMessage()
|
||||
{
|
||||
headerId = 1,
|
||||
timestamp = DateTime.Now,
|
||||
version = "00",
|
||||
manufacturer = "frld",
|
||||
serialNumber = "test-01",
|
||||
connectionState = "CONNECTIONBROKEN"
|
||||
};
|
||||
|
||||
var mqttClientOptions = new MqttClientOptionsBuilder()
|
||||
.WithClientId("AGV-Client-frldAGV")
|
||||
.WithTcpServer(_host, _port)
|
||||
.WithWillTopic(_connectionTopic)
|
||||
.WithWillPayload(JsonConvert.SerializeObject(willMessage))
|
||||
.WithWillRetain(true)
|
||||
.Build();
|
||||
|
||||
var managedMqttClientOptions = new ManagedMqttClientOptionsBuilder()
|
||||
.WithClientOptions(mqttClientOptions)
|
||||
.WithMaxPendingMessages(20)
|
||||
.WithPendingMessagesOverflowStrategy(MqttPendingMessagesOverflowStrategy.DropOldestQueuedMessage)
|
||||
.Build();
|
||||
|
||||
_client = new MqttFactory().CreateManagedMqttClient();
|
||||
_client.StartAsync(managedMqttClientOptions).GetAwaiter().GetResult();
|
||||
Console.WriteLine($" >> MQTT client initialized and connected to broker at {_host} - {_port}");
|
||||
}
|
||||
|
||||
private void InitializeVisualizationClient()
|
||||
{
|
||||
var mqttClientOptions = new MqttClientOptionsBuilder()
|
||||
.WithClientId("AGV-Visualization")
|
||||
.WithTcpServer(_host, _port)
|
||||
.Build();
|
||||
|
||||
var managedMqttClientOptions = new ManagedMqttClientOptionsBuilder()
|
||||
.WithClientOptions(mqttClientOptions)
|
||||
.WithMaxPendingMessages(10) // Prevent overloading
|
||||
.WithPendingMessagesOverflowStrategy(MqttPendingMessagesOverflowStrategy.DropOldestQueuedMessage)
|
||||
.Build();
|
||||
|
||||
_visualizationClient = new MqttFactory().CreateManagedMqttClient();
|
||||
_visualizationClient.StartAsync(managedMqttClientOptions).GetAwaiter().GetResult();
|
||||
Console.WriteLine("MQTT Visualization client initialized.");
|
||||
}
|
||||
|
||||
public void SetUpImmediateCommandListener(Action<string> onChangeCarFieldsReceived)
|
||||
{
|
||||
Console.WriteLine($"Subscribed to the topic: {_changeCarFields}");
|
||||
_client.SubscribeAsync(_changeCarFields).GetAwaiter().GetResult();
|
||||
|
||||
_client.ApplicationMessageReceivedAsync += async e =>
|
||||
{
|
||||
if (e.ApplicationMessage.Topic == _changeCarFields)
|
||||
{
|
||||
var payload = Encoding.UTF8.GetString(e.ApplicationMessage.Payload);
|
||||
LogMessage($"RECEIVE-{e.ApplicationMessage.Topic}", e.ApplicationMessage.Topic, payload);
|
||||
//var script = JsonConvert.DeserializeObject<string>(payload);
|
||||
Console.WriteLine($"Change car field: {payload}");
|
||||
onChangeCarFieldsReceived(payload);
|
||||
}
|
||||
};
|
||||
}
|
||||
|
||||
|
||||
public void SetUpInstanActionListener(Action<instanceAction> onInstanceActionReceived)
|
||||
{
|
||||
Console.WriteLine($"Subscribed to the topic: {_instanceAction}");
|
||||
|
||||
// Subscribe to the instanceAction topic
|
||||
_client.SubscribeAsync(_instanceAction).GetAwaiter().GetResult();
|
||||
|
||||
_client.ApplicationMessageReceivedAsync += async e =>
|
||||
{
|
||||
if (e.ApplicationMessage.Topic == _instanceAction)
|
||||
{
|
||||
var payload = Encoding.UTF8.GetString(e.ApplicationMessage.Payload);
|
||||
LogMessage($"RECEIVE-{e.ApplicationMessage.Topic}", e.ApplicationMessage.Topic, payload);
|
||||
var actions = JsonConvert.DeserializeObject<instanceAction>(payload);
|
||||
Console.WriteLine($"Received instance action: Header ID = {actions.headerId}, Timestamp = {actions.timestamp}");
|
||||
foreach (var action in actions.actions)
|
||||
{
|
||||
Console.WriteLine($"Action ID: {action.actionId}, Type: {action.actionType}");
|
||||
}
|
||||
|
||||
onInstanceActionReceived(actions);
|
||||
}
|
||||
};
|
||||
}
|
||||
|
||||
public async Task PublishConnectionStatus(string status)
|
||||
{
|
||||
var message = new connectionMessage()
|
||||
{
|
||||
headerId = 1,
|
||||
timestamp = DateTime.Now,
|
||||
version = "00",
|
||||
manufacturer = "frld",
|
||||
serialNumber = "test-01",
|
||||
connectionState = status
|
||||
};
|
||||
|
||||
var payload = JsonConvert.SerializeObject(message);
|
||||
|
||||
var content = new MqttApplicationMessageBuilder()
|
||||
.WithTopic(_connectionTopic)
|
||||
.WithPayload(payload)
|
||||
.WithQualityOfServiceLevel(MqttQualityOfServiceLevel.AtLeastOnce)
|
||||
.WithRetainFlag(true)
|
||||
.Build();
|
||||
|
||||
await _client.EnqueueAsync(content);
|
||||
LogMessage($"SEND-{_connectionTopic}", _connectionTopic, payload);
|
||||
|
||||
//await SendMessageAsync(message, _connectionTopic);
|
||||
}
|
||||
|
||||
public async Task PublishFactSheet(factsheetMessage message)
|
||||
{
|
||||
|
||||
//await SendMessageAsync(message, _factsheet);
|
||||
var payload = JsonConvert.SerializeObject(message);
|
||||
|
||||
var content = new MqttApplicationMessageBuilder()
|
||||
.WithTopic(_factsheet)
|
||||
.WithPayload(payload)
|
||||
.WithQualityOfServiceLevel(MqttQualityOfServiceLevel.AtMostOnce)
|
||||
//.WithRetainFlag(true)
|
||||
.Build();
|
||||
|
||||
await _client.EnqueueAsync(content);
|
||||
LogMessage($"SEND-{_factsheet}", _factsheet, payload);
|
||||
|
||||
}
|
||||
|
||||
public void SetupOrderListener(Action<orderMessage> orderReceived)
|
||||
{
|
||||
// Subscribe to the orders topic
|
||||
_client.SubscribeAsync(_orderTopic, MqttQualityOfServiceLevel.AtMostOnce).GetAwaiter().GetResult();
|
||||
_client.ApplicationMessageReceivedAsync += async e =>
|
||||
{
|
||||
if (e.ApplicationMessage.Topic == _orderTopic)
|
||||
{
|
||||
var payload = Encoding.UTF8.GetString(e.ApplicationMessage.Payload);
|
||||
LogMessage($"RECEIVE-{e.ApplicationMessage.Topic}", e.ApplicationMessage.Topic, payload);
|
||||
var order = JsonConvert.DeserializeObject<orderMessage>(payload);
|
||||
|
||||
// Save the order message to a file for debugging
|
||||
// SaveOrderToFile(payload);
|
||||
orderReceived(order);
|
||||
}
|
||||
await Task.CompletedTask;
|
||||
};
|
||||
}
|
||||
|
||||
private void SaveOrderToFile(string orderJson)
|
||||
{
|
||||
try
|
||||
{
|
||||
// Specify the file path (e.g., orders_log.txt in the current directory)
|
||||
string filePath = "orders_log.txt";
|
||||
|
||||
// Append the order JSON along with a timestamp
|
||||
File.AppendAllText(filePath, $"{DateTime.UtcNow:yyyy-MM-dd HH:mm:ss} - {orderJson}{Environment.NewLine}");
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
// Handle any exceptions that occur while writing to the file
|
||||
Console.WriteLine($"Failed to save order to file: {ex.Message}");
|
||||
}
|
||||
}
|
||||
|
||||
public async Task SendMessageAsync<T>(T message, string topic)
|
||||
{
|
||||
|
||||
var payload = JsonConvert.SerializeObject(message);
|
||||
|
||||
var content = new MqttApplicationMessageBuilder()
|
||||
.WithTopic(topic)
|
||||
.WithPayload(payload)
|
||||
.WithQualityOfServiceLevel(MqttQualityOfServiceLevel.AtMostOnce)
|
||||
.Build();
|
||||
|
||||
await _client.EnqueueAsync(content);
|
||||
|
||||
if (topic != "vda5050/frldAGV/visualization")
|
||||
{
|
||||
|
||||
LogMessage($"SEND-{topic}", topic, payload);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
public async Task SendVisualizationMessageAsync<T>(T message, string topic)
|
||||
{
|
||||
if (_visualizationClient == null) return; // Ensure client is initialized
|
||||
|
||||
var payload = JsonConvert.SerializeObject(message);
|
||||
|
||||
var content = new MqttApplicationMessageBuilder()
|
||||
.WithTopic(topic)
|
||||
.WithPayload(payload)
|
||||
.WithQualityOfServiceLevel(MqttQualityOfServiceLevel.AtMostOnce) // QoS 0 for lightweight visualization
|
||||
.Build();
|
||||
|
||||
if (_visualizationClient.PendingApplicationMessagesCount < 5) // Prevent flooding
|
||||
{
|
||||
await _visualizationClient.EnqueueAsync(content);
|
||||
}
|
||||
else
|
||||
{
|
||||
Console.WriteLine("Skipping visualization update to avoid MQTT congestion.");
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
public void LogMessage(string direction, string topic, string payload)
|
||||
{
|
||||
string formattedPayload = payload;
|
||||
string logDirectory = "Logs"; // Directory for log files
|
||||
var filePreName = direction;
|
||||
filePreName = filePreName.Replace("/", "_");
|
||||
string logFilePath = Path.Combine(logDirectory, filePreName + $"-{DateTime.Now:yyyy-MM-dd}.log");
|
||||
|
||||
if(!Directory.Exists(logDirectory)) Directory.CreateDirectory(logDirectory);
|
||||
// Try to parse the payload as JSON and pretty-print it
|
||||
try
|
||||
{
|
||||
var jsonObject = JsonConvert.DeserializeObject(payload);
|
||||
formattedPayload = JsonConvert.SerializeObject(jsonObject, Formatting.Indented);
|
||||
}
|
||||
catch (JsonReaderException)
|
||||
{
|
||||
// If the payload is not valid JSON, just leave it as is
|
||||
formattedPayload = payload;
|
||||
}
|
||||
|
||||
string logMessage = $"[{DateTime.Now:HH:mm:ss}] [{direction}] Topic: {topic}, Payload:\n{formattedPayload}\n";
|
||||
RotateLogFile(logFilePath, logDirectory);
|
||||
|
||||
try
|
||||
{
|
||||
File.AppendAllText(logFilePath, logMessage + Environment.NewLine);
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
Console.WriteLine($"Error writing to log file: {ex.Message}");
|
||||
}
|
||||
// Console.WriteLine($"[{DateTime.Now:HH:mm:ss}] [{direction}] Topic: {topic}, Payload: {formattedPayload}");
|
||||
// DLog.Log($"[{DateTime.Now:HH:mm:ss}] [{direction}] Topic: {topic}, Payload: {formattedPayload}");
|
||||
}
|
||||
|
||||
|
||||
private void RotateLogFile(string logFilePath, string logDirectory)
|
||||
{
|
||||
const long maxFileSize = 10 * 1024 * 1024; // 10 MB in bytes
|
||||
|
||||
FileInfo fileInfo = new FileInfo(logFilePath);
|
||||
if (fileInfo.Exists && fileInfo.Length > maxFileSize)
|
||||
{
|
||||
string archivePath = Path.Combine(logDirectory, $"log_{DateTime.Now:yyyy-MM-dd_HH-mm-ss}.log");
|
||||
|
||||
try
|
||||
{
|
||||
File.Move(logFilePath, archivePath); // Rename the current log file
|
||||
Console.WriteLine($"Log file rotated: {archivePath}");
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
Console.WriteLine($"Error rotating log file: {ex.Message}");
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
+18
@@ -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'}
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
@@ -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
|
||||
}
|
||||
|
||||
}
|
||||
+23
@@ -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;
|
||||
}
|
||||
|
||||
}
|
||||
@@ -0,0 +1,19 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Text;
|
||||
using CommonUsage.Protocols.VDA5050.Objects;
|
||||
|
||||
namespace CommonUsage.Protocols.VDA5050.Messages
|
||||
{
|
||||
public class instanceAction
|
||||
{
|
||||
|
||||
public uint headerId { get; set; } // Incremented for each new message.
|
||||
public string timestamp { get; set; } // ISO 8601 UTC timestamp.
|
||||
public string version { get; set; } // Protocol version.
|
||||
public string manufacturer { get; set; } // AGV manufacturer.
|
||||
public string serialNumber { get; set; } // Unique AGV serial number.
|
||||
|
||||
public List<actionState> actions { get; set; }
|
||||
}
|
||||
}
|
||||
@@ -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; }
|
||||
}
|
||||
}
|
||||
|
||||
@@ -0,0 +1,69 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.ComponentModel;
|
||||
using CommonUsage.Protocols.VDA5050.Objects;
|
||||
|
||||
namespace CommonUsage.Protocols.VDA5050.Messages
|
||||
{
|
||||
/// <summary>
|
||||
/// 6.10 Topic: "state" (from AGV to master control)
|
||||
/// todo: complete all fields required by VDA5050
|
||||
/// </summary>
|
||||
public class stateMessage
|
||||
{
|
||||
public uint headerId;
|
||||
public string timestamp = "";
|
||||
public string version = "";
|
||||
public string manufacturer = "";
|
||||
public string serialNumber = "";
|
||||
/// <summary>
|
||||
/// Unique order identification of the current order or the previously finished order.
|
||||
/// The orderId is kept until a new order is received.
|
||||
/// Empty string (""), if no previous orderId is available.
|
||||
/// </summary>
|
||||
public string orderId = "";
|
||||
|
||||
/// <summary>
|
||||
/// Order update identification to identify, that an order update has been accepted by the AGV.
|
||||
/// "0" if no previous orderUpdateId is available.
|
||||
/// </summary>
|
||||
public uint orderUpdatedId = 0;
|
||||
|
||||
public string lastNodeId;
|
||||
|
||||
public uint lastNodeSequenceId;
|
||||
|
||||
/// <summary>
|
||||
/// Array of nodeState objects that need to be traversed for fulfilling the order (empty array if idle)
|
||||
/// </summary>
|
||||
public nodeState[] nodeStates = [];
|
||||
|
||||
/// <summary>
|
||||
/// Array of edgeState objects that need to be traversed for fulfilling the order (empty array if idle)
|
||||
/// </summary>
|
||||
public edgeState[] edgeStates = [];
|
||||
|
||||
public agvPosition agvPosition;
|
||||
|
||||
public velocity velocity;
|
||||
public load[] loads = [];
|
||||
|
||||
public bool driving;
|
||||
|
||||
public bool paused;
|
||||
|
||||
public bool newBaseRequest;
|
||||
|
||||
public double distanceSinceLastNode;
|
||||
|
||||
public batteryState batteryState;
|
||||
|
||||
public actionState[] actionStates = Array.Empty<actionState>();
|
||||
public string operatingMode = "";
|
||||
public List<errorState> errors { get; set; } = new List<errorState>(); // Array of errorState objects
|
||||
|
||||
public info[] information = [];
|
||||
public safetyState safetyState;
|
||||
|
||||
}
|
||||
}
|
||||
+12
@@ -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;
|
||||
}
|
||||
}
|
||||
@@ -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; }
|
||||
|
||||
}
|
||||
}
|
||||
@@ -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
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
@@ -0,0 +1,63 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Text;
|
||||
using static CommonUsage.Protocols.VDA5050.Objects.agvGeometry;
|
||||
|
||||
namespace CommonUsage.Protocols.VDA5050.Objects
|
||||
{
|
||||
public class agvGeometry
|
||||
{
|
||||
// Wheel Definitions
|
||||
public List<wheelDefinition> wheelDefinitions { get; set; } = new List<wheelDefinition>();
|
||||
|
||||
// 2D Envelopes
|
||||
public List<envelope2D> envelopes2D { get; set; } = new List<envelope2D>();
|
||||
|
||||
// 3D Envelopes
|
||||
public List<envelope3D> envelopes3D { get; set; } = new List<envelope3D>();
|
||||
|
||||
public class wheelDefinition
|
||||
{
|
||||
public enum WheelType { DRIVE, CASTER, FIXED, MECANUM }
|
||||
|
||||
public WheelType type { get; set; }
|
||||
public bool isActiveDriven { get; set; }
|
||||
public bool isActiveSteered { get; set; }
|
||||
|
||||
// Wheel Position
|
||||
public double positionX { get; set; }
|
||||
public double positionY { get; set; }
|
||||
public double positionTheta { get; set; } // Required for fixed wheels
|
||||
|
||||
// Wheel Properties
|
||||
public double diameter { get; set; }
|
||||
public double width { get; set; }
|
||||
public double centerDisplacement { get; set; } = 0; // Default to 0 if not defined
|
||||
public string constraints { get; set; }
|
||||
}
|
||||
|
||||
public class envelope2D
|
||||
{
|
||||
public string set { get; set; }
|
||||
public List<polygonPoint> polygonPoints { get; set; } = new List<polygonPoint>();
|
||||
public string description { get; set; }
|
||||
|
||||
public class polygonPoint
|
||||
{
|
||||
public double x { get; set; }
|
||||
public double y { get; set; }
|
||||
}
|
||||
}
|
||||
|
||||
public class envelope3D
|
||||
{
|
||||
public string set { get; set; }
|
||||
public string format { get; set; }
|
||||
public object data { get; set; } // JSON object for 3D envelope data
|
||||
public string url { get; set; }
|
||||
public string description { get; set; }
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
@@ -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 = "";
|
||||
}
|
||||
}
|
||||
@@ -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;
|
||||
}
|
||||
}
|
||||
+14
@@ -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
|
||||
}
|
||||
}
|
||||
@@ -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;
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,31 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Numerics;
|
||||
using System.Text;
|
||||
|
||||
namespace CommonUsage.Protocols.VDA5050.Objects
|
||||
{
|
||||
public class edge : sequenceItem
|
||||
{
|
||||
public string edgeId;
|
||||
|
||||
public string edgeDescription;
|
||||
|
||||
public string startNodeId;
|
||||
|
||||
public string endNodeId;
|
||||
|
||||
public double maxSpeed;
|
||||
|
||||
public double orientation;
|
||||
|
||||
public trajectory? trajectory;
|
||||
|
||||
public float[] trackTypeInfo;
|
||||
// public List<Vector2> controlPoints;
|
||||
//
|
||||
// public List<float> weights;
|
||||
|
||||
public action[] action = [];
|
||||
}
|
||||
}
|
||||
@@ -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;
|
||||
}
|
||||
}
|
||||
@@ -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
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,15 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Text;
|
||||
|
||||
namespace CommonUsage.Protocols.VDA5050.Objects
|
||||
{
|
||||
public class errorState
|
||||
{
|
||||
public List<errorReference> errorReferences { get; set; } = new List<errorReference>(); // Array of references
|
||||
public string errorType { get; set; } // Required: Type/name of the error
|
||||
public string errorDescription { get; set; } // Verbose description of the error
|
||||
public string errorHint { get; set; } // Hint for resolving the error
|
||||
public string errorLevel { get; set; } // Required: WARNING or FATAL
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,26 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Text;
|
||||
|
||||
namespace CommonUsage.Protocols.VDA5050.Objects
|
||||
{
|
||||
public class info
|
||||
{
|
||||
public string infoType { get; set; } // Type/name of the information
|
||||
public List<infoReference> infoReferences { get; set; } = new List<infoReference>(); // List of references
|
||||
public string infoDescription { get; set; } // Description of the information
|
||||
public infoLevelEnum infoLevel { get; set; } // Debugging or visualization level
|
||||
|
||||
public class infoReference
|
||||
{
|
||||
public string ReferenceKey { get; set; } // Reference type (e.g., headerId, orderId)
|
||||
public string ReferenceValue { get; set; } // The actual referenced field value
|
||||
}
|
||||
|
||||
public enum infoLevelEnum
|
||||
{
|
||||
DEBUG, // Used for debugging
|
||||
INFO // Used for visualization
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -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 ∞)
|
||||
}
|
||||
}
|
||||
@@ -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)
|
||||
}
|
||||
}
|
||||
@@ -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 = [];
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,36 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Text;
|
||||
|
||||
namespace CommonUsage.Protocols.VDA5050.Objects
|
||||
{
|
||||
/// <summary>
|
||||
/// Defines the position on a map in a global project-specific world coordinate system.
|
||||
/// Each floor has its own map.
|
||||
/// All maps shall use the same project-specific global origin.
|
||||
/// </summary>
|
||||
public class nodePosition
|
||||
{
|
||||
/// <summary>
|
||||
/// X-position on the map in reference to the map coordinate system.
|
||||
/// Precision is up to the specific implementation.
|
||||
/// </summary>
|
||||
public double x;
|
||||
|
||||
/// <summary>
|
||||
/// Y-position on the map in reference to the map coordinate system.
|
||||
/// Precision is up to the specific implementation.
|
||||
/// </summary>
|
||||
public double y;
|
||||
|
||||
/// <summary>
|
||||
/// Range: [-Pi ... Pi]
|
||||
/// Absolute orientation of the AGV on the node.
|
||||
/// Optional: vehicle can plan the path by itself. If defined, the AGV has to assume the theta angle on this node.
|
||||
/// If previous edge disallows rotation, the AGV shall rotate on the node.
|
||||
/// If following edge has a differing orientation defined but disallows rotation,
|
||||
/// the AGV is to rotate on the node to the edges desired rotation before entering the edge.
|
||||
/// </summary>
|
||||
public double theta;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,26 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Text;
|
||||
|
||||
namespace CommonUsage.Protocols.VDA5050.Objects
|
||||
{
|
||||
public class nodeState : sequenceItem
|
||||
{
|
||||
/// <summary>
|
||||
/// Unique node identification.
|
||||
/// </summary>
|
||||
public string nodeId;
|
||||
|
||||
/// <summary>
|
||||
/// Additional information on the node.
|
||||
/// </summary>
|
||||
public string nodeDescription;
|
||||
|
||||
/// <summary>
|
||||
/// Node position.
|
||||
/// The object is defined in 6.6 Topic: "order" (from master control to AGV)
|
||||
/// Optional: Master control has this information. Can be sent additionally, e.g., for debugging purposes.
|
||||
/// </summary>
|
||||
public nodePosition nodePosition;
|
||||
}
|
||||
}
|
||||
+20
@@ -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;
|
||||
}
|
||||
}
|
||||
+10
@@ -0,0 +1,10 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Text;
|
||||
|
||||
namespace CommonUsage.Protocols.VDA5050.Objects
|
||||
{
|
||||
public class protocolFeatures
|
||||
{
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,10 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Text;
|
||||
|
||||
namespace CommonUsage.Protocols.VDA5050.Objects
|
||||
{
|
||||
public class protocolLimits
|
||||
{
|
||||
}
|
||||
}
|
||||
@@ -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
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -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;
|
||||
}
|
||||
}
|
||||
@@ -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;
|
||||
}
|
||||
}
|
||||
+15
@@ -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')
|
||||
}
|
||||
}
|
||||
@@ -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;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,118 @@
|
||||
//using CommonUsage.Protocols.VDA5050.Messages;
|
||||
//using System;
|
||||
//using System.Collections.Generic;
|
||||
//using System.Net.Http.Headers;
|
||||
//using System.Runtime.CompilerServices;
|
||||
//using System.Text;
|
||||
//using System.Threading;
|
||||
//using System.Threading.Tasks;
|
||||
//using CommonUsage.Protocols.VDA5050.Objects;
|
||||
//using ClumsyCore.Utilities;
|
||||
//using System.Linq;
|
||||
//using ClumsyCore.Pilot;
|
||||
//using System.Numerics;
|
||||
|
||||
//namespace CommonUsage.Protocols.VDA5050
|
||||
//{
|
||||
// public abstract class VDA5050Basic
|
||||
// {
|
||||
// protected static IVDACommunicationProtocol _communicationProtocol;
|
||||
|
||||
// public void Enable(IVDACommunicationProtocol protocol)
|
||||
// {
|
||||
// _communicationProtocol = protocol;
|
||||
// Task.Run(() => ManageConnection());
|
||||
// StartVisualizationLoop();
|
||||
// }
|
||||
// private void StartVisualizationLoop()
|
||||
// {
|
||||
// Console.WriteLine("Visualization in CommonUsage");
|
||||
// new Thread(async () =>
|
||||
// {
|
||||
// while (true)
|
||||
// {
|
||||
// var position = GetAGVPosition();
|
||||
// if (position != null)
|
||||
// {
|
||||
// var msg = new stateMessage()
|
||||
// {
|
||||
// serialNumber = "test-01",
|
||||
// agvPosition = new()
|
||||
// {
|
||||
// x = position.Value.X,
|
||||
// y = position.Value.Y,
|
||||
// theta = position.Value.Theta,
|
||||
// positionInitialized = true
|
||||
// }
|
||||
|
||||
// };
|
||||
// await _communicationProtocol.SendMessageAsync(msg, "vda5050/frldAGV/visualization");
|
||||
// }
|
||||
|
||||
// Thread.Sleep(100);
|
||||
// }
|
||||
// })
|
||||
// { Name = "VDA5050TopicVisualization" }.Start();
|
||||
// }
|
||||
|
||||
// private void ManageConnection()
|
||||
// {
|
||||
// while (true)
|
||||
// {
|
||||
// var status = CheckConnectionStatus() ? "ONLINE" : "OFFLINE";
|
||||
// _communicationProtocol.PublishConnectionStatus(status);
|
||||
// Thread.Sleep(1000);
|
||||
// }
|
||||
// }
|
||||
|
||||
// protected List<sequenceItem> OrganizeReceivedSequence(orderMessage order)
|
||||
// {
|
||||
// List<sequenceItem> receivedSequence = new();
|
||||
|
||||
// int ii = 0, jj = 0;
|
||||
// while (true)
|
||||
// {
|
||||
// var edge = order.edges[ii];
|
||||
// var node = order.nodes[jj];
|
||||
// var takeEdge = edge.sequenceId < node.sequenceId;
|
||||
|
||||
// if (takeEdge)
|
||||
// {
|
||||
// receivedSequence.Add(edge);
|
||||
// ii++;
|
||||
// if (ii == order.edges.Length) break;
|
||||
// }
|
||||
// else
|
||||
// {
|
||||
// receivedSequence.Add(node);
|
||||
// jj++;
|
||||
// if (jj == order.nodes.Length) break;
|
||||
// }
|
||||
// }
|
||||
// for (var i = ii; i < order.edges.Length; ++i) receivedSequence.Add(order.edges[i]);
|
||||
// for (var j = jj; j < order.nodes.Length; ++j) receivedSequence.Add(order.nodes[j]);
|
||||
|
||||
// for (var i = 1; i < receivedSequence.Count; i++)
|
||||
// {
|
||||
// if (receivedSequence[i - 1].sequenceId + 1 != receivedSequence[i].sequenceId)
|
||||
// throw new Exception("stateMessage not continuous!");
|
||||
// }
|
||||
|
||||
// return receivedSequence;
|
||||
// }
|
||||
|
||||
// public virtual bool CheckConnectionStatus()
|
||||
// {
|
||||
// return false;
|
||||
// }
|
||||
|
||||
// protected abstract Vector3? GetAGVPosition();
|
||||
// public struct Vector3
|
||||
// {
|
||||
// public double X;
|
||||
// public double Y;
|
||||
// public double Theta;
|
||||
// }
|
||||
// }
|
||||
|
||||
//}
|
||||
@@ -0,0 +1,11 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Text;
|
||||
|
||||
namespace CommonUsage.Protocols.VDA5050
|
||||
{
|
||||
public class VDA5050Helper
|
||||
{
|
||||
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,28 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Drawing;
|
||||
using System.Numerics;
|
||||
using System.Text;
|
||||
|
||||
namespace CommonUsage
|
||||
{
|
||||
public class Visualizer
|
||||
{
|
||||
public Action<Color, Vector2, Vector2, bool, bool, int> LineAction;
|
||||
|
||||
public Action<Color, string, Vector2> TextAction;
|
||||
|
||||
public Action Clear;
|
||||
|
||||
public void DrawLine(Color color, Vector2 src, Vector2 dst, bool startArrow = false, bool endArrow = false,
|
||||
int width = 1)
|
||||
{
|
||||
LineAction?.Invoke(color, src, dst, startArrow, endArrow, width);
|
||||
}
|
||||
|
||||
public void DrawText(Color color, string text, Vector2 pos)
|
||||
{
|
||||
TextAction?.Invoke(color, text, pos);
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,15 @@
|
||||
// 驱动器、急停、夹臂等安全报警
|
||||
using MDCSToolBox.Medulla.Chassis.MultiWheel;
|
||||
|
||||
namespace MedullaAdapter
|
||||
{
|
||||
public class AlarmRoutine : MultiWheelAlarmRoutine<DiverCartDefinition>
|
||||
{
|
||||
// M层单车安全：汇总停车机器人夹臂等自定义报警状态。
|
||||
public override void SetOtherAlarms()
|
||||
{
|
||||
AddAlarm("左夹臂驱动报警", 2, () => cart.LeftArmErrorCode != 0);
|
||||
AddAlarm("右夹臂驱动报警", 2, () => cart.RightArmErrorCode != 0);
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -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<PortConfig>();
|
||||
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
|
||||
|
||||
}
|
||||
}
|
||||
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,928 @@
|
||||
// C#调用MCU通信桥
|
||||
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Linq;
|
||||
using System.Runtime.InteropServices;
|
||||
|
||||
namespace MCUSerialBridgeCLR
|
||||
{
|
||||
/// <summary>
|
||||
/// 辅助方法与内部 C 结构体封装，用于 P/Invoke 交互
|
||||
/// </summary>
|
||||
internal static class PortStructHelper
|
||||
{
|
||||
/// <summary>
|
||||
/// 将任意结构体序列化为字节数组
|
||||
/// </summary>
|
||||
/// <typeparam name="T">结构体类型</typeparam>
|
||||
/// <param name="str">要序列化的结构体</param>
|
||||
/// <returns>返回结构体对应的字节数组</returns>
|
||||
/// <remarks>使用 Marshal 分配内存并复制内容</remarks>
|
||||
public static byte[] StructToBytes<T>(T str)
|
||||
where T : struct
|
||||
{
|
||||
int size = Marshal.SizeOf<T>();
|
||||
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;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 串口端口配置的原生结构体（与 MCU C 层对应）
|
||||
/// </summary>
|
||||
[StructLayout(LayoutKind.Sequential, Pack = 1)]
|
||||
public struct SerialPortConfigC
|
||||
{
|
||||
/// <summary>端口类型（0x01 = Serial）</summary>
|
||||
public byte port_type;
|
||||
|
||||
/// <summary>波特率</summary>
|
||||
public uint baud;
|
||||
|
||||
/// <summary>接收帧时间间隔</summary>
|
||||
public uint receive_frame_ms;
|
||||
|
||||
/// <summary>保留字节，填 0</summary>
|
||||
[MarshalAs(UnmanagedType.ByValArray, SizeConst = 7)]
|
||||
public byte[] reserved;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// CAN 端口配置的原生结构体（与 MCU C 层对应）
|
||||
/// </summary>
|
||||
[StructLayout(LayoutKind.Sequential, Pack = 1)]
|
||||
public struct CANPortConfigC
|
||||
{
|
||||
/// <summary>端口类型（0x02 = CAN）</summary>
|
||||
public byte port_type;
|
||||
|
||||
/// <summary>波特率</summary>
|
||||
public uint baud;
|
||||
|
||||
/// <summary>最大重发时间</summary>
|
||||
public uint retry_time_ms;
|
||||
|
||||
/// <summary>保留字节，填 0</summary>
|
||||
[MarshalAs(UnmanagedType.ByValArray, SizeConst = 7)]
|
||||
public byte[] reserved;
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// MCU 固件版本信息结构体
|
||||
/// </summary>
|
||||
[StructLayout(LayoutKind.Sequential, CharSet = CharSet.Ansi)]
|
||||
public struct VersionInfo
|
||||
{
|
||||
/// <summary>产品型号</summary>
|
||||
[MarshalAs(UnmanagedType.ByValTStr, SizeConst = 16)]
|
||||
public string ProductionName;
|
||||
|
||||
/// <summary>Git 标签</summary>
|
||||
[MarshalAs(UnmanagedType.ByValTStr, SizeConst = 8)]
|
||||
public string GitTag;
|
||||
|
||||
/// <summary>Git commit 哈希值</summary>
|
||||
[MarshalAs(UnmanagedType.ByValTStr, SizeConst = 8)]
|
||||
public string GitCommit;
|
||||
|
||||
/// <summary>编译时间（字符串）</summary>
|
||||
[MarshalAs(UnmanagedType.ByValTStr, SizeConst = 24)]
|
||||
public string BuildTime;
|
||||
|
||||
/// <summary>
|
||||
/// 转换为可读字符串
|
||||
/// </summary>
|
||||
/// <returns>返回包含产品、Tag、Commit、BuildTime 的字符串</returns>
|
||||
// M层MCU适配：格式化固件版本信息便于日志显示。
|
||||
public override string ToString()
|
||||
{
|
||||
return $"Product: {ProductionName}, Tag: {GitTag}, Commit: {GitCommit}, Built: {BuildTime}";
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// MCU 当前运行状态
|
||||
/// </summary>
|
||||
[StructLayout(LayoutKind.Explicit)]
|
||||
public struct MCUState
|
||||
{
|
||||
/// <summary>原始 32 位状态值</summary>
|
||||
[FieldOffset(0)]
|
||||
public uint RawValue;
|
||||
|
||||
/// <summary>状态子字段 0</summary>
|
||||
[FieldOffset(0)]
|
||||
public byte Substate0;
|
||||
|
||||
/// <summary>状态子字段 1</summary>
|
||||
[FieldOffset(1)]
|
||||
public byte Substate1;
|
||||
|
||||
/// <summary>状态子字段 2</summary>
|
||||
[FieldOffset(2)]
|
||||
public byte Substate2;
|
||||
|
||||
/// <summary>高字节模式标志</summary>
|
||||
[FieldOffset(3)]
|
||||
public byte Mode;
|
||||
|
||||
/// <summary>是否处于 Bridge 模式</summary>
|
||||
public bool IsBridge => (Mode & 0x80) == 0;
|
||||
|
||||
/// <summary>是否处于 DIVER 模式</summary>
|
||||
public bool IsDIVER => (Mode & 0x80) != 0;
|
||||
|
||||
/// <summary>
|
||||
/// 返回可读的状态字符串
|
||||
/// </summary>
|
||||
/// <returns>例如 "Bridge: Running" 或 "DIVER: Error"</returns>
|
||||
// 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}";
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 抽象端口配置基类
|
||||
/// </summary>
|
||||
public abstract class PortConfig
|
||||
{
|
||||
/// <summary>端口类型（由子类实现）</summary>
|
||||
public abstract byte PortType { get; }
|
||||
|
||||
/// <summary>序列化端口配置为字节数组（供 P/Invoke 使用）</summary>
|
||||
/// <returns>返回固定长度字节数组（16 bytes）</returns>
|
||||
// M层MCU适配：将端口配置序列化为原生接口字节。
|
||||
public abstract byte[] ToBytes();
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 串口配置
|
||||
/// </summary>
|
||||
/// <remarks>
|
||||
/// 构造函数
|
||||
/// </remarks>
|
||||
/// <param name="baud">波特率</param>
|
||||
/// <param name="receiveFrameMs">接收帧间隔</param>
|
||||
// M层MCU适配：创建串口通信参数配置。
|
||||
public class SerialPortConfig(uint baud, uint receiveFrameMs) : PortConfig
|
||||
{
|
||||
/// <summary>Serial 类型</summary>
|
||||
public override byte PortType => 0x01;
|
||||
|
||||
/// <summary>波特率</summary>
|
||||
public uint Baud { get; set; } = baud;
|
||||
|
||||
/// <summary>接收帧间隔</summary>
|
||||
public uint ReceiveFrameMs { get; set; } = receiveFrameMs;
|
||||
|
||||
/// <summary>
|
||||
/// 转换为字节数组
|
||||
/// </summary>
|
||||
/// <returns>16 字节数组</returns>
|
||||
// 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);
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// CAN 端口配置
|
||||
/// </summary>
|
||||
/// <remarks>
|
||||
/// 构造函数
|
||||
/// </remarks>
|
||||
/// <param name="baud">波特率</param>
|
||||
/// <param name="retryTimeMs">重发间隔</param>
|
||||
// M层MCU适配：创建CAN通信参数配置。
|
||||
public class CANPortConfig(uint baud, uint retryTimeMs) : PortConfig
|
||||
{
|
||||
/// <summary>CAN 类型</summary>
|
||||
public override byte PortType => 0x02;
|
||||
|
||||
/// <summary>波特率</summary>
|
||||
public uint Baud { get; set; } = baud;
|
||||
|
||||
/// <summary>重发间隔</summary>
|
||||
public uint RetryTimeMs { get; set; } = retryTimeMs;
|
||||
|
||||
/// <summary>
|
||||
/// 转换为字节数组
|
||||
/// </summary>
|
||||
/// <returns>16 字节数组</returns>
|
||||
// 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);
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// CAN 帧结构（标准帧 11-bit ID + 1-bit RTR + 4-bit DLC + Payload）
|
||||
/// </summary>
|
||||
public class CANMessage
|
||||
{
|
||||
/// <summary>标准帧 ID（0~0x7FF，11 位）</summary>
|
||||
public ushort ID { get; set; }
|
||||
|
||||
/// <summary>远程帧标志：false = 数据帧，true = 远程帧</summary>
|
||||
public bool RTR { get; set; }
|
||||
|
||||
/// <summary>数据长度码：0~8</summary>
|
||||
public byte DLC { get; set; }
|
||||
|
||||
/// <summary>数据负载，长度必须严格等于 DLC（DLC=0 时可为 null）</summary>
|
||||
public byte[] Payload { get; set; }
|
||||
|
||||
/// <summary>
|
||||
/// 序列化为 MCU 协议字节流
|
||||
/// </summary>
|
||||
/// <returns>返回字节数组：2 bytes header + Payload</returns>
|
||||
/// <exception cref="ArgumentOutOfRangeException">如果 DLC > 8</exception>
|
||||
/// <exception cref="ArgumentException">如果 Payload 长度 != DLC</exception>
|
||||
// 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;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 反序列化 MCU 协议字节流为 CANMessage
|
||||
/// </summary>
|
||||
/// <param name="data">原始字节数组</param>
|
||||
/// <param name="length">实际有效长度</param>
|
||||
/// <returns>CANMessage 实例</returns>
|
||||
/// <exception cref="ArgumentException">数据长度错误</exception>
|
||||
// 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<byte>();
|
||||
}
|
||||
|
||||
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})";
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 内部 P/Invoke 层，直接映射 C DLL 函数
|
||||
/// </summary>
|
||||
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
|
||||
);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// MCU 串口/端口操作托管封装类
|
||||
/// 实现 IDisposable 管理底层句柄生命周期
|
||||
/// </summary>
|
||||
public class MCUSerialBridge : IDisposable
|
||||
{
|
||||
public static uint MaxPortNumber = 16;
|
||||
|
||||
private IntPtr nativeHandle = IntPtr.Zero;
|
||||
|
||||
/// <summary>判断是否已打开</summary>
|
||||
public bool IsOpen => nativeHandle != IntPtr.Zero;
|
||||
|
||||
/// <summary>构造函数，初始化对象</summary>
|
||||
// M层MCU适配：创建串口桥包装器并固定原生回调委托。
|
||||
public MCUSerialBridge()
|
||||
{
|
||||
nativeHandle = IntPtr.Zero;
|
||||
}
|
||||
|
||||
/// <summary>析构函数</summary>
|
||||
// M层MCU适配：对象回收时兜底释放原生串口桥句柄。
|
||||
~MCUSerialBridge()
|
||||
{
|
||||
Dispose(false);
|
||||
}
|
||||
|
||||
/// <summary>显式释放资源</summary>
|
||||
// M层MCU适配：释放串口桥句柄和非托管资源。
|
||||
public void Dispose()
|
||||
{
|
||||
Dispose(true);
|
||||
GC.SuppressFinalize(this);
|
||||
}
|
||||
|
||||
/// <summary>内部释放资源方法</summary>
|
||||
/// <param name="disposing">true 表示手动释放，false 表示析构释放</param>
|
||||
// M层MCU适配：按托管或终结路径关闭原生句柄。
|
||||
private void Dispose(bool disposing)
|
||||
{
|
||||
if (nativeHandle != IntPtr.Zero)
|
||||
{
|
||||
MCUSerialBridgeCoreAPI.msb_close(nativeHandle);
|
||||
nativeHandle = IntPtr.Zero;
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>打开串口</summary>
|
||||
/// <param name="portName">串口名，如 "COM3"</param>
|
||||
/// <param name="baud">波特率</param>
|
||||
/// <returns>错误码</returns>
|
||||
// M层单车通信：按端口名和波特率连接MCU串口桥。
|
||||
public MCUSerialBridgeError Open(string portName, uint baud)
|
||||
{
|
||||
return MCUSerialBridgeCoreAPI.msb_open(out nativeHandle, portName, baud);
|
||||
}
|
||||
|
||||
/// <summary>关闭串口</summary>
|
||||
/// <returns>错误码</returns>
|
||||
// M层单车通信：关闭当前MCU串口桥连接。
|
||||
public MCUSerialBridgeError Close()
|
||||
{
|
||||
if (nativeHandle == IntPtr.Zero)
|
||||
return MCUSerialBridgeError.Win_HandleNotFound;
|
||||
|
||||
MCUSerialBridgeError error = MCUSerialBridgeCoreAPI.msb_close(nativeHandle);
|
||||
nativeHandle = IntPtr.Zero;
|
||||
return error;
|
||||
}
|
||||
|
||||
/// <summary>MCU 复位</summary>
|
||||
/// <returns>错误码</returns>
|
||||
// M层单车通信：请求MCU复位并等待结果。
|
||||
public MCUSerialBridgeError Reset(uint timeout = 200)
|
||||
{
|
||||
if (nativeHandle == IntPtr.Zero)
|
||||
return MCUSerialBridgeError.Win_HandleNotFound;
|
||||
|
||||
return MCUSerialBridgeCoreAPI.msb_reset(nativeHandle, timeout);
|
||||
}
|
||||
|
||||
/// <summary>获取固件版本</summary>
|
||||
/// <param name="version">输出版本信息</param>
|
||||
/// <param name="timeout">超时时间（ms）</param>
|
||||
/// <returns>错误码</returns>
|
||||
// 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);
|
||||
}
|
||||
|
||||
/// <summary>获取 MCU 当前状态</summary>
|
||||
/// <param name="state">输出状态</param>
|
||||
/// <param name="timeout">超时时间（ms）</param>
|
||||
/// <returns>错误码</returns>
|
||||
// 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);
|
||||
}
|
||||
|
||||
/// <summary>配置端口</summary>
|
||||
/// <param name="ports">端口集合</param>
|
||||
/// <param name="timeout">超时时间（ms）</param>
|
||||
/// <returns>错误码</returns>
|
||||
// M层MCU适配：批量配置CAN和串口通道参数。
|
||||
public MCUSerialBridgeError Configure(IEnumerable<PortConfig> 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);
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>读取输入（4 字节）</summary>
|
||||
/// <param name="inputs">输出数组</param>
|
||||
/// <param name="timeout">超时（ms）</param>
|
||||
/// <returns>错误码</returns>
|
||||
// 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);
|
||||
}
|
||||
|
||||
/// <summary>写输出（4 字节）</summary>
|
||||
/// <param name="outputs">数据数组</param>
|
||||
/// <param name="timeout">超时（ms）</param>
|
||||
/// <returns>错误码</returns>
|
||||
// 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);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 读取 Serial 端口的一帧数据。
|
||||
/// Serial 上报的数据按帧入队；本接口每次调用最多读取一帧。
|
||||
/// 注意，如果不及时调用该接口，数据可能会丢失。
|
||||
/// </summary>
|
||||
/// <param name="portIndex">Serial 端口索引</param>
|
||||
/// <param name="buffer">接收到的数据</param>
|
||||
/// <param name="timeout">
|
||||
/// 超时时间（毫秒）
|
||||
/// - 0：不等待，有数据立即返回，没有数据立即返回 MSB_Error_NoData
|
||||
/// - >0：若当前无数据，最多等待 timeout，期间有新帧到达则立即返回
|
||||
/// </param>
|
||||
/// <remarks>
|
||||
/// 如果已经注册回调，本函数将始终返回 MSB_Error_NoData
|
||||
/// </remarks>
|
||||
/// <returns>
|
||||
/// 错误码 MCUSerialBridgeError
|
||||
/// - OK 成功读取一帧
|
||||
/// - NoData 当前无可读数据（仅在 timeout == 0 或等待超时）
|
||||
/// - Win_InvalidParam 参数错误
|
||||
/// </returns>
|
||||
// M层串口通信：同步读取指定MCU串口的数据。
|
||||
public MCUSerialBridgeError ReadSerial(byte portIndex, out byte[] buffer, uint timeout)
|
||||
{
|
||||
buffer = Array.Empty<byte>();
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 写 Serial 端口数据。
|
||||
/// 注意：对于单个Serial端口，不支持多线程并行发送，不要在上一条数据没有发送完成之前调用该函数，否则有可能导致数据错误。
|
||||
/// 注意：超时时间一定要大于波特率和数据长度综合得出的帧时间
|
||||
/// </summary>
|
||||
/// <param name="portIndex">Serial 端口索引</param>
|
||||
/// <param name="data">待发送数据</param>
|
||||
/// <param name="timeout">超时时间（毫秒）</param>
|
||||
/// <returns>
|
||||
/// 错误码 MCUSerialBridgeError
|
||||
/// - OK 成功发送
|
||||
/// - 其他错误请查看 MCUSerialBridgeError
|
||||
/// </returns>
|
||||
// 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
|
||||
);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 读取 CAN 端口的一帧数据。
|
||||
/// CAN 上报的数据按帧入队；本接口每次调用最多读取一帧。
|
||||
/// 注意，如果不及时调用该接口，数据可能会丢失。
|
||||
/// </summary>
|
||||
/// <param name="portIndex">CAN 端口索引</param>
|
||||
/// <param name="message">输出 CAN 消息对象</param>
|
||||
/// <param name="timeout">超时时间（毫秒）</param>
|
||||
/// <remarks>
|
||||
/// 如果已经注册回调，本函数将始终返回 MSB_Error_NoData
|
||||
/// </remarks>
|
||||
/// <returns>
|
||||
/// 错误码 MCUSerialBridgeError
|
||||
/// - OK 成功读取一帧
|
||||
/// - NoData 当前无可读数据（仅在 timeout == 0 或等待超时）
|
||||
/// - Win_InvalidParam 参数错误
|
||||
/// - CAN_DataError CAN数据错误
|
||||
/// - Win_HandleNotFound 句柄无效
|
||||
/// </returns>
|
||||
// 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;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 写 CAN 端口数据。
|
||||
/// 注意：CAN 支持多线程发送，最多可同时发送 16 个消息。
|
||||
/// 但是，多个CAN消息会进入排队队列等待发送，如果消息太多，可能引起超时。
|
||||
/// </summary>
|
||||
/// <param name="portIndex">CAN 端口索引</param>
|
||||
/// <param name="message">待发送 CAN 消息对象</param>
|
||||
/// <param name="timeout">超时时间（毫秒），默认 500ms</param>
|
||||
/// <returns>
|
||||
/// 错误码 MCUSerialBridgeError
|
||||
/// - OK 成功发送
|
||||
/// - Win_InvalidParam 参数错误
|
||||
/// - CAN_DataError CAN 数据错误
|
||||
/// - Win_HandleNotFound 句柄无效
|
||||
/// </returns>
|
||||
// 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 = [];
|
||||
|
||||
/// <summary>
|
||||
/// 注册指定端口(Serial)的回调函数
|
||||
/// </summary>
|
||||
/// <param name="portIndex">端口索引</param>
|
||||
/// <param name="callback">接收数据回调，byte[] 为接收到的原始数据</param>
|
||||
/// <returns>错误码</returns>
|
||||
/// <remarks>
|
||||
/// 注意事项：
|
||||
/// 1. 回调会在底层 C 层线程中直接调用，请**不要在回调内阻塞**，例如等待 I/O 或 Sleep。
|
||||
/// 2. 回调内**不能调用 WriteSerial/WriteCAN 等发送函数**，否则可能导致死锁或丢帧。
|
||||
/// 3. 回调内只能做轻量级操作，例如简单解析、统计或打标记。
|
||||
/// 4. 若需要复杂处理（例如长时间解析、解码、存储数据库等），请**将数据入队到另一个线程**，再在后台处理。
|
||||
/// 5. 数据可能随时到来，请保证回调尽快返回，避免影响后续帧接收。
|
||||
/// 6. 不要把其他类型的端口注册到这个接口，接口不对 portIndex 做类型检查。
|
||||
/// </remarks>
|
||||
// M层串口通信：注册指定串口的异步接收回调。
|
||||
public MCUSerialBridgeError RegisterSerialPortCallback(
|
||||
byte portIndex,
|
||||
Action<byte[]> 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
|
||||
);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 注册指定端口(CAN)的回调函数
|
||||
/// </summary>
|
||||
/// <param name="portIndex">端口索引</param>
|
||||
/// <param name="callback">接收数据回调，CANMessage 为接收到的原始数据</param>
|
||||
/// <returns>错误码</returns>
|
||||
/// <remarks>
|
||||
/// 注意事项：
|
||||
/// 1. 回调会在底层 C 层线程中直接调用，请**不要在回调内阻塞**，例如等待 I/O 或 Sleep。
|
||||
/// 2. 回调内**不能调用 WriteSerial/WriteCAN 等发送函数**，否则可能导致死锁或丢帧。
|
||||
/// 3. 回调内只能做轻量级操作，例如简单解析、统计或打标记。
|
||||
/// 4. 若需要复杂处理（例如长时间解析、解码、存储数据库等），请**将数据入队到另一个线程**，再在后台处理。
|
||||
/// 5. 数据可能随时到来，请保证回调尽快返回，避免影响后续帧接收。
|
||||
/// 6. 不要把其他类型的端口注册到这个接口，接口不对 portIndex 做类型检查。
|
||||
/// </remarks>
|
||||
// M层CAN通信：注册指定CAN通道的异步接收回调。
|
||||
public MCUSerialBridgeError RegisterCANPortCallback(
|
||||
byte portIndex,
|
||||
Action<CANMessage> 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
|
||||
);
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -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",
|
||||
};
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,58 @@
|
||||
<Project Sdk="Microsoft.NET.Sdk">
|
||||
|
||||
<PropertyGroup>
|
||||
<TargetFramework>net8.0</TargetFramework>
|
||||
<ImplicitUsings>enable</ImplicitUsings>
|
||||
<Nullable>disable</Nullable>
|
||||
<Platforms>AnyCPU</Platforms>
|
||||
|
||||
<AssemblyName>MedullaAdapter</AssemblyName>
|
||||
|
||||
<AppendTargetFrameworkToOutputPath>false</AppendTargetFrameworkToOutputPath>
|
||||
<OutputPath>build\Medulla\plugins\</OutputPath>
|
||||
</PropertyGroup>
|
||||
|
||||
<ItemGroup>
|
||||
<Reference Include="CartActivator">
|
||||
<HintPath>ref\RefCartActivator.dll</HintPath>
|
||||
<Private>false</Private>
|
||||
</Reference>
|
||||
<Reference Include="MedullaCore">
|
||||
<HintPath>ref\RefMedullaCore.dll</HintPath>
|
||||
<Private>false</Private>
|
||||
</Reference>
|
||||
|
||||
<Reference Include="FundamentalLib">
|
||||
<HintPath>ref\RefFundamentalLib.dll</HintPath>
|
||||
<Private>false</Private>
|
||||
</Reference>
|
||||
|
||||
<Reference Include="MDCSToolBox">
|
||||
<HintPath>ref\MDCSToolBox.dll</HintPath>
|
||||
<Private>false</Private>
|
||||
</Reference>
|
||||
|
||||
<!-- <Reference Include="CycleGUI">
|
||||
<HintPath>ref\CycleGUI.dll</HintPath>
|
||||
<Private>false</Private>
|
||||
</Reference> -->
|
||||
<Reference Include="CommonUsage">
|
||||
<HintPath>..\ref\CommonUsage.dll</HintPath>
|
||||
</Reference>
|
||||
</ItemGroup>
|
||||
|
||||
<ItemGroup>
|
||||
<Compile Include="..\Shared\Models\ChassisCommand.cs"
|
||||
Link="Shared\Models\ChassisCommand.cs" />
|
||||
|
||||
<Compile Include="..\Shared\Mathematics\FrameTransform2D.cs"
|
||||
Link="Shared\Mathematics\FrameTransform2D.cs" />
|
||||
|
||||
<Compile Include="..\Shared\Mathematics\AngleMath.cs"
|
||||
Link="Shared\Mathematics\AngleMath.cs" />
|
||||
|
||||
<Compile Include="..\Shared\Chassis\MultiWheelChassisAdapter.cs"
|
||||
Link="Shared\Chassis\MultiWheelChassisAdapter.cs" />
|
||||
</ItemGroup>
|
||||
|
||||
</Project>
|
||||
@@ -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<DiverCartDefinition>
|
||||
{
|
||||
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);
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,86 @@
|
||||
// Medulla虚拟遥控器和夹臂控制
|
||||
using MDCSToolBox.Medulla.Chassis.MultiWheel;
|
||||
using CartActivator;
|
||||
|
||||
namespace MedullaAdapter
|
||||
{
|
||||
// M层单车虚拟遥控器：使用父类提供的底盘控制界面。
|
||||
public class Remote : MultiWheelRemote<DiverCartDefinition>
|
||||
{
|
||||
// 将父类虚拟遥控器界面输入统一转发到本车的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();
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -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
|
||||
{
|
||||
/// <summary>
|
||||
/// 在后台保存驱动器CAN速度事件和底盘周期快照，避免文件IO阻塞CAN回调。
|
||||
/// </summary>
|
||||
internal sealed class WheelSpeedDiagnosticLogger : IDisposable
|
||||
{
|
||||
private readonly struct LogRecord
|
||||
{
|
||||
public LogRecord(bool isCanEvent, string line)
|
||||
{
|
||||
IsCanEvent = isCanEvent;
|
||||
Line = line;
|
||||
}
|
||||
|
||||
public bool IsCanEvent { get; }
|
||||
|
||||
public string Line { get; }
|
||||
}
|
||||
|
||||
private const int MaximumQueuedRecords = 100000;
|
||||
private const double SnapshotIntervalMilliseconds = 20.0;
|
||||
private readonly ConcurrentQueue<LogRecord> _records = new();
|
||||
private readonly AutoResetEvent _recordsAvailable = new(false);
|
||||
private readonly object _lifecycleLock = new();
|
||||
private Stopwatch _stopwatch;
|
||||
private Thread _writerThread;
|
||||
private StreamWriter _canWriter;
|
||||
private StreamWriter _snapshotWriter;
|
||||
private volatile bool _isRunning;
|
||||
private int _queuedRecordCount;
|
||||
private long _receiveSequence;
|
||||
private long _droppedRecordCount;
|
||||
private double _lastSnapshotMilliseconds = double.NegativeInfinity;
|
||||
|
||||
public bool IsRunning => _isRunning;
|
||||
|
||||
public string CanLogPath { get; private set; } = "";
|
||||
|
||||
public string SnapshotLogPath { get; private set; } = "";
|
||||
|
||||
/// <summary>
|
||||
/// 创建本次诊断的两个CSV文件并启动后台写入线程。
|
||||
/// </summary>
|
||||
public void Start(string directory, int carNumber)
|
||||
{
|
||||
lock (_lifecycleLock)
|
||||
{
|
||||
if (_isRunning)
|
||||
return;
|
||||
|
||||
if (string.IsNullOrWhiteSpace(directory))
|
||||
throw new ArgumentException(
|
||||
"轮速诊断目录不能为空。",
|
||||
nameof(directory));
|
||||
|
||||
Directory.CreateDirectory(directory);
|
||||
|
||||
var filePrefix =
|
||||
$"{DateTime.Now:yyyyMMdd_HHmmss_fff}_Car{carNumber}";
|
||||
|
||||
CanLogPath = Path.Combine(
|
||||
directory,
|
||||
$"{filePrefix}_can.csv");
|
||||
|
||||
SnapshotLogPath = Path.Combine(
|
||||
directory,
|
||||
$"{filePrefix}_snapshot.csv");
|
||||
|
||||
_canWriter = CreateWriter(CanLogPath);
|
||||
_snapshotWriter = CreateWriter(SnapshotLogPath);
|
||||
|
||||
_canWriter.WriteLine(
|
||||
"ElapsedMs,ReceiveSequence,CanId,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();
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 停止记录并等待队列中的诊断数据写入磁盘。
|
||||
/// </summary>
|
||||
public void Stop()
|
||||
{
|
||||
Thread writerThread;
|
||||
|
||||
lock (_lifecycleLock)
|
||||
{
|
||||
if (!_isRunning &&
|
||||
_writerThread == null)
|
||||
return;
|
||||
|
||||
_isRunning = false;
|
||||
writerThread = _writerThread;
|
||||
_recordsAvailable.Set();
|
||||
}
|
||||
|
||||
writerThread?.Join(3000);
|
||||
|
||||
lock (_lifecycleLock)
|
||||
{
|
||||
_canWriter?.Flush();
|
||||
_snapshotWriter?.Flush();
|
||||
_canWriter?.Dispose();
|
||||
_snapshotWriter?.Dispose();
|
||||
_canWriter = null;
|
||||
_snapshotWriter = null;
|
||||
_writerThread = null;
|
||||
_stopwatch?.Stop();
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 将一帧驱动器速度反馈加入内存队列，不在CAN回调中执行文件写入。
|
||||
/// </summary>
|
||||
public void RecordCanFeedback(
|
||||
ushort canId,
|
||||
string motorName,
|
||||
float rawRpm,
|
||||
float speedMetersPerSecond)
|
||||
{
|
||||
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));
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 按最多50Hz记录一帧控制命令、PID输出、CAN反馈和舵角快照。
|
||||
/// </summary>
|
||||
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();
|
||||
}
|
||||
}
|
||||
}
|
||||
Binary file not shown.
@@ -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": ""
|
||||
}
|
||||
}
|
||||
}
|
||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
@@ -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();
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,45 @@
|
||||
using ClumsyCore;
|
||||
using ClumsyCore.Pilot;
|
||||
using MDCSToolBox.Clumsy.MotionControllers;
|
||||
using MDCSToolBox.Clumsy.Movements;
|
||||
using MDCSToolBox.Clumsy.Pilot;
|
||||
|
||||
namespace MultiWheelC;
|
||||
|
||||
public class ChassisController : MovementDefinition<MultiWheelGeometricController>
|
||||
{
|
||||
public float BaseSpeed = Configuration.conf.basicSpeed;
|
||||
|
||||
// 创建单车几何跟踪控制器（直接控本车底盘，不走多车 Auto 通道）
|
||||
public override MultiWheelGeometricController Get()
|
||||
{
|
||||
return new MultiWheelGeometricController
|
||||
{
|
||||
Chassis = BasicPilotBase.Chassis,
|
||||
BaseSpeed = BaseSpeed,
|
||||
SlowDistance = PilotDefinition.Conf.SlowDistance,
|
||||
SlowingPow = PilotDefinition.Conf.SlowingPow,
|
||||
FinishDistance = PilotDefinition.Conf.FinishDistance,
|
||||
FinishSpeed = PilotDefinition.Conf.FinishSpeed,
|
||||
FirstThAccuracy = PilotDefinition.Conf.FirstThAccuracy,
|
||||
FirstRotateSpeedFac = PilotDefinition.Conf.FirstRotateSpeedFac,
|
||||
FirstRotateMaxSpeed = PilotDefinition.Conf.FirstRotateMaxSpeed,
|
||||
NotContinuousAngle = PilotDefinition.Conf.NotContinuousAngle,
|
||||
DebugMode = PilotDefinition.Conf.MotionDebugPrint,
|
||||
DebugCurvature = PilotDefinition.Conf.DebugCurvature,
|
||||
PowerSteeringLookAhead = PilotDefinition.Conf.PowerSteeringLookAhead,
|
||||
SpeedLookAhead = PilotDefinition.Conf.SpeedLookAhead,
|
||||
SpeedLookAheadCurveDiff = PilotDefinition.Conf.SpeedLookAheadCurveDiff,
|
||||
SpeedLookBackCurveDiff = PilotDefinition.Conf.SpeedLookBackCurveDiff,
|
||||
SpeedLimitCurveDiffMin = PilotDefinition.Conf.SpeedLimitCurveDiffMin,
|
||||
SpeedLimitCurveMin = PilotDefinition.Conf.SpeedLimitCurveMin,
|
||||
MaxRotateSpeed = PilotDefinition.Conf.MaxRotateSpeedCurveLimit,
|
||||
MaxRotateAcc = PilotDefinition.Conf.MaxRotateAccCurveLimit,
|
||||
GcpThetaThreshold = PilotDefinition.Conf.GcpThetaThreshold,
|
||||
DthLinearFac = PilotDefinition.Conf.DthLinearFac,
|
||||
DthLinearThreshold = PilotDefinition.Conf.DthLinearThreshold,
|
||||
BiasFac = PilotDefinition.Conf.BiasFac,
|
||||
BiasThreshold = PilotDefinition.Conf.BiasThreshold,
|
||||
};
|
||||
}
|
||||
}
|
||||
@@ -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<float, float, float> CommandObserver;
|
||||
|
||||
// 运动坐标系相对车体坐标系的朝向：普通模式为0，蟹行为π/2。
|
||||
public double MotionFrameYawInBodyRadians = Math.PI / 2.0;
|
||||
private double _lastSCurveProgress;
|
||||
|
||||
public override IEnumerable<bool> 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);
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -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<Vector2>
|
||||
{
|
||||
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<Vector2>
|
||||
{
|
||||
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<Vector2>
|
||||
{
|
||||
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
|
||||
}
|
||||
@@ -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<bool> 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<bool> Get()
|
||||
{
|
||||
if (Second <= 0)
|
||||
{
|
||||
yield return false;
|
||||
yield break;
|
||||
}
|
||||
|
||||
var endTime = DateTime.UtcNow.AddSeconds(Second);
|
||||
while (DateTime.UtcNow < endTime)
|
||||
{
|
||||
Thread.Sleep(50);
|
||||
yield return true;
|
||||
}
|
||||
|
||||
yield return false;
|
||||
}
|
||||
}
|
||||
public class DriverAble : MovementDefinition
|
||||
{
|
||||
public int WaitTimeoutMs = 2000;
|
||||
public int PollIntervalMs = 50;
|
||||
|
||||
// C层单车硬件：请求全部驱动轮复位并恢复使能。
|
||||
public override IEnumerable<bool> Get()
|
||||
{
|
||||
PilotDefinition.Self.ResetFromC = true;
|
||||
|
||||
try
|
||||
{
|
||||
var start = DateTime.Now;
|
||||
var timeoutMs = Math.Max(0, WaitTimeoutMs);
|
||||
var pollMs = Math.Max(1, PollIntervalMs);
|
||||
|
||||
// 至少保留一个调度周期，确保M层能收到复位请求。
|
||||
yield return true;
|
||||
|
||||
while (!PilotDefinition.Self.WheelAbleState &&
|
||||
(DateTime.Now - start).TotalMilliseconds < timeoutMs)
|
||||
{
|
||||
Thread.Sleep(pollMs);
|
||||
yield return true;
|
||||
}
|
||||
}
|
||||
finally
|
||||
{
|
||||
PilotDefinition.Self.ResetFromC = false;
|
||||
}
|
||||
}
|
||||
}
|
||||
public class DriverDisable : MovementDefinition
|
||||
{
|
||||
public int WaitTimeoutMs = 3000;
|
||||
public int PollIntervalMs = 20;
|
||||
|
||||
// C层单车硬件：请求驱动轮退出使能，并等待M层状态反馈。
|
||||
public override IEnumerable<bool> Get()
|
||||
{
|
||||
var timeoutMs = Math.Max(0, WaitTimeoutMs);
|
||||
var pollMs = Math.Max(1, PollIntervalMs);
|
||||
var startTime = DateTime.UtcNow;
|
||||
var success = false;
|
||||
|
||||
PilotDefinition.Self.DisableFromC = true;
|
||||
|
||||
try
|
||||
{
|
||||
// 至少保持一个C层调度周期，确保M层能收到下使能请求。
|
||||
yield return true;
|
||||
|
||||
success = !PilotDefinition.Self.WheelAbleState;
|
||||
|
||||
while (!success &&
|
||||
(DateTime.UtcNow - startTime).TotalMilliseconds <
|
||||
timeoutMs)
|
||||
{
|
||||
Thread.Sleep(pollMs);
|
||||
|
||||
success =
|
||||
!PilotDefinition.Self.WheelAbleState;
|
||||
|
||||
if (!success)
|
||||
{
|
||||
yield return true;
|
||||
}
|
||||
}
|
||||
}
|
||||
finally
|
||||
{
|
||||
// 无论正常完成、超时、异常还是任务被停止，都撤销请求。
|
||||
PilotDefinition.Self.DisableFromC = false;
|
||||
}
|
||||
if (success)
|
||||
{
|
||||
Console.WriteLine(
|
||||
$"驱动器下使能完成，" +
|
||||
$"WheelAbleState=" +
|
||||
$"{PilotDefinition.Self.WheelAbleState}");
|
||||
}
|
||||
else
|
||||
{
|
||||
Console.WriteLine(
|
||||
$"驱动器下使能超时，" +
|
||||
$"WheelAbleState=" +
|
||||
$"{PilotDefinition.Self.WheelAbleState}，" +
|
||||
$"等待{timeoutMs}ms");
|
||||
}
|
||||
yield return false;
|
||||
}
|
||||
}
|
||||
#endregion
|
||||
|
||||
#region 直线运动
|
||||
//在世界坐标系下，从路径起点追踪到终点并停车
|
||||
public class DstTracker : MovementDefinition
|
||||
{
|
||||
public Vector2 Src;
|
||||
public Vector2 Dst;
|
||||
// 本次轨迹的巡航速度上限，单位m/s。
|
||||
public float MaxSpeed = PilotDefinition.Conf.DstTrackerMaxSpeed;
|
||||
public float CarDirectionBias = 0f;
|
||||
public Painter Painter = UI.GetPainter("DstTracker");
|
||||
public override IEnumerable<bool> Get()
|
||||
{
|
||||
var chassis = (MultiWheelChassis)PilotDefinition.Chassis;
|
||||
DriveTask task = null;
|
||||
try
|
||||
{
|
||||
Console.WriteLine($"DstTracker src:({Src.X:F2}, {Src.Y:F2}) dst:({Dst.X:F2}, {Dst.Y:F2})");
|
||||
Painter.DrawLine(Color.Cyan, Src.X, Src.Y, Dst.X, Dst.Y, width: 3);
|
||||
|
||||
var tracker = new ChassisController
|
||||
{
|
||||
BaseSpeed = MaxSpeed
|
||||
}.Get();
|
||||
// 要求路径末端速度下降到零。
|
||||
tracker.FinishSpeed = 0f;
|
||||
var linePath = new LineTrack(Src, Dst)
|
||||
{
|
||||
CarDirectionBias = CarDirectionBias,
|
||||
Speed = MaxSpeed
|
||||
};
|
||||
tracker.AddTrack(linePath);
|
||||
task = new DriveTask(tracker.Track());
|
||||
task.Wait();
|
||||
yield return false;
|
||||
}
|
||||
finally
|
||||
{
|
||||
task?.Stop();
|
||||
chassis.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<int> LeaveSrcFunction;
|
||||
// 接近目标后是否保留速度，交给下一个动作接管。
|
||||
public bool EnableHandover;
|
||||
// 进入动作衔接的剩余距离，单位mm。
|
||||
public float HandoverDistance = 80f;
|
||||
// HandoverSpeed小于0时，使用MaxSpeed的此比例。
|
||||
public float HandoverSpeedRatio = 0.5f;
|
||||
// 大于等于0时，直接作为衔接速度，单位m/s。
|
||||
public float HandoverSpeed = -1f;
|
||||
public float MinHandoverSpeed = 0.05f;
|
||||
private PIDController _pid;
|
||||
// 读取当前单车直线行驶里程，单位mm。
|
||||
private static float ReadPosition()
|
||||
{
|
||||
return
|
||||
(PilotDefinition.Self.LFLActualPos + PilotDefinition.Self.LFRActualPos) / 2f;
|
||||
}
|
||||
|
||||
// 根据动作启动位置和目标距离执行直线里程闭环。
|
||||
public override IEnumerable<bool> Get()
|
||||
{
|
||||
if (float.IsNaN(TargetDistance) || float.IsInfinity(TargetDistance))
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(
|
||||
nameof(TargetDistance),
|
||||
"目标行驶距离必须是有限值。");
|
||||
}
|
||||
|
||||
if (float.IsNaN(MaxSpeed) || float.IsInfinity(MaxSpeed) || MaxSpeed <= 0f)
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(
|
||||
nameof(MaxSpeed),
|
||||
"最大速度必须是大于零的有限值。");
|
||||
}
|
||||
var chassis = (MultiWheelChassis)PilotDefinition.Chassis;
|
||||
// 每次启动动作时重新读取起始编码器位置。
|
||||
var startPosition = ReadPosition();
|
||||
// PID仍然控制绝对编码器位置，但绝对目标由动作自动计算。
|
||||
var targetPosition = startPosition + TargetDistance;
|
||||
_pid = new PIDController(ReadPosition, Kp, Ki, Kd, 0, DeadZone, MaxSpeed)
|
||||
{
|
||||
SpeedAccPerSec = Math.Abs(MaxSpeed) / 2f
|
||||
};
|
||||
var handoverRequested = false;
|
||||
var keepHandoverSpeed = false;
|
||||
DLog.Log(
|
||||
$"直线里程动作：" +
|
||||
$"起点={startPosition:F1}mm，" +
|
||||
$"距离={TargetDistance:F1}mm，" +
|
||||
$"目标={targetPosition:F1}mm",
|
||||
"straight_line");
|
||||
try
|
||||
{
|
||||
while (true)
|
||||
{
|
||||
var currentPosition = ReadPosition();
|
||||
var remainingDistance = targetPosition - currentPosition;
|
||||
// 接近目标后，保留一定速度交给后续动作。
|
||||
if (EnableHandover && Math.Abs(remainingDistance) <= Math.Max(1f, HandoverDistance))
|
||||
{
|
||||
var direction = Math.Sign(remainingDistance);
|
||||
if (direction == 0)
|
||||
{
|
||||
direction = Math.Sign(TargetDistance);
|
||||
}
|
||||
var requestedSpeed = HandoverSpeed >= 0f ? Math.Abs(HandoverSpeed) : Math.Abs(MaxSpeed) * HandoverSpeedRatio;
|
||||
var maximumSpeed = Math.Abs(MaxSpeed);
|
||||
var minimumSpeed = Math.Min(Math.Abs(MinHandoverSpeed), maximumSpeed);
|
||||
var limitedSpeed = Math.Max(minimumSpeed, Math.Min(requestedSpeed, maximumSpeed));
|
||||
var handoverSpeed = limitedSpeed * direction;
|
||||
chassis.SendXYThSpeed(handoverSpeed, 0f, 0f);
|
||||
handoverRequested = true;
|
||||
// 保持一个调度周期，让速度命令实际生效。
|
||||
yield return true;
|
||||
break;
|
||||
}
|
||||
var speed = _pid.GetResponse(targetPosition);
|
||||
chassis.SendXYThSpeed(speed, 0f, 0f);
|
||||
if (_pid.IsArrived())
|
||||
{
|
||||
break;
|
||||
}
|
||||
yield return true;
|
||||
}
|
||||
if (SrcId != -1 &&
|
||||
LeaveSrcFunction != null)
|
||||
{
|
||||
LeaveSrcFunction(SrcId);
|
||||
DLog.Log($"释放放车点{SrcId}", "straight_line");
|
||||
}
|
||||
// 只有正常完成动作衔接时才允许保留非零速度。
|
||||
keepHandoverSpeed = handoverRequested;
|
||||
}
|
||||
finally
|
||||
{
|
||||
// 普通完成、人工停止或异常退出时都必须停车。
|
||||
if (!keepHandoverSpeed)
|
||||
{
|
||||
chassis.SendXYThSpeed(0f, 0f, 0f);
|
||||
}
|
||||
}
|
||||
yield return false;
|
||||
}
|
||||
}
|
||||
//直线行走基于detour
|
||||
public class LineTracking_based_detour : MovementDefinition
|
||||
{
|
||||
public float LineDistance = 1000f;
|
||||
public int SrcId = -1;
|
||||
public int DstId = -1;
|
||||
public Action<int> LeaveSrcFunction = null;
|
||||
public Painter painter = UI.GetPainter("Line", false);
|
||||
// C层单车轨迹：执行早期版本的两点直线跟踪动作。
|
||||
public override IEnumerable<bool> Get()
|
||||
{
|
||||
var curpose = DetourInterface.getCartLocation();
|
||||
Console.WriteLine($"curpose.th:{curpose.th}");
|
||||
var src = new Vector2((float)curpose.x, (float)curpose.y);
|
||||
var headingRadians =
|
||||
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
|
||||
{
|
||||
/// <summary>
|
||||
/// 旋转目标角度
|
||||
/// </summary>
|
||||
public float AngleTarget;
|
||||
|
||||
public Func<float> ThetaReader = () => (float)DetourInterface.getCartLocation().th;
|
||||
|
||||
public MultiWheelChassis Chassis = (MultiWheelChassis)PilotDefinition.Chassis;
|
||||
|
||||
public Func<PIDParams> PidparamsRead = () => new PIDParams() { };
|
||||
|
||||
public PIDController thPid;
|
||||
|
||||
// 将本周期PID角速度输出提供给实验记录器，单位deg/s。
|
||||
public Action<float> CommandAngularSpeedObserver;
|
||||
|
||||
// 自转前舵轮实际角度允许误差，单位deg。
|
||||
public float WheelAlignmentToleranceDegrees = 2f;
|
||||
|
||||
// 自转舵轮连续保持到位的时间，单位s。
|
||||
public float WheelAlignmentStableSeconds = 0.3f;
|
||||
|
||||
// 自转舵轮准备超时时间，单位s。
|
||||
public float WheelAlignmentTimeoutSeconds = 10f;
|
||||
|
||||
// 先准备自转舵角，再通过安全版SendXYThSpeed闭环旋转到目标角度。
|
||||
public override IEnumerable<bool> 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<bool> Get()
|
||||
{
|
||||
try
|
||||
{
|
||||
leftpid = new PIDController(
|
||||
() => PilotDefinition.Self.ActualPosLeftArm,
|
||||
ClampKp, ClampKi, ClampKd, ClampMaxI,
|
||||
ClampDeadZone, MaxClampSpeed)
|
||||
{
|
||||
SpeedAccPerSec = ClampSpeedAcc
|
||||
};
|
||||
|
||||
rightpid = new PIDController(
|
||||
() => PilotDefinition.Self.ActualPosRightArm,
|
||||
ClampKp, ClampKi, ClampKd, ClampMaxI,
|
||||
ClampDeadZone, MaxClampSpeed)
|
||||
{
|
||||
SpeedAccPerSec = ClampSpeedAcc
|
||||
};
|
||||
|
||||
var startTime = DateTime.UtcNow;
|
||||
while (true)
|
||||
{
|
||||
if (TimeoutSeconds > 0f &&
|
||||
(DateTime.UtcNow - startTime).TotalSeconds >
|
||||
TimeoutSeconds)
|
||||
{
|
||||
Console.WriteLine(
|
||||
$"夹臂运动超时({TimeoutSeconds:F1}s)，" +
|
||||
"停止左右夹臂。");
|
||||
yield break;
|
||||
}
|
||||
|
||||
var leftspeed =
|
||||
leftpid.GetResponse(LeftClampTarget);
|
||||
var rightspeed =
|
||||
rightpid.GetResponse(RightClampTarget);
|
||||
Console.WriteLine(
|
||||
$"left arm speed:{leftspeed} " +
|
||||
$"right arm speed:{rightspeed}");
|
||||
|
||||
PilotDefinition.Self.SpeedLeftArm = leftspeed;
|
||||
PilotDefinition.Self.SpeedRightArm = rightspeed;
|
||||
|
||||
var leftArrived = leftpid.IsArrived();
|
||||
var rightArrived = rightpid.IsArrived();
|
||||
if (leftArrived)
|
||||
PilotDefinition.Self.SpeedLeftArm = 0f;
|
||||
if (rightArrived)
|
||||
PilotDefinition.Self.SpeedRightArm = 0f;
|
||||
|
||||
if (leftArrived && rightArrived)
|
||||
break;
|
||||
|
||||
yield return true;
|
||||
}
|
||||
|
||||
Console.WriteLine(
|
||||
$"left clamp to target:{LeftClampTarget} " +
|
||||
$"right clamp to target:{RightClampTarget}");
|
||||
}
|
||||
finally
|
||||
{
|
||||
PilotDefinition.Self.SpeedLeftArm = 0f;
|
||||
PilotDefinition.Self.SpeedRightArm = 0f;
|
||||
}
|
||||
}
|
||||
}
|
||||
#endregion
|
||||
}
|
||||
@@ -0,0 +1,43 @@
|
||||
<Project Sdk="Microsoft.NET.Sdk">
|
||||
|
||||
<PropertyGroup>
|
||||
<TargetFramework>netstandard2.0</TargetFramework>
|
||||
<LangVersion>10</LangVersion>
|
||||
<AssemblyName>MultiWheelC</AssemblyName>
|
||||
<RootNamespace>MultiWheelC</RootNamespace>
|
||||
<AppendTargetFrameworkToOutputPath>false</AppendTargetFrameworkToOutputPath>
|
||||
<OutputPath>build\Clumsy\</OutputPath>
|
||||
</PropertyGroup>
|
||||
|
||||
<ItemGroup>
|
||||
<PackageReference Include="Newtonsoft.Json" Version="13.0.3" />
|
||||
<PackageReference Include="System.Numerics.Vectors" Version="4.6.1" />
|
||||
</ItemGroup>
|
||||
|
||||
<ItemGroup>
|
||||
<Compile Include="..\Shared\**\*.cs"
|
||||
Link="Shared\%(RecursiveDir)%(Filename)%(Extension)" />
|
||||
</ItemGroup>
|
||||
|
||||
<ItemGroup>
|
||||
<Reference Include="LessokajiWeaverUtilities">
|
||||
<HintPath>ref\LessokajiWeaverUtilities.dll</HintPath>
|
||||
</Reference>
|
||||
<Reference Include="MDCSToolBox">
|
||||
<HintPath>ref\MDCSToolBox.dll</HintPath>
|
||||
</Reference>
|
||||
<Reference Include="ClumsyCore">
|
||||
<HintPath>ref\RefClumsyCore.dll</HintPath>
|
||||
</Reference>
|
||||
<Reference Include="ClumsyDance">
|
||||
<HintPath>ref\RefClumsyDance.dll</HintPath>
|
||||
</Reference>
|
||||
<Reference Include="FundamentalLib">
|
||||
<HintPath>ref\RefFundamentalLib.dll</HintPath>
|
||||
</Reference>
|
||||
<Reference Include="CommonUsage">
|
||||
<HintPath>..\ref\CommonUsage.dll</HintPath>
|
||||
</Reference>
|
||||
</ItemGroup>
|
||||
|
||||
</Project>
|
||||
@@ -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
|
||||
|
||||
|
||||
|
||||
}
|
||||
@@ -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<PilotConfig, PilotDefinition>
|
||||
{
|
||||
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;
|
||||
}
|
||||
@@ -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<TrackingSample> _samples =
|
||||
new List<TrackingSample>();
|
||||
|
||||
private readonly object _sampleSyncRoot =
|
||||
new object();
|
||||
|
||||
private readonly object _commandSyncRoot =
|
||||
new object();
|
||||
|
||||
private readonly Stopwatch _stopwatch =
|
||||
new Stopwatch();
|
||||
|
||||
private Thread _worker;
|
||||
private volatile bool _running;
|
||||
private int _started;
|
||||
private int _saved;
|
||||
|
||||
private bool _hasExternalCommand;
|
||||
private float _externalCommandSpeed;
|
||||
private float _externalCommandVx;
|
||||
private float _externalCommandVy;
|
||||
private float _externalCommandAngularSpeed;
|
||||
|
||||
public TrackingExperimentRecorder(
|
||||
string controllerName,
|
||||
string trajectoryName,
|
||||
int trialNumber,
|
||||
Vector2 referenceStart,
|
||||
Vector2 referenceEnd,
|
||||
float referenceSpeed,
|
||||
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<TrackingSample> snapshot;
|
||||
|
||||
lock (_sampleSyncRoot)
|
||||
{
|
||||
snapshot =
|
||||
new List<TrackingSample>(_samples);
|
||||
}
|
||||
|
||||
var outputDirectory = Path.Combine(
|
||||
AppContext.BaseDirectory,
|
||||
"TrackingExperiments");
|
||||
|
||||
Directory.CreateDirectory(outputDirectory);
|
||||
|
||||
var fileName =
|
||||
$"{DateTime.Now:yyyyMMdd_HHmmss_fff}_" +
|
||||
$"{SanitizeFileName(_controllerName)}_" +
|
||||
$"{SanitizeFileName(_trajectoryName)}_" +
|
||||
$"Trial{_trialNumber}.csv";
|
||||
|
||||
SavedFilePath = Path.Combine(
|
||||
outputDirectory,
|
||||
fileName);
|
||||
|
||||
using (var writer = new StreamWriter(
|
||||
SavedFilePath,
|
||||
false,
|
||||
new UTF8Encoding(true)))
|
||||
{
|
||||
writer.WriteLine(
|
||||
"ElapsedSeconds," +
|
||||
"ControllerName," +
|
||||
"TrajectoryName," +
|
||||
"TrialNumber," +
|
||||
"DetourX," +
|
||||
"DetourY," +
|
||||
"DetourTheta," +
|
||||
"CommandSpeed," +
|
||||
// 保留旧列（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("\"", "\"\"") +
|
||||
"\"";
|
||||
}
|
||||
}
|
||||
}
|
||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
@@ -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": ""
|
||||
}
|
||||
}
|
||||
}
|
||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Some files were not shown because too many files have changed in this diff Show More
Reference in New Issue
Block a user