330 lines
12 KiB
C#
330 lines
12 KiB
C#
using System;
|
|
using System.Diagnostics;
|
|
using System.Threading;
|
|
using MultiWheelC.TrajectoryPlanning.CoarsePath;
|
|
using MultiWheelC.TrajectoryPlanning.Mapping;
|
|
using MultiWheelC.TrajectoryPlanning.PathSmoothing;
|
|
|
|
namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
|
|
|
|
internal interface IEmPlanningPublicationAuthorization
|
|
{
|
|
TimeSpan Remaining { get; }
|
|
|
|
bool IsExpired { get; }
|
|
|
|
EmPlanningPublicationDecision TryPublish(CancellationToken requestCallerCancellationToken,
|
|
CancellationToken coordinatorCallerCancellationToken, Action publish);
|
|
}
|
|
|
|
internal enum EmPlanningPublicationDecision
|
|
{
|
|
Published = 0,
|
|
CallerCancelled = 1,
|
|
DeadlineExpired = 2,
|
|
}
|
|
|
|
internal sealed class EmPlanningRequestPublicationAuthorization : IEmPlanningPublicationAuthorization
|
|
{
|
|
private readonly object gate = new object();
|
|
private readonly TimeSpan? initialRemaining;
|
|
private readonly Stopwatch stopwatch;
|
|
private readonly CancellationToken deadlineToken;
|
|
private bool callerCancelled;
|
|
private bool deadlineExpired;
|
|
|
|
public EmPlanningRequestPublicationAuthorization(TimeSpan? remaining, CancellationToken deadlineToken)
|
|
{
|
|
initialRemaining = remaining;
|
|
this.deadlineToken = deadlineToken;
|
|
stopwatch = remaining.HasValue ? Stopwatch.StartNew() : null;
|
|
deadlineExpired = remaining.HasValue && remaining.Value <= TimeSpan.Zero ||
|
|
deadlineToken.IsCancellationRequested;
|
|
}
|
|
|
|
public TimeSpan Remaining
|
|
{
|
|
get
|
|
{
|
|
lock (gate)
|
|
{
|
|
ObserveDeadlineInsideGate();
|
|
return ComputeRemainingInsideGate();
|
|
}
|
|
}
|
|
}
|
|
|
|
public bool IsExpired
|
|
{
|
|
get
|
|
{
|
|
lock (gate)
|
|
{
|
|
ObserveDeadlineInsideGate();
|
|
return deadlineExpired;
|
|
}
|
|
}
|
|
}
|
|
|
|
public EmPlanningPublicationDecision TryPublish(CancellationToken requestCallerCancellationToken,
|
|
CancellationToken coordinatorCallerCancellationToken, Action publish)
|
|
{
|
|
if (publish == null)
|
|
throw new ArgumentNullException(nameof(publish));
|
|
|
|
using CancellationTokenRegistration requestCallerRegistration =
|
|
requestCallerCancellationToken.Register(ObserveCallerCancellation);
|
|
using CancellationTokenRegistration coordinatorCallerRegistration =
|
|
coordinatorCallerCancellationToken.Register(ObserveCallerCancellation);
|
|
using CancellationTokenRegistration deadlineRegistration =
|
|
deadlineToken.Register(ObserveDeadlineCancellation);
|
|
lock (gate)
|
|
{
|
|
if (requestCallerCancellationToken.IsCancellationRequested ||
|
|
coordinatorCallerCancellationToken.IsCancellationRequested)
|
|
{
|
|
callerCancelled = true;
|
|
}
|
|
ObserveDeadlineInsideGate();
|
|
if (callerCancelled)
|
|
return EmPlanningPublicationDecision.CallerCancelled;
|
|
if (deadlineExpired)
|
|
return EmPlanningPublicationDecision.DeadlineExpired;
|
|
publish();
|
|
return EmPlanningPublicationDecision.Published;
|
|
}
|
|
}
|
|
|
|
private void ObserveCallerCancellation()
|
|
{
|
|
lock (gate)
|
|
callerCancelled = true;
|
|
}
|
|
|
|
private void ObserveDeadlineCancellation()
|
|
{
|
|
lock (gate)
|
|
deadlineExpired = true;
|
|
}
|
|
|
|
private void ObserveDeadlineInsideGate()
|
|
{
|
|
if (deadlineToken.IsCancellationRequested || ComputeRemainingInsideGate() <= TimeSpan.Zero)
|
|
deadlineExpired = true;
|
|
}
|
|
|
|
private TimeSpan ComputeRemainingInsideGate()
|
|
{
|
|
if (!initialRemaining.HasValue)
|
|
return deadlineExpired ? TimeSpan.Zero : TimeSpan.MaxValue;
|
|
TimeSpan remaining = initialRemaining.Value - stopwatch.Elapsed;
|
|
return remaining > TimeSpan.Zero && !deadlineExpired ? remaining : TimeSpan.Zero;
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// EM 单次规划的只读输入引用快照，绑定平滑参考路径、地图、车辆状态、目标方向段和输出身份。
|
|
/// 坐标使用 m，航向使用 rad，时间使用 UTC；调用方必须在进入服务前保证这些输入属于同一业务版本。
|
|
/// </summary>
|
|
public sealed class EmPlanningRequest
|
|
{
|
|
/// <summary>
|
|
/// 保存一次规划所需的引用快照和发布身份。构造器只赋值而不校验、复制或取得对象所有权；调用方仍拥有传入引用，服务随后负责验证并复制配置。
|
|
/// </summary>
|
|
public EmPlanningRequest(
|
|
PathSmoothingResult referencePath,
|
|
PlanningGridMap map,
|
|
VehicleParameters vehicle,
|
|
VehicleMotionState vehicleState,
|
|
EmPlannerConfiguration configuration,
|
|
int segmentIndex,
|
|
EmTrajectory previousTrajectory,
|
|
DateTimeOffset requestedAtUtc,
|
|
DateTimeOffset effectiveAtUtc,
|
|
string outputTrajectoryId,
|
|
string referencePathId,
|
|
string previousTrajectoryId,
|
|
EmMotionModel motionModel,
|
|
EmPlanningScope planningScope,
|
|
TimeSpan? cycleDeadlineRemaining = null,
|
|
CancellationToken callerCancellationToken = default,
|
|
CancellationToken cycleDeadlineToken = default)
|
|
{
|
|
ReferencePath = referencePath;
|
|
Map = map;
|
|
Vehicle = vehicle;
|
|
VehicleState = vehicleState;
|
|
Configuration = configuration;
|
|
SegmentIndex = segmentIndex;
|
|
PreviousTrajectory = previousTrajectory;
|
|
RequestedAtUtc = requestedAtUtc;
|
|
EffectiveAtUtc = effectiveAtUtc;
|
|
OutputTrajectoryId = outputTrajectoryId;
|
|
ReferencePathId = referencePathId;
|
|
PreviousTrajectoryId = previousTrajectoryId;
|
|
MotionModel = motionModel;
|
|
PlanningScope = planningScope;
|
|
CycleDeadlineRemaining = cycleDeadlineRemaining;
|
|
CallerCancellationToken = callerCancellationToken;
|
|
CycleDeadlineToken = cycleDeadlineToken;
|
|
PublicationAuthorization = new EmPlanningRequestPublicationAuthorization(
|
|
cycleDeadlineRemaining, cycleDeadlineToken);
|
|
}
|
|
|
|
internal EmPlanningRequest(
|
|
PathSmoothingResult referencePath,
|
|
PlanningGridMap map,
|
|
VehicleParameters vehicle,
|
|
VehicleMotionState vehicleState,
|
|
EmPlannerConfiguration configuration,
|
|
int segmentIndex,
|
|
EmTrajectory previousTrajectory,
|
|
DateTimeOffset requestedAtUtc,
|
|
DateTimeOffset effectiveAtUtc,
|
|
string outputTrajectoryId,
|
|
string referencePathId,
|
|
string previousTrajectoryId,
|
|
EmMotionModel motionModel,
|
|
EmPlanningScope planningScope,
|
|
TimeSpan? cycleDeadlineRemaining,
|
|
CancellationToken callerCancellationToken,
|
|
CancellationToken cycleDeadlineToken,
|
|
IEmPlanningPublicationAuthorization publicationAuthorization)
|
|
: this(referencePath, map, vehicle, vehicleState, configuration, segmentIndex, previousTrajectory,
|
|
requestedAtUtc, effectiveAtUtc, outputTrajectoryId, referencePathId, previousTrajectoryId,
|
|
motionModel, planningScope, cycleDeadlineRemaining, callerCancellationToken, cycleDeadlineToken)
|
|
{
|
|
PublicationAuthorization = publicationAuthorization ??
|
|
new EmPlanningRequestPublicationAuthorization(cycleDeadlineRemaining, cycleDeadlineToken);
|
|
}
|
|
|
|
/// <summary>
|
|
/// 平滑后的参考路径结果引用；服务要求其为可消费的成功结果，且调用方负责保持其与其他输入版本一致。
|
|
/// </summary>
|
|
public PathSmoothingResult ReferencePath { get; }
|
|
|
|
/// <summary>
|
|
/// 规划使用的栅格地图快照引用；地图坐标单位和快照身份由地图契约定义，构造器不检查 null 或就绪状态。
|
|
/// </summary>
|
|
public PlanningGridMap Map { get; }
|
|
|
|
/// <summary>
|
|
/// 车辆几何与运动限制引用；调用方拥有该对象，服务在请求验证后使用其约束。
|
|
/// </summary>
|
|
public VehicleParameters Vehicle { get; }
|
|
|
|
/// <summary>
|
|
/// 一次性捕获的车辆运动状态；其位置为世界坐标 m、航向为 rad，时效和数值有效性由服务验证。
|
|
/// </summary>
|
|
public VehicleMotionState VehicleState { get; }
|
|
|
|
/// <summary>
|
|
/// 本次规划的配置引用；请求不深拷贝它，验证通过后服务取得配置副本供本次规划使用。
|
|
/// </summary>
|
|
public EmPlannerConfiguration Configuration { get; }
|
|
|
|
/// <summary>
|
|
/// 参考路径中待规划的方向段索引；必须由服务验证为有效的非负范围。
|
|
/// </summary>
|
|
public int SegmentIndex { get; }
|
|
|
|
/// <summary>
|
|
/// 可选的上一条已发布轨迹引用，用于衔接或诊断；构造器允许为 null，调用方保有其所有权。
|
|
/// </summary>
|
|
public EmTrajectory PreviousTrajectory { get; }
|
|
|
|
/// <summary>
|
|
/// 本次规划请求发起的 UTC 时刻；用于车辆状态时效判断和生成轨迹元数据，构造器不验证其时间关系。
|
|
/// </summary>
|
|
public DateTimeOffset RequestedAtUtc { get; }
|
|
|
|
/// <summary>
|
|
/// 成功发布轨迹开始生效的 UTC 时刻；由下游消费者依据其调度语义使用。
|
|
/// </summary>
|
|
public DateTimeOffset EffectiveAtUtc { get; }
|
|
|
|
/// <summary>
|
|
/// 成功轨迹的发布标识；构造器不校验，轨迹元数据构造时要求为非空白字符串。
|
|
/// </summary>
|
|
public string OutputTrajectoryId { get; }
|
|
|
|
/// <summary>
|
|
/// 输入参考路径的业务标识，用于成功轨迹的来源追踪；构造器不校验其 null 或空白值。
|
|
/// </summary>
|
|
public string ReferencePathId { get; }
|
|
|
|
/// <summary>
|
|
/// 上一条轨迹的业务标识；允许为 null，成功轨迹元数据会将 null 规范化为空字符串。
|
|
/// </summary>
|
|
public string PreviousTrajectoryId { get; }
|
|
|
|
/// <summary>
|
|
/// 请求的车辆运动模型；不支持或无效的枚举值由服务转换为失败状态。
|
|
/// </summary>
|
|
public EmMotionModel MotionModel { get; }
|
|
|
|
/// <summary>
|
|
/// 本次请求覆盖滚动窗口或整个方向段的范围；服务负责验证其枚举值和资源约束。
|
|
/// </summary>
|
|
public EmPlanningScope PlanningScope { get; }
|
|
|
|
/// <summary>Optional remaining wall-clock budget for the complete outer planning cycle.</summary>
|
|
public TimeSpan? CycleDeadlineRemaining { get; }
|
|
|
|
/// <summary>Dynamic caller-cancellation origin used to preserve terminal-status precedence.</summary>
|
|
public CancellationToken CallerCancellationToken { get; }
|
|
|
|
/// <summary>Dynamic shared-cycle deadline origin checked again at atomic publication.</summary>
|
|
public CancellationToken CycleDeadlineToken { get; }
|
|
|
|
internal IEmPlanningPublicationAuthorization PublicationAuthorization { get; }
|
|
|
|
internal bool IsCycleDeadlineExpired()
|
|
{
|
|
try
|
|
{
|
|
return PublicationAuthorization.IsExpired;
|
|
}
|
|
catch
|
|
{
|
|
return true;
|
|
}
|
|
}
|
|
|
|
internal TimeSpan? DynamicCycleDeadlineRemaining()
|
|
{
|
|
try
|
|
{
|
|
TimeSpan remaining = PublicationAuthorization.Remaining;
|
|
return remaining > TimeSpan.Zero ? remaining : TimeSpan.Zero;
|
|
}
|
|
catch
|
|
{
|
|
return TimeSpan.Zero;
|
|
}
|
|
}
|
|
|
|
internal EmPlanningPublicationDecision TryAuthorizePublication(
|
|
CancellationToken coordinatorCallerCancellationToken, Action publish)
|
|
{
|
|
if (publish == null)
|
|
throw new ArgumentNullException(nameof(publish));
|
|
try
|
|
{
|
|
return PublicationAuthorization.TryPublish(CallerCancellationToken,
|
|
coordinatorCallerCancellationToken, publish);
|
|
}
|
|
catch
|
|
{
|
|
return EmPlanningPublicationDecision.DeadlineExpired;
|
|
}
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// 为请求契约保留的 <see cref="EmPlannerConfiguration"/> 部分声明；实际配置成员由其他同名分部提供。
|
|
/// </summary>
|
|
public sealed partial class EmPlannerConfiguration
|
|
{
|
|
}
|