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

feat: define full-direction EM planning scope

Browse Source
This commit is contained in:
梁薄云
2026-08-06 20:48:44 +08:00
parent fcf7df17d5
commit f4e89b4b4f
20 changed files with 183 additions and 33 deletions
Download Patch File Download Diff File Expand all files Collapse all files
ClumsyPilot/tests/EMPlannerVerificationHost/EmPlanningServiceChecks.cs
+10 -4
View File
@@ -320,6 +320,10 @@ internal static class EmPlanningServiceChecks
EmPlannerConfiguration configuration = EmPlannerConfiguration.CreateDefault();
configuration.Solver.MaximumOuterIterations = 2;
configuration.Scheduling.SolverTimeoutSeconds = 1d;
configuration.Longitudinal.MaximumForwardSpeedMetersPerSecond = 0.20d;
configuration.Longitudinal.MaximumReverseSpeedMetersPerSecond = 0.20d;
configuration.Longitudinal.DesiredForwardSpeedMetersPerSecond = 0.20d;
configuration.Longitudinal.DesiredReverseSpeedMetersPerSecond = 0.20d;
if (rolling)
configuration.Scheduling.DistanceHorizonMeters = 0.30d;
return new EmPlanningRequest(referencePath ?? CreateReferencePath(direction, endsAtGearSwitch), map ?? CreateMap(false),
@@ -332,21 +336,23 @@ internal static class EmPlanningServiceChecks
},
new VehicleMotionState(new Pose2D(0d, 0d, 0d), signedSpeed, 0d, requestedAtUtc, 7L), configuration, 0,
null, requestedAtUtc, requestedAtUtc, "output-trajectory", "reference-42", "prior-42",
EmMotionModel.NonholonomicForwardReverse);
EmMotionModel.NonholonomicForwardReverse, EmPlanningScope.RollingHorizon);
}
private static EmPlanningRequest ReplaceState(EmPlanningRequest source, VehicleMotionState state)
{
return new EmPlanningRequest(source.ReferencePath, source.Map, source.Vehicle, state, source.Configuration,
source.SegmentIndex, source.PreviousTrajectory, source.RequestedAtUtc, source.EffectiveAtUtc,
source.OutputTrajectoryId, source.ReferencePathId, source.PreviousTrajectoryId, source.MotionModel);
source.OutputTrajectoryId, source.ReferencePathId, source.PreviousTrajectoryId, source.MotionModel,
source.PlanningScope);
}
private static EmPlanningRequest ReplacePreviousTrajectory(EmPlanningRequest source, EmTrajectory previousTrajectory)
{
return new EmPlanningRequest(source.ReferencePath, source.Map, source.Vehicle, source.VehicleState,
source.Configuration, source.SegmentIndex, previousTrajectory, source.RequestedAtUtc, source.EffectiveAtUtc,
source.OutputTrajectoryId, source.ReferencePathId, source.PreviousTrajectoryId, source.MotionModel);
source.OutputTrajectoryId, source.ReferencePathId, source.PreviousTrajectoryId, source.MotionModel,
source.PlanningScope);
}
private static EmTrajectory CreateLongitudinalPreviousTrajectory(DateTimeOffset effectiveAtUtc,
@@ -354,7 +360,7 @@ internal static class EmPlanningServiceChecks
{
var metadata = new EmTrajectoryMetadata("previous-service", effectiveAtUtc, effectiveAtUtc, 1L,
"previous-reference", 1L, string.Empty, segmentIndex, direction, EmTerminalType.RollingSafetyStop,
EmLongitudinalMode.RollingContinuation);
EmLongitudinalMode.RollingContinuation, EmPlanningScope.RollingHorizon);
double sign = direction == TravelDirection.Forward ? 1d : -1d;
return new EmTrajectory(metadata, new[]
{