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

test: cover rolling-to-stop EM planning flow

Browse Source
This commit is contained in:
梁薄云
2026-08-05 21:17:53 +08:00
parent 2eb7902bc3
commit eb050b19e4
3 changed files with 111 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files
ClumsyPilot/tests/EMPlannerVerificationHost/EmFixtureFactory.cs
+10 -7
View File
@@ -7,23 +7,26 @@ namespace EMPlannerVerificationHost;
internal static class EmFixtureFactory
{
public static PathSmoothingResult CreateGearPairReferencePath()
public static PathSmoothingResult CreateGearPairReferencePath(double firstSegmentLength = 2d)
{
if (firstSegmentLength <= 0d)
throw new ArgumentOutOfRangeException(nameof(firstSegmentLength));
double midpoint = 0.5d * firstSegmentLength;
var points = new List<SmoothedPathPoint>
{
Point(0d, 0d, TravelDirection.Forward, false, SmoothedPathPointSource.Anchor),
Point(1d, 1d, TravelDirection.Forward, false, SmoothedPathPointSource.Anchor),
Point(2d, 2d, TravelDirection.Forward, false, SmoothedPathPointSource.Anchor),
Point(2d, 2d, TravelDirection.Reverse, true, SmoothedPathPointSource.GearSwitch),
Point(1d, 3d, TravelDirection.Reverse, false, SmoothedPathPointSource.Anchor),
Point(0d, 4d, TravelDirection.Reverse, false, SmoothedPathPointSource.Anchor),
Point(midpoint, midpoint, TravelDirection.Forward, false, SmoothedPathPointSource.Anchor),
Point(firstSegmentLength, firstSegmentLength, TravelDirection.Forward, false, SmoothedPathPointSource.Anchor),
Point(firstSegmentLength, firstSegmentLength, TravelDirection.Reverse, true, SmoothedPathPointSource.GearSwitch),
Point(midpoint, firstSegmentLength + midpoint, TravelDirection.Reverse, false, SmoothedPathPointSource.Anchor),
Point(0d, 2d * firstSegmentLength, TravelDirection.Reverse, false, SmoothedPathPointSource.Anchor),
};
var segments = new List<SmoothedPathSegment>
{
new SmoothedPathSegment(0, TravelDirection.Forward, 0, 2, false, true),
new SmoothedPathSegment(1, TravelDirection.Reverse, 3, 5, true, false),
};
var metrics = new PathQualityMetrics(true, 4d, 0d, 0d, 0d, 0d, 1d, 0d, 0d, 0d, 0d, 0d);
var metrics = new PathQualityMetrics(true, 2d * firstSegmentLength, 0d, 0d, 0d, 0d, 1d, 0d, 0d, 0d, 0d, 0d);
return PathSmoothingResult.PublishLocalG2(PathSmoothingStatus.Complete, points, segments,
new PathSmoothingDiagnostics(metrics, TimeSpan.Zero), new List<PathSmoothingRegionReport>());
}