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

feat: preserve EM planner segment boundaries

Browse Source
This commit is contained in:
梁薄云
2026-08-03 22:27:00 +08:00
parent b326431d63
commit e492e1610a
8 changed files with 407 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files
ClumsyPilot/tests/EMPlannerVerificationHost/SegmentationChecks.cs
+46
View File
@@ -0,0 +1,46 @@
using MultiWheelC.TrajectoryPlanning.EMPlanner;
namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
internal static class SegmentationChecks
{
public static void Run()
{
var referencePath = EMPlannerVerificationHost.EmFixtureFactory.CreateGearPairReferencePath();
var segments = ReferencePathSegmenter.Create(referencePath);
EMPlannerVerificationHost.Verification.Equal(2, segments.Count, "segment count");
EMPlannerVerificationHost.Verification.Equal(EmBoundaryType.GearSwitchApproach,
segments[0].EndBoundary.BoundaryType, "forward end boundary");
EMPlannerVerificationHost.Verification.Equal(EmBoundaryType.GearSwitchDeparture,
segments[1].StartBoundary.BoundaryType, "reverse start boundary");
EMPlannerVerificationHost.Verification.True(!segments[0].EndBoundary.Equals(segments[1].StartBoundary),
"gear-pair boundary identities differ");
EMPlannerVerificationHost.Verification.NearlyEqual(2d, referencePath.Path[2].ArcLength, "forward source arc length");
EMPlannerVerificationHost.Verification.NearlyEqual(2d, referencePath.Path[3].ArcLength, "reverse source arc length");
EMPlannerVerificationHost.Verification.NearlyEqual(0d, segments[1].Points[0].ArcLength, "rebased reverse start");
ReferenceHorizonSlice rolling = ReferenceHorizonSlicer.Slice(segments[0], 1.95d);
EMPlannerVerificationHost.Verification.Equal(EmBoundaryType.RollingSafetyStop,
rolling.TerminalBoundary.BoundaryType, "rolling terminal type");
EMPlannerVerificationHost.Verification.NearlyEqual(1.95d, rolling.TerminalBoundary.SegmentLocalS,
"rolling terminal local s");
EMPlannerVerificationHost.Verification.NearlyEqual(1.95d, rolling.Points[rolling.Points.Count - 1].ArcLength,
"rolling anchor path s");
EMPlannerVerificationHost.Verification.NearlyEqual(1.95d, rolling.Points[rolling.Points.Count - 1].X,
"rolling anchor x");
ReferenceHorizonSlice gearSwitch = ReferenceHorizonSlicer.Slice(segments[0], 2.05d);
EMPlannerVerificationHost.Verification.Equal(EmBoundaryType.GearSwitchApproach,
gearSwitch.TerminalBoundary.BoundaryType, "gear terminal type");
EMPlannerVerificationHost.Verification.NearlyEqual(2d, gearSwitch.TerminalBoundary.SegmentLocalS,
"gear terminal local s");
EMPlannerVerificationHost.Verification.NearlyEqual(2d, gearSwitch.Points[gearSwitch.Points.Count - 1].ArcLength,
"gear anchor path s");
for (int index = 0; index < gearSwitch.Points.Count; index++)
{
EMPlannerVerificationHost.Verification.Equal(segments[0].Direction, gearSwitch.Points[index].Direction,
"horizon point direction");
}
}
}