806 lines
37 KiB
C#
806 lines
37 KiB
C#
using System;
|
|
using System.Collections.Generic;
|
|
using System.Collections.ObjectModel;
|
|
using System.Globalization;
|
|
using System.Threading;
|
|
using System.Threading.Tasks;
|
|
using MultiWheelC.TrajectoryPlanning.CoarsePath;
|
|
using MultiWheelC.TrajectoryPlanning.CoarsePath.Facade;
|
|
using MultiWheelC.TrajectoryPlanning.EMPlanner;
|
|
using MultiWheelC.TrajectoryPlanning.Mapping;
|
|
using MultiWheelC.TrajectoryPlanning.PathSmoothing;
|
|
using MultiWheelC.TrajectoryPlanning.PathSmoothing.Facade;
|
|
|
|
namespace MultiWheelC.TrajectoryPlanning.TrajectoryObservation;
|
|
|
|
/// <summary>一次观察会话启动的不可变结果，成功时同时携带冻结的粗路径、平滑路径、地图和方向段。</summary>
|
|
public sealed class TrajectoryObservationBootstrapResult
|
|
{
|
|
private readonly VehicleParameters vehicle;
|
|
|
|
private TrajectoryObservationBootstrapResult(CoarsePathPlanningJob job, CoarsePathPlanningJobResult coarse,
|
|
PathSmoothingResult smoothedPath, IReadOnlyList<DirectionSegmentView> segments, string failureReason)
|
|
{
|
|
Job = job ?? throw new ArgumentNullException(nameof(job));
|
|
vehicle = CopyVehicle(job.Vehicle);
|
|
CoarseResult = coarse;
|
|
SmoothedPath = smoothedPath;
|
|
Segments = CopySegments(segments);
|
|
FailureReason = failureReason ?? string.Empty;
|
|
Succeeded = coarse != null && smoothedPath != null && Segments.Count > 0 && FailureReason.Length == 0;
|
|
}
|
|
|
|
public bool Succeeded { get; }
|
|
|
|
public CoarsePathPlanningJob Job { get; }
|
|
|
|
public VehicleParameters Vehicle => CopyVehicle(vehicle);
|
|
|
|
public CoarsePathPlanningJobResult CoarseResult { get; }
|
|
|
|
public PathSmoothingResult SmoothedPath { get; }
|
|
|
|
public IReadOnlyList<DirectionSegmentView> Segments { get; }
|
|
|
|
public string FailureReason { get; }
|
|
|
|
public PlanningGridMap Map => CoarseResult?.MapResult?.Map;
|
|
|
|
public static TrajectoryObservationBootstrapResult FromFailure(CoarsePathPlanningJob job,
|
|
CoarsePathPlanningJobResult coarse, PathSmoothingResult smoothedPath, string failureReason)
|
|
{
|
|
return new TrajectoryObservationBootstrapResult(job, coarse, smoothedPath,
|
|
Array.Empty<DirectionSegmentView>(), string.IsNullOrEmpty(failureReason) ? "Planning bootstrap failed." : failureReason);
|
|
}
|
|
|
|
public static TrajectoryObservationBootstrapResult Success(CoarsePathPlanningJob job,
|
|
CoarsePathPlanningJobResult coarse, PathSmoothingResult smoothedPath,
|
|
IReadOnlyList<DirectionSegmentView> segments)
|
|
{
|
|
if (coarse == null) throw new ArgumentNullException(nameof(coarse));
|
|
if (smoothedPath == null) throw new ArgumentNullException(nameof(smoothedPath));
|
|
if (segments == null || segments.Count == 0)
|
|
throw new ArgumentException("A successful bootstrap requires reference segments.", nameof(segments));
|
|
return new TrajectoryObservationBootstrapResult(job, coarse, smoothedPath, segments, string.Empty);
|
|
}
|
|
|
|
private static IReadOnlyList<DirectionSegmentView> CopySegments(IReadOnlyList<DirectionSegmentView> source)
|
|
{
|
|
var copy = new List<DirectionSegmentView>(source == null ? 0 : source.Count);
|
|
if (source != null)
|
|
{
|
|
for (int index = 0; index < source.Count; index++)
|
|
copy.Add(source[index]);
|
|
}
|
|
return new ReadOnlyCollection<DirectionSegmentView>(copy);
|
|
}
|
|
|
|
private static VehicleParameters CopyVehicle(VehicleParameters source)
|
|
{
|
|
if (source == null) return null;
|
|
return new VehicleParameters
|
|
{
|
|
LengthMeters = source.LengthMeters,
|
|
WidthMeters = source.WidthMeters,
|
|
SafetyMarginMeters = source.SafetyMarginMeters,
|
|
MaximumCurvaturePerMeter = source.MaximumCurvaturePerMeter,
|
|
MinimumTurningRadiusMeters = source.MinimumTurningRadiusMeters,
|
|
};
|
|
}
|
|
}
|
|
|
|
/// <summary>只在会话开始阶段构造并冻结规划输入；后续观察 tick 不会重新读取或改写该快照。</summary>
|
|
public sealed class TrajectoryObservationBootstrapper
|
|
{
|
|
private readonly CoarsePathPlanningService coarseService;
|
|
private readonly PathSmoothingService smoothingService;
|
|
|
|
public TrajectoryObservationBootstrapper()
|
|
: this(new CoarsePathPlanningService(), new PathSmoothingService())
|
|
{
|
|
}
|
|
|
|
public TrajectoryObservationBootstrapper(CoarsePathPlanningService coarseService,
|
|
PathSmoothingService smoothingService)
|
|
{
|
|
this.coarseService = coarseService ?? throw new ArgumentNullException(nameof(coarseService));
|
|
this.smoothingService = smoothingService ?? throw new ArgumentNullException(nameof(smoothingService));
|
|
}
|
|
|
|
public TrajectoryObservationBootstrapResult Bootstrap(CoarsePathPlanningJob job,
|
|
CancellationToken cancellationToken = default)
|
|
{
|
|
if (job == null) throw new ArgumentNullException(nameof(job));
|
|
|
|
TimeSpan configuredLimit = job.Configuration == null
|
|
? TimeSpan.FromSeconds(5d)
|
|
: job.Configuration.SearchTimeout;
|
|
using var deadline = new TrajectoryObservationPlanningDeadline(configuredLimit, cancellationToken);
|
|
return Bootstrap(job, deadline, cancellationToken);
|
|
}
|
|
|
|
public TrajectoryObservationBootstrapResult Bootstrap(CoarsePathPlanningJob job,
|
|
TrajectoryObservationPlanningDeadline deadline, CancellationToken cancellationToken = default)
|
|
{
|
|
if (job == null) throw new ArgumentNullException(nameof(job));
|
|
if (deadline == null) throw new ArgumentNullException(nameof(deadline));
|
|
if (DeadlineExpired(deadline))
|
|
return DeadlineFailure(job, null, null, deadline, "coarse");
|
|
|
|
CoarsePathPlanningJob effectiveJob = CopyJobWithDeadline(job, deadline);
|
|
bool coarseUsesCycleRemainder = job.Configuration != null &&
|
|
effectiveJob.Configuration.SearchTimeout < job.Configuration.SearchTimeout;
|
|
using var linkedCancellation = CancellationTokenSource.CreateLinkedTokenSource(
|
|
deadline.Token, cancellationToken);
|
|
|
|
CoarsePathPlanningJobResult coarse = coarseService.Plan(effectiveJob, linkedCancellation.Token);
|
|
if (DeadlineExpired(deadline) ||
|
|
(coarseUsesCycleRemainder && coarse.PlanningResult.Status == PlanningStatus.SearchTimeout))
|
|
{
|
|
return DeadlineFailure(job, coarse, null, deadline, "coarse");
|
|
}
|
|
if (coarse.PlanningResult.Status != PlanningStatus.Success)
|
|
return TrajectoryObservationBootstrapResult.FromFailure(
|
|
job, coarse, null, "Coarse planning status: " + coarse.PlanningResult.Status);
|
|
|
|
if (DeadlineExpired(deadline))
|
|
return DeadlineFailure(job, coarse, null, deadline, "smoothing");
|
|
|
|
var smoothingRequest = new PathSmoothingRequest(
|
|
CopyFiniteClearance(coarse.PlanningResult.Path, coarse.MapResult.Map),
|
|
coarse.PlanningResult.Segments, coarse.MapResult.Map, job.Vehicle,
|
|
new PathSmoothingConfiguration());
|
|
PathSmoothingResult smooth = smoothingService.Smooth(smoothingRequest, linkedCancellation.Token);
|
|
if (DeadlineExpired(deadline))
|
|
return DeadlineFailure(job, coarse, smooth, deadline, "smoothing");
|
|
if (!IsPublishedSmoothingStatus(smooth.Status))
|
|
return TrajectoryObservationBootstrapResult.FromFailure(
|
|
job, coarse, smooth, smooth.Diagnostics.TerminationReason);
|
|
return TrajectoryObservationBootstrapResult.Success(job, coarse, smooth, ReferencePathSegmenter.Create(smooth));
|
|
}
|
|
|
|
private static bool DeadlineExpired(TrajectoryObservationPlanningDeadline deadline)
|
|
{
|
|
return deadline.IsExpired && !deadline.CallerCancellationRequested;
|
|
}
|
|
|
|
private static TrajectoryObservationBootstrapResult DeadlineFailure(CoarsePathPlanningJob job,
|
|
CoarsePathPlanningJobResult coarse, PathSmoothingResult smooth,
|
|
TrajectoryObservationPlanningDeadline deadline, string phase)
|
|
{
|
|
string diagnostic = "cycleDeadlineExpired=true;phase=" + phase + ";remainingMs=" +
|
|
deadline.Remaining.TotalMilliseconds.ToString("F3", CultureInfo.InvariantCulture);
|
|
return TrajectoryObservationBootstrapResult.FromFailure(job, coarse, smooth, diagnostic);
|
|
}
|
|
|
|
private static CoarsePathPlanningJob CopyJobWithDeadline(CoarsePathPlanningJob source,
|
|
TrajectoryObservationPlanningDeadline deadline)
|
|
{
|
|
return new CoarsePathPlanningJob
|
|
{
|
|
MapRequest = source.MapRequest,
|
|
Start = source.Start,
|
|
Goal = source.Goal,
|
|
Vehicle = source.Vehicle,
|
|
Configuration = CopyConfigurationWithDeadline(source.Configuration, deadline),
|
|
StartVehicleCurvature = source.StartVehicleCurvature,
|
|
StartDirection = source.StartDirection,
|
|
GoalDirection = source.GoalDirection,
|
|
DebugOptions = source.DebugOptions,
|
|
};
|
|
}
|
|
|
|
private static HybridAStarConfiguration CopyConfigurationWithDeadline(HybridAStarConfiguration source,
|
|
TrajectoryObservationPlanningDeadline deadline)
|
|
{
|
|
if (source == null) return null;
|
|
return new HybridAStarConfiguration
|
|
{
|
|
PrimitiveLengthMeters = source.PrimitiveLengthMeters,
|
|
IntegrationStepMeters = source.IntegrationStepMeters,
|
|
MaximumCollisionCheckStepMeters = source.MaximumCollisionCheckStepMeters,
|
|
HeadingResolutionRadians = source.HeadingResolutionRadians,
|
|
CurvatureLevelCount = source.CurvatureLevelCount,
|
|
GoalPositionToleranceMeters = source.GoalPositionToleranceMeters,
|
|
GoalHeadingToleranceRadians = source.GoalHeadingToleranceRadians,
|
|
MaximumExpandedNodes = source.MaximumExpandedNodes,
|
|
SearchTimeout = deadline.Clamp(source.SearchTimeout),
|
|
HeuristicWeight = source.HeuristicWeight,
|
|
ReverseCostMultiplier = source.ReverseCostMultiplier,
|
|
GearSwitchPenaltyMeters = source.GearSwitchPenaltyMeters,
|
|
CurvatureMagnitudeWeight = source.CurvatureMagnitudeWeight,
|
|
CurvatureChangePenaltyMetersPerLevel = source.CurvatureChangePenaltyMetersPerLevel,
|
|
ClearanceCostWeight = source.ClearanceCostWeight,
|
|
ClearanceCostDistanceMeters = source.ClearanceCostDistanceMeters,
|
|
AllowReverse = source.AllowReverse,
|
|
};
|
|
}
|
|
|
|
private static IReadOnlyList<CoarsePathPoint> CopyFiniteClearance(IReadOnlyList<CoarsePathPoint> path,
|
|
PlanningGridMap map)
|
|
{
|
|
if (path == null) throw new ArgumentNullException(nameof(path));
|
|
if (map == null) throw new ArgumentNullException(nameof(map));
|
|
|
|
double widthMeters = (map.Bounds.XMax - map.Bounds.XMin) / 1000d;
|
|
double heightMeters = (map.Bounds.YMax - map.Bounds.YMin) / 1000d;
|
|
double mapDiagonalMeters = Math.Sqrt(widthMeters * widthMeters + heightMeters * heightMeters);
|
|
var copy = new List<CoarsePathPoint>(path.Count);
|
|
for (int index = 0; index < path.Count; index++)
|
|
{
|
|
CoarsePathPoint point = path[index] ?? throw new ArgumentException(
|
|
"The coarse path cannot contain null points.", nameof(path));
|
|
double clearance = double.IsPositiveInfinity(point.BodyClearance)
|
|
? mapDiagonalMeters
|
|
: point.BodyClearance;
|
|
copy.Add(new CoarsePathPoint(point.X, point.Y, point.Heading, point.UnwrappedHeading,
|
|
point.ArcLength, point.Direction, point.VehicleCurvature, clearance,
|
|
point.IsGearSwitchPoint, point.Source));
|
|
}
|
|
return new ReadOnlyCollection<CoarsePathPoint>(copy);
|
|
}
|
|
|
|
private static bool IsPublishedSmoothingStatus(PathSmoothingStatus status)
|
|
{
|
|
return status == PathSmoothingStatus.Complete ||
|
|
status == PathSmoothingStatus.PartialImprovement ||
|
|
status == PathSmoothingStatus.NotNeeded ||
|
|
status == PathSmoothingStatus.Unchanged;
|
|
}
|
|
}
|
|
|
|
/// <summary>单个观察 tick 的只读结果，供状态文本、报告和可视化共同消费。</summary>
|
|
public sealed class TrajectoryObservationObservation
|
|
{
|
|
internal TrajectoryObservationObservation(DateTimeOffset observedAtUtc, VehicleMotionState vehicleState,
|
|
EmTrajectory publishedTrajectory, EmTrajectoryPoint selectedPoint, TrajectoryControlCommand command,
|
|
TrajectoryExecutionState executorState)
|
|
{
|
|
ObservedAtUtc = observedAtUtc;
|
|
VehicleState = vehicleState ?? throw new ArgumentNullException(nameof(vehicleState));
|
|
PublishedTrajectory = publishedTrajectory;
|
|
SelectedPoint = selectedPoint;
|
|
Command = command;
|
|
ExecutorState = executorState;
|
|
}
|
|
|
|
public DateTimeOffset ObservedAtUtc { get; }
|
|
|
|
public VehicleMotionState VehicleState { get; }
|
|
|
|
public EmTrajectory PublishedTrajectory { get; }
|
|
|
|
public EmTrajectoryPoint SelectedPoint { get; }
|
|
|
|
public TrajectoryControlCommand Command { get; }
|
|
|
|
public TrajectoryExecutionState ExecutorState { get; }
|
|
}
|
|
|
|
/// <summary>协调一次冻结会话中的 EM 规划、轨迹采样和诊断；从不将预测命令发送给车辆。</summary>
|
|
public sealed class TrajectoryObservationController
|
|
{
|
|
private readonly TrajectoryObservationBootstrapResult bootstrap;
|
|
private readonly TrajectoryObservationSettings settings;
|
|
private readonly EmPlannerConfiguration configuration;
|
|
private readonly IEmPlanningService planningService;
|
|
private readonly TrajectoryObservationSegmentTracker segmentTracker;
|
|
private EmPlanningCoordinator coordinator;
|
|
private TrajectoryExecutor executor;
|
|
private EmTrajectory previousTrajectoryForVisualization;
|
|
private EmTrajectory activeFullDirectionTrajectory;
|
|
private EmPlanningCoordinator pendingFullDirectionCoordinator;
|
|
private readonly string sessionId;
|
|
private long cycleId;
|
|
private int plannedSegmentIndex;
|
|
private int pendingFullDirectionSegmentIndex = -1;
|
|
private bool planAttemptedForActiveSegment;
|
|
private bool pendingFullDirectionPlanAttempted;
|
|
|
|
public TrajectoryObservationController(TrajectoryObservationBootstrapResult bootstrap,
|
|
TrajectoryObservationSettings settings, IEmPlanningService planningService, string sessionId)
|
|
{
|
|
this.bootstrap = bootstrap ?? throw new ArgumentNullException(nameof(bootstrap));
|
|
if (!bootstrap.Succeeded)
|
|
throw new ArgumentException("A successful planning bootstrap is required.", nameof(bootstrap));
|
|
if (settings == null) throw new ArgumentNullException(nameof(settings));
|
|
if (planningService == null) throw new ArgumentNullException(nameof(planningService));
|
|
if (string.IsNullOrWhiteSpace(sessionId)) throw new ArgumentException("A session ID is required.", nameof(sessionId));
|
|
|
|
TrajectoryObservationSettings snapshot = settings.CreateValidatedSnapshot();
|
|
this.settings = snapshot;
|
|
configuration = EmPlannerConfiguration.CreateDefault();
|
|
configuration.Scheduling.ReplanPeriodSeconds = snapshot.ReplanPeriodSeconds;
|
|
configuration.Scheduling.SolverTimeoutSeconds = snapshot.SolverTimeoutSeconds;
|
|
configuration.Solver.MaximumOsqpIterations = snapshot.MaximumOsqpIterations;
|
|
configuration.Scheduling.TimeHorizonSeconds = snapshot.TimeHorizonSeconds;
|
|
configuration.Scheduling.OutputTimeStepSeconds = snapshot.OutputTimeStepSeconds;
|
|
this.planningService = planningService;
|
|
coordinator = new EmPlanningCoordinator(planningService);
|
|
executor = new TrajectoryExecutor(configuration);
|
|
segmentTracker = new TrajectoryObservationSegmentTracker(bootstrap.Segments, snapshot,
|
|
configuration.Longitudinal.StopSpeedToleranceMetersPerSecond);
|
|
this.sessionId = sessionId;
|
|
}
|
|
|
|
public EmTrajectory PublishedTrajectory => settings.PlanningScope == EmPlanningScope.FullDirectionSegment
|
|
? activeFullDirectionTrajectory
|
|
: coordinator.PublishedTrajectory;
|
|
|
|
public DirectionSegmentView ActiveSegment => bootstrap.Segments[segmentTracker.State.ActiveSegmentIndex];
|
|
|
|
public TrajectoryObservationSegmentState SegmentState => segmentTracker.State;
|
|
|
|
public EmTrajectory PreviousTrajectoryForVisualization => previousTrajectoryForVisualization;
|
|
|
|
internal EmPlannerConfiguration CreateEffectiveConfigurationSnapshot()
|
|
{
|
|
return configuration.Copy();
|
|
}
|
|
|
|
public TrajectoryObservationDiagnostic CreateConfigurationDiagnostic()
|
|
{
|
|
return TrajectoryObservationDiagnostics.CreateConfiguration(configuration);
|
|
}
|
|
|
|
public bool ShouldStartCycle(DateTimeOffset now)
|
|
{
|
|
if (settings.PlanningScope == EmPlanningScope.FullDirectionSegment)
|
|
{
|
|
if (plannedSegmentIndex != ActiveSegment.SegmentIndex)
|
|
{
|
|
plannedSegmentIndex = ActiveSegment.SegmentIndex;
|
|
planAttemptedForActiveSegment = false;
|
|
}
|
|
if (TryGetPendingFullDirectionSegmentIndex(out int pendingSegmentIndex))
|
|
{
|
|
if (pendingFullDirectionSegmentIndex != pendingSegmentIndex)
|
|
return true;
|
|
return !pendingFullDirectionPlanAttempted;
|
|
}
|
|
return !planAttemptedForActiveSegment;
|
|
}
|
|
return coordinator.ShouldStartCycle(now);
|
|
}
|
|
|
|
public async Task<PlanningCycleResult> StartCycle(DateTimeOffset now, VehicleMotionState state,
|
|
CancellationToken cancellationToken)
|
|
{
|
|
using var deadline = new TrajectoryObservationPlanningDeadline(
|
|
TimeSpan.FromSeconds(settings.SolverTimeoutSeconds), cancellationToken);
|
|
return await StartCycle(now, state, deadline, cancellationToken).ConfigureAwait(false);
|
|
}
|
|
|
|
public async Task<PlanningCycleResult> StartCycle(DateTimeOffset now, VehicleMotionState state,
|
|
TrajectoryObservationPlanningDeadline deadline, CancellationToken cancellationToken)
|
|
{
|
|
if (state == null) throw new ArgumentNullException(nameof(state));
|
|
if (deadline == null) throw new ArgumentNullException(nameof(deadline));
|
|
using var callerCancellation = CancellationTokenSource.CreateLinkedTokenSource(
|
|
deadline.CallerToken, cancellationToken);
|
|
|
|
int targetSegmentIndex = ActiveSegment.SegmentIndex;
|
|
int pendingSegmentIndex = -1;
|
|
bool planningPendingFullDirection = settings.PlanningScope == EmPlanningScope.FullDirectionSegment &&
|
|
TryGetPendingFullDirectionSegmentIndex(out pendingSegmentIndex);
|
|
EmPlanningCoordinator targetCoordinator;
|
|
EmTrajectory previousTrajectory;
|
|
if (planningPendingFullDirection)
|
|
{
|
|
targetSegmentIndex = pendingSegmentIndex;
|
|
if (pendingFullDirectionSegmentIndex != targetSegmentIndex || pendingFullDirectionCoordinator == null)
|
|
{
|
|
pendingFullDirectionSegmentIndex = targetSegmentIndex;
|
|
pendingFullDirectionPlanAttempted = false;
|
|
pendingFullDirectionCoordinator = new EmPlanningCoordinator(planningService);
|
|
}
|
|
pendingFullDirectionPlanAttempted = true;
|
|
targetCoordinator = pendingFullDirectionCoordinator;
|
|
previousTrajectory = null;
|
|
}
|
|
else
|
|
{
|
|
planAttemptedForActiveSegment = true;
|
|
plannedSegmentIndex = ActiveSegment.SegmentIndex;
|
|
targetCoordinator = coordinator;
|
|
previousTrajectory = coordinator.PublishedTrajectory;
|
|
}
|
|
|
|
long currentCycleId = Interlocked.Increment(ref cycleId);
|
|
EmPlannerConfiguration effectiveConfiguration = configuration.Copy();
|
|
TimeSpan cycleRemaining = deadline.Remaining;
|
|
TimeSpan configuredSolverLimit = TimeSpan.FromSeconds(
|
|
configuration.Scheduling.SolverTimeoutSeconds);
|
|
if (cycleRemaining > TimeSpan.Zero &&
|
|
cycleRemaining < configuredSolverLimit - TimeSpan.FromMilliseconds(1d))
|
|
{
|
|
effectiveConfiguration.Scheduling.SolverTimeoutSeconds = cycleRemaining.TotalSeconds;
|
|
}
|
|
var request = new EmPlanningRequest(
|
|
bootstrap.SmoothedPath, bootstrap.Map, bootstrap.Vehicle, state, effectiveConfiguration,
|
|
targetSegmentIndex, previousTrajectory, now, now,
|
|
sessionId + "-trajectory-" + currentCycleId, sessionId + "-reference",
|
|
previousTrajectory?.Metadata.TrajectoryId ?? string.Empty,
|
|
EmMotionModel.NonholonomicForwardReverse, settings.PlanningScope, cycleRemaining,
|
|
callerCancellation.Token, deadline.DeadlineToken,
|
|
deadline.PublicationAuthorization);
|
|
if (callerCancellation.IsCancellationRequested)
|
|
{
|
|
var cancelledResult = new EmPlanningResult(EmPlanningStatus.Cancelled, null,
|
|
"callerCancellation=true;phase=request");
|
|
return new PlanningCycleResult(currentCycleId, PlanningCycleIdentity.FromRequest(request),
|
|
cancelledResult, false, cancelledResult.FailureReason);
|
|
}
|
|
if (deadline.DeadlineToken.IsCancellationRequested)
|
|
{
|
|
var expiredResult = new EmPlanningResult(EmPlanningStatus.CycleDeadlineExpired, null,
|
|
"cycleDeadlineExpired=true;phase=request;remainingMs=" +
|
|
cycleRemaining.TotalMilliseconds.ToString("F3", CultureInfo.InvariantCulture));
|
|
return new PlanningCycleResult(currentCycleId, PlanningCycleIdentity.FromRequest(request),
|
|
expiredResult, false, expiredResult.FailureReason);
|
|
}
|
|
Action<EmTrajectory> fullDirectionCommit =
|
|
settings.PlanningScope == EmPlanningScope.FullDirectionSegment && !planningPendingFullDirection
|
|
? trajectory => activeFullDirectionTrajectory = trajectory
|
|
: null;
|
|
PlanningCycleResult result = await targetCoordinator.PlanLatestAsync(
|
|
new PlanningCycleInput(request, now), callerCancellation.Token,
|
|
fullDirectionCommit).ConfigureAwait(false);
|
|
if (!result.Published && deadline.IsExpired && !callerCancellation.IsCancellationRequested &&
|
|
result.Result.Status == EmPlanningStatus.Cancelled)
|
|
{
|
|
var expiredResult = new EmPlanningResult(EmPlanningStatus.CycleDeadlineExpired, null,
|
|
"cycleDeadlineExpired=true;phase=planning;remainingMs=" +
|
|
deadline.Remaining.TotalMilliseconds.ToString("F3", CultureInfo.InvariantCulture));
|
|
result = new PlanningCycleResult(result.Version, result.Identity, expiredResult, false,
|
|
expiredResult.FailureReason);
|
|
}
|
|
return result;
|
|
}
|
|
|
|
public bool TryAdvanceSegment(DateTimeOffset now, VehicleMotionState state)
|
|
{
|
|
if (state == null) throw new ArgumentNullException(nameof(state));
|
|
|
|
if (settings.PlanningScope == EmPlanningScope.FullDirectionSegment &&
|
|
segmentTracker.State.Phase == TrajectoryObservationSegmentPhase.WaitingForDirection &&
|
|
!HasPendingFullDirectionTrajectory())
|
|
{
|
|
return false;
|
|
}
|
|
|
|
EmTrajectory activeTrajectory = PublishedTrajectory;
|
|
TrajectoryObservationSegmentUpdate update = segmentTracker.Update(now, state, activeTrajectory);
|
|
if (!update.Advanced)
|
|
return false;
|
|
|
|
if (settings.PlanningScope == EmPlanningScope.FullDirectionSegment)
|
|
{
|
|
EmTrajectory pendingTrajectory = pendingFullDirectionCoordinator.PublishedTrajectory;
|
|
previousTrajectoryForVisualization = activeTrajectory;
|
|
activeFullDirectionTrajectory = RebaseEffectiveAt(pendingTrajectory, now);
|
|
coordinator = pendingFullDirectionCoordinator;
|
|
pendingFullDirectionCoordinator = null;
|
|
pendingFullDirectionSegmentIndex = -1;
|
|
pendingFullDirectionPlanAttempted = false;
|
|
executor = new TrajectoryExecutor(configuration);
|
|
plannedSegmentIndex = segmentTracker.State.ActiveSegmentIndex;
|
|
planAttemptedForActiveSegment = true;
|
|
return true;
|
|
}
|
|
|
|
previousTrajectoryForVisualization = coordinator.PublishedTrajectory;
|
|
coordinator = new EmPlanningCoordinator(planningService);
|
|
executor = new TrajectoryExecutor(configuration);
|
|
plannedSegmentIndex = segmentTracker.State.ActiveSegmentIndex;
|
|
planAttemptedForActiveSegment = false;
|
|
return true;
|
|
}
|
|
|
|
public TrajectoryObservationObservation Observe(DateTimeOffset now, VehicleMotionState state)
|
|
{
|
|
if (state == null) throw new ArgumentNullException(nameof(state));
|
|
|
|
EmTrajectory trajectory = PublishedTrajectory;
|
|
if (trajectory == null)
|
|
return new TrajectoryObservationObservation(now, state, null, null, null, null);
|
|
|
|
TrajectoryObservationSegmentState segmentState = segmentTracker.State;
|
|
TravelDirection currentDirection = ActiveSegment.Direction;
|
|
bool waitingForDirection = segmentState.Phase == TrajectoryObservationSegmentPhase.WaitingForStop ||
|
|
segmentState.Phase == TrajectoryObservationSegmentPhase.WaitingForDirection;
|
|
TravelDirection desiredDirection = waitingForDirection && segmentState.ExpectedDirection.HasValue
|
|
? segmentState.ExpectedDirection.Value
|
|
: currentDirection;
|
|
TrajectoryControlCommand command = executor.UpdateCommand(now, state, trajectory,
|
|
desiredDirection, currentDirection, !waitingForDirection);
|
|
TrajectoryExecutionState executorState = executor.State;
|
|
return new TrajectoryObservationObservation(now, state, trajectory, executorState.SelectedPoint,
|
|
command, executorState);
|
|
}
|
|
|
|
private bool TryGetPendingFullDirectionSegmentIndex(out int pendingSegmentIndex)
|
|
{
|
|
pendingSegmentIndex = -1;
|
|
if (settings.PlanningScope != EmPlanningScope.FullDirectionSegment ||
|
|
segmentTracker.State.Phase != TrajectoryObservationSegmentPhase.WaitingForDirection ||
|
|
segmentTracker.State.ActiveSegmentIndex + 1 >= bootstrap.Segments.Count)
|
|
{
|
|
return false;
|
|
}
|
|
pendingSegmentIndex = segmentTracker.State.ActiveSegmentIndex + 1;
|
|
return true;
|
|
}
|
|
|
|
private bool HasPendingFullDirectionTrajectory()
|
|
{
|
|
if (!TryGetPendingFullDirectionSegmentIndex(out int pendingSegmentIndex) ||
|
|
pendingFullDirectionSegmentIndex != pendingSegmentIndex || pendingFullDirectionCoordinator == null)
|
|
{
|
|
return false;
|
|
}
|
|
|
|
EmTrajectory pending = pendingFullDirectionCoordinator.PublishedTrajectory;
|
|
DirectionSegmentView segment = bootstrap.Segments[pendingSegmentIndex];
|
|
return pending != null && pending.Metadata.SegmentIndex == segment.SegmentIndex &&
|
|
pending.Metadata.Direction == segment.Direction;
|
|
}
|
|
|
|
private static EmTrajectory RebaseEffectiveAt(EmTrajectory source, DateTimeOffset effectiveAtUtc)
|
|
{
|
|
if (source == null) throw new ArgumentNullException(nameof(source));
|
|
EmTrajectoryMetadata metadata = source.Metadata;
|
|
return new EmTrajectory(new EmTrajectoryMetadata(metadata.TrajectoryId, metadata.GeneratedAtUtc, effectiveAtUtc,
|
|
metadata.MapSnapshotId, metadata.ReferencePathId, metadata.VehicleStateSequenceId,
|
|
metadata.PreviousTrajectoryId, metadata.SegmentIndex, metadata.Direction, metadata.TerminalType,
|
|
metadata.LongitudinalMode, metadata.PlanningScope), source.Points);
|
|
}
|
|
}
|
|
|
|
public sealed class TrajectoryObservationLoopTick
|
|
{
|
|
internal TrajectoryObservationLoopTick(TrajectoryObservationObservation observation,
|
|
PlanningCycleResult latestCycle, TimeSpan latestPlanningElapsed, bool planningInFlight,
|
|
bool planningStarted, bool planningCompleted, bool segmentAdvanced,
|
|
TrajectoryObservationSegmentState segmentState)
|
|
{
|
|
Observation = observation ?? throw new ArgumentNullException(nameof(observation));
|
|
LatestCycle = latestCycle;
|
|
LatestPlanningElapsed = latestPlanningElapsed;
|
|
PlanningInFlight = planningInFlight;
|
|
PlanningStarted = planningStarted;
|
|
PlanningCompleted = planningCompleted;
|
|
SegmentAdvanced = segmentAdvanced;
|
|
SegmentState = segmentState ?? throw new ArgumentNullException(nameof(segmentState));
|
|
}
|
|
|
|
public TrajectoryObservationObservation Observation { get; }
|
|
|
|
public PlanningCycleResult LatestCycle { get; }
|
|
|
|
public TimeSpan LatestPlanningElapsed { get; }
|
|
|
|
public bool PlanningInFlight { get; }
|
|
|
|
public bool PlanningStarted { get; }
|
|
|
|
public bool PlanningCompleted { get; }
|
|
|
|
public bool SegmentAdvanced { get; }
|
|
|
|
public TrajectoryObservationSegmentState SegmentState { get; }
|
|
|
|
public bool ShouldLog => true;
|
|
}
|
|
|
|
/// <summary>按观察周期捕获新鲜只读状态并发布快照的循环；规划耗时不能改变观察安全边界。</summary>
|
|
public sealed class TrajectoryObservationLoop
|
|
{
|
|
private readonly TrajectoryObservationController controller;
|
|
private Task<PlanningCycleResult> planningTask;
|
|
private DateTimeOffset planningStartedAtUtc;
|
|
private PlanningCycleResult latestCycle;
|
|
private TimeSpan latestPlanningElapsed;
|
|
|
|
public TrajectoryObservationLoop(TrajectoryObservationController controller)
|
|
{
|
|
this.controller = controller ?? throw new ArgumentNullException(nameof(controller));
|
|
}
|
|
|
|
public TrajectoryObservationLoopTick Tick(DateTimeOffset now, VehicleMotionState state,
|
|
CancellationToken cancellationToken)
|
|
{
|
|
if (state == null) throw new ArgumentNullException(nameof(state));
|
|
cancellationToken.ThrowIfCancellationRequested();
|
|
|
|
bool planningCompleted = ConsumeCompletedPlanning(now);
|
|
bool segmentAdvanced = planningTask == null && controller.TryAdvanceSegment(now, state);
|
|
if (segmentAdvanced)
|
|
latestCycle = null;
|
|
bool planningStarted = false;
|
|
if (planningTask == null && controller.ShouldStartCycle(now))
|
|
{
|
|
planningStarted = true;
|
|
planningStartedAtUtc = now;
|
|
planningTask = controller.StartCycle(now, state, cancellationToken);
|
|
planningCompleted |= ConsumeCompletedPlanning(now);
|
|
}
|
|
|
|
TrajectoryObservationObservation observation = controller.Observe(now, state);
|
|
return new TrajectoryObservationLoopTick(observation, latestCycle, latestPlanningElapsed,
|
|
planningTask != null, planningStarted, planningCompleted, segmentAdvanced, controller.SegmentState);
|
|
}
|
|
|
|
private bool ConsumeCompletedPlanning(DateTimeOffset observedAtUtc)
|
|
{
|
|
Task<PlanningCycleResult> completed = planningTask;
|
|
if (completed == null || !completed.IsCompleted) return false;
|
|
|
|
latestCycle = completed.GetAwaiter().GetResult();
|
|
TimeSpan elapsed = observedAtUtc - planningStartedAtUtc;
|
|
latestPlanningElapsed = elapsed < TimeSpan.Zero ? TimeSpan.Zero : elapsed;
|
|
planningTask = null;
|
|
return true;
|
|
}
|
|
}
|
|
|
|
public enum TrajectoryObservationSessionEndReason
|
|
{
|
|
BootstrapFailure,
|
|
RuntimeFault,
|
|
Cancellation,
|
|
}
|
|
|
|
/// <summary>集中定义观察会话的开始、停止和故障清理语义，避免留下旧会话可视化。</summary>
|
|
public static class TrajectoryObservationSessionLifecycle
|
|
{
|
|
public static bool ShouldClearLayers(TrajectoryObservationSessionEndReason reason)
|
|
{
|
|
switch (reason)
|
|
{
|
|
case TrajectoryObservationSessionEndReason.BootstrapFailure:
|
|
return false;
|
|
case TrajectoryObservationSessionEndReason.RuntimeFault:
|
|
case TrajectoryObservationSessionEndReason.Cancellation:
|
|
return true;
|
|
default:
|
|
throw new ArgumentOutOfRangeException(nameof(reason));
|
|
}
|
|
}
|
|
}
|
|
|
|
public sealed class TrajectoryObservationRuntimeState
|
|
{
|
|
public const string GearSwitchWaitingNotice =
|
|
"等待真实档位/方向确认；观察模式不会激活下一方向段";
|
|
|
|
private TrajectoryObservationRuntimeState(bool waitingAtGearSwitch)
|
|
{
|
|
WaitingAtGearSwitch = waitingAtGearSwitch;
|
|
WorldNotice = waitingAtGearSwitch ? GearSwitchWaitingNotice : string.Empty;
|
|
}
|
|
|
|
public bool WaitingAtGearSwitch { get; }
|
|
|
|
public string WorldNotice { get; }
|
|
|
|
public static TrajectoryObservationRuntimeState Create(TrajectoryObservationSegmentState segmentState)
|
|
{
|
|
if (segmentState == null) throw new ArgumentNullException(nameof(segmentState));
|
|
bool waiting = segmentState.Phase == TrajectoryObservationSegmentPhase.WaitingForStop ||
|
|
segmentState.Phase == TrajectoryObservationSegmentPhase.WaitingForDirection;
|
|
return new TrajectoryObservationRuntimeState(waiting);
|
|
}
|
|
|
|
public static TrajectoryObservationRuntimeState Create(DateTimeOffset now, EmTrajectory trajectory)
|
|
{
|
|
if (trajectory == null || trajectory.Metadata.TerminalType != EmTerminalType.GearSwitch)
|
|
return new TrajectoryObservationRuntimeState(false);
|
|
|
|
EmTrajectoryPoint finalPoint = trajectory.Points[trajectory.Points.Count - 1];
|
|
bool waiting = (now - trajectory.Metadata.EffectiveAtUtc).TotalSeconds >= finalPoint.TimeFromStart;
|
|
return new TrajectoryObservationRuntimeState(waiting);
|
|
}
|
|
}
|
|
|
|
public sealed class TrajectoryObservationLsSample
|
|
{
|
|
public TrajectoryObservationLsSample(double pathS, double lateralOffset)
|
|
{
|
|
PathS = pathS;
|
|
LateralOffset = lateralOffset;
|
|
}
|
|
|
|
public double PathS { get; }
|
|
|
|
public double LateralOffset { get; }
|
|
}
|
|
|
|
public sealed class TrajectoryObservationStSample
|
|
{
|
|
public TrajectoryObservationStSample(double timeFromStart, double pathS)
|
|
{
|
|
TimeFromStart = timeFromStart;
|
|
PathS = pathS;
|
|
}
|
|
|
|
public double TimeFromStart { get; }
|
|
|
|
public double PathS { get; }
|
|
}
|
|
|
|
public sealed class TrajectoryObservationSpeedSample
|
|
{
|
|
public TrajectoryObservationSpeedSample(double timeFromStart, double signedLongitudinalVelocity)
|
|
{
|
|
TimeFromStart = timeFromStart;
|
|
SignedLongitudinalVelocity = signedLongitudinalVelocity;
|
|
}
|
|
|
|
public double TimeFromStart { get; }
|
|
|
|
public double SignedLongitudinalVelocity { get; }
|
|
}
|
|
|
|
public sealed class TrajectoryObservationCharts
|
|
{
|
|
private TrajectoryObservationCharts(IReadOnlyList<TrajectoryObservationLsSample> lsSamples,
|
|
IReadOnlyList<TrajectoryObservationStSample> stSamples,
|
|
IReadOnlyList<TrajectoryObservationSpeedSample> speedSamples, int failedProjectionCount)
|
|
{
|
|
LsSamples = new ReadOnlyCollection<TrajectoryObservationLsSample>(
|
|
new List<TrajectoryObservationLsSample>(lsSamples));
|
|
StSamples = new ReadOnlyCollection<TrajectoryObservationStSample>(
|
|
new List<TrajectoryObservationStSample>(stSamples));
|
|
SpeedSamples = new ReadOnlyCollection<TrajectoryObservationSpeedSample>(
|
|
new List<TrajectoryObservationSpeedSample>(speedSamples));
|
|
FailedProjectionCount = failedProjectionCount;
|
|
}
|
|
|
|
public IReadOnlyList<TrajectoryObservationLsSample> LsSamples { get; }
|
|
|
|
public IReadOnlyList<TrajectoryObservationStSample> StSamples { get; }
|
|
|
|
public IReadOnlyList<TrajectoryObservationSpeedSample> SpeedSamples { get; }
|
|
|
|
public int FailedProjectionCount { get; }
|
|
|
|
public static TrajectoryObservationCharts Build(EmTrajectory trajectory, DirectionSegmentView segment,
|
|
double maximumProjectionDistanceMeters)
|
|
{
|
|
if (trajectory == null) throw new ArgumentNullException(nameof(trajectory));
|
|
if (segment == null) throw new ArgumentNullException(nameof(segment));
|
|
if (double.IsNaN(maximumProjectionDistanceMeters) || double.IsInfinity(maximumProjectionDistanceMeters) ||
|
|
maximumProjectionDistanceMeters < 0d)
|
|
throw new ArgumentOutOfRangeException(nameof(maximumProjectionDistanceMeters));
|
|
|
|
var ls = new List<TrajectoryObservationLsSample>(trajectory.Points.Count);
|
|
var st = new List<TrajectoryObservationStSample>(trajectory.Points.Count);
|
|
var speed = new List<TrajectoryObservationSpeedSample>(trajectory.Points.Count);
|
|
var projector = new FrenetProjector();
|
|
double seedReferenceS = 0d;
|
|
int failedProjectionCount = 0;
|
|
for (int index = 0; index < trajectory.Points.Count; index++)
|
|
{
|
|
EmTrajectoryPoint point = trajectory.Points[index];
|
|
var pose = new Pose2D(point.X, point.Y, point.Yaw);
|
|
if (projector.TryProject(pose, segment, 0d, segment.LengthMeters,
|
|
maximumProjectionDistanceMeters, seedReferenceS, out FrenetProjection projection))
|
|
{
|
|
ls.Add(new TrajectoryObservationLsSample(
|
|
segment.SourceStartArcLength + projection.ReferenceS, projection.LateralOffset));
|
|
seedReferenceS = projection.ReferenceS;
|
|
}
|
|
else
|
|
{
|
|
failedProjectionCount++;
|
|
}
|
|
|
|
st.Add(new TrajectoryObservationStSample(point.TimeFromStart, point.PathS));
|
|
speed.Add(new TrajectoryObservationSpeedSample(point.TimeFromStart, point.SignedLongitudinalVelocity));
|
|
}
|
|
|
|
return new TrajectoryObservationCharts(ls, st, speed, failedProjectionCount);
|
|
}
|
|
}
|