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

fix: normalize EM PathS resampling residuals

Browse Source
This commit is contained in:
梁薄云
2026-08-10 14:24:15 +08:00
parent 8a1ba2a6d5
commit 06d9037645
2 changed files with 43 additions and 11 deletions
Download Patch File Download Diff File Expand all files Collapse all files
ClumsyPilot/ParkrobTrajplanner/EMPlanner/Trajectory/EmTrajectoryAssembler.cs
+7 -3
View File
@@ -9,6 +9,7 @@ public sealed class EmTrajectoryAssembler
{
private readonly double outputTimeStepSeconds;
private readonly double zeroSpeedHoldSeconds;
private readonly double pathSTolerance;
private readonly int maximumPublishedSampleCount;
public EmTrajectoryAssembler()
@@ -18,13 +19,16 @@ public sealed class EmTrajectoryAssembler
public EmTrajectoryAssembler(EmPlannerConfiguration configuration)
{
if (configuration == null || configuration.Scheduling == null || configuration.Longitudinal == null)
if (configuration == null || configuration.Scheduling == null || configuration.Longitudinal == null ||
configuration.Validation == null)
throw new ArgumentNullException(nameof(configuration));
outputTimeStepSeconds = configuration.Scheduling.OutputTimeStepSeconds;
zeroSpeedHoldSeconds = configuration.Longitudinal.ZeroSpeedHoldSeconds;
pathSTolerance = configuration.Validation.KinematicTolerance;
maximumPublishedSampleCount = configuration.Scheduling.MaximumPublishedSampleCount;
if (!IsFinite(outputTimeStepSeconds) || outputTimeStepSeconds <= 0d || !IsFinite(zeroSpeedHoldSeconds) ||
zeroSpeedHoldSeconds < 0d || maximumPublishedSampleCount < 2)
zeroSpeedHoldSeconds < 0d || !IsFinite(pathSTolerance) || pathSTolerance < 0d ||
maximumPublishedSampleCount < 2)
{
throw new ArgumentOutOfRangeException(nameof(configuration));
}
@@ -64,7 +68,7 @@ public sealed class EmTrajectoryAssembler
return EmPlanningStatus.FullSegmentResourceLimitExceeded;
}
var schedule = new TrajectorySampleSchedule(longitudinal.Candidate, outputTimeStepSeconds, holdDurationSeconds,
metadata.LongitudinalMode, isFullDirectionSegment);
metadata.LongitudinalMode, isFullDirectionSegment, pathSTolerance);
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;