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

feat: select complete EM direction segments

Browse Source
This commit is contained in:
梁薄云
2026-08-06 20:55:47 +08:00
parent f4e89b4b4f
commit 048b4f618e
5 changed files with 74 additions and 11 deletions
Download Patch File Download Diff File Expand all files Collapse all files
ClumsyPilot/ParkrobTrajplanner/EMPlanner/Facade/EmPlanningService.cs
+3 -2
View File
@@ -55,7 +55,8 @@ public sealed class EmPlanningService : IEmPlanningService
double initialAcceleration = request.VehicleState.LongitudinalAccelerationMetersPerSecondSquared ?? 0d;
var horizonSelector = new PlanningHorizonSelector();
EmPlanningStatus horizonStatus = horizonSelector.Select(segment, startProjection.ReferenceS, initialProgressSpeed,
initialAcceleration, configuration, out PlanningHorizonSelection horizon, out string horizonReason);
initialAcceleration, request.PlanningScope, configuration, out PlanningHorizonSelection horizon,
out string horizonReason);
if (horizonStatus != EmPlanningStatus.Success)
return Failure(horizonStatus, request, horizonReason);
ReferenceHorizonSlice slice = ReferenceHorizonSlicer.Slice(segment, horizon.WindowEndReferenceS);
@@ -212,6 +213,6 @@ public sealed class EmPlanningService : IEmPlanningService
";reference=" + (request.ReferencePathId ?? string.Empty) +
";state=" + (request.VehicleState == null ? string.Empty : request.VehicleState.SequenceId.ToString()) +
";previous=" + (request.PreviousTrajectoryId ?? string.Empty) +
";segment=" + request.SegmentIndex;
";segment=" + request.SegmentIndex + ";scope=" + request.PlanningScope;
}
}
ClumsyPilot/ParkrobTrajplanner/EMPlanner/Segmentation/PlanningHorizonSelector.cs
+18 -2
View File
@@ -40,7 +40,8 @@ public sealed class PlanningHorizonSelector
public EmPlanningStatus Select(DirectionSegmentView segment, double currentSegmentReferenceS,
double initialProgressSpeedMetersPerSecond, double initialAccelerationMetersPerSecondSquared,
EmPlannerConfiguration configuration, out PlanningHorizonSelection selection, out string failureReason)
EmPlanningScope planningScope, EmPlannerConfiguration configuration, out PlanningHorizonSelection selection,
out string failureReason)
{
selection = null;
failureReason = string.Empty;
@@ -48,7 +49,8 @@ public sealed class PlanningHorizonSelector
!IsFinite(currentSegmentReferenceS) || currentSegmentReferenceS < 0d ||
currentSegmentReferenceS > segment.LengthMeters + BoundaryTolerance ||
!IsFinite(initialProgressSpeedMetersPerSecond) || initialProgressSpeedMetersPerSecond < 0d ||
!IsFinite(initialAccelerationMetersPerSecondSquared))
!IsFinite(initialAccelerationMetersPerSecondSquared) ||
!Enum.IsDefined(typeof(EmPlanningScope), planningScope))
{
failureReason = "Planning horizon inputs are invalid.";
return EmPlanningStatus.InvalidInput;
@@ -90,6 +92,20 @@ public sealed class PlanningHorizonSelector
return EmPlanningStatus.StoppingDistanceInsufficient;
}
if (planningScope == EmPlanningScope.FullDirectionSegment)
{
EmBoundaryType boundary = segment.EndBoundary.BoundaryType;
if (!IsStopBoundary(boundary))
{
failureReason = "A full direction segment must end at Goal or GearSwitch.";
return EmPlanningStatus.InvalidReferencePath;
}
selection = new PlanningHorizonSelection(segment.LengthMeters, boundary,
ToTerminalType(boundary), EmLongitudinalMode.ExactStopAtBoundary,
segment.LengthMeters, true);
return EmPlanningStatus.Success;
}
double windowEnd = Math.Min(currentSegmentReferenceS + scheduling.DistanceHorizonMeters, segment.LengthMeters);
bool windowReachesSegmentEnd = windowEnd >= segment.LengthMeters - BoundaryTolerance;
bool hasStopBoundary = windowReachesSegmentEnd && IsStopBoundary(segment.EndBoundary.BoundaryType);