diff --git a/ClumsyPilot/ClumsyPilot.csproj b/ClumsyPilot/ClumsyPilot.csproj index ceb2716..2553db3 100644 --- a/ClumsyPilot/ClumsyPilot.csproj +++ b/ClumsyPilot/ClumsyPilot.csproj @@ -21,9 +21,6 @@ - - ref\CommonUsage.dll - ref\LessokajiWeaverUtilities.dll @@ -39,6 +36,9 @@ ref\RefFundamentalLib.dll + + ..\ref\CommonUsage.dll + diff --git a/ClumsyPilot/build/Clumsy/ClumsyPilot.dll b/ClumsyPilot/build/Clumsy/ClumsyPilot.dll index 3d48544..bf4e181 100644 Binary files a/ClumsyPilot/build/Clumsy/ClumsyPilot.dll and b/ClumsyPilot/build/Clumsy/ClumsyPilot.dll differ diff --git a/ClumsyPilot/build/Clumsy/ClumsyPilot.pdb b/ClumsyPilot/build/Clumsy/ClumsyPilot.pdb index fc4ea1f..f62506f 100644 Binary files a/ClumsyPilot/build/Clumsy/ClumsyPilot.pdb and b/ClumsyPilot/build/Clumsy/ClumsyPilot.pdb differ diff --git a/ClumsyPilot/build/Clumsy/CommonUsage.dll b/ClumsyPilot/build/Clumsy/CommonUsage.dll index effb81e..cdb9124 100644 Binary files a/ClumsyPilot/build/Clumsy/CommonUsage.dll and b/ClumsyPilot/build/Clumsy/CommonUsage.dll differ diff --git a/ClumsyPilot/obj/Debug/ClumsyPilot.AssemblyInfo.cs b/ClumsyPilot/obj/Debug/ClumsyPilot.AssemblyInfo.cs index 1f11488..79a3c54 100644 --- a/ClumsyPilot/obj/Debug/ClumsyPilot.AssemblyInfo.cs +++ b/ClumsyPilot/obj/Debug/ClumsyPilot.AssemblyInfo.cs @@ -13,7 +13,7 @@ using System.Reflection; [assembly: System.Reflection.AssemblyCompanyAttribute("ClumsyPilot")] [assembly: System.Reflection.AssemblyConfigurationAttribute("Debug")] [assembly: System.Reflection.AssemblyFileVersionAttribute("1.0.0.0")] -[assembly: System.Reflection.AssemblyInformationalVersionAttribute("1.0.0+e6b99c45b352f24ff58092f7855d0eadd8828d42")] +[assembly: System.Reflection.AssemblyInformationalVersionAttribute("1.0.0")] [assembly: System.Reflection.AssemblyProductAttribute("ClumsyPilot")] [assembly: System.Reflection.AssemblyTitleAttribute("ClumsyPilot")] [assembly: System.Reflection.AssemblyVersionAttribute("1.0.0.0")] diff --git a/ClumsyPilot/obj/Debug/ClumsyPilot.AssemblyInfoInputs.cache b/ClumsyPilot/obj/Debug/ClumsyPilot.AssemblyInfoInputs.cache index 29381a2..8fb9fb2 100644 --- a/ClumsyPilot/obj/Debug/ClumsyPilot.AssemblyInfoInputs.cache +++ b/ClumsyPilot/obj/Debug/ClumsyPilot.AssemblyInfoInputs.cache @@ -1 +1 @@ -3e5507bf56e38facdc245d5ff407e181c060d8094f38b94ad743b8c3305523f2 +5d77794fa0720c6591db5b06ac60427413c18989a6f7b64420ccb07d122d85bc diff --git a/ClumsyPilot/obj/Debug/ClumsyPilot.GeneratedMSBuildEditorConfig.editorconfig b/ClumsyPilot/obj/Debug/ClumsyPilot.GeneratedMSBuildEditorConfig.editorconfig index f169771..a3aa451 100644 --- a/ClumsyPilot/obj/Debug/ClumsyPilot.GeneratedMSBuildEditorConfig.editorconfig +++ b/ClumsyPilot/obj/Debug/ClumsyPilot.GeneratedMSBuildEditorConfig.editorconfig @@ -1,6 +1,6 @@ is_global = true build_property.RootNamespace = MultiWheelC -build_property.ProjectDir = d:\MyParking\ClumsyPilot\ +build_property.ProjectDir = D:\Users\Desktop\入职培训\停车机器人\MyParking\ClumsyPilot\ build_property.EnableComHosting = build_property.EnableGeneratedComInterfaceComImportInterop = build_property.CsWinRTUseWindowsUIXamlProjections = false diff --git a/ClumsyPilot/obj/Debug/ClumsyPilot.assets.cache b/ClumsyPilot/obj/Debug/ClumsyPilot.assets.cache index 9dedb7e..3d28e6f 100644 Binary files a/ClumsyPilot/obj/Debug/ClumsyPilot.assets.cache and b/ClumsyPilot/obj/Debug/ClumsyPilot.assets.cache differ diff --git a/ClumsyPilot/obj/Debug/ClumsyPilot.csproj.AssemblyReference.cache b/ClumsyPilot/obj/Debug/ClumsyPilot.csproj.AssemblyReference.cache index 787d026..0de62a8 100644 Binary files a/ClumsyPilot/obj/Debug/ClumsyPilot.csproj.AssemblyReference.cache and b/ClumsyPilot/obj/Debug/ClumsyPilot.csproj.AssemblyReference.cache differ diff --git a/ClumsyPilot/obj/Debug/ClumsyPilot.csproj.CoreCompileInputs.cache b/ClumsyPilot/obj/Debug/ClumsyPilot.csproj.CoreCompileInputs.cache index 7c7bf92..d102794 100644 --- a/ClumsyPilot/obj/Debug/ClumsyPilot.csproj.CoreCompileInputs.cache +++ b/ClumsyPilot/obj/Debug/ClumsyPilot.csproj.CoreCompileInputs.cache @@ -1 +1 @@ -e5afef3021b83202643b31f468b83ac62f224cd367fe52dc0a1cbb98570c91e1 +e972a413d047c4137a8ce86cbff54a8d2e2558806d9d974d3d6312467ee8ba4d diff --git a/ClumsyPilot/obj/Debug/ClumsyPilot.dll b/ClumsyPilot/obj/Debug/ClumsyPilot.dll index 8837123..bf4e181 100644 Binary files a/ClumsyPilot/obj/Debug/ClumsyPilot.dll and b/ClumsyPilot/obj/Debug/ClumsyPilot.dll differ diff --git a/ClumsyPilot/obj/Debug/ClumsyPilot.pdb b/ClumsyPilot/obj/Debug/ClumsyPilot.pdb index ab65ced..f62506f 100644 Binary files a/ClumsyPilot/obj/Debug/ClumsyPilot.pdb and b/ClumsyPilot/obj/Debug/ClumsyPilot.pdb differ diff --git a/CommonUsage-MultiVehicleSync/commonusage/.gitattributes b/CommonUsage-MultiVehicleSync/commonusage/.gitattributes new file mode 100644 index 0000000..c6c8654 --- /dev/null +++ b/CommonUsage-MultiVehicleSync/commonusage/.gitattributes @@ -0,0 +1,2 @@ +*.cs text eol=crlf +.gitattributes text eol=lf diff --git a/CommonUsage-MultiVehicleSync/commonusage/.gitignore b/CommonUsage-MultiVehicleSync/commonusage/.gitignore new file mode 100644 index 0000000..1ee5385 --- /dev/null +++ b/CommonUsage-MultiVehicleSync/commonusage/.gitignore @@ -0,0 +1,362 @@ +## Ignore Visual Studio temporary files, build results, and +## files generated by popular Visual Studio add-ons. +## +## Get latest from https://github.com/github/gitignore/blob/master/VisualStudio.gitignore + +# User-specific files +*.rsuser +*.suo +*.user +*.userosscache +*.sln.docstates + +# User-specific files (MonoDevelop/Xamarin Studio) +*.userprefs + +# Mono auto generated files +mono_crash.* + +# Build results +[Dd]ebug/ +[Dd]ebugPublic/ +[Rr]elease/ +[Rr]eleases/ +x64/ +x86/ +[Ww][Ii][Nn]32/ +[Aa][Rr][Mm]/ +[Aa][Rr][Mm]64/ +bld/ +[Bb]in/ +[Oo]bj/ +[Ll]og/ +[Ll]ogs/ + +# Visual Studio 2015/2017 cache/options directory +.vs/ +# Uncomment if you have tasks that create the project's static files in wwwroot +#wwwroot/ + +# Visual Studio 2017 auto generated files +Generated\ Files/ + +# MSTest test Results +[Tt]est[Rr]esult*/ +[Bb]uild[Ll]og.* + +# NUnit +*.VisualState.xml +TestResult.xml +nunit-*.xml + +# Build Results of an ATL Project +[Dd]ebugPS/ +[Rr]eleasePS/ +dlldata.c + +# Benchmark Results +BenchmarkDotNet.Artifacts/ + +# .NET Core +project.lock.json +project.fragment.lock.json +artifacts/ + +# ASP.NET Scaffolding +ScaffoldingReadMe.txt + +# StyleCop +StyleCopReport.xml + +# Files built by Visual Studio +*_i.c +*_p.c +*_h.h +*.ilk +*.meta +*.obj +*.iobj +*.pch +*.pdb +*.ipdb +*.pgc +*.pgd +*.rsp +*.sbr +*.tlb +*.tli +*.tlh +*.tmp +*.tmp_proj +*_wpftmp.csproj +*.log +*.vspscc +*.vssscc +.builds +*.pidb +*.svclog +*.scc + +# Chutzpah Test files +_Chutzpah* + +# Visual C++ cache files +ipch/ +*.aps +*.ncb +*.opendb +*.opensdf +*.sdf +*.cachefile +*.VC.db +*.VC.VC.opendb + +# Visual Studio profiler +*.psess +*.vsp +*.vspx +*.sap + +# Visual Studio Trace Files +*.e2e + +# TFS 2012 Local Workspace +$tf/ + +# Guidance Automation Toolkit +*.gpState + +# ReSharper is a .NET coding add-in +_ReSharper*/ +*.[Rr]e[Ss]harper +*.DotSettings.user + +# TeamCity is a build add-in +_TeamCity* + +# DotCover is a Code Coverage Tool +*.dotCover + +# AxoCover is a Code Coverage Tool +.axoCover/* +!.axoCover/settings.json + +# Coverlet is a free, cross platform Code Coverage Tool +coverage*.json +coverage*.xml +coverage*.info + +# Visual Studio code coverage results +*.coverage +*.coveragexml + +# NCrunch +_NCrunch_* +.*crunch*.local.xml +nCrunchTemp_* + +# MightyMoose +*.mm.* +AutoTest.Net/ + +# Web workbench (sass) +.sass-cache/ + +# Installshield output folder +[Ee]xpress/ + +# DocProject is a documentation generator add-in +DocProject/buildhelp/ +DocProject/Help/*.HxT +DocProject/Help/*.HxC +DocProject/Help/*.hhc +DocProject/Help/*.hhk +DocProject/Help/*.hhp +DocProject/Help/Html2 +DocProject/Help/html + +# Click-Once directory +publish/ + +# Publish Web Output +*.[Pp]ublish.xml +*.azurePubxml +# Note: Comment the next line if you want to checkin your web deploy settings, +# but database connection strings (with potential passwords) will be unencrypted +*.pubxml +*.publishproj + +# Microsoft Azure Web App publish settings. Comment the next line if you want to +# checkin your Azure Web App publish settings, but sensitive information contained +# in these scripts will be unencrypted +PublishScripts/ + +# NuGet Packages +*.nupkg +# NuGet Symbol Packages +*.snupkg +# The packages folder can be ignored because of Package Restore +**/[Pp]ackages/* +# except build/, which is used as an MSBuild target. +!**/[Pp]ackages/build/ +# Uncomment if necessary however generally it will be regenerated when needed +#!**/[Pp]ackages/repositories.config +# NuGet v3's project.json files produces more ignorable files +*.nuget.props +*.nuget.targets + +# Microsoft Azure Build Output +csx/ +*.build.csdef + +# Microsoft Azure Emulator +ecf/ +rcf/ + +# Windows Store app package directories and files +AppPackages/ +BundleArtifacts/ +Package.StoreAssociation.xml +_pkginfo.txt +*.appx +*.appxbundle +*.appxupload + +# Visual Studio cache files +# files ending in .cache can be ignored +*.[Cc]ache +# but keep track of directories ending in .cache +!?*.[Cc]ache/ + +# Others +ClientBin/ +~$* +*~ +*.dbmdl +*.dbproj.schemaview +*.jfm +*.pfx +*.publishsettings +orleans.codegen.cs + +# Including strong name files can present a security risk +# (https://github.com/github/gitignore/pull/2483#issue-259490424) +#*.snk + +# Since there are multiple workflows, uncomment next line to ignore bower_components +# (https://github.com/github/gitignore/pull/1529#issuecomment-104372622) +#bower_components/ + +# RIA/Silverlight projects +Generated_Code/ + +# Backup & report files from converting an old project file +# to a newer Visual Studio version. Backup files are not needed, +# because we have git ;-) +_UpgradeReport_Files/ +Backup*/ +UpgradeLog*.XML +UpgradeLog*.htm +ServiceFabricBackup/ +*.rptproj.bak + +# SQL Server files +*.mdf +*.ldf +*.ndf + +# Business Intelligence projects +*.rdl.data +*.bim.layout +*.bim_*.settings +*.rptproj.rsuser +*- [Bb]ackup.rdl +*- [Bb]ackup ([0-9]).rdl +*- [Bb]ackup ([0-9][0-9]).rdl + +# Microsoft Fakes +FakesAssemblies/ + +# GhostDoc plugin setting file +*.GhostDoc.xml + +# Node.js Tools for Visual Studio +.ntvs_analysis.dat +node_modules/ + +# Visual Studio 6 build log +*.plg + +# Visual Studio 6 workspace options file +*.opt + +# Visual Studio 6 auto-generated workspace file (contains which files were open etc.) +*.vbw + +# Visual Studio LightSwitch build output +**/*.HTMLClient/GeneratedArtifacts +**/*.DesktopClient/GeneratedArtifacts +**/*.DesktopClient/ModelManifest.xml +**/*.Server/GeneratedArtifacts +**/*.Server/ModelManifest.xml +_Pvt_Extensions + +# Paket dependency manager +.paket/paket.exe +paket-files/ + +# FAKE - F# Make +.fake/ + +# CodeRush personal settings +.cr/personal + +# Python Tools for Visual Studio (PTVS) +__pycache__/ +*.pyc + +# Cake - Uncomment if you are using it +# tools/** +# !tools/packages.config + +# Tabs Studio +*.tss + +# Telerik's JustMock configuration file +*.jmconfig + +# BizTalk build output +*.btp.cs +*.btm.cs +*.odx.cs +*.xsd.cs + +# OpenCover UI analysis results +OpenCover/ + +# Azure Stream Analytics local run output +ASALocalRun/ + +# MSBuild Binary and Structured Log +*.binlog + +# NVidia Nsight GPU debugger configuration file +*.nvuser + +# MFractors (Xamarin productivity tool) working folder +.mfractor/ + +# Local History for Visual Studio +.localhistory/ + +# BeatPulse healthcheck temp database +healthchecksdb + +# Backup folder for Package Reference Convert tool in Visual Studio 2017 +MigrationBackup/ + +# Ionide (cross platform F# VS Code tools) working folder +.ionide/ + +# Fody - auto-generated XML schema +FodyWeavers.xsd diff --git a/CommonUsage-MultiVehicleSync/commonusage/Chassis/AbstractChassis.cs b/CommonUsage-MultiVehicleSync/commonusage/Chassis/AbstractChassis.cs new file mode 100644 index 0000000..41b0e23 --- /dev/null +++ b/CommonUsage-MultiVehicleSync/commonusage/Chassis/AbstractChassis.cs @@ -0,0 +1,126 @@ +using System; +using System.Collections.Generic; +using System.Diagnostics; +using System.IO; +using System.Numerics; +using System.Security.Cryptography.X509Certificates; +using System.Text; +using FundamentalLib; +using Newtonsoft.Json; + +namespace CommonUsage.Chassis +{ + public abstract class AbstractChassis + { + protected AbstractChassis() + { + Valid = false; + } + + public abstract void Initialize(); + + public abstract void Visualize(); + + public abstract void AfterDirectionChanged(); + + /// + /// 当前行进方向。 + /// + [Obsolete] + public float DirectionAngle + { + get => _originBiasTh; + set + { + _originBiasTh = value; + if (_originBiasTh != _lastDirectionAngle) AfterDirectionChanged(); + _lastDirectionAngle = _originBiasTh; + } + } + + protected float _originBiasX = 0f, _originBiasY = 0f, _originBiasTh; + + public Vector3 GetOriginBias() + { + return new Vector3(_originBiasX, _originBiasY, _originBiasTh); + } + + public class CarSpeed + { + public float Vx,Vy,Vw; + } + + public abstract CarSpeed GetCarSpeed(bool isActual = false); + public List GetGeometricControlPoints() + { + return GeometricControlPoints; + } + + public void ComputeWheelsGeometrically(float speed) + { + // 打印调用位置信息 + var stackTrace = new StackTrace(true); + var callerFrame = stackTrace.GetFrame(1); // 获取调用者的帧 + if (callerFrame != null) + { + var fileName = callerFrame.GetFileName(); + var lineNumber = callerFrame.GetFileLineNumber(); + DLog.Log($"s:{speed:0.000} from {fileName} ln.{lineNumber}", $"WheelComputeCaller"); + } + + DefineGeometricWheelComputation(speed); + } + + protected abstract void DefineGeometricWheelComputation(float speed); + + public void DriveStop() + { + PredefinedDriveStop(); + CustomDriveStop?.Invoke(); + } + + public abstract void PredefinedDriveStop(); + + public Action CustomDriveStop; + + public abstract bool ComputeRotateWheels(float rotSpeed); + + public abstract float CalculateTurningSpeedDecayFac(float turn); + + public enum ChassisState + { + Standby, + Running, + AbnormalFeedback, + ExceedMotionAbility, + } + + protected ChassisState State; + + protected string StateDescription; + + public (ChassisState State, string Description) GetChassisState() + { + return (State, StateDescription); + } + + public bool Debug = false; + public float AccPerSecond = 0.2f; + public float DeAccPerSecond = 0.2f; + public float MaxSpeed = 1; // m/s + public float MinTurnSpeedFac = 0.5f; + public float MaxTurnThreshold = 90f; + + public float GcpThetaPerSecond = 10f; + + public DateTime LastMoveTime = DateTime.MinValue; + + protected bool Valid = false; + protected List GeometricControlPoints = new(); + protected bool RotatingActive = false; + protected bool GoingActive = false; + protected bool GoingWheelAligned = false; + + private float _lastDirectionAngle = 0; + } +} diff --git a/CommonUsage-MultiVehicleSync/commonusage/Chassis/DiffSteerWheel.cs b/CommonUsage-MultiVehicleSync/commonusage/Chassis/DiffSteerWheel.cs new file mode 100644 index 0000000..4f179ae --- /dev/null +++ b/CommonUsage-MultiVehicleSync/commonusage/Chassis/DiffSteerWheel.cs @@ -0,0 +1,47 @@ +using System; +using System.Collections.Generic; +using System.Numerics; +using System.Text; + +namespace CommonUsage.Chassis +{ + public class DiffSteerWheel:SteerWheel + { + public DiffSteerWheel(float wheelDistance,Vector2 position, float angleLowerLimit, float angleUpperLimit, Action speedWriter, + Func speedReader, Action angleWriter, Func angleReader, Action leftSpeedWriter, Action rightSpeedWriter, + float angleLimitMarginDeg = 15f) : base(position, + angleLowerLimit, angleUpperLimit, speedWriter, speedReader, angleWriter, angleReader, angleLimitMarginDeg) + { + _leftSpeedWriter = leftSpeedWriter; + _rightSpeedWriter = rightSpeedWriter; + WheelDistance = wheelDistance; + } + + public float GetLeftSendSpeed() + { + return _leftSendSpeed; + } + + public float GetRightSendSpeed() + { + return _rightSendSpeed; + } + + public void WriteLeftSpeed(float speed) + { + _leftSpeedWriter(_leftSendSpeed = speed); + } + + public void WriteRightSpeed(float speed) + { + _rightSpeedWriter(_rightSendSpeed = speed); + } + + public float WheelDistance; + private readonly Action _leftSpeedWriter; + private readonly Action _rightSpeedWriter; + + private float _leftSendSpeed; + private float _rightSendSpeed; + } +} diff --git a/CommonUsage-MultiVehicleSync/commonusage/Chassis/DifferentialChassis.cs b/CommonUsage-MultiVehicleSync/commonusage/Chassis/DifferentialChassis.cs new file mode 100644 index 0000000..e6a7279 --- /dev/null +++ b/CommonUsage-MultiVehicleSync/commonusage/Chassis/DifferentialChassis.cs @@ -0,0 +1,170 @@ +using FundamentalLib; +using System; +using System.Collections.Generic; +using System.Numerics; +using System.Text; +using System.Diagnostics; + +namespace CommonUsage.Chassis +{ + public class DifferentialChassis : AbstractChassis + { + public void SetLeftRightWheels(Wheel wheelL, Wheel wheelR) + { + _leftWheel = wheelL; + _rightWheel = wheelR; + _halfWheelTrack = Math.Abs(_leftWheel.Position.Y); + } + + public override void Visualize() + { + + } + + public override CarSpeed GetCarSpeed(bool isActual = false) + { + if (!isActual) + { + return new CarSpeed() + { + Vx = (_speedL + _speedR) / 2f, + Vw = (_speedR - _speedL) / Math.Abs(_leftWheel.Position.Y - _rightWheel.Position.Y) / + (float)Math.PI * 180f * 1000f, + Vy = 0 + }; + } + else + { + return new CarSpeed() + { + Vx = GetLinearSpeed(), + Vw = (_rightWheel.ReadSpeed() - _leftWheel.ReadSpeed()) / + Math.Abs(_leftWheel.Position.Y - _rightWheel.Position.Y) / + (float)Math.PI * 180f * 1000f, + Vy = 0 + }; + } + + } + + public (Wheel,Wheel) GetWheels() + { + return (_leftWheel, _rightWheel); + } + + public float GetLinearSpeed() + { + return (_leftWheel.ReadSpeed() + _rightWheel.ReadSpeed()) / 2f; + } + + public override void Initialize() + { + GeometricControlPoints.Add(new GeometricControlPoint(new Vector2(0, 0))); + Valid = true; + } + + public override void AfterDirectionChanged() + { + + } + + public override void PredefinedDriveStop() + { + if (!Valid) return; + _sendSpeedL = _sendSpeedR = 0; + _speedL = _speedR = 0; + _leftWheel.WriteSpeed(_sendSpeedL); + _rightWheel.WriteSpeed(_sendSpeedR); + GoingActive = false; + RotatingActive = false; + } + + protected override void DefineGeometricWheelComputation(float speed) + { + var now = DateTime.Now; + if (!GoingActive) LastMoveTime = now; + + SendSpeed(speed, GeometricControlPoints[0].Theta, now - LastMoveTime); + + GoingActive = true; + RotatingActive = false; + } + + public override bool ComputeRotateWheels(float rotSpeed) + { + if (!RotatingActive) LastMoveTime = DateTime.Now; + + SendSpeed(0, rotSpeed); + + GoingActive = false; + RotatingActive = true; + return true; + } + + public override float CalculateTurningSpeedDecayFac(float turn) + { + return 1 - Math.Min(turn, MaxTurnThreshold) / MaxTurnThreshold * MinTurnSpeedFac; + } + + public void SendSpeed(float linearSpeed, float angularSpeed, TimeSpan? deltaTime = null) + { + var edgeLinearSpeed = (float)(angularSpeed / 180f * Math.PI * _halfWheelTrack / 1000); + var vl = linearSpeed - edgeLinearSpeed; + var vr = linearSpeed + edgeLinearSpeed; + _speedL = vl; + _speedR = vr; + AccumulateSpeed(vl, vr, deltaTime); + LastMoveTime = DateTime.Now; + } + + private void AccumulateSpeed(float vl, float vr, TimeSpan? deltaTime = null) + { + // var dTime = (float)(deltaTime ?? DateTime.Now - LastMoveTime).TotalSeconds; + // + // var speedSignL = Math.Sign(vl - _sendSpeedL); + // var accL = Math.Abs(vl) > Math.Abs(_sendSpeedL) ? AccPerSecond : DeAccPerSecond; + // _sendSpeedL += speedSignL * Math.Min(Math.Abs(vl - _sendSpeedL), accL * dTime); + // _leftWheel.WriteSpeed(_sendSpeedL); + // + // var speedSignR = Math.Sign(vr - _sendSpeedR); + // var accR = Math.Abs(vr) > Math.Abs(_sendSpeedR) ? AccPerSecond : DeAccPerSecond; + // _sendSpeedR += speedSignR * Math.Min(Math.Abs(vr - _sendSpeedR), accR * dTime); + // _rightWheel.WriteSpeed(_sendSpeedR); + + // if (Debug) + // Console.WriteLine($"DiffChassis, target:{v:0.00},send:{_sendSpeed:0.0}"); + // var dTime = (float)(deltaTime ?? DateTime.Now - LastMoveTime).TotalSeconds; + var dTime = (float)(deltaTime ?? DateTime.Now - LastMoveTime).TotalSeconds; + float diffL = vl - _sendSpeedL; + float diffR = vr - _sendSpeedR; + + float accL = Math.Abs(vl) > Math.Abs(_sendSpeedL) ? AccPerSecond : DeAccPerSecond; + float accR = Math.Abs(vr) > Math.Abs(_sendSpeedR) ? AccPerSecond : DeAccPerSecond; + + float maxDeltaL = accL * dTime; + float maxDeltaR = accR * dTime; + + float factorL = Math.Abs(diffL) > maxDeltaL ? maxDeltaL / Math.Abs(diffL) : 1.0f; + float factorR = Math.Abs(diffR) > maxDeltaR ? maxDeltaR / Math.Abs(diffR) : 1.0f; + + float factor = Math.Min(factorL, factorR); + + _sendSpeedL += diffL * factor; + _sendSpeedR += diffR * factor; + + _leftWheel.WriteSpeed(_sendSpeedL); + _rightWheel.WriteSpeed(_sendSpeedR); + // Console.WriteLine($"DiffChassis, target:{vl:0.00},send:{_sendSpeedL:0.00} dTime：{dTime} diffL:{diffL} factor:{factor}" ); + } + + private Wheel _leftWheel; + private Wheel _rightWheel; + private float _halfWheelTrack; // millimeter + + private float _sendSpeedL; + private float _sendSpeedR; + private int _direction = 1; // 1 forward, -1 backward + private float _speedL; + private float _speedR; + } +} diff --git a/CommonUsage-MultiVehicleSync/commonusage/Chassis/MultiWheelChassis.cs b/CommonUsage-MultiVehicleSync/commonusage/Chassis/MultiWheelChassis.cs new file mode 100644 index 0000000..fa96e28 --- /dev/null +++ b/CommonUsage-MultiVehicleSync/commonusage/Chassis/MultiWheelChassis.cs @@ -0,0 +1,1083 @@ +using System; +using System.Collections.Generic; +using System.Linq; +using System.Numerics; +using System.Diagnostics; +using System.Text; +using CommonUsage.Mathematics; +using FundamentalLib; +using System.Drawing; + +namespace CommonUsage.Chassis +{ + public class MultiWheelChassis : AbstractChassis + { + public float ThConsistentThreshold = 1.0f; + + public float ControlPointRadius = 500f; + + // 原地旋转纠偏钳位：SendRotateMotion 中每轮纠偏速度幅值不超过 该比例×本轮旋转切向速度， + // 防止减速末段旋转切向变小时纠偏盖过它、使轮向矢量乱摆（频繁打方向/卡死）。<0 关闭钳位。 + public float RotateCompTangentFrac = 0.5f; + + // 上一次 SendRotateMotion 的舵轮对齐状态：false 表示舵轮未追上目标角(gate=0、车未真正转动)。 + // 供上层做积分抗饱和（卡死时冻结积分）。 + public bool LastRotateAligned { get; private set; } = true; + + public string LastMotionDecomposeFailureReason { get; private set; } = ""; + + // 为true时，在两个等价舵角都可用的情况下优先选择机械转动距离更小的方案。 + // 默认关闭以保持旧项目原有行为，由需要该策略的车型适配器主动开启。 + public bool PreferMinimumSteeringTravel { get; set; } + + public float MinimumTurningAngleForAckermann = 60f; + + + public MultiWheelChassis() : base() + { + + } + + public void AddWheel(SteerWheel wheel) + { + _steerWheels.Add(wheel); + } + + public override void Initialize() + { + _steerWheels = _steerWheels.OrderByDescending(sw => sw.Position.X).ToList(); + _targetSpeeds = new float[_steerWheels.Count].ToList(); + _sendSpeeds = new float[_steerWheels.Count].ToList(); + _tmpSpeeds = new float[_steerWheels.Count].ToList(); + _wheelDirs = Enumerable.Repeat(1, _steerWheels.Count).ToList(); + _sendAngle = new float[_steerWheels.Count].ToList(); + _debugSpeeds = new float[_steerWheels.Count].ToList(); + + if (_steerWheels.Count < 2) + { + Console.WriteLine($"steer wheel num: {_steerWheels.Count}. invalid!"); + Valid = false; + return; + } + + CalculateAxes(); + Valid = true; + } + + public Visualizer MainVisualizer = null; + + public override void Visualize() + { + MainVisualizer?.Clear(); + + if (_rotCenter != Vector2.Zero) + { + MainVisualizer?.DrawText(Color.Chartreuse, $"({_rotCenter.X:f1}, {_rotCenter.Y:f2})", _rotCenter); + } + + for (var i = 0; i < _steerWheels.Count; ++i) + { + var sw = _steerWheels[i]; + if (sw is DiffSteerWheel dsw) + { + var pointing = CommonMath.Transform2D(dsw.Position, dsw.ZeroDirection, new Vector2(200, 0)); + MainVisualizer?.DrawLine(Color.Gray, dsw.Position, pointing); + MainVisualizer?.DrawText(Color.Gray, $"{dsw.GetSendSpeed():f2} {sw.GetSendAngle():f2}", dsw.Position); + + var left = CommonMath.Transform2D(dsw.Position, dsw.GetAngleRelativeToChassis() + 90, new Vector2(dsw.WheelDistance, 0)); + MainVisualizer?.DrawLine(Color.GreenYellow, left, + CommonMath.Transform2D(left, dsw.GetAngleRelativeToChassis(), new Vector2(200, 0)), endArrow: true); + MainVisualizer?.DrawText(Color.GreenYellow, $"{dsw.GetLeftSendSpeed():0.00}", left); + + var right = CommonMath.Transform2D(dsw.Position, dsw.GetAngleRelativeToChassis() - 90, new Vector2(dsw.WheelDistance, 0)); + MainVisualizer?.DrawLine(Color.DeepPink, right, + CommonMath.Transform2D(right, dsw.GetAngleRelativeToChassis(), new Vector2(200, 0)), endArrow: true); + MainVisualizer?.DrawText(Color.DeepPink, $"{dsw.GetRightSendSpeed():0.00}", right); + } + else + { + MainVisualizer?.DrawLine(Color.Green, sw.Position, + CommonMath.Transform2D(sw.Position, sw.GetAngleRelativeToChassis(), new Vector2(200, 0)), endArrow: true); + MainVisualizer?.DrawText(Color.Chartreuse, $"{sw.GetSendSpeed():f2} {sw.GetAngleRelativeToChassis():f2}", sw.Position); + } + + if (_rotCenter != Vector2.Zero) + { + MainVisualizer?.DrawLine(Color.DodgerBlue, sw.Position, _rotCenter); + } + } + } + + public override void AfterDirectionChanged() + { + Hedingben.ToastText($"AfterDirectionChanged", "MultiWheelChassis-AfterDirectionChanged"); + + foreach (var sw in _steerWheels) + { + sw.Position = CommonMath.Transform2D(new Vector2(_originBiasX, _originBiasY), _originBiasTh, + sw.PhysicalPosition); + sw.ZeroDirection = _originBiasTh; + } + + CalculateAxes(); + GoingWheelAligned = false; + } + + protected override void DefineGeometricWheelComputation(float speed) + { + if (!GoingActive) ResetMotionState(); + var targetGcpTheta0 = GeometricControlPoints[0].Theta; + var targetGcpTheta1 = GeometricControlPoints[1].Theta; + var lastGcpTheta0Before = _lastGcpTheta0; + var lastGcpTheta1Before = _lastGcpTheta1; + + void Process(ref float current, float target, int id) + { + current += Math.Sign(target - current) * Math.Min(Math.Abs(target - current), + GcpThetaPerSecond * (float)(DateTime.Now - LastMoveTime).TotalSeconds); + // Hedingben.ToastText($"current:{current:0.00} target:{target:0.00}", $"gcp-accumulation-{id}"); + } + + Process(ref _lastGcpTheta0, GeometricControlPoints[0].Theta, 0); + Process(ref _lastGcpTheta1, GeometricControlPoints[1].Theta, 1); + if ((DateTime.Now - _geometricComputeLastLog).TotalMilliseconds >= 200) + { + _geometricComputeLastLog = DateTime.Now; + DLog.Log( + $"ComputeWheelsGeometrically speed:{speed:F3} " + + $"targetGcp:({targetGcpTheta0:F1},{targetGcpTheta1:F1}) " + + $"smoothBefore:({lastGcpTheta0Before:F1},{lastGcpTheta1Before:F1}) " + + $"smoothAfter:({_lastGcpTheta0:F1},{_lastGcpTheta1:F1}) " + + $"originBias:({_originBiasX:F1},{_originBiasY:F1},{_originBiasTh:F1}) goingActive:{GoingActive}", + "MultiWheelGoDiag"); + } + SendMotion(speed, _lastGcpTheta0, _lastGcpTheta1); + + GoingActive = true; + RotatingActive = false; + XYThActive = false; + } + + private float _lastGcpTheta0; + + private float _lastGcpTheta1; + + /// + /// 转速单位为度/s，逆时针为正。 + /// + /// + public override bool ComputeRotateWheels(float rotSpeed) + { + if (!RotatingActive) ResetMotionState(); + + var feasible = SendRotateMotion(rotSpeed); + + GoingActive = false; + RotatingActive = true; + XYThActive = false; + return feasible; + } + + public override void PredefinedDriveStop() + { + if (!Valid) return; + for (var i = 0; i < _steerWheels.Count; i++) + { + _sendSpeeds[i] = 0; + _debugSpeeds[i] = 0; + if (_steerWheels[i] is DiffSteerWheel dfw) + { + dfw.WriteLeftSpeed(0); + dfw.WriteRightSpeed(0); + } + else + { + _steerWheels[i].WriteSpeed(0); + } + } + GoingActive = false; + RotatingActive = false; + XYThActive = false; + } + + public void RampStop(TimeSpan? deltaTime = null) + { + if (!Valid) return; + + for (var i = 0; i < _steerWheels.Count; i++) + { + _debugSpeeds[i] = 0; + AccumulateSpeed(i, 0, false, Vector2.Zero, deltaTime); + } + + GoingActive = false; + RotatingActive = false; + XYThActive = false; + LastMoveTime = DateTime.Now; + } + + private struct WheelAngleCandidate + { + public bool Valid; + public float RawAngle; + public float NormalizedAngle; + public float Margin; + public int Direction; + public float Lower; + public float Upper; + } + + private WheelAngleCandidate BuildWheelAngleCandidate(SteerWheel sw, float rawAngle, int direction) + { + var lower = (float)CommonMath.RoundTh(sw.AngleLowerLimit); + var upper = (float)CommonMath.RoundTh(sw.AngleUpperLimit); + while (upper < lower) upper += 360; + + var normalized = (float)CommonMath.RoundTh(rawAngle); + while (normalized < lower) normalized += 360; + while (normalized > upper && normalized - 360 >= lower) normalized -= 360; + + var margin = Math.Min(normalized - lower, upper - normalized); + var marginRequired = Math.Max(0, sw.AngleLimitMarginDeg); + return new WheelAngleCandidate + { + Valid = normalized >= lower && normalized <= upper && margin >= marginRequired, + RawAngle = rawAngle, + NormalizedAngle = normalized, + Margin = margin, + Direction = direction, + Lower = lower, + Upper = upper + }; + } + + private bool TryResolveWheelAngle(int wheelIndex, float targetAngle, string entry, + out float angle, out int direction, out string reason) + { + var sw = _steerWheels[wheelIndex]; + var forward = BuildWheelAngleCandidate(sw, targetAngle, 1); + var reverse = BuildWheelAngleCandidate(sw, targetAngle + 180, -1); + + WheelAngleCandidate? selected = null; + + if (PreferMinimumSteeringTravel && + forward.Valid && + reverse.Valid) + { + var actualAngle = sw.ReadAngle(); + + if (!float.IsNaN(actualAngle) && + !float.IsInfinity(actualAngle)) + { + // 将反馈角映射到与候选角相同的机械限位区间。 + // 这里比较的是受限机械舵角，不能使用圆周最短角度差。 + var actual = BuildWheelAngleCandidate( + sw, + actualAngle, + 1); + var forwardError = Math.Abs( + forward.NormalizedAngle - + actual.NormalizedAngle); + var reverseError = Math.Abs( + reverse.NormalizedAngle - + actual.NormalizedAngle); + + // 迟滞避免两个方案转动量接近时频繁翻转轮速方向。 + const float switchHysteresisDegrees = 5f; + + if (reverseError + + switchHysteresisDegrees < + forwardError) + { + selected = reverse; + } + else if (forwardError + + switchHysteresisDegrees < + reverseError) + { + selected = forward; + } + } + } + + // 未启用最小转舵策略、反馈无效或两个候选差异落在迟滞区时， + // 保持旧版轮速方向，避免破坏其他车型的既有行为。 + if (_wheelDirs != null && wheelIndex < _wheelDirs.Count) + { + if (selected == null && + _wheelDirs[wheelIndex] < 0 && + reverse.Valid) + { + selected = reverse; + } + else if (selected == null && + _wheelDirs[wheelIndex] >= 0 && + forward.Valid) + { + selected = forward; + } + } + + if (selected == null) + { + if (forward.Valid) selected = forward; + else if (reverse.Valid) selected = reverse; + } + + if (selected != null) + { + angle = selected.Value.NormalizedAngle; + direction = selected.Value.Direction; + reason = ""; + return true; + } + + angle = 0; + direction = 0; + var marginRequired = Math.Max(0, sw.AngleLimitMarginDeg); + reason = + $"{entry} infeasible wheel={wheelIndex} target={CommonMath.RoundTh(targetAngle):F1} " + + $"reverse={CommonMath.RoundTh(targetAngle + 180):F1} " + + $"limit=[{forward.Lower:F1},{forward.Upper:F1}] requiredMargin={marginRequired:F1} " + + $"forward(norm={forward.NormalizedAngle:F1},margin={forward.Margin:F1},valid={forward.Valid}) " + + $"reverse(norm={reverse.NormalizedAngle:F1},margin={reverse.Margin:F1},valid={reverse.Valid})"; + return false; + } + + private bool FailMotionDecomposition(string entry, string reason, TimeSpan? deltaTime) + { + LastMotionDecomposeFailureReason = $"{entry}: {reason}"; + Console.WriteLine(LastMotionDecomposeFailureReason); + DLog.Log(LastMotionDecomposeFailureReason, "MultiWheelMotionDecompose"); + Hedingben.ToastText($"Motion decompose failed: {reason}", "MultiWheelChassis-decompose"); + RampStop(deltaTime); + return false; + } + + private Vector2 _rotCenter; + + public Vector2 GetRotCenter() + { + return _rotCenter; + } + + public void SetOriginBias(float x, float y, float th) + { + _originBiasX = x; + _originBiasY = y; + _originBiasTh = th; + + foreach (var sw in _steerWheels) + { + sw.Position = CommonMath.Transform2D(new Vector2(_originBiasX, _originBiasY), _originBiasTh, + sw.PhysicalPosition); + sw.ZeroDirection = _originBiasTh; + } + + CalculateAxes(); + } + + public Visualizer SendMotionVisualizer = null; + + /// + /// frontTh和rearTh是基于当前行进方向，前后轴应该打的角度。 + /// + /// 单位m/s。 + /// 角度制。基于当前行进方向。 + /// 角度制。基于当前行进方向。 + /// 上次下发SendMotion到本次的间隔时间。 + /// 用于多车联动，大于0时生效。 + /// 用于多车联动，localControlRadius大于0时生效。 + /// 用于多车联动，localControlRadius大于0时生效。 + /// 用于多车联动，localControlRadius大于0时生效。 + /// + public bool SendMotion(float speed, float frontTh, float rearTh, TimeSpan? deltaTime = null, + float localControlRadius = 0, float localCompensateX = 0f, float localCompensateY = 0, float localCompensateTh = 0) + { + if (!Valid) return FailMotionDecomposition("SendMotion", "invalid chassis", deltaTime); + LastMotionDecomposeFailureReason = ""; + + var compInputSpeed = speed; + var compInputFrontTh = frontTh; + var compInputRearTh = rearTh; + var compMotionTh = 0f; + var compAlong = 0f; + var compSide = 0f; + var compDSpeed = 0f; + var compDTh = 0f; + + if (localControlRadius > 0) + { + var motionTh = AverageAngle(frontTh, rearTh); + var radMotionTh = motionTh / 180f * Math.PI; + var alongComp = (float)(localCompensateX * Math.Cos(radMotionTh) + + localCompensateY * Math.Sin(radMotionTh)); + var sideComp = (float)(-localCompensateX * Math.Sin(radMotionTh) + + localCompensateY * Math.Cos(radMotionTh)); + var dSpeed = alongComp / 1000f; + speed += dSpeed; + + var speedSign = Math.Abs(compInputSpeed) > 1e-4f ? Math.Sign(compInputSpeed) : 1; + var steerBaseSpeed = Math.Max(Math.Abs(speed), 0.05f); + var dTh = (float)(Math.Atan2(sideComp / 1000f, steerBaseSpeed) / Math.PI * 180f) * speedSign; + compMotionTh = motionTh; + compAlong = alongComp; + compSide = sideComp; + compDSpeed = dSpeed; + compDTh = dTh; + Hedingben.ToastText($"s:{compInputSpeed:F2} d:{dSpeed:F3}", "MultiWheelChassis-dSpeed"); + Hedingben.ToastText($"F:{frontTh:F1} R:{rearTh:F1} d:{dTh:F1}", "MultiWheelChassis-dTh"); + frontTh += dTh; + rearTh += dTh; + + if (Math.Abs(localCompensateX) > 1e-3f || Math.Abs(localCompensateY) > 1e-3f || + Math.Abs(localCompensateTh) > 1e-3f) + DLog.Log( + $"SendMotionComp in speed:{compInputSpeed:F3} fTh:{compInputFrontTh:F1} rTh:{compInputRearTh:F1} " + + $"motionTh:{motionTh:F1} biasTh:{_originBiasTh:F1} comp({localCompensateX:F1},{localCompensateY:F1},{localCompensateTh:F2}) " + + $"along:{alongComp:F1} side:{sideComp:F1} dSpeed:{dSpeed:F3} dTh:{dTh:F1} " + + $"out speed:{speed:F3} fTh:{frontTh:F1} rTh:{rearTh:F1}", + "MultiWheelMotionComp"); + } + + float scaleFactor = 1.0f; + float currentFrontTh = frontTh; + float currentRearTh = rearTh; + bool needAdjust = true; + const float minScaleFactor = 0.1f; + var finalAngles = new List(); + + var axisDiffFlag = false; + var myRotCenter = new Vector2(); + var lastInvalidReason = ""; + + float AverageAngle(float left, float right) + { + var leftRad = left / 180f * Math.PI; + var rightRad = right / 180f * Math.PI; + var x = Math.Cos(leftRad) + Math.Cos(rightRad); + var y = Math.Sin(leftRad) + Math.Sin(rightRad); + if (Math.Abs(x) < 1e-6 && Math.Abs(y) < 1e-6) + return (float)CommonMath.RoundTh(left); + return (float)CommonMath.RoundTh(Math.Atan2(y, x) / Math.PI * 180f); + } + + float Interpolate(float left, float current, float right) + { + if (current <= left) return 0; + if (current >= right) return 1; + return (current - left) / (right - left); + } + + while (needAdjust && scaleFactor > minScaleFactor) + { + needAdjust = false; + var tmpAngles = new List(); + + // ... 现有的插值函数和旋转中心计算代码 ... + var thDiff = CommonMath.ThDiff(currentFrontTh, currentRearTh); + axisDiffFlag = Math.Abs(thDiff) > ThConsistentThreshold; + + var sign = -1; + if (axisDiffFlag) + { + // Vector2 pFront = new(_frontBase, 0), pRear = new(_rearBase, 0), + Vector2 pFront = new(ControlPointRadius, 0), pRear = new(-ControlPointRadius, 0), + normFront = CommonMath.Transform2D(pFront, currentFrontTh + 90, Vector2.UnitX), + normRear = CommonMath.Transform2D(pRear, currentRearTh + 90, Vector2.UnitX); + var (intersect, center) = + CommonMath.TwoLinesIntersection(pFront, normFront, pRear, normRear); + if (!intersect) + return FailMotionDecomposition("SendMotion", + $"front/rear axes do not intersect frontTh={currentFrontTh:F1} rearTh={currentRearTh:F1}", + deltaTime); + myRotCenter = center; + sign = myRotCenter.Y > 1 ? 1 : -1; + + if (localControlRadius > 0 && localCompensateTh != 0) + { + void VisPointing(Color color, Vector2 point, float th, int width) + { + var localFrontPointing = CommonMath.Transform2D(point, th, new Vector2(200, 0)); + SendMotionVisualizer?.DrawLine(color, point, localFrontPointing, width: width); + } + + var localPFront = CommonMath.Transform2D(new Vector2(_originBiasX, _originBiasY), _originBiasTh, + new Vector2(localControlRadius, 0)); + var localPRear = CommonMath.Transform2D(new Vector2(_originBiasX, _originBiasY), _originBiasTh, + new Vector2(-localControlRadius, 0)); + // SendMotionVisualizer?.DrawLine(Color.DarkRed, myRotCenter, localPFront); + // SendMotionVisualizer?.DrawLine(Color.DarkRed, myRotCenter, localPRear); + + // 计算_rotCenter在"localPRear指向localPFront的向量"的左侧还是右侧 + // 使用叉积判断：叉积 > 0 表示在左侧，< 0 表示在右侧 + var vecRearToFront = new Vector2(localPFront.X - localPRear.X, localPFront.Y - localPRear.Y); + var vecRearToCenter = new Vector2(myRotCenter.X - localPRear.X, myRotCenter.Y - localPRear.Y); + var crossProduct = vecRearToFront.X * vecRearToCenter.Y - vecRearToFront.Y * vecRearToCenter.X; + var side = crossProduct > 0 ? 1 : -1; + + var localFrontTh = + (float)(Math.Atan2(localPFront.Y - myRotCenter.Y, localPFront.X - myRotCenter.X) / Math.PI * + 180) + 90 * side; + var localRearTh = + (float)(Math.Atan2(localPRear.Y - myRotCenter.Y, localPRear.X - myRotCenter.X) / Math.PI * + 180) + 90 * side; + var sameDirectionError = + Math.Abs(CommonMath.ThDiff(localFrontTh, currentFrontTh)) + + Math.Abs(CommonMath.ThDiff(localRearTh, currentRearTh)); + var reverseDirectionError = + Math.Abs(CommonMath.ThDiff(localFrontTh + 180, currentFrontTh)) + + Math.Abs(CommonMath.ThDiff(localRearTh + 180, currentRearTh)); + if (reverseDirectionError < sameDirectionError) + { + localFrontTh = (float)CommonMath.RoundTh(localFrontTh + 180); + localRearTh = (float)CommonMath.RoundTh(localRearTh + 180); + } + // VisPointing(Color.DarkRed, localPFront, localFrontTh, 1); + // VisPointing(Color.DarkRed, localPRear, localRearTh, 1); + + var newLocalFrontTh = localFrontTh + localCompensateTh * Math.Sign(speed); + var newLocalRearTh = localRearTh - localCompensateTh * Math.Sign(speed); + // VisPointing(Color.Red, localPFront, newLocalFrontTh, 2); + // VisPointing(Color.Red, localPRear, newLocalRearTh, 2); + + var localThDiff = CommonMath.ThDiff(newLocalFrontTh, newLocalRearTh); + axisDiffFlag = Math.Abs(localThDiff) > ThConsistentThreshold; + + if (axisDiffFlag) + { + var localNormFront = CommonMath.Transform2D(localPFront, newLocalFrontTh + 90, Vector2.UnitX); + var localNormRear = CommonMath.Transform2D(localPRear, newLocalRearTh + 90, Vector2.UnitX); + var (localIntersect, localCenter) = + CommonMath.TwoLinesIntersection(localPFront, localNormFront, localPRear, localNormRear); + if (!localIntersect) + return FailMotionDecomposition("SendMotion", + $"compensated axes do not intersect frontTh={newLocalFrontTh:F1} rearTh={newLocalRearTh:F1}", + deltaTime); + myRotCenter = localCenter; + sign = myRotCenter.Y > 1 ? 1 : -1; + + // SendMotionVisualizer?.DrawLine(Color.Red, myRotCenter, localPFront, width: 2); + // SendMotionVisualizer?.DrawLine(Color.Red, myRotCenter, localPRear, width: 2); + } + else + { + currentFrontTh = newLocalFrontTh; + currentRearTh = newLocalRearTh; + thDiff = CommonMath.ThDiff(currentFrontTh, currentRearTh); + } + } + } + + _rotCenter = myRotCenter; + Hedingben.ToastText($"fTh:{frontTh:0.0}, rTh:{rearTh:0.0}, DA:{_originBiasTh:0.00}, " + + $"rc:({_rotCenter.X:0.0},{_rotCenter.Y:0.0}), _rb:{_rearBase}, _fb:{_frontBase}", "rotCenter"); + var r0 = _rotCenter.Length(); + + // 计算所有轮子的角度 + bool anyWheelInvalid = false; + for (var i = 0; i < _steerWheels.Count; i++) + { + var sw = _steerWheels[i]; + float th; + + if (axisDiffFlag) + { + var sTh = (float)(Math.Atan2(sw.Position.Y - _rotCenter.Y, sw.Position.X - _rotCenter.X) / + Math.PI * 180) + sign * 90; + th = CommonMath.ThDiff(sTh, sw.ZeroDirection); + var r1 = Vector2.Distance(sw.Position, _rotCenter); + _tmpSpeeds[i] = r1 / (r0 + 0.00001f); + } + else + { + var axisTh = currentRearTh + thDiff * Interpolate(_rearBase, _wheelBases[i], _frontBase); + _tmpSpeeds[i] = 1; + th = CommonMath.ThDiff(axisTh, sw.ZeroDirection); + } + + if (!TryResolveWheelAngle(i, th, "SendMotion", out var resolvedTh, out var resolvedDir, + out var resolveReason)) + { + anyWheelInvalid = true; + needAdjust = true; + lastInvalidReason = resolveReason; + break; + } + + _wheelDirs[i] = resolvedDir; + tmpAngles.Add(resolvedTh); + } + + if (needAdjust && anyWheelInvalid) + { + return FailMotionDecomposition("SendMotion", lastInvalidReason, deltaTime); + } + + finalAngles = tmpAngles; + break; + } + + if (finalAngles.Count == _steerWheels.Count) + { + for (var i = 0; i < _steerWheels.Count; i++) + { + SendTh(i, finalAngles[i]); + } + } + else + { + return FailMotionDecomposition("SendMotion", + $"unable to find valid wheel angles frontTh={frontTh:F1} rearTh={rearTh:F1}", + deltaTime); + } + + // // todo: find better way to limit max speed + // var totalTurn = Math.Abs(frontTh) + Math.Abs(rearTh); + // var turnThresholdSpeed = CalculateTurningSpeedDecayFac(totalTurn) * MaxSpeed; + for (int i = 0; i < _steerWheels.Count; i++) + { + _debugSpeeds[i] = _tmpSpeeds[i] * speed * _wheelDirs[i]; + } + var speedBeforeAlignGate = speed; + + if (!GoingWheelAligned) + { + Hedingben.ToastText("Wheel Not Aligned", "MultiWheelChassis-SendMotion-notAligned"); + speed = 0; + var aligned = _steerWheels.Select((sw, i) => (sw, i)).All(ww => + Math.Abs(CommonMath.ThDiff(ww.sw.ReadAngle(), _sendAngle[ww.i])) < 1); + GoingWheelAligned = aligned; + } + + for (var i = 0; i < _steerWheels.Count; i++) + { + AccumulateSpeed(i, _tmpSpeeds[i] * speed * _wheelDirs[i], axisDiffFlag, _rotCenter, deltaTime); + } + + if ((DateTime.Now - _sendMotionDetailLastLog).TotalMilliseconds >= 200) + { + _sendMotionDetailLastLog = DateTime.Now; + var sb = new StringBuilder(); + sb.Append($"SendMotionDetail speed:{speed:F3} speedBeforeGate:{speedBeforeAlignGate:F3} fTh:{frontTh:F1} rTh:{rearTh:F1} " + + $"curF:{currentFrontTh:F1} curR:{currentRearTh:F1} " + + $"originBias:({_originBiasX:F1},{_originBiasY:F1},{_originBiasTh:F1}) " + + $"in(speed:{compInputSpeed:F3},f:{compInputFrontTh:F1},r:{compInputRearTh:F1}) " + + $"comp({localCompensateX:F1},{localCompensateY:F1},{localCompensateTh:F2}) " + + $"motionTh:{compMotionTh:F1} along:{compAlong:F1} side:{compSide:F1} dSpeed:{compDSpeed:F3} dTh:{compDTh:F1} " + + $"axisDiff:{axisDiffFlag} rotCenter:({_rotCenter.X:F1},{_rotCenter.Y:F1}) goingAligned:{GoingWheelAligned}"); + for (var i = 0; i < _steerWheels.Count; i++) + { + var sw = _steerWheels[i]; + var readTh = sw.ReadAngle(); + var targetTh = _sendAngle[i]; + var errTh = CommonMath.ThDiff(readTh, targetTh); + sb.Append($" | w{i} tgt:{targetTh:F1} read:{readTh:F1} err:{errTh:F1} " + + $"zero:{sw.ZeroDirection:F1} rel:{sw.GetAngleRelativeToChassis():F1} " + + $"dir:{_wheelDirs[i]} tmp:{_tmpSpeeds[i]:F2} targetV:{_targetSpeeds[i]:F3} sendV:{_sendSpeeds[i]:F3} " + + $"lim:[{sw.AngleLowerLimit:F1},{sw.AngleUpperLimit:F1}]"); + if (sw is DiffSteerWheel dsw) + { + sb.Append($" diffSend(L:{dsw.GetLeftSendSpeed():F3},R:{dsw.GetRightSendSpeed():F3})"); + } + else + { + sb.Append($" wheelSend:{sw.GetSendSpeed():F3}"); + } + } + DLog.Log(sb.ToString(), "MultiWheelMotionDetail"); + } + + LastMoveTime = DateTime.Now; + LastMotionDecomposeFailureReason = ""; + return true; + } + + /// + /// 绕"已被 SetOriginBias 偏置到车队中心的原点"做原地旋转，可叠加一个车体系小幅纠偏旋量。 + /// + /// 绕车队中心角速度(deg/s，逆时针为正)。 + /// 车体系纵向(前+)修正速度(mm/s)，多车联动维持队形用。 + /// 车体系横向(左+)修正速度(mm/s)。 + /// 绕本车几何中心附加角速度(deg/s)，修正朝向偏差。 + public bool SendRotateMotion(float rotSpeed, TimeSpan? deltaTime = null, + float localCompensateX = 0f, float localCompensateY = 0f, float localCompensateTh = 0f) + { + if (!Valid) return FailMotionDecomposition("SendRotateMotion", "invalid chassis", deltaTime); + LastMotionDecomposeFailureReason = ""; + if (Math.Abs(rotSpeed) < 1e-6f && + Math.Abs(localCompensateX) < 1e-6f && + Math.Abs(localCompensateY) < 1e-6f && + Math.Abs(localCompensateTh) < 1e-6f) + { + LastRotateAligned = true; + RampStop(deltaTime); + return true; + } + + var ths = new float[_steerWheels.Count]; + var dirs = new int[_steerWheels.Count]; + // 每轮合速度大小(m/s)，含"绕队心旋转 + 车体平移纠偏 + 绕本车中心微转纠偏"三项矢量和。 + var speedMags = new float[_steerWheels.Count]; + var allWheelAligned = true; + + // 把车体系纠偏旋量换算到 sw.Position 所在的偏置帧 F(原点=车队中心, 朝向随 _originBiasTh)。 + // body->F 旋转为 R(_originBiasTh)，与 SetOriginBias 里 Transform2D 的约定一致。 + var radBias = _originBiasTh / 180.0 * Math.PI; + var cosB = Math.Cos(radBias); + var sinB = Math.Sin(radBias); + var compVxF = localCompensateX * cosB - localCompensateY * sinB; // mm/s，F帧 + var compVyF = localCompensateX * sinB + localCompensateY * cosB; + var rotRad = rotSpeed / 180.0 * Math.PI; // 绕队心(F原点)角速度 rad/s + var compRad = localCompensateTh / 180.0 * Math.PI; // 绕本车几何中心附加角速度 rad/s + var biasX = (double)_originBiasX; // 本车几何中心在 F 中的位置 + var biasY = (double)_originBiasY; + + for (var i = 0; i < _steerWheels.Count; i++) + { + var sw = _steerWheels[i]; + var px = (double)sw.Position.X; + var py = (double)sw.Position.Y; + // 合成轮速矢量(mm/s, F帧)：v = ω_rot ẑ×p + [R(bias)·v_comp + ω_comp ẑ×(p - 本车中心)] + // 旋转切向项与纠偏项分开算，便于把纠偏钳到旋转切向的一定比例。 + var vxRot = -rotRad * py; + var vyRot = rotRad * px; + var vxComp = compVxF - compRad * (py - biasY); + var vyComp = compVyF + compRad * (px - biasX); + // 纠偏钳位：|纠偏| ≤ RotateCompTangentFrac × |旋转切向|，限制合矢量相对纯切向的最大偏角 + // (frac=0.5 → 偏角≤26.6°)，避免减速末段纠偏盖过旋转切向导致舵轮 180° 乱翻。 + if (RotateCompTangentFrac >= 0f) + { + var rotMag = Math.Sqrt(vxRot * vxRot + vyRot * vyRot); + var compMag = Math.Sqrt(vxComp * vxComp + vyComp * vyComp); + var compLimit = RotateCompTangentFrac * rotMag; + if (compMag > compLimit && compMag > 1e-6) + { + var s = compLimit / compMag; + vxComp *= s; + vyComp *= s; + } + } + var vx = vxRot + vxComp; + var vy = vyRot + vyComp; + var vmag = Math.Sqrt(vx * vx + vy * vy); + speedMags[i] = (float)(vmag / 1000.0); + + // 轮子应指向的运动方向(F帧)。纠偏后合速度≈0 时退化为纯旋转切向，避免 atan2 抖动。 + var angleF = vmag < 1e-6 + ? Math.Atan2(px, -py) / Math.PI * 180 + : Math.Atan2(vy, vx) / Math.PI * 180; + var tangent = CommonMath.ThDiff((float)angleF, sw.ZeroDirection); + + if (!TryResolveWheelAngle(i, tangent, "SendRotateMotion", out ths[i], out dirs[i], + out var resolveReason)) + return FailMotionDecomposition("SendRotateMotion", resolveReason, deltaTime); + _wheelDirs[i] = dirs[i]; + } + + for (var i = 0; i < _steerWheels.Count; ++i) + SendTh(i, ths[i]); + var slowFac = 1f; + var maxDth = 0f; + for (var i = 0; i < _steerWheels.Count; ++i) + { + var dth = Math.Abs(CommonMath.ThDiff(ths[i], _steerWheels[i].ReadAngle())); + maxDth = Math.Max(maxDth, dth); + slowFac = Math.Min(slowFac, CommonMath.gaussmf(dth, rotSync, 0)); + // if (dth > 5) + // { + // allWheelAligned = false; + // break; + // } + } + LastRotateAligned = allWheelAligned; // 供上层做积分抗饱和 + + // 对齐门控：未对齐则整体停车；对齐后用 slowFac 平滑提速。旋转与纠偏被同等缩放，保持几何一致。 + var gateFac = allWheelAligned ? slowFac : 0f; + for (var i = 0; i < _steerWheels.Count; ++i) + AccumulateSpeed(i, speedMags[i] * gateFac * dirs[i], true, Vector2.Zero, deltaTime); + + // === 原地旋转诊断（节流 ~300ms）=== + // 关注 allWheelAligned（false 说明舵轮追不上目标角）、dirs/半径，以及叠加的纠偏旋量 comp(vx,vy,om)。 + if ((DateTime.Now - _rotDbgLast).TotalMilliseconds >= 300) + { + _rotDbgLast = DateTime.Now; + var sb = new StringBuilder(); + sb.Append($"SendRotateMotion in:{rotSpeed:F1} aligned:{allWheelAligned} maxDth:{maxDth:F1} slowFac:{slowFac:F2} gate:{gateFac:F2} biasTh:{_originBiasTh:F1} comp(vx:{localCompensateX:F1} vy:{localCompensateY:F1} om:{localCompensateTh:F1})"); + for (var i = 0; i < _steerWheels.Count; ++i) + { + var sw = _steerWheels[i]; + sb.Append($" | w{i} tgt:{ths[i]:F1} read:{sw.ReadAngle():F1} dth:{Math.Abs(CommonMath.ThDiff(ths[i], sw.ReadAngle())):F1} dir:{dirs[i]} R:{sw.Position.Length():F0} v:{speedMags[i] * gateFac * dirs[i]:F3} send:{sw.GetSendSpeed():F2}"); + } + DLog.Log(sb.ToString(), "RotateDbg"); + } + + LastMoveTime = DateTime.Now; + LastMotionDecomposeFailureReason = ""; + return true; + } + + private DateTime _rotDbgLast = DateTime.MinValue; + + public override float CalculateTurningSpeedDecayFac(float turn) + { + return 1 - Math.Min(turn, MaxTurnThreshold) / MaxTurnThreshold * MinTurnSpeedFac; + } + + [Obsolete] + public List GetSteerWheels() + { + return _steerWheels.ToList(); + } + + private void SendTh(int i, float targetTh) + { + _steerWheels[i].WriteAngle(targetTh); + _sendAngle[i] = targetTh; + } + + private (int direction, float rangeFront, float rangeRear) DetermineWheelDirection(SteerWheel wheel) + { + var lower = CommonMath.RoundTh(wheel.AngleLowerLimit + wheel.ZeroDirection); + var upper = CommonMath.RoundTh(wheel.AngleUpperLimit + wheel.ZeroDirection); + while (upper < lower) upper += 360; + + var frontDir = 0; + var rearDir = 180; + + // 计算前向和后向的可用范围 + var rangeFront = Math.Min(upper - frontDir, frontDir - lower); + var rangeRear = Math.Min(upper - rearDir, rearDir - lower); + + // 选择范围更大的方向 + if (rangeFront >= rangeRear && rangeFront > 0) + { + return (1, rangeFront, rangeRear); + } + else if (rangeRear > rangeFront && rangeRear > 0) + { + return (-1, rangeFront, rangeRear); + } + + // 如果两个方向都不可行 + return (0, rangeFront, rangeRear); + } + + private void AccumulateSpeed(int i, float v, bool axisDiff, Vector2 rotCenter, TimeSpan? deltaTime = null) + { + _targetSpeeds[i] = v; + var speedSign = Math.Sign(_targetSpeeds[i] - _sendSpeeds[i]); + var acc = Math.Abs(_targetSpeeds[i]) > Math.Abs(_sendSpeeds[i]) ? AccPerSecond : DeAccPerSecond; + _sendSpeeds[i] += speedSign * Math.Min(Math.Abs(_targetSpeeds[i] - _sendSpeeds[i]), + acc * (float)(deltaTime ?? DateTime.Now - LastMoveTime).TotalSeconds); + if (_steerWheels[i] is DiffSteerWheel dsw) + { + if (axisDiff) + { + var wheelRadius = Vector2.Distance(rotCenter, dsw.Position); + var wheelDir = _sendAngle[i]; + + var left = CommonMath.Transform2D(dsw.Position, wheelDir + 90, new Vector2(dsw.WheelDistance, 0)); + var right = CommonMath.Transform2D(dsw.Position, wheelDir - 90, new Vector2(dsw.WheelDistance, 0)); + dsw.WriteLeftSpeed(_sendSpeeds[i] / wheelRadius * Vector2.Distance(rotCenter, left)); + dsw.WriteRightSpeed(_sendSpeeds[i] / wheelRadius * Vector2.Distance(rotCenter, right)); + } + else + { + dsw.WriteLeftSpeed(_sendSpeeds[i]); + dsw.WriteRightSpeed(_sendSpeeds[i]); + } + } + else + { + _steerWheels[i].WriteSpeed(_sendSpeeds[i]); + } + + if (Debug) + Console.WriteLine($"Ackermann wheel{i}: target:{_targetSpeeds[i]:0.00},send:{_sendSpeeds[i]:0.0}"); + } + + private void CalculateAxes() + { + var axes = _steerWheels + .Select(sw => (sw, Vector2.Dot(sw.Position, new Vector2(1, 0)))) + .OrderByDescending(ax => ax.Item2).ToList(); + _wheelBases = axes.Select(ax => ax.Item2).ToList(); + _steerWheels = axes.Select(ax => ax.sw).ToList(); + GeometricControlPoints = new List() + { + new (new Vector2(ControlPointRadius, 0)), + new (new Vector2(-ControlPointRadius, 0)) + }; + _frontBase = _wheelBases.First(); + _rearBase = _wheelBases.Last(); + } + + private List _steerWheels = new (); + private float _frontBase, _rearBase; + private List _wheelBases; + private List _sendSpeeds; + private List _targetSpeeds; + private List _sendAngle; + private List _debugSpeeds; + private DateTime _sendMotionDetailLastLog = DateTime.MinValue; + private DateTime _geometricComputeLastLog = DateTime.MinValue; + + private List _tmpSpeeds; + // add TimeStamp, prevent the wheel from swaying. + // for example, target angle is 90, current wheel is around 0. if no TimeStamp, + // wheel will swing between 90 and -90. + private List _wheelDirs; + + // call this before a new continuous motion happens + private void ResetMotionState() + { + LastMoveTime = DateTime.Now; + GoingWheelAligned = false; + _wheelDirs = Enumerable.Repeat(1, _steerWheels.Count).ToList(); + } + + public void AddTestFunction() + { + + } + /// + /// 得到相对舵轮在车体坐标系下的分解速度 + /// + /// + /// + /// + /// 单位为°/s + /// + private Vector2 VectorVelocity(Vector2 pos, float vx, float vy, float vth, int i) + { + var vRotX = -vth / 180 * (float)Math.PI * pos.Y / 1000; + var vRotY = vth / 180 * (float)Math.PI * pos.X / 1000; + Hedingben.ToastText($"vRot:({vRotX:F3},{vRotY:F3}) pos:({pos.X:F1},{pos.Y:F1})", + $"SendXYThSpeed-VectorVelocity{i}"); + return new Vector2(vx + vRotX, vy + vRotY); + } + /// + /// 获得车轮应该打的角度和速度 + /// + /// + /// + /// + /// 单位为°/s + /// + private (float angle, float speed) AngleAndSpeed(Vector2 pos, float vx, float vy, float vth, int i) + { + var v = VectorVelocity(pos, vx, vy, vth, i); + return ((float)(Math.Atan2(v.Y, v.X) / Math.PI * 180), v.Length()); + } + + private float w = 0; + private float wAcc = 0.1f; + private float rotSync = 3f; + private bool XYThActive = false; + + public bool SendXYThSpeed(float vx, float vy, float vth, TimeSpan? deltaTime = null) + { + if (!Valid) return FailMotionDecomposition("SendXYThSpeed", "invalid chassis", deltaTime); + if (!XYThActive) + { + ResetMotionState(); + w = 0; + } + XYThActive = true; + GoingActive = false; + RotatingActive = false; + + if (Math.Abs(vx) < 1e-6f && Math.Abs(vy) < 1e-6f && Math.Abs(vth) < 1e-6f) + { + RampStop(deltaTime); + LastMotionDecomposeFailureReason = ""; + return true; + } + + w = w + wAcc; + if (w > 1) w = 1; + float[] sendSpeed = new float[_steerWheels.Count]; + for (var i = 0; i < _steerWheels.Count; i++) + { + var sw = _steerWheels[i]; + var (angle, speed) = AngleAndSpeed(sw.Position, vx, vy, vth, i); + var actualTh = sw.ReadAngle(); + if (!TryResolveWheelAngle(i, CommonMath.ThDiff(angle, sw.ZeroDirection), "SendXYThSpeed", + out var useAngle, out var dir, out var resolveReason)) + return FailMotionDecomposition("SendXYThSpeed", resolveReason, deltaTime); + speed *= dir; + _wheelDirs[i] = dir; + + sendSpeed[i] = speed; + if (speed!=0) + SendTh(i, useAngle); + w = (float)Math.Min(w, CommonMath.gaussmf(CommonMath.ThDiff(actualTh, _sendAngle[i]), rotSync, 0)); + + Hedingben.ToastText($"w:{w:F2} s:{speed:F3} th:{_sendAngle[i]:F1} actualTh:{actualTh:F1}", $"SendXYThSpeed-{i}"); + } + for (var i = 0; i < _steerWheels.Count; i++) + AccumulateSpeed(i, w * sendSpeed[i],false,new Vector2(0f,0f), deltaTime); + //todo 计算rotCenter填入 + LastMoveTime = DateTime.Now; + LastMotionDecomposeFailureReason = ""; + return true; + } + + public override CarSpeed GetCarSpeed(bool isActual = false) + { + List vx = new List(); + List vy = new List(); + List vth = new List(); + for (var i = 0; i < _steerWheels.Count; ++i) + { + var sw1 = _steerWheels[i]; + Vector2 a = sw1.Position / 1000; + //var tha = _sendAngle[i] / 180 * (float)Math.PI; + var tha = isActual ? sw1.ReadAngle() / 180 * (float)Math.PI : _sendAngle[i] / 180 * (float)Math.PI; + //var speeda = _sendSpeeds[i]; + //var speeda = isActual ? sw1.ReadSpeed() : _sendSpeeds[i]; + var speeda = isActual ? sw1.ReadSpeed() : _debugSpeeds[i]; + Vector2 va = new Vector2(speeda * (float)Math.Cos(tha), speeda * (float)Math.Sin(tha)); + // painter.DrawLine(Color.Green, sw1.Position, + // LessMath.Transform2D(sw1.Position, sw1.ReadAngle(), new Vector2(200, 0)), endArrow: true); + for (var j = i + 1; j < _steerWheels.Count; ++j) + { + var sw2 = _steerWheels[j]; + Vector2 b = sw2.Position / 1000; + //var thb = _sendAngle[j] / 180 * (float)Math.PI; + var thb = isActual ? sw2.ReadAngle() / 180 * (float)Math.PI : _sendAngle[j] / 180 * (float)Math.PI; + //var speedb = _sendSpeeds[j]; + //var speedb = isActual ? sw2.ReadSpeed() : _sendSpeeds[j]; + var speedb = isActual ? sw2.ReadSpeed() : _debugSpeeds[j]; + Vector2 vb = new Vector2(speedb * (float)Math.Cos(thb), speedb * (float)Math.Sin(thb)); + var (tempvx, tempvy, tempvth) = CenterVelocityFromPoints(a, va, b, vb); + vx.Add(tempvx); + vy.Add(tempvy); + vth.Add(tempvth); + } + } + return new CarSpeed() + { Vx = vx.Average(), Vy = vy.Average(), Vw = (float)(vth.Average() / Math.PI * 180f) }; + } + + private static (float, float, float) CenterVelocityFromPoints(Vector2 a, Vector2 va, Vector2 b, Vector2 vb) + { + float vth_x = 0, vth_y = 0, vth = 0, vx = 0, vy = 0; + var eps = 0.0000001; + if (Math.Abs(a.Y - b.Y) > eps) + { + vth_x = (va.X - vb.X) / (b.Y - a.Y); + } + + if (Math.Abs(a.X - b.X) > eps) + { + vth_y = (va.Y - vb.Y) / (a.X - b.X); + } + vth = vth_x == 0 ? vth_y : vth_x; + vx = va.X + vth * a.Y; + vy = va.Y - vth * a.X; + return (vx, vy, vth); + } + } +} diff --git a/CommonUsage-MultiVehicleSync/commonusage/Chassis/SingleSteerChassis.cs b/CommonUsage-MultiVehicleSync/commonusage/Chassis/SingleSteerChassis.cs new file mode 100644 index 0000000..cb0df3e --- /dev/null +++ b/CommonUsage-MultiVehicleSync/commonusage/Chassis/SingleSteerChassis.cs @@ -0,0 +1,172 @@ +using System; +using System.Collections.Generic; +using System.Linq; +using System.Numerics; +using System.Text; +using System.Diagnostics; +using CommonUsage.Mathematics; +using FundamentalLib; + +namespace CommonUsage.Chassis +{ + public class SingleSteerChassis : AbstractChassis + { + public void SetSteerWheel(SteerWheel wheel) + { + _steerWheel = wheel; + } + + public SteerWheel GetSteerWheel() + { + return _steerWheel; + } + + public override void Visualize() + { + + } + + public override CarSpeed GetCarSpeed(bool isActual = false) + { + if (!isActual) + { + var sendAngle = _steerWheel.GetSendAngle(); + var sendAngleRad = _steerWheel.GetSendAngle() / 180f * Math.PI; + // VSteer* Cos = v; + var vsteer = _sendSpeed / ((Math.Cos(Math.Abs(sendAngleRad)) + 0.000001)); + var vsteerY = vsteer * Math.Sin(sendAngleRad); + // Console.WriteLine($"{vsteer} {vsteerY} {sendAngleRad} {_sendSpeed}"); + return new CarSpeed() + { + Vx = (float)(_sendSpeed * Math.Cos(Math.Abs(sendAngle) / 180f * Math.PI)), + Vy = 0, + Vw = (float)(_sendSpeed * Math.Sin(Math.Abs(sendAngle) / 180f * Math.PI) / + Math.Abs(_steerWheel.Position.X / 1000f) / Math.PI * 180f) + //阿克曼 + // Vx = (float)(_sendSpeed), + // Vy = 0, + // Vw = (float)(vsteerY / Math.Abs(_steerWheel.Position.X / 1000f) / Math.PI * 180f) + }; + } + else + { + return new CarSpeed() + { + Vx = (float)(_steerWheel.ReadSpeed() * + Math.Cos(Math.Abs(_steerWheel.ReadAngle()) / 180f * Math.PI)), + Vy = 0, + Vw = (float)(_steerWheel.ReadSpeed() * + Math.Sin(Math.Abs(_steerWheel.ReadAngle()) / 180f * Math.PI) / + Math.Abs(_steerWheel.Position.X / 1000f) / Math.PI * 180f) + }; + } + + } + + public override void Initialize() + { + GeometricControlPoints = new List() + { + new (_steerWheel.Position), + new (Vector2.Zero) + }; + Valid = true; + } + + public override void AfterDirectionChanged() + { + if (Math.Abs(CommonMath.ThDiff(0, _originBiasTh)) > 90) + { + GeometricControlPoints = new List() + { + new (-_steerWheel.Position), + }; + _direction = -1; + } + else + { + GeometricControlPoints = new List() + { + new (_steerWheel.Position), + }; + _direction = 1; + } + } + + public override void PredefinedDriveStop() + { + if (!Valid) return; + _sendSpeed = 0; + _steerWheel.WriteSpeed(_sendSpeed); + GoingActive = false; + RotatingActive = false; + } + + protected override void DefineGeometricWheelComputation(float speed) + { + var now = DateTime.Now; + + if (!GoingActive) + { + LastMoveTime = now; + GoingWheelAligned = false; + } + + SendSteerMotion(speed * _direction, GeometricControlPoints[0].Theta, now - LastMoveTime); + + GoingActive = true; + RotatingActive = false; + } + + public override bool ComputeRotateWheels(float rotSpeed) + { + if (!RotatingActive) + { + LastMoveTime = DateTime.Now; + GoingWheelAligned = false; + } + + SendSteerMotion(rotSpeed, 90); + + GoingActive = false; + RotatingActive = true; + return true; + } + + public void SendSteerMotion(float speed, float theta, TimeSpan? deltaTime = null) + { + _steerWheel.WriteAngle(theta); + + if (!GoingWheelAligned && Math.Abs(CommonMath.ThDiff(_steerWheel.ReadAngle(), theta)) < 1) + GoingWheelAligned = true; + if (!GoingWheelAligned) speed = 0; + + var turnThresholdSpeed = CalculateTurningSpeedDecayFac(Math.Abs(theta)) * MaxSpeed; + AccumulateSpeed(Math.Min(turnThresholdSpeed, Math.Abs(speed)) * Math.Sign(speed), deltaTime); + + LastMoveTime = DateTime.Now; + } + + public override float CalculateTurningSpeedDecayFac(float turn) + { + return 1 - Math.Min(turn, MaxTurnThreshold) / MaxTurnThreshold * MinTurnSpeedFac; + } + + private void AccumulateSpeed(float v, TimeSpan? deltaTime = null) + { + // _targetSpeed = v; + var speedSign = Math.Sign(v - _sendSpeed); + var acc = Math.Abs(v) > Math.Abs(_sendSpeed) ? AccPerSecond : DeAccPerSecond; + _sendSpeed += speedSign * Math.Min(Math.Abs(v - _sendSpeed), + acc * (float)(deltaTime ?? DateTime.Now - LastMoveTime).TotalSeconds); + _steerWheel.WriteSpeed(_sendSpeed); + + if (Debug) + Console.WriteLine($"SingleSteer, target:{v:0.00},send:{_sendSpeed:0.0}"); + } + + private SteerWheel _steerWheel; + private float _sendSpeed; + private int _direction = 1; // 1 forward, -1 backward + } +} diff --git a/CommonUsage-MultiVehicleSync/commonusage/Chassis/SteerWheel.cs b/CommonUsage-MultiVehicleSync/commonusage/Chassis/SteerWheel.cs new file mode 100644 index 0000000..00bc7c4 --- /dev/null +++ b/CommonUsage-MultiVehicleSync/commonusage/Chassis/SteerWheel.cs @@ -0,0 +1,100 @@ +using Newtonsoft.Json; +using System; +using System.Collections.Generic; +using System.Numerics; +using System.Text; +using CommonUsage.Mathematics; + +namespace CommonUsage.Chassis +{ + public class SteerWheel : Wheel + { + public SteerWheel(Vector2 position, float angleLowerLimit, float angleUpperLimit, Action speedWriter, + Func speedReader, Action angleWriter, Func angleReader, float angleLimitMarginDeg = 15f) : base(position, speedWriter, + speedReader) + { + _angleLowerLimit = angleLowerLimit; + _angleUpperLimit = angleUpperLimit; + _angleWriter = angleWriter; + _angleReader = angleReader; + _centerDistance = position.Length(); + AngleLimitMarginDeg = angleLimitMarginDeg; + } + + public float AngleLimitMarginDeg = 15f; + + public bool TrySetDirection(bool allowReverse, ref float desireDirection, ref int dir) + { + if (TryNormalizeAngleInLimit(desireDirection, out var normalized)) + { + dir = 1; + desireDirection = normalized; + return true; + } + + if (!allowReverse) return false; + + var oppositeTh = (float)CommonMath.RoundTh(desireDirection + 180); + if (TryNormalizeAngleInLimit(oppositeTh, out normalized)) + { + dir = -1; + desireDirection = normalized; + return true; + } + + dir = 0; + return false; + } + + private bool TryNormalizeAngleInLimit(float angle, out float normalized) + { + var lower = CommonMath.RoundTh(_angleLowerLimit); + var upper = CommonMath.RoundTh(_angleUpperLimit); + while (upper < lower) upper += 360; + + normalized = (float)CommonMath.RoundTh(angle); + while (normalized < lower) normalized += 360; + while (normalized > upper && normalized - 360 >= lower) normalized -= 360; + + var margin = Math.Min(normalized - lower, upper - normalized); + return normalized >= lower && normalized <= upper && margin >= Math.Max(0, AngleLimitMarginDeg); + } + + public float ReadAngle() + { + return _angleReader(); + } + + public void WriteAngle(float angle) + { + _angleWriter.Invoke(_sendAngle = Math.Max(_angleLowerLimit, Math.Min(angle, _angleUpperLimit))); + } + + public float GetSendAngle() + { + return _sendAngle; + } + + public float GetAngleRelativeToChassis() + { + return ZeroDirection + _sendAngle; + } + + public float CenterDistance() + { + return _centerDistance; + } + + public float AngleLowerLimit => _angleLowerLimit; + + public float AngleUpperLimit => _angleUpperLimit; + + [JsonIgnore] private readonly Action _angleWriter; + [JsonIgnore] private readonly Func _angleReader; + + private float _angleLowerLimit = -90, _angleUpperLimit = 90; + private float _centerDistance; + + public float _sendAngle = 0f; + } +} diff --git a/CommonUsage-MultiVehicleSync/commonusage/Chassis/Wheel.cs b/CommonUsage-MultiVehicleSync/commonusage/Chassis/Wheel.cs new file mode 100644 index 0000000..dea87e1 --- /dev/null +++ b/CommonUsage-MultiVehicleSync/commonusage/Chassis/Wheel.cs @@ -0,0 +1,56 @@ +using System; +using System.Collections.Generic; +using System.Numerics; +using System.Text; +using Newtonsoft.Json; + +namespace CommonUsage.Chassis +{ + public class Wheel + { + public Wheel(Vector2 position, Action speedWriter, Func speedReader) + { + PhysicalPosition = Position = position; + SpeedWriter = speedWriter; + SpeedReader = speedReader; + } + + public void WriteSpeed(float speed) + { + SpeedWriter.Invoke(_sendSpeed = speed); + } + + public float GetSendSpeed() + { + return _sendSpeed; + } + + public float ReadSpeed() + { + return SpeedReader(); + } + + // PhysicalPosition ===(chassis transform)===> Position + // useful in dual agv coordination + public readonly Vector2 PhysicalPosition; + public Vector2 Position; + + public float ZeroDirection = 0; + + [JsonIgnore] public readonly Action SpeedWriter; + [JsonIgnore] public readonly Func SpeedReader; + + public float _sendSpeed; + } + + public class GeometricControlPoint + { + public GeometricControlPoint(Vector2 position) + { + Position = position; + } + + public Vector2 Position; + public float Theta; + } +} diff --git a/CommonUsage-MultiVehicleSync/commonusage/CommonUsage.csproj b/CommonUsage-MultiVehicleSync/commonusage/CommonUsage.csproj new file mode 100644 index 0000000..d5f4d92 --- /dev/null +++ b/CommonUsage-MultiVehicleSync/commonusage/CommonUsage.csproj @@ -0,0 +1,48 @@ + + + + netstandard2.0 + + + + latest + True + + + + embedded + + + + embedded + + + + + + + + + + + + + + + + + + ..\..\MedullaAdapter\ref\RefFundamentalLib.dll + + + ..\..\ClumsyPilot\ref\RefClumsyCore.dll + + + + + + + + + diff --git a/CommonUsage-MultiVehicleSync/commonusage/CommonUsageSln.sln b/CommonUsage-MultiVehicleSync/commonusage/CommonUsageSln.sln new file mode 100644 index 0000000..227bf56 --- /dev/null +++ b/CommonUsage-MultiVehicleSync/commonusage/CommonUsageSln.sln @@ -0,0 +1,25 @@ + +Microsoft Visual Studio Solution File, Format Version 12.00 +# Visual Studio Version 17 +VisualStudioVersion = 17.5.33424.131 +MinimumVisualStudioVersion = 10.0.40219.1 +Project("{9A19103F-16F7-4668-BE54-9A1E7A4F7556}") = "CommonUsage", "CommonUsage.csproj", "{E1C5DEA8-3785-40A9-9965-E51E65BF5947}" +EndProject +Global + GlobalSection(SolutionConfigurationPlatforms) = preSolution + Debug|Any CPU = Debug|Any CPU + Release|Any CPU = Release|Any CPU + EndGlobalSection + GlobalSection(ProjectConfigurationPlatforms) = postSolution + {E1C5DEA8-3785-40A9-9965-E51E65BF5947}.Debug|Any CPU.ActiveCfg = Debug|Any CPU + {E1C5DEA8-3785-40A9-9965-E51E65BF5947}.Debug|Any CPU.Build.0 = Debug|Any CPU + {E1C5DEA8-3785-40A9-9965-E51E65BF5947}.Release|Any CPU.ActiveCfg = Release|Any CPU + {E1C5DEA8-3785-40A9-9965-E51E65BF5947}.Release|Any CPU.Build.0 = Release|Any CPU + EndGlobalSection + GlobalSection(SolutionProperties) = preSolution + HideSolutionNode = FALSE + EndGlobalSection + GlobalSection(ExtensibilityGlobals) = postSolution + SolutionGuid = {709F9C19-45DB-46AD-B70A-0E1E3C6CFB0C} + EndGlobalSection +EndGlobal diff --git a/CommonUsage-MultiVehicleSync/commonusage/Deps/RefFundamentalLib.dll b/CommonUsage-MultiVehicleSync/commonusage/Deps/RefFundamentalLib.dll new file mode 100644 index 0000000..8ada7ed Binary files /dev/null and b/CommonUsage-MultiVehicleSync/commonusage/Deps/RefFundamentalLib.dll differ diff --git a/CommonUsage-MultiVehicleSync/commonusage/Geometries/AbstractGeometry.cs b/CommonUsage-MultiVehicleSync/commonusage/Geometries/AbstractGeometry.cs new file mode 100644 index 0000000..28ed7b9 --- /dev/null +++ b/CommonUsage-MultiVehicleSync/commonusage/Geometries/AbstractGeometry.cs @@ -0,0 +1,93 @@ +using System; +using System.Collections.Generic; +using System.Drawing; +using System.Numerics; +using System.Text; +using static CommonUsage.Geometries.CircularArc; + +namespace CommonUsage.Geometries +{ + /// + /// 便于直接创建几何形状并求几何形状的切点、切线等。 + /// + public abstract class AbstractGeometry + { + protected AbstractGeometry() + { + PaddingType = Padding.StartExtendEndExtend; + VisualizeOption = new VisualizeOption(Color.Red, Color.Gray); + } + + public Padding PaddingType; + + public abstract (Vector2 Pt, float Angle, float Bias, float Position) QueryTangentPoint(Vector2 point); + + public abstract void Visualize(Action processDot, Action processLine, + bool visExtendedPart = false); + + public VisualizeOption VisualizeOption; + + /// + /// 查询指定位置的曲率。 + /// + /// 从起点到查询位置的距离。 + /// + public abstract float QueryCurvature(float position); + + public abstract float Length(); + } + + public class VisualizeOption + { + public VisualizeOption(Color mainColor, Color auxiliaryColor) + { + MainColor = mainColor; + AuxiliaryColor = auxiliaryColor; + } + + public Color MainColor; + public Color AuxiliaryColor; + public bool DrawAuxiliary = true; + public bool VisualizeDirection = true; + } + + public enum Padding + { + StartLineEndLine = 0b_0001_0001, + StartLineEndExtend = 0b_0001_0010, + StartExtendEndLine = 0b_0010_0001, + StartExtendEndExtend = 0b_0010_0010, + } + + public class VisDot + { + public VisDot(Vector2 point, Color color) + { + Point = point; + Color = color; + } + + public Vector2 Point; + public Color Color; + } + + public class VisLine + { + public VisLine(Vector2 start, Vector2 end, bool startArrow, bool endArrow, Color color, float width = 1) + { + Start = start; + End = end; + StartArrow = startArrow; + EndArrow = endArrow; + Color = color; + Width = width; + } + + public Vector2 Start; + public Vector2 End; + public bool StartArrow = false; + public bool EndArrow = false; + public Color Color; + public float Width; + } +} diff --git a/CommonUsage-MultiVehicleSync/commonusage/Geometries/BezierCurve.cs b/CommonUsage-MultiVehicleSync/commonusage/Geometries/BezierCurve.cs new file mode 100644 index 0000000..c4c8bbe --- /dev/null +++ b/CommonUsage-MultiVehicleSync/commonusage/Geometries/BezierCurve.cs @@ -0,0 +1,334 @@ +using System; +using System.Collections.Generic; +using System.Diagnostics; +using System.Linq; +using System.Numerics; +using System.Reflection; +using CommonUsage.Mathematics; + +namespace CommonUsage.Geometries +{ + public class BezierCurve : AbstractGeometry + { + + public BezierCurve(List controlPoints, int resolution = 100) + { + // Console.WriteLine($"BezierCurve1"); + // Console.WriteLine(string.Join(" ",controlPoints.Select(p=>$"{p.X:f2},{p.Y:f2}"))); + _controlPoints = controlPoints; + _resolution = resolution; + InitializeBezier(); + } + + public override void Visualize(Action processDot, Action processLine, bool visExtendedPart = false) + { + if (VisualizeOption.DrawAuxiliary) + for (var i = 0; i < _controlPoints.Count - 1; ++i) + { + processLine(new VisLine(_controlPoints[i], _controlPoints[i + 1], + false, false, VisualizeOption.AuxiliaryColor)); + if (i == 0) continue; + processDot(new VisDot(_controlPoints[i], VisualizeOption.AuxiliaryColor)); + } + + for (var i = 0; i < _bezierPoints.Count - 1; ++i) + { + if (Direction == -1) + { + processLine(new VisLine(_bezierPoints[i + 1], _bezierPoints[i], + false, i == (int)(_bezierPoints.Count / 2), VisualizeOption.MainColor, 2)); + } + else + { + processLine(new VisLine(_bezierPoints[i], _bezierPoints[i + 1], + false, i == (int)(_bezierPoints.Count / 2), VisualizeOption.MainColor, 2)); + } + + + } + } + + public (Vector2 Point, int Id) QueryPoint(Vector2 point) + { + var p = new Vector2(); + var id = -1; + var bestDistance = float.MaxValue; + + var hashes = _bias.Select(bb => CalculateHash(point, 100, bb.X, bb.Y)).ToList(); + + void TryQuery(Dictionary> dict, List hashList) + { + foreach (var hash in hashList) + { + if (!dict.TryGetValue(hash, out var ll)) continue; + foreach (var (q, qId) in ll) + { + var d = Vector2.Distance(q, point); + if (d < bestDistance) + { + p = q; + id = qId; + bestDistance = d; + } + } + } + } + //map目前有bug,取消cpu占用也不严重，必要时候在优化 + // TryQuery(_pointsMappingSmall, hashes); + // + // if (id == -1) + // { + // hashes = _bias.Select(bb => CalculateHash(point, 1000, bb.X, bb.Y)).ToList(); + // TryQuery(_pointsMappingBig, hashes); + // } + + if (id == -1) + { + // todo: improve the way to find closest point if mappings fail + (p, id) = _bezierPoints.Select((p, i) => (p, i)) + .OrderBy(pair => CommonMath.dist(pair.p.X, pair.p.Y, point.X, point.Y)).First(); + } + + return (p, id); + } + + public override (Vector2 Pt, float Angle, float Bias, float Position) QueryTangentPoint(Vector2 point) + { + var (p, id) = QueryPoint(point); + var tangent = _tangents[id]; + var (bias, lp, fd) = CommonMath.Project2DLine(point, p, tangent); + var next = fd > 0 ? id + 1 : id - 1; + if (id == 0) next = 1; + // Console.WriteLine($"id:{id} next:{next} tangent:{tangent} _tangents.Count:{_tangents.Count}"); + if (next > 0 && next < _tangents.Count)//线性插值 + { + var (_, _, t) = CommonMath.Project2DLine(point, _bezierPoints[id], _bezierPoints[next]); + var partial = t / Vector2.Distance(_bezierPoints[id], _bezierPoints[next]); + if (partial >= 0 && partial <= 1) + { + tangent = CommonMath.RoundTh(_tangents[id] + + partial * CommonMath.RoundTh(_tangents[next] - _tangents[id])); + if (CommonMath.RoundTh(_tangents[next] - _tangents[id]) > 5) + Console.WriteLine($"bezier tangents bug, tanget: {id}:{_tangents[id]} {next}:{_tangents[next]}"); + } + // else Console.WriteLine("bezier tangents bug"); + } + return (lp, tangent, bias, fd + _sumDistances[id]); + } + + public override float Length() + { + return _length; + } + + public override float QueryCurvature(float position) + { + int id = _sumDistances.Count - 1; + if (position <= 0) id = 0; + else + { + for (int i = 1; i < _sumDistances.Count; i++) + { + if (position > _sumDistances[i - 1] && position <= _sumDistances[i]) + { + id = i; + break; + } + } + } + + var result = _curvatures[id]; + if (id > 0 && id < _sumDistances.Count - 1)//插值 + { + var partial = (position - _sumDistances[id - 1]) / (_sumDistances[id] - _sumDistances[id - 1]); + if (partial >= 0 && partial <= 1) result = (1 - partial) * _curvatures[id - 1] + partial * _curvatures[id]; + else Console.WriteLine("bezier curvature bug"); + } + + return result; + } + + public Vector3 QueryBezierPointsById(int id) + { + if (id < 0 || id > Resolution) + { + Console.WriteLine($"QueryBezierPointsById out of range, Resolution:{Resolution},id:{id}."); + return new Vector3(0, 0, 0); + } + + return new Vector3(_bezierPoints[id].X, _bezierPoints[id].Y, _tangents[id]); + } + + public List ControlPoints => _controlPoints; + + public int Resolution => _resolution; + /// + /// 仅用于simple显示路径方向 + /// + public int Direction = 1; + public int Order => _order; + + // public List Tangents => _tangents; + + public void UpdateControlPoint(int id, Vector2 point) + { + _controlPoints[id] = point; + InitializeBezier(); + } + + public void AddControlPoint(int id, Vector2 point) + { + _controlPoints.Insert(id, point); + InitializeBezier(); + } + + public void RemoveControlPoint(int id) + { + _controlPoints.RemoveAt(id); + InitializeBezier(); + } + + public Vector2 GetMidPoint() + { + return _bezierPoints[(int)Math.Ceiling(_resolution / 2d)]; + } + + private void InitializeBezier() + { + _order = _controlPoints.Count - 1; + // _bezierPoints = new List(); + var delta = 1.0f / _resolution; + // for (int t = 0; t <= _resolution; t += 1)//下面循环算了，没必要先递归算一遍 + // _bezierPoints.Add(new Vector2(DeCasteljauX(_order, 0, t*delta), DeCasteljauY(_order, 0, t*delta))); + var allPoints = new List>>(); + for (var i = 0; i < _order; i++) + { + var size = allPoints.Count; + var morePoints = new List>(); + for (var j = 0; j < _order - i; j++) + { + var points = new List(); + for (int t = 0; t <= _resolution; t += 1) + { + float p0x; + float p1x; + float p0y; + float p1y; + var z = t; + if (size > 0) + { + p0x = allPoints[i - 1][j][z].X; + p1x = allPoints[i - 1][j + 1][z].X; + p0y = allPoints[i - 1][j][z].Y; + p1y = allPoints[i - 1][j + 1][z].Y; + } + else + { + p0x = _controlPoints[j].X; + p1x = _controlPoints[j + 1].X; + p0y = _controlPoints[j].Y; + p1y = _controlPoints[j + 1].Y; + } + + var part = t * delta; + points.Add(new Vector2((1 - part) * p0x + part * p1x, (1 - part) * p0y + part * p1y)); + } + morePoints.Add(points); + } + allPoints.Add(morePoints); + } + + _bezierPoints = allPoints.Last().Last(); + _tangentInfo = allPoints; + _tangents = Enumerable.Repeat(0f, _bezierPoints.Count).ToList(); + _curvatures = Enumerable.Repeat(0f, _bezierPoints.Count).ToList(); + var p2 = allPoints[Order - 2]; + for (var id = 0; id < _bezierPoints.Count; ++id) + { + _tangents[id] = + (float)(Math.Atan2(p2[1][id].Y - p2[0][id].Y, p2[1][id].X - p2[0][id].X) / Math.PI * 180); + if (id != 0) _curvatures[id] = (float)((CommonMath.ThDiff(_tangents[id], _tangents[id - 1]) / 180 * Math.PI) + / (Vector2.Distance(_bezierPoints[id], _bezierPoints[id - 1]) / 1000)); + } + // Console.WriteLine($"{string.Join("\n", _tangents.Select((val, i) => $"{i}: {val}"))}"); + _tangents[0] = _tangents[1]; // todo: here is temporary fix + _curvatures[0] = _curvatures[1]; + for (var id = 1; id < _bezierPoints.Count - 1; ++id)//前移0.5 + _curvatures[id] = (_curvatures[id] + _curvatures[id + 1]) / 2; + + _remainDistances = Enumerable.Repeat(0f, _bezierPoints.Count).ToList(); + _sumDistances = Enumerable.Repeat(0f, _bezierPoints.Count).ToList(); + for (var i = _bezierPoints.Count - 2; i >= 0; --i) + { + _remainDistances[i] = + _remainDistances[i + 1] + Vector2.Distance(_bezierPoints[i], _bezierPoints[i + 1]); + } + for (var i = 1; i < _bezierPoints.Count; ++i) + { + _sumDistances[i] = + _sumDistances[i - 1] + Vector2.Distance(_bezierPoints[i], _bezierPoints[i - 1]); + } + _length = _sumDistances.Last(); + + _minX = _bezierPoints.Min(pp => pp.X); + _minY = _bezierPoints.Min(pp => pp.Y); + + var tmpList = _bezierPoints.Select((point, index) => (point, index)).ToList(); + return; + void GenerateGridMapping(ref Dictionary> dict, float gSize) + { + dict = new Dictionary>(); + foreach (var (point, index) in tmpList) + { + var hash = CalculateHash(point, gSize); + if (dict.TryGetValue(hash, out var ll)) + ll.Add((point, index)); + else dict[hash] = new List<(Vector2 Point, int Id)>() { (point, index) }; + } + } + + GenerateGridMapping(ref _pointsMappingSmall, 100); + GenerateGridMapping(ref _pointsMappingBig, 1000); + } + + private uint CalculateHash(Vector2 point, float gridSize, int xBias = 0, int yBias = 0) + { + return (uint)(((int)((point.X - _minX) / gridSize) + xBias) << 16 + (((int)((point.Y - _minY) / gridSize) + yBias) & 0xffff)); + } + + private readonly List<(int X, int Y)> _bias = new() + { + new(-1, -1), new(-1, 0), new(-1, 1), + new(0, -1), new(0, 0), new(0, 1), + new(1, -1), new(1, 0), new(1, 1), + }; + + private float DeCasteljauX(int i, int j, float t) + { + if (i == 1) + return (1 - t) * _controlPoints[j].X + t * _controlPoints[j + 1].X; + return (1 - t) * DeCasteljauX(i - 1, j, t) + t * DeCasteljauX(i - 1, j + 1, t); + } + + private float DeCasteljauY(int i, int j, float t) + { + if (i == 1) + return (1 - t) * _controlPoints[j].Y + t * _controlPoints[j + 1].Y; + return (1 - t) * DeCasteljauY(i - 1, j, t) + t * DeCasteljauY(i - 1, j + 1, t); + } + + private int _order; + private int _resolution; + private List _controlPoints; + private List _bezierPoints; + private List>> _tangentInfo; + private List _tangents; + private List _remainDistances; + private List _sumDistances; + private List _curvatures; + private Dictionary> _pointsMappingSmall; + private Dictionary> _pointsMappingBig; + private float _minX, _minY; + private float _length; + } +} diff --git a/CommonUsage-MultiVehicleSync/commonusage/Geometries/CircularArc.cs b/CommonUsage-MultiVehicleSync/commonusage/Geometries/CircularArc.cs new file mode 100644 index 0000000..2126385 --- /dev/null +++ b/CommonUsage-MultiVehicleSync/commonusage/Geometries/CircularArc.cs @@ -0,0 +1,256 @@ +using System; +using System.Collections.Generic; +using System.Drawing; +using System.Numerics; +using System.Reflection; +using System.Security.Cryptography; +using System.Text; +using CommonUsage.Mathematics; + +namespace CommonUsage.Geometries +{ + public class CircularArc : AbstractGeometry + { + /// + /// 以center为圆心、radius为半径，从angleStart逆时针转到angleEnd所构成的圆弧。direction表示圆弧走向。 + /// + /// + /// + /// + /// + /// 表示圆弧走向，1为angleStart到angleEnd，-1为angleEnd到angleStart + public CircularArc(Vector2 center, float radius, float angleStart, float angleEnd, int direction, Padding paddingType) + { + _center = center; + _radius = radius; + _angleStart = angleStart; + _angleEnd = angleEnd; + _direction = direction; + PaddingType = paddingType; + + ChangeShape(); + CalculateVisPoints(); + } + + public override void Visualize(Action processDot, Action processLine, + bool visExtendedPart = false) + { + lock (_visPoints) + { + if (visExtendedPart) + { + + } + + for (var i = 0; i < _visPoints.Length - 1; ++i) + { + if ((i == 0 || i == _visPoints.Length - 2) && !visExtendedPart) continue; + var color = Color.Red; + if (i == 0 || i == _visPoints.Length - 2) color = Color.Gray; + processLine(new VisLine(_visPoints[i], _visPoints[i + 1], + false, i == (_visPoints.Length - 1) / 2, color)); + } + + if (visExtendedPart) + { + + } + } + } + + public void SwitchSide() + { + (_angleStart, _angleEnd) = (_angleEnd, _angleStart); + ChangeShape(); + CalculateVisPoints(); + } + + public float VisAngleResolution = 1; + + public Vector2 Center + { + get => _center; + set + { + _center = value; + ChangeShape(); + CalculateVisPoints(); + } + } + + public float Radius + { + get => _radius; + set + { + _radius = value; + ChangeShape(); + CalculateVisPoints(); + } + } + + public float AngleStart + { + get => _angleStart; + set + { + _angleStart = value; + ChangeShape(); + CalculateVisPoints(); + } + } + + public float AngleEnd + { + get => _angleEnd; + set + { + _angleEnd = value; + ChangeShape(); + CalculateVisPoints(); + } + } + + public int Direction + { + get => _direction; + set + { + _direction = value; + ChangeShape(); + CalculateVisPoints(); + } + } + + public float AngleRange => _totalTh; + + public Vector2 PointStart => _center + new Vector2(_radius * (float)Math.Cos(_angleStart / 180 * Math.PI), + _radius * (float)Math.Sin(_angleStart / 180 * Math.PI)); + + public Vector2 PointEnd => _center + new Vector2(_radius * (float)Math.Cos(_angleEnd / 180 * Math.PI), + _radius * (float)Math.Sin(_angleEnd / 180 * Math.PI)); + + public Vector2 Src => _src; + + public Vector2 Dst => _dst; + + public float TangentSrc => _tangentSrc; + + public float TangentDst => _tangentDst; + + public override (Vector2 Pt, float Angle, float Bias, float Position) QueryTangentPoint(Vector2 point) + { + var queryTh = (float)(Math.Atan2(point.Y - _center.Y, point.X - _center.X) / Math.PI * 180); + + var p = new Vector2(); + var tangent = 0f; + var pd = 0f; + var bestBias = float.MaxValue; + + if ((((int)PaddingType >> 4) & 0x1) == 1) + { + var (bias1, hPnt1, fd1) = CommonMath.Project2DLine(point, _beforeStartSrc, _src); + if (fd1 <= 1000) + { + p = hPnt1; + tangent = (_direction >= 0 ? _angleStart : _angleEnd) + 90 * _direction; + pd = fd1; + bestBias = bias1; + } + } + + if (((int)PaddingType & 0x1) == 1) + { + var (bias2, hPnt2, fd2) = CommonMath.Project2DLine(point, _dst, _afterEndDst); + if (fd2 >= 0 && Math.Abs(bias2) < Math.Abs(bestBias)) + { + p = hPnt2; + tangent = (_direction >= 0 ? _angleEnd : _angleStart) + 90 * _direction; + pd = _totalLen + fd2; + bestBias = bias2; + } + } + + var th1 = _direction == 1 ? CommonMath.ThDiff(queryTh, _angleStart) : CommonMath.ThDiff(_angleEnd, queryTh); + // todo: urgent bug! should use better strategy to prevent sign problem + if (th1 < -55) th1 += 360; + var arcBias = (_radius - Vector2.Distance(point, _center)) * _direction; + if (Math.Abs(arcBias) < Math.Abs(bestBias)) + { + p = _center + _radius * new Vector2((float)Math.Cos(queryTh / 180 * Math.PI), + (float)Math.Sin(queryTh / 180 * Math.PI)); + tangent = queryTh + 90 * _direction; + pd = _radius * th1 / 180 * (float)Math.PI; + bestBias = arcBias; + } + + return (p, tangent, bestBias, pd); + } + + public override float QueryCurvature(float position) + { + // var theta = (float)(_angleEnd - position / _radius / Math.PI * 180f + Math.PI); + // return Vectoriel.FromAngleLen(theta, 1f / _radius); + return 1000f / _radius * _direction; + } + + public override float Length() + { + return _totalLen; + } + + private void CalculateVisPoints() + { + lock (_visPoints) + { + // todo: overlapping start and end is problematic + var ptCnt = (int)Math.Ceiling((_angleEnd + 360 - _angleStart) % 360 / VisAngleResolution); + _visPoints = new Vector2[ptCnt + 2]; + + var starting = _angleStart; + if (_direction == -1) starting = _angleEnd; + _visPoints[0] = _beforeStartSrc; + + for (var j = 0; j < ptCnt; ++j) + { + var th = starting + j * VisAngleResolution * _direction; + var radAngle = (float)(th / 180f * Math.PI); + _visPoints[j + 1] = Center + new Vector2((float)Math.Cos(radAngle), (float)Math.Sin(radAngle)) * Radius; + } + + _visPoints[ptCnt + 1] = _afterEndDst; + } + } + + private void ChangeShape() + { + _totalTh = CommonMath.ThDiff(_angleEnd, _angleStart); + if (_totalTh < 0) _totalTh += 360; + _totalLen = _radius * _totalTh / 180 * (float)Math.PI; + + var radAngleStart = _angleStart / 180 * Math.PI; + var radAngleEnd = _angleEnd / 180 * Math.PI; + double srcAngle = radAngleStart, dstAngle = radAngleEnd; + if (_direction == -1) (srcAngle, dstAngle) = (dstAngle, srcAngle); + _src = _center + new Vector2((float)Math.Cos(srcAngle), (float)Math.Sin(srcAngle)) * _radius; + _dst = _center + new Vector2((float)Math.Cos(dstAngle), (float)Math.Sin(dstAngle)) * _radius; + _beforeStartSrc = CommonMath.Transform2D(_src, + (_direction >= 0 ? _angleStart : _angleEnd) + 90 * _direction, new Vector2(-1000, 0)); + _afterEndDst = CommonMath.Transform2D(_dst, (_direction >= 0 ? _angleEnd : _angleStart) + 90 * _direction, + new Vector2(1000, 0)); + _tangentSrc = QueryTangentPoint(_src).Angle; + _tangentDst = QueryTangentPoint(_dst).Angle; + } + + private Vector2 _center; + private float _radius, _angleStart, _angleEnd; + private int _direction; + + private float _totalTh, _totalLen; + private Vector2 _src, _dst; + private float _tangentSrc, _tangentDst; + private Vector2 _beforeStartSrc, _afterEndDst; + + private Vector2[] _visPoints = Array.Empty(); + } +} diff --git a/CommonUsage-MultiVehicleSync/commonusage/Geometries/NurbsCurve.cs b/CommonUsage-MultiVehicleSync/commonusage/Geometries/NurbsCurve.cs new file mode 100644 index 0000000..aa45227 --- /dev/null +++ b/CommonUsage-MultiVehicleSync/commonusage/Geometries/NurbsCurve.cs @@ -0,0 +1,349 @@ +using CommonUsage.Mathematics; +using System.Collections.Generic; +using System.Numerics; +using System; +using System.Linq; + +namespace CommonUsage.Geometries +{ + public class NurbsCurve : AbstractGeometry + { + + public NurbsCurve(List controlPoints, List weights, List knotVector, int frame = 100) + { + _controlPoints = controlPoints; + _weights = weights; + _knotVector = knotVector; + _frame = frame; + InitializeNurbs(); + } + + public override void Visualize(Action processDot, Action processLine, bool visExtendedPart = false) + { + if (VisualizeOption.DrawAuxiliary) + for (var i = 0; i < _controlPoints.Count - 1; ++i) + { + processLine(new VisLine(_controlPoints[i], _controlPoints[i + 1], + false, false, VisualizeOption.AuxiliaryColor)); + if (i == 0) continue; + processDot(new VisDot(_controlPoints[i], VisualizeOption.AuxiliaryColor)); + } + + for (var i = 0; i < _nurbsPoints.Count - 1; ++i) + { + if (Direction == -1) + { + processLine(new VisLine(_nurbsPoints[i + 1], _nurbsPoints[i], + false, i == (int)(_nurbsPoints.Count / 2), VisualizeOption.MainColor, 2)); + } + else + { + processLine(new VisLine(_nurbsPoints[i], _nurbsPoints[i + 1], + false, i == (int)(_nurbsPoints.Count / 2), VisualizeOption.MainColor, 2)); + } + } + } + public (Vector2 Point, int Id) QueryPoint(Vector2 point) + { + var p = new Vector2(); + var id = -1; + var bestDistance = float.MaxValue; + + var hashes = _bias.Select(bb => CalculateHash(point, 100, bb.X, bb.Y)).ToList(); + + void TryQuery(Dictionary> dict, List hashList) + { + foreach (var hash in hashList) + { + if (!dict.TryGetValue(hash, out var ll)) continue; + foreach (var (q, qId) in ll) + { + var d = Vector2.Distance(q, point); + if (d < bestDistance) + { + p = q; + id = qId; + bestDistance = d; + } + } + } + } + + if (id == -1) + { + // todo: improve the way to find closest point if mappings fail + (p, id) = _nurbsPoints.Select((p, i) => (p, i)) + .OrderBy(pair => CommonMath.dist(pair.p.X, pair.p.Y, point.X, point.Y)).First(); + } + + return (p, id); + } + + private uint CalculateHash(Vector2 point, float gridSize, int xBias = 0, int yBias = 0) + { + return (uint)(((int)((point.X - _minX) / gridSize) + xBias) << 16 + (((int)((point.Y - _minY) / gridSize) + yBias) & 0xffff)); + } + + private readonly List<(int X, int Y)> _bias = new() + { + new(-1, -1), new(-1, 0), new(-1, 1), + new(0, -1), new(0, 0), new(0, 1), + new(1, -1), new(1, 0), new(1, 1), + }; + + public override (Vector2 Pt, float Angle, float Bias, float Position) QueryTangentPoint(Vector2 point) + { + var (p, id) = QueryPoint(point); + var tangent = _tangents[id]; + var (bias, lp, fd) = CommonMath.Project2DLine(point, p, tangent); + var next = fd > 0 ? id + 1 : id - 1; + if (next > 0 && next < _tangents.Count)//线性插值 + { + var (_, _, t) = CommonMath.Project2DLine(point, _nurbsPoints[id], _nurbsPoints[next]); + var partial = t / Vector2.Distance(_nurbsPoints[id], _nurbsPoints[next]); + if (partial >= 0 && partial <= 1) + { + tangent = CommonMath.RoundTh(_tangents[id] + + partial * CommonMath.RoundTh(_tangents[next] - _tangents[id])); + if (CommonMath.RoundTh(_tangents[next] - _tangents[id]) > 5) + Console.WriteLine($"Nurbs tangents bug, tanget: {id}:{_tangents[id]} {next}:{_tangents[next]}"); + } + else Console.WriteLine("Nurbs tangents bug"); + } + return (lp, tangent, bias, fd + _sumDistances[id]); + } + + public override float QueryCurvature(float position) + { + int id = _sumDistances.Count - 1; + if (position <= 0) id = 0; + else + { + for (int i = 1; i < _sumDistances.Count; i++) + { + if (position > _sumDistances[i - 1] && position <= _sumDistances[i]) + { + id = i; + break; + } + } + } + + var result = _curvatures[id]; + if (id > 0 && id < _sumDistances.Count - 1)//插值 + { + var partial = (position - _sumDistances[id - 1]) / (_sumDistances[id] - _sumDistances[id - 1]); + if (partial >= 0 && partial <= 1) result = (1 - partial) * _curvatures[id - 1] + partial * _curvatures[id]; + else Console.WriteLine("Nurbs curvature bug"); + } + + return result; + } + + public Vector3 QueryNurbsPointsById(int id) + { + if (id < 0 || id > Frame) + { + Console.WriteLine($"QueryBezierPointsById out of range, Resolution:{Frame},id:{id}."); + return new Vector3(0, 0, 0); + } + + return new Vector3(_nurbsPoints[id].X, _nurbsPoints[id].Y, _tangents[id]); + } + public override float Length() + { + return _length; + } + + public int Order => _order; + public List ControlPoints => _controlPoints; + public List Weights => _weights; + public List KnotVector => _knotVector; + public int Frame => _frame; + + public int Direction = 1; + + + public void UpdateControlPoint(int id, Vector2 point) + { + _controlPoints[id] = point; + InitializeNurbs(); + } + public void UpdateNurbsWeihgts(int id, float weight) + { + _weights[id] = weight; + InitializeNurbs(); + + } + public void AddControlPoint(int id, Vector2 point) + { + _controlPoints.Insert(id, point); + InitializeNurbs(); + } + + public void RemoveControlPoint(int id) + { + _controlPoints.RemoveAt(id); + InitializeNurbs(); + } + + public Vector2 GetMidPoint() + { + return _nurbsPoints[(int)Math.Ceiling(_frame / 2d)]; + } + + private void InitializeNurbs() + { + _order = _controlPoints.Count - 1; + List> allpoints = new List>(); + List nurbsCurvePoints = new List(); + float delta = 1.0f / Frame; + + for (float t = 0; t <= 1; t += delta) + { + var (point, tangent) = DeBoorAlgorithm(t); + var points = new List + { + point, + point + tangent // Tangent endpoint + }; + allpoints.Add(points); + nurbsCurvePoints.Add(point); // Store the curve point separately + } + + _nurbsPoints = nurbsCurvePoints; + _tangents = Enumerable.Repeat(0f, _nurbsPoints.Count).ToList(); + _curvatures = Enumerable.Repeat(0f, _nurbsPoints.Count).ToList(); + + for (var id = 0; id < _nurbsPoints.Count - 1; ++id) + { + Vector2 p1 = _nurbsPoints[id]; + Vector2 p2 = _nurbsPoints[id + 1]; + + float tangentAngle = (float)Math.Atan2(p2.Y - p1.Y, p2.X - p1.X) * 180 / (float)Math.PI; + _tangents[id] = tangentAngle; + + // Calculate curvature using finite differences of tangent (second derivative approximation) + if (id > 0) + { + float previousTangent = _tangents[id - 1]; + float curvature = (float)(CommonMath.ThDiff(tangentAngle, previousTangent) * Math.PI / 180) / + (Vector2.Distance(p1, p2) / 1000); + _curvatures[id] = curvature; + } + } + + _curvatures.Insert(0, _curvatures[0]); + for (var id = 1; id < _curvatures.Count - 1; ++id) + { + _curvatures[id] = (_curvatures[id] + _curvatures[id + 1]) / 2; + } + + _remainDistances = Enumerable.Repeat(0f, _nurbsPoints.Count).ToList(); + _sumDistances = Enumerable.Repeat(0f, _nurbsPoints.Count).ToList(); + _remainDistances[_nurbsPoints.Count - 1] = 0; + + for (var i = _nurbsPoints.Count - 2; i >= 0; --i) + { + _remainDistances[i] = _remainDistances[i + 1] + Vector2.Distance(_nurbsPoints[i], _nurbsPoints[i + 1]); + } + + _sumDistances[0] = 0; + for (var i = 1; i < _nurbsPoints.Count; ++i) + { + _sumDistances[i] = _sumDistances[i - 1] + Vector2.Distance(_nurbsPoints[i], _nurbsPoints[i - 1]); + } + + _length = _sumDistances.Last(); + + _minX = _nurbsPoints.Min(pp => pp.X); + _minY = _nurbsPoints.Min(pp => pp.Y); + + var tmpList = _nurbsPoints.Select((point, index) => (point, index)).ToList(); + // GenerateGridMapping(ref _pointsMappingSmall, 100, tmpList); + // GenerateGridMapping(ref _pointsMappingBig, 1000, tmpList); + } + + + + + private float CalculateLength() + { + return _nurbsPoints.Zip(_nurbsPoints.Skip(1), Vector2.Distance).Sum(); + } + + private (Vector2, Vector2) DeBoorAlgorithm(float t) + { + Vector2 numerator = Vector2.Zero; + Vector2 tangentNumerator = Vector2.Zero; + float denominator = 0f; + + // Calculate the point on the curve + for (int i = 0; i < ControlPoints.Count; ++i) + { + float basis = BasisFunction(i, _order, t) * Weights[i]; + numerator += basis * ControlPoints[i]; + denominator += basis; + } + + Vector2 point = numerator / denominator; + + // Calculate the tangent vector using the analytical derivative + for (int i = 0; i < ControlPoints.Count; ++i) + { + float basisDerivative = BasisFunctionDerivative(i, _order, t) * Weights[i]; + tangentNumerator += basisDerivative * ControlPoints[i]; + } + Vector2 tangent = tangentNumerator / denominator; + + return (point, tangent); + } + + private float BasisFunction(int i, int p, float t) + { + if (p == 0) + return (KnotVector[i] <= t && t < KnotVector[i + 1]) ? 1.0f : 0.0f; + + float denom1 = KnotVector[i + p] - KnotVector[i]; + float term1 = denom1 == 0 ? 0 : ((t - KnotVector[i]) / denom1) * BasisFunction(i, p - 1, t); + + float denom2 = KnotVector[i + p + 1] - KnotVector[i + 1]; + float term2 = denom2 == 0 ? 0 : ((KnotVector[i + p + 1] - t) / denom2) * BasisFunction(i + 1, p - 1, t); + + return term1 + term2; + } + + private float BasisFunctionDerivative(int i, int k, float t) + { + if (k == 0) return 0; + + float denom1 = KnotVector[i + k] - KnotVector[i]; + float denom2 = KnotVector[i + k + 1] - KnotVector[i + 1]; + + float term1 = denom1 != 0 ? BasisFunction(i, k - 1, t) / denom1 : 0; + float term2 = denom1 != 0 ? (t - KnotVector[i]) * BasisFunctionDerivative(i, k - 1, t) / denom1 : 0; + + float term3 = denom2 != 0 ? -BasisFunction(i + 1, k - 1, t) / denom2 : 0; + float term4 = denom2 != 0 ? (KnotVector[i + k + 1] - t) * BasisFunctionDerivative(i + 1, k - 1, t) / denom2 : 0; + + return term1 + term2 + term3 + term4; + } + + private int _order; + private List _controlPoints; + private List _weights; + private List _knotVector; + private int _frame; + private List _nurbsPoints; + private List>> _tangentPoints; + private List _curvatures; + private List _sumDistances; + private List _remainDistances; + private float _minX, _minY; + private float _length; + private Dictionary> _pointsMappingSmall; + private Dictionary> _pointsMappingBig; + private List _tangents; + + } +} diff --git a/CommonUsage-MultiVehicleSync/commonusage/Geometries/Vectoriel.cs b/CommonUsage-MultiVehicleSync/commonusage/Geometries/Vectoriel.cs new file mode 100644 index 0000000..86b72a9 --- /dev/null +++ b/CommonUsage-MultiVehicleSync/commonusage/Geometries/Vectoriel.cs @@ -0,0 +1,71 @@ +using System; +using System.Collections.Generic; +using System.Numerics; +using System.Text; + +namespace CommonUsage.Geometries +{ + /// + /// MDCS数学类：向量。 + /// + public class Vectoriel + { + public Vectoriel() + { + _vec2 = Vector2.Zero; + _dir2 = Vector2.Normalize(_vec2); + _len = _vec2.Length(); + _angle = (float)(Math.Atan2(_vec2.Y, _vec2.X) / Math.PI * 180f); + } + + public Vectoriel(Vector2 vec) + { + _vec2 = vec; + _dir2 = Vector2.Normalize(_vec2); + _len = _vec2.Length(); + _angle = (float)(Math.Atan2(_vec2.Y, _vec2.X) / Math.PI * 180f); + } + + /// + /// 通过笛卡尔坐标系X和Y值构建向量。 + /// + /// + /// + /// + public static Vectoriel FromXY(float x, float y) + { + return new Vectoriel(new Vector2(x, y)); + } + + /// + /// 通过极坐标系的角度和距离值构建向量。 + /// + /// + /// + /// + public static Vectoriel FromAngleLen(float angle, float len) + { + var rad = angle / 180f * Math.PI; + return new Vectoriel(new Vector2((float)Math.Cos(rad), (float)Math.Sin(rad)) * len); + } + + public static implicit operator Vector2(Vectoriel vec) + { + return vec._vec2; + } + + public static explicit operator Vectoriel(Vector2 vec) + { + return FromXY(vec.X, vec.Y); + } + + public Vector2 Direction => _dir2; + + public float Length => _len; + + public float Angle => _angle; + + private Vector2 _vec2, _dir2; + private float _len, _angle; + } +} diff --git a/CommonUsage-MultiVehicleSync/commonusage/Mathematics/CommonMath.cs b/CommonUsage-MultiVehicleSync/commonusage/Mathematics/CommonMath.cs new file mode 100644 index 0000000..df49d5b --- /dev/null +++ b/CommonUsage-MultiVehicleSync/commonusage/Mathematics/CommonMath.cs @@ -0,0 +1,774 @@ +using System; +using System.Collections.Generic; +using System.Drawing; +using System.Linq; +using System.Numerics; +using System.Reflection; +using System.Runtime.CompilerServices; +using System.Runtime.InteropServices; + +namespace CommonUsage.Mathematics +{ + + using T3 = Tuple; + using D3 = Tuple; + + public class CommonMath + { + public class PrimeEnumerator + { + public PrimeEnumerator(List items, Func process) + { + _n = items.Count; + _items = items; + _process = process; + + foreach (var pNum in _primes) + { + if (_n % pNum != 0) + { + _a = pNum; + _b = 11; + break; + } + } + } + + public void Enumerate() + { + using (var enumerator = Get().GetEnumerator()) + { + while (enumerator.MoveNext()) { } + } + } + + private readonly int _n, _a, _b; + private readonly int[] _primes = new[] { 29, 23, 19, 17, 13 }; + private List _items; + private readonly Func _process; + + private IEnumerable Get() + { + for (var i = 0; i < _n; ++i) + { + var id = (i * _a + _b) % _n; + yield return _process(_items[id]); + } + } + } + + private static IEnumerable> GetPermutationsInternal(IEnumerable list, int length) + { + if (length == 1) return list.Select(t => new T[] { t }); + + return GetPermutationsInternal(list, length - 1) + .SelectMany(t => list.Where(e => !t.Contains(e)), + (t1, t2) => t1.Concat(new T[] { t2 })); + } + + /// + /// 得到一组数据的所有排列。 + /// + /// 元素数据类型 + /// 所有待选元素 + /// 所选出的元素数量 + /// + public static List> GetPermutations(List list, int selectNum) + { + return GetPermutationsInternal(list, selectNum).Select(ll => ll.ToList()).ToList(); + } + + public static (float bias, Vector2 hPnt, float d) Project2DLine(Vector2 pnt, Vector2 segSt, + Vector2 segEnd) + { + var dir = Vector2.Normalize(segEnd - segSt); + var fd = Vector2.Dot(pnt - segSt, dir); + var hPnt = segSt + fd * dir; + var bias = dir.X * (pnt.Y-segSt.Y) - (pnt.X-segSt.X) * dir.Y; + return (bias, hPnt, fd); + } + + public static (float bias, Vector2 hPnt, float fd) Project2DLine(Vector2 pnt, Vector2 segSt, float tangent) + { + var dir = new Vector2((float)System.Math.Cos(tangent / 180 * System.Math.PI), (float)System.Math.Sin(tangent / 180 * System.Math.PI)); + var fd = Vector2.Dot(pnt - segSt, dir); + var hPnt = segSt + fd * dir; + var bias = dir.X * (pnt.Y - segSt.Y) - (pnt.X - segSt.X) * dir.Y; + return (bias, hPnt, fd); + } + + public class LineEqu + { + public double A, B, C, ln, dAB; + public double px1, px2, py1, py2; + public float midX; + public float midY; + } + // Fit line with PCA. + public LineEqu CalcLine(IEnumerable tls) + { + var lidarPoint2Ds = tls as Vector2[] ?? tls.ToArray(); + float fx = lidarPoint2Ds.Average(f => f.X); + float fy = lidarPoint2Ds.Average(f => f.Y); + float fxx = lidarPoint2Ds.Average(f => f.X * f.X); + float fxy = lidarPoint2Ds.Average(f => f.X * f.Y); + float fyy = lidarPoint2Ds.Average(f => f.Y * f.Y); + float a = fxx - fx * fx, b = fxy - fx * fy, c = fyy - fy * fy; + double sqt = System.Math.Sqrt((a - c) * (a - c) + 4 * b * b); + double l1 = a + c + sqt; + double l2 = a + c - sqt; + double dx, dy; + if (System.Math.Abs(a - l1 / 2) > System.Math.Abs(c - l1 / 2)) + { + dy = l1 / 2 - a; dx = b; + } + else + { + dx = l1 / 2 - c; dy = b; + } + double norm = System.Math.Sqrt(dx * dx + dy * dy); + dx /= norm; dy /= norm; + double A = dy, B = -dx, C = dx * fy - dy * fx; + double dAB = System.Math.Sqrt(A * A + B * B); + + return new CommonMath.LineEqu + { + A = A, + B = B, + C = C, + ln = lidarPoint2Ds.Average(p => System.Math.Abs(p.X * A + p.Y * B + C) / dAB), + midX = fx, + midY = fy + }; + } + + public static double QuadInterp3(double[] confsF) + { + if (confsF[0] > confsF[1] && confsF[0] > confsF[2]) + { + //printf("left overflow...\n"); + return -1; + } + + if (confsF[1] > confsF[0] && confsF[1] > confsF[2]) + { + return (-(confsF[2] - confsF[0]) / 2.0f / (confsF[0] + confsF[2] - 2.0f * confsF[1] + 0.0001f)); + } + if (confsF[2] > confsF[0] && confsF[2] > confsF[1]) + { + //printf("right overflow...\n"); + return 1; + } + return 0; + } + + public static double cross(PointF O, PointF A, PointF B) + { + return (A.X - O.X) * (B.Y - O.Y) - (A.Y - O.Y) * (B.X - O.X); + } + + public static List GetConvexHull(List points) + { + if (points == null) + return null; + + if (points.Count() <= 1) + return points; + + int n = points.Count(), k = 0; + List H = new List(new PointF[2 * n]); + + points.Sort((a, b) => + a.X == b.X ? a.Y.CompareTo(b.Y) : a.X.CompareTo(b.X)); + + // Build lower hull + for (int i = 0; i < n; ++i) + { + while (k >= 2 && cross(H[k - 2], H[k - 1], points[i]) <= 0) + k--; + H[k++] = points[i]; + } + + // Build upper hull + for (int i = n - 2, t = k + 1; i >= 0; i--) + { + while (k >= t && cross(H[k - 2], H[k - 1], points[i]) <= 0) + k--; + H[k++] = points[i]; + } + + return H.Take(k - 1).ToList(); + } + + public static bool IsPointInPolygon4(PointF[] polygon, PointF testPoint) + { + // ray casting odd even test. + bool result = false; + int j = polygon.Count() - 1; + for (int i = 0; i < polygon.Count(); i++) + { + if (polygon[i].Y < testPoint.Y && polygon[j].Y >= testPoint.Y || + polygon[j].Y < testPoint.Y && polygon[i].Y >= testPoint.Y) + { + if (polygon[i].X + (testPoint.Y - polygon[i].Y) / (polygon[j].Y - polygon[i].Y) * + (polygon[j].X - polygon[i].X) < testPoint.X) + { + result = !result; + } + } + + j = i; + } + + return result; + } + + public static double Exp(double val) + { + if (val < -20) return 0.0000001; + if (val > 20) return 99999999999999; + long tmp = (long)(1512775 * val + 1072632447); + return BitConverter.Int64BitsToDouble(tmp << 32); + } + public static double gaussmf(double x, double sig, double c) + { + return Exp(-(x - c) * (x - c) / (2 * sig * sig)); + } + + public static float Exp(float x) + { + if (x < -10) return 0; + if (x > 10) return 99999999999999; + x = 1.0f + x / 64f; + x *= x; + x *= x; + x *= x; + x *= x; + x *= x; + x *= x; + return x; + } + public static float gaussmf(float x, float sig, float c) + { + return Exp(-(x - c) * (x - c) / (2 * sig * sig)); + } + + public static D3 Transform2D(D3 src, D3 t) + { + var rth = src.Item3 / 180.0 * System.Math.PI; + var p1dtx = (src.Item1 + System.Math.Cos(rth) * t.Item1 - + System.Math.Sin(rth) * t.Item2); + var p1dty = (src.Item2 + System.Math.Sin(rth) * t.Item1 + + System.Math.Cos(rth) * t.Item2); + var p1dtth = src.Item3 + t.Item3; + return Tuple.Create(p1dtx, p1dty, p1dtth); + } + + public struct LngLatToXY + { + public double scale; + public double rad; + public double biasX, biasY; + } + + public static LngLatToXY GetTransformLngLatToXY(Vector2 lnglat1, Vector2 xy1, Vector2 lnglat2, Vector2 xy2) + { + var scale = (xy1 - xy2).Length() / (lnglat1 - lnglat2).Length(); + var dxy = (xy1 - xy2); + var dlnglat = lnglat1 - lnglat2; + var rad = System.Math.Atan2(dxy.X, dxy.Y) - System.Math.Atan2(dlnglat.X, dlnglat.Y); + var intm = lnglat1 * scale; + var biasX = xy1.X - (intm.X * System.Math.Cos(rad) - intm.Y * System.Math.Sin(rad)); + var biasY = xy1.Y - (intm.X * System.Math.Sin(rad) + intm.Y * System.Math.Cos(rad)); + return new LngLatToXY {rad = rad, biasX = biasX, biasY = biasY, scale = scale}; + } + + public Vector2 TransformLngLatToXY(Vector2 lnglat, LngLatToXY t) + { + var intm = lnglat * (float) t.scale; + return new Vector2((float) (intm.X * System.Math.Cos(t.rad) - intm.Y * System.Math.Sin(t.rad) + t.biasX), + (float) (intm.X * System.Math.Sin(t.rad) + intm.Y * System.Math.Cos(t.rad) + t.biasY)); + } + + public static D3 ReverseTransform(D3 dest, D3 t) + { + var rth = (dest.Item3 - t.Item3) / 180.0 * System.Math.PI; + var nxT = (dest.Item1 - System.Math.Cos(rth) * t.Item1 + + System.Math.Sin(rth) * t.Item2); + var nyT = (dest.Item2 - System.Math.Sin(rth) * t.Item1 - + System.Math.Cos(rth) * t.Item2); + var pth = dest.Item3 - t.Item3; + return Tuple.Create(nxT, nyT, pth); + } + + public static D3 SolveTransform2D(D3 src, D3 dest) + { + var th = dest.Item3 - src.Item3; + th = (th - System.Math.Round((th) / 360.0f) * 360); + var rth = src.Item3 / 180.0 * System.Math.PI; + var x = ((dest.Item1 - src.Item1) * System.Math.Cos(rth) + + (dest.Item2 - src.Item2) * System.Math.Sin(rth)); + var y = (-(dest.Item1 - src.Item1) * System.Math.Sin(rth) + + (dest.Item2 - src.Item2) * System.Math.Cos(rth)); + return Tuple.Create(x, y, th); + } + + public static T3 Transform2D(T3 src, T3 t) + { + var rth = src.Item3 / 180.0 * System.Math.PI; + var p1dtx = (float)(src.Item1 + System.Math.Cos(rth) * t.Item1 - + System.Math.Sin(rth) * t.Item2); + var p1dty = (float)(src.Item2 + System.Math.Sin(rth) * t.Item1 + + System.Math.Cos(rth) * t.Item2); + var p1dtth = src.Item3 + t.Item3; + return Tuple.Create(p1dtx, p1dty, p1dtth); + } + + public static Vector2 Transform2D(Vector3 src, Vector3 t) + { + var tup = Transform2D(Tuple.Create(src.X, src.Y, src.Z), Tuple.Create(t.X, t.Y, t.Z)); + return new Vector2(tup.Item1, tup.Item2); + } + + public static Vector2 Transform2D(Vector2 srcPos, float srcTh, Vector2 dt, float dth = 0) + { + var tup = Transform2D(Tuple.Create(srcPos.X, srcPos.Y, srcTh), Tuple.Create(dt.X, dt.Y, dth)); + return new Vector2(tup.Item1, tup.Item2); + } + + public static T3 ReverseTransform(T3 dest, T3 t) + { + var rth = (dest.Item3 - t.Item3) / 180.0 * System.Math.PI; + var nxT = (float)(dest.Item1 - System.Math.Cos(rth) * t.Item1 + + System.Math.Sin(rth) * t.Item2); + var nyT = (float)(dest.Item2 - System.Math.Sin(rth) * t.Item1 - + System.Math.Cos(rth) * t.Item2); + var pth = dest.Item3 - t.Item3; + return Tuple.Create(nxT, nyT, pth); + } + + public static T3 SolveTransform2D(T3 src, T3 dest) + { + var th = dest.Item3 - src.Item3; + th = (float)(th - System.Math.Round((th) / 360.0f) * 360); + var rth = src.Item3 / 180.0 * System.Math.PI; + var x = (float)((dest.Item1 - src.Item1) * System.Math.Cos(rth) + + (dest.Item2 - src.Item2) * System.Math.Sin(rth)); + var y = (float)(-(dest.Item1 - src.Item1) * System.Math.Sin(rth) + + (dest.Item2 - src.Item2) * System.Math.Cos(rth)); + return Tuple.Create(x, y, th); + } + + public static Vector2 SolveTransform2D(Vector2 srcPos, float srcTh, Vector2 dt, float dth = 0) + { + var tup = SolveTransform2D(Tuple.Create(srcPos.X, srcPos.Y, srcTh), Tuple.Create(dt.X, dt.Y, dth)); + return new Vector2(tup.Item1, tup.Item2); + } + + public static double dist(double x1, double y1, double x2, double y2) + { + return System.Math.Sqrt((x1 - x2) * (x1 - x2) + (y1 - y2) * (y1 - y2)); + } + + [StructLayout(LayoutKind.Explicit)] + private struct FloatIntUnion + { + [FieldOffset(0)] public float f; + + [FieldOffset(0)] public int tmp; + } + + public static float Sqrt(float z) + { + FloatIntUnion u; + u.tmp = 0; + u.f = z; + u.tmp -= 1 << 23; /* Subtract 2^m. */ + u.tmp >>= 1; /* Divide by 2. */ + u.tmp += 1 << 29; /* Add ((b + 1) / 2) * 2^m. */ + return u.f; + } + + public static float dist2(float x1, float y1, float x2, float y2) + { + return ((x1 - x2) * (x1 - x2) + (y1 - y2) * (y1 - y2)); + } + + + public static float d2(float x1, float y1, float x2, float y2) + { + return (x1 - x2) * (x1 - x2) + (y1 - y2) * (y1 - y2); + } + + public static float ThAverage(List angles) + { + var anchor = angles[0]; + var diff = 0f; + foreach (var angle in angles) + diff += ThDiff(angle, anchor); + return RoundTh(anchor + diff / angles.Count); + } + + public static float ThDiff(float th1, float th2) + { + return (float)(th1 - th2 - + System.Math.Round((th1 - th2) / 360.0f) * 360); + } + + public static double ThDiff(double th1, double th2) + { + return th1 - th2 - + System.Math.Round((th1 - th2) / 360.0f) * 360; + } + + public static double refine(double x) + { + if (x < 1 && x > -1) return x; + if (x > 1) + return (2 / (1 + System.Math.Exp(-((x - 1) * 2)))); + return (2 / (1 + System.Math.Exp(-((x + 1) * 2)))) - 2; + } + + /// + /// 求点p到两点式直线p1p2的距离 + /// + /// 点p的x坐标 + /// 点p的y坐标 + /// 直线点p1的x坐标 + /// 直线点p1的y坐标 + /// 直线点p2的x坐标 + /// 直线点p2的y坐标 + /// + public static double Point2LineDist(double x, double y, double x1, double y1, double x2, double y2) + { + double a1 = -(y1 - y2) / 10; + double b1 = (x1 - x2) / 10; + double c1 = (x1 * (y1 - y2) - y1 * (x1 - x2)) / 10; + return System.Math.Abs(a1 * x + b1 * y + c1) / System.Math.Sqrt(a1 * a1 + b1 * b1); + } + + public static double Point2LineDist(Vector2 p, LineSegment ll) + { + double a1 = -(ll.Src.Y - ll.Dst.Y) / 10; + double b1 = (ll.Src.X - ll.Dst.X) / 10; + double c1 = (ll.Src.X * (ll.Src.Y - ll.Dst.Y) - ll.Src.Y * (ll.Src.X - ll.Dst.X)) / 10; + return System.Math.Abs(a1 * p.X + b1 * p.Y + c1) / System.Math.Sqrt(a1 * a1 + b1 * b1); + } + + /// + /// 两条两点式直线间的夹角 + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// 角度制 + public static double AngleBetweenLines(double x1, double y1, double x2, double y2, double x3, double y3, + double x4, double y4) + { + var vec1 = new Vector2((float)(x2 - x1), (float)(y2 - y1)); + var vec2 = new Vector2((float)(x4 - x3), (float)(y4 - y3)); + return System.Math.Acos(System.Math.Abs(Vector2.Dot(vec1, vec2) / vec1.Length() / vec2.Length())) / System.Math.PI * 180; + } + + public static double AngleBetweenLines(LineSegment ls1, LineSegment ls2) + { + return AngleBetweenLines(ls1.Src.X, ls1.Src.Y, ls1.Dst.X, ls1.Dst.Y, ls2.Src.X, ls2.Src.Y, ls2.Dst.X, + ls2.Dst.Y); + } + + /// + /// 两向量间夹角 + /// + /// + /// + /// + /// + /// + /// + /// + /// + /// 角度制 + public static double AngleBetweenVectors(double x1, double y1, double x2, double y2, double x3, double y3, + double x4, double y4) + { + var vec1 = new Vector2((float)(x2 - x1), (float)(y2 - y1)); + var vec2 = new Vector2((float)(x4 - x3), (float)(y4 - y3)); + return System.Math.Acos(Vector2.Dot(vec1, vec2) / vec1.Length() / vec2.Length()) / System.Math.PI * 180; + } + + /// + /// 两向量间夹角. + /// + /// + /// + /// 角度制 + public static double AngleBetweenVectors(Vector2 vec1, Vector2 vec2) + { + return System.Math.Acos(Vector2.Dot(vec1, vec2) / vec1.Length() / vec2.Length()) / System.Math.PI * 180; + } + + public static double AngleBetweenVectors(Vector3 vector1, Vector3 vector2) + { + float dotProduct = Vector3.Dot(vector1, vector2); + float magnitude1 = vector1.Length(); + float magnitude2 = vector2.Length(); + float cosine = dotProduct / (magnitude1 * magnitude2); + + return System.Math.Acos(cosine) / System.Math.PI * 180; + } + + /// + /// 求点到直线的垂足 + /// + /// + /// + /// + /// + /// + /// + /// + public static (double, double) PerpendicularPoint(double x, double y, double x1, double y1, double x2, double y2) + { + double lx = x2 - x1, ly = y2 - y1, dAB = lx * lx + ly * ly; + var u = ((x - x1) * lx + (y - y1) * ly) / dAB; + return new(x1 + u * lx, y1 + u * ly); + } + + public static Vector2 PerpendicularPoint(Vector2 p, LineSegment ls) + { + double lx = ls.Dst.X - ls.Src.X, ly = ls.Dst.Y - ls.Src.Y, dAB = lx * lx + ly * ly; + var u = ((p.X - ls.Src.X) * lx + (p.Y - ls.Src.Y) * ly) / dAB; + return new Vector2((float)(ls.Src.X + u * lx), (float)(ls.Src.Y + u * ly)); + } + + public static double PerpendicularPosition(double x, double y, double x1, double y1, double x2, double y2) + { + double lx = x2 - x1, ly = y2 - y1; + var dAB = CommonMath.Sqrt((float)(lx * lx + ly * ly)); + lx /= dAB; + ly /= dAB; + return (x - x1) * lx + (y - y1) * ly; + } + + + /// + /// 最小二乘法拟合直线，得到两点式。 + /// + /// 待拟合的点集，应至少有2个点。 + /// 检查是否所有点距直线的距离均小于maxDist2Line，若为-1则不检查。 + /// 返回两点式的两个端点坐标。若坐标为全0，则拟合失败。 + public static (bool, Vector2, Vector2) FitLineSegment(List pts, double maxDist2Line = -1) + { + if (pts.Count < 2) + { + Console.WriteLine($"Points too Few! {pts.Count}! Cannot perform line fitting!", + MethodBase.GetCurrentMethod()?.Name ?? "FitLine"); + return (false, Vector2.Zero, Vector2.Zero); + }; + + // y = kx + b + double A = 0, B = 0, C = 0, D = 0; + foreach (var p in pts) + { + A += p.X * p.X; + B += p.X; + C += p.X * p.Y; + D += p.Y; + } + + var tmp = A * pts.Count - B * B; + var k = (C * pts.Count - B * D) / tmp; + var b = (A * D - C * B) / tmp; + + double x1 = 0, + y1 = k * x1 + b, + x2 = 1000, + y2 = k * x2 + b; + + double CalcDist(ref bool fail, ref Vector2 endP, ref Vector2 endQ) + { + double distSum = 0; + double lx = x2 - x1, ly = y2 - y1, dAB = lx * lx + ly * ly; + double minU = double.MaxValue, maxU = double.MinValue; + + foreach (var p in pts) + { + var u = ((p.X - x1) * lx + (p.Y - y1) * ly) / dAB; + var perp = new Vector2((float)(x1 + u * lx), (float)(y1 + u * ly)); + if (u < minU) + { + endP = perp; + minU = u; + } + if (u > maxU) + { + endQ = perp; + maxU = u; + } + + var curDist = dist(perp.X, perp.Y, p.X, p.Y); + if (maxDist2Line > -1 && curDist > maxDist2Line) fail = true; + distSum += curDist; + } + + return distSum; + } + + var kbFail = false; + Vector2 endP1 = new Vector2(), endQ1 = new Vector2(); + double kbDist = CalcDist(ref kbFail, ref endP1, ref endQ1); + + // x = my + n + A = 0; + B = 0; + C = 0; + D = 0; + foreach (var p in pts) + { + A += p.X * p.Y; + B += p.Y * p.Y; + C += p.Y; + D += p.X; + } + + tmp = C * C - B * pts.Count; + var m = (C * D - A * pts.Count) / tmp; + var n = (A * C - B * D) / tmp; + y1 = 0; + x1 = m * y1 + n; + y2 = 1000; + x2 = m * y2 + n; + + var mnFail = false; + Vector2 endP2 = new Vector2(), endQ2 = new Vector2(); + double mnDist = CalcDist(ref mnFail, ref endP2, ref endQ2); + + Vector2 endP = endP1, endQ = endQ1; + if (mnDist < kbDist) + { + if (mnFail) return (false, new Vector2(), new Vector2()); + endP = endP2; + endQ = endQ2; + } + else if (kbFail) return (false, new Vector2(), new Vector2()); + + return (true, endP, endQ); + } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static int toId(int x, int y, int z) + { + return (x * 1140671485 + 12820163) ^ (y * 134775813 + 1) ^ (z * 1103515245 + 12345); + } + + public class Clustering + { + public int numIteration = 3; + public int itemNumThreshold = 10; + + public Func inRange; + public Func, T> average; + private readonly List _inputData; + private Dictionary> _clusters = new Dictionary>(); + + public Clustering(List data, Func inRange, Func, T> average) + { + _inputData = data; + this.inRange = inRange; + this.average = average; + } + + public Dictionary> GetClusters() + { + var tmp = new List<(T center, List items)>(); + + for (var iter = 0; iter < numIteration; iter++) + { + tmp = tmp.Where(cluster => cluster.items.Count > itemNumThreshold) + .Select(cluster => (average(cluster.items), new List())).ToList(); + + foreach (var data in _inputData) + { + var added = false; + foreach (var cluster in tmp) + { + if (inRange(cluster.center, data)) + { + cluster.items.Add(data); + added = true; + break; + } + } + + if (!added) + tmp.Add((data, new List() { data })); + } + } + + _clusters = tmp.Where(cluster => cluster.items.Count > itemNumThreshold) + .ToDictionary(cluster => cluster.center, cluster => cluster.items); + return _clusters; + } + } + + public static (bool, Vector2) TwoLinesIntersection(Vector2 A, Vector2 B, Vector2 C, Vector2 D) + { + // Line AB represented as a1x + b1y = c1 + double a1 = B.Y - A.Y; + double b1 = A.X - B.X; + double c1 = a1 * (A.X) + b1 * (A.Y); + + // Line CD represented as a2x + b2y = c2 + double a2 = D.Y - C.Y; + double b2 = C.X - D.X; + double c2 = a2 * (C.X) + b2 * (C.Y); + + double determinant = a1 * b2 - a2 * b1; + + if (determinant == 0) + { + // The lines are parallel. This is simplified + // by returning a pair of FLT_MAX + return new(false, new Vector2()); + } + else + { + double x = (b2 * c1 - b1 * c2) / determinant; + double y = (a1 * c2 - a2 * c1) / determinant; + return (true, new Vector2((float)x, (float)y)); + } + } + + public static bool IsAtLeft(Vector2 anchor, Vector2 dest, Vector2 p) + { + var v1 = new Vector3(anchor - p, 0); + var v2 = new Vector3(dest - p, 0); + return Vector3.Cross(v1, v2).Z > 0; + } + + /// + /// 将角度转化至-180到180度的范围内。 + /// + /// + /// + public static double RoundTh(double th) + { + return th - System.Math.Round(th / 360) * 360; + } + + /// + /// 将角度转化至-180到180度的范围内。 + /// + /// + /// + public static float RoundTh(float th) + { + return th - (float)System.Math.Round(th / 360f) * 360f; + } + } +} diff --git a/CommonUsage-MultiVehicleSync/commonusage/Mathematics/LineSegment.cs b/CommonUsage-MultiVehicleSync/commonusage/Mathematics/LineSegment.cs new file mode 100644 index 0000000..87e0405 --- /dev/null +++ b/CommonUsage-MultiVehicleSync/commonusage/Mathematics/LineSegment.cs @@ -0,0 +1,161 @@ +using System; +using System.Numerics; + +namespace CommonUsage.Mathematics +{ + /// + /// 表示一条线段。 + /// + public class LineSegment + { + /// + /// 默认构造函数。所有坐标初始化为0。 + /// + public LineSegment() + { + + } + + /// + /// 使用两个端点初始化一段2D线段。 + /// + /// 线段起点。 + /// 线段终点。 + public LineSegment(Vector2 src, Vector2 dst) + { + Src = src; + Dst = dst; + } + + /// + /// 使用两个端点初始化一段3D线段。 + /// + /// 线段起点。 + /// 线段终点。 + public LineSegment(Vector3 src, Vector3 dst) + { + Src3D = src; + Dst3D = dst; + } + + /// + /// 使用两个端点初始化一条线段，2D。 + /// + /// 线段起点x坐标。 + /// 线段起点y坐标。 + /// 线段终点x坐标。 + /// 线段终点y坐标。 + public LineSegment(double x1, double y1, double x2, double y2) + { + Src = new Vector2((float)x1, (float)y1); + Dst = new Vector2((float)x2, (float)y2); + } + + /// + /// 使用两个端点初始化一条线段，2D。 + /// + /// 线段起点x坐标。 + /// 线段起点y坐标。 + /// 线段起点y坐标。 + /// 线段终点x坐标。 + /// 线段终点y坐标。 + /// 线段终点y坐标。 + public LineSegment(double x1, double y1, double z1, double x2, double y2, double z2) + { + Src3D = new Vector3((float)x1, (float)y1, (float)z1); + Dst3D = new Vector3((float)x2, (float)y2, (float)z2); + } + + /// + /// 返回线段长度，2D。 + /// + /// + public double Length() + { + return Vector2.Distance(Src, Dst); + } + + /// + /// 返回线段长度，3D。 + /// + /// + public double Length3D() + { + return Vector3.Distance(Src3D, Dst3D); + } + + /// + /// 返回线段与x轴正方向夹角度数，角度制。 + /// + /// + public double Angle() + { + return Math.Atan2(Dst.Y - Src.Y, Dst.X - Src.X) / Math.PI * 180; + } + + /// + /// 返回一段方向相反的线段。 + /// + /// + public LineSegment Reverse() + { + return new LineSegment(Dst3D, Src3D); + } + + /// + /// 线段2D起点。 + /// + public Vector2 Src + { + get => new(_srcX, _srcY); + set + { + _srcX = value.X; + _srcY = value.Y; + } + } + + /// + /// 线段2D终点。 + /// + public Vector2 Dst + { + get => new(_dstX, _dstY); + set + { + _dstX = value.X; + _dstY = value.Y; + } + } + + /// + /// 线段3D起点。 + /// + public Vector3 Src3D + { + get => new(_srcX, _srcY, _srcZ); + set + { + _srcX = value.X; + _srcY = value.Y; + _srcZ = value.Z; + } + } + + /// + /// 线段3D终点。 + /// + public Vector3 Dst3D + { + get => new(_dstX, _dstY, _dstZ); + set + { + _dstX = value.X; + _dstY = value.Y; + _dstZ = value.Z; + } + } + + private float _srcX, _srcY, _srcZ, _dstX, _dstY, _dstZ; + } +} diff --git a/CommonUsage-MultiVehicleSync/commonusage/Properties/launchSettings.json b/CommonUsage-MultiVehicleSync/commonusage/Properties/launchSettings.json new file mode 100644 index 0000000..53c176e --- /dev/null +++ b/CommonUsage-MultiVehicleSync/commonusage/Properties/launchSettings.json @@ -0,0 +1,12 @@ +{ + "profiles": { + "CommonUsage": { + "commandName": "Project" + }, + "配置文件 1": { + "commandName": "Executable", + "executablePath": "D:\\Code\\Core\\Medulla\\build\\Medulla.exe", + "workingDirectory": "D:\\Code\\Core\\Medulla\\build\\" + } + } +} \ No newline at end of file diff --git a/CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/CommunicationProtocolFactory.cs b/CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/CommunicationProtocolFactory.cs new file mode 100644 index 0000000..3376c97 --- /dev/null +++ b/CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/CommunicationProtocolFactory.cs @@ -0,0 +1,19 @@ +using System; +using System.Collections.Generic; +using System.Text; + +namespace CommonUsage.Protocols.VDA5050 +{ + public class CommunicationProtocolFactory + { + public static IVDACommunicationProtocol CreateProtocol(string protocolType, string host, int port) + { + return protocolType.ToLower() switch + { + "http" => new HTTPCommunication(host, port), + "mqtt" => new MQTTCommunication(host, port), + _ => throw new NotSupportedException($"Protocol {protocolType} is not supported") + }; + } + } +} diff --git a/CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/HTTPCommunication.cs b/CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/HTTPCommunication.cs new file mode 100644 index 0000000..5ff34a8 --- /dev/null +++ b/CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/HTTPCommunication.cs @@ -0,0 +1,94 @@ +using System; +using System.Collections.Generic; +using System.Net.Http; +using System.Text; +using System.Threading.Tasks; +using CommonUsage.Protocols.VDA5050.Messages; +using FundamentalLib; +using Newtonsoft.Json; + +namespace CommonUsage.Protocols.VDA5050 +{ + public class HTTPCommunication : IVDACommunicationProtocol + { + private readonly string _host; + private readonly int _port; + + public HTTPCommunication(string host, int port) + { + _host = host; + _port = port; + } + + public async Task PublishConnectionStatus(string status) + { + var message = new connectionMessage() + { + serialNumber = "test-01", + headerId = 1, + timestamp = DateTime.Now, + connectionState = status + }; + + await SendMessageAsync(message, "vda5050/connection"); + } + + public void SetupOrderListener(Action orderReceived) + { + PicoHttpServer.AddPostTextHandler("/order", new { }, (_, str) => + { + var order = JsonConvert.DeserializeObject(str); + orderReceived(order); + return ""; + }); + } + + public void SetUpInstanActionListener(Action onInstanceActionReceived) + { + PicoHttpServer.AddPostTextHandler("/instanceAction", new { }, (_, str) => + { + var instanceAction = JsonConvert.DeserializeObject(str); + onInstanceActionReceived(instanceAction); + return ""; + }); + } + + public void SetUpImmediateCommandListener(Action onChangeCarFieldsReceived) + { + throw new NotImplementedException(); + } + + public async Task SendMessageAsync(T message, string topic) + { + try + { + var url = $"http://{_host}:{_port}/{topic}"; + using var client = new HttpClient(); + var response = await client.PostAsync(url, new StringContent(JsonConvert.SerializeObject(message), Encoding.UTF8, "application/json")); + + if (!response.IsSuccessStatusCode) + { + Console.WriteLine($" >> Sending Message: Failed to send message. Status Code: {response.StatusCode}"); + } + } + catch (Exception ex) + { + Console.WriteLine($"Error in sending message: {ex.Message}"); + } + } + + public async Task SendVisualizationMessageAsync(T msg, string topic) + { + throw new NotImplementedException(); + } + + public void SetupTestListener(Action testMsg) + { + throw new NotImplementedException(); + } + public async Task PublishFactSheet(factsheetMessage message) + { + throw new NotImplementedException(); + } + } +} diff --git a/CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/IVDACommunicationProtocol.cs b/CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/IVDACommunicationProtocol.cs new file mode 100644 index 0000000..f8e998f --- /dev/null +++ b/CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/IVDACommunicationProtocol.cs @@ -0,0 +1,19 @@ +using System; +using System.Collections.Generic; +using System.Text; +using System.Threading.Tasks; +using CommonUsage.Protocols.VDA5050.Messages; + +namespace CommonUsage.Protocols.VDA5050 +{ + public interface IVDACommunicationProtocol + { + Task PublishConnectionStatus(string status); + Task PublishFactSheet(factsheetMessage msg); + void SetupOrderListener(Action orderReceived); + void SetUpInstanActionListener(Action onInstanceActionReceived); + void SetUpImmediateCommandListener(Action onChangeCarFieldsReceived); + Task SendMessageAsync(T message, string topic); + Task SendVisualizationMessageAsync(T message, string topic); + } +} diff --git a/CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/MQTTCommunication.cs b/CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/MQTTCommunication.cs new file mode 100644 index 0000000..133fd46 --- /dev/null +++ b/CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/MQTTCommunication.cs @@ -0,0 +1,319 @@ +using System.IO; +using System; +using System.Collections.Generic; +using System.Text; +using System.Threading; +using System.Threading.Tasks; +using CommonUsage.Protocols.VDA5050.Messages; +using MQTTnet; +using MQTTnet.Client; +using MQTTnet.Extensions.ManagedClient; +using MQTTnet.Packets; +using MQTTnet.Protocol; +using MQTTnet.Server; +using Newtonsoft.Json; +using FundamentalLib; +using CommonUsage.Protocols.VDA5050.Objects; +using FundamentalLib.MiscHelpers; + +namespace CommonUsage.Protocols.VDA5050 +{ + public class MQTTCommunication : IVDACommunicationProtocol + { + private readonly string _host; + private readonly int _port; + private readonly string _orderTopic = "vda5050/frldAGV/order"; + private readonly string _connectionTopic = "vda5050/frldAGV/connection"; + private readonly string _instanceAction = "vda5050/frldAGV/instantActions"; + private readonly string _factsheet = "vda5050/frldAGV/factsheet"; + private readonly string _changeCarFields = "vda5050/frldAGV/changeCarFields"; + + private IManagedMqttClient _client; + private IManagedMqttClient _visualizationClient; + + public MQTTCommunication(string host, int port) + { + _host = host; + _port = port; + InitializeClient(); + InitializeVisualizationClient(); + } + + private void InitializeClient() + { + var willMessage = new connectionMessage() + { + headerId = 1, + timestamp = DateTime.Now, + version = "00", + manufacturer = "frld", + serialNumber = "test-01", + connectionState = "CONNECTIONBROKEN" + }; + + var mqttClientOptions = new MqttClientOptionsBuilder() + .WithClientId("AGV-Client-frldAGV") + .WithTcpServer(_host, _port) + .WithWillTopic(_connectionTopic) + .WithWillPayload(JsonConvert.SerializeObject(willMessage)) + .WithWillRetain(true) + .Build(); + + var managedMqttClientOptions = new ManagedMqttClientOptionsBuilder() + .WithClientOptions(mqttClientOptions) + .WithMaxPendingMessages(20) + .WithPendingMessagesOverflowStrategy(MqttPendingMessagesOverflowStrategy.DropOldestQueuedMessage) + .Build(); + + _client = new MqttFactory().CreateManagedMqttClient(); + _client.StartAsync(managedMqttClientOptions).GetAwaiter().GetResult(); + Console.WriteLine($" >> MQTT client initialized and connected to broker at {_host} - {_port}"); + } + + private void InitializeVisualizationClient() + { + var mqttClientOptions = new MqttClientOptionsBuilder() + .WithClientId("AGV-Visualization") + .WithTcpServer(_host, _port) + .Build(); + + var managedMqttClientOptions = new ManagedMqttClientOptionsBuilder() + .WithClientOptions(mqttClientOptions) + .WithMaxPendingMessages(10) // Prevent overloading + .WithPendingMessagesOverflowStrategy(MqttPendingMessagesOverflowStrategy.DropOldestQueuedMessage) + .Build(); + + _visualizationClient = new MqttFactory().CreateManagedMqttClient(); + _visualizationClient.StartAsync(managedMqttClientOptions).GetAwaiter().GetResult(); + Console.WriteLine("MQTT Visualization client initialized."); + } + + public void SetUpImmediateCommandListener(Action onChangeCarFieldsReceived) + { + Console.WriteLine($"Subscribed to the topic: {_changeCarFields}"); + _client.SubscribeAsync(_changeCarFields).GetAwaiter().GetResult(); + + _client.ApplicationMessageReceivedAsync += async e => + { + if (e.ApplicationMessage.Topic == _changeCarFields) + { + var payload = Encoding.UTF8.GetString(e.ApplicationMessage.Payload); + LogMessage($"RECEIVE-{e.ApplicationMessage.Topic}", e.ApplicationMessage.Topic, payload); + //var script = JsonConvert.DeserializeObject(payload); + Console.WriteLine($"Change car field: {payload}"); + onChangeCarFieldsReceived(payload); + } + }; + } + + + public void SetUpInstanActionListener(Action onInstanceActionReceived) + { + Console.WriteLine($"Subscribed to the topic: {_instanceAction}"); + + // Subscribe to the instanceAction topic + _client.SubscribeAsync(_instanceAction).GetAwaiter().GetResult(); + + _client.ApplicationMessageReceivedAsync += async e => + { + if (e.ApplicationMessage.Topic == _instanceAction) + { + var payload = Encoding.UTF8.GetString(e.ApplicationMessage.Payload); + LogMessage($"RECEIVE-{e.ApplicationMessage.Topic}", e.ApplicationMessage.Topic, payload); + var actions = JsonConvert.DeserializeObject(payload); + Console.WriteLine($"Received instance action: Header ID = {actions.headerId}, Timestamp = {actions.timestamp}"); + foreach (var action in actions.actions) + { + Console.WriteLine($"Action ID: {action.actionId}, Type: {action.actionType}"); + } + + onInstanceActionReceived(actions); + } + }; + } + + public async Task PublishConnectionStatus(string status) + { + var message = new connectionMessage() + { + headerId = 1, + timestamp = DateTime.Now, + version = "00", + manufacturer = "frld", + serialNumber = "test-01", + connectionState = status + }; + + var payload = JsonConvert.SerializeObject(message); + + var content = new MqttApplicationMessageBuilder() + .WithTopic(_connectionTopic) + .WithPayload(payload) + .WithQualityOfServiceLevel(MqttQualityOfServiceLevel.AtLeastOnce) + .WithRetainFlag(true) + .Build(); + + await _client.EnqueueAsync(content); + LogMessage($"SEND-{_connectionTopic}", _connectionTopic, payload); + + //await SendMessageAsync(message, _connectionTopic); + } + + public async Task PublishFactSheet(factsheetMessage message) + { + + //await SendMessageAsync(message, _factsheet); + var payload = JsonConvert.SerializeObject(message); + + var content = new MqttApplicationMessageBuilder() + .WithTopic(_factsheet) + .WithPayload(payload) + .WithQualityOfServiceLevel(MqttQualityOfServiceLevel.AtMostOnce) + //.WithRetainFlag(true) + .Build(); + + await _client.EnqueueAsync(content); + LogMessage($"SEND-{_factsheet}", _factsheet, payload); + + } + + public void SetupOrderListener(Action orderReceived) + { + // Subscribe to the orders topic + _client.SubscribeAsync(_orderTopic, MqttQualityOfServiceLevel.AtMostOnce).GetAwaiter().GetResult(); + _client.ApplicationMessageReceivedAsync += async e => + { + if (e.ApplicationMessage.Topic == _orderTopic) + { + var payload = Encoding.UTF8.GetString(e.ApplicationMessage.Payload); + LogMessage($"RECEIVE-{e.ApplicationMessage.Topic}", e.ApplicationMessage.Topic, payload); + var order = JsonConvert.DeserializeObject(payload); + + // Save the order message to a file for debugging + // SaveOrderToFile(payload); + orderReceived(order); + } + await Task.CompletedTask; + }; + } + + private void SaveOrderToFile(string orderJson) + { + try + { + // Specify the file path (e.g., orders_log.txt in the current directory) + string filePath = "orders_log.txt"; + + // Append the order JSON along with a timestamp + File.AppendAllText(filePath, $"{DateTime.UtcNow:yyyy-MM-dd HH:mm:ss} - {orderJson}{Environment.NewLine}"); + } + catch (Exception ex) + { + // Handle any exceptions that occur while writing to the file + Console.WriteLine($"Failed to save order to file: {ex.Message}"); + } + } + + public async Task SendMessageAsync(T message, string topic) + { + + var payload = JsonConvert.SerializeObject(message); + + var content = new MqttApplicationMessageBuilder() + .WithTopic(topic) + .WithPayload(payload) + .WithQualityOfServiceLevel(MqttQualityOfServiceLevel.AtMostOnce) + .Build(); + + await _client.EnqueueAsync(content); + + if (topic != "vda5050/frldAGV/visualization") + { + + LogMessage($"SEND-{topic}", topic, payload); + } + + } + + public async Task SendVisualizationMessageAsync(T message, string topic) + { + if (_visualizationClient == null) return; // Ensure client is initialized + + var payload = JsonConvert.SerializeObject(message); + + var content = new MqttApplicationMessageBuilder() + .WithTopic(topic) + .WithPayload(payload) + .WithQualityOfServiceLevel(MqttQualityOfServiceLevel.AtMostOnce) // QoS 0 for lightweight visualization + .Build(); + + if (_visualizationClient.PendingApplicationMessagesCount < 5) // Prevent flooding + { + await _visualizationClient.EnqueueAsync(content); + } + else + { + Console.WriteLine("Skipping visualization update to avoid MQTT congestion."); + } + } + + + public void LogMessage(string direction, string topic, string payload) + { + string formattedPayload = payload; + string logDirectory = "Logs"; // Directory for log files + var filePreName = direction; + filePreName = filePreName.Replace("/", "_"); + string logFilePath = Path.Combine(logDirectory, filePreName + $"-{DateTime.Now:yyyy-MM-dd}.log"); + + if(!Directory.Exists(logDirectory)) Directory.CreateDirectory(logDirectory); + // Try to parse the payload as JSON and pretty-print it + try + { + var jsonObject = JsonConvert.DeserializeObject(payload); + formattedPayload = JsonConvert.SerializeObject(jsonObject, Formatting.Indented); + } + catch (JsonReaderException) + { + // If the payload is not valid JSON, just leave it as is + formattedPayload = payload; + } + + string logMessage = $"[{DateTime.Now:HH:mm:ss}] [{direction}] Topic: {topic}, Payload:\n{formattedPayload}\n"; + RotateLogFile(logFilePath, logDirectory); + + try + { + File.AppendAllText(logFilePath, logMessage + Environment.NewLine); + } + catch (Exception ex) + { + Console.WriteLine($"Error writing to log file: {ex.Message}"); + } + // Console.WriteLine($"[{DateTime.Now:HH:mm:ss}] [{direction}] Topic: {topic}, Payload: {formattedPayload}"); + // DLog.Log($"[{DateTime.Now:HH:mm:ss}] [{direction}] Topic: {topic}, Payload: {formattedPayload}"); + } + + + private void RotateLogFile(string logFilePath, string logDirectory) + { + const long maxFileSize = 10 * 1024 * 1024; // 10 MB in bytes + + FileInfo fileInfo = new FileInfo(logFilePath); + if (fileInfo.Exists && fileInfo.Length > maxFileSize) + { + string archivePath = Path.Combine(logDirectory, $"log_{DateTime.Now:yyyy-MM-dd_HH-mm-ss}.log"); + + try + { + File.Move(logFilePath, archivePath); // Rename the current log file + Console.WriteLine($"Log file rotated: {archivePath}"); + } + catch (Exception ex) + { + Console.WriteLine($"Error rotating log file: {ex.Message}"); + } + } + } + } +} diff --git a/CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/Messages/connectionMessage.cs b/CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/Messages/connectionMessage.cs new file mode 100644 index 0000000..a4f5c70 --- /dev/null +++ b/CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/Messages/connectionMessage.cs @@ -0,0 +1,18 @@ +using System; +using System.Collections.Generic; +using System.Text; + +namespace CommonUsage.Protocols.VDA5050.Messages +{ + public class connectionMessage + { + public int headerId; + public DateTime timestamp; + public string version = ""; + public string manufacturer = ""; + public string serialNumber = ""; + public string connectionState = ""; // Enum: {'ONLINE', 'OFFLINE', 'CONNECTIONBROKEN'} + + } + +} diff --git a/CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/Messages/errorMessage.cs b/CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/Messages/errorMessage.cs new file mode 100644 index 0000000..9888fe2 --- /dev/null +++ b/CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/Messages/errorMessage.cs @@ -0,0 +1,16 @@ +using System; +using System.Collections.Generic; +using System.Text; + +namespace CommonUsage.Protocols.VDA5050.Messages +{ + public class errorMessage + { + public string serialNumber = ""; + public string errorCode = ""; // Unique error code + public string description = ""; // Error description + public string severity = ""; // Enum {'WARNING', 'FATAL'} + public DateTime timestamp; // Time of the error + } + +} diff --git a/CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/Messages/factsheetMessage.cs b/CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/Messages/factsheetMessage.cs new file mode 100644 index 0000000..bcbd349 --- /dev/null +++ b/CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/Messages/factsheetMessage.cs @@ -0,0 +1,23 @@ +using System; +using System.Collections.Generic; +using System.Text; +using CommonUsage.Protocols.VDA5050.Objects; + +namespace CommonUsage.Protocols.VDA5050.Messages +{ + public class factsheetMessage + { + public int headerId; + public DateTime timestamp; + public string version = ""; + public string manufacturer = ""; + public string serialNumber = ""; + + public typeSpecification typeSpecification; + public physicalParameters physicalParameters; + public protocolLimits protocolLimits; + public protocolFeatures protocolFeatures; + public agvGeometry agvGeometry; + } + +} diff --git a/CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/Messages/instanceAction.cs b/CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/Messages/instanceAction.cs new file mode 100644 index 0000000..e5c5f8d --- /dev/null +++ b/CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/Messages/instanceAction.cs @@ -0,0 +1,19 @@ +using System; +using System.Collections.Generic; +using System.Text; +using CommonUsage.Protocols.VDA5050.Objects; + +namespace CommonUsage.Protocols.VDA5050.Messages +{ + public class instanceAction + { + + public uint headerId { get; set; } // Incremented for each new message. + public string timestamp { get; set; } // ISO 8601 UTC timestamp. + public string version { get; set; } // Protocol version. + public string manufacturer { get; set; } // AGV manufacturer. + public string serialNumber { get; set; } // Unique AGV serial number. + + public List actions { get; set; } + } +} diff --git a/CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/Messages/orderMessage.cs b/CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/Messages/orderMessage.cs new file mode 100644 index 0000000..68516c7 --- /dev/null +++ b/CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/Messages/orderMessage.cs @@ -0,0 +1,27 @@ +using System; +using System.Collections.Generic; +using System.Text; +using CommonUsage.Protocols.VDA5050.Objects; + +namespace CommonUsage.Protocols.VDA5050.Messages +{ + public class orderMessage + { + public uint headerId; + public string timestamp = ""; + public string version = ""; + public string manufacturer = ""; + public string serialNumber = ""; + + public string orderId { get; set; } + + public uint orderUpdateId { get; set; } + + public node[] nodes { get; set; } + + public edge[] edges { get; set; } + + //public action[] action { get; set; } + } +} + diff --git a/CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/Messages/stateMessage.cs b/CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/Messages/stateMessage.cs new file mode 100644 index 0000000..44b0438 --- /dev/null +++ b/CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/Messages/stateMessage.cs @@ -0,0 +1,69 @@ +using System; +using System.Collections.Generic; +using System.ComponentModel; +using CommonUsage.Protocols.VDA5050.Objects; + +namespace CommonUsage.Protocols.VDA5050.Messages +{ + /// + /// 6.10 Topic: "state" (from AGV to master control) + /// todo: complete all fields required by VDA5050 + /// + public class stateMessage + { + public uint headerId; + public string timestamp = ""; + public string version = ""; + public string manufacturer = ""; + public string serialNumber = ""; + /// + /// Unique order identification of the current order or the previously finished order. + /// The orderId is kept until a new order is received. + /// Empty string (""), if no previous orderId is available. + /// + public string orderId = ""; + + /// + /// Order update identification to identify, that an order update has been accepted by the AGV. + /// "0" if no previous orderUpdateId is available. + /// + public uint orderUpdatedId = 0; + + public string lastNodeId; + + public uint lastNodeSequenceId; + + /// + /// Array of nodeState objects that need to be traversed for fulfilling the order (empty array if idle) + /// + public nodeState[] nodeStates = []; + + /// + /// Array of edgeState objects that need to be traversed for fulfilling the order (empty array if idle) + /// + public edgeState[] edgeStates = []; + + public agvPosition agvPosition; + + public velocity velocity; + public load[] loads = []; + + public bool driving; + + public bool paused; + + public bool newBaseRequest; + + public double distanceSinceLastNode; + + public batteryState batteryState; + + public actionState[] actionStates = Array.Empty(); + public string operatingMode = ""; + public List errors { get; set; } = new List(); // Array of errorState objects + + public info[] information = []; + public safetyState safetyState; + + } +} diff --git a/CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/Messages/visualizationMessage.cs b/CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/Messages/visualizationMessage.cs new file mode 100644 index 0000000..c895311 --- /dev/null +++ b/CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/Messages/visualizationMessage.cs @@ -0,0 +1,12 @@ +using CommonUsage.Protocols.VDA5050.Objects; +using System; +using System.Collections.Generic; +using System.Text; + +namespace CommonUsage.Protocols.VDA5050.Messages +{ + public class visualizationMessage + { + public agvPosition agvPosition; + } +} diff --git a/CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/Objects/action.cs b/CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/Objects/action.cs new file mode 100644 index 0000000..e4212b4 --- /dev/null +++ b/CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/Objects/action.cs @@ -0,0 +1,18 @@ +using System; +using System.Collections.Generic; +using System.Text; + +namespace CommonUsage.Protocols.VDA5050.Objects +{ + public class action + { + + public string actionId { get; set; } + + public string actionType { get; set; } + public string actionDescription { get; set; } + + public string blockingType { get; set; } + + } +} diff --git a/CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/Objects/actionState.cs b/CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/Objects/actionState.cs new file mode 100644 index 0000000..fe454e2 --- /dev/null +++ b/CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/Objects/actionState.cs @@ -0,0 +1,45 @@ +using Newtonsoft.Json.Converters; +using Newtonsoft.Json; +using System; +using System.Collections.Generic; +using System.Text; + +namespace CommonUsage.Protocols.VDA5050.Objects +{ + public class actionState + { + public actionState(action action,ActionStateEnum state) + { + actionId = action.actionId; + actionDescription = action.actionDescription; + actionType = action.actionType; + actionStatus = state; + } + + public actionState() + { + } + + public string actionId { get; set; } + + public string actionType { get; set; } + public string actionDescription { get; set; } + + [JsonConverter(typeof(StringEnumConverter))] + public ActionStateEnum actionStatus { get; set; } + + public string resultDescription { get; set; } + + public enum ActionStateEnum + { + WAITING, + INITIALIZING, + RUNNING, + PAUSED, + FINISHED, + FAILED + } + + } + +} \ No newline at end of file diff --git a/CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/Objects/agvGeometry.cs b/CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/Objects/agvGeometry.cs new file mode 100644 index 0000000..d96e7b7 --- /dev/null +++ b/CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/Objects/agvGeometry.cs @@ -0,0 +1,63 @@ +using System; +using System.Collections.Generic; +using System.Text; +using static CommonUsage.Protocols.VDA5050.Objects.agvGeometry; + +namespace CommonUsage.Protocols.VDA5050.Objects +{ + public class agvGeometry + { + // Wheel Definitions + public List wheelDefinitions { get; set; } = new List(); + + // 2D Envelopes + public List envelopes2D { get; set; } = new List(); + + // 3D Envelopes + public List envelopes3D { get; set; } = new List(); + + public class wheelDefinition + { + public enum WheelType { DRIVE, CASTER, FIXED, MECANUM } + + public WheelType type { get; set; } + public bool isActiveDriven { get; set; } + public bool isActiveSteered { get; set; } + + // Wheel Position + public double positionX { get; set; } + public double positionY { get; set; } + public double positionTheta { get; set; } // Required for fixed wheels + + // Wheel Properties + public double diameter { get; set; } + public double width { get; set; } + public double centerDisplacement { get; set; } = 0; // Default to 0 if not defined + public string constraints { get; set; } + } + + public class envelope2D + { + public string set { get; set; } + public List polygonPoints { get; set; } = new List(); + public string description { get; set; } + + public class polygonPoint + { + public double x { get; set; } + public double y { get; set; } + } + } + + public class envelope3D + { + public string set { get; set; } + public string format { get; set; } + public object data { get; set; } // JSON object for 3D envelope data + public string url { get; set; } + public string description { get; set; } + } + } + +} + diff --git a/CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/Objects/agvPosition.cs b/CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/Objects/agvPosition.cs new file mode 100644 index 0000000..8659cd3 --- /dev/null +++ b/CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/Objects/agvPosition.cs @@ -0,0 +1,21 @@ +using System; +using System.Collections.Generic; +using System.Text; + +namespace CommonUsage.Protocols.VDA5050.Objects +{ + public class agvPosition + { + public bool positionInitialized; + + public double x; + + public double y; + + public double theta; + public double localizationScore; + public double deviationRange; + public string mapId = ""; + public string mapDescription = ""; + } +} diff --git a/CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/Objects/batteryState.cs b/CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/Objects/batteryState.cs new file mode 100644 index 0000000..1d3392c --- /dev/null +++ b/CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/Objects/batteryState.cs @@ -0,0 +1,15 @@ +using System; +using System.Collections.Generic; +using System.Text; + +namespace CommonUsage.Protocols.VDA5050.Objects +{ + public class batteryState + { + public double batteryCharge; + public double batteryVoltage; + public double batteryHealth; + public bool charging; + public int reach; + } +} diff --git a/CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/Objects/boundingBoxReference.cs b/CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/Objects/boundingBoxReference.cs new file mode 100644 index 0000000..be48032 --- /dev/null +++ b/CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/Objects/boundingBoxReference.cs @@ -0,0 +1,14 @@ +using System; +using System.Collections.Generic; +using System.Text; + +namespace CommonUsage.Protocols.VDA5050.Objects +{ + public class boundingBoxReference + { + public double X { get; set; } // Reference point X in AGV coordinate system + public double Y { get; set; } // Reference point Y in AGV coordinate system + public double Z { get; set; } // Reference point Z in AGV coordinate system + public double Theta { get; set; } // Orientation of the load bounding box + } +} diff --git a/CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/Objects/controlPoint.cs b/CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/Objects/controlPoint.cs new file mode 100644 index 0000000..03300b8 --- /dev/null +++ b/CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/Objects/controlPoint.cs @@ -0,0 +1,21 @@ +using System; +using System.Collections.Generic; +using System.Text; + +namespace CommonUsage.Protocols.VDA5050.Objects +{ + public class controlPoint + { + public float x; + + public float y; + + public float weight; + public controlPoint(float x, float y, float weight) + { + this.x = x; + this.y = y; + this.weight = weight; + } + } +} diff --git a/CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/Objects/edge.cs b/CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/Objects/edge.cs new file mode 100644 index 0000000..e207eb5 --- /dev/null +++ b/CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/Objects/edge.cs @@ -0,0 +1,31 @@ +using System; +using System.Collections.Generic; +using System.Numerics; +using System.Text; + +namespace CommonUsage.Protocols.VDA5050.Objects +{ + public class edge : sequenceItem + { + public string edgeId; + + public string edgeDescription; + + public string startNodeId; + + public string endNodeId; + + public double maxSpeed; + + public double orientation; + + public trajectory? trajectory; + + public float[] trackTypeInfo; + // public List controlPoints; + // + // public List weights; + + public action[] action = []; + } +} diff --git a/CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/Objects/edgeState.cs b/CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/Objects/edgeState.cs new file mode 100644 index 0000000..e5d8af3 --- /dev/null +++ b/CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/Objects/edgeState.cs @@ -0,0 +1,15 @@ +using System; +using System.Collections.Generic; +using System.Text; + +namespace CommonUsage.Protocols.VDA5050.Objects +{ + public class edgeState : sequenceItem + { + public string edgeId; + + public string edgeDescription; + + public trajectory trajectory; + } +} diff --git a/CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/Objects/errorReference.cs b/CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/Objects/errorReference.cs new file mode 100644 index 0000000..f44d5c7 --- /dev/null +++ b/CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/Objects/errorReference.cs @@ -0,0 +1,12 @@ +using System; +using System.Collections.Generic; +using System.Text; + +namespace CommonUsage.Protocols.VDA5050.Objects +{ + public class errorReference + { + public string referenceKey { get; set; } // Type of reference (e.g., nodeId, edgeId, actionId) + public string referenceValue { get; set; } // Value corresponding to the referenceKey + } +} diff --git a/CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/Objects/errorState.cs b/CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/Objects/errorState.cs new file mode 100644 index 0000000..227ed80 --- /dev/null +++ b/CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/Objects/errorState.cs @@ -0,0 +1,15 @@ +using System; +using System.Collections.Generic; +using System.Text; + +namespace CommonUsage.Protocols.VDA5050.Objects +{ + public class errorState + { + public List errorReferences { get; set; } = new List(); // Array of references + public string errorType { get; set; } // Required: Type/name of the error + public string errorDescription { get; set; } // Verbose description of the error + public string errorHint { get; set; } // Hint for resolving the error + public string errorLevel { get; set; } // Required: WARNING or FATAL + } +} diff --git a/CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/Objects/info.cs b/CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/Objects/info.cs new file mode 100644 index 0000000..a638ec8 --- /dev/null +++ b/CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/Objects/info.cs @@ -0,0 +1,26 @@ +using System; +using System.Collections.Generic; +using System.Text; + +namespace CommonUsage.Protocols.VDA5050.Objects +{ + public class info + { + public string infoType { get; set; } // Type/name of the information + public List infoReferences { get; set; } = new List(); // List of references + public string infoDescription { get; set; } // Description of the information + public infoLevelEnum infoLevel { get; set; } // Debugging or visualization level + + public class infoReference + { + public string ReferenceKey { get; set; } // Reference type (e.g., headerId, orderId) + public string ReferenceValue { get; set; } // The actual referenced field value + } + + public enum infoLevelEnum + { + DEBUG, // Used for debugging + INFO // Used for visualization + } + } +} diff --git a/CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/Objects/load.cs b/CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/Objects/load.cs new file mode 100644 index 0000000..91530d0 --- /dev/null +++ b/CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/Objects/load.cs @@ -0,0 +1,16 @@ +using System; +using System.Collections.Generic; +using System.Text; + +namespace CommonUsage.Protocols.VDA5050.Objects +{ + public class load + { + public string loadId { get; set; } // Unique ID (barcode, RFID, etc.) + public string loadType { get; set; } // Type of load + public string loadPosition { get; set; } // Load handling position (e.g., "front", "back") + public boundingBoxReference boundingBoxReference { get; set; } = new boundingBoxReference(); + public loadDimensions loadDimensions { get; set; } = new loadDimensions(); + public double weight { get; set; } // Weight of load in kg (0.0 to ∞) + } +} diff --git a/CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/Objects/loadDimensions.cs b/CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/Objects/loadDimensions.cs new file mode 100644 index 0000000..90469c6 --- /dev/null +++ b/CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/Objects/loadDimensions.cs @@ -0,0 +1,13 @@ +using System; +using System.Collections.Generic; +using System.Text; + +namespace CommonUsage.Protocols.VDA5050.Objects +{ + public class loadDimensions + { + public double Length { get; set; } // Length of the bounding box + public double Width { get; set; } // Width of the bounding box + public double Height { get; set; } // Height of the bounding box (optional) + } +} diff --git a/CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/Objects/node.cs b/CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/Objects/node.cs new file mode 100644 index 0000000..c6617cf --- /dev/null +++ b/CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/Objects/node.cs @@ -0,0 +1,17 @@ +using System; +using System.Collections.Generic; +using System.Text; + +namespace CommonUsage.Protocols.VDA5050.Objects +{ + public class node : sequenceItem + { + public string nodeId; + + public string nodeDescription; + + public nodePosition nodePosition; + + public action[] actions = []; + } +} diff --git a/CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/Objects/nodePosition.cs b/CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/Objects/nodePosition.cs new file mode 100644 index 0000000..a1c362a --- /dev/null +++ b/CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/Objects/nodePosition.cs @@ -0,0 +1,36 @@ +using System; +using System.Collections.Generic; +using System.Text; + +namespace CommonUsage.Protocols.VDA5050.Objects +{ + /// + /// Defines the position on a map in a global project-specific world coordinate system. + /// Each floor has its own map. + /// All maps shall use the same project-specific global origin. + /// + public class nodePosition + { + /// + /// X-position on the map in reference to the map coordinate system. + /// Precision is up to the specific implementation. + /// + public double x; + + /// + /// Y-position on the map in reference to the map coordinate system. + /// Precision is up to the specific implementation. + /// + public double y; + + /// + /// Range: [-Pi ... Pi] + /// Absolute orientation of the AGV on the node. + /// Optional: vehicle can plan the path by itself. If defined, the AGV has to assume the theta angle on this node. + /// If previous edge disallows rotation, the AGV shall rotate on the node. + /// If following edge has a differing orientation defined but disallows rotation, + /// the AGV is to rotate on the node to the edges desired rotation before entering the edge. + /// + public double theta; + } +} diff --git a/CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/Objects/nodeState.cs b/CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/Objects/nodeState.cs new file mode 100644 index 0000000..2a67bdf --- /dev/null +++ b/CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/Objects/nodeState.cs @@ -0,0 +1,26 @@ +using System; +using System.Collections.Generic; +using System.Text; + +namespace CommonUsage.Protocols.VDA5050.Objects +{ + public class nodeState : sequenceItem + { + /// + /// Unique node identification. + /// + public string nodeId; + + /// + /// Additional information on the node. + /// + public string nodeDescription; + + /// + /// Node position. + /// The object is defined in 6.6 Topic: "order" (from master control to AGV) + /// Optional: Master control has this information. Can be sent additionally, e.g., for debugging purposes. + /// + public nodePosition nodePosition; + } +} diff --git a/CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/Objects/physicalParameters.cs b/CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/Objects/physicalParameters.cs new file mode 100644 index 0000000..3b4e0ec --- /dev/null +++ b/CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/Objects/physicalParameters.cs @@ -0,0 +1,20 @@ +using System; +using System.Collections.Generic; +using System.Text; + +namespace CommonUsage.Protocols.VDA5050.Objects +{ + public class physicalParameters + { + public double speedMin; + public double speedMax; + public double angularSpeedMin; + public double angularSpeedMax; + public double accelerationMax; + public double decelerationMax; + public double heightMin; + public double heightMax; + public double width; + public double length; + } +} diff --git a/CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/Objects/protocolFeatures.cs b/CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/Objects/protocolFeatures.cs new file mode 100644 index 0000000..bcead23 --- /dev/null +++ b/CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/Objects/protocolFeatures.cs @@ -0,0 +1,10 @@ +using System; +using System.Collections.Generic; +using System.Text; + +namespace CommonUsage.Protocols.VDA5050.Objects +{ + public class protocolFeatures + { + } +} diff --git a/CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/Objects/protocolLimits.cs b/CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/Objects/protocolLimits.cs new file mode 100644 index 0000000..64c7df7 --- /dev/null +++ b/CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/Objects/protocolLimits.cs @@ -0,0 +1,10 @@ +using System; +using System.Collections.Generic; +using System.Text; + +namespace CommonUsage.Protocols.VDA5050.Objects +{ + public class protocolLimits + { + } +} diff --git a/CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/Objects/safetyState.cs b/CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/Objects/safetyState.cs new file mode 100644 index 0000000..5f67a98 --- /dev/null +++ b/CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/Objects/safetyState.cs @@ -0,0 +1,20 @@ +using System; +using System.Collections.Generic; +using System.Text; + +namespace CommonUsage.Protocols.VDA5050.Objects +{ + public class safetyState + { + public eStopEnum eStop { get; set; } // Emergency stop status + public bool fieldViolation { get; set; } // "true" if a safety field is violated, "false" otherwise + + public enum eStopEnum + { + AUTOACK, // Auto-acknowledged emergency stop (e.g., triggered by a bumper) + MANUAL, // Manually confirmed emergency stop + REMOTE, // Remote-confirmed emergency stop + NONE // No emergency stop activated + } + } +} diff --git a/CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/Objects/sequenceItem.cs b/CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/Objects/sequenceItem.cs new file mode 100644 index 0000000..b77f535 --- /dev/null +++ b/CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/Objects/sequenceItem.cs @@ -0,0 +1,13 @@ +using System; +using System.Collections.Generic; +using System.Text; + +namespace CommonUsage.Protocols.VDA5050.Objects +{ + public class sequenceItem + { + public uint sequenceId; + + public bool released; + } +} diff --git a/CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/Objects/trajectory.cs b/CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/Objects/trajectory.cs new file mode 100644 index 0000000..1b792c4 --- /dev/null +++ b/CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/Objects/trajectory.cs @@ -0,0 +1,15 @@ +using System; +using System.Collections.Generic; +using System.Text; + +namespace CommonUsage.Protocols.VDA5050.Objects +{ + public class trajectory + { + public float degree; + + public float[] knotVector; + + public controlPoint[] controlPoints; + } +} diff --git a/CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/Objects/typeSpecification.cs b/CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/Objects/typeSpecification.cs new file mode 100644 index 0000000..1f8d6e9 --- /dev/null +++ b/CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/Objects/typeSpecification.cs @@ -0,0 +1,15 @@ +using System; +using System.Collections.Generic; +using System.Text; + +namespace CommonUsage.Protocols.VDA5050.Objects +{ + public class typeSpecification + { + public string agvKinemantic = ""; + public string agvClass = ""; + public double maxLoadMass; + public string[] localizationTypes; // Simplified description of localization type (e.g., NATURAL, REFLECTOR, RFID, DMC, GRID) + public string[] navigationTypes; // Path planning types (e.g., 'AUTONOMOUS', 'VIRTUAL_LINE_GUIDED') + } +} diff --git a/CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/Objects/velocity.cs b/CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/Objects/velocity.cs new file mode 100644 index 0000000..d774ba1 --- /dev/null +++ b/CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/Objects/velocity.cs @@ -0,0 +1,15 @@ +using System; +using System.Collections.Generic; +using System.Text; + +namespace CommonUsage.Protocols.VDA5050.Objects +{ + public class velocity + { + public double vx; + + public double vy; + + public double omega; + } +} diff --git a/CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/VDA5050Basic.cs b/CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/VDA5050Basic.cs new file mode 100644 index 0000000..029348c --- /dev/null +++ b/CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/VDA5050Basic.cs @@ -0,0 +1,118 @@ +//using CommonUsage.Protocols.VDA5050.Messages; +//using System; +//using System.Collections.Generic; +//using System.Net.Http.Headers; +//using System.Runtime.CompilerServices; +//using System.Text; +//using System.Threading; +//using System.Threading.Tasks; +//using CommonUsage.Protocols.VDA5050.Objects; +//using ClumsyCore.Utilities; +//using System.Linq; +//using ClumsyCore.Pilot; +//using System.Numerics; + +//namespace CommonUsage.Protocols.VDA5050 +//{ +// public abstract class VDA5050Basic +// { +// protected static IVDACommunicationProtocol _communicationProtocol; + +// public void Enable(IVDACommunicationProtocol protocol) +// { +// _communicationProtocol = protocol; +// Task.Run(() => ManageConnection()); +// StartVisualizationLoop(); +// } +// private void StartVisualizationLoop() +// { +// Console.WriteLine("Visualization in CommonUsage"); +// new Thread(async () => +// { +// while (true) +// { +// var position = GetAGVPosition(); +// if (position != null) +// { +// var msg = new stateMessage() +// { +// serialNumber = "test-01", +// agvPosition = new() +// { +// x = position.Value.X, +// y = position.Value.Y, +// theta = position.Value.Theta, +// positionInitialized = true +// } + +// }; +// await _communicationProtocol.SendMessageAsync(msg, "vda5050/frldAGV/visualization"); +// } + +// Thread.Sleep(100); +// } +// }) +// { Name = "VDA5050TopicVisualization" }.Start(); +// } + +// private void ManageConnection() +// { +// while (true) +// { +// var status = CheckConnectionStatus() ? "ONLINE" : "OFFLINE"; +// _communicationProtocol.PublishConnectionStatus(status); +// Thread.Sleep(1000); +// } +// } + +// protected List OrganizeReceivedSequence(orderMessage order) +// { +// List receivedSequence = new(); + +// int ii = 0, jj = 0; +// while (true) +// { +// var edge = order.edges[ii]; +// var node = order.nodes[jj]; +// var takeEdge = edge.sequenceId < node.sequenceId; + +// if (takeEdge) +// { +// receivedSequence.Add(edge); +// ii++; +// if (ii == order.edges.Length) break; +// } +// else +// { +// receivedSequence.Add(node); +// jj++; +// if (jj == order.nodes.Length) break; +// } +// } +// for (var i = ii; i < order.edges.Length; ++i) receivedSequence.Add(order.edges[i]); +// for (var j = jj; j < order.nodes.Length; ++j) receivedSequence.Add(order.nodes[j]); + +// for (var i = 1; i < receivedSequence.Count; i++) +// { +// if (receivedSequence[i - 1].sequenceId + 1 != receivedSequence[i].sequenceId) +// throw new Exception("stateMessage not continuous!"); +// } + +// return receivedSequence; +// } + +// public virtual bool CheckConnectionStatus() +// { +// return false; +// } + +// protected abstract Vector3? GetAGVPosition(); +// public struct Vector3 +// { +// public double X; +// public double Y; +// public double Theta; +// } +// } + +//} diff --git a/CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/VDA5050Helper.cs b/CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/VDA5050Helper.cs new file mode 100644 index 0000000..9f5217f --- /dev/null +++ b/CommonUsage-MultiVehicleSync/commonusage/Protocols/VDA5050/VDA5050Helper.cs @@ -0,0 +1,11 @@ +using System; +using System.Collections.Generic; +using System.Text; + +namespace CommonUsage.Protocols.VDA5050 +{ + public class VDA5050Helper + { + + } +} diff --git a/CommonUsage-MultiVehicleSync/commonusage/Visualizer.cs b/CommonUsage-MultiVehicleSync/commonusage/Visualizer.cs new file mode 100644 index 0000000..f17c27d --- /dev/null +++ b/CommonUsage-MultiVehicleSync/commonusage/Visualizer.cs @@ -0,0 +1,28 @@ +using System; +using System.Collections.Generic; +using System.Drawing; +using System.Numerics; +using System.Text; + +namespace CommonUsage +{ + public class Visualizer + { + public Action LineAction; + + public Action TextAction; + + public Action Clear; + + public void DrawLine(Color color, Vector2 src, Vector2 dst, bool startArrow = false, bool endArrow = false, + int width = 1) + { + LineAction?.Invoke(color, src, dst, startArrow, endArrow, width); + } + + public void DrawText(Color color, string text, Vector2 pos) + { + TextAction?.Invoke(color, text, pos); + } + } +} diff --git a/MedullaAdapter/MedullaAdapter.csproj b/MedullaAdapter/MedullaAdapter.csproj index fbac02d..421f8ce 100644 --- a/MedullaAdapter/MedullaAdapter.csproj +++ b/MedullaAdapter/MedullaAdapter.csproj @@ -27,11 +27,6 @@ false - - ref\CommonUsage.dll - false - - ref\MDCSToolBox.dll false @@ -41,6 +36,9 @@ ref\CycleGUI.dll false + + ..\ref\CommonUsage.dll + diff --git a/MedullaAdapter/build/Medulla/plugins/CommonUsage.dll b/MedullaAdapter/build/Medulla/plugins/CommonUsage.dll new file mode 100644 index 0000000..cdb9124 Binary files /dev/null and b/MedullaAdapter/build/Medulla/plugins/CommonUsage.dll differ diff --git a/MedullaAdapter/build/Medulla/plugins/MedullaAdapter.deps.json b/MedullaAdapter/build/Medulla/plugins/MedullaAdapter.deps.json index f850763..c1ccd7d 100644 --- a/MedullaAdapter/build/Medulla/plugins/MedullaAdapter.deps.json +++ b/MedullaAdapter/build/Medulla/plugins/MedullaAdapter.deps.json @@ -7,9 +7,20 @@ "targets": { ".NETCoreApp,Version=v8.0": { "MedullaAdapter/1.0.0": { + "dependencies": { + "CommonUsage": "1.0.0.0" + }, "runtime": { "MedullaAdapter.dll": {} } + }, + "CommonUsage/1.0.0.0": { + "runtime": { + "CommonUsage.dll": { + "assemblyVersion": "1.0.0.0", + "fileVersion": "1.0.0.0" + } + } } } }, @@ -18,6 +29,11 @@ "type": "project", "serviceable": false, "sha512": "" + }, + "CommonUsage/1.0.0.0": { + "type": "reference", + "serviceable": false, + "sha512": "" } } } \ No newline at end of file diff --git a/MedullaAdapter/build/Medulla/plugins/MedullaAdapter.dll b/MedullaAdapter/build/Medulla/plugins/MedullaAdapter.dll index 52be01c..cfce787 100644 Binary files a/MedullaAdapter/build/Medulla/plugins/MedullaAdapter.dll and b/MedullaAdapter/build/Medulla/plugins/MedullaAdapter.dll differ diff --git a/MedullaAdapter/build/Medulla/plugins/MedullaAdapter.pdb b/MedullaAdapter/build/Medulla/plugins/MedullaAdapter.pdb index f3a1f58..9ab97e2 100644 Binary files a/MedullaAdapter/build/Medulla/plugins/MedullaAdapter.pdb and b/MedullaAdapter/build/Medulla/plugins/MedullaAdapter.pdb differ diff --git a/Shared/MultiWheelChassisAdapter.cs b/Shared/MultiWheelChassisAdapter.cs index 0af8468..2ad3417 100644 --- a/Shared/MultiWheelChassisAdapter.cs +++ b/Shared/MultiWheelChassisAdapter.cs @@ -119,6 +119,10 @@ namespace MyParking.Shared // 禁用旧版DirectionAngle/ZeroDirection坐标偏置， // 保证SendXYThSpeed直接使用真实车体坐标系。 ResetToBodyFrame(); + + // 停车机器人优先保持当前机械舵角，通过反转轮速表达反向运动， + // 避免蟹行正反切换时四个舵轮无意义地旋转180°。 + _chassis.PreferMinimumSteeringTravel = true; } diff --git a/ref/CommonUsage.dll b/ref/CommonUsage.dll new file mode 100644 index 0000000..cdb9124 Binary files /dev/null and b/ref/CommonUsage.dll differ