First_Dev_Dept/ParkingRobot
10
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

chore: save current workspace progress

Browse Source
This commit is contained in:
梁薄云
2026-08-09 22:13:18 +08:00
parent 650c2ab0e3
commit 2f4fd15e52
449 changed files with 76593 additions and 971 deletions
Download Patch File Download Diff File Expand all files Collapse all files
ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Test/Fixtures/path-smoothing-fixtures.json
+8 -8
View File
@@ -4,7 +4,7 @@
{
"id": "straight",
"fixtureVersion": 1,
"configurationFingerprint": "sha256:d8c625157c21789a8bd33052a7f5927e4b1f697a2567cf3644b37ce00e164c51",
"configurationFingerprint": "sha256:d42ce997e1e51d421fe923c5819a332706007d900cc74a39d4b0ac88c6ac4a21",
"map": {
"xMinMm": 0.0,
"xMaxMm": 6000.0,
@@ -1914,7 +1914,7 @@
{
"id": "single-turn",
"fixtureVersion": 1,
"configurationFingerprint": "sha256:a0218dbe66746b347139161d545656d6a55516f8e87769b4f9f17c5cf8d5d84f",
"configurationFingerprint": "sha256:49571e2eeb2e58f0a5dc0a56e61e530700c3da7d60e2a9c66809721963e5d9f8",
"map": {
"xMinMm": -7000.0,
"xMaxMm": 11000.0,
@@ -3020,7 +3020,7 @@
{
"id": "s-bend",
"fixtureVersion": 1,
"configurationFingerprint": "sha256:aa1ae45d914e3ddc181bef415d71659fb357f9e2288e3eecec52376e6ca663bc",
"configurationFingerprint": "sha256:810b601e9090644292e4eb5652379f5f0db6fa7158aaf6e4faeae4b337ad5ec5",
"map": {
"xMinMm": 0.0,
"xMaxMm": 6000.0,
@@ -6715,7 +6715,7 @@
{
"id": "large-heading-change",
"fixtureVersion": 1,
"configurationFingerprint": "sha256:5a5b66530a8782e81b456102602112d3e9f41f031d976e7daad035f9fa70311b",
"configurationFingerprint": "sha256:44b6c95a3b553bb04df3912d5cf7c9110f4d629e37551b2c130d8d7f22706fce",
"map": {
"xMinMm": -7000.0,
"xMaxMm": 11000.0,
@@ -8253,7 +8253,7 @@
{
"id": "rectangle-detour",
"fixtureVersion": 1,
"configurationFingerprint": "sha256:de5a9261b3b073e4324526ecaf0dfc9251aa4b02f85e3c07abc0c63cfe72c85c",
"configurationFingerprint": "sha256:6ad46b2782bc519b17d62dac9e238542a88f4265fb7dd3a1ebe84c04313cf6ce",
"map": {
"xMinMm": 0.0,
"xMaxMm": 6000.0,
@@ -12158,7 +12158,7 @@
{
"id": "multi-obstacle-detour",
"fixtureVersion": 1,
"configurationFingerprint": "sha256:278442046751c251baea0961aeb6d1bb4976498b2abb321754e5a5e941b5d2ce",
"configurationFingerprint": "sha256:d062cfcf4fec6c0119575bb0c139697d8e4d5164e958cf511c0200d0f8d9fb5d",
"map": {
"xMinMm": 0.0,
"xMaxMm": 6000.0,
@@ -15589,7 +15589,7 @@
{
"id": "narrow-corridor",
"fixtureVersion": 1,
"configurationFingerprint": "sha256:4ddf1423d67649faa9f52ae5ca8455c594e79ddf48374a4759a214cb5075c1b6",
"configurationFingerprint": "sha256:5ec976b32eec84abdd781b8cedeb21d07dfd6809ea61c108bcce8cff0b9d3a37",
"map": {
"xMinMm": 0.0,
"xMaxMm": 6000.0,
@@ -17520,7 +17520,7 @@
{
"id": "forward-reverse-switch",
"fixtureVersion": 1,
"configurationFingerprint": "sha256:68563bf572a2fb3d9db25e7639bf488e36f83c990dbf5578373e48f3ed6270ed",
"configurationFingerprint": "sha256:86e1436370027ad30a7a205a78da291f81345e6acff2c0bb4f2256803ad9d962",
"map": {
"xMinMm": 0.0,
"xMaxMm": 6000.0,
ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Test/LocalG2DiagnosticVisualizationDemo.cs
+171
View File
@@ -0,0 +1,171 @@
using System;
using System.Collections.Generic;
using System.IO;
using System.Security.Cryptography;
using System.Threading;
using MultiWheelC.TrajectoryPlanning.CoarsePath;
using MultiWheelC.TrajectoryPlanning.PathSmoothing.Output.Comparison;
using MultiWheelC.TrajectoryPlanning.PathSmoothing.Facade;
using MultiWheelC.TrajectoryPlanning.PathSmoothing.LocalG2;
using MultiWheelC.TrajectoryPlanning.PathSmoothing.Processing;
using MultiWheelC.TrajectoryPlanning.PathSmoothing.Output.Visualization;
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Test;
public sealed class LocalG2DiagnosticVisualizationDemo
{
private readonly LocalG2DiagnosticEvidenceLoader _evidenceLoader = new LocalG2DiagnosticEvidenceLoader();
private readonly PathSmoothingComparisonService _comparisonService = new PathSmoothingComparisonService();
private readonly PathSmoothingPreprocessor _preprocessor = new PathSmoothingPreprocessor();
private readonly LocalG2PathSplicer _splicer = new LocalG2PathSplicer();
private readonly PathGeometryAnalyzer _analyzer = new PathGeometryAnalyzer();
private readonly SmoothingFigureModelBuilder _figureBuilder = new SmoothingFigureModelBuilder();
private readonly SmoothingReportExporter _exporter = new SmoothingReportExporter();
public SmoothingReportExportResult Export(
string fixturePath,
string evidencePath,
string outputDirectory,
CancellationToken cancellationToken = default)
{
LocalG2DiagnosticEvidence evidence = _evidenceLoader.LoadAndVerify(evidencePath);
RequireFixtureHash(fixturePath, evidence.FixtureSha256);
PathSmoothingComparisonRequest request = FindFixtureRequest(fixturePath, evidence.ScenarioId);
PathSmoothingComparisonResult comparison = _comparisonService.Compare(request, cancellationToken);
PathSmoothingComparisonEntry localG2 = FindEntry(comparison, SmoothingMethod.LocalG2Quintic);
Require(localG2 != null && localG2.Status == PathSmoothingStatus.Unchanged,
"Strict Local G2 result must be Unchanged for the diagnostic evidence.");
RequireSameGeometry(comparison.RawPathBaseline.Path, localG2.Path);
Require(_preprocessor.TryPrepare(request.SmoothingRequest, out PreparedPath prepared, out string reason), reason);
LocalG2CandidateGeometry candidate = CreateCandidate(evidence);
Require(_splicer.TryReplace(prepared, candidate, out PreparedPath spliced, out reason), reason);
Require(_analyzer.TryAnalyze(spliced.Segments, request.SmoothingRequest.Configuration.OutputSpacingMeters,
out PathGeometryAnalysis analysis, out reason), reason);
CoarsePathPoint first = request.SmoothingRequest.CoarsePath[0];
CoarsePathPoint last = request.SmoothingRequest.CoarsePath[request.SmoothingRequest.CoarsePath.Count - 1];
SmoothingFigureModel normal = _figureBuilder.Build(
comparison,
request.SmoothingRequest.Map,
new Pose2D(first.X, first.Y, first.Heading),
new Pose2D(last.X, last.Y, last.Heading),
evidence.ScenarioId,
evidence.ScenarioId);
SmoothingFigureModel augmented = normal.WithAdditionalSeries(CreateDiagnosticSeries(analysis.Path));
return _exporter.Export(new SmoothingReportExportRequest
{
Model = augmented,
OutputDirectory = outputDirectory,
FileStem = "comparison",
});
}
private static void RequireFixtureHash(string fixturePath, string expectedFixtureSha256)
{
byte[] fixtureBytes = File.ReadAllBytes(fixturePath);
byte[] hash;
using (SHA256 sha256 = SHA256.Create())
hash = sha256.ComputeHash(fixtureBytes);
string actualFixtureSha256 = BitConverter.ToString(hash).Replace("-", string.Empty).ToLowerInvariant();
Require(string.Equals(actualFixtureSha256, expectedFixtureSha256, StringComparison.Ordinal),
"Diagnostic evidence fixture SHA-256 does not match the supplied fixture.");
}
private static PathSmoothingComparisonRequest FindFixtureRequest(string fixturePath, string scenarioId)
{
IReadOnlyList<SmoothingScenarioFixture> fixtures = SmoothingScenarioFixtureLoader.LoadAndVerify(fixturePath);
IReadOnlyList<PathSmoothingComparisonRequest> requests = SmoothingScenarioFactory.CreateFixtureRequests(fixturePath);
Require(fixtures.Count == requests.Count, "Fixture requests do not match fixture records.");
int fixtureIndex = -1;
for (int index = 0; index < fixtures.Count; index++)
{
if (!string.Equals(fixtures[index].Id, scenarioId, StringComparison.Ordinal)) continue;
Require(fixtureIndex < 0, "Diagnostic fixture ID must be unique: " + scenarioId);
fixtureIndex = index;
}
Require(fixtureIndex >= 0, "Diagnostic fixture ID was not found: " + scenarioId);
return requests[fixtureIndex];
}
private static PathSmoothingComparisonEntry FindEntry(PathSmoothingComparisonResult comparison, SmoothingMethod method)
{
if (comparison == null) return null;
for (int index = 0; index < comparison.Entries.Count; index++)
{
if (comparison.Entries[index].Method == method) return comparison.Entries[index];
}
return null;
}
private static LocalG2CandidateGeometry CreateCandidate(LocalG2DiagnosticEvidence evidence)
{
Require(evidence != null && evidence.CandidatePoints != null, "Diagnostic evidence is required.");
var points = new List<SmoothingPoint2D>(evidence.CandidatePoints.Count);
for (int index = 0; index < evidence.CandidatePoints.Count; index++)
{
LocalG2DiagnosticEvidencePoint point = evidence.CandidatePoints[index];
points.Add(new SmoothingPoint2D(
point.X.Value, point.Y.Value, point.ReferenceArcLengthMeters.Value,
point.HeadingRadians.Value, point.UnwrappedHeadingRadians.Value,
0d, false, SmoothedPathPointSource.LocalG2Transition));
}
return new LocalG2CandidateGeometry(
evidence.CandidateIndex.Value,
evidence.SegmentIndex.Value,
evidence.WindowStartArcLengthMeters.Value,
evidence.WindowEndArcLengthMeters.Value,
0d,
0d,
points,
evidence.StartVehicleCurvaturePerMeter.Value,
evidence.EndVehicleCurvaturePerMeter.Value,
evidence.StartGeometricCurvaturePerMeter.Value,
evidence.EndGeometricCurvaturePerMeter.Value,
true);
}
private static SmoothingFigureSeries CreateDiagnosticSeries(IReadOnlyList<SmoothedPathPoint> path)
{
var points = new List<SmoothingFigurePoint>(path == null ? 0 : path.Count);
if (path != null)
{
for (int index = 0; index < path.Count; index++)
{
SmoothedPathPoint point = path[index];
points.Add(new SmoothingFigurePoint(point.X, point.Y, point.ArcLength, point.VehicleCurvature));
}
}
return new SmoothingFigureSeries(
null,
"local-g2-diagnostic",
"G2 诊断候选（净空拒绝，未发布）",
PathSmoothingStatus.Infeasible,
IeeeFigureStyle.LocalG2DiagnosticColor,
string.Empty,
false,
points,
Array.Empty<SmoothingFigurePoint>());
}
private static void RequireSameGeometry(IReadOnlyList<SmoothedPathPoint> expected, IReadOnlyList<SmoothedPathPoint> actual)
{
Require(expected != null && actual != null && expected.Count == actual.Count,
"Strict Local G2 output must retain raw-path geometry.");
for (int index = 0; index < expected.Count; index++)
{
SmoothedPathPoint left = expected[index];
SmoothedPathPoint right = actual[index];
Require(left.X == right.X && left.Y == right.Y && left.Heading == right.Heading &&
left.UnwrappedHeading == right.UnwrappedHeading && left.ArcLength == right.ArcLength &&
left.Direction == right.Direction && left.GeometricCurvature == right.GeometricCurvature &&
left.VehicleCurvature == right.VehicleCurvature && left.VehicleCurvatureDerivative == right.VehicleCurvatureDerivative &&
left.BodyClearance == right.BodyClearance && left.IsGearSwitchPoint == right.IsGearSwitchPoint && left.Source == right.Source,
"Strict Local G2 output must retain raw-path geometry.");
}
}
private static void Require(bool condition, string reason)
{
if (!condition) throw new InvalidOperationException(reason);
}
}
ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Test/PathSmoothingComparisonDemo.cs
+164
View File
@@ -0,0 +1,164 @@
using System;
using System.Collections.Generic;
using System.Threading;
using MultiWheelC.TrajectoryPlanning.CoarsePath;
using MultiWheelC.TrajectoryPlanning.CoarsePath.Facade;
using MultiWheelC.TrajectoryPlanning.CoarsePath.Test;
using MultiWheelC.TrajectoryPlanning.PathSmoothing.Output.Comparison;
using MultiWheelC.TrajectoryPlanning.PathSmoothing.Facade;
using MultiWheelC.TrajectoryPlanning.PathSmoothing.Output.Visualization;
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Test;
/// <summary>开发人员离线导出八个快速夹具和四个 Hybrid A* 端到端场景的比较报告。</summary>
public sealed class PathSmoothingComparisonDemo
{
private readonly PathSmoothingService _smoothingService = new PathSmoothingService();
private readonly PathSmoothingComparisonService _comparisonService = new PathSmoothingComparisonService();
private readonly SmoothingFigureModelBuilder _figureBuilder = new SmoothingFigureModelBuilder();
private readonly SmoothingReportExporter _reportExporter = new SmoothingReportExporter();
/// <summary>导出八个已验证夹具；不运行 Hybrid A*。</summary>
public IReadOnlyList<PathSmoothingComparisonScenarioResult> ExportFixtureReports(
string fixturePath,
string outputDirectory,
CancellationToken cancellationToken = default)
{
IReadOnlyList<SmoothingScenarioFixture> fixtures = SmoothingScenarioFixtureLoader.LoadAndVerify(fixturePath);
IReadOnlyList<PathSmoothingComparisonRequest> requests = SmoothingScenarioFactory.CreateFixtureRequests(fixturePath);
var results = new List<PathSmoothingComparisonScenarioResult>(requests.Count);
for (int index = 0; index < requests.Count; index++)
{
cancellationToken.ThrowIfCancellationRequested();
results.Add(Export(fixtures[index].Id, requests[index], outputDirectory, cancellationToken));
}
return results.AsReadOnly();
}
/// <summary>运行四个固定 Hybrid A* 场景，并仅为成功粗路径导出平滑比较报告。</summary>
public IReadOnlyList<PathSmoothingComparisonScenarioResult> ExportEndToEndReports(
string outputDirectory,
CancellationToken cancellationToken = default)
{
CoarsePathTestScenario[] scenarios =
{
CoarsePathTestScenario.ExplicitEmpty,
CoarsePathTestScenario.RectangleDetour,
CoarsePathTestScenario.ManualAndTwoLeg,
CoarsePathTestScenario.ReverseGearSwitch,
};
var planner = new CoarsePathPlanningService();
var results = new List<PathSmoothingComparisonScenarioResult>(scenarios.Length);
for (int index = 0; index < scenarios.Length; index++)
{
cancellationToken.ThrowIfCancellationRequested();
CoarsePathPlanningJob job = CoarsePathScenarioFactory.Create(scenarios[index]);
CoarsePathPlanningJobResult coarse = planner.Plan(job, cancellationToken);
if (coarse.PlanningResult.Status != PlanningStatus.Success)
{
results.Add(PathSmoothingComparisonScenarioResult.CoarsePathFailure(
scenarios[index].ToString(), coarse.PlanningResult.Status, coarse.PlanningResult.Diagnostics.TerminationReason));
continue;
}
results.Add(Export(
scenarios[index].ToString(),
SmoothingScenarioFactory.CreateEndToEndRequest(job, coarse),
outputDirectory,
cancellationToken));
}
return results.AsReadOnly();
}
/// <summary>供业务调用示例使用：仅在已成功的粗路径上调用平滑服务。</summary>
public PathSmoothingResult SmoothSuccessfulCoarsePath(
CoarsePathPlanningJob job,
CoarsePathPlanningJobResult coarseResult,
PathSmoothingConfiguration configuration,
CancellationToken cancellationToken = default)
{
if (job == null || coarseResult == null || coarseResult.PlanningResult.Status != PlanningStatus.Success)
throw new ArgumentException("只有成功的粗路径能够进入平滑流程。", nameof(coarseResult));
return _smoothingService.Smooth(new PathSmoothingRequest(
coarseResult.PlanningResult.Path,
coarseResult.PlanningResult.Segments,
coarseResult.MapResult.Map,
job.Vehicle,
configuration), cancellationToken);
}
/// <summary>业务调用方只可消费成功平滑或显式回退的路径。</summary>
public static bool IsConsumable(PathSmoothingStatus status)
{
return status == PathSmoothingStatus.Complete ||
status == PathSmoothingStatus.PartialImprovement ||
status == PathSmoothingStatus.NotNeeded ||
status == PathSmoothingStatus.Unchanged;
}
private PathSmoothingComparisonScenarioResult Export(
string scenarioId,
PathSmoothingComparisonRequest request,
string outputDirectory,
CancellationToken cancellationToken)
{
if (request == null || request.SmoothingRequest == null || request.SmoothingRequest.CoarsePath.Count == 0)
throw new ArgumentException("比较请求必须包含粗路径。", nameof(request));
PathSmoothingComparisonResult comparison = _comparisonService.Compare(request, cancellationToken);
CoarsePathPoint first = request.SmoothingRequest.CoarsePath[0];
CoarsePathPoint last = request.SmoothingRequest.CoarsePath[request.SmoothingRequest.CoarsePath.Count - 1];
SmoothingFigureModel model = _figureBuilder.Build(
comparison,
request.SmoothingRequest.Map,
new Pose2D(first.X, first.Y, first.Heading),
new Pose2D(last.X, last.Y, last.Heading),
scenarioId,
scenarioId);
SmoothingReportExportResult report = _reportExporter.Export(new SmoothingReportExportRequest
{
Model = model,
OutputDirectory = System.IO.Path.Combine(outputDirectory, scenarioId),
FileStem = "comparison",
});
return PathSmoothingComparisonScenarioResult.ComparisonComplete(scenarioId, comparison, report);
}
}
/// <summary>批处理为一个场景发布的粗路径状态、平滑比较和报告结果。</summary>
public sealed class PathSmoothingComparisonScenarioResult
{
private PathSmoothingComparisonScenarioResult(
string scenarioId,
PlanningStatus coarsePathStatus,
string diagnostic,
PathSmoothingComparisonResult comparison,
SmoothingReportExportResult report)
{
ScenarioId = scenarioId ?? string.Empty;
CoarsePathStatus = coarsePathStatus;
Diagnostic = diagnostic ?? string.Empty;
Comparison = comparison;
Report = report;
}
public string ScenarioId { get; }
public PlanningStatus CoarsePathStatus { get; }
public string Diagnostic { get; }
public PathSmoothingComparisonResult Comparison { get; }
public SmoothingReportExportResult Report { get; }
internal static PathSmoothingComparisonScenarioResult CoarsePathFailure(
string scenarioId,
PlanningStatus status,
string diagnostic)
{
return new PathSmoothingComparisonScenarioResult(scenarioId, status, diagnostic, null, null);
}
internal static PathSmoothingComparisonScenarioResult ComparisonComplete(
string scenarioId,
PathSmoothingComparisonResult comparison,
SmoothingReportExportResult report)
{
return new PathSmoothingComparisonScenarioResult(scenarioId, PlanningStatus.Success, string.Empty, comparison, report);
}
}
ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Test/SmoothingScenarioFactory.cs
+1 -1
View File
@@ -3,7 +3,7 @@ using System.Collections.Generic;
using System.Threading;
using MultiWheelC.TrajectoryPlanning.CoarsePath;
using MultiWheelC.TrajectoryPlanning.CoarsePath.Facade;
using MultiWheelC.TrajectoryPlanning.PathSmoothing.Comparison;
using MultiWheelC.TrajectoryPlanning.PathSmoothing.Output.Comparison;
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Test;
ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Test/SmoothingScenarioFixtureLoader.cs
+16 -2
View File
@@ -239,6 +239,20 @@ public static class SmoothingScenarioFixtureLoader
private static void Append(StringBuilder builder, string value) { builder.Append(value ?? string.Empty).Append('|'); }
private static void Append(StringBuilder builder, int value) { builder.Append(value.ToString(CultureInfo.InvariantCulture)).Append('|'); }
private static void Append(StringBuilder builder, float value) { builder.Append(value.ToString("R", CultureInfo.InvariantCulture)).Append('|'); }
private static void Append(StringBuilder builder, double value) { builder.Append(value.ToString("R", CultureInfo.InvariantCulture)).Append('|'); }
private static void Append(StringBuilder builder, float value)
{
AppendBits(builder, BitConverter.GetBytes(value));
}
private static void Append(StringBuilder builder, double value)
{
AppendBits(builder, BitConverter.GetBytes(value));
}
private static void AppendBits(StringBuilder builder, byte[] bytes)
{
for (int index = bytes.Length - 1; index >= 0; index--)
builder.Append(bytes[index].ToString("x2", CultureInfo.InvariantCulture));
builder.Append('|');
}
}