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

feat: derive adaptive full-segment ST schedule

Browse Source
This commit is contained in:
梁薄云
2026-08-06 23:54:34 +08:00
parent dad4ff4b04
commit 3269d556b6
14 changed files with 1642 additions and 108 deletions
Download Patch File Download Diff File Expand all files Collapse all files
ClumsyPilot/ParkrobTrajplanner/EMPlanner/Facade/EmPlanningService.cs
+29 -4
View File
@@ -83,16 +83,34 @@ public sealed class EmPlanningService : IEmPlanningService
return Failure(lateral.Status, request, lateral.FailureReason);
EmitDebug(request, "LS optimization and validation succeeded");
IReadOnlyList<double> knotTimes = LongitudinalCandidate.CreateKnotTimes(
configuration.Scheduling.TimeHorizonSeconds, configuration.Scheduling.OutputTimeStepSeconds);
EmPlanningStatus envelopeStatus = new PathSpeedLimitBuilder().Build(lateral.Path, segment.Direction,
initialProgressSpeed, horizon.TerminalType, configuration, out PathSpeedLimit speedLimit,
out string envelopeReason);
if (envelopeStatus != EmPlanningStatus.Success)
return Failure(envelopeStatus, request, envelopeReason);
LongitudinalKnotSchedule knotSchedule;
if (request.PlanningScope == EmPlanningScope.FullDirectionSegment)
{
EmPlanningStatus scheduleStatus = new FullDirectionSegmentScheduleBuilder().TryBuild(lateral.Path, speedLimit,
initialProgressSpeed, initialAcceleration, DesiredSpeed(configuration, segment.Direction), configuration,
out knotSchedule, out string scheduleReason);
if (scheduleStatus != EmPlanningStatus.Success)
return Failure(scheduleStatus, request, scheduleReason);
}
else
{
knotSchedule = LongitudinalKnotSchedule.CreateRolling(configuration.Scheduling.TimeHorizonSeconds,
configuration.Scheduling.OutputTimeStepSeconds);
}
LongitudinalPreviousTrajectorySeed previousLongitudinalSeed =
new LongitudinalPreviousTrajectorySeedBuilder().Build(
request.PreviousTrajectory, lateral.Path, request.EffectiveAtUtc, knotTimes,
request.PreviousTrajectory, lateral.Path, request.EffectiveAtUtc, knotSchedule,
segment.SegmentIndex, segment.Direction);
var longitudinalInput = new LongitudinalPlanningInput(lateral.Path, segment.Direction, initialProgressSpeed,
initialAcceleration, horizon.TerminalType, horizon.LongitudinalMode, configuration,
request.PlanningScope, knotSchedule,
previousLongitudinalSeed.PathS, previousLongitudinalSeed.ProgressSpeedMetersPerSecond);
EmPlanningStatus envelopeStatus = new PathSpeedLimitBuilder().Build(longitudinalInput, out _, out string envelopeReason);
envelopeStatus = new PathSpeedLimitBuilder().Build(longitudinalInput, out _, out envelopeReason);
if (envelopeStatus != EmPlanningStatus.Success)
return Failure(envelopeStatus, request, envelopeReason);
EmitDebug(request, "PathS speed envelope succeeded");
@@ -195,6 +213,13 @@ public sealed class EmPlanningService : IEmPlanningService
: state.SignedLongitudinalSpeedMetersPerSecond < 0d;
}
private static double DesiredSpeed(EmPlannerConfiguration configuration, TravelDirection direction)
{
return direction == TravelDirection.Forward
? configuration.Longitudinal.DesiredForwardSpeedMetersPerSecond
: configuration.Longitudinal.DesiredReverseSpeedMetersPerSecond;
}
private static bool IsSuccess(EmPlanningStatus status)
{
return status == EmPlanningStatus.Success || status == EmPlanningStatus.SuccessWithFallback;