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

feat: validate EM terminal world pose

Browse Source
This commit is contained in:
梁薄云
2026-08-07 08:01:18 +08:00
parent 0bba8d7e61
commit e844fe00a8
6 changed files with 249 additions and 11 deletions
Download Patch File Download Diff File Expand all files Collapse all files
ClumsyPilot/ParkrobTrajplanner/EMPlanner/Trajectory/EmTrajectoryAssembler.cs
+28 -4
View File
@@ -9,6 +9,7 @@ public sealed class EmTrajectoryAssembler
{
private readonly double outputTimeStepSeconds;
private readonly double zeroSpeedHoldSeconds;
private readonly int maximumPublishedSampleCount;
public EmTrajectoryAssembler()
: this(EmPlannerConfiguration.CreateDefault())
@@ -21,8 +22,9 @@ public sealed class EmTrajectoryAssembler
throw new ArgumentNullException(nameof(configuration));
outputTimeStepSeconds = configuration.Scheduling.OutputTimeStepSeconds;
zeroSpeedHoldSeconds = configuration.Longitudinal.ZeroSpeedHoldSeconds;
maximumPublishedSampleCount = configuration.Scheduling.MaximumPublishedSampleCount;
if (!IsFinite(outputTimeStepSeconds) || outputTimeStepSeconds <= 0d || !IsFinite(zeroSpeedHoldSeconds) ||
zeroSpeedHoldSeconds < 0d)
zeroSpeedHoldSeconds < 0d || maximumPublishedSampleCount < 2)
{
throw new ArgumentOutOfRangeException(nameof(configuration));
}
@@ -30,6 +32,18 @@ public sealed class EmTrajectoryAssembler
public EmTrajectory Assemble(LateralPath path, LongitudinalPlanningResult longitudinal, EmTrajectoryMetadata metadata)
{
EmPlanningStatus status = TryAssemble(path, longitudinal, metadata, out EmTrajectory trajectory,
out string failureReason);
if (status != EmPlanningStatus.Success)
throw new ArgumentException(failureReason, nameof(longitudinal));
return trajectory;
}
public EmPlanningStatus TryAssemble(LateralPath path, LongitudinalPlanningResult longitudinal,
EmTrajectoryMetadata metadata, out EmTrajectory trajectory, out string failureReason)
{
trajectory = null;
failureReason = string.Empty;
if (longitudinal == null || longitudinal.Candidate == null ||
(longitudinal.Status != EmPlanningStatus.Success && longitudinal.Status != EmPlanningStatus.SuccessWithFallback))
{
@@ -40,8 +54,17 @@ public sealed class EmTrajectoryAssembler
var interpolator = new LateralPathInterpolator(path);
bool isFullDirectionSegment = metadata.PlanningScope == EmPlanningScope.FullDirectionSegment;
var schedule = new TrajectorySampleSchedule(longitudinal.Candidate, outputTimeStepSeconds,
isFullDirectionSegment ? 0d : zeroSpeedHoldSeconds, metadata.LongitudinalMode, isFullDirectionSegment);
double holdDurationSeconds = isFullDirectionSegment ? 0d : zeroSpeedHoldSeconds;
if (TrajectorySampleSchedule.ExceedsMaximumSampleCount(longitudinal.Candidate, outputTimeStepSeconds,
holdDurationSeconds, metadata.LongitudinalMode, isFullDirectionSegment, maximumPublishedSampleCount,
out int requiredSampleCount))
{
failureReason = "FullSegmentResourceLimitExceeded: MaximumPublishedSampleCount=" +
maximumPublishedSampleCount + ";required-at-least=" + requiredSampleCount;
return EmPlanningStatus.FullSegmentResourceLimitExceeded;
}
var schedule = new TrajectorySampleSchedule(longitudinal.Candidate, outputTimeStepSeconds, holdDurationSeconds,
metadata.LongitudinalMode, isFullDirectionSegment);
double terminalPathS = path.Points[path.Points.Count - 1].PathS;
var points = new List<EmTrajectoryPoint>(schedule.Samples.Count);
double directionSign = metadata.Direction == TravelDirection.Forward ? 1d : -1d;
@@ -62,7 +85,8 @@ public sealed class EmTrajectoryAssembler
boundaryType, sample.Acceleration, sample.Jerk));
}
return new EmTrajectory(metadata, points);
trajectory = new EmTrajectory(metadata, points);
return EmPlanningStatus.Success;
}
private static EmBoundaryType ToBoundaryType(EmTerminalType terminalType)