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

feat: separate rolling horizons from stop boundaries

Browse Source
This commit is contained in:
梁薄云
2026-08-05 17:13:09 +08:00
parent e56220fc29
commit 21b20d09d4
9 changed files with 246 additions and 96 deletions
Download Patch File Download Diff File Expand all files Collapse all files
ClumsyPilot/ParkrobTrajplanner/EMPlanner/Facade/EmPlanningService.cs
+4 -3
View File
@@ -58,11 +58,11 @@ public sealed class EmPlanningService : IEmPlanningService
initialAcceleration, configuration, out PlanningHorizonSelection horizon, out string horizonReason);
if (horizonStatus != EmPlanningStatus.Success)
return Failure(horizonStatus, request, horizonReason);
ReferenceHorizonSlice slice = ReferenceHorizonSlicer.Slice(segment, horizon.TerminalReferenceS);
ReferenceHorizonSlice slice = ReferenceHorizonSlicer.Slice(segment, horizon.WindowEndReferenceS);
EmitDebug(request, "exact horizon and terminal selection succeeded");
IReadOnlyList<FrenetProjection> previousSeed = ProjectPreviousTrajectorySeed(request.PreviousTrajectory, segment,
startProjection.ReferenceS, horizon.TerminalReferenceS, configuration.Frenet.MaximumProjectionDistanceMeters);
startProjection.ReferenceS, horizon.WindowEndReferenceS, configuration.Frenet.MaximumProjectionDistanceMeters);
var corridorSeed = new List<FrenetProjection>(previousSeed.Count + 1) { startProjection };
for (int index = 0; index < previousSeed.Count; index++) corridorSeed.Add(previousSeed[index]);
EmitDebug(request, "previous-trajectory seed projection completed");
@@ -83,7 +83,8 @@ public sealed class EmPlanningService : IEmPlanningService
EmitDebug(request, "LS optimization and validation succeeded");
var longitudinalInput = new LongitudinalPlanningInput(lateral.Path, segment.Direction, initialProgressSpeed,
initialAcceleration, horizon.TerminalType, configuration, Array.Empty<double>(), Array.Empty<double>());
initialAcceleration, horizon.TerminalType, horizon.LongitudinalMode, configuration,
Array.Empty<double>(), Array.Empty<double>());
EmPlanningStatus envelopeStatus = new PathSpeedLimitBuilder().Build(longitudinalInput, out _, out string envelopeReason);
if (envelopeStatus != EmPlanningStatus.Success)
return Failure(envelopeStatus, request, envelopeReason);