Compare commits
2
Commits
main
..
097853234f
| Author | SHA1 | Date | |
|---|---|---|---|
  yuxiang.shenandCursor
|
097853234f | ||
|
yuxiang.shen
|
d9432bd529 |
@@ -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.
|
||||
+79
-42
@@ -1,56 +1,93 @@
|
||||
# Build results
|
||||
[Bb]in/
|
||||
[Oo]bj/
|
||||
build/
|
||||
artifacts/
|
||||
**/bin/
|
||||
**/obj/
|
||||
# Visual Studio / Rider / VS Code
|
||||
##################################################
|
||||
# Visual Studio
|
||||
##################################################
|
||||
|
||||
# Visual Studio 工作区缓存
|
||||
.vs/
|
||||
.idea/
|
||||
.vscode/
|
||||
**/.vs/
|
||||
|
||||
# 用户配置
|
||||
*.user
|
||||
*.suo
|
||||
*.rsuser
|
||||
*.userosscache
|
||||
*.sln.docstates
|
||||
|
||||
# .NET generated files
|
||||
project.assets.json
|
||||
project.nuget.cache
|
||||
*.nuget.g.props
|
||||
*.nuget.g.targets
|
||||
*.AssemblyInfo.cs
|
||||
*.GeneratedMSBuildEditorConfig.editorconfig
|
||||
*.GlobalUsings.g.cs
|
||||
*.FileListAbsolute.txt
|
||||
*.CoreCompileInputs.cache
|
||||
*.AssemblyReference.cache
|
||||
*.assets.cache
|
||||
##################################################
|
||||
# Build 输出
|
||||
##################################################
|
||||
|
||||
# Test and coverage output
|
||||
TestResults/
|
||||
coverage/
|
||||
*.coverage
|
||||
*.coveragexml
|
||||
coverage*.json
|
||||
# 编译输出目录
|
||||
bin/
|
||||
obj/
|
||||
**/bin/
|
||||
**/obj/
|
||||
|
||||
##################################################
|
||||
# Rider / VS Code
|
||||
##################################################
|
||||
|
||||
.idea/
|
||||
.vscode/
|
||||
|
||||
##################################################
|
||||
# NuGet
|
||||
##################################################
|
||||
|
||||
*.nupkg
|
||||
packages/
|
||||
|
||||
##################################################
|
||||
# 日志
|
||||
##################################################
|
||||
|
||||
# Logs, diagnostics, and temporary files
|
||||
*.log
|
||||
*.tlog
|
||||
*.binlog
|
||||
*.pdb
|
||||
*.cache
|
||||
|
||||
##################################################
|
||||
# 临时文件
|
||||
##################################################
|
||||
|
||||
*.tmp
|
||||
*.temp
|
||||
*.swp
|
||||
*.bak
|
||||
|
||||
# Local runtime configuration
|
||||
cartparams.json
|
||||
appsettings.Development.json
|
||||
*.local.json
|
||||
##################################################
|
||||
# 测试结果
|
||||
##################################################
|
||||
|
||||
TestResults/
|
||||
|
||||
##################################################
|
||||
# 发布目录
|
||||
##################################################
|
||||
|
||||
publish/
|
||||
|
||||
##################################################
|
||||
# Windows
|
||||
##################################################
|
||||
|
||||
# OS files
|
||||
Thumbs.db
|
||||
.DS_Store
|
||||
Desktop.ini
|
||||
|
||||
##################################################
|
||||
# JetBrains
|
||||
##################################################
|
||||
|
||||
_ReSharper*/
|
||||
*.DotSettings.user
|
||||
|
||||
##################################################
|
||||
# 缓存
|
||||
##################################################
|
||||
|
||||
*.cache
|
||||
|
||||
##################################################
|
||||
# 数据库（如果有）
|
||||
##################################################
|
||||
|
||||
*.db
|
||||
*.sqlite
|
||||
*.sqlite3
|
||||
|
||||
*.csv
|
||||
*.png
|
||||
@@ -0,0 +1,59 @@
|
||||
# Karpathy原则
|
||||
|
||||
## 先理解再修改
|
||||
|
||||
- 检查实际实现、调用链和现有约束，不凭名称猜测行为。
|
||||
- 明确必要假设；遇到会显著改变结果的歧义时先说明。
|
||||
|
||||
## 保持简单
|
||||
|
||||
- 使用满足当前需求的最小方案。
|
||||
- 不增加推测性功能、无必要抽象或配置。
|
||||
|
||||
## 精确修改
|
||||
|
||||
- 只触碰与当前任务直接相关的代码，保留既有风格和无关改动。
|
||||
- 清理由本次修改产生的废弃代码，不顺便清理原有无关代码。
|
||||
|
||||
## 面向验证
|
||||
|
||||
- 修改前明确可观察的成功标准，修改后运行相关检查。
|
||||
- 如实报告警告、限制和未验证项。
|
||||
|
||||
# MyParking项目规则
|
||||
|
||||
## 代码边界
|
||||
|
||||
- `CommonUsage-MultiVehicleSync`是独立的通用底盘库，不反向依赖`Shared`、M层或C层。
|
||||
- `Shared`只包含M/C共享的数据模型、数学方法和底盘适配代码。
|
||||
- `MedullaAdapter`负责M层硬件通信、IO和底盘命令。
|
||||
- `MultiWheelC`负责C层动作、控制、实验和数据记录。
|
||||
|
||||
## 代码规范
|
||||
|
||||
- 新增或修改的类、结构体和方法使用一句话的`/// <summary>`说明业务用途。
|
||||
- 单位、坐标系或正负方向不明确时补充说明，不复述代码字面内容。
|
||||
- Shared统一使用SI单位：m、m/s、rad、rad/s。
|
||||
- 车体坐标系为X向前、Y向左、逆时针为正；旧接口单位只在边界处转换。
|
||||
- 角度归一化、最短角差和度弧度转换统一使用`Shared/Mathematics/AngleMath.cs`，不重复手写。
|
||||
- 弧度归一化范围为`[-π, π)`，度归一化范围为`[-180°, 180°)`。
|
||||
- 车辆航向可用圆周最短角差；受`[-120°, 120°]`限制的机械舵角误差必须直接使用目标值减实际值。
|
||||
|
||||
## 控制安全
|
||||
|
||||
- 未经明确要求，不改变速度或舵角符号、CAN ID、遥控器映射、机械限位和模式切换策略。
|
||||
- 修改底盘命令、模式切换或四轮解算时，说明对实际运动的影响。
|
||||
- 实车测试采用低速、短距离，并确保可以立即停车。
|
||||
|
||||
## 构建验证
|
||||
|
||||
- 修改`CommonUsage-MultiVehicleSync`、`Shared`、`MedullaAdapter`或`MultiWheelC`后，在`MyParking`目录运行：
|
||||
|
||||
```powershell
|
||||
powershell -NoProfile -ExecutionPolicy Bypass -File .\build-and-package.ps1
|
||||
```
|
||||
|
||||
- Release构建在命令末尾追加`-Configuration Release`。
|
||||
- 报告各项目的警告和错误，并确认M/C部署包使用同一份`CommonUsage.dll`。
|
||||
- 不直接编辑`bin`、`obj`、`build`和`output`中的产物。
|
||||
- 不手工覆盖`ref/CommonUsage.dll`，由构建脚本统一更新。
|
||||
@@ -1,534 +0,0 @@
|
||||
using ClumsyCore;
|
||||
using ClumsyCore.Interfaces;
|
||||
using ClumsyCore.Sensors;
|
||||
using CommonUsage.Chassis;
|
||||
using CommonUsage.Mathematics;
|
||||
using FundamentalLib;
|
||||
using MDCSToolBox.Clumsy.AgvInterfaces;
|
||||
using MDCSToolBox.Clumsy.Calibration;
|
||||
using MDCSToolBox.Clumsy.MotionControllers;
|
||||
using MDCSToolBox.Clumsy.Tracks;
|
||||
using MDCSToolBox.Commons.Controllers;
|
||||
using Newtonsoft.Json;
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Net.Http;
|
||||
using System.Numerics;
|
||||
using System.Security.Cryptography;
|
||||
using System.Threading;
|
||||
using System.Threading.Tasks;
|
||||
using static ClumsyCore.DTools.Painter;
|
||||
|
||||
namespace MultiWheelC
|
||||
{
|
||||
public class SetLocationRes
|
||||
{
|
||||
public float x, y, th;
|
||||
public int l_step;
|
||||
public long tick;
|
||||
public string error;
|
||||
}
|
||||
public class AGV : MultiWheelInterface
|
||||
{
|
||||
public override AbstractGeometricController GetController()
|
||||
{
|
||||
return new ChassisController().Get();
|
||||
}
|
||||
|
||||
public override MultiWheelMagTracker GetMagController()
|
||||
{
|
||||
return new MultiWheelMagTracker();
|
||||
}
|
||||
|
||||
public override NaiveMagnetController GetNaiveMagnetController()
|
||||
{
|
||||
return new NaiveMagnetController();
|
||||
}
|
||||
|
||||
public void Sleep(float s)
|
||||
{
|
||||
new DriveTask(new Sleep() { Second = s }.Get()).Wait();
|
||||
}
|
||||
|
||||
public void ControlChargePort(bool open)
|
||||
{
|
||||
DLog.Log($"call ControlChargePort({open})");
|
||||
PilotDefinition.Self.OpenChargeByClumsy = open;
|
||||
}
|
||||
|
||||
public void SwitchLidarArea(int area)
|
||||
{
|
||||
DLog.Log($"call SwitchLidarArea({area})");
|
||||
PilotDefinition.Self.AreaChoose = area;
|
||||
}
|
||||
|
||||
public void SwitchIoArea(int area)
|
||||
{
|
||||
if (area != -1)
|
||||
{
|
||||
PilotDefinition.Self.IOObstacleArea = area;
|
||||
}
|
||||
}
|
||||
public void RotateToTarget(float target)
|
||||
{
|
||||
//if (!needrotate) return;
|
||||
var dl = new DriveTask(new MultiWheelRotateInPlace()
|
||||
{
|
||||
AngleTarget = target,
|
||||
PidparamsRead = () => new PIDParams()
|
||||
{
|
||||
Kp = PilotDefinition.Conf.TireFollowingThkp,
|
||||
Ki = PilotDefinition.Conf.TireFollowingThki,
|
||||
Kd = PilotDefinition.Conf.TireFollowingThkd,
|
||||
DeadZone = PilotDefinition.Conf.TireFollowingThDeadZone,
|
||||
SpeedAccPerSec = PilotDefinition.Conf.TireFollowingThSpeedAccPerSec,
|
||||
OutputUpperThreshold = PilotDefinition.Conf.TireFollowingThThresh,
|
||||
MaxI = PilotDefinition.Conf.TireFollowingThMaxI,
|
||||
}
|
||||
}.Get());
|
||||
dl.Wait();
|
||||
}
|
||||
|
||||
//参数1：tireNum 需要钻过的轮胎对数量
|
||||
//参数2：frontLidarDetect true:前雷达识别 false:后雷达识别
|
||||
public void TireFollowing(int tireNum, bool frontLidarDetect, int srcId, int dstId)
|
||||
{
|
||||
while (!TryLock(dstId))
|
||||
{
|
||||
Thread.Sleep(50);
|
||||
}
|
||||
DLog.Log($"锁点{dstId}完成", "TireFollowing");
|
||||
var lidarName = frontLidarDetect ? "前雷达" : "后雷达";
|
||||
DLog.Log($"开始钻车动作，通过{lidarName}识别结果钻{tireNum}对轮胎", "TireFollowing");
|
||||
if (tireNum != 1 && tireNum != 2)
|
||||
{
|
||||
DLog.Log($"TireNum必须是1或2 (当前输入:{tireNum})", "TireFollowing");
|
||||
return;
|
||||
}
|
||||
|
||||
if (PilotDefinition.Self.GhostMode)
|
||||
{
|
||||
while (!TryLock(dstId))
|
||||
{
|
||||
Console.WriteLine("等待锁取货点中...");
|
||||
Thread.Sleep(200);
|
||||
}
|
||||
Console.WriteLine($"锁点{dstId}完成");
|
||||
Thread.Sleep(1000);
|
||||
Console.WriteLine($"开始钻车动作，通过{lidarName}识别结果钻{tireNum}对轮胎");
|
||||
Thread.Sleep(1000);
|
||||
Leave(srcId);
|
||||
Console.WriteLine($"开始第一段盲走，此时释放预取货点{srcId}");
|
||||
Thread.Sleep(2000);
|
||||
//Leave(dstId);
|
||||
//Console.WriteLine($"结束第一段盲走，此时释放取货点{dstId}");
|
||||
Thread.Sleep(2000);
|
||||
Console.WriteLine($"结束钻车动作");
|
||||
return;
|
||||
}
|
||||
|
||||
var detectors = new List<TireFollowing.DetectorDefinition>()
|
||||
{
|
||||
new TireFollowing.DetectorDefinition()
|
||||
{
|
||||
DetectFunction = (_, lastDetectX, filters) => TireDetect.Detect(lastDetectX, filters, frontLidarDetect),
|
||||
StartGuessingX = frontLidarDetect ? PilotDefinition.Conf.TireFollowingStage1GuessX : -PilotDefinition.Conf.TireFollowingStage1GuessX,
|
||||
StartGuessingY = 0,
|
||||
SwitchWalkBlindCondition = rd => rd <= PilotDefinition.Conf.TireFollowingWalkBlindSwitchingDistance,
|
||||
FinishWalkBlindCondition = rd => rd <= PilotDefinition.Conf.TireFollowingWalkBlindFinishDistance,
|
||||
PathTransformation = new Tuple<float, float, float>(
|
||||
frontLidarDetect ? PilotDefinition.Conf.TireFollowingFrontLidarPathTransformationX : PilotDefinition.Conf.TireFollowingBackLidarPathTransformationX,
|
||||
frontLidarDetect ? PilotDefinition.Conf.TireFollowingFrontLidarPathTransformationY : PilotDefinition.Conf.TireFollowingBackLidarPathTransformationY,
|
||||
0),
|
||||
LeaveSrcFunction = Leave,
|
||||
SrcId = srcId,
|
||||
DstId = dstId,
|
||||
},
|
||||
new TireFollowing.DetectorDefinition()
|
||||
{
|
||||
DetectFunction = (_, lastDetectX, filters) => TireDetect.Detect(lastDetectX, filters, frontLidarDetect),
|
||||
StartGuessingX = frontLidarDetect ? PilotDefinition.Conf.TireFollowingStage2GuessX : -PilotDefinition.Conf.TireFollowingStage2GuessX,
|
||||
StartGuessingY = 0,
|
||||
SwitchWalkBlindCondition = rd => rd <= PilotDefinition.Conf.TireFollowingWalkBlindSwitchingDistance,
|
||||
FinishWalkBlindCondition = rd => rd <= PilotDefinition.Conf.TireFollowingWalkBlindFinishDistance,
|
||||
PathTransformation = new Tuple<float, float, float>(
|
||||
frontLidarDetect ? PilotDefinition.Conf.TireFollowingFrontLidarPathTransformationX : PilotDefinition.Conf.TireFollowingBackLidarPathTransformationX,
|
||||
frontLidarDetect ? PilotDefinition.Conf.TireFollowingFrontLidarPathTransformationY : PilotDefinition.Conf.TireFollowingBackLidarPathTransformationY,
|
||||
0)
|
||||
},
|
||||
};
|
||||
DLog.Log($"钻胎为{tireNum}", "TireFollowing");
|
||||
var following = new TireFollowing()
|
||||
{
|
||||
GetController = () => new ChassisController().Get(),
|
||||
GuessRangeX = PilotDefinition.Conf.TireFilterLength / 2,
|
||||
GuessRangeY = PilotDefinition.Conf.TireFilterWidth / 2,
|
||||
detectors = detectors,
|
||||
SlowDistance = PilotDefinition.Conf.TireFollowingSlowDistance,
|
||||
MaxSpeed = PilotDefinition.Conf.TireFollowingMaxSpeed,
|
||||
TireNum = tireNum,
|
||||
CarDirection = frontLidarDetect ? 0f : 180f,
|
||||
WalkBlindTh = frontLidarDetect ? PilotDefinition.Conf.TireFollowingFrontLidarWalkBlindTh : PilotDefinition.Conf.TireFollowingBackLidarWalkBlindTh,
|
||||
};
|
||||
var _dt = new DriveTask(following.Get());
|
||||
_dt.Wait();
|
||||
DLog.Log("钻车动作结束", "TireFollowing");
|
||||
}
|
||||
|
||||
//离车一定是后雷达识别一个轮胎
|
||||
public void LeaveCar(int srcId, float srcX, float srcY, int dstId, float dstX, float dstY)
|
||||
{
|
||||
while (!TryLock(dstId))
|
||||
{
|
||||
Thread.Sleep(50);
|
||||
}
|
||||
DLog.Log($"锁点{dstId}完成", "TireFollowing");
|
||||
DLog.Log($"开始钻车动作，通过后雷达识别结果钻1对轮胎", "TireFollowing");
|
||||
var chassis = (MultiWheelChassis)PilotDefinition.Chassis;
|
||||
chassis.SetOriginBias(0, 0, 0);
|
||||
var following = new TireFollowing()
|
||||
{
|
||||
GetController = () => new ChassisController().Get(),
|
||||
GuessRangeX = PilotDefinition.Conf.TireFilterLength / 2,
|
||||
GuessRangeY = PilotDefinition.Conf.TireFilterWidth / 2,
|
||||
detectors = new List<TireFollowing.DetectorDefinition>()
|
||||
{
|
||||
new TireFollowing.DetectorDefinition()
|
||||
{
|
||||
DetectFunction = (_, lastDetectX, filters) => TireDetect.Detect(lastDetectX, filters, false),
|
||||
StartGuessingX = -PilotDefinition.Conf.TireFollowingStage2GuessX,
|
||||
StartGuessingY = 0,
|
||||
SwitchWalkBlindCondition = rd => rd <= PilotDefinition.Conf.TireFollowingLeaveCarWalkBlindSwitchingDistance,
|
||||
FinishWalkBlindCondition = rd => rd <= PilotDefinition.Conf.TireFollowingWalkBlindFinishDistance,
|
||||
PathTransformation = new Tuple<float, float, float>(
|
||||
PilotDefinition.Conf.TireFollowingLeaveCarBackLidarPathTransformationX,
|
||||
PilotDefinition.Conf.TireFollowingBackLidarPathTransformationY,
|
||||
0),
|
||||
LeaveSrcFunction = Leave,
|
||||
SrcId = srcId,
|
||||
DstId = dstId,
|
||||
},
|
||||
},
|
||||
CarDirection = 180f,
|
||||
SlowDistance = PilotDefinition.Conf.TireFollowingSlowDistance,
|
||||
MaxSpeed = 0.25f,
|
||||
EnableHandover = true,
|
||||
HandoverDistance = 200f,
|
||||
HandoverSpeed = 0.3f,
|
||||
WalkBlindTh = 0,
|
||||
TireNum = 1
|
||||
};
|
||||
|
||||
IEnumerable<bool> LeaveThenFollow()
|
||||
{
|
||||
foreach (var running in following.Get())
|
||||
{
|
||||
if (!running) break;
|
||||
yield return true;
|
||||
}
|
||||
DLog.Log($"释放锁点{srcId}完成", "TireFollowing");
|
||||
DLog.Log("离车TireFollowing结束，开始DstTracker", "TireFollowing");
|
||||
|
||||
foreach (var running in new DstTracker()
|
||||
{
|
||||
Src = new Vector2(srcX, srcY),
|
||||
Dst = new Vector2(dstX, dstY),
|
||||
CarDirectionBias = 180f,
|
||||
InitialSendSpeed = 0.3f
|
||||
}.Get())
|
||||
{
|
||||
if (!running) break;
|
||||
yield return true;
|
||||
}
|
||||
|
||||
yield return false;
|
||||
}
|
||||
|
||||
var _dt = new DriveTask(LeaveThenFollow());
|
||||
_dt.Wait();
|
||||
DLog.Log("离车动作1结束", "TireFollowing");
|
||||
}
|
||||
|
||||
public void LineTracking(int srcId, float srcX, float srcY, int dstId, float dstX, float dstY)
|
||||
{
|
||||
while (!TryLock(dstId))
|
||||
{
|
||||
Thread.Sleep(50);
|
||||
}
|
||||
var chassis = (MultiWheelChassis)PilotDefinition.Chassis;
|
||||
chassis.SetOriginBias(0, 0, 0);
|
||||
DLog.Log($"锁点{dstId}完成", "TireFollowing");
|
||||
IEnumerable<bool> TrackThenFollow()
|
||||
{
|
||||
foreach (var running in new LineTracking()
|
||||
{
|
||||
Target = PilotDefinition.Conf.LineTrackDistance + (PilotDefinition.Self.LFLActualPos + PilotDefinition.Self.LFRActualPos) / 2,
|
||||
LeaveSrcFunction = Leave,
|
||||
SrcId = srcId,
|
||||
EnableHandover = true,
|
||||
HandoverDistance = 200,
|
||||
HandoverSpeed = 0.3f,
|
||||
}.Get())
|
||||
{
|
||||
if (!running) break;
|
||||
yield return true;
|
||||
}
|
||||
DLog.Log($"释放锁点{srcId}完成", "TireFollowing");
|
||||
DLog.Log("离车LineTracking结束，开始DstTracker", "TireFollowing");
|
||||
while (!TryLock(426))
|
||||
{
|
||||
Thread.Sleep(20);
|
||||
}
|
||||
Leave(dstId);
|
||||
DLog.Log($"释放锁点{dstId}完成", "TireFollowing");
|
||||
foreach (var running in new DstTracker()
|
||||
{
|
||||
Src = new Vector2(srcX, srcY),
|
||||
Dst = new Vector2(dstX, dstY),
|
||||
InitialSendSpeed = 0.3f
|
||||
}.Get())
|
||||
{
|
||||
if (!running) break;
|
||||
yield return true;
|
||||
}
|
||||
yield return false;
|
||||
}
|
||||
var _dt = new DriveTask(TrackThenFollow());
|
||||
_dt.Wait();
|
||||
DLog.Log("离车动作2结束", "TireFollowing");
|
||||
}
|
||||
|
||||
//驱动器上使能
|
||||
public void DriverAble()
|
||||
{
|
||||
var dl = new DriveTask(new DriverAble() { }.Get());
|
||||
dl.Wait();
|
||||
DLog.Log("驱动器上使能完成", "TireFollowing");
|
||||
}
|
||||
|
||||
//驱动器下使能
|
||||
public void DriverDisable()
|
||||
{
|
||||
var dl = new DriveTask(new DriverDisable() { }.Get());
|
||||
dl.Wait();
|
||||
DLog.Log("驱动器下使能完成", "TireFollowing");
|
||||
}
|
||||
|
||||
// 夹抱：close 为 true 时关闭夹抱，否则打开夹抱。
|
||||
public void ClamptoTarget(bool close)
|
||||
{
|
||||
if (PilotDefinition.Self.GhostMode)
|
||||
{
|
||||
Thread.Sleep(2000);
|
||||
Console.WriteLine("夹抱完成");
|
||||
return;
|
||||
}
|
||||
new DriveTask(new ClampToTarget()
|
||||
{
|
||||
LeftClampTarget = close ? PilotDefinition.Self.LeftArmUpperPos : PilotDefinition.Self.LeftArmLowerPos,
|
||||
RightClampTarget = close ? PilotDefinition.Self.RightArmUpperPos : PilotDefinition.Self.RightArmLowerPos
|
||||
}.Get()).Wait();
|
||||
}
|
||||
// Fleet crab walk: convert scheduler src/dst into the same relative crab-walk path used by MovementTest.
|
||||
public void FleetCrabWalk(float srcX, float srcY, int srcId, float dstX, float dstY, int dstId,
|
||||
float speed)
|
||||
{
|
||||
var dx = dstX - srcX;
|
||||
var dy = dstY - srcY;
|
||||
var pathLength = (float)Math.Sqrt(dx * dx + dy * dy);
|
||||
|
||||
if (pathLength <= 1f)
|
||||
{
|
||||
DLog.Log("FleetCrabWalk abort: path length is too short.", "FleetCrabDbg");
|
||||
return;
|
||||
}
|
||||
|
||||
var self = PilotDefinition.Self;
|
||||
if (!self.TryGetFleetCenterFromMembers(out var centerX, out var centerY, out var centerTh) &&
|
||||
!self.TryGetFleetCenterFromSlam(out centerX, out centerY, out centerTh))
|
||||
{
|
||||
DLog.Log("FleetCrabWalk abort: failed to read fleet center.", "FleetCrabDbg");
|
||||
Hedingben.ToastText("FleetCrab requires master localization", "FleetCrab");
|
||||
return;
|
||||
}
|
||||
|
||||
var pathAngle = (float)CommonMath.RoundTh((float)(Math.Atan2(dy, dx) / Math.PI * 180.0));
|
||||
var crabAngle = (float)CommonMath.ThDiff(pathAngle, centerTh);
|
||||
var targetBodyWorldHeading = (float)CommonMath.RoundTh(PilotDefinition.Conf.FleetCrabBodyWorldHeadingDeg);
|
||||
var bodyToPathAngle = (float)CommonMath.ThDiff(pathAngle, targetBodyWorldHeading);
|
||||
DLog.Log(
|
||||
$"call FleetCrabWalk(src=({srcX:0},{srcY:0},id:{srcId}), dst=({dstX:0},{dstY:0},id:{dstId}), " +
|
||||
$"len={pathLength:0.0}, speed={speed:0.000}, pathAngle={pathAngle:0.0}, " +
|
||||
$"center=({centerX:0},{centerY:0},{centerTh:0.0}), crabAngle={crabAngle:0.0}, " +
|
||||
$"targetBodyWorld={targetBodyWorldHeading:0.0}, bodyToPath={bodyToPathAngle:0.0})",
|
||||
"FleetCrabDbg");
|
||||
|
||||
if (dstId != -1)
|
||||
{
|
||||
while (!TryLock(dstId))
|
||||
{
|
||||
Thread.Sleep(50);
|
||||
}
|
||||
DLog.Log($"锁点{dstId}完成", "FleetCrabDbg");
|
||||
}
|
||||
|
||||
var action = new MultiWheelC.FleetCrabWalk
|
||||
{
|
||||
CrabAngleDeg = crabAngle,
|
||||
BodyToPathAngleDeg = bodyToPathAngle,
|
||||
CrabLengthMm = pathLength,
|
||||
CrabSpeed = speed,
|
||||
FleetCrabAccel = PilotDefinition.Conf.FleetCrabAccel,
|
||||
FleetCrabStartAccel = PilotDefinition.Conf.FleetCrabStartAccel,
|
||||
FleetCrabSlowDistance = PilotDefinition.Conf.FleetCrabSlowDistance,
|
||||
FleetCrabFinishDistance = PilotDefinition.Conf.FleetCrabFinishDistance,
|
||||
FleetCrabFinishSpeed = PilotDefinition.Conf.FleetCrabFinishSpeed,
|
||||
FleetCrabSlowingPow = PilotDefinition.Conf.FleetCrabSlowingPow,
|
||||
GcpThetaThreshold = PilotDefinition.Conf.FleetCrabGcpThetaThreshold
|
||||
};
|
||||
|
||||
try
|
||||
{
|
||||
new DriveTask(action.Get()).Wait();
|
||||
}
|
||||
finally
|
||||
{
|
||||
if (srcId != -1)
|
||||
{
|
||||
Leave(srcId);
|
||||
DLog.Log($"释放放车点{srcId}", "FleetCrabDbg");
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
public void FleetCurveWalk(float srcX, float srcY, int srcId, float dstX, float dstY, int dstId,
|
||||
float speed, params float[] trackTypeInfo)
|
||||
{
|
||||
if (trackTypeInfo == null || trackTypeInfo.Length < 2)
|
||||
{
|
||||
DLog.Log("FleetCurveWalk abort: invalid trackTypeInfo, expected Bezier type info.", "FleetCurveDbg");
|
||||
Hedingben.ToastText("FleetCurve invalid trackTypeInfo", "FleetCurve");
|
||||
return;
|
||||
}
|
||||
|
||||
var trackType = (int)trackTypeInfo[0];
|
||||
if (trackType != 2)
|
||||
{
|
||||
DLog.Log($"FleetCurveWalk abort: unsupported trackType={trackType}, only Bezier(type=2) is supported.",
|
||||
"FleetCurveDbg");
|
||||
Hedingben.ToastText("FleetCurve only supports Bezier trackType=2", "FleetCurve");
|
||||
return;
|
||||
}
|
||||
|
||||
var controlPointNum = (int)trackTypeInfo[1];
|
||||
var expectedLength = 2 + controlPointNum * 2;
|
||||
if (controlPointNum < 3 || trackTypeInfo.Length < expectedLength)
|
||||
{
|
||||
DLog.Log(
|
||||
$"FleetCurveWalk abort: invalid Bezier trackTypeInfo. controlPointNum={controlPointNum}, " +
|
||||
$"length={trackTypeInfo.Length}, expected>={expectedLength}.",
|
||||
"FleetCurveDbg");
|
||||
Hedingben.ToastText("FleetCurve invalid Bezier trackTypeInfo", "FleetCurve");
|
||||
return;
|
||||
}
|
||||
|
||||
BezierTrack track;
|
||||
try
|
||||
{
|
||||
track = ProcessTrackTypeInfo(srcX, srcY, dstX, dstY, trackTypeInfo) as BezierTrack;
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
DLog.Log($"FleetCurveWalk abort: failed to process trackTypeInfo. {ex.Message}", "FleetCurveDbg");
|
||||
Hedingben.ToastText("FleetCurve failed to process track", "FleetCurve");
|
||||
return;
|
||||
}
|
||||
|
||||
if (track == null)
|
||||
{
|
||||
DLog.Log("FleetCurveWalk abort: ProcessTrackTypeInfo did not return BezierTrack.", "FleetCurveDbg");
|
||||
Hedingben.ToastText("FleetCurve requires BezierTrack", "FleetCurve");
|
||||
return;
|
||||
}
|
||||
|
||||
track.Speed = speed;
|
||||
track.CarDirectionBias = 0f;
|
||||
|
||||
DLog.Log(
|
||||
$"call FleetCurveWalk(src=({srcX:0},{srcY:0},id:{srcId}), dst=({dstX:0},{dstY:0},id:{dstId}), " +
|
||||
$"speed={speed:0.000}, trackType={trackType}, controls={controlPointNum}, track={track.GetType().Name}, " +
|
||||
$"carDirectionBias=0.0)",
|
||||
"FleetCurveDbg");
|
||||
|
||||
if (dstId != -1)
|
||||
{
|
||||
while (!TryLock(dstId))
|
||||
{
|
||||
Thread.Sleep(50);
|
||||
}
|
||||
DLog.Log($"閿佺偣{dstId}瀹屾垚", "FleetCurveDbg");
|
||||
}
|
||||
|
||||
var action = new MultiWheelC.FleetCurveWalk
|
||||
{
|
||||
Track = track,
|
||||
CurveSpeed = speed,
|
||||
CarDirectionBias = 0f,
|
||||
SlowDistance = PilotDefinition.Conf.FleetCurveSlowDistance,
|
||||
FinishDistance = PilotDefinition.Conf.FleetCurveFinishDistance,
|
||||
FinishSpeed = PilotDefinition.Conf.FleetCurveFinishSpeed,
|
||||
SlowingPow = PilotDefinition.Conf.FleetCurveSlowingPow,
|
||||
GcpThetaThreshold = PilotDefinition.Conf.FleetCrabGcpThetaThreshold,
|
||||
StartSyncTimeoutSec = PilotDefinition.Conf.FleetCrabStartSyncTimeoutSec
|
||||
};
|
||||
|
||||
try
|
||||
{
|
||||
new DriveTask(action.Get()).Wait();
|
||||
}
|
||||
finally
|
||||
{
|
||||
if (srcId != -1)
|
||||
{
|
||||
Leave(srcId);
|
||||
DLog.Log($"release srcId={srcId}", "FleetCurveDbg");
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
public void ChangeAvoidanceDistance(float stopDistance, float slowDistance)
|
||||
{
|
||||
DLog.Log($"call ChangeAvoidanceDistance({stopDistance},{slowDistance})");
|
||||
PilotDefinition.Self.SlowDistance = slowDistance;
|
||||
PilotDefinition.Self.StopDistance = stopDistance;
|
||||
}
|
||||
public void ChangeAvoidanceParam(float length = -1, float width = -1)
|
||||
{
|
||||
|
||||
PilotDefinition.Self.CarLength = length;
|
||||
PilotDefinition.Self.CarWidth = width;
|
||||
|
||||
}
|
||||
public void SetLocation(float x, float y, float th)
|
||||
{
|
||||
DLog.Log($"call SetLocation({x},{y},{th})");
|
||||
Console.WriteLine($"call SetLocation({x},{y},{th})");
|
||||
Queue(() =>
|
||||
{
|
||||
while (true)
|
||||
{
|
||||
var str1 = new HttpClient()
|
||||
.GetStringAsync(
|
||||
$"http://127.0.0.1:4321/setLocation?x={x}&y={y}&th={th}")
|
||||
.Result;
|
||||
Thread.Sleep(500);
|
||||
Console.WriteLine($"SetLocation str={str1}");
|
||||
var setLocationRes = JsonConvert.DeserializeObject<SetLocationRes>(str1);
|
||||
Console.WriteLine(setLocationRes.l_step);
|
||||
if (setLocationRes != null && setLocationRes.l_step == 2) break;
|
||||
}
|
||||
});
|
||||
}
|
||||
public float baseSpeed = 0;
|
||||
}
|
||||
}
|
||||
@@ -1,100 +0,0 @@
|
||||
using System;
|
||||
using System.Numerics;
|
||||
using ClumsyCore;
|
||||
using ClumsyCore.Interfaces;
|
||||
using ClumsyCore.Pilot;
|
||||
using FundamentalLib;
|
||||
using MDCSToolBox.Clumsy.MotionControllers;
|
||||
using MDCSToolBox.Clumsy.Movements;
|
||||
using MDCSToolBox.Clumsy.Pilot;
|
||||
|
||||
namespace MultiWheelC;
|
||||
|
||||
public class ChassisController : MovementDefinition<MultiWheelGeometricController>
|
||||
{
|
||||
public float BaseSpeed = Configuration.conf.basicSpeed;
|
||||
|
||||
private DateTime _sendMotionDbgLast = DateTime.MinValue;
|
||||
|
||||
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,
|
||||
|
||||
MultiVehicleSendMotion = (speed, frontTh, rearTh, idealPos, idealAngle) =>
|
||||
{
|
||||
var self = PilotDefinition.Self;
|
||||
self.MultiVehicleAutoEnabled = true;
|
||||
// A: 用固定锁对象（不再锁会被替换的字段引用）。
|
||||
int fleetCnt;
|
||||
lock (self.FleetLock)
|
||||
fleetCnt = self.MultiVehicleFleet.Count;
|
||||
|
||||
// 诊断（节流 ~300ms）：确认回调被调用、编队是否就绪、是否因数量不符提前 return（导致不下发速度）。
|
||||
if ((DateTime.Now - _sendMotionDbgLast).TotalMilliseconds >= 300)
|
||||
{
|
||||
_sendMotionDbgLast = DateTime.Now;
|
||||
DLog.Log(
|
||||
$"SENDMOTION speed={speed:0.000} fTh={frontTh:0.0} rTh={rearTh:0.0} " +
|
||||
$"ideal=({idealPos.X:0},{idealPos.Y:0},{idealAngle:0.0}) " +
|
||||
$"editCnt={fleetCnt}/{PilotDefinition.Conf.MultiVehicleFleetNum} " +
|
||||
$"earlyReturn={fleetCnt != PilotDefinition.Conf.MultiVehicleFleetNum}",
|
||||
"FleetCrabDbg");
|
||||
}
|
||||
|
||||
if (fleetCnt != PilotDefinition.Conf.MultiVehicleFleetNum)
|
||||
return;
|
||||
|
||||
self.MultiVehicleAutoVx = speed;
|
||||
self.MultiVehicleAutoFrontTh = frontTh;
|
||||
self.MultiVehicleAutoRearTh = rearTh;
|
||||
// D: 透传路径控制器算出的理想车队中心位姿（此前被丢弃），供各车按 layout 做前馈。
|
||||
self.MultiVehicleAutoIdealX = idealPos.X;
|
||||
self.MultiVehicleAutoIdealY = idealPos.Y;
|
||||
self.MultiVehicleAutoIdealTh = idealAngle;
|
||||
self.MultiVehicleAutoHasIdeal = true;
|
||||
// B: 标记命令新鲜度。路径结束/早退/卡顿不再刷新此时刻 → 主车超时后清零速度，避免滑行。
|
||||
self.MultiVehicleAutoCmdTime = DateTime.Now;
|
||||
},
|
||||
|
||||
// G: 读取车队中心原子快照，避免跨线程读到撕裂的 x/y/th 组合。
|
||||
MultiVehicleGetFleetPos = () =>
|
||||
{
|
||||
var snap = PilotDefinition.Self.GetFleetCenterSnapshot();
|
||||
return new Location
|
||||
{
|
||||
x = snap.X,
|
||||
y = snap.Y,
|
||||
th = snap.Th,
|
||||
l_step = 1,
|
||||
tick = DateTime.Now.Ticks
|
||||
};
|
||||
}
|
||||
};
|
||||
}
|
||||
}
|
||||
@@ -1,526 +0,0 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Numerics;
|
||||
using ClumsyCore;
|
||||
using ClumsyCore.Interfaces;
|
||||
using ClumsyCore.Pilot;
|
||||
using FundamentalLib;
|
||||
using CommonUsage.Chassis;
|
||||
using CommonUsage.Mathematics;
|
||||
using MDCSToolBox.Clumsy.Movements;
|
||||
using MDCSToolBox.Clumsy.Pilot;
|
||||
|
||||
namespace MultiWheelC;
|
||||
|
||||
// ===== 车队联动-自动蟹行动作 =====
|
||||
// 以当前车队中心为起点，构造指定方向和长度的直线路径；
|
||||
// 执行侧直接写 MultiVehicleAuto...，由 TickMultiVehicle 自动分支统一下发。
|
||||
//
|
||||
// 控制思路参考 MDCSToolbox 几何控制器，但实现收在 MultiWheelC 内：
|
||||
// 1) 读取主车 Detour 反推车队中心，计算沿直线的进度、横向偏差和车身目标朝向偏差；
|
||||
// 2) 根据横向偏差给前后 GCP 同向修正，根据车身目标朝向偏差给前后 GCP 反向修正；
|
||||
// 3) 根据终点距离减速，并发布 ideal fleet center 给从车做前馈。
|
||||
//
|
||||
// 前提：在主车(MultiVehicleMasterEndpoint=="/")运行，且主车有 Detour 定位。
|
||||
public class FleetCrabWalk : MovementDefinition
|
||||
{
|
||||
/// <summary>路径方向相对启动时车队朝向的夹角(deg，逆时针为正)。</summary>
|
||||
public float CrabAngleDeg = 45f;
|
||||
|
||||
/// <summary>路径方向相对车身目标朝向的夹角(deg，逆时针为正)。MovementTest 会设为 CrabAngleDeg，以保持启动时车身朝向。</summary>
|
||||
public float BodyToPathAngleDeg = 45f;
|
||||
|
||||
/// <summary>路径长度(mm)。</summary>
|
||||
public float CrabLengthMm = 2000f;
|
||||
|
||||
/// <summary>行驶速度(m/s)。</summary>
|
||||
public float CrabSpeed = 0.2f;
|
||||
|
||||
/// <summary>速度命令加速度限制(m/s^2)，小于等于 0 表示不限制。</summary>
|
||||
public float FleetCrabAccel = 0.2f;
|
||||
|
||||
/// <summary>预对齐后正式下发速度前 5 秒加速度限制(m/s^2)，小于等于 0 表示不限制。</summary>
|
||||
public float FleetCrabStartAccel = 0.01f;
|
||||
|
||||
/// <summary>末端开始减速距离(mm)。</summary>
|
||||
public float FleetCrabSlowDistance = 2000f;
|
||||
|
||||
/// <summary>完成距离(mm)，低于该剩余距离结束动作。</summary>
|
||||
public float FleetCrabFinishDistance = 20f;
|
||||
|
||||
/// <summary>末端最低速度(m/s)。</summary>
|
||||
public float FleetCrabFinishSpeed = 0.02f;
|
||||
|
||||
/// <summary>末端减速曲线指数。</summary>
|
||||
public float FleetCrabSlowingPow = 0.8f;
|
||||
|
||||
/// <summary>前后 GCP 舵角修正上限(deg)。</summary>
|
||||
public float GcpThetaThreshold = 95f;
|
||||
|
||||
private bool _stopping;
|
||||
|
||||
private void Cleanup()
|
||||
{
|
||||
var self = PilotDefinition.Self;
|
||||
self.MultiVehicleScriptVx = 0;
|
||||
self.MultiVehicleScriptVy = 0;
|
||||
self.MultiVehicleScriptVth = 0;
|
||||
self.MultiVehicleScriptMode = 0;
|
||||
self.MultiVehicleScriptEnabled = false;
|
||||
self.MultiVehicleAutoVx = 0;
|
||||
self.MultiVehicleAutoFrontTh = 0;
|
||||
self.MultiVehicleAutoRearTh = 0;
|
||||
self.MultiVehicleAutoHasIdeal = false;
|
||||
self.MultiVehicleAutoEnabled = false;
|
||||
}
|
||||
|
||||
public void Stop()
|
||||
{
|
||||
_stopping = true;
|
||||
Cleanup();
|
||||
}
|
||||
|
||||
private static float Clamp(float value, float min, float max)
|
||||
{
|
||||
if (value < min) return min;
|
||||
if (value > max) return max;
|
||||
return value;
|
||||
}
|
||||
|
||||
private static float ClampAbs(float value, float limit)
|
||||
{
|
||||
var absLimit = Math.Abs(limit);
|
||||
if (absLimit <= 0) return value;
|
||||
if (value > absLimit) return absLimit;
|
||||
if (value < -absLimit) return -absLimit;
|
||||
return value;
|
||||
}
|
||||
|
||||
private static float Slew(float current, float target, float maxDelta)
|
||||
{
|
||||
if (maxDelta <= 0) return target;
|
||||
if (target > current + maxDelta) return current + maxDelta;
|
||||
if (target < current - maxDelta) return current - maxDelta;
|
||||
return target;
|
||||
}
|
||||
|
||||
private static float AverageAngle(float frontTh, float rearTh)
|
||||
{
|
||||
var diff = (float)CommonMath.ThDiff(frontTh, rearTh);
|
||||
return (float)CommonMath.RoundTh(rearTh + diff / 2f);
|
||||
}
|
||||
|
||||
private static void ResolveCrabDriveEquivalent(float speed, float rawFrontTh, float rawRearTh, float steerLimit,
|
||||
out float driveSpeed, out float frontTh, out float rearTh, out bool reverseEquivalent, out float rawBaseTh)
|
||||
{
|
||||
var limit = Math.Min(179f, Math.Max(1f, Math.Abs(steerLimit)));
|
||||
rawBaseTh = AverageAngle(rawFrontTh, rawRearTh);
|
||||
driveSpeed = speed;
|
||||
frontTh = rawFrontTh;
|
||||
rearTh = rawRearTh;
|
||||
reverseEquivalent = false;
|
||||
|
||||
if (rawBaseTh > limit)
|
||||
{
|
||||
frontTh = (float)CommonMath.RoundTh(frontTh - 180f);
|
||||
rearTh = (float)CommonMath.RoundTh(rearTh - 180f);
|
||||
driveSpeed = -driveSpeed;
|
||||
reverseEquivalent = true;
|
||||
}
|
||||
else if (rawBaseTh < -limit)
|
||||
{
|
||||
frontTh = (float)CommonMath.RoundTh(frontTh + 180f);
|
||||
rearTh = (float)CommonMath.RoundTh(rearTh + 180f);
|
||||
driveSpeed = -driveSpeed;
|
||||
reverseEquivalent = true;
|
||||
}
|
||||
|
||||
frontTh = ClampAbs(frontTh, limit);
|
||||
rearTh = ClampAbs(rearTh, limit);
|
||||
}
|
||||
|
||||
private static float ProbeSpeed(float speed)
|
||||
{
|
||||
return Math.Abs(speed) > 1e-4f ? speed : 1f;
|
||||
}
|
||||
|
||||
private static bool TryGetMotionYawSign(float frontTh, float rearTh, float driveSpeed, float controlRadius,
|
||||
out float yawSign)
|
||||
{
|
||||
yawSign = 0f;
|
||||
if (Math.Abs(CommonMath.ThDiff(frontTh, rearTh)) <= 1e-3f)
|
||||
return false;
|
||||
|
||||
var radius = Math.Max(1f, Math.Abs(controlRadius));
|
||||
Vector2 pFront = new(radius, 0), pRear = new(-radius, 0),
|
||||
normFront = CommonMath.Transform2D(pFront, frontTh + 90f, Vector2.UnitX),
|
||||
normRear = CommonMath.Transform2D(pRear, rearTh + 90f, Vector2.UnitX);
|
||||
var (intersect, center) = CommonMath.TwoLinesIntersection(pFront, normFront, pRear, normRear);
|
||||
if (!intersect)
|
||||
return false;
|
||||
|
||||
// Match MultiWheelChassis.SendMotion: the tangent side is selected by
|
||||
// rotCenter.Y > 1, and reverse-equivalent motion flips the yaw direction.
|
||||
var tangentSign = center.Y > 1f ? 1f : -1f;
|
||||
var speedSign = driveSpeed >= 0f ? 1f : -1f;
|
||||
yawSign = speedSign * tangentSign;
|
||||
return true;
|
||||
}
|
||||
|
||||
private static float GetYawSplitSign(float baseTh, float speed, float steerLimit, float controlRadius)
|
||||
{
|
||||
const float probeDth = 1f;
|
||||
ResolveCrabDriveEquivalent(ProbeSpeed(speed), baseTh + probeDth, baseTh - probeDth, steerLimit,
|
||||
out var probeSpeed, out var probeFrontTh, out var probeRearTh, out _, out _);
|
||||
return TryGetMotionYawSign(probeFrontTh, probeRearTh, probeSpeed, controlRadius, out var yawSign)
|
||||
? yawSign
|
||||
: 1f;
|
||||
}
|
||||
|
||||
private static float EstimateLateralVelocity(float bodyTh, float frontTh, float rearTh, float driveSpeed,
|
||||
Vector2 pathLeft)
|
||||
{
|
||||
var motionTh = (float)CommonMath.RoundTh(bodyTh + AverageAngle(frontTh, rearTh));
|
||||
var rad = motionTh / 180f * Math.PI;
|
||||
var dir = new Vector2((float)Math.Cos(rad), (float)Math.Sin(rad));
|
||||
if (driveSpeed < 0f)
|
||||
dir = -dir;
|
||||
return Vector2.Dot(dir, pathLeft);
|
||||
}
|
||||
|
||||
private static float ScoreBiasSign(float baseTh, float bodyTh, float speed, float steerLimit, Vector2 pathLeft,
|
||||
float lateral, float biasProbe)
|
||||
{
|
||||
ResolveCrabDriveEquivalent(ProbeSpeed(speed), baseTh + biasProbe, baseTh + biasProbe, steerLimit,
|
||||
out var probeSpeed, out var probeFrontTh, out var probeRearTh, out _, out _);
|
||||
var lateralVelocity = EstimateLateralVelocity(bodyTh, probeFrontTh, probeRearTh, probeSpeed, pathLeft);
|
||||
return -Math.Sign(lateral) * lateralVelocity;
|
||||
}
|
||||
|
||||
private static float GetLateralBiasSign(float baseTh, float bodyTh, float speed, float steerLimit, Vector2 pathLeft,
|
||||
float lateral)
|
||||
{
|
||||
if (Math.Abs(lateral) <= 1e-3f)
|
||||
return 1f;
|
||||
|
||||
const float probeBias = 1f;
|
||||
var positiveScore = ScoreBiasSign(baseTh, bodyTh, speed, steerLimit, pathLeft, lateral, probeBias);
|
||||
var negativeScore = ScoreBiasSign(baseTh, bodyTh, speed, steerLimit, pathLeft, lateral, -probeBias);
|
||||
return positiveScore >= negativeScore ? 1f : -1f;
|
||||
}
|
||||
|
||||
private static bool TryGetControlFleetCenter(PilotDefinition self, out float centerX, out float centerY,
|
||||
out float centerTh, out string source)
|
||||
{
|
||||
if (self.TryGetFleetCenterFromMembers(out centerX, out centerY, out centerTh))
|
||||
{
|
||||
source = "fleet";
|
||||
return true;
|
||||
}
|
||||
|
||||
if (self.TryGetFleetCenterFromSlam(out centerX, out centerY, out centerTh))
|
||||
{
|
||||
source = "slam";
|
||||
return true;
|
||||
}
|
||||
|
||||
source = "none";
|
||||
return false;
|
||||
}
|
||||
|
||||
public override IEnumerable<bool> Get()
|
||||
{
|
||||
var self = PilotDefinition.Self;
|
||||
var conf = PilotDefinition.Conf;
|
||||
var chassis = BasicPilotBase.Chassis as MultiWheelChassis;
|
||||
if (chassis == null)
|
||||
{
|
||||
DLog.Log("ABORT: FleetCrabWalk requires MultiWheelChassis.", "FleetCrabDbg");
|
||||
yield break;
|
||||
}
|
||||
_stopping = false;
|
||||
|
||||
DLog.Log(
|
||||
$"ENTER master?={conf.MultiVehicleMasterEndpoint == "/"} endpoint={conf.MultiVehicleMasterEndpoint} " +
|
||||
$"fleetNum={conf.MultiVehicleFleetNum} useDetect={conf.MultiVehicleUseDetect} " +
|
||||
$"syncUseDetour={conf.MultiVehicleSyncUseDetour} useIdealCenter={conf.MultiVehicleAutoUseIdealCenter} " +
|
||||
$"autoFields=true pathMode=relative pathAngle={CrabAngleDeg:0.0} " +
|
||||
$"bodyToPath={BodyToPathAngleDeg:0.0} gcpLimit={GcpThetaThreshold:0.0} " +
|
||||
$"biasFac={conf.BiasFac:0.00} fleetCrabDthFac={conf.FleetCrabDthLinearFac:0.00}",
|
||||
"FleetCrabDbg");
|
||||
|
||||
if (conf.MultiVehicleMasterEndpoint != "/")
|
||||
{
|
||||
DLog.Log($"ABORT: 非主车 (endpoint={conf.MultiVehicleMasterEndpoint})", "FleetCrabDbg");
|
||||
Hedingben.ToastText("车队蟹行需在主车(主车端点=\"/\")运行", "FleetCrab");
|
||||
yield break;
|
||||
}
|
||||
|
||||
// 注意：getCartLocation() 在无有效 Detour 定位时会阻塞——若卡在这里且后面看不到 CENTER 日志，即定位未就绪。
|
||||
DLog.Log("主车校验通过，开始读取车队中心 (getCartLocation 无定位会阻塞)…", "FleetCrabDbg");
|
||||
if (!TryGetControlFleetCenter(self, out var x0, out var y0, out var theta, out var initialCenterSource))
|
||||
{
|
||||
DLog.Log("ABORT: TryGetFleetCenterFromSlam 返回 false (无定位)", "FleetCrabDbg");
|
||||
Hedingben.ToastText("车队蟹行需要主车 Detour 定位", "FleetCrab");
|
||||
yield break;
|
||||
}
|
||||
DLog.Log($"CENTER 车队中心=({x0:0},{y0:0},{theta:0.0})", "FleetCrabDbg");
|
||||
|
||||
DLog.Log($"CENTER_SOURCE source={initialCenterSource} center=({x0:0},{y0:0},{theta:0.0})", "FleetCrabDbg");
|
||||
|
||||
var pathStart = new Vector2(x0, y0);
|
||||
var pathLengthMm = CrabLengthMm;
|
||||
var phi = CommonMath.RoundTh(theta + CrabAngleDeg);
|
||||
var dst = CommonMath.Transform2D(pathStart, phi, new Vector2(pathLengthMm, 0));
|
||||
var targetBodyTh = CommonMath.RoundTh(phi - BodyToPathAngleDeg);
|
||||
var phiRad = phi / 180.0 * Math.PI;
|
||||
var pathDir = new Vector2((float)Math.Cos(phiRad), (float)Math.Sin(phiRad));
|
||||
var pathLeft = new Vector2(-pathDir.Y, pathDir.X);
|
||||
|
||||
DLog.Log(
|
||||
$"START center=({x0:0},{y0:0},{theta:0.0}) pathMode=relative " +
|
||||
$"src=({pathStart.X:0},{pathStart.Y:0}) pathAngle={CrabAngleDeg:0.0} bodyToPath={BodyToPathAngleDeg:0.0} " +
|
||||
$"phi={phi:0.0} targetBody={targetBodyTh:0.0} " +
|
||||
$"len={pathLengthMm:0} dst=({dst.X:0},{dst.Y:0}) speed={CrabSpeed:0.000} startAccel={FleetCrabStartAccel:0.000} accel={FleetCrabAccel:0.000} " +
|
||||
$"slow={FleetCrabSlowDistance:0} finishDist={FleetCrabFinishDistance:0} " +
|
||||
$"finishSpeed={FleetCrabFinishSpeed:0.000} slowingPow={FleetCrabSlowingPow:0.00}",
|
||||
"FleetCrabDbg");
|
||||
|
||||
var gcpLimit = Math.Max(1f, Math.Abs(GcpThetaThreshold));
|
||||
var controlRadius = Math.Max(1f, Math.Abs(conf.TestCarSyncDistance) / 2f);
|
||||
ResolveCrabDriveEquivalent(0f, (float)CommonMath.ThDiff(phi, theta),
|
||||
(float)CommonMath.ThDiff(phi, theta), gcpLimit, out _, out var holdFrontTh, out var holdRearTh,
|
||||
out _, out _);
|
||||
var warmStart = DateTime.Now;
|
||||
var warmSeqBaseline = self.BeginFleetMotionWarmup();
|
||||
|
||||
self.MultiVehicleScriptEnabled = false;
|
||||
self.MultiVehicleScriptMode = 0;
|
||||
self.MultiVehicleScriptVx = 0;
|
||||
self.MultiVehicleScriptVy = 0;
|
||||
self.MultiVehicleScriptVth = 0;
|
||||
self.MultiVehicleAutoEnabled = true;
|
||||
self.MultiVehicleAutoVx = 0;
|
||||
self.MultiVehicleAutoFrontTh = holdFrontTh;
|
||||
self.MultiVehicleAutoRearTh = holdRearTh;
|
||||
self.MultiVehicleAutoIdealX = pathStart.X;
|
||||
self.MultiVehicleAutoIdealY = pathStart.Y;
|
||||
self.MultiVehicleAutoIdealTh = targetBodyTh;
|
||||
self.MultiVehicleAutoHasIdeal = true;
|
||||
self.MultiVehicleAutoCmdTime = DateTime.Now;
|
||||
self.PrimeMasterAutoFromSlam();
|
||||
DLog.Log(
|
||||
$"WARMUP auto fields enabled, waiting for fleet startup sync seqBase={warmSeqBaseline} " +
|
||||
$"hold=({holdFrontTh:0.00},{holdRearTh:0.00})",
|
||||
"FleetCrabDbg");
|
||||
|
||||
var warmEnd = warmStart.AddSeconds(Math.Max(1.0f, conf.FleetCrabStartSyncTimeoutSec));
|
||||
var warmIter = 0;
|
||||
var warmReady = false;
|
||||
var warmDetail = "";
|
||||
while (!_stopping && DateTime.Now < warmEnd)
|
||||
{
|
||||
warmIter++;
|
||||
self.MultiVehicleScriptEnabled = false;
|
||||
self.MultiVehicleScriptMode = 0;
|
||||
self.MultiVehicleAutoEnabled = true;
|
||||
self.MultiVehicleAutoVx = 0;
|
||||
self.MultiVehicleAutoFrontTh = holdFrontTh;
|
||||
self.MultiVehicleAutoRearTh = holdRearTh;
|
||||
self.MultiVehicleAutoIdealX = pathStart.X;
|
||||
self.MultiVehicleAutoIdealY = pathStart.Y;
|
||||
self.MultiVehicleAutoIdealTh = targetBodyTh;
|
||||
self.MultiVehicleAutoHasIdeal = true;
|
||||
self.MultiVehicleAutoCmdTime = DateTime.Now;
|
||||
self.PrimeMasterAutoFromSlam();
|
||||
var snap = self.GetFleetCenterSnapshot();
|
||||
int cnt;
|
||||
lock (self.FleetLock) cnt = self.MultiVehicleFleet.Count;
|
||||
if (warmIter % 5 == 0)
|
||||
DLog.Log(
|
||||
$"WARMUP#{warmIter} 快照=({snap.X:0},{snap.Y:0},{snap.Th:0.0}) tick={snap.Tick} " +
|
||||
$"autoEn={self.MultiVehicleAutoEnabled} scriptEn={self.MultiVehicleScriptEnabled} cnt={cnt}/{conf.MultiVehicleFleetNum} " +
|
||||
$"detail={warmDetail}",
|
||||
"FleetCrabDbg");
|
||||
if (self.IsFleetMotionWarmupReady(warmStart, warmSeqBaseline,
|
||||
conf.TestCarSyncTh, conf.TestCarSyncDistance, out warmDetail))
|
||||
{
|
||||
warmReady = true;
|
||||
DLog.Log(
|
||||
$"WARMUP done iter={warmIter} 快照=({snap.X:0},{snap.Y:0},{snap.Th:0.0}) cnt={cnt} detail={warmDetail}",
|
||||
"FleetCrabDbg");
|
||||
break;
|
||||
}
|
||||
yield return true;
|
||||
}
|
||||
if (!warmReady)
|
||||
{
|
||||
DLog.Log($"WARMUP timeout: fleet startup sync failed, abort action. detail={warmDetail}",
|
||||
"FleetCrabDbg");
|
||||
Hedingben.ToastText("车队蟹行启动同步超时，已取消", "FleetCrab");
|
||||
Cleanup();
|
||||
yield break;
|
||||
}
|
||||
|
||||
Hedingben.ToastText($"车队蟹行 路径{phi:0.0}° 车身夹角{BodyToPathAngleDeg:0.0}° 长度{pathLengthMm:0}mm", "FleetCrab");
|
||||
|
||||
if (warmReady && self.TryGetFleetCenterFromMembers(out var warmX, out var warmY, out var warmTh))
|
||||
{
|
||||
x0 = warmX;
|
||||
y0 = warmY;
|
||||
theta = warmTh;
|
||||
pathStart = new Vector2(x0, y0);
|
||||
phi = CommonMath.RoundTh(theta + CrabAngleDeg);
|
||||
dst = CommonMath.Transform2D(pathStart, phi, new Vector2(pathLengthMm, 0));
|
||||
targetBodyTh = CommonMath.RoundTh(phi - BodyToPathAngleDeg);
|
||||
phiRad = phi / 180.0 * Math.PI;
|
||||
pathDir = new Vector2((float)Math.Cos(phiRad), (float)Math.Sin(phiRad));
|
||||
pathLeft = new Vector2(-pathDir.Y, pathDir.X);
|
||||
self.MultiVehicleAutoIdealX = pathStart.X;
|
||||
self.MultiVehicleAutoIdealY = pathStart.Y;
|
||||
self.MultiVehicleAutoIdealTh = targetBodyTh;
|
||||
self.MultiVehicleAutoCmdTime = DateTime.Now;
|
||||
DLog.Log(
|
||||
$"WARMUP_REBASE source=fleet center=({x0:0},{y0:0},{theta:0.0}) phi={phi:0.0} targetBody={targetBodyTh:0.0} dst=({dst.X:0},{dst.Y:0})",
|
||||
"FleetCrabDbg");
|
||||
}
|
||||
|
||||
var iter = 0;
|
||||
var lastLog = DateTime.MinValue;
|
||||
var finishDistance = Math.Max(0f, FleetCrabFinishDistance);
|
||||
var slowDistance = Math.Max(finishDistance + 1f, FleetCrabSlowDistance);
|
||||
var baseSpeed = Math.Abs(CrabSpeed);
|
||||
var finishSpeed = Math.Min(baseSpeed, Math.Abs(FleetCrabFinishSpeed));
|
||||
var slowingPow = Math.Max(0.01f, FleetCrabSlowingPow);
|
||||
var accel = Math.Abs(FleetCrabAccel);
|
||||
var startAccel = Math.Abs(FleetCrabStartAccel);
|
||||
var cmdSpeed = 0f;
|
||||
var lastTick = DateTime.Now;
|
||||
var speedRampStart = DateTime.Now;
|
||||
var stopReason = "done";
|
||||
|
||||
while (!_stopping)
|
||||
{
|
||||
iter++;
|
||||
|
||||
if (!TryGetControlFleetCenter(self, out var cx, out var cy, out var cth, out var centerSource))
|
||||
{
|
||||
stopReason = "fleet center invalid";
|
||||
DLog.Log("ABORT: TryGetControlFleetCenter returned false during auto crab.", "FleetCrabDbg");
|
||||
break;
|
||||
}
|
||||
var delta = new Vector2(cx - pathStart.X, cy - pathStart.Y);
|
||||
var along = Vector2.Dot(delta, pathDir);
|
||||
var lateral = Vector2.Dot(delta, pathLeft);
|
||||
var remain = pathLengthMm - along;
|
||||
if (remain <= finishDistance)
|
||||
break;
|
||||
|
||||
var targetSpeed = baseSpeed;
|
||||
var slowRatio = 1f;
|
||||
if (remain < slowDistance)
|
||||
{
|
||||
slowRatio = (float)Math.Pow(Clamp(Math.Max(0, remain) / slowDistance, 0f, 1f), slowingPow);
|
||||
targetSpeed = slowRatio * (baseSpeed - finishSpeed) + finishSpeed;
|
||||
}
|
||||
var now = DateTime.Now;
|
||||
var dt = Math.Max(0.001f, (float)(now - lastTick).TotalSeconds);
|
||||
lastTick = now;
|
||||
var rampElapsed = (now - speedRampStart).TotalSeconds;
|
||||
var activeAccel = rampElapsed < 5.0 ? startAccel : accel;
|
||||
var speed = activeAccel > 0 ? Slew(cmdSpeed, targetSpeed, activeAccel * dt) : targetSpeed;
|
||||
cmdSpeed = speed;
|
||||
|
||||
var baseCrabTh = (float)CommonMath.ThDiff(phi, cth);
|
||||
var headingErr = (float)CommonMath.ThDiff(targetBodyTh, cth);
|
||||
var headingErrReverse = (float)CommonMath.ThDiff(cth, targetBodyTh);
|
||||
var targetBodyToPath = (float)CommonMath.ThDiff(phi, targetBodyTh);
|
||||
var rawBiasMagnitude = (float)(Math.Atan(conf.BiasFac * Math.Abs(lateral) / 1000f /
|
||||
Math.Max(speed, 0.3f)) / Math.PI * 180.0);
|
||||
var biasSign = GetLateralBiasSign(baseCrabTh, cth, speed, gcpLimit, pathLeft, lateral);
|
||||
var rawBiasItem = rawBiasMagnitude * biasSign;
|
||||
var biasItem = ClampAbs(rawBiasItem, conf.BiasThreshold);
|
||||
var yawSplitSign = GetYawSplitSign(baseCrabTh + biasItem, speed, gcpLimit, controlRadius);
|
||||
var rawDthItem = conf.FleetCrabDthLinearFac * headingErr * yawSplitSign;
|
||||
var dthItem = ClampAbs(rawDthItem, conf.FleetCrabDthLinearThreshold);
|
||||
var rawFrontTh = baseCrabTh + biasItem + dthItem;
|
||||
var rawRearTh = baseCrabTh + biasItem - dthItem;
|
||||
ResolveCrabDriveEquivalent(speed, rawFrontTh, rawRearTh, gcpLimit, out var driveSpeed,
|
||||
out var frontTh, out var rearTh, out var reverseEquivalent, out var rawBaseTh);
|
||||
holdFrontTh = frontTh;
|
||||
holdRearTh = rearTh;
|
||||
var idealAlong = Clamp(along, 0f, pathLengthMm);
|
||||
var ideal = pathStart + pathDir * idealAlong;
|
||||
|
||||
self.MultiVehicleScriptEnabled = false;
|
||||
self.MultiVehicleScriptMode = 0;
|
||||
self.MultiVehicleScriptVx = 0;
|
||||
self.MultiVehicleScriptVy = 0;
|
||||
self.MultiVehicleScriptVth = 0;
|
||||
self.MultiVehicleAutoEnabled = true;
|
||||
self.MultiVehicleAutoVx = driveSpeed;
|
||||
self.MultiVehicleAutoFrontTh = frontTh;
|
||||
self.MultiVehicleAutoRearTh = rearTh;
|
||||
self.MultiVehicleAutoIdealX = ideal.X;
|
||||
self.MultiVehicleAutoIdealY = ideal.Y;
|
||||
self.MultiVehicleAutoIdealTh = targetBodyTh;
|
||||
self.MultiVehicleAutoHasIdeal = true;
|
||||
self.MultiVehicleAutoCmdTime = DateTime.Now;
|
||||
|
||||
if ((DateTime.Now - lastLog).TotalMilliseconds >= 300)
|
||||
{
|
||||
lastLog = DateTime.Now;
|
||||
var snap = self.GetFleetCenterSnapshot();
|
||||
int fleetCnt;
|
||||
lock (self.FleetLock) fleetCnt = self.MultiVehicleFleet.Count;
|
||||
DLog.Log(
|
||||
$"ITER#{iter} centerSrc={centerSource} center=({cx:0},{cy:0},{cth:0.0}) snap=({snap.X:0},{snap.Y:0},{snap.Th:0.0}) " +
|
||||
$"along={along:0} lateral={lateral:0} remain={remain:0} headingErr={headingErr:0.0} " +
|
||||
$"baseTh={baseCrabTh:0.0} bias={biasItem:0.0} dth={dthItem:0.0} " +
|
||||
$"slowRatio={slowRatio:0.000} targetV={targetSpeed:0.000} rampT={rampElapsed:0.0} accel={activeAccel:0.000} auto=(vx:{driveSpeed:0.000},fTh:{frontTh:0.0},rTh:{rearTh:0.0}) " +
|
||||
$"ideal=({ideal.X:0},{ideal.Y:0},{targetBodyTh:0.0}) scriptEn={self.MultiVehicleScriptEnabled} " +
|
||||
$"cnt={fleetCnt}/{conf.MultiVehicleFleetNum}",
|
||||
"FleetCrabDbg");
|
||||
DLog.Log(
|
||||
$"CTRL iter={iter} centerSrc:{centerSource} phi:{phi:0.00} targetBody:{targetBodyTh:0.00} startTheta:{theta:0.00} " +
|
||||
$"cth:{cth:0.00} crabAngle:{CrabAngleDeg:0.00} bodyToPathCfg:{BodyToPathAngleDeg:0.00} " +
|
||||
$"targetBodyToPath:{targetBodyToPath:0.00} bodyToPathNow:{baseCrabTh:0.00} " +
|
||||
$"headingErr(target-current):{headingErr:0.00} reverse(current-target):{headingErrReverse:0.00} yawSign:{yawSplitSign:0} " +
|
||||
$"fleetCrabDthFac:{conf.FleetCrabDthLinearFac:0.000} rawDth:{rawDthItem:0.00} dth:{dthItem:0.00} dthLimit:{conf.FleetCrabDthLinearThreshold:0.00} " +
|
||||
$"lateral:{lateral:0.0} biasFac:{conf.BiasFac:0.000} biasSign:{biasSign:0} rawBias:{rawBiasItem:0.00} bias:{biasItem:0.00} biasLimit:{conf.BiasThreshold:0.00} " +
|
||||
$"baseTh:{baseCrabTh:0.00} rawBase:{rawBaseTh:0.00} rawOut(f:{rawFrontTh:0.00},r:{rawRearTh:0.00}) " +
|
||||
$"out(f:{frontTh:0.00},r:{rearTh:0.00}) gcpLimit:{gcpLimit:0.00} revEq:{reverseEquivalent} " +
|
||||
$"speedRaw:{speed:0.000} speed:{driveSpeed:0.000} rampT:{rampElapsed:0.0} accel:{activeAccel:0.000} along:{along:0.0} remain:{remain:0.0} ideal=({ideal.X:0.0},{ideal.Y:0.0},{targetBodyTh:0.00})",
|
||||
"FleetCrabHeadingDbg");
|
||||
}
|
||||
yield return true;
|
||||
}
|
||||
|
||||
if (_stopping)
|
||||
stopReason = "stop";
|
||||
|
||||
self.MultiVehicleAutoVx = 0;
|
||||
self.MultiVehicleAutoFrontTh = holdFrontTh;
|
||||
self.MultiVehicleAutoRearTh = holdRearTh;
|
||||
self.MultiVehicleAutoCmdTime = DateTime.Now;
|
||||
DLog.Log(
|
||||
$"STOP_HOLD iter={iter} reason={stopReason} hold=(fTh:{holdFrontTh:0.0},rTh:{holdRearTh:0.0}) cmdSpeed={cmdSpeed:0.000}",
|
||||
"FleetCrabDbg");
|
||||
var settleEnd = DateTime.Now.AddMilliseconds(Math.Max(100, conf.MultiVehicleSyncInterval * 3));
|
||||
while (!_stopping && DateTime.Now < settleEnd)
|
||||
{
|
||||
self.MultiVehicleScriptEnabled = false;
|
||||
self.MultiVehicleScriptMode = 0;
|
||||
self.MultiVehicleAutoEnabled = true;
|
||||
self.MultiVehicleAutoVx = 0;
|
||||
self.MultiVehicleAutoFrontTh = holdFrontTh;
|
||||
self.MultiVehicleAutoRearTh = holdRearTh;
|
||||
self.MultiVehicleAutoCmdTime = DateTime.Now;
|
||||
yield return true;
|
||||
}
|
||||
|
||||
Cleanup();
|
||||
Hedingben.ToastText("车队蟹行完成", "FleetCrab");
|
||||
DLog.Log($"DONE iter={iter} reason={stopReason}", "FleetCrabDbg");
|
||||
}
|
||||
}
|
||||
@@ -1,411 +0,0 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Globalization;
|
||||
using System.Numerics;
|
||||
using ClumsyCore;
|
||||
using ClumsyCore.Interfaces;
|
||||
using ClumsyCore.Pilot;
|
||||
using FundamentalLib;
|
||||
using CommonUsage.Chassis;
|
||||
using CommonUsage.Mathematics;
|
||||
using MDCSToolBox.Clumsy.MotionControllers;
|
||||
using MDCSToolBox.Clumsy.Movements;
|
||||
using MDCSToolBox.Clumsy.Pilot;
|
||||
using MDCSToolBox.Clumsy.Tracks;
|
||||
|
||||
namespace MultiWheelC;
|
||||
|
||||
public class FleetCurveWalk : MovementDefinition
|
||||
{
|
||||
public BezierTrack Track;
|
||||
public List<Vector2> ControlPoints = new();
|
||||
public float CurveSpeed = 0.2f;
|
||||
public float CarDirectionBias = 0f;
|
||||
public int BezierResolution = 100;
|
||||
public float SlowDistance = 2000f;
|
||||
public float FinishDistance = 20f;
|
||||
public float FinishSpeed = 0.02f;
|
||||
public float SlowingPow = 0.8f;
|
||||
public float GcpThetaThreshold = 95f;
|
||||
public float StartSyncTimeoutSec = 8f;
|
||||
|
||||
private bool _stopping;
|
||||
private MultiWheelGeometricController _controller;
|
||||
private MultiWheelChassis _chassis;
|
||||
private bool _savedControlPoints;
|
||||
private float _savedControlRadius;
|
||||
private Vector2 _savedGcp0;
|
||||
private Vector2 _savedGcp1;
|
||||
|
||||
public void Stop()
|
||||
{
|
||||
_stopping = true;
|
||||
if (_controller != null)
|
||||
_controller.BreakAndHold = true;
|
||||
Cleanup();
|
||||
}
|
||||
|
||||
public static bool TryParsePointList(string text, out List<Vector2> points, out string error)
|
||||
{
|
||||
points = new List<Vector2>();
|
||||
error = "";
|
||||
if (string.IsNullOrWhiteSpace(text))
|
||||
{
|
||||
error = "empty control point list";
|
||||
return false;
|
||||
}
|
||||
|
||||
var segments = text.Split(new[] { ';', '|' }, StringSplitOptions.RemoveEmptyEntries);
|
||||
for (var i = 0; i < segments.Length; i++)
|
||||
{
|
||||
var pair = segments[i].Split(new[] { ',', ' ', '\t' }, StringSplitOptions.RemoveEmptyEntries);
|
||||
if (pair.Length != 2)
|
||||
{
|
||||
error = $"invalid point #{i + 1}: {segments[i]}";
|
||||
return false;
|
||||
}
|
||||
|
||||
if (!TryParseFloat(pair[0], out var x) || !TryParseFloat(pair[1], out var y))
|
||||
{
|
||||
error = $"invalid number in point #{i + 1}: {segments[i]}";
|
||||
return false;
|
||||
}
|
||||
points.Add(new Vector2(x, y));
|
||||
}
|
||||
|
||||
if (points.Count < 3)
|
||||
{
|
||||
error = "Bezier curve requires at least 3 control points";
|
||||
return false;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
public static List<Vector2> BuildRelativeControlPoints(Vector2 start, float startTh, List<Vector2> relativePoints)
|
||||
{
|
||||
var source = relativePoints ?? new List<Vector2>();
|
||||
var normalized = new List<Vector2>();
|
||||
if (source.Count == 0 || Vector2.Distance(source[0], Vector2.Zero) > 1f)
|
||||
normalized.Add(Vector2.Zero);
|
||||
for (var i = 0; i < source.Count; i++)
|
||||
normalized.Add(source[i]);
|
||||
if (normalized.Count < 2)
|
||||
normalized.Add(new Vector2(1000f, 0f));
|
||||
if (normalized.Count < 3)
|
||||
normalized.Add(new Vector2(2000f, 0f));
|
||||
|
||||
var result = new List<Vector2>();
|
||||
for (var i = 0; i < normalized.Count; i++)
|
||||
result.Add(CommonMath.Transform2D(start, startTh, normalized[i]));
|
||||
return result;
|
||||
}
|
||||
|
||||
public static List<Vector2> BuildAgvControlPoints(float srcX, float srcY, float dstX, float dstY,
|
||||
params float[] controlPointCoords)
|
||||
{
|
||||
var src = new Vector2(srcX, srcY);
|
||||
var dst = new Vector2(dstX, dstY);
|
||||
var result = new List<Vector2>();
|
||||
if (controlPointCoords == null || controlPointCoords.Length == 0)
|
||||
{
|
||||
result.Add(src);
|
||||
result.Add((src + dst) / 2f);
|
||||
result.Add(dst);
|
||||
return result;
|
||||
}
|
||||
|
||||
if (controlPointCoords.Length % 2 != 0)
|
||||
throw new ArgumentException("FleetCurve controlPointCoords must contain x,y pairs.");
|
||||
|
||||
var supplied = new List<Vector2>();
|
||||
for (var i = 0; i < controlPointCoords.Length; i += 2)
|
||||
supplied.Add(new Vector2(controlPointCoords[i], controlPointCoords[i + 1]));
|
||||
|
||||
if (supplied.Count >= 3 &&
|
||||
Vector2.Distance(supplied[0], src) <= 10f &&
|
||||
Vector2.Distance(supplied[supplied.Count - 1], dst) <= 10f)
|
||||
return supplied;
|
||||
|
||||
result.Add(src);
|
||||
for (var i = 0; i < supplied.Count; i++)
|
||||
result.Add(supplied[i]);
|
||||
result.Add(dst);
|
||||
if (result.Count < 3)
|
||||
result.Insert(1, (src + dst) / 2f);
|
||||
return result;
|
||||
}
|
||||
|
||||
private static bool TryParseFloat(string text, out float value)
|
||||
{
|
||||
return float.TryParse(text, NumberStyles.Float, CultureInfo.InvariantCulture, out value) ||
|
||||
float.TryParse(text, out value);
|
||||
}
|
||||
|
||||
private static float ClampAbs(float value, float limit)
|
||||
{
|
||||
var absLimit = Math.Abs(limit);
|
||||
if (absLimit <= 0) return value;
|
||||
if (value > absLimit) return absLimit;
|
||||
if (value < -absLimit) return -absLimit;
|
||||
return value;
|
||||
}
|
||||
|
||||
private static bool TryGetControlFleetCenter(PilotDefinition self, out float centerX, out float centerY,
|
||||
out float centerTh, out string source)
|
||||
{
|
||||
if (self.TryGetFleetCenterFromMembers(out centerX, out centerY, out centerTh))
|
||||
{
|
||||
source = "fleet";
|
||||
return true;
|
||||
}
|
||||
|
||||
if (self.TryGetFleetCenterFromSlam(out centerX, out centerY, out centerTh))
|
||||
{
|
||||
source = "slam";
|
||||
return true;
|
||||
}
|
||||
|
||||
source = "none";
|
||||
return false;
|
||||
}
|
||||
|
||||
private void Cleanup()
|
||||
{
|
||||
var self = PilotDefinition.Self;
|
||||
self.MultiVehicleScriptVx = 0;
|
||||
self.MultiVehicleScriptVy = 0;
|
||||
self.MultiVehicleScriptVth = 0;
|
||||
self.MultiVehicleScriptMode = 0;
|
||||
self.MultiVehicleScriptEnabled = false;
|
||||
self.MultiVehicleAutoVx = 0;
|
||||
self.MultiVehicleAutoFrontTh = 0;
|
||||
self.MultiVehicleAutoRearTh = 0;
|
||||
self.MultiVehicleAutoHasIdeal = false;
|
||||
self.MultiVehicleAutoEnabled = false;
|
||||
RestoreControlPointRadius();
|
||||
}
|
||||
|
||||
private void ApplyFleetControlPointRadius(MultiWheelChassis chassis, float radius)
|
||||
{
|
||||
if (!_savedControlPoints)
|
||||
{
|
||||
_chassis = chassis;
|
||||
_savedControlRadius = chassis.ControlPointRadius;
|
||||
var gcps = chassis.GetGeometricControlPoints();
|
||||
if (gcps.Count >= 2)
|
||||
{
|
||||
_savedGcp0 = gcps[0].Position;
|
||||
_savedGcp1 = gcps[1].Position;
|
||||
}
|
||||
_savedControlPoints = true;
|
||||
}
|
||||
|
||||
chassis.ControlPointRadius = radius;
|
||||
var points = chassis.GetGeometricControlPoints();
|
||||
if (points.Count >= 2)
|
||||
{
|
||||
points[0].Position = new Vector2(radius, 0);
|
||||
points[1].Position = new Vector2(-radius, 0);
|
||||
}
|
||||
}
|
||||
|
||||
private void RestoreControlPointRadius()
|
||||
{
|
||||
if (!_savedControlPoints || _chassis == null)
|
||||
return;
|
||||
|
||||
_chassis.ControlPointRadius = _savedControlRadius;
|
||||
var points = _chassis.GetGeometricControlPoints();
|
||||
if (points.Count >= 2)
|
||||
{
|
||||
points[0].Position = _savedGcp0;
|
||||
points[1].Position = _savedGcp1;
|
||||
}
|
||||
_savedControlPoints = false;
|
||||
}
|
||||
|
||||
private static void WriteWarmupAuto(PilotDefinition self, Vector2 idealPos, float idealTh,
|
||||
float frontTh, float rearTh)
|
||||
{
|
||||
self.MultiVehicleScriptEnabled = false;
|
||||
self.MultiVehicleScriptMode = 0;
|
||||
self.MultiVehicleScriptVx = 0;
|
||||
self.MultiVehicleScriptVy = 0;
|
||||
self.MultiVehicleScriptVth = 0;
|
||||
self.MultiVehicleAutoEnabled = true;
|
||||
self.MultiVehicleAutoVx = 0;
|
||||
self.MultiVehicleAutoFrontTh = frontTh;
|
||||
self.MultiVehicleAutoRearTh = rearTh;
|
||||
self.MultiVehicleAutoIdealX = idealPos.X;
|
||||
self.MultiVehicleAutoIdealY = idealPos.Y;
|
||||
self.MultiVehicleAutoIdealTh = idealTh;
|
||||
self.MultiVehicleAutoHasIdeal = true;
|
||||
self.MultiVehicleAutoCmdTime = DateTime.Now;
|
||||
}
|
||||
|
||||
public override IEnumerable<bool> Get()
|
||||
{
|
||||
var self = PilotDefinition.Self;
|
||||
var conf = PilotDefinition.Conf;
|
||||
var chassis = BasicPilotBase.Chassis as MultiWheelChassis;
|
||||
_stopping = false;
|
||||
|
||||
if (chassis == null)
|
||||
{
|
||||
DLog.Log("ABORT: FleetCurveWalk requires MultiWheelChassis.", "FleetCurveDbg");
|
||||
yield break;
|
||||
}
|
||||
|
||||
if (conf.MultiVehicleMasterEndpoint != "/")
|
||||
{
|
||||
DLog.Log($"ABORT: FleetCurveWalk must run on master endpoint, endpoint={conf.MultiVehicleMasterEndpoint}",
|
||||
"FleetCurveDbg");
|
||||
Hedingben.ToastText("FleetCurve requires master vehicle", "FleetCurve");
|
||||
yield break;
|
||||
}
|
||||
|
||||
if (Track == null && (ControlPoints == null || ControlPoints.Count < 3))
|
||||
{
|
||||
DLog.Log("ABORT: FleetCurveWalk requires a BezierTrack or at least 3 control points.", "FleetCurveDbg");
|
||||
Hedingben.ToastText("FleetCurve requires track or >=3 control points", "FleetCurve");
|
||||
yield break;
|
||||
}
|
||||
|
||||
if (!TryGetControlFleetCenter(self, out var x0, out var y0, out var theta, out var initialCenterSource))
|
||||
{
|
||||
DLog.Log("ABORT: FleetCurveWalk failed to read fleet center.", "FleetCurveDbg");
|
||||
Hedingben.ToastText("FleetCurve requires master localization", "FleetCurve");
|
||||
yield break;
|
||||
}
|
||||
|
||||
var baseSpeed = Math.Abs(CurveSpeed);
|
||||
if (baseSpeed <= 1e-4f)
|
||||
{
|
||||
DLog.Log("ABORT: FleetCurveWalk speed is zero.", "FleetCurveDbg");
|
||||
yield break;
|
||||
}
|
||||
|
||||
var resolution = Math.Max(2, BezierResolution);
|
||||
var speedFinish = Math.Min(baseSpeed, Math.Abs(FinishSpeed));
|
||||
var gcpLimit = Math.Max(1f, Math.Abs(GcpThetaThreshold));
|
||||
var controlRadius = Math.Max(1f, Math.Abs(conf.TestCarSyncDistance) / 2f);
|
||||
|
||||
ApplyFleetControlPointRadius(chassis, controlRadius);
|
||||
|
||||
try
|
||||
{
|
||||
var track = Track;
|
||||
var trackSource = "external";
|
||||
if (track == null)
|
||||
{
|
||||
var points = new List<Vector2>(ControlPoints);
|
||||
track = new BezierTrack(points, resolution);
|
||||
trackSource = "controlPoints";
|
||||
}
|
||||
track.CarDirectionBias = CarDirectionBias;
|
||||
track.Speed = baseSpeed;
|
||||
|
||||
var center = new Vector2(x0, y0);
|
||||
var (idealPos, idealAngle, bias, pd) = track.QueryTangentPoint(center);
|
||||
var carDirection = (float)CommonMath.ThDiff(theta, CarDirectionBias);
|
||||
var holdTh = ClampAbs((float)CommonMath.ThDiff(idealAngle, carDirection), gcpLimit);
|
||||
var targetBodyTh = (float)CommonMath.RoundTh(idealAngle + CarDirectionBias);
|
||||
|
||||
DLog.Log(
|
||||
$"START center=({x0:0},{y0:0},{theta:0.0}) source={initialCenterSource} " +
|
||||
$"track={track.GetType().Name} trackSource={trackSource} controls={ControlPoints?.Count ?? 0} " +
|
||||
$"len={track.Length():0} speed={baseSpeed:0.000} bias={CarDirectionBias:0.0} " +
|
||||
$"query=({idealPos.X:0},{idealPos.Y:0}) tangent={idealAngle:0.0} targetBody={targetBodyTh:0.0} " +
|
||||
$"pathBias={bias:0.0} pd={pd:0.0} hold={holdTh:0.0} radius={controlRadius:0}",
|
||||
"FleetCurveDbg");
|
||||
|
||||
var warmStart = DateTime.Now;
|
||||
var warmSeqBaseline = self.BeginFleetMotionWarmup();
|
||||
WriteWarmupAuto(self, idealPos, targetBodyTh, holdTh, holdTh);
|
||||
self.PrimeMasterAutoFromSlam();
|
||||
|
||||
var warmEnd = warmStart.AddSeconds(Math.Max(1.0f, StartSyncTimeoutSec));
|
||||
var warmIter = 0;
|
||||
var warmReady = false;
|
||||
var warmDetail = "";
|
||||
while (!_stopping && DateTime.Now < warmEnd)
|
||||
{
|
||||
warmIter++;
|
||||
WriteWarmupAuto(self, idealPos, targetBodyTh, holdTh, holdTh);
|
||||
self.PrimeMasterAutoFromSlam();
|
||||
|
||||
if (warmIter % 5 == 0)
|
||||
{
|
||||
var snap = self.GetFleetCenterSnapshot();
|
||||
int cnt;
|
||||
lock (self.FleetLock) cnt = self.MultiVehicleFleet.Count;
|
||||
DLog.Log(
|
||||
$"WARMUP#{warmIter} snap=({snap.X:0},{snap.Y:0},{snap.Th:0.0}) " +
|
||||
$"cnt={cnt}/{conf.MultiVehicleFleetNum} detail={warmDetail}",
|
||||
"FleetCurveDbg");
|
||||
}
|
||||
|
||||
if (self.IsFleetMotionWarmupReady(warmStart, warmSeqBaseline,
|
||||
conf.TestCarSyncTh, conf.TestCarSyncDistance, out warmDetail))
|
||||
{
|
||||
warmReady = true;
|
||||
DLog.Log($"WARMUP done iter={warmIter} detail={warmDetail}", "FleetCurveDbg");
|
||||
break;
|
||||
}
|
||||
yield return true;
|
||||
}
|
||||
|
||||
if (!warmReady)
|
||||
{
|
||||
DLog.Log($"WARMUP timeout: fleet startup sync failed, abort curve action. detail={warmDetail}",
|
||||
"FleetCurveDbg");
|
||||
Hedingben.ToastText("FleetCurve startup sync timeout", "FleetCurve");
|
||||
Cleanup();
|
||||
yield break;
|
||||
}
|
||||
|
||||
_controller = new ChassisController { BaseSpeed = baseSpeed }.Get();
|
||||
_controller.MultiVehicleSync = true;
|
||||
_controller.BaseSpeed = baseSpeed;
|
||||
_controller.SlowDistance = Math.Max(FinishDistance + 1f, SlowDistance);
|
||||
_controller.FinishDistance = Math.Max(0f, FinishDistance);
|
||||
_controller.FinishSpeed = speedFinish;
|
||||
_controller.SlowingPow = Math.Max(0.01f, SlowingPow);
|
||||
_controller.GcpThetaThreshold = gcpLimit;
|
||||
_controller.AddTrack(track, "FleetCurve");
|
||||
|
||||
Hedingben.ToastText($"FleetCurve len {track.Length():0}mm speed {baseSpeed:0.00}", "FleetCurve");
|
||||
|
||||
foreach (var running in _controller.Track())
|
||||
{
|
||||
if (_stopping)
|
||||
break;
|
||||
if (!running)
|
||||
break;
|
||||
yield return true;
|
||||
}
|
||||
|
||||
if (!_stopping)
|
||||
{
|
||||
self.MultiVehicleAutoVx = 0;
|
||||
self.MultiVehicleAutoCmdTime = DateTime.Now;
|
||||
var settleEnd = DateTime.Now.AddMilliseconds(Math.Max(100, conf.MultiVehicleSyncInterval * 3));
|
||||
while (!_stopping && DateTime.Now < settleEnd)
|
||||
{
|
||||
self.MultiVehicleAutoEnabled = true;
|
||||
self.MultiVehicleAutoVx = 0;
|
||||
self.MultiVehicleAutoCmdTime = DateTime.Now;
|
||||
yield return true;
|
||||
}
|
||||
}
|
||||
|
||||
DLog.Log($"DONE stopping={_stopping}", "FleetCurveDbg");
|
||||
}
|
||||
finally
|
||||
{
|
||||
Cleanup();
|
||||
_controller = null;
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -1,606 +0,0 @@
|
||||
using ClumsyCore;
|
||||
using ClumsyCore.DTools;
|
||||
using ClumsyCore.Interfaces;
|
||||
using ClumsyCore.Pilot;
|
||||
using ClumsyCore.Utilities;
|
||||
using ClumsyDance.ClumsyDance.Detectors;
|
||||
using ClumsyDance.ClumsyWalk.Detectors;
|
||||
using CommonUsage.Chassis;
|
||||
using CommonUsage.Mathematics;
|
||||
using FundamentalLib;
|
||||
using MDCSToolBox.Clumsy.Calibration;
|
||||
using MDCSToolBox.Clumsy.Tracks;
|
||||
using MDCSToolBox.Commons.Controllers;
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Drawing;
|
||||
using System.Linq;
|
||||
using System.Numerics;
|
||||
using System.Security.Cryptography;
|
||||
using System.Threading;
|
||||
using LineSegment = ClumsyCore.Utilities.LineSegment;
|
||||
|
||||
namespace MultiWheelC
|
||||
{
|
||||
[MovementTest(name = "轮胎检测")]
|
||||
public class TireDetect : MovementTest
|
||||
{
|
||||
public override void TestStop()
|
||||
{
|
||||
_running = false;
|
||||
}
|
||||
|
||||
public override void Test()
|
||||
{
|
||||
_painter = UI.GetPainter("TwoLegDetectTest", false);
|
||||
_painter.Clear();
|
||||
|
||||
var frontlidar = UI.GetInput("1是用前雷达识别，2是用后雷达识别");
|
||||
var result = int.Parse(frontlidar.ToString());
|
||||
var lastDetectX = result == 1 ? PilotDefinition.Conf.TireFollowingStage1GuessX : -PilotDefinition.Conf.TireFollowingStage1GuessX;
|
||||
var lastDetectY = 0f;
|
||||
while (_running)
|
||||
{
|
||||
var ld = Detect(lastDetectX, SetFilters(lastDetectX, lastDetectY), result == 1 ? true : false);
|
||||
if(ld == null)
|
||||
{
|
||||
//Console.WriteLine("ld == null");
|
||||
continue;
|
||||
}
|
||||
_painter.Clear();
|
||||
var center = (ld.Src + ld.Dst) / 2;
|
||||
var distanceToCarOrigin = Vector2.Distance(Vector2.Zero, center);
|
||||
var distanceLabelPos = center / 2;
|
||||
_painter.DrawLine(Color.Cyan, Vector2.Zero, center, width: 2);
|
||||
_painter.DrawText(Color.Yellow, $"{distanceToCarOrigin:F3}", distanceLabelPos.X, distanceLabelPos.Y);
|
||||
lastDetectX = center.X;
|
||||
lastDetectY = center.Y;
|
||||
Thread.Sleep(100);
|
||||
}
|
||||
}
|
||||
|
||||
public static LineSegment Detect(float guessX, List<DetectFilter> filters, bool frontlidar)
|
||||
{
|
||||
return new Lidar2dDetect2LegTray()
|
||||
{
|
||||
BlobDist = frontlidar ? PilotDefinition.Conf.TireFrontTwoLegBlobDist : PilotDefinition.Conf.TireBackTwoLegBlobDist,
|
||||
BlobPtCount = frontlidar ? PilotDefinition.Conf.TireTwoLegBlobPtCount : PilotDefinition.Conf.TireTwoLegBlobPtCount,
|
||||
BlobSize = frontlidar ? PilotDefinition.Conf.TireFrontTwoLegBlobSize : PilotDefinition.Conf.TireBackTwoLegBlobSize,
|
||||
CenterChange = Tuple.Create(frontlidar ? PilotDefinition.Conf.TireFrontTwoLegCenterChangeX : PilotDefinition.Conf.TireBackTwoLegCenterChangeX, 0f, 0f),
|
||||
LegWidth = PilotDefinition.Conf.TireTwoLegWidth,
|
||||
LegWidthErr = frontlidar ? PilotDefinition.Conf.TireTwoLegWidthErr : PilotDefinition.Conf.TireTwoLegWidthErr,
|
||||
Padding = frontlidar ? PilotDefinition.Conf.TireFrontPadding : PilotDefinition.Conf.TireBackPadding,
|
||||
PillarFindingScope = frontlidar ? PilotDefinition.Conf.TireFrontTwoLegPillarFindingScope : PilotDefinition.Conf.TireBackTwoLegPillarFindingScope,
|
||||
SgnDir = PilotDefinition.Conf.TwoLegSgnDir,
|
||||
}.DetectWithGuess(frontlidar ? "frontlidar" : "leftlidar,rightlidar", new LineSegment(new Vector2(guessX, 0), Vector2.Zero),
|
||||
guessCoordinateSystem: CoordinateSystem.Car2D, outCoordinateSystem: CoordinateSystem.Car2D, filters);
|
||||
}
|
||||
|
||||
private List<DetectFilter> SetFilters(float guessCenterX, float guessCenterY)
|
||||
{
|
||||
var painter = UI.GetPainter("GeneralFollowing.SetFilters", false);
|
||||
painter.Clear();
|
||||
painter.Clear(3000);
|
||||
|
||||
var box = new Vector2[]
|
||||
{
|
||||
new (guessCenterX - PilotDefinition.Conf.TireFilterLength / 2, guessCenterY - PilotDefinition.Conf.TireFilterWidth / 2),
|
||||
new (guessCenterX + PilotDefinition.Conf.TireFilterLength / 2, guessCenterY - PilotDefinition.Conf.TireFilterWidth / 2),
|
||||
new (guessCenterX + PilotDefinition.Conf.TireFilterLength / 2, guessCenterY + PilotDefinition.Conf.TireFilterWidth / 2),
|
||||
new (guessCenterX - PilotDefinition.Conf.TireFilterLength / 2, guessCenterY + PilotDefinition.Conf.TireFilterWidth / 2),
|
||||
};
|
||||
|
||||
for (var i = 0; i < box.Length; ++i)
|
||||
painter.DrawLine(Color.DarkOliveGreen, box[i], box[(i + 1) % 4]);
|
||||
|
||||
// PC filter in car coordinate frame
|
||||
return new List<DetectFilter>()
|
||||
{
|
||||
new(CoordinateSystem.Car2D,
|
||||
p => LessMath.IsPointInPolygon4(
|
||||
box.Select(v => new PointF(v.X, v.Y)).ToArray(), new PointF(p.X, p.Y))),
|
||||
};
|
||||
}
|
||||
|
||||
private Painter _painter;
|
||||
private bool _running = true;
|
||||
}
|
||||
|
||||
[MovementTest(name = "钻车测试")]
|
||||
public class FollowTire : MovementTest
|
||||
{
|
||||
public override void TestStop()
|
||||
{
|
||||
_dt?.Stop();
|
||||
}
|
||||
|
||||
public override void Test()
|
||||
{
|
||||
var front = UI.GetInput("1是用前雷达识别，2是用后雷达识别");
|
||||
var result = int.Parse(front.ToString());
|
||||
var lidarname = result == 1 ? "前雷达" : "后雷达";
|
||||
DLog.Log($"开始钻车测试，用{lidarname}识别", "TireFollowing");
|
||||
|
||||
var following = new TireFollowing()
|
||||
{
|
||||
GetController = () => new ChassisController().Get(),
|
||||
GuessRangeX = PilotDefinition.Conf.TireFilterLength / 2,
|
||||
GuessRangeY = PilotDefinition.Conf.TireFilterWidth / 2,
|
||||
detectors = new List<TireFollowing.DetectorDefinition>()
|
||||
{
|
||||
new TireFollowing.DetectorDefinition()
|
||||
{
|
||||
DetectFunction = (_, lastDetectX, filters) => TireDetect.Detect(lastDetectX, filters, result == 1 ? true : false),
|
||||
StartGuessingX = result == 1 ? PilotDefinition.Conf.TireFollowingStage1GuessX : -PilotDefinition.Conf.TireFollowingStage1GuessX,
|
||||
StartGuessingY = 0,
|
||||
SwitchWalkBlindCondition = rd => rd <= PilotDefinition.Conf.TireFollowingWalkBlindSwitchingDistance,
|
||||
FinishWalkBlindCondition = rd => rd <= PilotDefinition.Conf.TireFollowingWalkBlindFinishDistance,
|
||||
PathTransformation = new Tuple<float, float, float>(
|
||||
result == 1 ? PilotDefinition.Conf.TireFollowingFrontLidarPathTransformationX : PilotDefinition.Conf.TireFollowingBackLidarPathTransformationX,
|
||||
result == 1 ? PilotDefinition.Conf.TireFollowingFrontLidarPathTransformationY : PilotDefinition.Conf.TireFollowingBackLidarPathTransformationY,
|
||||
0)
|
||||
},
|
||||
new TireFollowing.DetectorDefinition()
|
||||
{
|
||||
DetectFunction = (_, lastDetectX, filters) => TireDetect.Detect(lastDetectX, filters, result == 1 ? true : false),
|
||||
StartGuessingX = result == 1 ? PilotDefinition.Conf.TireFollowingStage2GuessX : -PilotDefinition.Conf.TireFollowingStage2GuessX,
|
||||
StartGuessingY = 0,
|
||||
SwitchWalkBlindCondition = rd => rd <= PilotDefinition.Conf.TireFollowingWalkBlindSwitchingDistance,
|
||||
FinishWalkBlindCondition = rd => rd <= PilotDefinition.Conf.TireFollowingWalkBlindFinishDistance,
|
||||
PathTransformation = new Tuple<float, float, float>(
|
||||
result == 1 ? PilotDefinition.Conf.TireFollowingFrontLidarPathTransformationX : PilotDefinition.Conf.TireFollowingBackLidarPathTransformationX,
|
||||
result == 1 ? PilotDefinition.Conf.TireFollowingFrontLidarPathTransformationY : PilotDefinition.Conf.TireFollowingBackLidarPathTransformationY,
|
||||
0)
|
||||
},
|
||||
},
|
||||
CarDirection = result == 1 ? 0f : 180f,
|
||||
SlowDistance = PilotDefinition.Conf.TireFollowingSlowDistance,
|
||||
MaxSpeed = PilotDefinition.Conf.TireFollowingMaxSpeed,
|
||||
TireNum = PilotDefinition.Conf.TireFollowingTireNum,
|
||||
WalkBlindTh = result == 1 ? PilotDefinition.Conf.TireFollowingFrontLidarWalkBlindTh : PilotDefinition.Conf.TireFollowingBackLidarWalkBlindTh,
|
||||
};
|
||||
_dt = new DriveTask(following.Get());
|
||||
_dt.Wait();
|
||||
DLog.Log($"结束钻车测试", "TireFollowing");
|
||||
}
|
||||
private DriveTask _dt;
|
||||
}
|
||||
|
||||
[MovementTest(name = "离车测试")]
|
||||
public class LeaveCar : MovementTest
|
||||
{
|
||||
public override void TestStop()
|
||||
{
|
||||
_dt?.Stop();
|
||||
}
|
||||
|
||||
public override void Test()
|
||||
{
|
||||
DLog.Log($"开始离车测试，用后雷达识别", "TireFollowing");
|
||||
var following = new TireFollowing()
|
||||
{
|
||||
GetController = () => new ChassisController().Get(),
|
||||
GuessRangeX = PilotDefinition.Conf.TireFilterLength / 2,
|
||||
GuessRangeY = PilotDefinition.Conf.TireFilterWidth / 2,
|
||||
detectors = new List<TireFollowing.DetectorDefinition>()
|
||||
{
|
||||
new TireFollowing.DetectorDefinition()
|
||||
{
|
||||
DetectFunction = (_, lastDetectX, filters) => TireDetect.Detect(lastDetectX, filters, false),
|
||||
StartGuessingX = -PilotDefinition.Conf.TireFollowingStage2GuessX,
|
||||
StartGuessingY = 0,
|
||||
SwitchWalkBlindCondition = rd => rd <= PilotDefinition.Conf.TireFollowingLeaveCarWalkBlindSwitchingDistance,
|
||||
FinishWalkBlindCondition = rd => rd <= PilotDefinition.Conf.TireFollowingWalkBlindFinishDistance,
|
||||
PathTransformation = new Tuple<float, float, float>(
|
||||
PilotDefinition.Conf.TireFollowingLeaveCarBackLidarPathTransformationX,
|
||||
PilotDefinition.Conf.TireFollowingBackLidarPathTransformationY,
|
||||
0)
|
||||
},
|
||||
},
|
||||
CarDirection = 180f,
|
||||
SlowDistance = PilotDefinition.Conf.TireFollowingSlowDistance,
|
||||
MaxSpeed = PilotDefinition.Conf.TireFollowingMaxSpeed,
|
||||
WalkBlindTh = 0,
|
||||
TireNum = 1
|
||||
};
|
||||
_dt = new DriveTask(following.Get());
|
||||
_dt.Wait();
|
||||
DLog.Log($"结束离车测试", "TireFollowing");
|
||||
}
|
||||
private DriveTask _dt;
|
||||
}
|
||||
|
||||
[MovementTest(name = "抱夹关闭")]
|
||||
public class ClampTest1 : MovementTest
|
||||
{
|
||||
public override void TestStop()
|
||||
{
|
||||
_dt?.Stop();
|
||||
PilotDefinition.Self.SpeedLeftArm = 0;
|
||||
PilotDefinition.Self.SpeedRightArm = 0;
|
||||
}
|
||||
|
||||
public override void Test()
|
||||
{
|
||||
_dt = new DriveTask(new ClampToTarget()
|
||||
{
|
||||
LeftClampTarget = PilotDefinition.Self.LeftArmUpperPos,
|
||||
RightClampTarget = PilotDefinition.Self.RightArmUpperPos
|
||||
}.Get());
|
||||
_dt.Wait();
|
||||
}
|
||||
|
||||
private DriveTask _dt;
|
||||
}
|
||||
|
||||
[MovementTest(name = "抱夹打开")]
|
||||
public class ClampTest2 : MovementTest
|
||||
{
|
||||
public override void TestStop()
|
||||
{
|
||||
_dt?.Stop();
|
||||
PilotDefinition.Self.SpeedLeftArm = 0;
|
||||
PilotDefinition.Self.SpeedRightArm = 0;
|
||||
}
|
||||
|
||||
public override void Test()
|
||||
{
|
||||
_dt = new DriveTask(new ClampToTarget()
|
||||
{
|
||||
LeftClampTarget = PilotDefinition.Self.LeftArmLowerPos,
|
||||
RightClampTarget = PilotDefinition.Self.RightArmLowerPos
|
||||
}.Get());
|
||||
_dt.Wait();
|
||||
}
|
||||
|
||||
private DriveTask _dt;
|
||||
}
|
||||
|
||||
[MovementTest(name = "测试前进基于轮里程")]
|
||||
public class LineTrackingTest : MovementTest
|
||||
{
|
||||
public override void TestStop()
|
||||
{
|
||||
_dt?.Stop();
|
||||
}
|
||||
public override void Test()
|
||||
{
|
||||
_dt = new DriveTask(new LineTracking()
|
||||
{
|
||||
Target = PilotDefinition.Conf.LineTrackDistance + (PilotDefinition.Self.LFLActualPos + PilotDefinition.Self.LFRActualPos) / 2,
|
||||
}.Get());
|
||||
_dt.Wait();
|
||||
}
|
||||
private DriveTask _dt;
|
||||
}
|
||||
|
||||
[MovementTest(name = "测试后退基于轮里程")]
|
||||
public class ReverseLineTrackingTest : MovementTest
|
||||
{
|
||||
public override void TestStop()
|
||||
{
|
||||
_dt?.Stop();
|
||||
}
|
||||
public override void Test()
|
||||
{
|
||||
_dt = new DriveTask(new LineTracking()
|
||||
{
|
||||
Target = -PilotDefinition.Conf.LineTrackDistance + (PilotDefinition.Self.LFLActualPos + PilotDefinition.Self.LFRActualPos) / 2,
|
||||
}.Get());
|
||||
_dt.Wait();
|
||||
}
|
||||
private DriveTask _dt;
|
||||
}
|
||||
|
||||
[MovementTest(name = "测试终点跟踪动作-前进")]
|
||||
public class DstTrackerForward : MovementTest
|
||||
{
|
||||
public bool UseInteractivePick = true;
|
||||
public float srcX;
|
||||
public float srcY;
|
||||
public float dstX;
|
||||
public float dstY;
|
||||
public float carDirectionBias = 0f;
|
||||
private readonly Painter _painter = UI.GetPainter("DstTrackerTest");
|
||||
|
||||
public override void TestStop()
|
||||
{
|
||||
_dt?.Stop();
|
||||
_painter?.Clear();
|
||||
}
|
||||
public override void Test()
|
||||
{
|
||||
var p1 = UI.GetPoint("point1");
|
||||
var p2 = UI.GetPoint("point2");
|
||||
|
||||
_painter.Clear();
|
||||
_dt = new DriveTask(new DstTracker()
|
||||
{
|
||||
Src = p1,
|
||||
Dst = p2,
|
||||
CarDirectionBias = carDirectionBias,
|
||||
}.Get());
|
||||
_dt.Wait();
|
||||
}
|
||||
|
||||
private DriveTask _dt;
|
||||
}
|
||||
|
||||
[MovementTest(name = "测试终点跟踪动作-后退")]
|
||||
public class DstTrackerhoutui : MovementTest
|
||||
{
|
||||
public bool UseInteractivePick = true;
|
||||
public float srcX;
|
||||
public float srcY;
|
||||
public float dstX;
|
||||
public float dstY;
|
||||
public float carDirectionBias = 180f;
|
||||
private readonly Painter _painter = UI.GetPainter("DstTrackerTest");
|
||||
|
||||
public override void TestStop()
|
||||
{
|
||||
_dt?.Stop();
|
||||
_painter?.Clear();
|
||||
}
|
||||
public override void Test()
|
||||
{
|
||||
var p1 = UI.GetPoint("point1");
|
||||
var p2 = UI.GetPoint("point2");
|
||||
|
||||
_painter.Clear();
|
||||
_dt = new DriveTask(new DstTracker()
|
||||
{
|
||||
Src = p1,
|
||||
Dst = p2,
|
||||
CarDirectionBias = carDirectionBias,
|
||||
}.Get());
|
||||
_dt.Wait();
|
||||
}
|
||||
|
||||
private DriveTask _dt;
|
||||
}
|
||||
|
||||
[MovementTest(name = "测试先直行再终点跟踪")]
|
||||
public class LineTrackThenDstTrackerTest : MovementTest
|
||||
{
|
||||
public float carDirectionBias = 0f;
|
||||
private readonly Painter _painter = UI.GetPainter("LineTrackThenDstTrackerTest");
|
||||
public override void TestStop()
|
||||
{
|
||||
_dt?.Stop();
|
||||
_painter?.Clear();
|
||||
}
|
||||
public override void Test()
|
||||
{
|
||||
var src = UI.GetPoint("请在上位机选择起点(src)");
|
||||
var dst = UI.GetPoint("请在上位机选择终点(dst)");
|
||||
_painter.Clear();
|
||||
_painter.DrawLine(Color.Cyan, src.X, src.Y, dst.X, dst.Y, width: 3);
|
||||
_painter.DrawCircle(Color.LimeGreen, src.X, src.Y, 80f);
|
||||
_painter.DrawCircle(Color.OrangeRed, dst.X, dst.Y, 80f);
|
||||
_painter.DrawText(Color.LimeGreen, "src", src.X + 80f, src.Y + 80f);
|
||||
_painter.DrawText(Color.OrangeRed, "dst", dst.X + 80f, dst.Y + 80f);
|
||||
IEnumerable<bool> TrackThenFollow()
|
||||
{
|
||||
foreach (var running in new LineTracking()
|
||||
{
|
||||
Target = PilotDefinition.Conf.LineTrackDistance + (PilotDefinition.Self.LFLActualPos + PilotDefinition.Self.LFRActualPos) / 2,
|
||||
EnableHandover = true,
|
||||
HandoverDistance = 200f,
|
||||
HandoverSpeed = 0.3f,
|
||||
}.Get())
|
||||
{
|
||||
if (!running) break;
|
||||
yield return true;
|
||||
}
|
||||
foreach (var running in new DstTracker()
|
||||
{
|
||||
Src = src,
|
||||
Dst = dst,
|
||||
CarDirectionBias = carDirectionBias,
|
||||
InitialSendSpeed = 0.3f
|
||||
}.Get())
|
||||
{
|
||||
if (!running) break;
|
||||
yield return true;
|
||||
}
|
||||
yield return false;
|
||||
}
|
||||
_dt = new DriveTask(TrackThenFollow());
|
||||
_dt.Wait();
|
||||
}
|
||||
private DriveTask _dt;
|
||||
}
|
||||
|
||||
[MovementTest(name = "测试先离车再终点跟踪")]
|
||||
public class LeaveCarThenDstTrackerTest : MovementTest
|
||||
{
|
||||
private readonly Painter _painter = UI.GetPainter("LeaveCarThenDstTrackerTest");
|
||||
|
||||
public override void TestStop()
|
||||
{
|
||||
_dt?.Stop();
|
||||
_painter?.Clear();
|
||||
}
|
||||
|
||||
public override void Test()
|
||||
{
|
||||
var src = UI.GetPoint("请在上位机选择离车后起点(src)");
|
||||
var dst = UI.GetPoint("请在上位机选择终点(dst)");
|
||||
|
||||
_painter.Clear();
|
||||
_painter.DrawLine(Color.Cyan, src.X, src.Y, dst.X, dst.Y, width: 3);
|
||||
_painter.DrawCircle(Color.LimeGreen, src.X, src.Y, 80f);
|
||||
_painter.DrawCircle(Color.OrangeRed, dst.X, dst.Y, 80f);
|
||||
_painter.DrawText(Color.LimeGreen, "src", src.X + 80f, src.Y + 80f);
|
||||
_painter.DrawText(Color.OrangeRed, "dst", dst.X + 80f, dst.Y + 80f);
|
||||
|
||||
IEnumerable<bool> LeaveThenFollow()
|
||||
{
|
||||
var following = new TireFollowing()
|
||||
{
|
||||
GetController = () => new ChassisController().Get(),
|
||||
GuessRangeX = PilotDefinition.Conf.TireFilterLength / 2,
|
||||
GuessRangeY = PilotDefinition.Conf.TireFilterWidth / 2,
|
||||
detectors = new List<TireFollowing.DetectorDefinition>()
|
||||
{
|
||||
new TireFollowing.DetectorDefinition()
|
||||
{
|
||||
DetectFunction = (_, lastDetectX, filters) => TireDetect.Detect(lastDetectX, filters, false),
|
||||
StartGuessingX = -PilotDefinition.Conf.TireFollowingStage2GuessX,
|
||||
StartGuessingY = 0,
|
||||
SwitchWalkBlindCondition = rd => rd <= PilotDefinition.Conf.TireFollowingWalkBlindSwitchingDistance,
|
||||
FinishWalkBlindCondition = rd => rd <= PilotDefinition.Conf.TireFollowingWalkBlindFinishDistance,
|
||||
PathTransformation = new Tuple<float, float, float>(
|
||||
PilotDefinition.Conf.TireFollowingLeaveCarBackLidarPathTransformationX,
|
||||
PilotDefinition.Conf.TireFollowingBackLidarPathTransformationY,
|
||||
0),
|
||||
},
|
||||
},
|
||||
CarDirection = 180f,
|
||||
SlowDistance = PilotDefinition.Conf.TireFollowingSlowDistance,
|
||||
MaxSpeed = PilotDefinition.Conf.TireFollowingMaxSpeed,
|
||||
WalkBlindTh = 0,
|
||||
TireNum = 1
|
||||
};
|
||||
|
||||
foreach (var running in following.Get())
|
||||
{
|
||||
if (!running) break;
|
||||
yield return true;
|
||||
}
|
||||
|
||||
foreach (var running in new DstTracker()
|
||||
{
|
||||
Src = src,
|
||||
Dst = dst,
|
||||
CarDirectionBias = 180f,
|
||||
}.Get())
|
||||
{
|
||||
if (!running) break;
|
||||
yield return true;
|
||||
}
|
||||
|
||||
yield return false;
|
||||
}
|
||||
|
||||
_dt = new DriveTask(LeaveThenFollow());
|
||||
_dt.Wait();
|
||||
}
|
||||
|
||||
private DriveTask _dt;
|
||||
}
|
||||
|
||||
|
||||
[MovementTest(name = "驱动器下使能测试")]
|
||||
public class DriverDisableTest : MovementTest
|
||||
{
|
||||
public override void TestStop()
|
||||
{
|
||||
throw new NotImplementedException();
|
||||
}
|
||||
|
||||
public override void Test()
|
||||
{
|
||||
new DriveTask(new DriverDisable(){ }.Get()).Wait();
|
||||
}
|
||||
}
|
||||
|
||||
[MovementTest(name = "驱动器复位测试")]
|
||||
public class DriverAbleTest : MovementTest
|
||||
{
|
||||
public override void TestStop()
|
||||
{
|
||||
throw new NotImplementedException();
|
||||
}
|
||||
|
||||
public override void Test()
|
||||
{
|
||||
new DriveTask(new DriverAble(){ }.Get()).Wait();
|
||||
}
|
||||
}
|
||||
|
||||
[MovementTest(name = "底盘旋转测试")]
|
||||
public class RotateToAngleTest : MovementTest
|
||||
{
|
||||
public override void TestStop()
|
||||
{
|
||||
throw new NotImplementedException();
|
||||
}
|
||||
|
||||
public override void Test()
|
||||
{
|
||||
var target = UI.GetInput("输入旋转角度：");
|
||||
var chassis = (MultiWheelChassis)PilotDefinition.Chassis;
|
||||
new DriveTask(new MultiWheelRotateInPlace()
|
||||
{
|
||||
AngleTarget = float.Parse(target),
|
||||
PidparamsRead = () => new PIDParams()
|
||||
{
|
||||
Kp = PilotDefinition.Conf.TireFollowingThkp,
|
||||
Ki = PilotDefinition.Conf.TireFollowingThki,
|
||||
Kd = PilotDefinition.Conf.TireFollowingThkd,
|
||||
DeadZone = PilotDefinition.Conf.TireFollowingThDeadZone,
|
||||
SpeedAccPerSec = PilotDefinition.Conf.TireFollowingThSpeedAccPerSec,
|
||||
OutputUpperThreshold = PilotDefinition.Conf.TireFollowingThThresh,
|
||||
MaxI = PilotDefinition.Conf.TireFollowingThMaxI,
|
||||
}
|
||||
}.Get()).Wait();
|
||||
}
|
||||
}
|
||||
|
||||
public class utils
|
||||
{
|
||||
public static List<(float x, float y, float th)> RemoveOutliers(List<(float x, float y, float th)> data, float threshold = 2.0f)
|
||||
{
|
||||
var means = CalculateMean(data);
|
||||
var stdDevs = CalculateStandardDeviation(data, means);
|
||||
|
||||
return data.Where(point =>
|
||||
Math.Abs(point.x - means.x) <= threshold * stdDevs.x &&
|
||||
Math.Abs(point.y - means.y) <= threshold * stdDevs.y &&
|
||||
AngularDistance(point.th, means.th) <= threshold * stdDevs.th
|
||||
).ToList();
|
||||
}
|
||||
|
||||
public static (float x, float y, float th) CalculateMean(List<(float x, float y, float th)> data)
|
||||
{
|
||||
float meanX = data.Average(point => point.x);
|
||||
float meanY = data.Average(point => point.y);
|
||||
float sinSum = data.Sum(point => (float)Math.Sin(DegreeToRadian(point.th)));
|
||||
float cosSum = data.Sum(point => (float)Math.Cos(DegreeToRadian(point.th)));
|
||||
float meanTh = RadianToDegree((float)Math.Atan2(sinSum, cosSum));
|
||||
|
||||
return (meanX, meanY, meanTh);
|
||||
}
|
||||
|
||||
public static (float x, float y, float th) CalculateStandardDeviation(List<(float x, float y, float th)> data, (float x, float y, float th) means)
|
||||
{
|
||||
float varianceX = data.Average(point => (point.x - means.x) * (point.x - means.x));
|
||||
float varianceY = data.Average(point => (point.y - means.y) * (point.y - means.y));
|
||||
|
||||
// 计算角度的方差
|
||||
float varianceTh = data.Average(point => AngularDistance(point.th, means.th) * AngularDistance(point.th, means.th));
|
||||
|
||||
return ((float)Math.Sqrt(varianceX), (float)Math.Sqrt(varianceY), (float)Math.Sqrt(varianceTh));
|
||||
}
|
||||
|
||||
public static float DegreeToRadian(float degree)
|
||||
{
|
||||
return (float)(degree * Math.PI / 180.0);
|
||||
}
|
||||
|
||||
public static float RadianToDegree(float radian)
|
||||
{
|
||||
return (float)(radian * 180.0 / Math.PI);
|
||||
}
|
||||
|
||||
public static float AngularDistance(float angle1, float angle2)
|
||||
{
|
||||
return CommonMath.ThDiff(angle1, angle2);
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
||||
@@ -1,136 +0,0 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Drawing;
|
||||
using System.Linq;
|
||||
using System.Numerics;
|
||||
using System.Threading;
|
||||
using ClumsyCore;
|
||||
using ClumsyCore.DTools;
|
||||
using ClumsyCore.Pilot;
|
||||
using ClumsyCore.Utilities;
|
||||
using ClumsyDance.ClumsyDance.Detectors;
|
||||
using ClumsyDance.ClumsyWalk.Detectors;
|
||||
using FundamentalLib;
|
||||
using LineSegment = ClumsyCore.Utilities.LineSegment;
|
||||
|
||||
namespace MultiWheelC;
|
||||
|
||||
/// <summary>
|
||||
/// 2腿检测：用单线激光雷达识别两腿托盘/轮胎，按上一帧结果作为下一帧猜测做闭环检测。
|
||||
/// 从 StandardMultiWheelLifter 移植；参数全部走 PilotConfig（Fields 面板），雷达选择改为配置项而非阻塞输入。
|
||||
/// </summary>
|
||||
[MovementTest(name = "多舵轮-2腿检测")]
|
||||
public class TwoLegDetect : MovementTest
|
||||
{
|
||||
private Painter _painter;
|
||||
private bool _running = true;
|
||||
|
||||
public override void TestStop() => _running = false;
|
||||
|
||||
public override void Test()
|
||||
{
|
||||
_running = true;
|
||||
_painter = UI.GetPainter("MultiWheelTwoLegDetect", false);
|
||||
_painter.Clear();
|
||||
|
||||
var lidar = PilotDefinition.Conf.TwoLegLidarName;
|
||||
var lastDetectX = PilotDefinition.Conf.TwoLegGuessX;
|
||||
var lastDetectY = 0f;
|
||||
|
||||
while (_running)
|
||||
{
|
||||
var ld = Detect(lidar, lastDetectX, SetFilters(lastDetectX, lastDetectY));
|
||||
if (ld == null)
|
||||
{
|
||||
Thread.Sleep(100);
|
||||
continue;
|
||||
}
|
||||
|
||||
var center = (ld.Src + ld.Dst) / 2;
|
||||
var distanceToCarOrigin = Vector2.Distance(Vector2.Zero, center);
|
||||
|
||||
_painter.Clear();
|
||||
_painter.DrawLine(Color.Cyan, Vector2.Zero, center, width: 2);
|
||||
_painter.DrawText(Color.Yellow, $"{distanceToCarOrigin:F1}", center.X / 2, center.Y / 2);
|
||||
|
||||
Hedingben.ToastText(
|
||||
$"[Test检测] lidar:{lidar} guess x:{lastDetectX:F0} y:{lastDetectY:F0} | " +
|
||||
$"中心 x:{center.X:F0} y:{center.Y:F0} dist:{distanceToCarOrigin:F0}",
|
||||
"MultiWheelTwoLegDetect-test");
|
||||
|
||||
// 用本帧中心作为下一帧猜测，实现闭环跟踪
|
||||
lastDetectX = center.X;
|
||||
lastDetectY = center.Y;
|
||||
Thread.Sleep(100);
|
||||
}
|
||||
|
||||
_painter.Clear();
|
||||
}
|
||||
|
||||
/// <summary>在车体坐标系下，按猜测位置检测两腿，返回连接两腿的线段（车体系）。</summary>
|
||||
public static LineSegment Detect(string lidarName, float guessX, List<DetectFilter> filters)
|
||||
{
|
||||
var conf = PilotDefinition.Conf;
|
||||
#pragma warning disable CS0612, CS0618
|
||||
var detector = new Lidar2dDetect2LegTray
|
||||
{
|
||||
BlobDist = conf.TwoLegBlobDist,
|
||||
BlobPtCount = conf.TwoLegBlobPtCount,
|
||||
BlobSize = conf.TwoLegBlobSize,
|
||||
CenterChange = Tuple.Create(conf.TwoLegCenterChangeX, 0f, 0f),
|
||||
LegWidth = conf.TwoLegWidth,
|
||||
LegWidthErr = conf.TwoLegWidthErr,
|
||||
Padding = conf.TwoLegPadding,
|
||||
PillarFindingScope = conf.TwoLegPillarFindingScope,
|
||||
SgnDir = conf.TwoLegSgnDir,
|
||||
};
|
||||
#pragma warning restore CS0612, CS0618
|
||||
|
||||
var result = detector.DetectWithGuess(
|
||||
lidarName,
|
||||
new LineSegment(new Vector2(guessX, 0), Vector2.Zero),
|
||||
guessCoordinateSystem: CoordinateSystem.Car2D,
|
||||
outCoordinateSystem: CoordinateSystem.Car2D,
|
||||
filters);
|
||||
return ApplyOutputBias(result);
|
||||
}
|
||||
|
||||
private static LineSegment ApplyOutputBias(LineSegment result)
|
||||
{
|
||||
if (result == null) return null;
|
||||
|
||||
var conf = PilotDefinition.Conf;
|
||||
if (Math.Abs(conf.TwoLegOutputBiasX) < 1e-6f && Math.Abs(conf.TwoLegOutputBiasY) < 1e-6f)
|
||||
return result;
|
||||
|
||||
var bias = new Vector2(conf.TwoLegOutputBiasX, conf.TwoLegOutputBiasY);
|
||||
return new LineSegment(result.Src + bias, result.Dst + bias);
|
||||
}
|
||||
|
||||
/// <summary>在猜测中心周围构造一个矩形 ROI，过滤掉框外点云，降低误识别。</summary>
|
||||
public static List<DetectFilter> SetFilters(float guessCenterX, float guessCenterY)
|
||||
{
|
||||
var conf = PilotDefinition.Conf;
|
||||
var painter = UI.GetPainter("MultiWheelTwoLegDetect.Filter", false);
|
||||
painter.Clear();
|
||||
|
||||
var box = new[]
|
||||
{
|
||||
new Vector2(guessCenterX - conf.TwoLegFilterLength / 2, guessCenterY - conf.TwoLegFilterWidth / 2),
|
||||
new Vector2(guessCenterX + conf.TwoLegFilterLength / 2, guessCenterY - conf.TwoLegFilterWidth / 2),
|
||||
new Vector2(guessCenterX + conf.TwoLegFilterLength / 2, guessCenterY + conf.TwoLegFilterWidth / 2),
|
||||
new Vector2(guessCenterX - conf.TwoLegFilterLength / 2, guessCenterY + conf.TwoLegFilterWidth / 2),
|
||||
};
|
||||
|
||||
for (var i = 0; i < box.Length; ++i)
|
||||
painter.DrawLine(Color.DarkOliveGreen, box[i], box[(i + 1) % 4]);
|
||||
|
||||
// 点云滤波在车体坐标系下进行
|
||||
return new List<DetectFilter>
|
||||
{
|
||||
new(CoordinateSystem.Car2D,
|
||||
p => LessMath.IsPointInPolygon4(
|
||||
box.Select(v => new PointF(v.X, v.Y)).ToArray(), new PointF(p.X, p.Y))),
|
||||
};
|
||||
}
|
||||
}
|
||||
@@ -1,608 +0,0 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Numerics;
|
||||
using ClumsyCore;
|
||||
using ClumsyCore.Interfaces;
|
||||
using ClumsyCore.Pilot;
|
||||
using FundamentalLib;
|
||||
using CommonUsage.Chassis;
|
||||
using CommonUsage.Mathematics;
|
||||
using MDCSToolBox.Clumsy.Movements;
|
||||
using MDCSToolBox.Clumsy.Pilot;
|
||||
using MDCSToolBox.Clumsy.Tracks;
|
||||
|
||||
namespace MultiWheelC;
|
||||
|
||||
public class MultiForwardTest : MovementDefinition
|
||||
{
|
||||
public float Speed = 0.2f;
|
||||
public float DurationSeconds = 2f;
|
||||
|
||||
public override IEnumerable<bool> Get()
|
||||
{
|
||||
var chassis = (MultiWheelChassis)BasicPilotBase.Chassis;
|
||||
chassis.SetOriginBias(0, 0, 0);
|
||||
var end = DateTime.Now.AddSeconds(DurationSeconds);
|
||||
while (DateTime.Now < end)
|
||||
{
|
||||
chassis.SendMotion(Speed, 0, 0);
|
||||
yield return true;
|
||||
}
|
||||
|
||||
chassis.SendMotion(0, 0, 0);
|
||||
}
|
||||
}
|
||||
|
||||
// 原地旋转到指定世界坐标系朝向：先把舵轮打到旋转所需角度，对齐后再旋转，按目标角度停止（非固定时长）。
|
||||
public class MultiRotateToWorldAngle : MovementDefinition
|
||||
{
|
||||
/// <summary>目标朝向（世界坐标系，单位 deg）。</summary>
|
||||
public float TargetWorldDeg;
|
||||
|
||||
/// <summary>旋转角速度（deg/s，逆时针为正）。</summary>
|
||||
public float RotSpeed = 30f;
|
||||
|
||||
/// <summary>到位角度精度（deg）。</summary>
|
||||
public float ArriveDeg = 1f;
|
||||
|
||||
/// <summary>起转前舵轮对齐精度（deg）。</summary>
|
||||
public float WheelAlignDeg = 2f;
|
||||
|
||||
public override IEnumerable<bool> Get()
|
||||
{
|
||||
var chassis = (MultiWheelChassis)BasicPilotBase.Chassis;
|
||||
chassis.SetOriginBias(0, 0, 0);
|
||||
|
||||
// 阶段一：仅把舵轮打到原地旋转所需角度（下发 0 速度，只对齐不旋转）。
|
||||
while (true)
|
||||
{
|
||||
chassis.SendRotateMotion(0);
|
||||
if (WheelsAligned(chassis, WheelAlignDeg)) break;
|
||||
yield return true;
|
||||
}
|
||||
|
||||
// 阶段二：旋转到目标世界朝向，到位即停。
|
||||
var target = CommonMath.RoundTh(TargetWorldDeg);
|
||||
while (true)
|
||||
{
|
||||
var cur = CommonMath.RoundTh((float)DetourInterface.getCartLocation().th);
|
||||
var diff = CommonMath.ThDiff(target, cur); // 逆时针为正
|
||||
if (Math.Abs(diff) <= ArriveDeg) break;
|
||||
chassis.SendRotateMotion(Math.Sign(diff) * RotSpeed);
|
||||
yield return true;
|
||||
}
|
||||
|
||||
chassis.PredefinedDriveStop();
|
||||
}
|
||||
|
||||
private static bool WheelsAligned(MultiWheelChassis chassis, float tolDeg)
|
||||
{
|
||||
#pragma warning disable CS0612, CS0618
|
||||
var wheels = chassis.GetSteerWheels();
|
||||
#pragma warning restore CS0612, CS0618
|
||||
foreach (var sw in wheels)
|
||||
if (Math.Abs(CommonMath.ThDiff(sw.ReadAngle(), sw.GetSendAngle())) > tolDeg)
|
||||
return false;
|
||||
return true;
|
||||
}
|
||||
}
|
||||
|
||||
[MovementTest(name = "多舵轮-前进2秒")]
|
||||
public class MultiForwardMovementTest : MovementTest
|
||||
{
|
||||
private DriveTask _task;
|
||||
|
||||
public override void Test()
|
||||
{
|
||||
_task = new DriveTask(new MultiForwardTest().Get());
|
||||
_task.Wait();
|
||||
}
|
||||
|
||||
public override void TestStop() => _task?.Stop();
|
||||
}
|
||||
|
||||
[MovementTest(name = "多舵轮-原地旋转到目标角度")]
|
||||
public class MultiRotateMovementTest : MovementTest
|
||||
{
|
||||
private DriveTask _task;
|
||||
|
||||
public override void Test()
|
||||
{
|
||||
_task = new DriveTask(new MultiRotateToWorldAngle
|
||||
{
|
||||
TargetWorldDeg = PilotDefinition.Conf.InPlaceRotateTargetWorldDeg,
|
||||
RotSpeed = PilotDefinition.Conf.InPlaceRotateSpeed,
|
||||
ArriveDeg = PilotDefinition.Conf.InPlaceRotateArriveDeg,
|
||||
WheelAlignDeg = PilotDefinition.Conf.InPlaceRotateWheelAlignDeg
|
||||
}.Get());
|
||||
_task.Wait();
|
||||
}
|
||||
|
||||
public override void TestStop()
|
||||
{
|
||||
_task?.Stop();
|
||||
((MultiWheelChassis)BasicPilotBase.Chassis).PredefinedDriveStop();
|
||||
}
|
||||
}
|
||||
|
||||
// ===== 车队联动-原地旋转动作 =====
|
||||
// 等价于 FleetRemote 的「原地旋转」模式（已实测可用）：FleetRemote 通过 Medulla 手动 IO
|
||||
// (MultiVehicleManualEnabled + Mode=2 + Vth) 驱动 PilotDefinition.TickMultiVehicle 绕车队中心旋转。
|
||||
// 手动 IO 是 [AsLowerIO]（Medulla→Clumsy，每周期回写），Clumsy 侧动作直接写会被覆盖；
|
||||
// 因此本动作改用 Clumsy 内部脚本字段 MultiVehicleScript*（TickMultiVehicle 已将其作为手动等价输入），
|
||||
// 不写一行底盘指令——实际的 SendRotateMotion + PI 纠偏 + 向从车广播均由 TickMultiVehicle 完成。
|
||||
//
|
||||
// 前提：在「主车」(MultiVehicleMasterEndpoint == "/") 的 Clumsy 上运行，且从车已注册(编队就绪)。
|
||||
// 停止条件：主车 SLAM 朝向累计转过 |TargetDeltaDeg|（刚体原地旋转，整车朝向变化量 == 车队转角）；
|
||||
// 无定位时退化为按 |TargetDeltaDeg| / |Omega| 估算时长；并带安全超时。
|
||||
public class FleetRotateInPlace : MovementDefinition
|
||||
{
|
||||
/// <summary>角速度大小(deg/s)；实际方向由 TargetDeltaDeg 的符号决定。</summary>
|
||||
public float Omega = 15f;
|
||||
|
||||
/// <summary>目标相对转角(deg，带符号，+ 为逆时针)。</summary>
|
||||
public float TargetDeltaDeg = 90f;
|
||||
|
||||
/// <summary>到位角度精度(deg)。</summary>
|
||||
public float ArriveDeg = 1.5f;
|
||||
|
||||
/// <summary>减速区宽度(deg)：剩余角度小于此值时，角速度按剩余比例线性降到 MinOmega，抑制惯性超调。</summary>
|
||||
public float SlowDeg = 25f;
|
||||
|
||||
/// <summary>减速区末段最小角速度(deg/s)：避免越接近目标越慢、长尾停不下/到不了位。</summary>
|
||||
public float MinOmega = 3f;
|
||||
|
||||
/// <summary>缓启动角加速度(deg/s²)：起步时角速度从 0 按此斜率爬升到巡航值，抑制起步抖动/队形骤偏。仅作用于起步加速，<=0 关闭缓启动（阶跃起步）。</summary>
|
||||
public float AccelDegPerSec2 = 20f;
|
||||
|
||||
/// <summary>
|
||||
/// 是否用 Detour 主车航向闭环判停（读 getCartLocation().th 累计实际转角，到 |TargetDeltaDeg| 停）。
|
||||
/// 与 MultiVehicleSyncUseDetour 解耦：转到指定角度需要角度反馈，故默认 true。
|
||||
/// false 时退化为按估算时长开环停止（实际转速≠指令时不精确）。注意 true 时若无有效全局定位，
|
||||
/// getCartLocation() 会阻塞（与单车 MultiRotateToWorldAngle 行为一致）。
|
||||
/// </summary>
|
||||
public bool UseDetourHeading = true;
|
||||
|
||||
// 注：不设超时上限——旋转持续到到位（或无定位时按估算时长结束），或被 Stop()/TestStop() 主动中止。
|
||||
|
||||
/// <summary>到位后保持脚本使能、角速度归零的安定时长(s)，让纠偏把队形稳住再撤离。</summary>
|
||||
public float SettleSec = 0.5f;
|
||||
|
||||
private void ClearScript()
|
||||
{
|
||||
var self = PilotDefinition.Self;
|
||||
self.MultiVehicleScriptVx = 0;
|
||||
self.MultiVehicleScriptVy = 0;
|
||||
self.MultiVehicleScriptVth = 0;
|
||||
self.MultiVehicleScriptEnabled = false;
|
||||
}
|
||||
|
||||
public void Stop() => ClearScript();
|
||||
|
||||
public override IEnumerable<bool> Get()
|
||||
{
|
||||
var self = PilotDefinition.Self;
|
||||
var conf = PilotDefinition.Conf;
|
||||
|
||||
if (conf.MultiVehicleMasterEndpoint != "/")
|
||||
{
|
||||
Hedingben.ToastText("车队原地旋转需在主车(主车端点=\"/\")运行", "FleetRotate");
|
||||
yield break;
|
||||
}
|
||||
|
||||
var dir = Math.Sign(TargetDeltaDeg);
|
||||
if (dir == 0) dir = 1;
|
||||
var maxOmega = Math.Abs(Omega);
|
||||
var minOmega = Math.Min(Math.Abs(MinOmega), maxOmega); // 最小不超过最大
|
||||
var slowDeg = Math.Max(1e-3f, SlowDeg); // 减速区宽度
|
||||
var accel = AccelDegPerSec2; // 缓启动角加速度，仅作用于起步，<=0 关闭
|
||||
var targetMag = Math.Abs(TargetDeltaDeg);
|
||||
|
||||
var hasPos = UseDetourHeading;
|
||||
var prevTh = hasPos ? (float)DetourInterface.getCartLocation().th : 0f;
|
||||
var startTh = prevTh;
|
||||
var accumulated = 0f; // 累计带符号转角(deg)
|
||||
var start = DateTime.Now;
|
||||
var lastTime = start;
|
||||
var lastLog = DateTime.MinValue;
|
||||
var lastCenterLog = DateTime.MinValue;
|
||||
var cmdMag = 0f; // 当前实际下发角速度大小(deg/s)，缓启动从 0 斜坡爬升
|
||||
var centerTracking = false;
|
||||
float centerStartX = 0, centerStartY = 0, centerStartTh = 0;
|
||||
float centerLastX = 0, centerLastY = 0, centerLastTh = 0, centerMaxDrift = 0;
|
||||
// 无定位按时长估算时，补上缓启动斜坡少转的等效时间(≈ maxOmega/(2·accel))，使时长更接近目标角。
|
||||
var estDuration = maxOmega > 1e-3 ? targetMag / maxOmega : 0;
|
||||
if (accel > 1e-3) estDuration += maxOmega / (2 * accel);
|
||||
|
||||
DLog.Log(
|
||||
$"REQUEST target={TargetDeltaDeg:0.0} dir={dir} omega={maxOmega:0.0} accel={accel:0.0} " +
|
||||
$"slowDeg={slowDeg:0.0} minOmega={minOmega:0.0} useDetourHeading={hasPos} startTh={startTh:0.00} " +
|
||||
$"estDuration={estDuration:0.00}s syncUseDetour={conf.MultiVehicleSyncUseDetour}",
|
||||
"FleetRotateDbg");
|
||||
|
||||
// 使能脚本驱动的原地旋转（mode2）。TickMultiVehicle 后台循环据此执行旋转并广播给从车。
|
||||
// 起步从 0 角速度开始，由缓启动斜坡爬升，避免阶跃下发导致队形骤偏/抖动。
|
||||
self.MultiVehicleScriptVx = 0;
|
||||
self.MultiVehicleScriptVy = 0;
|
||||
self.MultiVehicleScriptMode = 2;
|
||||
self.MultiVehicleScriptVth = 0;
|
||||
self.MultiVehicleScriptEnabled = true;
|
||||
self.MultiVehicleRotateWheelsReady = false;
|
||||
self.MultiVehicleRotateFleetReady = false;
|
||||
|
||||
DLog.Log("WAIT_ALIGN fleet rotate wheels", "FleetRotateDbg");
|
||||
while (!self.MultiVehicleRotateFleetReady)
|
||||
{
|
||||
self.MultiVehicleScriptVx = 0;
|
||||
self.MultiVehicleScriptVy = 0;
|
||||
self.MultiVehicleScriptMode = 2;
|
||||
self.MultiVehicleScriptVth = 0;
|
||||
self.MultiVehicleScriptEnabled = true;
|
||||
Hedingben.ToastText("车队原地旋转舵轮预对齐中", "FleetRotate");
|
||||
yield return true;
|
||||
}
|
||||
|
||||
float centerStartCarX = 0, centerStartCarY = 0, centerStartCarTh = 0;
|
||||
if (hasPos)
|
||||
{
|
||||
var startPos = DetourInterface.getCartLocation();
|
||||
centerStartCarX = (float)startPos.x;
|
||||
centerStartCarY = (float)startPos.y;
|
||||
centerStartCarTh = (float)startPos.th;
|
||||
prevTh = centerStartCarTh;
|
||||
startTh = prevTh;
|
||||
if (self.TryGetFleetCenterFromPose(centerStartCarX, centerStartCarY, centerStartCarTh,
|
||||
out centerStartX, out centerStartY, out centerStartTh))
|
||||
{
|
||||
centerLastX = centerStartX;
|
||||
centerLastY = centerStartY;
|
||||
centerLastTh = centerStartTh;
|
||||
centerMaxDrift = 0;
|
||||
centerTracking = true;
|
||||
}
|
||||
}
|
||||
accumulated = 0f;
|
||||
start = DateTime.Now;
|
||||
lastTime = start;
|
||||
lastLog = DateTime.MinValue;
|
||||
lastCenterLog = DateTime.MinValue;
|
||||
cmdMag = 0f;
|
||||
DLog.Log(
|
||||
$"START target={TargetDeltaDeg:0.0} dir={dir} omega={maxOmega:0.0} startTh={startTh:0.00} " +
|
||||
$"fleetAligned={self.MultiVehicleRotateFleetReady}",
|
||||
"FleetRotateDbg");
|
||||
if (centerTracking)
|
||||
{
|
||||
DLog.Log(
|
||||
$"START center=({centerStartX:0.0},{centerStartY:0.0},{centerStartTh:0.00}) " +
|
||||
$"car=({centerStartCarX:0.0},{centerStartCarY:0.0},{centerStartCarTh:0.00}) " +
|
||||
$"target={TargetDeltaDeg:0.0} omega={maxOmega:0.0}",
|
||||
"FleetRotateCenterDbg");
|
||||
}
|
||||
|
||||
var stopReason = "stop()";
|
||||
while (true)
|
||||
{
|
||||
var now = DateTime.Now;
|
||||
var dt = (float)Math.Min(0.2, Math.Max(0, (now - lastTime).TotalSeconds));
|
||||
lastTime = now;
|
||||
var elapsed = (now - start).TotalSeconds;
|
||||
|
||||
float desiredMag;
|
||||
float curTh = 0f, remaining = 0f, actualRate = 0f;
|
||||
if (hasPos)
|
||||
{
|
||||
var carPos = DetourInterface.getCartLocation();
|
||||
curTh = (float)carPos.th;
|
||||
var step = (float)CommonMath.ThDiff(curTh, prevTh); // 本帧实际转角(逆时针为正)
|
||||
accumulated += step;
|
||||
actualRate = dt > 1e-3 ? step / dt : 0f; // 实际角速率(deg/s)，用于对比指令
|
||||
prevTh = curTh;
|
||||
|
||||
remaining = targetMag - Math.Abs(accumulated);
|
||||
if (remaining <= ArriveDeg) { stopReason = "arrived"; break; }
|
||||
|
||||
// 减速区：剩余角度 < SlowDeg 时，目标角速度按剩余比例线性降到 MinOmega，
|
||||
// 使切断指令瞬间残余动量足够小，抑制惯性滑行造成的超调。宽度直观、便于现场调试。
|
||||
desiredMag = remaining < slowDeg
|
||||
? Math.Max(minOmega, maxOmega * (remaining / slowDeg))
|
||||
: maxOmega;
|
||||
|
||||
if (centerTracking &&
|
||||
self.TryGetFleetCenterFromPose((float)carPos.x, (float)carPos.y, (float)carPos.th,
|
||||
out centerLastX, out centerLastY, out centerLastTh))
|
||||
{
|
||||
var centerDx = centerLastX - centerStartX;
|
||||
var centerDy = centerLastY - centerStartY;
|
||||
var centerDrift = (float)Math.Sqrt(centerDx * centerDx + centerDy * centerDy);
|
||||
centerMaxDrift = Math.Max(centerMaxDrift, centerDrift);
|
||||
var centerDth = (float)CommonMath.ThDiff(centerLastTh, centerStartTh);
|
||||
if ((now - lastCenterLog).TotalMilliseconds >= 250)
|
||||
{
|
||||
lastCenterLog = now;
|
||||
DLog.Log(
|
||||
$"ACTION t={elapsed:0.00}s center=({centerLastX:0.0},{centerLastY:0.0},{centerLastTh:0.00}) " +
|
||||
$"start=({centerStartX:0.0},{centerStartY:0.0},{centerStartTh:0.00}) " +
|
||||
$"drift=({centerDx:0.0},{centerDy:0.0}) dist={centerDrift:0.0} max={centerMaxDrift:0.0} dth={centerDth:0.00} " +
|
||||
$"cmdW={dir * cmdMag:0.000} actualW={actualRate:0.000} acc={accumulated:0.0} remain={remaining:0.0} " +
|
||||
$"wheelReady={self.MultiVehicleRotateWheelsReady} fleetReady={self.MultiVehicleRotateFleetReady}",
|
||||
"FleetRotateCenterDbg");
|
||||
}
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
// 无定位：按时长估算，无法测角，目标维持巡航速度到估算时长（仅缓启动整形）。
|
||||
desiredMag = maxOmega;
|
||||
if (elapsed >= estDuration) { stopReason = "estDuration"; break; }
|
||||
}
|
||||
|
||||
// 缓启动：只对“加速(目标>当前)”按角加速度限斜率，让起步平滑爬升；
|
||||
// “减速(目标<当前)”跟随上面的减速曲线立即下调，保证及时刹车不超调。
|
||||
if (accel > 1e-3 && desiredMag > cmdMag)
|
||||
cmdMag = Math.Min(desiredMag, cmdMag + accel * dt);
|
||||
else
|
||||
cmdMag = desiredMag;
|
||||
|
||||
self.MultiVehicleScriptVth = dir * cmdMag;
|
||||
|
||||
// 落盘诊断(节流~150ms)：实际航向/累计转角/实际角速率 vs 指令角速率，定位"开环转速不足"。
|
||||
if ((now - lastLog).TotalMilliseconds >= 150)
|
||||
{
|
||||
lastLog = now;
|
||||
DLog.Log(
|
||||
hasPos
|
||||
? $"t={elapsed:0.00}s curTh={curTh:0.00} acc={accumulated:0.0} remain={remaining:0.0} " +
|
||||
$"cmdW={dir * cmdMag:0.0} actualW={actualRate:0.0} (实际/指令={(Math.Abs(cmdMag) > 1e-3 ? actualRate / (dir * cmdMag) : 0):0.00})"
|
||||
: $"t={elapsed:0.00}s/{estDuration:0.00}s (无航向反馈,开环按时长) cmdW={dir * cmdMag:0.0}",
|
||||
"FleetRotateDbg");
|
||||
}
|
||||
|
||||
Hedingben.ToastText(
|
||||
hasPos
|
||||
? $"车队原地旋转 目标{TargetDeltaDeg:0.0}° 已转{accumulated:0.0}° 余{targetMag - Math.Abs(accumulated):0.0}° ω={cmdMag:0.0}"
|
||||
: $"车队原地旋转(无定位,按时长) {elapsed:0.0}/{estDuration:0.0}s ω={cmdMag:0.0}",
|
||||
"FleetRotate");
|
||||
|
||||
yield return true;
|
||||
}
|
||||
|
||||
// 到位：角速度先归零，保持脚本使能让 TickMultiVehicle 的 PI 把队形稳住一小段时间再撤离。
|
||||
self.MultiVehicleScriptVth = 0;
|
||||
var settleEnd = DateTime.Now.AddSeconds(Math.Max(0, SettleSec));
|
||||
while (DateTime.Now < settleEnd)
|
||||
yield return true;
|
||||
|
||||
ClearScript();
|
||||
DLog.Log(
|
||||
$"DONE reason={stopReason} 累计转角={accumulated:0.0}° 目标={TargetDeltaDeg:0.0}° " +
|
||||
$"用时={(DateTime.Now - start).TotalSeconds:0.00}s useDetourHeading={hasPos}",
|
||||
"FleetRotateDbg");
|
||||
if (centerTracking)
|
||||
{
|
||||
var centerDx = centerLastX - centerStartX;
|
||||
var centerDy = centerLastY - centerStartY;
|
||||
var centerDrift = (float)Math.Sqrt(centerDx * centerDx + centerDy * centerDy);
|
||||
var centerDth = (float)CommonMath.ThDiff(centerLastTh, centerStartTh);
|
||||
DLog.Log(
|
||||
$"DONE reason={stopReason} center=({centerLastX:0.0},{centerLastY:0.0},{centerLastTh:0.00}) " +
|
||||
$"start=({centerStartX:0.0},{centerStartY:0.0},{centerStartTh:0.00}) " +
|
||||
$"drift=({centerDx:0.0},{centerDy:0.0}) dist={centerDrift:0.0} max={centerMaxDrift:0.0} dth={centerDth:0.00} " +
|
||||
$"acc={accumulated:0.0} target={TargetDeltaDeg:0.0}",
|
||||
"FleetRotateCenterDbg");
|
||||
}
|
||||
Hedingben.ToastText($"车队原地旋转完成({stopReason}) 累计{accumulated:0.0}°", "FleetRotate");
|
||||
}
|
||||
}
|
||||
|
||||
[MovementTest(name = "车队联动-原地旋转")]
|
||||
public class FleetRotateInPlaceTest : MovementTest
|
||||
{
|
||||
private FleetRotateInPlace _proc;
|
||||
private DriveTask _task;
|
||||
|
||||
public override void Test()
|
||||
{
|
||||
_proc = new FleetRotateInPlace
|
||||
{
|
||||
Omega = PilotDefinition.Conf.FleetRotateOmega,
|
||||
TargetDeltaDeg = PilotDefinition.Conf.FleetRotateTargetDeltaDeg,
|
||||
ArriveDeg = PilotDefinition.Conf.FleetRotateArriveDeg,
|
||||
SlowDeg = PilotDefinition.Conf.FleetRotateSlowDeg,
|
||||
MinOmega = PilotDefinition.Conf.FleetRotateMinOmega,
|
||||
AccelDegPerSec2 = PilotDefinition.Conf.FleetRotateAccel,
|
||||
SettleSec = PilotDefinition.Conf.FleetRotateSettleSec,
|
||||
UseDetourHeading = PilotDefinition.Conf.FleetRotateUseDetourHeading
|
||||
};
|
||||
_task = new DriveTask(_proc.Get());
|
||||
_task.Wait();
|
||||
}
|
||||
|
||||
public override void TestStop()
|
||||
{
|
||||
_proc?.Stop();
|
||||
_task?.Stop();
|
||||
}
|
||||
}
|
||||
|
||||
[MovementTest(name = "车队联动-曲线行走")]
|
||||
public class FleetCurveWalkTest : MovementTest
|
||||
{
|
||||
private FleetCurveWalk _proc;
|
||||
private DriveTask _task;
|
||||
|
||||
public override void Test()
|
||||
{
|
||||
var self = PilotDefinition.Self;
|
||||
if (!self.TryGetFleetCenterFromMembers(out var x, out var y, out var th) &&
|
||||
!self.TryGetFleetCenterFromSlam(out x, out y, out th))
|
||||
{
|
||||
DLog.Log("FleetCurveWalkTest abort: failed to read fleet center.", "FleetCurveDbg");
|
||||
Hedingben.ToastText("FleetCurve requires master localization", "FleetCurve");
|
||||
return;
|
||||
}
|
||||
|
||||
var pointCount = Math.Max(3, PilotDefinition.Conf.FleetCurveTestControlPointCount);
|
||||
var controlPoints = new List<Vector2>();
|
||||
for (var i = 0; i < pointCount; i++)
|
||||
controlPoints.Add(UI.GetPoint($"FleetCurve point {i + 1}/{pointCount}"));
|
||||
var fleetCenter = new Vector2(x, y);
|
||||
if (Vector2.Distance(fleetCenter, controlPoints[0]) >
|
||||
Vector2.Distance(fleetCenter, controlPoints[controlPoints.Count - 1]))
|
||||
controlPoints.Reverse();
|
||||
var track = new BezierTrack(controlPoints)
|
||||
{
|
||||
Speed = PilotDefinition.Conf.FleetCurveSpeed,
|
||||
CarDirectionBias = 0f
|
||||
};
|
||||
|
||||
_proc = new FleetCurveWalk
|
||||
{
|
||||
Track = track,
|
||||
CurveSpeed = PilotDefinition.Conf.FleetCurveSpeed,
|
||||
CarDirectionBias = 0f,
|
||||
SlowDistance = PilotDefinition.Conf.FleetCurveSlowDistance,
|
||||
FinishDistance = PilotDefinition.Conf.FleetCurveFinishDistance,
|
||||
FinishSpeed = PilotDefinition.Conf.FleetCurveFinishSpeed,
|
||||
SlowingPow = PilotDefinition.Conf.FleetCurveSlowingPow,
|
||||
GcpThetaThreshold = PilotDefinition.Conf.FleetCrabGcpThetaThreshold,
|
||||
StartSyncTimeoutSec = PilotDefinition.Conf.FleetCrabStartSyncTimeoutSec
|
||||
};
|
||||
_task = new DriveTask(_proc.Get());
|
||||
_task.Wait();
|
||||
}
|
||||
|
||||
public override void TestStop()
|
||||
{
|
||||
_proc?.Stop();
|
||||
_task?.Stop();
|
||||
}
|
||||
}
|
||||
|
||||
[MovementTest(name = "车队联动-自动蟹行")]
|
||||
public class FleetCrabWalkTest : MovementTest
|
||||
{
|
||||
private FleetCrabWalk _proc;
|
||||
private DriveTask _task;
|
||||
|
||||
public override void Test()
|
||||
{
|
||||
_proc = new FleetCrabWalk
|
||||
{
|
||||
CrabAngleDeg = PilotDefinition.Conf.FleetCrabAngleDeg,
|
||||
BodyToPathAngleDeg = PilotDefinition.Conf.FleetCrabAngleDeg,
|
||||
CrabLengthMm = PilotDefinition.Conf.FleetCrabLengthMm,
|
||||
CrabSpeed = PilotDefinition.Conf.FleetCrabSpeed,
|
||||
FleetCrabAccel = PilotDefinition.Conf.FleetCrabAccel,
|
||||
FleetCrabStartAccel = PilotDefinition.Conf.FleetCrabStartAccel,
|
||||
FleetCrabSlowDistance = PilotDefinition.Conf.FleetCrabSlowDistance,
|
||||
FleetCrabFinishDistance = PilotDefinition.Conf.FleetCrabFinishDistance,
|
||||
FleetCrabFinishSpeed = PilotDefinition.Conf.FleetCrabFinishSpeed,
|
||||
FleetCrabSlowingPow = PilotDefinition.Conf.FleetCrabSlowingPow,
|
||||
GcpThetaThreshold = PilotDefinition.Conf.FleetCrabGcpThetaThreshold
|
||||
};
|
||||
_task = new DriveTask(_proc.Get());
|
||||
_task.Wait();
|
||||
}
|
||||
|
||||
public override void TestStop()
|
||||
{
|
||||
_proc?.Stop();
|
||||
_task?.Stop();
|
||||
}
|
||||
}
|
||||
|
||||
// ===== 调用 Playground WebAPI 瞬移小车（前移 / 左移 / 旋转）=====
|
||||
// 平移/旋转量在 Fields 面板配置：WebApiTranslateMm（默认100mm）、WebApiRotateDeg（默认5度）。
|
||||
|
||||
[MovementTest(name = "多舵轮-WebAPI前移")]
|
||||
public class WebApiForwardMoveTest : MovementTest
|
||||
{
|
||||
public override void Test()
|
||||
{
|
||||
var url = PilotDefinition.Conf.PlaygroundWebApiUrl;
|
||||
var name = PilotDefinition.Conf.PlaygroundRobotName;
|
||||
var d = PilotDefinition.Conf.WebApiTranslateMm;
|
||||
|
||||
var pose = PlaygroundWebApi.GetPose(url, name);
|
||||
// 车体系前向 (d, 0) 变换到世界系：车头方向即朝向 yaw
|
||||
var dst = CommonMath.Transform2D(new Vector2(pose.X, pose.Y), pose.YawDeg, new Vector2(d, 0));
|
||||
PlaygroundWebApi.Move(url, name, dst.X, dst.Y, pose.YawDeg);
|
||||
Hedingben.ToastText($"前移 {d:f0}mm -> ({dst.X:f0},{dst.Y:f0})", "WebApiForward");
|
||||
}
|
||||
|
||||
public override void TestStop()
|
||||
{
|
||||
}
|
||||
}
|
||||
|
||||
[MovementTest(name = "多舵轮-WebAPI左移")]
|
||||
public class WebApiLeftMoveTest : MovementTest
|
||||
{
|
||||
public override void Test()
|
||||
{
|
||||
var url = PilotDefinition.Conf.PlaygroundWebApiUrl;
|
||||
var name = PilotDefinition.Conf.PlaygroundRobotName;
|
||||
var d = PilotDefinition.Conf.WebApiTranslateMm;
|
||||
|
||||
var pose = PlaygroundWebApi.GetPose(url, name);
|
||||
// 车体系左向 (0, d) 变换到世界系（车体 +Y 即左侧）
|
||||
var dst = CommonMath.Transform2D(new Vector2(pose.X, pose.Y), pose.YawDeg, new Vector2(0, d));
|
||||
PlaygroundWebApi.Move(url, name, dst.X, dst.Y, pose.YawDeg);
|
||||
Hedingben.ToastText($"左移 {d:f0}mm -> ({dst.X:f0},{dst.Y:f0})", "WebApiLeft");
|
||||
}
|
||||
|
||||
public override void TestStop()
|
||||
{
|
||||
}
|
||||
}
|
||||
|
||||
[MovementTest(name = "多舵轮-WebAPI旋转")]
|
||||
public class WebApiRotateTest : MovementTest
|
||||
{
|
||||
public override void Test()
|
||||
{
|
||||
var url = PilotDefinition.Conf.PlaygroundWebApiUrl;
|
||||
var name = PilotDefinition.Conf.PlaygroundRobotName;
|
||||
var deg = PilotDefinition.Conf.WebApiRotateDeg;
|
||||
|
||||
var pose = PlaygroundWebApi.GetPose(url, name);
|
||||
var ny = pose.YawDeg + deg; // 逆时针为正
|
||||
PlaygroundWebApi.Move(url, name, pose.X, pose.Y, ny);
|
||||
Hedingben.ToastText($"旋转 {deg:f1}° -> {ny:f1}°", "WebApiRotate");
|
||||
}
|
||||
|
||||
public override void TestStop()
|
||||
{
|
||||
}
|
||||
}
|
||||
|
||||
[MovementTest(name = "多舵轮-WebAPI恢复运动")]
|
||||
public class WebApiMotionResumeTest : MovementTest
|
||||
{
|
||||
public override void Test()
|
||||
{
|
||||
var url = PilotDefinition.Conf.PlaygroundWebApiUrl;
|
||||
PlaygroundWebApi.ResumeMotion(url);
|
||||
Hedingben.ToastText("已恢复车辆运动", "WebApiMotion");
|
||||
}
|
||||
|
||||
public override void TestStop()
|
||||
{
|
||||
}
|
||||
}
|
||||
|
||||
[MovementTest(name = "多舵轮-WebAPI暂停运动")]
|
||||
public class WebApiMotionPauseTest : MovementTest
|
||||
{
|
||||
public override void Test()
|
||||
{
|
||||
var url = PilotDefinition.Conf.PlaygroundWebApiUrl;
|
||||
PlaygroundWebApi.PauseMotion(url); // 默认 zero 模式：反馈归零
|
||||
Hedingben.ToastText("已暂停车辆运动 (zero)", "WebApiMotion");
|
||||
}
|
||||
|
||||
public override void TestStop()
|
||||
{
|
||||
}
|
||||
}
|
||||
@@ -1,326 +0,0 @@
|
||||
using ClumsyCore;
|
||||
using ClumsyCore.DTools;
|
||||
using ClumsyCore.Interfaces;
|
||||
using ClumsyCore.Pilot;
|
||||
using ClumsyCore.Sensors;
|
||||
using ClumsyCore.Utilities;
|
||||
using ClumsyDance.ClumsyWalk.Detectors;
|
||||
using ClumsyDance.Sensors;
|
||||
using CommonUsage.Chassis;
|
||||
using FundamentalLib;
|
||||
using MDCSToolBox.Clumsy.Calibration;
|
||||
using MDCSToolBox.Clumsy.HighLevelSecurity;
|
||||
using MDCSToolBox.Clumsy.Movements;
|
||||
using MDCSToolBox.Clumsy.Pilot;
|
||||
using MDCSToolBox.Clumsy.Tracks;
|
||||
using MDCSToolBox.Commons;
|
||||
using MDCSToolBox.Commons.Controllers;
|
||||
using Newtonsoft.Json;
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Drawing;
|
||||
using System.Linq;
|
||||
using System.Net.Http;
|
||||
using System.Numerics;
|
||||
using System.Reflection;
|
||||
using System.Text;
|
||||
using System.Threading;
|
||||
using static ClumsyCore.DTools.Painter;
|
||||
|
||||
namespace MultiWheelC
|
||||
{
|
||||
public class MultiWheelRotateInPlace : MovementDefinition
|
||||
{
|
||||
/// <summary>
|
||||
/// 旋转目标角度
|
||||
/// </summary>
|
||||
public float AngleTarget;
|
||||
|
||||
public float MaxSpeed;
|
||||
|
||||
public Func<float> ThetaReader = () => (float)DetourInterface.getCartLocation().th;
|
||||
|
||||
public MultiWheelChassis Chassis = (MultiWheelChassis)PilotDefinition.Chassis;
|
||||
|
||||
public Func<PIDParams> PidparamsRead = () => new PIDParams() { };
|
||||
|
||||
public PIDController thPid;
|
||||
|
||||
private static float RangeAngle(float theta)
|
||||
{
|
||||
return (float)(theta - Math.Round(theta / 360.0f) * 360);
|
||||
}
|
||||
|
||||
public override IEnumerable<bool> Get()
|
||||
{
|
||||
var targetAngle = RangeAngle(AngleTarget);
|
||||
var p = PidparamsRead();
|
||||
thPid = new PIDController(ThetaReader, p.Kp);
|
||||
thPid.ChangeParameters(p.Kp, p.Ki, p.Kd, p.MaxI, p.DeadZone, p.OutputUpperThreshold, p.SpeedAccPerSec);
|
||||
|
||||
DateTime lastTime = DateTime.Now;
|
||||
while (true)
|
||||
{
|
||||
var s = thPid.GetResponse(targetAngle, true);
|
||||
Console.WriteLine($"s:{s} AngleTarget:{AngleTarget}");
|
||||
Chassis.SendXYThSpeed(0, 0, s);
|
||||
lastTime = DateTime.Now;
|
||||
if (thPid.IsArrived()) break;
|
||||
yield return true;
|
||||
}
|
||||
Chassis.SendXYThSpeed(0, 0, 0);
|
||||
Console.WriteLine($"final rotate to {targetAngle}");
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
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;
|
||||
private PIDController leftpid, rightpid;
|
||||
|
||||
public override IEnumerable<bool> Get()
|
||||
{
|
||||
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 };
|
||||
|
||||
while (true)
|
||||
{
|
||||
var leftspeed = leftpid.GetResponse(LeftClampTarget);
|
||||
var rightspeed = rightpid.GetResponse(RightClampTarget);
|
||||
Console.WriteLine($"left arm speed:{leftspeed} right arm speed:{rightspeed}");
|
||||
PilotDefinition.Self.SpeedLeftArm = leftspeed;
|
||||
PilotDefinition.Self.SpeedRightArm = rightspeed;
|
||||
if (leftpid.IsArrived()) PilotDefinition.Self.SpeedLeftArm = 0;
|
||||
if (rightpid.IsArrived()) PilotDefinition.Self.SpeedRightArm = 0;
|
||||
|
||||
if (leftpid.IsArrived() && rightpid.IsArrived()) break;
|
||||
yield return true;
|
||||
}
|
||||
|
||||
PilotDefinition.Self.SpeedLeftArm = 0;
|
||||
PilotDefinition.Self.SpeedRightArm = 0;
|
||||
Console.WriteLine($"left clamp to target:{LeftClampTarget} right clamp to target:{RightClampTarget}");
|
||||
}
|
||||
}
|
||||
|
||||
public class Sleep : MovementDefinition
|
||||
{
|
||||
public float Second = 2;
|
||||
public override IEnumerable<bool> Get()
|
||||
{
|
||||
var start = DateTime.Now;
|
||||
while ((DateTime.Now-start).TotalSeconds<Second)
|
||||
{
|
||||
yield return true;
|
||||
Thread.Sleep(1000);
|
||||
Console.WriteLine("Sleep");
|
||||
}
|
||||
|
||||
yield return false;
|
||||
}
|
||||
}
|
||||
|
||||
//直线行走基于detour
|
||||
public class LineTracking1 : MovementDefinition
|
||||
{
|
||||
public float LineDistance = 1000f;
|
||||
public int SrcId = -1;
|
||||
public int DstId = -1;
|
||||
public Action<int> LeaveSrcFunction = null;
|
||||
public Painter painter = UI.GetPainter("Line", false);
|
||||
public override IEnumerable<bool> Get()
|
||||
{
|
||||
var curpose = DetourInterface.getCartLocation();
|
||||
Console.WriteLine($"curpose.th:{curpose.th}");
|
||||
var src = new Vector2((float)curpose.x, (float)curpose.y);
|
||||
var dst = new Vector2((float)curpose.x + LineDistance * (float)Math.Cos(curpose.th),
|
||||
(float)curpose.y + LineDistance * (float)Math.Sin(curpose.th));
|
||||
Console.WriteLine($"src:{src.X} {src.Y}");
|
||||
Console.WriteLine($"dst:{dst.X} {dst.Y}");
|
||||
painter.DrawLine(Color.Green, src.X, src.Y, dst.X, dst.Y, width: 3);
|
||||
|
||||
var tracker = new ChassisController().Get();
|
||||
var linePath = new LineTrack(src, dst) { CarDirectionBias = LineDistance > 0 ? 0 : 180 };
|
||||
tracker.AddTrack(linePath);
|
||||
var _dt = new DriveTask(tracker.Track());
|
||||
_dt.Wait();
|
||||
if (SrcId != -1 && LeaveSrcFunction != null)
|
||||
{
|
||||
LeaveSrcFunction(SrcId);
|
||||
DLog.Log($"释放放车点{SrcId}", "TireFollowing");
|
||||
}
|
||||
yield return false;
|
||||
}
|
||||
}
|
||||
|
||||
//在世界坐标系下，从路径起点追踪到终点并停车
|
||||
public class DstTracker : MovementDefinition
|
||||
{
|
||||
public Vector2 Src;
|
||||
public Vector2 Dst;
|
||||
public float CarDirectionBias = 0f;
|
||||
public Painter Painter = UI.GetPainter("DstTracker");
|
||||
|
||||
public float InitialSendSpeed = 0;
|
||||
|
||||
public override IEnumerable<bool> Get()
|
||||
{
|
||||
Console.WriteLine($"DstTracker src:({Src.X:F2}, {Src.Y:F2}) dst:({Dst.X:F2}, {Dst.Y:F2})");
|
||||
Painter.DrawLine(Color.Cyan, Src.X, Src.Y, Dst.X, Dst.Y, width: 3);
|
||||
|
||||
var tracker = new ChassisController().Get();
|
||||
|
||||
if (InitialSendSpeed != 0)
|
||||
{
|
||||
tracker.SkipInitialRotate = true;
|
||||
tracker.InitialSendSpeed = InitialSendSpeed;
|
||||
}
|
||||
|
||||
var linePath = new LineTrack(Src, Dst) { CarDirectionBias = CarDirectionBias, Speed = PilotDefinition.Conf.DstTrackerMaxSpeed };
|
||||
tracker.AddTrack(linePath);
|
||||
var task = new DriveTask(tracker.Track());
|
||||
task.Wait();
|
||||
|
||||
// 到点后兜底停车
|
||||
var chassis = (MultiWheelChassis)PilotDefinition.Chassis;
|
||||
chassis.SendXYThSpeed(0f, 0f, 0f);
|
||||
yield return false;
|
||||
}
|
||||
}
|
||||
|
||||
//直线行走基于轮里程
|
||||
public class LineTracking : MovementDefinition
|
||||
{
|
||||
public float Target;
|
||||
public float MaxSpeed = PilotDefinition.Conf.LineTrackMaxSpeed;
|
||||
public float Kp = PilotDefinition.Conf.LineTrackKp;
|
||||
public float Ki = PilotDefinition.Conf.LineTrackKi;
|
||||
public float Kd = PilotDefinition.Conf.LineTrackKd;
|
||||
public float DeadZone = PilotDefinition.Conf.LineTrackDeadZone;
|
||||
public int SrcId = -1;
|
||||
public int DstId = -1;
|
||||
public Action<int> LeaveSrcFunction = null;
|
||||
private PIDController pid;
|
||||
// 末段衔接：接近目标后不再让 PID 把速度降到 0，保留一个接力速度给后续动作接管
|
||||
public bool EnableHandover = false;
|
||||
public float HandoverDistance = 80f; // mm
|
||||
public float HandoverSpeed = 0.15f; // m/s
|
||||
|
||||
public override IEnumerable<bool> Get()
|
||||
{
|
||||
|
||||
pid = new PIDController(() =>
|
||||
(PilotDefinition.Self.LFLActualPos + PilotDefinition.Self.LFRActualPos) / 2,
|
||||
Kp, Ki, Kd, 0, DeadZone, MaxSpeed)
|
||||
{ SpeedAccPerSec = MaxSpeed / 2f };
|
||||
|
||||
var chassis = (MultiWheelChassis)PilotDefinition.Chassis;
|
||||
//chassis.SetOriginBias(0, 0, 0);
|
||||
DLog.Log($"直线行驶距离:{Target}", "TireFollowing");
|
||||
while (true)
|
||||
{
|
||||
var current = (PilotDefinition.Self.LFLActualPos + PilotDefinition.Self.LFRActualPos) / 2;
|
||||
var remain = Target - current;
|
||||
if (EnableHandover && Math.Abs(remain) <= Math.Max(1f, HandoverDistance))
|
||||
{
|
||||
var handoverSign = Math.Sign(remain);
|
||||
if (handoverSign == 0) handoverSign = 1;
|
||||
var handoverSpeed = Math.Abs(HandoverSpeed) * handoverSign;
|
||||
Console.WriteLine($"handover speed: {handoverSpeed:F3}, remain: {remain:F2}");
|
||||
chassis.SendXYThSpeed(handoverSpeed, 0, 0);
|
||||
// 保留一拍接力速度，让后续 DstTracker 无缝接管
|
||||
yield return true;
|
||||
break;
|
||||
}
|
||||
var speed = pid.GetResponse(Target);
|
||||
Console.WriteLine($"output: {speed} current: {(PilotDefinition.Self.LFLActualPos + PilotDefinition.Self.LFRActualPos) / 2}");
|
||||
|
||||
chassis.SendXYThSpeed(speed, 0, 0);
|
||||
|
||||
if (pid.IsArrived()) break;
|
||||
yield return true;
|
||||
}
|
||||
if (SrcId != -1 && LeaveSrcFunction != null)
|
||||
{
|
||||
LeaveSrcFunction(SrcId);
|
||||
DLog.Log($"释放放车点{SrcId}", "TireFollowing");
|
||||
}
|
||||
yield return false;
|
||||
}
|
||||
}
|
||||
|
||||
public class DriverAble : MovementDefinition
|
||||
{
|
||||
public int WaitTimeoutMs = 2000;
|
||||
public int PollIntervalMs = 50;
|
||||
|
||||
public override IEnumerable<bool> Get()
|
||||
{
|
||||
Console.WriteLine("驱动器上使能");
|
||||
PilotDefinition.Self.ResetFromC = true;
|
||||
|
||||
var start = DateTime.Now;
|
||||
var timeoutMs = Math.Max(0, WaitTimeoutMs);
|
||||
var pollMs = Math.Max(1, PollIntervalMs);
|
||||
var success = PilotDefinition.Self.WheelAbleState;
|
||||
while (!success && (DateTime.Now - start).TotalMilliseconds < timeoutMs)
|
||||
{
|
||||
Thread.Sleep(pollMs);
|
||||
success = PilotDefinition.Self.WheelAbleState;
|
||||
if (!success) yield return true;
|
||||
}
|
||||
|
||||
PilotDefinition.Self.ResetFromC = false;
|
||||
if (success)
|
||||
Console.WriteLine($"驱动器上使能完成，WheelAbleState={PilotDefinition.Self.WheelAbleState}");
|
||||
else
|
||||
Console.WriteLine($"驱动器上使能超时，WheelAbleState={PilotDefinition.Self.WheelAbleState}，等待{timeoutMs}ms");
|
||||
yield return false;
|
||||
}
|
||||
}
|
||||
|
||||
public class DriverDisable : MovementDefinition
|
||||
{
|
||||
public int WaitTimeoutMs = 3000;
|
||||
public int PollIntervalMs = 20;
|
||||
|
||||
public override IEnumerable<bool> Get()
|
||||
{
|
||||
Console.WriteLine("驱动器下使能");
|
||||
PilotDefinition.Self.DisableFromC = true;
|
||||
|
||||
var start = DateTime.Now;
|
||||
var timeoutMs = Math.Max(0, WaitTimeoutMs);
|
||||
var pollMs = Math.Max(1, PollIntervalMs);
|
||||
var success = !PilotDefinition.Self.WheelAbleState;
|
||||
while (!success && (DateTime.Now - start).TotalMilliseconds < timeoutMs)
|
||||
{
|
||||
Thread.Sleep(pollMs);
|
||||
success = !PilotDefinition.Self.WheelAbleState;
|
||||
if (!success) yield return true;
|
||||
}
|
||||
|
||||
PilotDefinition.Self.DisableFromC = false;
|
||||
if (success)
|
||||
Console.WriteLine($"驱动器下使能完成，WheelAbleState={PilotDefinition.Self.WheelAbleState}");
|
||||
else
|
||||
Console.WriteLine($"驱动器下使能超时，WheelAbleState={PilotDefinition.Self.WheelAbleState}，等待{timeoutMs}ms");
|
||||
yield return false;
|
||||
}
|
||||
}
|
||||
}
|
||||
File diff suppressed because it is too large
Load Diff
@@ -1,81 +0,0 @@
|
||||
using System;
|
||||
using System.Net.Http;
|
||||
using System.Text;
|
||||
using Newtonsoft.Json;
|
||||
using Newtonsoft.Json.Linq;
|
||||
|
||||
namespace MultiWheelC;
|
||||
|
||||
/// <summary>
|
||||
/// Playground 仿真器 HTTP Web API 轻量客户端：查询小车位姿、瞬移小车。
|
||||
/// 服务端实现见 Playground/Web/PlaygroundWebApi.cs，默认监听 http://localhost:18090。
|
||||
/// 坐标单位 mm，朝向 yawDeg 单位为度，世界坐标系与场景 JSON 一致。
|
||||
/// </summary>
|
||||
public static class PlaygroundWebApi
|
||||
{
|
||||
// 禁用系统代理：本机 Playground 走 localhost，若经系统代理(如 127.0.0.1:7890)会连接失败。
|
||||
private static readonly HttpClient Http = new HttpClient(new HttpClientHandler { UseProxy = false })
|
||||
{
|
||||
Timeout = TimeSpan.FromSeconds(3)
|
||||
};
|
||||
|
||||
public struct Pose
|
||||
{
|
||||
public float X;
|
||||
public float Y;
|
||||
public float YawDeg;
|
||||
}
|
||||
|
||||
/// <summary>查询单台小车的世界位姿。GET /api/robots/{name}。</summary>
|
||||
public static Pose GetPose(string baseUrl, string robotName)
|
||||
{
|
||||
var url = $"{baseUrl.TrimEnd('/')}/api/robots/{Uri.EscapeDataString(robotName)}";
|
||||
var json = Http.GetStringAsync(url).GetAwaiter().GetResult();
|
||||
var o = JObject.Parse(json);
|
||||
return new Pose
|
||||
{
|
||||
X = o.Value<float>("x"),
|
||||
Y = o.Value<float>("y"),
|
||||
YawDeg = o.Value<float>("yawDeg")
|
||||
};
|
||||
}
|
||||
|
||||
/// <summary>将小车瞬移到目标世界位姿。POST /api/robots/{name}/move。</summary>
|
||||
public static void Move(string baseUrl, string robotName, float x, float y, float yawDeg)
|
||||
{
|
||||
var url = $"{baseUrl.TrimEnd('/')}/api/robots/{Uri.EscapeDataString(robotName)}/move";
|
||||
var body = JsonConvert.SerializeObject(new { x, y, yaw = yawDeg, stop = true });
|
||||
using var content = new StringContent(body, Encoding.UTF8, "application/json");
|
||||
var resp = Http.PostAsync(url, content).GetAwaiter().GetResult();
|
||||
resp.EnsureSuccessStatusCode();
|
||||
}
|
||||
|
||||
/// <summary>查询车辆运动是否启用（暂停时为 false）。GET /api/motion。</summary>
|
||||
public static bool MotionEnabled(string baseUrl)
|
||||
{
|
||||
var url = $"{baseUrl.TrimEnd('/')}/api/motion";
|
||||
var json = Http.GetStringAsync(url).GetAwaiter().GetResult();
|
||||
return JObject.Parse(json).Value<bool>("motionEnabled");
|
||||
}
|
||||
|
||||
/// <summary>恢复车辆运动。POST /api/motion/resume。</summary>
|
||||
public static void ResumeMotion(string baseUrl)
|
||||
{
|
||||
var url = $"{baseUrl.TrimEnd('/')}/api/motion/resume";
|
||||
var resp = Http.PostAsync(url, null).GetAwaiter().GetResult();
|
||||
resp.EnsureSuccessStatusCode();
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 暂停车辆运动（仅冻结运动，不停止仿真；传感器继续扫描）。POST /api/motion/pause。
|
||||
/// feedback: "zero"(默认,反馈归零) / "none"(不上报) / "hold"(保留暂停瞬间值)。
|
||||
/// </summary>
|
||||
public static void PauseMotion(string baseUrl, string feedback = "zero")
|
||||
{
|
||||
var url = $"{baseUrl.TrimEnd('/')}/api/motion/pause";
|
||||
var body = JsonConvert.SerializeObject(new { feedback });
|
||||
using var content = new StringContent(body, Encoding.UTF8, "application/json");
|
||||
var resp = Http.PostAsync(url, content).GetAwaiter().GetResult();
|
||||
resp.EnsureSuccessStatusCode();
|
||||
}
|
||||
}
|
||||
@@ -1,511 +0,0 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Drawing;
|
||||
using System.Linq;
|
||||
using System.Numerics;
|
||||
using System.Reflection;
|
||||
using System.Text;
|
||||
using System.Threading;
|
||||
using ClumsyCore;
|
||||
using ClumsyCore.DTools;
|
||||
using ClumsyCore.Interfaces;
|
||||
using ClumsyCore.Pilot;
|
||||
using ClumsyCore.Utilities;
|
||||
using ClumsyDance.ClumsyWalk.Detectors;
|
||||
using CommonUsage.Chassis;
|
||||
using FundamentalLib;
|
||||
using MDCSToolBox;
|
||||
using MDCSToolBox.Clumsy.Calibration;
|
||||
using MDCSToolBox.Clumsy.MotionControllers;
|
||||
using MDCSToolBox.Clumsy.Movements;
|
||||
using MDCSToolBox.Clumsy.Pilot;
|
||||
using MDCSToolBox.Clumsy.Tracks;
|
||||
using MDCSToolBox.Commons.Controllers;
|
||||
using static ClumsyCore.DTools.Painter;
|
||||
using LineSegment = ClumsyCore.Utilities.LineSegment;
|
||||
|
||||
namespace MultiWheelC
|
||||
{
|
||||
public class TireFollowing : MovementDefinition
|
||||
{
|
||||
public Func<AbstractGeometricController> GetController;
|
||||
|
||||
/// <summary>
|
||||
/// 车辆方向
|
||||
/// </summary>
|
||||
public float CarDirection = 0;
|
||||
|
||||
/// <summary>
|
||||
/// 停止距离
|
||||
/// </summary>
|
||||
//public float FinishDistance = 1000;
|
||||
|
||||
/// <summary>
|
||||
/// 减速距离
|
||||
/// </summary>
|
||||
public float SlowDistance = 1000;
|
||||
|
||||
/// <summary>
|
||||
/// 最大速度
|
||||
/// </summary>
|
||||
public float MaxSpeed = 0.3f;
|
||||
|
||||
// 末段衔接：接近盲走终点时给非零速度，供后续动作连续接管
|
||||
public bool EnableHandover = false;
|
||||
public float HandoverDistance = 200f; // mm
|
||||
public float HandoverSpeed = 0.2f; // m/s
|
||||
|
||||
/// <summary>
|
||||
/// 钻轮胎数量
|
||||
/// </summary>
|
||||
public int TireNum = 1;
|
||||
|
||||
/// <summary>
|
||||
/// 盲走角度偏移
|
||||
/// </summary>
|
||||
public float WalkBlindTh = -1f;
|
||||
|
||||
/// <summary>
|
||||
/// 是否检测到目标
|
||||
/// </summary>
|
||||
public bool NoTarget = false;
|
||||
|
||||
public float GuessRangeX;
|
||||
|
||||
public float GuessRangeY;
|
||||
|
||||
/// <summary>
|
||||
/// 检测器定义
|
||||
/// </summary>
|
||||
public class DetectorDefinition
|
||||
{
|
||||
/// <summary>
|
||||
/// 开始检测距离
|
||||
/// </summary>
|
||||
public float StartGuessingX;
|
||||
|
||||
/// <summary>
|
||||
/// 开始检测距离
|
||||
/// </summary>
|
||||
public float StartGuessingY;
|
||||
|
||||
/// <summary>
|
||||
/// 检测函数
|
||||
/// </summary>
|
||||
public Func<float, float, List<DetectFilter>, LineSegment> DetectFunction = null;
|
||||
|
||||
public Action<int> LeaveSrcFunction = null;
|
||||
|
||||
public int SrcId = -1;
|
||||
public int DstId = -1;
|
||||
|
||||
/// <summary>
|
||||
/// 路径偏移
|
||||
/// </summary>
|
||||
public Tuple<float, float, float> PathTransformation = Tuple.Create(0f, 0f, 0f);
|
||||
|
||||
public float PathTransformationAnchorDistance = 0f;
|
||||
|
||||
/// <summary>
|
||||
/// 切换条件
|
||||
/// </summary>
|
||||
public Func<float, bool> SwitchWalkBlindCondition = null;
|
||||
|
||||
/// <summary>
|
||||
/// 盲走停止距离
|
||||
/// </summary>
|
||||
public Func<float, bool> FinishWalkBlindCondition = null;
|
||||
}
|
||||
|
||||
public Func<bool> FinishCondition;
|
||||
|
||||
/// <summary>
|
||||
/// 多个检测器列表
|
||||
/// </summary>
|
||||
public List<DetectorDefinition> detectors = null;
|
||||
|
||||
private Painter _painter;
|
||||
private List<float> _remainDistanceList = new List<float>();
|
||||
private List<float> _remainAngleList = new List<float>();
|
||||
private List<float> _targetYList = new List<float>();
|
||||
|
||||
private List<DetectFilter> SetFilters(float guessCenterX, float guessCenterY)
|
||||
{
|
||||
var painter = UI.GetPainter("GeneralFollowing.SetFilters", false);
|
||||
painter.Clear();
|
||||
painter.Clear(3000);
|
||||
|
||||
var box = new Vector2[]
|
||||
{
|
||||
new (guessCenterX - GuessRangeX, guessCenterY - GuessRangeY),
|
||||
new (guessCenterX + GuessRangeX, guessCenterY - GuessRangeY),
|
||||
new (guessCenterX + GuessRangeX, guessCenterY + GuessRangeY),
|
||||
new (guessCenterX - GuessRangeX, guessCenterY + GuessRangeY),
|
||||
};
|
||||
|
||||
for (var i = 0; i < box.Length; ++i)
|
||||
painter.DrawLine(Color.DarkOliveGreen, box[i], box[(i + 1) % 4]);
|
||||
|
||||
// PC filter in car coordinate frame
|
||||
return new List<DetectFilter>()
|
||||
{
|
||||
new(CoordinateSystem.Car2D,
|
||||
p => LessMath.IsPointInPolygon4(
|
||||
box.Select(v => new PointF(v.X, v.Y)).ToArray(), new PointF(p.X, p.Y))),
|
||||
};
|
||||
}
|
||||
|
||||
public void Stop()
|
||||
{
|
||||
_dt?.Stop();
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
///计算车体中心的位移和角度增量
|
||||
/// </summary>
|
||||
/// <param name="a">a轮在车体坐标系下位置</param>
|
||||
/// <param name="va">a轮在车体坐标系下位移增量</param>
|
||||
/// <param name="b">b轮在车体坐标系下位置</param>
|
||||
/// <param name="vb">b轮在车体坐标系下位移增量</param>
|
||||
/// <returns></returns>
|
||||
private static (float, float, float) CenterMoveFromPoints(Vector2 a,
|
||||
Vector2 aDelta,
|
||||
Vector2 b,
|
||||
Vector2 bDelta)
|
||||
{
|
||||
float th_x = 0, th_y = 0, th = 0, x = 0, y = 0;
|
||||
var eps = 0.0000001;
|
||||
if (Math.Abs(a.Y - b.Y) > eps)
|
||||
{
|
||||
th_x = (aDelta.X - bDelta.X) / (b.Y - a.Y);
|
||||
}
|
||||
if (Math.Abs(a.X - b.X) > eps)
|
||||
{
|
||||
th_y = (aDelta.Y - bDelta.Y) / (a.X - b.X);
|
||||
}
|
||||
th = th_x == 0 ? th_y : th_x;
|
||||
|
||||
x = (aDelta.X + bDelta.X) / 2f - (a.Y - b.Y) / 2f * th;
|
||||
|
||||
y = (aDelta.Y + bDelta.Y) / 2f + (a.X - b.X) / 2f * th;
|
||||
|
||||
return (x, y, th);
|
||||
}
|
||||
|
||||
|
||||
public override IEnumerable<bool> Get()
|
||||
{
|
||||
_painter = UI.GetPainter("GeneralFollowing", false);
|
||||
var lastDetectX = detectors[0].StartGuessingX;
|
||||
var lastDetectY = detectors[0].StartGuessingY;
|
||||
var detectorIndex = 0;
|
||||
|
||||
var controller = (MultiWheelGeometricController)GetController.Invoke();
|
||||
controller.BaseSpeed = MaxSpeed;
|
||||
controller.FinishDistance = float.MinValue;
|
||||
controller.FirstThAccuracy = 999;
|
||||
_dt = new DriveTask(controller.Track(true, CoordinateSystem.Car2D));
|
||||
void HardStop()
|
||||
{
|
||||
_dt?.Stop();
|
||||
((MultiWheelChassis)PilotDefinition.Chassis).DriveStop();
|
||||
DLog.Log($"Hard Stop!", "TireFollowing");
|
||||
}
|
||||
float WalkBlindCarPathDstX = -1f, WalkBlindCarPathDstY = -1f, WalkBlindCarPathDstTh = -1f;
|
||||
bool WalkBlindStage1 = false, WalkBlindStage2 = false;
|
||||
var angle2target = -1f;
|
||||
float _lastLFLEncoder = -1, _lastLFREncoder = -1, _lastRFLEncoder = -1, _lastRFREncoder = -1;
|
||||
float _lastLRLEncoder = -1, _lastLRREncoder = -1, _lastRRLEncoder = -1, _lastRRREncoder = -1;
|
||||
|
||||
(float, float, float) GetCurrentPos2Dst(float lastX, float lastY, float lastTh)
|
||||
{
|
||||
// Read current encoders
|
||||
var curLFLEncoder = PilotDefinition.Self.LFLActualPos;
|
||||
var curLFREncoder = PilotDefinition.Self.LFRActualPos;
|
||||
var curRFLEncoder = PilotDefinition.Self.RFLActualPos;
|
||||
var curRFREncoder = PilotDefinition.Self.RFRActualPos;
|
||||
var curLRLEncoder = PilotDefinition.Self.LRLActualPos;
|
||||
var curLRREncoder = PilotDefinition.Self.LRRActualPos;
|
||||
var curRRLEncoder = PilotDefinition.Self.RRLActualPos;
|
||||
var curRRREncoder = PilotDefinition.Self.RRRActualPos;
|
||||
|
||||
// Average delta per wheel pair (LF, LR, RF, RR)
|
||||
var lfDelta = (curLFLEncoder - _lastLFLEncoder + curLFREncoder - _lastLFREncoder) / 2f;
|
||||
var lrDelta = (curLRLEncoder - _lastLRLEncoder + curLRREncoder - _lastLRREncoder) / 2f;
|
||||
var rfDelta = (curRFLEncoder - _lastRFLEncoder + curRFREncoder - _lastRFREncoder) / 2f;
|
||||
var rrDelta = (curRRLEncoder - _lastRRLEncoder + curRRREncoder - _lastRRREncoder) / 2f;
|
||||
var deltaList = new List<float> { lfDelta, lrDelta, rfDelta, rrDelta };
|
||||
|
||||
var xs = new List<float>();
|
||||
var ys = new List<float>();
|
||||
var ths = new List<float>();
|
||||
var chassis = (MultiWheelChassis)BasicPilotBase.Chassis;
|
||||
var steerWheels = chassis.GetSteerWheels();
|
||||
for (var i = 0; i < steerWheels.Count; ++i)
|
||||
{
|
||||
var sw1 = steerWheels[i];
|
||||
var a = sw1.Position;
|
||||
var tha = sw1.ReadAngle() / 180f * (float)Math.PI;
|
||||
var deltaa = deltaList[i];
|
||||
var va = new Vector2(deltaa * (float)Math.Cos(tha), deltaa * (float)Math.Sin(tha));
|
||||
for (var j = i + 1; j < steerWheels.Count; ++j)
|
||||
{
|
||||
var sw2 = steerWheels[j];
|
||||
var b = sw2.Position;
|
||||
var thb = sw2.ReadAngle() / 180f * (float)Math.PI;
|
||||
var deltab = deltaList[j];
|
||||
var vb = new Vector2(deltab * (float)Math.Cos(thb), deltab * (float)Math.Sin(thb));
|
||||
var (tempx, tempy, tempth) = CenterMoveFromPoints(a, va, b, vb);
|
||||
Hedingben.ToastText($"{tempx:f2} {tempy:f2} {tempth / Math.PI * 180f:f2} ", $"{i}_{j}");
|
||||
xs.Add(tempx);
|
||||
ys.Add(tempy);
|
||||
ths.Add(tempth);
|
||||
}
|
||||
}
|
||||
var x = xs.Average();
|
||||
var y = ys.Average();
|
||||
var Th = ths.Average() / (float)Math.PI * 180;
|
||||
var moveTup = Tuple.Create(x, y, Th);
|
||||
var moved = MathTools.SolveTransform2D(MathTools.SolveTransform2D(Tuple.Create(lastX, lastY, lastTh), moveTup), Tuple.Create(0f, 0f, 0f));
|
||||
|
||||
_lastLFLEncoder = curLFLEncoder;
|
||||
_lastLFREncoder = curLFREncoder;
|
||||
_lastRFLEncoder = curRFLEncoder;
|
||||
_lastRFREncoder = curRFREncoder;
|
||||
_lastLRLEncoder = curLRLEncoder;
|
||||
_lastLRREncoder = curLRREncoder;
|
||||
_lastRRLEncoder = curRRLEncoder;
|
||||
_lastRRREncoder = curRRREncoder;
|
||||
return (moved.Item1, moved.Item2, moved.Item3);
|
||||
}
|
||||
|
||||
while (true)
|
||||
{
|
||||
if (detectorIndex > detectors.Count - 1)
|
||||
throw new Exception("detector index out of range!");
|
||||
|
||||
_painter.Clear();
|
||||
if (WalkBlindStage1 || WalkBlindStage2)
|
||||
{
|
||||
//第二次盲走时或只钻一个轮胎时
|
||||
if (WalkBlindStage2 || detectors.Count == 1 || TireNum == 1)
|
||||
{
|
||||
//controller.FinishDistance = 10f;
|
||||
controller.SlowDistance = SlowDistance;
|
||||
controller.SlowingPow = 0.7f;
|
||||
}
|
||||
if (EnableHandover)
|
||||
{
|
||||
controller.SlowDistance = float.MinValue;
|
||||
controller.FinishSpeed = 0.2f;
|
||||
controller.FinishDistance = 50;
|
||||
}
|
||||
(WalkBlindCarPathDstX, WalkBlindCarPathDstY, WalkBlindCarPathDstTh) = GetCurrentPos2Dst(WalkBlindCarPathDstX, WalkBlindCarPathDstY, WalkBlindCarPathDstTh);
|
||||
var walkBlindPathEnd = Tuple.Create(WalkBlindCarPathDstX, WalkBlindCarPathDstY, WalkBlindCarPathDstTh);
|
||||
var walkBlindPathStart = LessMath.Transform2D(walkBlindPathEnd, Tuple.Create(CarDirection == 0 ? -3000f : 3000f, 0f, 0f));
|
||||
var walkBlindPathDst = new Vector2(WalkBlindCarPathDstX, WalkBlindCarPathDstY);
|
||||
var walkBlindPathSrc = new Vector2(walkBlindPathStart.Item1, walkBlindPathStart.Item2);
|
||||
var walkBlindPath = new LineSegment(walkBlindPathSrc, walkBlindPathDst);
|
||||
|
||||
DLog.Log($"盲走目标点:{walkBlindPath.Src.X:F2} {walkBlindPath.Src.Y:F2} {walkBlindPath.Dst.X:F2} {walkBlindPath.Dst.Y:F2}", "TireFollowing");
|
||||
_painter.DrawDot(Color.Purple, walkBlindPathDst, sz: 3);
|
||||
_painter.DrawLine(Color.GreenYellow, walkBlindPath.Src, walkBlindPath.Dst, endArrow: true, width: 2);
|
||||
|
||||
var track = new LineTrack(walkBlindPath.Src, walkBlindPath.Dst);
|
||||
track.CarDirectionBias = CarDirection;
|
||||
controller.UpdateTracks(new List<AbstractTrack> { track });
|
||||
var rd = (float)LessMath.PerpendicularPosition(0, 0, walkBlindPath.Dst.X, walkBlindPath.Dst.Y,
|
||||
walkBlindPath.Src.X, walkBlindPath.Src.Y);
|
||||
_remainDistanceList.Add(rd);
|
||||
while (_remainDistanceList.Count > 3) _remainDistanceList.RemoveAt(0);
|
||||
rd = _remainDistanceList.Average();
|
||||
DLog.Log($"盲走投影点剩余距离:{rd:0.0} ", "TireFollowing");
|
||||
// 检查是否达到盲走结束条件
|
||||
if (detectors[detectorIndex].FinishWalkBlindCondition(rd))
|
||||
{
|
||||
if (WalkBlindStage1)
|
||||
{
|
||||
DLog.Log("达到第一次盲走停止距离，停下或开始钻第二对轮胎", "TireFollowing");
|
||||
//if (detectors[detectorIndex].DstId != -1 && detectors[detectorIndex].LeaveSrcFunction != null)
|
||||
//{
|
||||
// detectors[detectorIndex].LeaveSrcFunction(detectors[detectorIndex].DstId);
|
||||
// DLog.Log($"释放取车点{detectors[detectorIndex].DstId}", "TireFollowing");
|
||||
//}
|
||||
WalkBlindStage1 = false;
|
||||
_remainAngleList.Clear();
|
||||
_remainDistanceList.Clear();
|
||||
detectorIndex++;
|
||||
if ((detectors.Count == 1 || TireNum == 1) && !EnableHandover)
|
||||
{
|
||||
HardStop();
|
||||
yield return false;
|
||||
}
|
||||
}
|
||||
else if (WalkBlindStage2)
|
||||
{
|
||||
DLog.Log("达到第二对轮胎处，停止移动", "TireFollowing");
|
||||
if (!EnableHandover)
|
||||
{
|
||||
HardStop();
|
||||
}
|
||||
yield return false;
|
||||
}
|
||||
}
|
||||
yield return true;
|
||||
continue;
|
||||
}
|
||||
|
||||
var target = detectors[detectorIndex].DetectFunction(CarDirection, lastDetectX,
|
||||
SetFilters(lastDetectX, lastDetectY));
|
||||
|
||||
if (target == null)
|
||||
{
|
||||
DLog.Log("无目标，等待下一帧", "TireFollowing");
|
||||
controller.FirstRotateMaxSpeed = 0;
|
||||
yield return true;
|
||||
continue;
|
||||
}
|
||||
else controller.FirstRotateMaxSpeed = 5;
|
||||
|
||||
var targetAngle = CalculateAngle2YAxis(target.Src, target.Dst);
|
||||
var targetPos = new Vector2((target.Src.X + target.Dst.X) / 2f, (target.Src.Y + target.Dst.Y) / 2f);
|
||||
var dis2target = (float)Math.Sqrt(Math.Pow(targetPos.X, 2) + Math.Pow(targetPos.Y, 2));
|
||||
//距离较近以后角度容易跳变
|
||||
if (dis2target < PilotDefinition.Conf.TireFollowingCloseDistance && Math.Abs(targetAngle) > PilotDefinition.Conf.TireFollowingAngleIgnoreThr)
|
||||
{
|
||||
yield return true;
|
||||
continue;
|
||||
}
|
||||
else _remainAngleList.Add(targetAngle);
|
||||
while (_remainAngleList.Count > 10) _remainAngleList.RemoveAt(0);
|
||||
angle2target = _remainAngleList.Average();
|
||||
var distanceLabelPos = targetPos / 2f;
|
||||
_painter.DrawLine(Color.Cyan, Vector2.Zero, targetPos, width: 2);
|
||||
_painter.DrawText(Color.Yellow, $"{dis2target:F3}", distanceLabelPos.X, distanceLabelPos.Y);
|
||||
|
||||
var path = DetectorHelper.GetApproachPath(target, CoordinateSystem.Car2D, pathLen: 3000,
|
||||
bias: detectors[detectorIndex].PathTransformation,
|
||||
biasAnchorDistance: detectors[detectorIndex].PathTransformationAnchorDistance);
|
||||
|
||||
if (path == null)
|
||||
{
|
||||
DLog.Log("no path!", "TireFollowing");
|
||||
NoTarget = true;
|
||||
}
|
||||
else
|
||||
{
|
||||
lastDetectX = ((target.Src + target.Dst) / 2f).X;
|
||||
lastDetectY = ((target.Src + target.Dst) / 2f).Y;
|
||||
|
||||
var currentY = path.CarPath.Dst.Y;
|
||||
if (Math.Abs(targetAngle) < PilotDefinition.Conf.TireFollowingAngleIgnoreThr &&
|
||||
dis2target < PilotDefinition.Conf.TireFollowingCloseDistance)
|
||||
{
|
||||
_targetYList.Add(currentY);
|
||||
while (_targetYList.Count > PilotDefinition.Conf.TireFollowingYAverageFrameCount) _targetYList.RemoveAt(0);
|
||||
}
|
||||
|
||||
var trackDstY = _targetYList.Count > 0 ? _targetYList.Average() : currentY;
|
||||
Hedingben.ToastText($"target Y:{_targetYList.Count} {trackDstY}", "target Y");
|
||||
|
||||
var trackDst = new Vector2(path.CarPath.Dst.X, trackDstY);
|
||||
_painter.DrawLine(Color.GreenYellow, path.CarPath.Src, trackDst, endArrow: true);
|
||||
|
||||
var rd = (float)LessMath.PerpendicularPosition(0, 0, trackDst.X, trackDst.Y,
|
||||
path.CarPath.Src.X, path.CarPath.Src.Y);
|
||||
_remainDistanceList.Add(rd);
|
||||
while (_remainDistanceList.Count > 3) _remainDistanceList.RemoveAt(0);
|
||||
rd = _remainDistanceList.Average();
|
||||
_painter.DrawText(Color.Green, $"{rd:F3}", distanceLabelPos.X, distanceLabelPos.Y - 200);
|
||||
if(rd < PilotDefinition.Conf.TireFollowingReleaseDistance)
|
||||
{
|
||||
if (detectors[detectorIndex].SrcId != -1 && detectors[detectorIndex].LeaveSrcFunction != null)
|
||||
{
|
||||
detectors[detectorIndex].LeaveSrcFunction(detectors[detectorIndex].SrcId);
|
||||
DLog.Log($"释放预取车点{detectors[detectorIndex].SrcId}", "TireFollowing");
|
||||
}
|
||||
}
|
||||
|
||||
if (detectorIndex < detectors.Count - 1)
|
||||
{
|
||||
controller.SlowDistance = 1;
|
||||
if (detectors[detectorIndex].SwitchWalkBlindCondition(rd))
|
||||
{
|
||||
WalkBlindStage1 = true;
|
||||
//if (detectors[detectorIndex].SrcId != -1 && detectors[detectorIndex].LeaveSrcFunction != null)
|
||||
//{
|
||||
// detectors[detectorIndex].LeaveSrcFunction(detectors[detectorIndex].SrcId);
|
||||
// DLog.Log($"释放预取车点{detectors[detectorIndex].SrcId}", "TireFollowing");
|
||||
//}
|
||||
WalkBlindCarPathDstX = trackDst.X;
|
||||
WalkBlindCarPathDstY = trackDst.Y;
|
||||
WalkBlindCarPathDstTh = angle2target + WalkBlindTh;
|
||||
DLog.Log($"切换至第一次盲走时刻目标点:{WalkBlindCarPathDstX:F2} " +
|
||||
$"{WalkBlindCarPathDstY:F2} " +
|
||||
$"{WalkBlindCarPathDstTh:F2}", "TireFollowing");
|
||||
_lastLFLEncoder = PilotDefinition.Self.LFLActualPos;
|
||||
_lastLFREncoder = PilotDefinition.Self.LFRActualPos;
|
||||
_lastRFLEncoder = PilotDefinition.Self.RFLActualPos;
|
||||
_lastRFREncoder = PilotDefinition.Self.RFRActualPos;
|
||||
_lastLRLEncoder = PilotDefinition.Self.LRLActualPos;
|
||||
_lastLRREncoder = PilotDefinition.Self.LRRActualPos;
|
||||
_lastRRLEncoder = PilotDefinition.Self.RRLActualPos;
|
||||
_lastRRREncoder = PilotDefinition.Self.RRRActualPos;
|
||||
_remainDistanceList.Clear();
|
||||
_targetYList.Clear();
|
||||
lastDetectX = detectors[detectorIndex + 1].StartGuessingX;
|
||||
lastDetectY = detectors[detectorIndex + 1].StartGuessingY;
|
||||
continue;
|
||||
}
|
||||
}
|
||||
else if (detectorIndex == detectors.Count - 1)
|
||||
{
|
||||
if (detectors[detectorIndex].SwitchWalkBlindCondition(rd))
|
||||
{
|
||||
WalkBlindStage2 = true;
|
||||
WalkBlindCarPathDstX = trackDst.X;
|
||||
WalkBlindCarPathDstY = trackDst.Y;
|
||||
WalkBlindCarPathDstTh = angle2target + WalkBlindTh;
|
||||
DLog.Log($"切换至最后一次盲走时刻目标点:{WalkBlindCarPathDstX:F2} " +
|
||||
$"{WalkBlindCarPathDstY:F2} " +
|
||||
$"{WalkBlindCarPathDstTh:F2}", "TireFollowing");
|
||||
if (detectors.Count == 1)
|
||||
{
|
||||
if (detectors[detectorIndex].SrcId != -1 && detectors[detectorIndex].LeaveSrcFunction != null)
|
||||
{
|
||||
detectors[detectorIndex].LeaveSrcFunction(detectors[detectorIndex].SrcId);
|
||||
DLog.Log($"释放预取车点{detectors[detectorIndex].SrcId}", "TireFollowing");
|
||||
}
|
||||
}
|
||||
|
||||
_lastLFLEncoder = PilotDefinition.Self.LFLActualPos;
|
||||
_lastLFREncoder = PilotDefinition.Self.LFRActualPos;
|
||||
_lastRFLEncoder = PilotDefinition.Self.RFLActualPos;
|
||||
_lastRFREncoder = PilotDefinition.Self.RFRActualPos;
|
||||
_lastLRLEncoder = PilotDefinition.Self.LRLActualPos;
|
||||
_lastLRREncoder = PilotDefinition.Self.LRRActualPos;
|
||||
_lastRRLEncoder = PilotDefinition.Self.RRLActualPos;
|
||||
_lastRRREncoder = PilotDefinition.Self.RRRActualPos;
|
||||
_remainDistanceList.Clear();
|
||||
_targetYList.Clear();
|
||||
continue;
|
||||
}
|
||||
}
|
||||
DLog.Log($"投影点剩余距离:{rd:F2}", "TireFollowing");
|
||||
var track = new LineTrack(path.CarPath.Src, trackDst);
|
||||
track.CarDirectionBias = CarDirection;
|
||||
controller.UpdateTracks(new List<AbstractTrack> { track });
|
||||
NoTarget = false;
|
||||
}
|
||||
yield return true;
|
||||
}
|
||||
}
|
||||
|
||||
private static float CalculateAngle2YAxis(Vector2 point1, Vector2 point2)
|
||||
{
|
||||
return -(float)(Math.Atan((point1.X - point2.X) / (point1.Y - point2.Y)) * 180 / Math.PI);
|
||||
}
|
||||
|
||||
private DriveTask _dt;
|
||||
}
|
||||
}
|
||||
@@ -1,277 +0,0 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.IO;
|
||||
using System.Text;
|
||||
|
||||
namespace MultiWheelC;
|
||||
|
||||
internal static class VehicleSyncBinaryCodec
|
||||
{
|
||||
private const byte Version = 2;
|
||||
private const byte RegisterType = 1;
|
||||
private const byte NotificationType = 2;
|
||||
private static readonly byte[] Magic = Encoding.ASCII.GetBytes("MVS1");
|
||||
|
||||
public static byte[] EncodeRegister(int carNum, VehicleSyncInfo info)
|
||||
{
|
||||
using var stream = new MemoryStream();
|
||||
using var writer = new BinaryWriter(stream, Encoding.UTF8);
|
||||
WriteHeader(writer, RegisterType);
|
||||
writer.Write(carNum);
|
||||
WriteInfo(writer, info);
|
||||
writer.Flush();
|
||||
return stream.ToArray();
|
||||
}
|
||||
|
||||
public static (int CarNum, VehicleSyncInfo Info) DecodeRegister(byte[] payload)
|
||||
{
|
||||
using var stream = new MemoryStream(payload ?? throw new ArgumentNullException(nameof(payload)));
|
||||
using var reader = new BinaryReader(stream, Encoding.UTF8);
|
||||
var version = ReadHeader(reader, RegisterType);
|
||||
var carNum = reader.ReadInt32();
|
||||
var info = ReadInfo(reader, version);
|
||||
EnsureFullyRead(stream);
|
||||
return (carNum, info);
|
||||
}
|
||||
|
||||
public static byte[] EncodeNotification(VehicleSyncNotification notification)
|
||||
{
|
||||
using var stream = new MemoryStream();
|
||||
using var writer = new BinaryWriter(stream, Encoding.UTF8);
|
||||
WriteHeader(writer, NotificationType);
|
||||
|
||||
writer.Write(notification.Seq);
|
||||
writer.Write(BuildNotificationFlags(notification));
|
||||
writer.Write(notification.Mode);
|
||||
writer.Write(notification.FleetStopSourceCar);
|
||||
writer.Write(notification.CenterX);
|
||||
writer.Write(notification.CenterY);
|
||||
writer.Write(notification.CenterTh);
|
||||
writer.Write(notification.FleetVx);
|
||||
writer.Write(notification.FleetFrontTh);
|
||||
writer.Write(notification.FleetRearTh);
|
||||
writer.Write(notification.FleetOmega);
|
||||
writer.Write(notification.RequestedFleetOmega);
|
||||
writer.Write(notification.SyncTh);
|
||||
writer.Write(notification.SyncDistance);
|
||||
writer.Write(notification.DeltaDetectCenter);
|
||||
writer.Write(notification.RotateActiveOmega);
|
||||
writer.Write(notification.RotateCompXyFac);
|
||||
writer.Write(notification.RotateCompXyIFac);
|
||||
writer.Write(notification.RotateCompXyMax);
|
||||
writer.Write(notification.RotateCompThFac);
|
||||
writer.Write(notification.RotateCompThIFac);
|
||||
writer.Write(notification.RotateCompThMax);
|
||||
writer.Write(notification.RotateCompTangentFrac);
|
||||
writer.Write(notification.RotateStartWheelAlignDeg);
|
||||
writer.Write(notification.RotateActiveWheelAlignDeg);
|
||||
writer.Write(notification.IdealX);
|
||||
writer.Write(notification.IdealY);
|
||||
writer.Write(notification.IdealTh);
|
||||
WriteString(writer, notification.FleetStopReason);
|
||||
|
||||
var fleet = notification.Fleet ?? new Dictionary<int, VehicleSyncInfo>();
|
||||
if (fleet.Count > ushort.MaxValue)
|
||||
throw new InvalidOperationException($"Fleet count {fleet.Count} exceeds binary protocol limit.");
|
||||
writer.Write((ushort)fleet.Count);
|
||||
foreach (var kv in fleet)
|
||||
{
|
||||
writer.Write(kv.Key);
|
||||
WriteInfo(writer, kv.Value);
|
||||
}
|
||||
|
||||
writer.Flush();
|
||||
return stream.ToArray();
|
||||
}
|
||||
|
||||
public static VehicleSyncNotification DecodeNotification(byte[] payload)
|
||||
{
|
||||
using var stream = new MemoryStream(payload ?? throw new ArgumentNullException(nameof(payload)));
|
||||
using var reader = new BinaryReader(stream, Encoding.UTF8);
|
||||
var version = ReadHeader(reader, NotificationType);
|
||||
|
||||
var notification = new VehicleSyncNotification
|
||||
{
|
||||
Seq = reader.ReadInt64()
|
||||
};
|
||||
|
||||
ApplyNotificationFlags(notification, reader.ReadUInt16());
|
||||
notification.Mode = reader.ReadInt32();
|
||||
notification.FleetStopSourceCar = reader.ReadInt32();
|
||||
notification.CenterX = reader.ReadSingle();
|
||||
notification.CenterY = reader.ReadSingle();
|
||||
notification.CenterTh = reader.ReadSingle();
|
||||
notification.FleetVx = reader.ReadSingle();
|
||||
notification.FleetFrontTh = reader.ReadSingle();
|
||||
notification.FleetRearTh = reader.ReadSingle();
|
||||
notification.FleetOmega = reader.ReadSingle();
|
||||
notification.RequestedFleetOmega = reader.ReadSingle();
|
||||
notification.SyncTh = reader.ReadSingle();
|
||||
notification.SyncDistance = reader.ReadSingle();
|
||||
notification.DeltaDetectCenter = reader.ReadSingle();
|
||||
notification.RotateActiveOmega = reader.ReadSingle();
|
||||
notification.RotateCompXyFac = reader.ReadSingle();
|
||||
notification.RotateCompXyIFac = reader.ReadSingle();
|
||||
notification.RotateCompXyMax = reader.ReadSingle();
|
||||
notification.RotateCompThFac = reader.ReadSingle();
|
||||
notification.RotateCompThIFac = reader.ReadSingle();
|
||||
notification.RotateCompThMax = reader.ReadSingle();
|
||||
notification.RotateCompTangentFrac = reader.ReadSingle();
|
||||
notification.RotateStartWheelAlignDeg = reader.ReadSingle();
|
||||
notification.RotateActiveWheelAlignDeg = reader.ReadSingle();
|
||||
notification.IdealX = reader.ReadSingle();
|
||||
notification.IdealY = reader.ReadSingle();
|
||||
notification.IdealTh = reader.ReadSingle();
|
||||
notification.FleetStopReason = ReadString(reader);
|
||||
|
||||
var fleetCount = reader.ReadUInt16();
|
||||
notification.Fleet = new Dictionary<int, VehicleSyncInfo>(fleetCount);
|
||||
for (var i = 0; i < fleetCount; ++i)
|
||||
{
|
||||
var carNum = reader.ReadInt32();
|
||||
notification.Fleet[carNum] = ReadInfo(reader, version);
|
||||
}
|
||||
|
||||
EnsureFullyRead(stream);
|
||||
return notification;
|
||||
}
|
||||
|
||||
private static void WriteHeader(BinaryWriter writer, byte type)
|
||||
{
|
||||
writer.Write(Magic);
|
||||
writer.Write(Version);
|
||||
writer.Write(type);
|
||||
writer.Write((ushort)0);
|
||||
}
|
||||
|
||||
private static byte ReadHeader(BinaryReader reader, byte expectedType)
|
||||
{
|
||||
for (var i = 0; i < Magic.Length; ++i)
|
||||
{
|
||||
if (reader.ReadByte() != Magic[i])
|
||||
throw new InvalidDataException("Invalid multi-vehicle sync binary magic.");
|
||||
}
|
||||
|
||||
var version = reader.ReadByte();
|
||||
if (version < 1 || version > Version)
|
||||
throw new InvalidDataException($"Unsupported multi-vehicle sync binary version {version}.");
|
||||
|
||||
var type = reader.ReadByte();
|
||||
if (type != expectedType)
|
||||
throw new InvalidDataException($"Unexpected multi-vehicle sync packet type {type}.");
|
||||
|
||||
var reserved = reader.ReadUInt16();
|
||||
if (reserved != 0)
|
||||
throw new InvalidDataException("Invalid multi-vehicle sync binary reserved field.");
|
||||
|
||||
return version;
|
||||
}
|
||||
|
||||
private static void WriteInfo(BinaryWriter writer, VehicleSyncInfo info)
|
||||
{
|
||||
writer.Write(BuildInfoFlags(info));
|
||||
WriteString(writer, info.Ip);
|
||||
writer.Write(info.Port);
|
||||
writer.Write(info.X);
|
||||
writer.Write(info.Y);
|
||||
writer.Write(info.Th);
|
||||
writer.Write(info.LayoutX);
|
||||
writer.Write(info.LayoutY);
|
||||
writer.Write(info.LayoutTh);
|
||||
WriteString(writer, info.MotionInfeasibleReason);
|
||||
WriteString(writer, info.RotateWheelAlignDetail);
|
||||
writer.Write(info.AppliedNotificationSeq);
|
||||
}
|
||||
|
||||
private static VehicleSyncInfo ReadInfo(BinaryReader reader, byte version)
|
||||
{
|
||||
var info = new VehicleSyncInfo();
|
||||
ApplyInfoFlags(info, reader.ReadUInt16());
|
||||
info.Ip = ReadString(reader);
|
||||
info.Port = reader.ReadInt32();
|
||||
info.X = reader.ReadSingle();
|
||||
info.Y = reader.ReadSingle();
|
||||
info.Th = reader.ReadSingle();
|
||||
info.LayoutX = reader.ReadSingle();
|
||||
info.LayoutY = reader.ReadSingle();
|
||||
info.LayoutTh = reader.ReadSingle();
|
||||
info.MotionInfeasibleReason = ReadString(reader);
|
||||
info.RotateWheelAlignDetail = ReadString(reader);
|
||||
info.AppliedNotificationSeq = version >= 2 ? reader.ReadInt64() : -1;
|
||||
return info;
|
||||
}
|
||||
|
||||
private static ushort BuildInfoFlags(VehicleSyncInfo info)
|
||||
{
|
||||
ushort flags = 0;
|
||||
if (info.Master) flags |= 1 << 0;
|
||||
if (info.PosAvailable) flags |= 1 << 1;
|
||||
if (info.Aligned) flags |= 1 << 2;
|
||||
if (info.DetectOk) flags |= 1 << 3;
|
||||
if (info.MotionFeasible) flags |= 1 << 4;
|
||||
if (info.RotateWheelsAligned) flags |= 1 << 5;
|
||||
return flags;
|
||||
}
|
||||
|
||||
private static void ApplyInfoFlags(VehicleSyncInfo info, ushort flags)
|
||||
{
|
||||
info.Master = (flags & (1 << 0)) != 0;
|
||||
info.PosAvailable = (flags & (1 << 1)) != 0;
|
||||
info.Aligned = (flags & (1 << 2)) != 0;
|
||||
info.DetectOk = (flags & (1 << 3)) != 0;
|
||||
info.MotionFeasible = (flags & (1 << 4)) != 0;
|
||||
info.RotateWheelsAligned = (flags & (1 << 5)) != 0;
|
||||
}
|
||||
|
||||
private static ushort BuildNotificationFlags(VehicleSyncNotification notification)
|
||||
{
|
||||
ushort flags = 0;
|
||||
if (notification.PosAvailable) flags |= 1 << 0;
|
||||
if (notification.Aligned) flags |= 1 << 1;
|
||||
if (notification.FleetMotionReleased) flags |= 1 << 2;
|
||||
if (notification.FleetStopActive) flags |= 1 << 3;
|
||||
if (notification.AutoEnabled) flags |= 1 << 4;
|
||||
if (notification.ManualEnabled) flags |= 1 << 5;
|
||||
if (notification.HasIdeal) flags |= 1 << 6;
|
||||
if (notification.RotateParamsValid) flags |= 1 << 7;
|
||||
if (notification.UseDetourCorrection) flags |= 1 << 8;
|
||||
return flags;
|
||||
}
|
||||
|
||||
private static void ApplyNotificationFlags(VehicleSyncNotification notification, ushort flags)
|
||||
{
|
||||
notification.PosAvailable = (flags & (1 << 0)) != 0;
|
||||
notification.Aligned = (flags & (1 << 1)) != 0;
|
||||
notification.FleetMotionReleased = (flags & (1 << 2)) != 0;
|
||||
notification.FleetStopActive = (flags & (1 << 3)) != 0;
|
||||
notification.AutoEnabled = (flags & (1 << 4)) != 0;
|
||||
notification.ManualEnabled = (flags & (1 << 5)) != 0;
|
||||
notification.HasIdeal = (flags & (1 << 6)) != 0;
|
||||
notification.RotateParamsValid = (flags & (1 << 7)) != 0;
|
||||
notification.UseDetourCorrection = (flags & (1 << 8)) != 0;
|
||||
}
|
||||
|
||||
private static void WriteString(BinaryWriter writer, string value)
|
||||
{
|
||||
var bytes = Encoding.UTF8.GetBytes(value ?? "");
|
||||
if (bytes.Length > ushort.MaxValue)
|
||||
throw new InvalidOperationException($"String payload length {bytes.Length} exceeds binary protocol limit.");
|
||||
writer.Write((ushort)bytes.Length);
|
||||
writer.Write(bytes);
|
||||
}
|
||||
|
||||
private static string ReadString(BinaryReader reader)
|
||||
{
|
||||
var length = reader.ReadUInt16();
|
||||
var bytes = reader.ReadBytes(length);
|
||||
if (bytes.Length != length)
|
||||
throw new EndOfStreamException("Truncated multi-vehicle sync string payload.");
|
||||
return Encoding.UTF8.GetString(bytes);
|
||||
}
|
||||
|
||||
private static void EnsureFullyRead(MemoryStream stream)
|
||||
{
|
||||
if (stream.Position != stream.Length)
|
||||
throw new InvalidDataException("Unexpected trailing bytes in multi-vehicle sync packet.");
|
||||
}
|
||||
}
|
||||
@@ -1,75 +0,0 @@
|
||||
using System.Collections.Generic;
|
||||
using ClumsyCore;
|
||||
using Newtonsoft.Json;
|
||||
|
||||
namespace MultiWheelC;
|
||||
|
||||
public class VehicleSyncInfo
|
||||
{
|
||||
[JsonProperty("Master")] public bool Master { get; set; }
|
||||
[JsonProperty("Ip")] public string Ip { get; set; } = "";
|
||||
[JsonProperty("Port")] public int Port { get; set; } = 8008;
|
||||
[JsonProperty("PosAvailable")] public bool PosAvailable { get; set; }
|
||||
[JsonProperty("X")] public float X { get; set; }
|
||||
[JsonProperty("Y")] public float Y { get; set; }
|
||||
[JsonProperty("Th")] public float Th { get; set; }
|
||||
[JsonProperty("LayoutX")] public float LayoutX { get; set; }
|
||||
[JsonProperty("LayoutY")] public float LayoutY { get; set; }
|
||||
[JsonProperty("LayoutTh")] public float LayoutTh { get; set; }
|
||||
[JsonProperty("Aligned")] public bool Aligned { get; set; }
|
||||
// 本车本轮是否成功识别到邻车（关闭互识别时恒为 true）。任一车为 false 则整队停车。
|
||||
[JsonProperty("DetectOk")] public bool DetectOk { get; set; }
|
||||
[JsonProperty("MotionFeasible")] public bool MotionFeasible { get; set; } = true;
|
||||
[JsonProperty("MotionInfeasibleReason")] public string MotionInfeasibleReason { get; set; } = "";
|
||||
[JsonProperty("RotateWheelsAligned")] public bool RotateWheelsAligned { get; set; } = true;
|
||||
[JsonProperty("RotateWheelAlignDetail")] public string RotateWheelAlignDetail { get; set; } = "";
|
||||
[JsonProperty("AppliedNotificationSeq")] public long AppliedNotificationSeq { get; set; } = -1;
|
||||
}
|
||||
|
||||
public class VehicleSyncNotification
|
||||
{
|
||||
[JsonProperty("PosAvailable")] public bool PosAvailable { get; set; }
|
||||
[JsonProperty("CenterX")] public float CenterX { get; set; }
|
||||
[JsonProperty("CenterY")] public float CenterY { get; set; }
|
||||
[JsonProperty("CenterTh")] public float CenterTh { get; set; }
|
||||
[JsonProperty("Aligned")] public bool Aligned { get; set; }
|
||||
[JsonProperty("Fleet")] public Dictionary<int, VehicleSyncInfo> Fleet { get; set; } = new();
|
||||
[JsonProperty("FleetVx")] public float FleetVx { get; set; }
|
||||
[JsonProperty("FleetFrontTh")] public float FleetFrontTh { get; set; }
|
||||
[JsonProperty("FleetRearTh")] public float FleetRearTh { get; set; }
|
||||
// 联动运动模式：0=常规(前进+转向) 1=蟹行(四轮同向平移) 2=原地旋转(绕车队中心)
|
||||
[JsonProperty("Mode")] public int Mode { get; set; }
|
||||
// 原地旋转角速度(deg/s，逆时针为正)，仅 Mode==2 有效
|
||||
[JsonProperty("FleetOmega")] public float FleetOmega { get; set; }
|
||||
[JsonProperty("RequestedFleetOmega")] public float RequestedFleetOmega { get; set; }
|
||||
[JsonProperty("FleetMotionReleased")] public bool FleetMotionReleased { get; set; } = true;
|
||||
[JsonProperty("FleetStopActive")] public bool FleetStopActive { get; set; }
|
||||
[JsonProperty("FleetStopReason")] public string FleetStopReason { get; set; } = "";
|
||||
[JsonProperty("FleetStopSourceCar")] public int FleetStopSourceCar { get; set; }
|
||||
[JsonProperty("AutoEnabled")] public bool AutoEnabled { get; set; }
|
||||
[JsonProperty("ManualEnabled")] public bool ManualEnabled { get; set; }
|
||||
[JsonProperty("UseDetourCorrection")] public bool UseDetourCorrection { get; set; }
|
||||
[JsonProperty("SyncTh")] public float SyncTh { get; set; }
|
||||
[JsonProperty("SyncDistance")] public float SyncDistance { get; set; }
|
||||
[JsonProperty("DeltaDetectCenter")] public float DeltaDetectCenter { get; set; }
|
||||
// 原地旋转纠偏参数由主车广播，从车运行时使用同一套增益/限幅，避免主从补偿强度不一致。
|
||||
[JsonProperty("RotateParamsValid")] public bool RotateParamsValid { get; set; }
|
||||
[JsonProperty("RotateActiveOmega")] public float RotateActiveOmega { get; set; }
|
||||
[JsonProperty("RotateCompXyFac")] public float RotateCompXyFac { get; set; }
|
||||
[JsonProperty("RotateCompXyIFac")] public float RotateCompXyIFac { get; set; }
|
||||
[JsonProperty("RotateCompXyMax")] public float RotateCompXyMax { get; set; }
|
||||
[JsonProperty("RotateCompThFac")] public float RotateCompThFac { get; set; }
|
||||
[JsonProperty("RotateCompThIFac")] public float RotateCompThIFac { get; set; }
|
||||
[JsonProperty("RotateCompThMax")] public float RotateCompThMax { get; set; }
|
||||
[JsonProperty("RotateCompTangentFrac")] public float RotateCompTangentFrac { get; set; }
|
||||
[JsonProperty("RotateStartWheelAlignDeg")] public float RotateStartWheelAlignDeg { get; set; }
|
||||
[JsonProperty("RotateActiveWheelAlignDeg")] public float RotateActiveWheelAlignDeg { get; set; }
|
||||
// F: 单调递增序列号，从车据此丢弃乱序到达的旧 notify 包。
|
||||
[JsonProperty("Seq")] public long Seq { get; set; }
|
||||
// D: 自动模式下主车路径控制器算出的车队中心理想位姿（世界系），由 idealPos/idealAngle 透传而来。
|
||||
// HasIdeal=true 时各从车按各自 layout 推算 per-car 目标位姿做前馈+补偿，弧线路径不再只靠事后纠偏。
|
||||
[JsonProperty("HasIdeal")] public bool HasIdeal { get; set; }
|
||||
[JsonProperty("IdealX")] public float IdealX { get; set; }
|
||||
[JsonProperty("IdealY")] public float IdealY { get; set; }
|
||||
[JsonProperty("IdealTh")] public float IdealTh { get; set; }
|
||||
}
|
||||
Binary file not shown.
@@ -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);
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -1,21 +1,15 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Linq;
|
||||
using System.Text;
|
||||
using CartActivator;
|
||||
using FundamentalLib;
|
||||
using MDCSToolBox.Medulla.Chassis;
|
||||
// 驱动器、急停、夹臂等安全报警
|
||||
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);
|
||||
//AddAlarm("夹臂不同步报警", 2, () => cart.ClampOutOfSync);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -1,13 +1,12 @@
|
||||
using CartActivator;
|
||||
// 定义车型、上下层IO、参数和MCU初始化
|
||||
using CartActivator;
|
||||
using MCUSerialBridgeCLR;
|
||||
using MDCSToolBox.Medulla.Chassis.MultiWheel;
|
||||
using Medulla.Types;
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Runtime.InteropServices;
|
||||
using System.Text;
|
||||
using System.Threading;
|
||||
|
||||
using MyParking.Shared;
|
||||
|
||||
namespace MedullaAdapter
|
||||
{
|
||||
@@ -15,49 +14,32 @@ namespace MedullaAdapter
|
||||
[UseLadderLogic(logic = typeof(MotorRoutine), scanInterval = 50)]
|
||||
[UseLadderLogic(logic = typeof(MCURoutine), scanInterval = 20)]
|
||||
[UseManualController(manualController = typeof(Remote))]
|
||||
public partial class DiverCartDefinition : MultiWheelCartDefinition
|
||||
public class DiverCartDefinition : MultiWheelCartDefinition
|
||||
{
|
||||
public DiverCommunication Embedded;
|
||||
#region 基本成员
|
||||
public MCUSerialBridge Bridge;
|
||||
[AsUpperIO(desc = "转弯半径")] public float Radius;
|
||||
#region MultiVehicleCoordination
|
||||
|
||||
[AsInitParam(desc = "遥控器最大线速度")]
|
||||
public float MaxManualSpeed = 0.3f;
|
||||
|
||||
[AsInitParam(desc = "遥控器最大角速度")]
|
||||
public float MaxManualAngularSpeed = 45f;
|
||||
|
||||
|
||||
[AsLowerIO(desc = "启用（手动）多车联动")] public bool MultiVehicleManualEnabled = false;
|
||||
[AsLowerIO(desc = "（手动）多车联动模式")] public int MultiVehicleManualMode = 0;
|
||||
|
||||
[AsUpperIO(desc = "多车联动：声光同步，-1未启动，0灭，1亮")] public int MultiVehicleLightSync = -1;
|
||||
|
||||
[AsLowerIO(desc = "多车联动模式-暂停")] public bool MultiVehicleHold = false;
|
||||
|
||||
[AsLowerIO(desc = "多车联动：遥控器Vx")] public float MultiVehicleManualVx = 0f;
|
||||
[AsLowerIO(desc = "多车联动：蟹行方向比例")] public float MultiVehicleManualVy = 0f;
|
||||
[AsLowerIO(desc = "多车联动：遥控器Vth")] public float MultiVehicleManualVth = 0f;
|
||||
|
||||
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
|
||||
[IOObjectMonitor(desc = "左前左轮下发速度")] public float SpeedLFL;
|
||||
[IOObjectMonitor(desc = "左前右轮下发速度")] public float SpeedLFR;
|
||||
[IOObjectMonitor(desc = "右前左轮下发速度")] public float SpeedRFL;
|
||||
[IOObjectMonitor(desc = "右前右轮下发速度")] public float SpeedRFR;
|
||||
[IOObjectMonitor(desc = "左后左轮下发速度")] public float SpeedLRL;
|
||||
[IOObjectMonitor(desc = "左后右轮下发速度")] public float SpeedLRR;
|
||||
[IOObjectMonitor(desc = "右后左轮下发速度")] public float SpeedRRL;
|
||||
[IOObjectMonitor(desc = "右后右轮下发速度")] public float SpeedRRR;
|
||||
//[AsUpperIO(desc = "左前左轮下发速度", timeOutReset = true)] public float SpeedLFL;
|
||||
//[AsUpperIO(desc = "左前右轮下发速度", timeOutReset = true)] public float SpeedLFR;
|
||||
//[AsUpperIO(desc = "右前左轮下发速度", timeOutReset = true)] public float SpeedRFL;
|
||||
//[AsUpperIO(desc = "右前右轮下发速度", timeOutReset = true)] public float SpeedRFR;
|
||||
//[AsUpperIO(desc = "左后左轮下发速度", timeOutReset = true)] public float SpeedLRL;
|
||||
//[AsUpperIO(desc = "左后右轮下发速度", timeOutReset = true)] public float SpeedLRR;
|
||||
//[AsUpperIO(desc = "右后左轮下发速度", timeOutReset = true)] public float SpeedRRL;
|
||||
//[AsUpperIO(desc = "右后右轮下发速度", timeOutReset = true)] public float SpeedRRR;
|
||||
|
||||
#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;
|
||||
@@ -66,11 +48,8 @@ namespace MedullaAdapter
|
||||
[AsLowerIO(desc = "左后右轮实际位置")] public float LRRActualPos;
|
||||
[AsLowerIO(desc = "右后左轮实际位置")] public float RRLActualPos;
|
||||
[AsLowerIO(desc = "右后右轮实际位置")] public float RRRActualPos;
|
||||
|
||||
[AsUpperIO(desc = "左夹臂下发速度", timeOutReset = true)] public float SpeedLeftArm;
|
||||
[AsUpperIO(desc = "右夹臂下发速度", timeOutReset = true)] public float SpeedRightArm;
|
||||
[AsUpperIO(desc = "左夹臂实际速度")] public float ActualSpeedLeftArm;
|
||||
[AsUpperIO(desc = "右夹臂实际速度")] public float ActualSpeedRightArm;
|
||||
[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;
|
||||
@@ -79,86 +58,116 @@ namespace MedullaAdapter
|
||||
[AsLowerIO(desc = "右夹臂电流")] public float RightArmElectric;
|
||||
[AsLowerIO(desc = "左夹臂实际位置")] public float ActualPosLeftArm;
|
||||
[AsLowerIO(desc = "右夹臂实际位置")] public float ActualPosRightArm;
|
||||
[AsLowerIO(desc = "test")] public float test;
|
||||
[AsLowerIO(desc = "test1")] public byte test1;
|
||||
[AsLowerIO(desc = "test2")] public byte test2;
|
||||
[AsInitParam(desc = "test3")] public int test3=24;
|
||||
[IOObjectMonitor(desc = "灯光模式")] public int LightMode = 0;
|
||||
[AsInitParam(desc = "触发模式")] public int trigger = 0;
|
||||
|
||||
[AsInitParam(desc = "手动控制夹臂速度系数")] public float ManualArmSpeedFac = 1.0f;
|
||||
[AsLowerIO(desc = "驱动轮使能状态")] public bool WheelAbleState = true;
|
||||
[AsLowerIO(desc = "电池健康状态")] public float SOH;
|
||||
[AsInitParam(desc = "车号")][AsLowerIO] public int CarNum = 1;
|
||||
[AsInitParam(desc = "初始音量")] public int MusicVolume = 5;
|
||||
#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;
|
||||
|
||||
[AsInitParam(desc = "MCU端口号")] public string MCUPort = "COM4";
|
||||
|
||||
[AsLowerIO(desc = "陀螺仪角度")] public float GyrosTh;
|
||||
[AsLowerIO(desc = "电池健康状态")] public float SOH;
|
||||
#endregion
|
||||
|
||||
[AsLowerIO(desc = "左前左驱动器远程帧701")] public byte LFLRemoteCode = 0;
|
||||
[AsLowerIO(desc = "左前右驱动器远程帧702")] public byte LFRRemoteCode = 0;
|
||||
[AsLowerIO(desc = "右前左驱动器远程帧703")] public byte RFLRemoteCode = 0;
|
||||
[AsLowerIO(desc = "右前右驱动器远程帧704")] public byte RFRRemoteCode = 0;
|
||||
[AsLowerIO(desc = "左后左驱动器远程帧705")] public byte LRLRemoteCode = 0;
|
||||
[AsLowerIO(desc = "左后右驱动器远程帧706")] public byte LRRRemoteCode = 0;
|
||||
[AsLowerIO(desc = "右后左驱动器远程帧707")] public byte RRLRemoteCode = 0;
|
||||
[AsLowerIO(desc = "右后右驱动器远程帧708")] public byte RRRRemoteCode = 0;
|
||||
[AsLowerIO(desc = "左夹臂驱动器远程帧709")] public byte LArmRemoteCode = 0;
|
||||
[AsLowerIO(desc = "右夹臂驱动器远程帧70A")] public byte RArmRemoteCode = 0;
|
||||
[AsUpperIO(desc = "从M上复位")] public bool ResetFromM;
|
||||
[AsUpperIO(desc = "从M将驱动轮下使能")] public bool DisableFromM;
|
||||
[AsUpperIO(desc = "从C上复位")] public bool ResetFromC;
|
||||
[AsUpperIO(desc = "从C将驱动轮下使能")] public bool DisableFromC;
|
||||
[AsUpperIO(desc = "夹臂不同步报警")] public bool ClampOutOfSync;
|
||||
|
||||
internal ManualControlMode TransmitterControlMode = ManualControlMode.Normal;
|
||||
|
||||
[IOObjectMonitor] public float TransmitterSpeed = 0.3f;
|
||||
[AsLowerIO(desc = "驱动轮使能状态")] public bool WheelAbleState = true;
|
||||
|
||||
[AsInitParam(desc = "遥控器速度上限")] public float TransmitterSpeedUpperLimit = 1.0f;
|
||||
[AsInitParam(desc = "遥控器速度下限")] public float TransmitterSpeedLowerLimit = 0.0f;
|
||||
internal DateTime TransmitterLastTime;
|
||||
#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: {0}", err.ToDescription());
|
||||
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: {0}", err.ToDescription());
|
||||
Console.WriteLine($"MCU Reset FAILED: {err.ToDescription()}");
|
||||
return;
|
||||
}
|
||||
else
|
||||
@@ -166,31 +175,28 @@ namespace MedullaAdapter
|
||||
Thread.Sleep(500);
|
||||
Console.WriteLine("MCU Reset OK");
|
||||
}
|
||||
|
||||
//Step3:GetVersion
|
||||
//Step3:获取MCU版本号
|
||||
err = Bridge.GetVersion(out var version, 100);
|
||||
if (err != MCUSerialBridgeError.OK)
|
||||
{
|
||||
Console.WriteLine("MCU GetVersion FAILED: {0}", err.ToDescription());
|
||||
Console.WriteLine($"MCU GetVersion FAILED: {err.ToDescription()}");
|
||||
return;
|
||||
}
|
||||
else
|
||||
{
|
||||
Console.WriteLine("MCU GetVersion OK: {0}", version.ToString());
|
||||
Console.WriteLine($"MCU GetVersion OK: {version}");
|
||||
}
|
||||
|
||||
//Step4:GetState
|
||||
//Step4:获取MCU状态
|
||||
err = Bridge.GetState(out var state, 100);
|
||||
if (err != MCUSerialBridgeError.OK)
|
||||
{
|
||||
Console.WriteLine("MCU GetState FAILED: {0}", err.ToDescription());
|
||||
Console.WriteLine($"MCU GetState FAILED: {err.ToDescription()}");
|
||||
return;
|
||||
}
|
||||
else
|
||||
{
|
||||
Console.WriteLine("MCU GetState OK: {0}", state.ToString());
|
||||
Console.WriteLine($"MCU GetState OK: {state}");
|
||||
}
|
||||
|
||||
//Step5:串口/CAN配置
|
||||
try
|
||||
{
|
||||
@@ -203,18 +209,15 @@ namespace MedullaAdapter
|
||||
for (int i = 0; i < ports.Count; i++)
|
||||
{
|
||||
if (ports[i] is SerialPortConfig s)
|
||||
Console.WriteLine("Port {0}: Serial, Baud={1}, ReceiveFrameMs={2}",
|
||||
i,
|
||||
s.Baud,
|
||||
s.ReceiveFrameMs);
|
||||
Console.WriteLine($"Port {i}: Serial, Baud={s.Baud}, ReceiveFrameMs={s.ReceiveFrameMs}");
|
||||
else if (ports[i] is CANPortConfig c)
|
||||
Console.WriteLine("Port {0}: CAN, Baud={1}, RetryTimeMs={2}", i, c.Baud, c.RetryTimeMs);
|
||||
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: {0}", ret.ToDescription());
|
||||
Console.WriteLine($"MCU Configure FAILED: {ret.ToDescription()}");
|
||||
return;
|
||||
}
|
||||
else
|
||||
@@ -225,54 +228,305 @@ namespace MedullaAdapter
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
Console.WriteLine("Configure Exception: {0}", ex.Message);
|
||||
Console.WriteLine($"Configure Exception: {ex.Message}");
|
||||
return;
|
||||
}
|
||||
//MCUInterface<DiverCartDefinition>.Start(this);
|
||||
State = 0;
|
||||
}
|
||||
|
||||
internal void ManualControl(ManualControlMode mode, float x, float y, float frontDirection,
|
||||
float speedThreshold, TimeSpan? interval = null)
|
||||
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 thPow = (float)Math.Pow(Math.Abs(x), ManualThetaPow) * Math.Sign(x);
|
||||
var frontTh = -thPow * MaxManualTheta;
|
||||
var rearTh = thPow * MaxManualTheta;
|
||||
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:
|
||||
ManualMode = 0;
|
||||
Chassis.DirectionAngle = frontDirection;
|
||||
Chassis.SendMotion(speed, frontTh, rearTh, interval);
|
||||
// 普通模式统一使用车体速度命令：
|
||||
// X向前，行驶中连续改变角速度时舵轮边转、车辆边走。
|
||||
// SendBodyCommand(
|
||||
// vx: speed,
|
||||
// vy: 0.0,
|
||||
// omegaRadiansPerSecond: omega,
|
||||
// interval);
|
||||
Chassis.SendMotion(
|
||||
speed,
|
||||
frontTh,
|
||||
rearTh,
|
||||
interval);
|
||||
break;
|
||||
|
||||
case ManualControlMode.Sway:
|
||||
ManualMode = 1;
|
||||
Chassis.DirectionAngle = frontDirection;
|
||||
Chassis.SendMotion(speed, frontTh, -rearTh, interval);
|
||||
break;
|
||||
|
||||
case ManualControlMode.Crab:
|
||||
ManualMode = 2;
|
||||
Chassis.DirectionAngle = 90;
|
||||
Chassis.SendMotion(speed, frontTh, rearTh, interval);
|
||||
break;
|
||||
// 舵轮机械范围为[-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:
|
||||
ManualMode = 3;
|
||||
Chassis.SendRotateMotion(speed * MaxAngularSpeed, interval);
|
||||
// 摇杆处于中位时只清零驱动速度，保持已经准备好的
|
||||
// 自转舵角；下次推动摇杆时仍会重新检查实际舵角。
|
||||
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;
|
||||
}
|
||||
}
|
||||
|
||||
internal enum ManualControlMode
|
||||
|
||||
// 停车后切换模式：先预转舵轮，实际角度到位后才允许发送运动命令。
|
||||
private bool EnsureManualModeReady(
|
||||
ManualControlMode mode,
|
||||
TimeSpan? interval)
|
||||
{
|
||||
Normal = 0,
|
||||
Spin = 1,
|
||||
Crab = 2,
|
||||
Sway = 3
|
||||
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
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
@@ -1,144 +0,0 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.IO.Ports;
|
||||
using System.Text;
|
||||
using System.Threading;
|
||||
using FundamentalLib;
|
||||
|
||||
|
||||
namespace MedullaAdapter
|
||||
{
|
||||
public class DiverCommunication
|
||||
{
|
||||
private byte[] _receiveBytes ;
|
||||
private SerialPort _port;
|
||||
private int _state = 0;
|
||||
private List<byte> _receiveList = new List<byte>();
|
||||
private int _length = 0;
|
||||
|
||||
private MCUInterface<DiverCartDefinition> _interface;
|
||||
|
||||
public DiverCommunication(string name, int baudRate)
|
||||
{
|
||||
_interface = new MCUInterface<DiverCartDefinition>();
|
||||
_port = new SerialPort();
|
||||
_port.PortName = name; // 根据你的实际串口名称修改
|
||||
_port.BaudRate = baudRate;
|
||||
_port.Parity = Parity.None;
|
||||
_port.DataBits = 8;
|
||||
_port.StopBits = StopBits.One;
|
||||
_port.Handshake = Handshake.None;
|
||||
_port.Open();
|
||||
Console.WriteLine($"buffer:{_port.ReadBufferSize}");
|
||||
//_port.DataReceived += OnDataReceived;
|
||||
new Thread(() =>
|
||||
{
|
||||
while (true)
|
||||
{
|
||||
Thread.Sleep(10);
|
||||
try
|
||||
{
|
||||
int bytesToRead = _port.BytesToRead;
|
||||
if (bytesToRead > 0)
|
||||
{
|
||||
var readBuffer = new byte[bytesToRead];
|
||||
var start = DateTime.Now;
|
||||
_port.Read(readBuffer, 0, bytesToRead);
|
||||
var time1 = DateTime.Now - start;
|
||||
ProcessBuffer(readBuffer);
|
||||
var time2 = DateTime.Now - start;
|
||||
Hedingben.ToastText($"total time:{time2.TotalMilliseconds},read time:{time1},read count:{bytesToRead}", "timeDebug");
|
||||
}
|
||||
}
|
||||
catch (Exception exception)
|
||||
{
|
||||
Console.WriteLine(exception);
|
||||
}
|
||||
}
|
||||
}).Start();
|
||||
}
|
||||
|
||||
private void ProcessBuffer(byte[] buffer)
|
||||
{
|
||||
for (int i = 0; i < buffer.Length; i++)
|
||||
{
|
||||
var newByte = buffer[i];
|
||||
switch (_state)
|
||||
{
|
||||
case 0:
|
||||
_receiveList.Clear();
|
||||
if (newByte == 0xBB)
|
||||
{
|
||||
_receiveList.Add(newByte);
|
||||
_state = 1;
|
||||
}
|
||||
break;
|
||||
case 1:
|
||||
if (newByte == 0xAA)
|
||||
{
|
||||
_receiveList.Add(newByte);
|
||||
_state = 2;
|
||||
}
|
||||
else
|
||||
{
|
||||
_state = 0;
|
||||
}
|
||||
break;
|
||||
case 2:
|
||||
_receiveList.Add(newByte);
|
||||
if (_receiveList.Count >= 4)
|
||||
{
|
||||
_length = BitConverter.ToUInt16(_receiveList.ToArray(), 2);
|
||||
}
|
||||
|
||||
if (_receiveList.Count == _length + 7)
|
||||
{
|
||||
_state = 0;
|
||||
if (_receiveList[_receiveList.Count - 1] == 0xEE)
|
||||
{
|
||||
_receiveBytes = _receiveList.ToArray();
|
||||
Hedingben.ToastText($"receive from mcu:{BitConverter.ToString(_receiveBytes)}","DiverReceive");
|
||||
if (_receiveBytes[5] == 0xA0)
|
||||
{
|
||||
var dataLength1 = BitConverter.ToUInt32(_receiveBytes, 6);
|
||||
var data1 = new byte[dataLength1];
|
||||
Array.Copy(_receiveBytes, 14, data1, 0, dataLength1);
|
||||
MCUInterface<DiverCartDefinition>.NotifyLowerData("default", data1);
|
||||
var logBytes = new byte[BitConverter.ToInt32(_receiveBytes, 10)];
|
||||
var memorySize = BitConverter.ToInt32(_receiveBytes, 6);
|
||||
if (logBytes.Length > 0)
|
||||
{
|
||||
Array.Copy(_receiveBytes, 14 + memorySize, logBytes, 0, logBytes.Length);
|
||||
string log = System.Text.Encoding.ASCII.GetString(logBytes);
|
||||
string[] result = log.Split(new[] { "\r\n", "\n" }, StringSplitOptions.None);
|
||||
for (int j = 0; j < result.Length; j++)
|
||||
{
|
||||
Hedingben.ToastText(result[j], $"mcuLog" + j);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
_state = 0;
|
||||
Hedingben.ToastText($"mcu date error,should end with 0xEE , actual {_receiveList[_receiveList.Count - 1]}","DiverError");
|
||||
}
|
||||
}
|
||||
break;
|
||||
default:break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
public void SendMessage(byte[] data)
|
||||
{
|
||||
Hedingben.ToastText($"send to mcu:{BitConverter.ToString(data)}", "DiverSend");
|
||||
_port.Write(data, 0, data.Length);
|
||||
}
|
||||
|
||||
public byte[] GetMessage()
|
||||
{
|
||||
return _receiveBytes;
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -1,181 +0,0 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.IO.Ports;
|
||||
using System.Linq;
|
||||
using System.Threading;
|
||||
using FundamentalLib;
|
||||
using Medulla;
|
||||
|
||||
namespace MedullaAdapter
|
||||
{
|
||||
public class EmbeddedCommunication
|
||||
{
|
||||
private SerialPort _port;
|
||||
|
||||
private byte[] _receiveBytes;
|
||||
|
||||
private byte[] _receiveData;
|
||||
|
||||
private const int BufferThreshold = 300; // 缓冲区大小阈值
|
||||
|
||||
private List<byte> _buffer = new List<byte>(); // 缓存接收到的数据
|
||||
|
||||
public bool CommunicationError = false;
|
||||
|
||||
private DateTime _lastTime = DateTime.Now;
|
||||
|
||||
private MCUInterface<DiverCartDefinition> _interface;
|
||||
|
||||
public EmbeddedCommunication(string name, int baudRate)
|
||||
{
|
||||
_interface = new MCUInterface<DiverCartDefinition>();
|
||||
_port = new SerialPort();
|
||||
_port.PortName = name; // 根据你的实际串口名称修改
|
||||
_port.BaudRate = baudRate;
|
||||
_port.Parity = Parity.None;
|
||||
_port.DataBits = 8;
|
||||
_port.StopBits = StopBits.One;
|
||||
_port.Handshake = Handshake.None;
|
||||
_port.Open();
|
||||
_port.DataReceived += OnDataReceived;
|
||||
}
|
||||
|
||||
public void SendMessage(byte[] data)
|
||||
{
|
||||
Hedingben.ToastText($"send to mcu:{BitConverter.ToString(data)}","DiverSend");
|
||||
_port.Write(data, 0, data.Length);
|
||||
}
|
||||
|
||||
public byte[] GetMessage()
|
||||
{
|
||||
return _receiveBytes;
|
||||
}
|
||||
|
||||
private void OnDataReceived(object sender, SerialDataReceivedEventArgs e)
|
||||
{
|
||||
try
|
||||
{
|
||||
int bytesToRead = _port.BytesToRead;
|
||||
byte[] buffer = new byte[bytesToRead];
|
||||
var startRead = DateTime.Now;
|
||||
_port.Read(buffer, 0, bytesToRead);
|
||||
var time = DateTime.Now - startRead;
|
||||
// 将新接收到的数据添加到缓冲区
|
||||
startRead = DateTime.Now;
|
||||
_buffer.AddRange(buffer);
|
||||
var time2 = DateTime.Now - startRead;
|
||||
Hedingben.ToastText($"buffer length:{_buffer.Count}","bufferDebug");
|
||||
DLog.Log($"buffer length:{_buffer.Count}");
|
||||
//if (_buffer.Count > BufferThreshold)
|
||||
//{
|
||||
// _buffer.Clear();
|
||||
// return;
|
||||
//}
|
||||
|
||||
// 尝试解析缓冲区中的报文
|
||||
var parseTime = DateTime.Now;
|
||||
ParseBuffer();
|
||||
var time3 = DateTime.Now - parseTime;
|
||||
Hedingben.ToastText(
|
||||
$"读取报文时间:{time.TotalMilliseconds},增加到缓存区:{time2.TotalMilliseconds},处理时间:{time3.TotalMilliseconds}",
|
||||
"timeDebug1");
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
Console.WriteLine($"读取串口数据时发生错误: {ex.Message}"+ex.StackTrace);
|
||||
}
|
||||
}
|
||||
private void ParseBuffer()
|
||||
{
|
||||
while (_buffer.Count >= 7) // 报文的最小长度是 7
|
||||
{
|
||||
// 查找报文头部
|
||||
var findTime = DateTime.Now;
|
||||
int startIndex = _buffer.FindIndex(0, b => b == 0xBB);
|
||||
var time1 = DateTime.Now - findTime;
|
||||
if (startIndex == -1)
|
||||
{
|
||||
// 如果没有找到头部或剩余长度不足最小报文长度，结束解析
|
||||
_buffer.Clear();
|
||||
return;
|
||||
}
|
||||
|
||||
// 确保头部后还有至少 6 个字节
|
||||
if (startIndex + 1 >= _buffer.Count || _buffer[startIndex + 1] != 0xAA)
|
||||
{
|
||||
// 如果第二字节不是 0xAA，丢弃无效字节
|
||||
_buffer.RemoveAt(startIndex);
|
||||
continue;
|
||||
}
|
||||
|
||||
// 检查数据段长度
|
||||
if (startIndex + 4 >= _buffer.Count) break; // 数据不足，等待下次接收
|
||||
int dataLength = _buffer[startIndex + 2] | (_buffer[startIndex + 3] << 8);
|
||||
|
||||
// 计算报文总长度
|
||||
int totalLength = dataLength + 7;
|
||||
|
||||
// 检查总长度是否足够
|
||||
if (startIndex + totalLength > _buffer.Count) break; // 数据不足，等待下次接收
|
||||
|
||||
// 检查尾部是否是 0xCC 0xEE
|
||||
if (_buffer[startIndex + totalLength - 2] == 0xCC && _buffer[startIndex + totalLength - 1] == 0xEE)
|
||||
{
|
||||
var time2 = DateTime.Now - findTime;
|
||||
var copyTime = DateTime.Now;
|
||||
// 提取完整报文
|
||||
_receiveBytes = _buffer.Skip(startIndex).Take(totalLength).ToArray();
|
||||
var time3 = DateTime.Now - copyTime;
|
||||
if (_receiveBytes[5] == 0xA0)
|
||||
{
|
||||
var dataLength1 = BitConverter.ToUInt32(_receiveBytes, 6);
|
||||
var data1 = new byte[dataLength1];
|
||||
Array.Copy(_receiveBytes, 14, data1, 0, dataLength1);
|
||||
var notifyTime = DateTime.Now;
|
||||
MCUInterface<DiverCartDefinition>.NotifyLowerData("default", data1);
|
||||
var time4 = DateTime.Now - notifyTime;
|
||||
var logBytes = new byte[BitConverter.ToInt32(_receiveBytes, 10)];
|
||||
var memorySize = BitConverter.ToInt32(_receiveBytes, 6);
|
||||
if (logBytes.Length > 0)
|
||||
{
|
||||
Array.Copy(_receiveBytes, 14 + memorySize, logBytes, 0, logBytes.Length);
|
||||
string log = System.Text.Encoding.ASCII.GetString(logBytes);
|
||||
string[] result = log.Split(new[] { "\r\n", "\n" }, StringSplitOptions.None);
|
||||
for (int i = 0; i < result.Length; i++)
|
||||
{
|
||||
Hedingben.ToastText(result[i], $"mcuLog" + i);
|
||||
}
|
||||
}
|
||||
|
||||
Hedingben.ToastText(
|
||||
$"find head time:{time1.TotalMilliseconds},find total time:{time2.TotalMilliseconds},copy time:{time3.TotalMilliseconds},notify time:{time4.TotalMilliseconds}","timeDebug2");
|
||||
}
|
||||
|
||||
Hedingben.ToastText(
|
||||
$"receive from mcu:{BitConverter.ToString(_receiveBytes)},time:{(DateTime.Now - _lastTime).TotalMilliseconds}",
|
||||
"DiverReceive");
|
||||
if ((DateTime.Now - _lastTime).TotalMilliseconds > 200)
|
||||
{
|
||||
Hedingben.ToastText($"{(DateTime.Now - _lastTime).TotalMilliseconds}ms no message from diver","DiverTimeout");
|
||||
}
|
||||
_lastTime = DateTime.Now;
|
||||
// 从缓冲区中移除已解析的报文
|
||||
_buffer.RemoveRange(0, startIndex + totalLength);
|
||||
return; // 成功解析一条报文后退出本轮解析
|
||||
|
||||
}
|
||||
else
|
||||
{
|
||||
// 如果尾部无效，丢弃头部并继续解析
|
||||
DLog.Log("DIVER报文无效");
|
||||
Console.WriteLine("DIVER报文无效");
|
||||
var errorBytes = _buffer.Skip(startIndex).Take(totalLength).ToArray();
|
||||
DLog.Log("错误报文"+string.Join(" ",errorBytes.Select(p=>$"{p:X2}")));
|
||||
_buffer.RemoveAt(startIndex);
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
@@ -1,246 +0,0 @@
|
||||
using System;
|
||||
using CartActivator;
|
||||
using CycleGUI;
|
||||
using CycleGUI.API;
|
||||
using FundamentalLib;
|
||||
using Medulla.Types;
|
||||
|
||||
namespace MedullaAdapter
|
||||
{
|
||||
public partial class DiverCartDefinition
|
||||
{
|
||||
private bool _fleetRemoteActive;
|
||||
private DateTime _fleetDiagLastStick = DateTime.MinValue;
|
||||
|
||||
private void FleetDiag(string msg)
|
||||
{
|
||||
DLog.Log($"car{CarNum} {msg}", "FleetDiagMedulla");
|
||||
}
|
||||
|
||||
private void ZeroFleetManualFields()
|
||||
{
|
||||
MultiVehicleManualEnabled = false;
|
||||
MultiVehicleManualMode = 0;
|
||||
MultiVehicleManualVx = 0;
|
||||
MultiVehicleManualVy = 0;
|
||||
MultiVehicleManualVth = 0;
|
||||
}
|
||||
|
||||
public float ApplyFleetManualCommand(int mode, float px, float py, float speedRatio)
|
||||
{
|
||||
var ratio = Clamp(speedRatio, 0, 1);
|
||||
px = Clamp(px, -1, 1);
|
||||
py = Clamp(py, -1, 1);
|
||||
|
||||
if (mode < 0 || mode > 2) mode = 0;
|
||||
MultiVehicleManualMode = mode;
|
||||
|
||||
if (mode == 2)
|
||||
{
|
||||
MultiVehicleManualVx = 0;
|
||||
MultiVehicleManualVy = 0;
|
||||
MultiVehicleManualVth = px * MaxManualAngularSpeed * ratio;
|
||||
}
|
||||
else if (mode == 1)
|
||||
{
|
||||
MultiVehicleManualVx = py * MaxManualSpeed * ratio;
|
||||
MultiVehicleManualVy = px * MaxManualSpeed * ratio;
|
||||
MultiVehicleManualVth = 0;
|
||||
}
|
||||
else
|
||||
{
|
||||
MultiVehicleManualVx = py * MaxManualSpeed * ratio;
|
||||
MultiVehicleManualVy = 0;
|
||||
MultiVehicleManualVth = px * MaxManualAngularSpeed * ratio;
|
||||
}
|
||||
|
||||
return ratio;
|
||||
}
|
||||
|
||||
[IOObjectUtility]
|
||||
public void FleetRemote()
|
||||
{
|
||||
if (_fleetRemoteActive)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
_fleetRemoteActive = true;
|
||||
var fleetOn = false;
|
||||
var crabOn = false;
|
||||
var rotateOn = false;
|
||||
var speedRatio = 1f;
|
||||
UseGesture manip = null;
|
||||
|
||||
int CurrentMode()
|
||||
{
|
||||
if (crabOn) return 1;
|
||||
if (rotateOn) return 2;
|
||||
return 0;
|
||||
}
|
||||
|
||||
void ApplyMode()
|
||||
{
|
||||
MultiVehicleManualMode = fleetOn ? CurrentMode() : 0;
|
||||
}
|
||||
|
||||
void CleanupFleetRemote()
|
||||
{
|
||||
if (manip != null)
|
||||
{
|
||||
manip.End();
|
||||
manip = null;
|
||||
}
|
||||
|
||||
fleetOn = false;
|
||||
crabOn = false;
|
||||
rotateOn = false;
|
||||
ZeroFleetManualFields();
|
||||
_fleetRemoteActive = false;
|
||||
}
|
||||
|
||||
manip = new UseGesture();
|
||||
|
||||
manip.AddWidget(new UseGesture.StickWidget
|
||||
{
|
||||
name = "fleet_stick",
|
||||
text = "速度摇杆",
|
||||
position = "37.5%+10px, 18%+10px",
|
||||
size = "37.5%-10px, 32%-10px",
|
||||
bounceBack = true,
|
||||
keyboard = "Up,Down,Left,Right",
|
||||
joystick = "Axis0,Axis1",
|
||||
OnValue = (pos, manipulating) =>
|
||||
{
|
||||
if (!fleetOn)
|
||||
{
|
||||
MultiVehicleManualVx = 0;
|
||||
MultiVehicleManualVy = 0;
|
||||
MultiVehicleManualVth = 0;
|
||||
if ((DateTime.Now - _fleetDiagLastStick).TotalMilliseconds >= 200)
|
||||
{
|
||||
_fleetDiagLastStick = DateTime.Now;
|
||||
FleetDiag($"STICK(ignored,fleetOff) pos=({pos.X:0.00},{pos.Y:0.00}) manip={manipulating}");
|
||||
}
|
||||
return;
|
||||
}
|
||||
|
||||
var mode = CurrentMode();
|
||||
var ratio = ApplyFleetManualCommand(mode, pos.X, pos.Y, speedRatio);
|
||||
|
||||
if ((DateTime.Now - _fleetDiagLastStick).TotalMilliseconds >= 200)
|
||||
{
|
||||
_fleetDiagLastStick = DateTime.Now;
|
||||
FleetDiag($"STICK mode={mode} pos=({pos.X:0.00},{pos.Y:0.00}) manip={manipulating} ratio={ratio:0.00} " +
|
||||
$"-> Vx={MultiVehicleManualVx:0.000} Vy={MultiVehicleManualVy:0.000} Vth={MultiVehicleManualVth:0.0} en={MultiVehicleManualEnabled}");
|
||||
}
|
||||
}
|
||||
});
|
||||
|
||||
manip.AddWidget(new UseGesture.ToggleWidget
|
||||
{
|
||||
name = "fleet_crab",
|
||||
text = "横移模式",
|
||||
position = "12.5%+10px, 52%+10px",
|
||||
size = "37.5%-10px, 10%-10px",
|
||||
OnValue = b =>
|
||||
{
|
||||
crabOn = b;
|
||||
ApplyMode();
|
||||
FleetDiag($"TOGGLE crab={b} -> mode={MultiVehicleManualMode}");
|
||||
}
|
||||
});
|
||||
|
||||
manip.AddWidget(new UseGesture.ToggleWidget
|
||||
{
|
||||
name = "fleet_rotate",
|
||||
text = "原地旋转",
|
||||
position = "50%+10px, 52%+10px",
|
||||
size = "37.5%-10px, 10%-10px",
|
||||
OnValue = b =>
|
||||
{
|
||||
rotateOn = b;
|
||||
ApplyMode();
|
||||
FleetDiag($"TOGGLE rotate={b} -> mode={MultiVehicleManualMode}");
|
||||
}
|
||||
});
|
||||
|
||||
manip.AddWidget(new UseGesture.ToggleWidget
|
||||
{
|
||||
name = "fleet_enable",
|
||||
text = "车队联动",
|
||||
position = "12.5%+10px, 64%+10px",
|
||||
size = "37.5%-10px, 10%-10px",
|
||||
OnValue = b =>
|
||||
{
|
||||
fleetOn = b;
|
||||
MultiVehicleManualEnabled = b;
|
||||
ApplyMode();
|
||||
if (!b) ZeroFleetManualFields();
|
||||
FleetDiag($"TOGGLE fleetOn={b} -> ManualEnabled={MultiVehicleManualEnabled} mode={MultiVehicleManualMode}");
|
||||
}
|
||||
});
|
||||
|
||||
manip.AddWidget(new UseGesture.ThrottleWidget
|
||||
{
|
||||
name = "fleet_speed_ratio",
|
||||
text = "速度比例",
|
||||
position = "50%+10px, 64%+10px",
|
||||
size = "37.5%-10px, 10%-10px",
|
||||
bounceBack = false,
|
||||
OnValue = (val, _) => speedRatio = Clamp(val, 0, 1)
|
||||
});
|
||||
|
||||
manip.AddWidget(new UseGesture.ButtonWidget
|
||||
{
|
||||
name = "fleet_stop",
|
||||
text = "急停/归零",
|
||||
position = "31.25%+10px, 76%+10px",
|
||||
size = "37.5%-10px, 10%-10px",
|
||||
OnPressed = pressed =>
|
||||
{
|
||||
if (!pressed) return;
|
||||
MultiVehicleManualVx = 0;
|
||||
MultiVehicleManualVy = 0;
|
||||
MultiVehicleManualVth = 0;
|
||||
FleetDiag("BUTTON stop/zero");
|
||||
}
|
||||
});
|
||||
|
||||
manip.ChangeState(new SetAppearance { drawGuizmo = false });
|
||||
manip.Start();
|
||||
|
||||
GUI.PromptOrBringToFront(pb =>
|
||||
{
|
||||
if (pb.Closing())
|
||||
{
|
||||
CleanupFleetRemote();
|
||||
pb.Panel.Exit();
|
||||
return;
|
||||
}
|
||||
|
||||
pb.Panel.TopMost(true)
|
||||
.SetDefaultDocking(Panel.Docking.None)
|
||||
.ShowTitle("车队联动遥控")
|
||||
.InitSize(340, 190)
|
||||
.InitPos(false, -32, 32, 1, 0, 1, 0);
|
||||
|
||||
pb.SeparatorText("状态");
|
||||
var modeName = MultiVehicleManualMode == 2 ? "原地旋转" : MultiVehicleManualMode == 1 ? "横移" : "常规";
|
||||
pb.Label($"车队联动: {MultiVehicleManualEnabled} 模式: {modeName}");
|
||||
pb.Label($"速度比例: {speedRatio:0.00}");
|
||||
pb.Label($"Vx={MultiVehicleManualVx:0.000} Vy={MultiVehicleManualVy:0.000} m/s, Vth={MultiVehicleManualVth:0.0}");
|
||||
pb.Label($"优先级: {CartActivator.CartDefinition.currentPriority} ({CartActivator.CartDefinition.currentPriorityDesc})");
|
||||
pb.Label("请勿同时打开手动控制面板");
|
||||
pb.Panel.Repaint();
|
||||
}, instancingObject: this);
|
||||
}
|
||||
|
||||
private static float Clamp(float value, float min, float max)
|
||||
{
|
||||
if (value < min) return min;
|
||||
if (value > max) return max;
|
||||
return value;
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -1,3 +0,0 @@
|
||||
<Weavers xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance" xsi:noNamespaceSchemaLocation="FodyWeavers.xsd">
|
||||
<DiverCompiler />
|
||||
</Weavers>
|
||||
@@ -1,26 +0,0 @@
|
||||
<?xml version="1.0" encoding="utf-8"?>
|
||||
<xs:schema xmlns:xs="http://www.w3.org/2001/XMLSchema">
|
||||
<!-- This file was generated by Fody. Manual changes to this file will be lost when your project is rebuilt. -->
|
||||
<xs:element name="Weavers">
|
||||
<xs:complexType>
|
||||
<xs:all>
|
||||
<xs:element name="DiverCompiler" minOccurs="0" maxOccurs="1" type="xs:anyType" />
|
||||
</xs:all>
|
||||
<xs:attribute name="VerifyAssembly" type="xs:boolean">
|
||||
<xs:annotation>
|
||||
<xs:documentation>'true' to run assembly verification (PEVerify) on the target assembly after all weavers have been executed.</xs:documentation>
|
||||
</xs:annotation>
|
||||
</xs:attribute>
|
||||
<xs:attribute name="VerifyIgnoreCodes" type="xs:string">
|
||||
<xs:annotation>
|
||||
<xs:documentation>A comma-separated list of error codes that can be safely ignored in assembly verification.</xs:documentation>
|
||||
</xs:annotation>
|
||||
</xs:attribute>
|
||||
<xs:attribute name="GenerateXsd" type="xs:boolean">
|
||||
<xs:annotation>
|
||||
<xs:documentation>'false' to turn off automatic generation of the XML Schema file.</xs:documentation>
|
||||
</xs:annotation>
|
||||
</xs:attribute>
|
||||
</xs:complexType>
|
||||
</xs:element>
|
||||
</xs:schema>
|
||||
@@ -1,291 +0,0 @@
|
||||
using CartActivator;
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.IO;
|
||||
using System.Linq;
|
||||
using System.Reflection;
|
||||
using System.Text;
|
||||
using System.Threading;
|
||||
using Newtonsoft.Json;
|
||||
using FundamentalLib;
|
||||
|
||||
|
||||
namespace MedullaAdapter
|
||||
{
|
||||
// todo: currently use this ladderlogic to act interaction with Medulla.
|
||||
// todo: directly integrate into CartActivator.
|
||||
|
||||
// MCU->Medulla is intervally exchanged information.
|
||||
public class MCUInterface<T> : LadderLogic<T> where T : DiverCartDefinition
|
||||
{
|
||||
|
||||
/// ////////////////////////////// IMPLEMENT THESE ////////////////////////////////////////////
|
||||
|
||||
public static void SetMCUProgram(string mcu_device_url, byte[] program)
|
||||
{
|
||||
List<byte[]> SplitArrayIntoChunks(byte[] array, int chunkSize)
|
||||
{
|
||||
List<byte[]> chunks = new List<byte[]>();
|
||||
|
||||
for (int i = 0; i < array.Length; i += chunkSize)
|
||||
{
|
||||
int currentChunkSize = Math.Min(chunkSize, array.Length - i);
|
||||
byte[] chunk = new byte[currentChunkSize];
|
||||
Array.Copy(array, i, chunk, 0, currentChunkSize);
|
||||
chunks.Add(chunk);
|
||||
}
|
||||
return chunks;
|
||||
}
|
||||
//todo: modify this.
|
||||
var codeList = SplitArrayIntoChunks(program, 1024);
|
||||
int i = 0;
|
||||
foreach (var codePack in codeList)
|
||||
{
|
||||
var downloadCode =
|
||||
new DownloadCodePack(program.Length, 1024 * i, codePack.Length, codePack).GetPack();
|
||||
Console.WriteLine(string.Join(" ", downloadCode.Select(p => $"{p:X2}")));
|
||||
cart.Embedded.SendMessage(downloadCode);
|
||||
Console.WriteLine($"send bytes length:{downloadCode.Length}");
|
||||
i++;
|
||||
Thread.Sleep(50);
|
||||
//while (true)
|
||||
//{
|
||||
// var receive = cart.Embedded.GetMessage();
|
||||
// if (receive == null) continue;
|
||||
// if (receive[5] == 0x90 && (receive[6] == 0x05 || receive[6]==0x06)) break;
|
||||
//}
|
||||
}
|
||||
var controlPack2 = new ControlPack(0x01).GetPack();
|
||||
cart.Embedded.SendMessage(controlPack2);
|
||||
Console.WriteLine("下发代码");
|
||||
//MCUTestRunner.DebugSetMCUProgram(program, (bs) => NotifyLowerData("default", bs));
|
||||
}
|
||||
|
||||
public static void SendUpperData(string mcu_device_url, byte[] data)
|
||||
{
|
||||
//todo: this is for VM data exchange, contains upperIO/lowerIO modifications.
|
||||
var upperPack = new UpperIOPack(data, data.Length).GetPack();
|
||||
cart.Embedded.SendMessage(upperPack);
|
||||
//MCUTestRunner.DebugSendUpper(data);
|
||||
}
|
||||
|
||||
|
||||
/// ////////////////////////////// INTERFACES ////////////////////////////////////////////
|
||||
|
||||
public static void NotifyPrint(string mcu_device_url, string message)
|
||||
{
|
||||
Console.WriteLine($"{mcu_device_url}:{message}");
|
||||
}
|
||||
|
||||
// whenever a lower io data is uploaded, call this.
|
||||
public static void NotifyLowerData(string mcu_device_url, byte[] lowerIOData)
|
||||
{
|
||||
using var ms = new MemoryStream(lowerIOData);
|
||||
using var br = new BinaryReader(ms);
|
||||
if (mcu_logics.TryGetValue(mcu_device_url, out var tup))
|
||||
{
|
||||
Hedingben.ToastText($"recv iter {br.ReadInt32()} lowerIO data from {mcu_device_url}, operation {tup.name}", $"DIVER-{tup.name}");
|
||||
while (ms.Position < lowerIOData.Length)
|
||||
{
|
||||
var cid = br.ReadInt16();
|
||||
if (cid < 0 || cid > tup.fields.Length) throw new Exception("invalid Cartfield id!");
|
||||
// if it's upperio skip, otherwise write data.
|
||||
var typeid = br.ReadByte();
|
||||
if (tup.fields[cid].typeid != typeid)
|
||||
throw new Exception($"??? typeid not match for {tup.fields[cid].field}({cid}), expected {tup.fields[cid].typeid} got {typeid}");
|
||||
|
||||
object value;
|
||||
switch (tup.fields[cid].typeid)
|
||||
{
|
||||
case 0:
|
||||
value = br.ReadBoolean();
|
||||
break;
|
||||
case 1:
|
||||
value = br.ReadByte();
|
||||
break;
|
||||
case 2:
|
||||
value = br.ReadSByte();
|
||||
break;
|
||||
case 3:
|
||||
value = br.ReadChar();
|
||||
break;
|
||||
case 4:
|
||||
value = br.ReadInt16();
|
||||
break;
|
||||
case 5:
|
||||
value = br.ReadUInt16();
|
||||
break;
|
||||
case 6:
|
||||
value = br.ReadInt32();
|
||||
break;
|
||||
case 7:
|
||||
value = br.ReadUInt32();
|
||||
break;
|
||||
case 8:
|
||||
value = br.ReadSingle();
|
||||
break;
|
||||
default:
|
||||
throw new Exception($"Unsupported type ID: {tup.fields[cid].typeid}");
|
||||
}
|
||||
|
||||
if (tup.fields[cid].isUpper) continue;
|
||||
tup.fields[cid].fi.SetValue(cart, value);
|
||||
}
|
||||
|
||||
// ok to send current data.
|
||||
using var sends = new MemoryStream();
|
||||
using var bw = new BinaryWriter(sends);
|
||||
bw.Write(tup.iterations++);
|
||||
for (var cid = 0; cid < tup.fields.Length; cid++)
|
||||
{
|
||||
bw.Write((short)cid);
|
||||
bw.Write((byte)tup.fields[cid].typeid);
|
||||
var val = tup.fields[cid].fi.GetValue(cart);
|
||||
switch (tup.fields[cid].typeid)
|
||||
{
|
||||
case 0:
|
||||
bw.Write((bool)val);
|
||||
break;
|
||||
case 1:
|
||||
bw.Write((byte)val);
|
||||
break;
|
||||
case 2:
|
||||
bw.Write((sbyte)val);
|
||||
break;
|
||||
case 3:
|
||||
bw.Write((char)val);
|
||||
break;
|
||||
case 4:
|
||||
bw.Write((short)val);
|
||||
break;
|
||||
case 5:
|
||||
bw.Write((ushort)val);
|
||||
break;
|
||||
case 6:
|
||||
bw.Write((int)val);
|
||||
break;
|
||||
case 7:
|
||||
bw.Write((uint)val);
|
||||
break;
|
||||
case 8:
|
||||
bw.Write((float)val);
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
SendUpperData(mcu_device_url, sends.ToArray());
|
||||
}
|
||||
else
|
||||
{
|
||||
Hedingben.ToastText($"warning: {mcu_device_url} received lowerIOData but not registered","DIVERWarning");
|
||||
}
|
||||
}
|
||||
|
||||
public static void Start(T c)
|
||||
{
|
||||
cart = c;
|
||||
|
||||
var logics = typeof(T).Assembly.GetTypes()
|
||||
.Where(p => p.GetCustomAttribute<LogicRunOnMCUAttribute>() != null).ToArray();
|
||||
foreach (var logic in logics)
|
||||
{
|
||||
var attr = logic.GetCustomAttribute<LogicRunOnMCUAttribute>();
|
||||
Console.WriteLine($"Set logic {logic.Name} to run on MCU-VM @ device {attr.mcu_url}");
|
||||
|
||||
byte[] ReadAllBytes(Stream stream)
|
||||
{
|
||||
using (var ms = new MemoryStream())
|
||||
{
|
||||
stream.CopyTo(ms);
|
||||
return ms.ToArray();
|
||||
}
|
||||
}
|
||||
var bytes = ReadAllBytes(Assembly.GetExecutingAssembly().GetManifestResourceStream($"{logic.Name}.bin"));
|
||||
SetMCUProgram(attr.mcu_url, bytes);
|
||||
var json = UTF8Encoding.UTF8.GetString(ReadAllBytes(Assembly.GetExecutingAssembly()
|
||||
.GetManifestResourceStream($"{logic.Name}.bin.json")));
|
||||
Console.WriteLine(json);
|
||||
var fields = JsonConvert.DeserializeObject<PField[]>(json);
|
||||
if (mcu_logics.ContainsKey(attr.mcu_url))
|
||||
throw new Exception($"Already have logic for {attr.mcu_url}: LadderLogic {logic.Name}");
|
||||
mcu_logics[attr.mcu_url] = new LogicInfo() { fields = fields, name = logic.Name };
|
||||
foreach (var pField in fields)
|
||||
{
|
||||
pField.fi = typeof(T).GetField(pField.field);
|
||||
if (pField.fi == null)
|
||||
throw new Exception($"field {pField.field} doesn't exist in cart object?");
|
||||
pField.isUpper = pField.fi.IsDefined(typeof(AsUpperIO));
|
||||
// todo: check type.
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// ///////////////////////////////// DONT CARE /////////////////////////////////////
|
||||
private static T cart;
|
||||
|
||||
class PField
|
||||
{
|
||||
public string field;
|
||||
public FieldInfo fi;
|
||||
public bool isUpper;
|
||||
public int typeid, offset;
|
||||
}
|
||||
|
||||
class LogicInfo
|
||||
{
|
||||
public string name;
|
||||
public PField[] fields;
|
||||
public int iterations;
|
||||
}
|
||||
private static Dictionary<string, LogicInfo> mcu_logics = new();
|
||||
public override void Operation(int iteration)
|
||||
{
|
||||
// just send LIO.
|
||||
}
|
||||
}
|
||||
|
||||
//unsafe class MCUTestRunner
|
||||
//{
|
||||
|
||||
// public delegate void NotifyLowerDelegate(byte* changedStates, int length);
|
||||
// [DllImport("MCURuntime.dll", CallingConvention = CallingConvention.Cdecl)]
|
||||
// public static extern void set_lowerio_cb(NotifyLowerDelegate callback);
|
||||
|
||||
// private static NotifyLowerDelegate DNotifyStateChanged = StateChanged;
|
||||
|
||||
// [DllImport("MCURuntime.dll")]
|
||||
// static extern void test(byte* bin, int len);
|
||||
// [DllImport("MCURuntime.dll")]
|
||||
// static extern void put_upper(byte* bin, int len);
|
||||
|
||||
// private static Action<byte[]> lo_notifier;
|
||||
// private static void StateChanged(byte* changedstates, int length)
|
||||
// {
|
||||
// byte[] byteArray = new byte[length];
|
||||
// Marshal.Copy((IntPtr)changedstates, byteArray, 0, length);
|
||||
// lo_notifier(byteArray);
|
||||
// }
|
||||
|
||||
// public static void DebugSendUpper(byte[] data)
|
||||
// {
|
||||
// fixed (byte* ptr = data)
|
||||
// {
|
||||
// put_upper(ptr, data.Length);
|
||||
// }
|
||||
// }
|
||||
// public static void DebugSetMCUProgram(byte[] program, Action<byte[]> notifier)
|
||||
// {
|
||||
// lo_notifier = notifier;
|
||||
// set_lowerio_cb(DNotifyStateChanged);
|
||||
// new Thread(() =>
|
||||
// {
|
||||
// var allb = new byte[10240]; //10K runtime
|
||||
// Array.Copy(program, allb, program.Length);
|
||||
// fixed (byte* ptr = allb)
|
||||
// {
|
||||
// test(ptr, 10240);
|
||||
// }
|
||||
// }).Start();
|
||||
// }
|
||||
//}
|
||||
}
|
||||
+163
-14
@@ -1,13 +1,14 @@
|
||||
using CartActivator;
|
||||
// 实际CAN协议、反馈解析、IO、电池、急停
|
||||
using CartActivator;
|
||||
using FundamentalLib;
|
||||
using MCUSerialBridgeCLR;
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.IO;
|
||||
|
||||
namespace MedullaAdapter
|
||||
{
|
||||
//[LogicRunOnMCU(scanInterval = 20)]
|
||||
public class MCURoutine:LadderLogic<DiverCartDefinition>
|
||||
public class MCURoutine : LadderLogic<DiverCartDefinition>
|
||||
{
|
||||
private int _lastIteration = 0;
|
||||
private int _count = 0;
|
||||
@@ -26,25 +27,35 @@ namespace MedullaAdapter
|
||||
private bool io_bit4 = false;//黄灯
|
||||
private const byte BatteryPortIndex = 3;
|
||||
private static readonly byte[] BatteryRequest = BuildBatteryRequest();
|
||||
private readonly WheelSpeedDiagnosticLogger
|
||||
_wheelSpeedLogger =
|
||||
new WheelSpeedDiagnosticLogger();
|
||||
|
||||
// M层单车底盘：将车轮线速度换算为驱动电机转速。
|
||||
private static float ConvertMps2Rpm(float mps)
|
||||
{
|
||||
return (float)(mps / (Math.PI * 85f) * 10.5f * 60f * 1000f);
|
||||
}
|
||||
// M层单车底盘：将驱动电机转速换算为车轮线速度。
|
||||
private static float ConvertRpm2Mps(float rpm)
|
||||
{
|
||||
return (float)(rpm / 10.5f / 60f * Math.PI * 85f / 1000f);
|
||||
}
|
||||
// M层单车底盘：将夹臂或执行器转数换算为毫米位移。
|
||||
private float ConvertR2MM(float r)
|
||||
{
|
||||
return (float)(r / 10.5f * Math.PI * 85);
|
||||
}
|
||||
// M层CAN解析：从驱动器报文中解码有符号转速。
|
||||
private static float DecodeRpmFromPayload(byte[] payload, int offset = 4)
|
||||
{
|
||||
return BitConverter.ToInt32(payload, offset) * 1875f / 512f / 10000f;
|
||||
}
|
||||
// M层硬件主循环：交换IO、发送轮组指令并更新车辆反馈状态。
|
||||
public override void Operation(int iteration)
|
||||
{
|
||||
UpdateWheelSpeedDiagnosticState();
|
||||
|
||||
if (_lastIteration != iteration)
|
||||
{
|
||||
_lastIteration = iteration;
|
||||
@@ -153,6 +164,8 @@ namespace MedullaAdapter
|
||||
cart.ActualSpeedLeftRear = (cart.ActualSpeedLeftRearLeft + cart.ActualSpeedLeftRearRight) / 2;
|
||||
cart.ActualSpeedRightFront = (cart.ActualSpeedRightFrontLeft + cart.ActualSpeedRightFrontRight) / 2;
|
||||
cart.ActualSpeedRightRear = (cart.ActualSpeedRightRearLeft + cart.ActualSpeedRightRearRight) / 2;
|
||||
_wheelSpeedLogger.RecordSnapshot(cart);
|
||||
// M层CAN辅助：封装本周期驱动器CAN发送参数。
|
||||
MCUSerialBridgeError SendCan(byte port, ushort standardId, byte[] payload, bool RTR = false, uint timeout = 2)
|
||||
{
|
||||
var canSend = new CANMessage
|
||||
@@ -260,6 +273,7 @@ namespace MedullaAdapter
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
// 没有错误 没有节点保护 就发06 07 0F使能
|
||||
else if (_operationTime == 4)
|
||||
{
|
||||
@@ -417,16 +431,91 @@ namespace MedullaAdapter
|
||||
{
|
||||
//_errorCount++;
|
||||
}
|
||||
|
||||
|
||||
#endregion
|
||||
|
||||
}
|
||||
|
||||
// M层诊断：根据界面开关创建或关闭本次轮速CSV记录。
|
||||
private void UpdateWheelSpeedDiagnosticState()
|
||||
{
|
||||
if (cart == null)
|
||||
return;
|
||||
|
||||
if (cart.WheelSpeedDiagnosticEnabled)
|
||||
{
|
||||
if (_wheelSpeedLogger.IsRunning)
|
||||
return;
|
||||
|
||||
try
|
||||
{
|
||||
var configuredDirectory =
|
||||
string.IsNullOrWhiteSpace(
|
||||
cart.WheelSpeedDiagnosticDirectory)
|
||||
? @"logs\wheel-speed"
|
||||
: cart.WheelSpeedDiagnosticDirectory;
|
||||
|
||||
var logDirectory =
|
||||
Path.IsPathRooted(configuredDirectory)
|
||||
? configuredDirectory
|
||||
: Path.Combine(
|
||||
AppContext.BaseDirectory,
|
||||
configuredDirectory);
|
||||
|
||||
_wheelSpeedLogger.Start(
|
||||
logDirectory,
|
||||
cart.CarNum);
|
||||
|
||||
cart.WheelSpeedDiagnosticStatus =
|
||||
"记录中：" +
|
||||
_wheelSpeedLogger.SnapshotLogPath;
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
cart.WheelSpeedDiagnosticEnabled =
|
||||
false;
|
||||
cart.WheelSpeedDiagnosticStatus =
|
||||
"启动失败：" + ex.Message;
|
||||
|
||||
Console.WriteLine(
|
||||
"轮速诊断启动失败：" +
|
||||
ex.Message);
|
||||
}
|
||||
|
||||
return;
|
||||
}
|
||||
|
||||
if (!_wheelSpeedLogger.IsRunning)
|
||||
return;
|
||||
|
||||
try
|
||||
{
|
||||
var snapshotPath =
|
||||
_wheelSpeedLogger.SnapshotLogPath;
|
||||
|
||||
_wheelSpeedLogger.Stop();
|
||||
|
||||
cart.WheelSpeedDiagnosticStatus =
|
||||
"已保存：" + snapshotPath;
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
cart.WheelSpeedDiagnosticStatus =
|
||||
"停止失败：" + ex.Message;
|
||||
|
||||
Console.WriteLine(
|
||||
"轮速诊断停止失败：" +
|
||||
ex.Message);
|
||||
}
|
||||
}
|
||||
|
||||
// M层CAN安全：判断单个驱动节点是否处于可运行状态。
|
||||
private static bool IsNodeOperational(byte remoteCode)
|
||||
{
|
||||
return remoteCode == 5 || remoteCode == 133;
|
||||
}
|
||||
|
||||
// M层CAN安全：检查全部车轮和夹臂节点是否已上线。
|
||||
private bool AreAllNodesOperational()
|
||||
{
|
||||
return IsNodeOperational(cart.LFLRemoteCode)
|
||||
@@ -441,6 +530,7 @@ namespace MedullaAdapter
|
||||
&& IsNodeOperational(cart.RArmRemoteCode);
|
||||
}
|
||||
|
||||
// M层CAN维护：轮询所有驱动节点的在线状态。
|
||||
private static void SendNodeGuardRequests(Func<byte, ushort, byte[], bool, uint, MCUSerialBridgeError> sendCan)
|
||||
{
|
||||
sendCan(0, 0x709, Array.Empty<byte>(), true, 2);
|
||||
@@ -455,6 +545,7 @@ namespace MedullaAdapter
|
||||
sendCan(0, 0x708, Array.Empty<byte>(), true, 2);
|
||||
}
|
||||
|
||||
// M层CAN通信：按需注册驱动器反馈报文回调。
|
||||
private void EnsureCanCallbacksRegistered()
|
||||
{
|
||||
if (_canCallbackRegistered)
|
||||
@@ -475,9 +566,12 @@ namespace MedullaAdapter
|
||||
handler(msg);
|
||||
}
|
||||
});
|
||||
_canCallbackRegistered = true;
|
||||
// _canCallbackRegistered = true;
|
||||
_canCallbackRegistered =
|
||||
err0 == MCUSerialBridgeError.OK;
|
||||
}
|
||||
|
||||
// M层串口通信：按需注册电池等串口设备回调。
|
||||
private void EnsureSerialCallbacksRegistered()
|
||||
{
|
||||
if (_serialCallbackRegistered)
|
||||
@@ -497,6 +591,7 @@ namespace MedullaAdapter
|
||||
_serialCallbackRegistered = err1 == MCUSerialBridgeError.OK;
|
||||
}
|
||||
|
||||
// M层单车电源：周期发送Modbus电池状态查询。
|
||||
private void PollBatterySerial(int iteration)
|
||||
{
|
||||
if (cart?.Bridge == null) return;
|
||||
@@ -522,6 +617,7 @@ namespace MedullaAdapter
|
||||
TryUpdateBatteryData(msg);
|
||||
}
|
||||
|
||||
// M层单车电源：校验并解析电池Modbus响应。
|
||||
private void TryUpdateBatteryData(byte[] msg)
|
||||
{
|
||||
// Modbus RTU response: [id,03,08,data(8),crc(2)]
|
||||
@@ -536,6 +632,7 @@ namespace MedullaAdapter
|
||||
cart.ElectricCurrent = -ReadInt16BE(msg, 9) * 0.1f;
|
||||
}
|
||||
|
||||
// M层单车电源：构造读取电池寄存器的Modbus请求。
|
||||
private static byte[] BuildBatteryRequest()
|
||||
{
|
||||
// 01 03 00 03 00 04 CRC (读取寄存器 03~06)
|
||||
@@ -546,6 +643,7 @@ namespace MedullaAdapter
|
||||
return req;
|
||||
}
|
||||
|
||||
// M层协议辅助：计算Modbus RTU的CRC16校验值。
|
||||
private static ushort ComputeModbusCrc(byte[] data, int length)
|
||||
{
|
||||
ushort crc = 0xFFFF;
|
||||
@@ -562,16 +660,19 @@ namespace MedullaAdapter
|
||||
return crc;
|
||||
}
|
||||
|
||||
// M层协议辅助：按大端序读取有符号16位数值。
|
||||
private static short ReadInt16BE(byte[] data, int offset)
|
||||
{
|
||||
return (short)((data[offset] << 8) | data[offset + 1]);
|
||||
}
|
||||
|
||||
// M层协议辅助：按大端序读取无符号16位数值。
|
||||
private static ushort ReadUInt16BE(byte[] data, int offset)
|
||||
{
|
||||
return (ushort)((data[offset] << 8) | data[offset + 1]);
|
||||
}
|
||||
|
||||
// M层CAN解析：建立驱动器报文标识符到处理函数的分发表。
|
||||
private Dictionary<ushort, Action<CANMessage>> BuildCanDispatchTable()
|
||||
{
|
||||
return new Dictionary<ushort, Action<CANMessage>>
|
||||
@@ -583,7 +684,13 @@ namespace MedullaAdapter
|
||||
var payload = msg.Payload;
|
||||
if (payload == null || payload.Length < 8) return;
|
||||
var rpm = DecodeRpmFromPayload(payload);
|
||||
cart.ActualSpeedLeftFrontLeft = ConvertRpm2Mps(rpm);
|
||||
var speed = ConvertRpm2Mps(rpm);
|
||||
cart.ActualSpeedLeftFrontLeft = speed;
|
||||
_wheelSpeedLogger.RecordCanFeedback(
|
||||
0x281,
|
||||
"LFL",
|
||||
rpm,
|
||||
speed);
|
||||
cart.LFLActualPos = ConvertR2MM(BitConverter.ToInt32(payload, 0) / 10000f);
|
||||
},
|
||||
[0x282] = (msg) =>
|
||||
@@ -592,7 +699,13 @@ namespace MedullaAdapter
|
||||
var payload = msg.Payload;
|
||||
if (payload == null || payload.Length < 8) return;
|
||||
var rpm = DecodeRpmFromPayload(payload);
|
||||
cart.ActualSpeedLeftFrontRight = -ConvertRpm2Mps(rpm);
|
||||
var speed = -ConvertRpm2Mps(rpm);
|
||||
cart.ActualSpeedLeftFrontRight = speed;
|
||||
_wheelSpeedLogger.RecordCanFeedback(
|
||||
0x282,
|
||||
"LFR",
|
||||
rpm,
|
||||
speed);
|
||||
cart.LFRActualPos = ConvertR2MM(-BitConverter.ToInt32(payload, 0) / 10000f);
|
||||
},
|
||||
[0x283] = (msg) =>
|
||||
@@ -601,7 +714,13 @@ namespace MedullaAdapter
|
||||
var payload = msg.Payload;
|
||||
if (payload == null || payload.Length < 8) return;
|
||||
var rpm = DecodeRpmFromPayload(payload);
|
||||
cart.ActualSpeedRightFrontLeft = ConvertRpm2Mps(rpm);
|
||||
var speed = ConvertRpm2Mps(rpm);
|
||||
cart.ActualSpeedRightFrontLeft = speed;
|
||||
_wheelSpeedLogger.RecordCanFeedback(
|
||||
0x283,
|
||||
"RFL",
|
||||
rpm,
|
||||
speed);
|
||||
cart.RFLActualPos = ConvertR2MM(BitConverter.ToInt32(payload, 0) / 10000f);
|
||||
},
|
||||
[0x284] = (msg) =>
|
||||
@@ -610,7 +729,13 @@ namespace MedullaAdapter
|
||||
var payload = msg.Payload;
|
||||
if (payload == null || payload.Length < 8) return;
|
||||
var rpm = DecodeRpmFromPayload(payload);
|
||||
cart.ActualSpeedRightFrontRight = -ConvertRpm2Mps(rpm);
|
||||
var speed = -ConvertRpm2Mps(rpm);
|
||||
cart.ActualSpeedRightFrontRight = speed;
|
||||
_wheelSpeedLogger.RecordCanFeedback(
|
||||
0x284,
|
||||
"RFR",
|
||||
rpm,
|
||||
speed);
|
||||
cart.RFRActualPos = ConvertR2MM(-BitConverter.ToInt32(payload, 0) / 10000f);
|
||||
},
|
||||
[0x285] = (msg) =>
|
||||
@@ -619,7 +744,13 @@ namespace MedullaAdapter
|
||||
var payload = msg.Payload;
|
||||
if (payload == null || payload.Length < 8) return;
|
||||
var rpm = DecodeRpmFromPayload(payload);
|
||||
cart.ActualSpeedLeftRearLeft = ConvertRpm2Mps(rpm);
|
||||
var speed = ConvertRpm2Mps(rpm);
|
||||
cart.ActualSpeedLeftRearLeft = speed;
|
||||
_wheelSpeedLogger.RecordCanFeedback(
|
||||
0x285,
|
||||
"LRL",
|
||||
rpm,
|
||||
speed);
|
||||
cart.LRLActualPos = ConvertR2MM(BitConverter.ToInt32(payload, 0) / 10000f);
|
||||
},
|
||||
[0x286] = (msg) =>
|
||||
@@ -628,7 +759,13 @@ namespace MedullaAdapter
|
||||
var payload = msg.Payload;
|
||||
if (payload == null || payload.Length < 8) return;
|
||||
var rpm = DecodeRpmFromPayload(payload);
|
||||
cart.ActualSpeedLeftRearRight = -ConvertRpm2Mps(rpm);
|
||||
var speed = -ConvertRpm2Mps(rpm);
|
||||
cart.ActualSpeedLeftRearRight = speed;
|
||||
_wheelSpeedLogger.RecordCanFeedback(
|
||||
0x286,
|
||||
"LRR",
|
||||
rpm,
|
||||
speed);
|
||||
cart.LRRActualPos = ConvertR2MM(-BitConverter.ToInt32(payload, 0) / 10000f);
|
||||
},
|
||||
[0x287] = (msg) =>
|
||||
@@ -637,7 +774,13 @@ namespace MedullaAdapter
|
||||
var payload = msg.Payload;
|
||||
if (payload == null || payload.Length < 8) return;
|
||||
var rpm = DecodeRpmFromPayload(payload);
|
||||
cart.ActualSpeedRightRearLeft = ConvertRpm2Mps(rpm);
|
||||
var speed = ConvertRpm2Mps(rpm);
|
||||
cart.ActualSpeedRightRearLeft = speed;
|
||||
_wheelSpeedLogger.RecordCanFeedback(
|
||||
0x287,
|
||||
"RRL",
|
||||
rpm,
|
||||
speed);
|
||||
cart.RRLActualPos = ConvertR2MM(BitConverter.ToInt32(payload, 0) / 10000f);
|
||||
},
|
||||
[0x288] = (msg) =>
|
||||
@@ -646,7 +789,13 @@ namespace MedullaAdapter
|
||||
var payload = msg.Payload;
|
||||
if (payload == null || payload.Length < 8) return;
|
||||
var rpm = DecodeRpmFromPayload(payload);
|
||||
cart.ActualSpeedRightRearRight = -ConvertRpm2Mps(rpm);
|
||||
var speed = -ConvertRpm2Mps(rpm);
|
||||
cart.ActualSpeedRightRearRight = speed;
|
||||
_wheelSpeedLogger.RecordCanFeedback(
|
||||
0x288,
|
||||
"RRR",
|
||||
rpm,
|
||||
speed);
|
||||
cart.RRRActualPos = ConvertR2MM(-BitConverter.ToInt32(payload, 0) / 10000f);
|
||||
},
|
||||
[0x289] = (msg) =>
|
||||
@@ -784,7 +933,7 @@ namespace MedullaAdapter
|
||||
{
|
||||
//Console.WriteLine($"Received 0x18D CAN Message {DateTime.Now:yyyy-MM-dd HH:mm:ss.ffffff}");
|
||||
var payload = msg.Payload;
|
||||
if (payload == null || payload.Length < 4) return;
|
||||
if (payload == null || payload.Length < 4) return;
|
||||
cart.ActualThLeftRear = BitConverter.ToInt32(payload, 0);
|
||||
cart.ActualThLeftRear = cart.ActualThLeftRear >= 16384
|
||||
? (cart.ActualThLeftRear - 98303) / 4096f / 5f * 360 - cart.ThBiasLeftRear
|
||||
|
||||
@@ -1,3 +1,5 @@
|
||||
// C#调用MCU通信桥
|
||||
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Linq;
|
||||
@@ -102,6 +104,7 @@ namespace MCUSerialBridgeCLR
|
||||
/// 转换为可读字符串
|
||||
/// </summary>
|
||||
/// <returns>返回包含产品、Tag、Commit、BuildTime 的字符串</returns>
|
||||
// M层MCU适配：格式化固件版本信息便于日志显示。
|
||||
public override string ToString()
|
||||
{
|
||||
return $"Product: {ProductionName}, Tag: {GitTag}, Commit: {GitCommit}, Built: {BuildTime}";
|
||||
@@ -144,6 +147,7 @@ namespace MCUSerialBridgeCLR
|
||||
/// 返回可读的状态字符串
|
||||
/// </summary>
|
||||
/// <returns>例如 "Bridge: Running" 或 "DIVER: Error"</returns>
|
||||
// M层MCU适配：格式化MCU运行状态便于日志显示。
|
||||
public override string ToString()
|
||||
{
|
||||
string modeStr = IsBridge ? "Bridge" : "DIVER";
|
||||
@@ -174,6 +178,7 @@ namespace MCUSerialBridgeCLR
|
||||
|
||||
/// <summary>序列化端口配置为字节数组（供 P/Invoke 使用）</summary>
|
||||
/// <returns>返回固定长度字节数组（16 bytes）</returns>
|
||||
// M层MCU适配：将端口配置序列化为原生接口字节。
|
||||
public abstract byte[] ToBytes();
|
||||
}
|
||||
|
||||
@@ -185,6 +190,7 @@ namespace MCUSerialBridgeCLR
|
||||
/// </remarks>
|
||||
/// <param name="baud">波特率</param>
|
||||
/// <param name="receiveFrameMs">接收帧间隔</param>
|
||||
// M层MCU适配：创建串口通信参数配置。
|
||||
public class SerialPortConfig(uint baud, uint receiveFrameMs) : PortConfig
|
||||
{
|
||||
/// <summary>Serial 类型</summary>
|
||||
@@ -200,6 +206,7 @@ namespace MCUSerialBridgeCLR
|
||||
/// 转换为字节数组
|
||||
/// </summary>
|
||||
/// <returns>16 字节数组</returns>
|
||||
// M层MCU适配：序列化串口波特率和组帧时间。
|
||||
public override byte[] ToBytes()
|
||||
{
|
||||
var c = new PortStructHelper.SerialPortConfigC
|
||||
@@ -221,6 +228,7 @@ namespace MCUSerialBridgeCLR
|
||||
/// </remarks>
|
||||
/// <param name="baud">波特率</param>
|
||||
/// <param name="retryTimeMs">重发间隔</param>
|
||||
// M层MCU适配：创建CAN通信参数配置。
|
||||
public class CANPortConfig(uint baud, uint retryTimeMs) : PortConfig
|
||||
{
|
||||
/// <summary>CAN 类型</summary>
|
||||
@@ -236,6 +244,7 @@ namespace MCUSerialBridgeCLR
|
||||
/// 转换为字节数组
|
||||
/// </summary>
|
||||
/// <returns>16 字节数组</returns>
|
||||
// M层MCU适配：序列化CAN波特率和重试时间。
|
||||
public override byte[] ToBytes()
|
||||
{
|
||||
var c = new PortStructHelper.CANPortConfigC
|
||||
@@ -272,6 +281,7 @@ namespace MCUSerialBridgeCLR
|
||||
/// <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)
|
||||
@@ -303,6 +313,7 @@ namespace MCUSerialBridgeCLR
|
||||
/// <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)
|
||||
@@ -332,6 +343,7 @@ namespace MCUSerialBridgeCLR
|
||||
return msg;
|
||||
}
|
||||
|
||||
// M层CAN诊断：格式化CAN标识符和数据内容。
|
||||
public override string ToString()
|
||||
{
|
||||
string payloadStr =
|
||||
@@ -351,6 +363,7 @@ namespace MCUSerialBridgeCLR
|
||||
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,
|
||||
@@ -358,12 +371,15 @@ namespace MCUSerialBridgeCLR
|
||||
);
|
||||
|
||||
[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,
|
||||
@@ -371,6 +387,7 @@ namespace MCUSerialBridgeCLR
|
||||
);
|
||||
|
||||
[DllImport(DLL, CallingConvention = CallingConvention.Cdecl)]
|
||||
// M层原生接口：读取MCU当前运行状态。
|
||||
public static extern MCUSerialBridgeError mcu_state(
|
||||
IntPtr handle,
|
||||
out MCUState state,
|
||||
@@ -378,6 +395,7 @@ namespace MCUSerialBridgeCLR
|
||||
);
|
||||
|
||||
[DllImport(DLL, CallingConvention = CallingConvention.Cdecl)]
|
||||
// M层原生接口：下发串口桥端口配置。
|
||||
internal static extern MCUSerialBridgeError msb_configure(
|
||||
IntPtr handle,
|
||||
uint num_ports,
|
||||
@@ -386,6 +404,7 @@ namespace MCUSerialBridgeCLR
|
||||
);
|
||||
|
||||
[DllImport(DLL, CallingConvention = CallingConvention.Cdecl)]
|
||||
// M层原生接口：读取MCU数字输入。
|
||||
internal static extern MCUSerialBridgeError msb_read_input(
|
||||
IntPtr handle,
|
||||
[Out] byte[] inputs,
|
||||
@@ -393,6 +412,7 @@ namespace MCUSerialBridgeCLR
|
||||
);
|
||||
|
||||
[DllImport(DLL, CallingConvention = CallingConvention.Cdecl)]
|
||||
// M层原生接口：写入MCU数字输出。
|
||||
internal static extern MCUSerialBridgeError msb_write_output(
|
||||
IntPtr handle,
|
||||
[In] byte[] outputs,
|
||||
@@ -400,6 +420,7 @@ namespace MCUSerialBridgeCLR
|
||||
);
|
||||
|
||||
[DllImport(DLL, CallingConvention = CallingConvention.Cdecl)]
|
||||
// M层原生接口：从指定串口或CAN端口读取数据。
|
||||
internal static extern MCUSerialBridgeError msb_read_port(
|
||||
IntPtr handle,
|
||||
byte port_index,
|
||||
@@ -410,6 +431,7 @@ namespace MCUSerialBridgeCLR
|
||||
);
|
||||
|
||||
[UnmanagedFunctionPointer(CallingConvention.Cdecl)]
|
||||
// M层硬件桥接：定义串口或CAN端口收到原生数据时的回调签名。
|
||||
internal delegate void msb_on_port_data_callback_function_t(
|
||||
IntPtr dst_data,
|
||||
uint dst_data_size,
|
||||
@@ -417,6 +439,7 @@ namespace MCUSerialBridgeCLR
|
||||
);
|
||||
|
||||
[DllImport(DLL, CallingConvention = CallingConvention.Cdecl)]
|
||||
// M层原生接口：注册端口数据到达回调。
|
||||
internal static extern MCUSerialBridgeError msb_register_port_data_callback(
|
||||
IntPtr handle,
|
||||
byte port_index,
|
||||
@@ -425,6 +448,7 @@ namespace MCUSerialBridgeCLR
|
||||
);
|
||||
|
||||
[DllImport(DLL, CallingConvention = CallingConvention.Cdecl)]
|
||||
// M层原生接口：向指定串口或CAN端口写入数据。
|
||||
internal static extern MCUSerialBridgeError msb_write_port(
|
||||
IntPtr handle,
|
||||
byte port_index,
|
||||
@@ -448,18 +472,21 @@ namespace MCUSerialBridgeCLR
|
||||
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);
|
||||
@@ -468,6 +495,7 @@ namespace MCUSerialBridgeCLR
|
||||
|
||||
/// <summary>内部释放资源方法</summary>
|
||||
/// <param name="disposing">true 表示手动释放，false 表示析构释放</param>
|
||||
// M层MCU适配：按托管或终结路径关闭原生句柄。
|
||||
private void Dispose(bool disposing)
|
||||
{
|
||||
if (nativeHandle != IntPtr.Zero)
|
||||
@@ -481,6 +509,7 @@ namespace MCUSerialBridgeCLR
|
||||
/// <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);
|
||||
@@ -488,6 +517,7 @@ namespace MCUSerialBridgeCLR
|
||||
|
||||
/// <summary>关闭串口</summary>
|
||||
/// <returns>错误码</returns>
|
||||
// M层单车通信：关闭当前MCU串口桥连接。
|
||||
public MCUSerialBridgeError Close()
|
||||
{
|
||||
if (nativeHandle == IntPtr.Zero)
|
||||
@@ -500,6 +530,7 @@ namespace MCUSerialBridgeCLR
|
||||
|
||||
/// <summary>MCU 复位</summary>
|
||||
/// <returns>错误码</returns>
|
||||
// M层单车通信：请求MCU复位并等待结果。
|
||||
public MCUSerialBridgeError Reset(uint timeout = 200)
|
||||
{
|
||||
if (nativeHandle == IntPtr.Zero)
|
||||
@@ -512,6 +543,7 @@ namespace MCUSerialBridgeCLR
|
||||
/// <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();
|
||||
@@ -525,6 +557,7 @@ namespace MCUSerialBridgeCLR
|
||||
/// <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();
|
||||
@@ -538,6 +571,7 @@ namespace MCUSerialBridgeCLR
|
||||
/// <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)
|
||||
@@ -587,6 +621,7 @@ namespace MCUSerialBridgeCLR
|
||||
/// <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];
|
||||
@@ -600,6 +635,7 @@ namespace MCUSerialBridgeCLR
|
||||
/// <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)
|
||||
@@ -629,6 +665,7 @@ namespace MCUSerialBridgeCLR
|
||||
/// - NoData 当前无可读数据（仅在 timeout == 0 或等待超时）
|
||||
/// - Win_InvalidParam 参数错误
|
||||
/// </returns>
|
||||
// M层串口通信：同步读取指定MCU串口的数据。
|
||||
public MCUSerialBridgeError ReadSerial(byte portIndex, out byte[] buffer, uint timeout)
|
||||
{
|
||||
buffer = Array.Empty<byte>();
|
||||
@@ -670,6 +707,7 @@ namespace MCUSerialBridgeCLR
|
||||
/// - OK 成功发送
|
||||
/// - 其他错误请查看 MCUSerialBridgeError
|
||||
/// </returns>
|
||||
// M层串口通信：向指定MCU串口发送数据。
|
||||
public MCUSerialBridgeError WriteSerial(byte portIndex, byte[] data, uint timeout)
|
||||
{
|
||||
if (nativeHandle == IntPtr.Zero)
|
||||
@@ -706,6 +744,7 @@ namespace MCUSerialBridgeCLR
|
||||
/// - CAN_DataError CAN数据错误
|
||||
/// - Win_HandleNotFound 句柄无效
|
||||
/// </returns>
|
||||
// M层CAN通信：同步读取指定CAN通道的一帧消息。
|
||||
public MCUSerialBridgeError ReadCAN(byte portIndex, out CANMessage message, uint timeout)
|
||||
{
|
||||
message = null;
|
||||
@@ -753,6 +792,7 @@ namespace MCUSerialBridgeCLR
|
||||
/// - CAN_DataError CAN 数据错误
|
||||
/// - Win_HandleNotFound 句柄无效
|
||||
/// </returns>
|
||||
// M层CAN通信：向指定CAN通道发送一帧消息。
|
||||
public MCUSerialBridgeError WriteCAN(byte portIndex, CANMessage message, uint timeout)
|
||||
{
|
||||
if (nativeHandle == IntPtr.Zero)
|
||||
@@ -796,6 +836,7 @@ namespace MCUSerialBridgeCLR
|
||||
/// 5. 数据可能随时到来，请保证回调尽快返回，避免影响后续帧接收。
|
||||
/// 6. 不要把其他类型的端口注册到这个接口，接口不对 portIndex 做类型检查。
|
||||
/// </remarks>
|
||||
// M层串口通信：注册指定串口的异步接收回调。
|
||||
public MCUSerialBridgeError RegisterSerialPortCallback(
|
||||
byte portIndex,
|
||||
Action<byte[]> callback
|
||||
@@ -808,6 +849,7 @@ namespace MCUSerialBridgeCLR
|
||||
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];
|
||||
@@ -842,6 +884,7 @@ namespace MCUSerialBridgeCLR
|
||||
/// 5. 数据可能随时到来，请保证回调尽快返回，避免影响后续帧接收。
|
||||
/// 6. 不要把其他类型的端口注册到这个接口，接口不对 portIndex 做类型检查。
|
||||
/// </remarks>
|
||||
// M层CAN通信：注册指定CAN通道的异步接收回调。
|
||||
public MCUSerialBridgeError RegisterCANPortCallback(
|
||||
byte portIndex,
|
||||
Action<CANMessage> callback
|
||||
@@ -854,6 +897,7 @@ namespace MCUSerialBridgeCLR
|
||||
return MCUSerialBridgeError.Config_PortNumOver;
|
||||
|
||||
// 包装 C# 回调为 P/Invoke 委托
|
||||
// M层CAN回调：还原原生CAN帧并转交托管回调处理。
|
||||
void del(IntPtr dst_data, uint dst_data_size, IntPtr user_ctx)
|
||||
{
|
||||
try
|
||||
|
||||
@@ -1,4 +1,4 @@
|
||||
using System;
|
||||
// MCU通信错误码
|
||||
|
||||
namespace MCUSerialBridgeCLR
|
||||
{
|
||||
@@ -47,6 +47,7 @@ namespace MCUSerialBridgeCLR
|
||||
|
||||
public static class MCUSerialBridgeErrorExtensions
|
||||
{
|
||||
// M层MCU适配：把串口桥错误码转换为便于诊断的说明。
|
||||
public static string ToDescription(this MCUSerialBridgeError err)
|
||||
{
|
||||
return err switch
|
||||
|
||||
@@ -1,44 +1,58 @@
|
||||
<Project Sdk="Microsoft.NET.Sdk">
|
||||
|
||||
<PropertyGroup>
|
||||
<TargetFramework>netstandard2.0</TargetFramework>
|
||||
<LangVersion>latest</LangVersion>
|
||||
<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>
|
||||
<Compile Remove="EmbeddedCommunication.cs" />
|
||||
</ItemGroup>
|
||||
|
||||
<ItemGroup>
|
||||
<PackageReference Include="Fody" Version="6.9.3">
|
||||
<PrivateAssets>all</PrivateAssets>
|
||||
<IncludeAssets>runtime; build; native; contentfiles; analyzers; buildtransitive</IncludeAssets>
|
||||
</PackageReference>
|
||||
<PackageReference Include="Newtonsoft.Json" Version="13.0.4" />
|
||||
<PackageReference Include="System.IO.Ports" Version="6.0.0" />
|
||||
<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>
|
||||
<Reference Include="CommonUsage">
|
||||
<HintPath>parkingRobotRef\CommonUsage.dll</HintPath>
|
||||
</Reference>
|
||||
<Reference Include="CycleGUI">
|
||||
<HintPath>ref\CycleGUI.dll</HintPath>
|
||||
</Reference>
|
||||
<Reference Include="MDCSToolBox">
|
||||
<HintPath>parkingRobotRef\MDCSToolBox.dll</HintPath>
|
||||
</Reference>
|
||||
<Reference Include="CartActivator">
|
||||
<HintPath>parkingRobotRef\RefCartActivator.dll</HintPath>
|
||||
</Reference>
|
||||
<Reference Include="FundamentalLib">
|
||||
<HintPath>parkingRobotRef\RefFundamentalLib.dll</HintPath>
|
||||
</Reference>
|
||||
<Reference Include="MedullaCore">
|
||||
<HintPath>parkingRobotRef\RefMedullaCore.dll</HintPath>
|
||||
</Reference>
|
||||
</ItemGroup>
|
||||
<ItemGroup>
|
||||
<WeaverFiles Include="deps\DiverCompiler.exe" />
|
||||
</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>
|
||||
|
||||
+312
-181
@@ -1,233 +1,364 @@
|
||||
using CartActivator;
|
||||
using CommonUsage.Mathematics;
|
||||
// 计算8个驱动电机的目标速度和舵角PID
|
||||
using CartActivator;
|
||||
using FundamentalLib;
|
||||
using MDCSToolBox.Commons;
|
||||
using MDCSToolBox.Commons.Controllers;
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Linq;
|
||||
using static MDCSToolBox.Medulla.Chassis.BasicCartDefinition;
|
||||
|
||||
namespace MedullaAdapter
|
||||
{
|
||||
public class MotorRoutine:LadderLogic<DiverCartDefinition>
|
||||
public class MotorRoutine : LadderLogic<DiverCartDefinition>
|
||||
{
|
||||
private bool _wasTransmitterControlling;
|
||||
private DateTime _lastMoveTime = DateTime.Now;
|
||||
private DateTime _transmitterFleetDbgLastLog = DateTime.MinValue;
|
||||
private DateTime _transmitterFleetResetLastLog = DateTime.MinValue;
|
||||
|
||||
private void LogTransmitterFleet(string branch, bool force = false)
|
||||
{
|
||||
var now = DateTime.Now;
|
||||
if (!force && (now - _transmitterFleetDbgLastLog).TotalMilliseconds < 200) return;
|
||||
_transmitterFleetDbgLastLog = now;
|
||||
DLog.Log(
|
||||
$"branch={branch} connected={cart.TransmitterConnected} ctrl={cart.TransmitterControlEnable} " +
|
||||
$"SA={cart.Transmitter_SA} SC={cart.Transmitter_SC} SB={cart.Transmitter_SB} SD={cart.Transmitter_SD} car={cart.CarNum} " +
|
||||
$"joyLx={cart.TransmitterLeftJoystickValX:0.000} joyRy={cart.TransmitterRightJoystickValY:0.000} joyRx={cart.TransmitterRightJoystickValX:0.000} " +
|
||||
$"rawLx={cart.TransmitterLeftJoystickValXRaw} rawRy={cart.TransmitterRightJoystickValYRaw} " +
|
||||
$"remotePanel={cart.MultiVehicleRemoteManualEnabled} outEn={cart.MultiVehicleManualEnabled} outMode={cart.MultiVehicleManualMode} " +
|
||||
$"outVx={cart.MultiVehicleManualVx:0.000} outVy={cart.MultiVehicleManualVy:0.000} outVth={cart.MultiVehicleManualVth:0.000} hold={cart.MultiVehicleHold}",
|
||||
"TransmitterFleetDbg");
|
||||
}
|
||||
|
||||
private void LogTransmitterFleetReset(string reason)
|
||||
{
|
||||
var now = DateTime.Now;
|
||||
if ((now - _transmitterFleetResetLastLog).TotalMilliseconds < 500) return;
|
||||
_transmitterFleetResetLastLog = now;
|
||||
DLog.Log(
|
||||
$"reset reason={reason} remotePanel={cart.MultiVehicleRemoteManualEnabled} beforeEn={cart.MultiVehicleManualEnabled} " +
|
||||
$"beforeMode={cart.MultiVehicleManualMode} beforeVx={cart.MultiVehicleManualVx:0.000} beforeVy={cart.MultiVehicleManualVy:0.000} " +
|
||||
$"beforeVth={cart.MultiVehicleManualVth:0.000} hold={cart.MultiVehicleHold}",
|
||||
"TransmitterFleetDbg");
|
||||
}
|
||||
|
||||
public override void Operation(int iteration)
|
||||
{
|
||||
void ResetMultiVehicle(string reason)
|
||||
{
|
||||
if (cart.MultiVehicleRemoteManualEnabled)
|
||||
{
|
||||
LogTransmitterFleet("reset-skipped-remote-panel");
|
||||
return;
|
||||
}
|
||||
|
||||
if (cart.MultiVehicleManualEnabled || cart.MultiVehicleManualVx != 0 || cart.MultiVehicleManualVy != 0 ||
|
||||
cart.MultiVehicleManualVth != 0 || cart.MultiVehicleHold)
|
||||
LogTransmitterFleetReset(reason);
|
||||
|
||||
cart.MultiVehicleManualEnabled = false;
|
||||
cart.MultiVehicleManualVx = 0;
|
||||
cart.MultiVehicleManualVy = 0;
|
||||
cart.MultiVehicleManualVth = 0;
|
||||
cart.MultiVehicleHold = false;
|
||||
}
|
||||
|
||||
TriggerOnce(cart.TransmitterConnected && cart.Transmitter_SA, 700,
|
||||
if (!cart.GhostMode && cart.State == -1) return;
|
||||
// SA稳定打开后，使能实体遥控器。
|
||||
TriggerOnce(
|
||||
cart.TransmitterConnected && cart.Transmitter_SA,
|
||||
300,
|
||||
() =>
|
||||
{
|
||||
Console.WriteLine("遥控器使能!");
|
||||
cart.TransmitterControlEnable = true;
|
||||
cart.TransmitterLastTime = DateTime.Now;
|
||||
LogTransmitterFleet("enable", true);
|
||||
});
|
||||
TriggerOnce(cart.TransmitterConnected && !cart.Transmitter_SA, 700,
|
||||
() =>
|
||||
{
|
||||
Console.WriteLine("遥控器断使能!");
|
||||
cart.TransmitterControlEnable = false;
|
||||
LogTransmitterFleet("disable", true);
|
||||
});
|
||||
|
||||
if (cart.TransmitterControlEnable && cart.Transmitter_SC == cart.CarNum)
|
||||
{
|
||||
// 遥控器断连或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");
|
||||
ResetMultiVehicle("single-car-control");
|
||||
LogTransmitterFleet("single-car-control");
|
||||
if (transmitterControlling)
|
||||
{
|
||||
TransmitterChassisControl();
|
||||
cart.TransmitterLastTime = DateTime.Now;
|
||||
cart.CarStatu = "Transmitter控制模式使能";
|
||||
}
|
||||
else if (cart.TransmitterControlEnable && cart.Transmitter_SC == 3)
|
||||
{
|
||||
cart.CarStatu = "Transmitter双车联动模式使能";
|
||||
cart.MultiVehicleManualEnabled = true;
|
||||
var fleetMode = 0;
|
||||
if (cart.Transmitter_SB == TransmitterState.Mode0) fleetMode = 0;
|
||||
else if (cart.Transmitter_SB == TransmitterState.Mode1) fleetMode = 1;
|
||||
else if (cart.Transmitter_SB == TransmitterState.Mode2) fleetMode = 2;
|
||||
cart.ApplyFleetManualCommand(fleetMode, -cart.TransmitterLeftJoystickValX,
|
||||
cart.TransmitterRightJoystickValY, 1f);
|
||||
|
||||
// hold multi-vehicle sync, make speed 0
|
||||
cart.MultiVehicleHold = cart.Transmitter_SD == TransmitterState.Mode1;
|
||||
LogTransmitterFleet("fleet-control");
|
||||
cart.CarStatu = "实体遥控器控制";
|
||||
}
|
||||
else
|
||||
{
|
||||
ResetMultiVehicle("idle-or-sc-mismatch");
|
||||
LogTransmitterFleet("idle-or-sc-mismatch");
|
||||
// 只在遥控器刚刚退出时发送一次停车，
|
||||
// 不能每周期停车，否则会覆盖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.ClumsyControl = true;
|
||||
cart.LeftFrontPid.ChangeParameters(cart.DiffSteerKp, cart.DiffSteerKi, cart.DiffSteerKd, cart.DiffSteerMaxI,
|
||||
cart.DiffSteerDeadZone, cart.DiffSteerThresh, cart.DiffSteerSpeedAcc);
|
||||
cart.LeftRearPid.ChangeParameters(cart.DiffSteerKp, cart.DiffSteerKi, cart.DiffSteerKd, cart.DiffSteerMaxI,
|
||||
cart.DiffSteerDeadZone, cart.DiffSteerThresh, cart.DiffSteerSpeedAcc);
|
||||
cart.RightFrontPid.ChangeParameters(cart.DiffSteerKp, cart.DiffSteerKi, cart.DiffSteerKd, cart.DiffSteerMaxI,
|
||||
cart.DiffSteerDeadZone, cart.DiffSteerThresh, cart.DiffSteerSpeedAcc);
|
||||
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 diffRf = cart.RightFrontPid.GetResponse(cart.ThRightFront, false, false, "RF");
|
||||
var diffLr = cart.LeftRearPid.GetResponse(cart.ThLeftRear, false, false, "LR");
|
||||
var diffRr = cart.RightRearPid.GetResponse(cart.ThRightRear, false, false, "RR");
|
||||
// Hedingben.ToastText($"LF:{diffLf:0.00},LR:{diffLr:0.00},RF:{diffRf:0.00},RR:{diffRr:0.00}", "wheel-pid");
|
||||
DLog.Log($"[wheel pid] {cart.ActualThLeftFront:0.00}->{cart.ThLeftFront:0.00} diff:{CommonMath.ThDiff(cart.ThLeftFront, cart.ActualThLeftFront):0.00} pid:{diffLf:0.00}");
|
||||
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.SpeedRFL = cart.SpeedRightFrontLeft - diffRf;
|
||||
cart.SpeedRFR = cart.SpeedRightFrontRight + diffRf;
|
||||
|
||||
// 左后腿。
|
||||
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;
|
||||
List<float> wheelSpeed = new float[]
|
||||
{
|
||||
Math.Abs(cart.SpeedLFL), Math.Abs(cart.SpeedLFR), Math.Abs(cart.SpeedRFL),
|
||||
Math.Abs(cart.SpeedRFR), Math.Abs(cart.SpeedLRL),Math.Abs(cart.SpeedLRR), Math.Abs(cart.SpeedRRL),
|
||||
Math.Abs(cart.SpeedRRR)
|
||||
}.ToList();
|
||||
var wheelMaxSpeed = wheelSpeed.Max();
|
||||
var speedSign = Math.Sign(cart.ThresSpeed - cart.SendThresSpeed);
|
||||
var acc = Math.Abs(cart.ThresSpeed) > Math.Abs(cart.SendThresSpeed) ? cart.Chassis.AccPerSecond : cart.Chassis.DeAccPerSecond;
|
||||
cart.SendThresSpeed += speedSign * Math.Min(Math.Abs(cart.ThresSpeed - cart.SendThresSpeed),
|
||||
acc * (float)(DateTime.Now - _lastMoveTime).TotalSeconds);
|
||||
if (cart.ThresSpeed < wheelMaxSpeed && cart.SendThresSpeed > wheelMaxSpeed)
|
||||
cart.SendThresSpeed = Math.Min(cart.SendThresSpeed, wheelMaxSpeed);
|
||||
_lastMoveTime = DateTime.Now;
|
||||
}
|
||||
|
||||
//灯光控制
|
||||
if (cart.AlarmLevel == 2)
|
||||
// M层单车限速：按照加速度和减速度平滑更新实际下发速度上限。
|
||||
private void UpdateSendSpeedLimit()
|
||||
{
|
||||
if (cart.Chassis == null)
|
||||
{
|
||||
//故障红灯常亮
|
||||
cart.LightMode = 2;
|
||||
cart.SendThresSpeed = 0;
|
||||
return;
|
||||
}
|
||||
else if (cart.ThresSpeed != 1)
|
||||
|
||||
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)
|
||||
{
|
||||
//避障黄灯常亮
|
||||
cart.LightMode = 3;
|
||||
}
|
||||
else if (!cart.WheelAbleState)
|
||||
{
|
||||
FlipFlop(ref cart.LightMode, 500, 0, 3);
|
||||
}
|
||||
else if (cart.Soc < cart.LowBatteryAlarmThreshold && cart.ElectricCurrent >= -10)
|
||||
{
|
||||
cart.LightMode = 3;
|
||||
}
|
||||
else if (cart.ElectricCurrent < -10)
|
||||
{
|
||||
FlipFlop(ref cart.LightMode, 500, 0, 1);
|
||||
currentLimit = targetLimit;
|
||||
}
|
||||
else
|
||||
{
|
||||
FlipFlop(ref cart.LightMode, 500, 0, 1);
|
||||
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;
|
||||
}
|
||||
public void TransmitterChassisControl()
|
||||
|
||||
// M层单车灯光：根据报警、驱动器、限速和电量状态生成红黄绿灯模式。
|
||||
private void UpdateLightMode()
|
||||
{
|
||||
TriggerOnce(cart.Transmitter_SB == TransmitterState.Mode0, 100,
|
||||
() => cart.TransmitterControlMode = DiverCartDefinition.ManualControlMode.Normal);
|
||||
TriggerOnce(cart.Transmitter_SB == TransmitterState.Mode1, 100,
|
||||
() => cart.TransmitterControlMode = DiverCartDefinition.ManualControlMode.Crab);
|
||||
TriggerOnce(cart.Transmitter_SB == TransmitterState.Mode2, 100,
|
||||
() => cart.TransmitterControlMode = DiverCartDefinition.ManualControlMode.Spin);
|
||||
// 二级报警：红灯常亮。
|
||||
if (cart.AlarmLevel == 2)
|
||||
{
|
||||
cart.LightMode = 2;
|
||||
return;
|
||||
}
|
||||
|
||||
Hedingben.ToastText($"X方向速度: {cart.TransmitterLeftJoystickValX} Y方向速度: {cart.TransmitterRightJoystickValY}");
|
||||
// 一级报警：黄灯常亮。
|
||||
if (cart.AlarmLevel == 1)
|
||||
{
|
||||
cart.LightMode = 3;
|
||||
return;
|
||||
}
|
||||
|
||||
if (cart.Transmitter_SD == TransmitterState.Mode1)
|
||||
// 驱动轮未使能：黄灯闪烁。
|
||||
if (!cart.WheelAbleState)
|
||||
{
|
||||
if(cart.Transmitter_T3 == 1694)
|
||||
cart.WheelReset();
|
||||
else if(cart.Transmitter_T3 == 151)
|
||||
cart.WheelDisable();
|
||||
FlipFlop(ref cart.LightMode, 500, 0, 3);
|
||||
return;
|
||||
}
|
||||
if (cart.Transmitter_SD == TransmitterState.Mode1 && cart.TransmitterLeftJoystickValXRaw == 353)
|
||||
|
||||
// C层正在进行限速：黄灯常亮。
|
||||
if (Math.Abs(cart.ThresSpeed - 1f) > 0.001f)
|
||||
{
|
||||
TriggerOnce(cart.TransmitterRightJoystickValYRaw == 353, 100, () => cart.TransmitterSpeed += 0.1f);
|
||||
TriggerOnce(cart.TransmitterRightJoystickValYRaw == 1694, 100, () => cart.TransmitterSpeed -= 0.1f);
|
||||
cart.LightMode = 3;
|
||||
return;
|
||||
}
|
||||
cart.TransmitterSpeed = Math.Max(Math.Min(cart.TransmitterSpeed, cart.TransmitterSpeedUpperLimit), cart.TransmitterSpeedLowerLimit);
|
||||
//cart.TransmitterSpeed = Math.Max(cart.TransmitterSpeed, cart.TransmitterSpeedLowerLimit);
|
||||
if (!cart.Transmitter_SA)
|
||||
|
||||
// 电量低于预警值：黄灯常亮。
|
||||
if (cart.Soc <= cart.LowBatteryAlarmThreshold)
|
||||
{
|
||||
cart.ManualControl(cart.TransmitterControlMode, 0,
|
||||
0, 0, cart.TransmitterSpeed,
|
||||
DateTime.Now - cart.TransmitterLastTime);
|
||||
cart.SpeedLeftArm = 0;
|
||||
cart.SpeedRightArm = 0;
|
||||
}
|
||||
//摇杆控制底盘
|
||||
else if (cart.Transmitter_SD == TransmitterState.Mode0)
|
||||
{
|
||||
cart.ManualControl(cart.TransmitterControlMode, cart.TransmitterLeftJoystickValX,
|
||||
cart.TransmitterRightJoystickValY, 0, cart.TransmitterSpeed,
|
||||
DateTime.Now - cart.TransmitterLastTime);
|
||||
cart.SpeedLeftArm = 0;
|
||||
cart.SpeedRightArm = 0;
|
||||
}
|
||||
//右摇杆同时控制左右夹臂
|
||||
else if (cart.Transmitter_SD == TransmitterState.Mode1 && cart.TransmitterLeftJoystickValX == 0)
|
||||
{
|
||||
cart.ManualControl(cart.TransmitterControlMode, 0,
|
||||
0, 0, cart.TransmitterSpeed,
|
||||
DateTime.Now - cart.TransmitterLastTime);
|
||||
cart.SpeedLeftArm = cart.TransmitterRightJoystickValX * cart.ManualArmSpeedFac;
|
||||
cart.SpeedRightArm = cart.TransmitterRightJoystickValX * cart.ManualArmSpeedFac;
|
||||
cart.LightMode = 3;
|
||||
return;
|
||||
}
|
||||
|
||||
// 正常运行：绿灯闪烁。
|
||||
FlipFlop(ref cart.LightMode, 500, 0, 1);
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
@@ -1,168 +0,0 @@
|
||||
using System;
|
||||
|
||||
namespace MedullaAdapter
|
||||
{
|
||||
public class BasePack
|
||||
{
|
||||
private static ushort CalculateCRC16(byte[] data)
|
||||
{
|
||||
const ushort polynomial = 0xA001; // CRC-16-IBM多项式
|
||||
ushort crc = 0xFFFF; // 初始值
|
||||
|
||||
foreach (byte b in data)
|
||||
{
|
||||
crc ^= b; // 将数据字节与CRC寄存器按位异或
|
||||
|
||||
for (int i = 0; i < 8; i++)
|
||||
{
|
||||
if ((crc & 0x0001) != 0)
|
||||
{
|
||||
crc >>= 1;
|
||||
crc ^= polynomial;
|
||||
}
|
||||
else
|
||||
{
|
||||
crc >>= 1;
|
||||
}
|
||||
}
|
||||
}
|
||||
return crc;
|
||||
}
|
||||
|
||||
private byte[] _data;
|
||||
|
||||
public void SetData(byte[] data)
|
||||
{
|
||||
_data = data;
|
||||
}
|
||||
|
||||
public byte[] GetPack()
|
||||
{
|
||||
var pack = new byte[_data.Length+7];
|
||||
pack[0] = 0xBB;
|
||||
pack[1] = 0xAA;
|
||||
var length = BitConverter.GetBytes(_data.Length);
|
||||
pack[2] = length[0];
|
||||
pack[3] = length[1];
|
||||
for (int i = 0; i < _data.Length; i++)
|
||||
{
|
||||
pack[4+i] = _data[i];
|
||||
}
|
||||
|
||||
var crcByes = new byte[pack.Length - 5];
|
||||
Array.Copy(pack,2,crcByes,0,crcByes.Length);
|
||||
var crc = BitConverter.GetBytes(CalculateCRC16(crcByes));
|
||||
pack[pack.Length-1] = 0xEE;
|
||||
pack[pack.Length-2] = crc[1];
|
||||
pack[pack.Length-3] = crc[0];
|
||||
return pack;
|
||||
}
|
||||
}
|
||||
|
||||
public class ControlPack : BasePack
|
||||
{
|
||||
public ControlPack(byte controlCode)
|
||||
{
|
||||
byte[] controlData = new byte[3]{0x01,0x00,controlCode};
|
||||
SetData(controlData);
|
||||
}
|
||||
}
|
||||
|
||||
public class SetConfigPack : BasePack
|
||||
{
|
||||
public SetConfigPack(int can1BaudRate, int can2BaudRate,int modbus0BaudRate, int modbus1BaudRate, int modbus2BaudRate,
|
||||
int serialBaudRate,int upperSize = 1024,int lowerSize =1024 )
|
||||
{
|
||||
var upper = BitConverter.GetBytes(upperSize);
|
||||
var lower = BitConverter.GetBytes(lowerSize);
|
||||
var can = BitConverter.GetBytes(can1BaudRate);
|
||||
var can2 = BitConverter.GetBytes(can2BaudRate);
|
||||
var modbus0 = BitConverter.GetBytes(modbus0BaudRate);
|
||||
var modbus1 = BitConverter.GetBytes(modbus1BaudRate);
|
||||
var modbus2 = BitConverter.GetBytes(modbus2BaudRate);
|
||||
var serial = BitConverter.GetBytes(serialBaudRate);
|
||||
var canBuffer = BitConverter.GetBytes(128);
|
||||
var mbBuffer = BitConverter.GetBytes(512);
|
||||
var serialBuffer = BitConverter.GetBytes(1024);
|
||||
byte[] configData = new byte[]
|
||||
{
|
||||
0x01, 0x10, 0x02, upper[0], upper[1], upper[2], upper[3], lower[0], lower[1],
|
||||
lower[2], lower[3], 0x06, 0x00, 0x00, can[0], can[1], can[2], can[3], canBuffer[0], canBuffer[1], 0x00,
|
||||
can2[0], can2[1], can2[2], can2[3], canBuffer[0], canBuffer[1], 0x10, modbus0[0],
|
||||
modbus0[1], modbus0[2], modbus0[3], mbBuffer[0], mbBuffer[1], 0x10, modbus1[0],
|
||||
modbus1[1], modbus1[2], modbus1[3], mbBuffer[0], mbBuffer[1], 0x10, modbus2[0],
|
||||
modbus2[1], modbus2[2], modbus2[3], mbBuffer[0], mbBuffer[1], 0x20, serial[0], serial[1], serial[2],
|
||||
serial[3], serialBuffer[0], serialBuffer[1]
|
||||
};
|
||||
SetData(configData);
|
||||
}
|
||||
}
|
||||
|
||||
public class ReadConfigPack : BasePack
|
||||
{
|
||||
public ReadConfigPack()
|
||||
{
|
||||
var readConfigData = new byte[] { 0x01, 0x10, 0x00 };
|
||||
SetData(readConfigData);
|
||||
}
|
||||
}
|
||||
|
||||
public class DownloadCodePack : BasePack
|
||||
{
|
||||
public DownloadCodePack(int totalLength, int offset, int currentLength, byte[] codeBytes)
|
||||
{
|
||||
var codeData = new byte[12 + codeBytes.Length];
|
||||
codeData[0] = 0x01;
|
||||
codeData[1] = 0x11;
|
||||
var totalLengthBytes = BitConverter.GetBytes(totalLength);
|
||||
for (int i = 0; i < 4; i++)
|
||||
{
|
||||
codeData[2 + i] = totalLengthBytes[i];
|
||||
}
|
||||
var offsetBytes = BitConverter.GetBytes(offset);
|
||||
for (int i = 0; i < 4; i++)
|
||||
{
|
||||
codeData[6 + i] = offsetBytes[i];
|
||||
}
|
||||
var curLengthBytes = BitConverter.GetBytes(currentLength);
|
||||
for (int i = 0; i < 2; i++)
|
||||
{
|
||||
codeData[10 + i] = curLengthBytes[i];
|
||||
}
|
||||
|
||||
for (int i = 0; i < codeBytes.Length; i++)
|
||||
{
|
||||
codeData[12+i] = codeBytes[i];
|
||||
}
|
||||
SetData(codeData);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
public class HeartBeatPack : BasePack
|
||||
{
|
||||
public HeartBeatPack(int state, int error)
|
||||
{
|
||||
var errorBytes = BitConverter.GetBytes(error);
|
||||
var heartBearData = new byte[] { 0x01, 0xF0, (byte)state, errorBytes[0], errorBytes[1] };
|
||||
SetData(heartBearData);
|
||||
}
|
||||
}
|
||||
|
||||
public class UpperIOPack : BasePack
|
||||
{
|
||||
public UpperIOPack(byte[] data,int size)
|
||||
{
|
||||
byte[] UpperData = new byte[6 + size];
|
||||
UpperData[0] = 0x01;
|
||||
UpperData[1] = 0x20;
|
||||
var length = BitConverter.GetBytes(size);
|
||||
for (int i = 0; i < 4; i++)
|
||||
{
|
||||
UpperData[2+i] = length[i];
|
||||
}
|
||||
Array.Copy(data,0,UpperData,6,size);
|
||||
SetData(UpperData);
|
||||
}
|
||||
}
|
||||
}
|
||||
+57
-18
@@ -1,13 +1,51 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Text;
|
||||
using CartActivator;
|
||||
// Medulla虚拟遥控器和夹臂控制
|
||||
using MDCSToolBox.Medulla.Chassis.MultiWheel;
|
||||
using CartActivator;
|
||||
|
||||
namespace MedullaAdapter
|
||||
{
|
||||
public class Remote:MultiWheelRemote<DiverCartDefinition>
|
||||
// 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;
|
||||
|
||||
@@ -16,26 +54,22 @@ namespace MedullaAdapter
|
||||
|
||||
[AsControlItem(name = "夹抱关闭", LayoutRow = 1, LayoutCol = 2)]
|
||||
public Button Close;
|
||||
|
||||
public override void MultiVehicleModeChassisLogic()
|
||||
{
|
||||
// cart.MultiVehicleRemoteManualEnabled = true;
|
||||
// cart.MultiVehicleManualEnabled = true;
|
||||
// cart.MultiVehicleManualVx = SpeedPad.y * SpeedThreshold.val;
|
||||
// cart.MultiVehicleManualVth = -(float)Math.Pow(Math.Abs(SpeedPad.x), cart.ManualThetaPow) * Math.Sign(SpeedPad.x);
|
||||
}
|
||||
|
||||
// M层虚拟遥控器：控制左右夹臂同步打开或关闭。
|
||||
public override void CustomOperation()
|
||||
{
|
||||
if (Open.pressed)
|
||||
{
|
||||
cart.SpeedLeftArm = -ArmSpeed.val * cart.ManualArmSpeedFac;
|
||||
cart.SpeedRightArm = -ArmSpeed.val * cart.ManualArmSpeedFac;
|
||||
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;
|
||||
cart.SpeedLeftArm =
|
||||
ArmSpeed.val * cart.ManualArmSpeedFac;
|
||||
cart.SpeedRightArm =
|
||||
ArmSpeed.val * cart.ManualArmSpeedFac;
|
||||
}
|
||||
else
|
||||
{
|
||||
@@ -43,5 +77,10 @@ namespace MedullaAdapter
|
||||
cart.SpeedRightArm = 0;
|
||||
}
|
||||
}
|
||||
// 单车不支持多车联动，误打开多车开关时主动停车。
|
||||
public override void MultiVehicleModeChassisLogic()
|
||||
{
|
||||
cart.Chassis?.PredefinedDriveStop();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@@ -1,47 +0,0 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
|
||||
namespace CartActivator
|
||||
{
|
||||
public class LogicRunOnMCUAttribute:Attribute
|
||||
{
|
||||
public string mcu_url = "default";
|
||||
public int scanInterval = 50;
|
||||
}
|
||||
|
||||
public class RunOnMCU
|
||||
{
|
||||
// if return null: not data, or return payload data excluding CRC
|
||||
public static byte[] ReadEvent(int port, int event_id) => default;
|
||||
|
||||
public static void WriteEvent(byte[] payload, int port, int event_id) { }
|
||||
|
||||
// if return null: not data.
|
||||
public static byte[] ReadStream(int port) => default;
|
||||
|
||||
public static void WriteStream(byte[] payload, int port){}
|
||||
|
||||
// always have the same sized data.
|
||||
public static byte[] ReadSnapshot() => default;
|
||||
|
||||
public static void WriteSnapshot(byte[] payload)
|
||||
{
|
||||
}
|
||||
|
||||
public static T BytesToStruct<T>() where T: struct => default;
|
||||
|
||||
public static byte[] StructToBytes<T>(T what) where T : struct => default;
|
||||
|
||||
public static int GetMillisFromStart() => default;
|
||||
|
||||
public static T Iterate<T>(IEnumerable<T> ie) => default;
|
||||
}
|
||||
|
||||
public class MCUManager
|
||||
{
|
||||
public static void Use<T>()
|
||||
{
|
||||
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -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.
Some files were not shown because too many files have changed in this diff Show More
Reference in New Issue
Block a user