From 4a7dd875a45a33db65803f4436ceb02d9cceed2f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E6=A2=81=E8=96=84=E4=BA=91?= Date: Tue, 4 Aug 2026 14:21:13 +0800 Subject: [PATCH] docs: document trajectory execution contracts --- .../EmPlanningCoordinator.cs | 26 ++++++++++++++++--- .../TrajectoryExecution/GearSwitchState.cs | 7 +++++ .../GearSwitchStateMachine.cs | 19 +++++++++++++- .../IEmPlanningCycleSink.cs | 4 ++- .../IVehicleStateProvider.cs | 4 ++- .../PlanningCycleIdentity.cs | 12 ++++++++- .../TrajectoryExecution/PlanningCycleInput.cs | 9 ++++++- .../PlanningCycleResult.cs | 13 +++++++++- .../TrajectoryControlAdapter.cs | 6 ++++- .../TrajectoryControlCommand.cs | 16 +++++++++++- .../TrajectoryExecutionState.cs | 5 +++- .../TrajectoryExecution/TrajectoryExecutor.cs | 23 ++++++++++++++-- .../TrajectoryHandoffSelector.cs | 16 ++++++++++-- .../TrajectoryExecution/TrajectorySampler.cs | 7 ++++- 14 files changed, 149 insertions(+), 18 deletions(-) diff --git a/ClumsyPilot/ParkrobTrajplanner/TrajectoryExecution/EmPlanningCoordinator.cs b/ClumsyPilot/ParkrobTrajplanner/TrajectoryExecution/EmPlanningCoordinator.cs index 6a0fcfa..98f0ac3 100644 --- a/ClumsyPilot/ParkrobTrajplanner/TrajectoryExecution/EmPlanningCoordinator.cs +++ b/ClumsyPilot/ParkrobTrajplanner/TrajectoryExecution/EmPlanningCoordinator.cs @@ -6,8 +6,8 @@ using MultiWheelC.TrajectoryPlanning.CoarsePath; namespace MultiWheelC.TrajectoryPlanning.EMPlanner; /// -/// Latest-wins rolling coordinator around the pure one-shot planning service. -/// It owns no localization, hardware, UI, or wall-clock source; callers supply each cycle's state and time. +/// 围绕纯单次 的 latest-wins 滚动规划协调器。 +/// 它只使用调用方提供的状态快照和时钟值;不读取定位、硬件、UI 或系统时钟,并且只原子发布当前身份的完整成功轨迹。 /// public sealed class EmPlanningCoordinator { @@ -22,12 +22,16 @@ public sealed class EmPlanningCoordinator private DateTimeOffset? lastCycleStartedAtUtc; private double replanPeriodSeconds = 0.20d; + /// 创建滚动规划协调器。 + /// 同步纯规划服务;每个周期在后台任务中调用其 Plan 方法。 + /// 可选周期完成观察器;观察器异常只追加诊断,不能回滚发布结果。 public EmPlanningCoordinator(IEmPlanningService planningService, IEmPlanningCycleSink sink = null) { this.planningService = planningService ?? throw new ArgumentNullException(nameof(planningService)); this.sink = sink; } + /// 最后一条已原子发布的完整不可变轨迹;尚无成功周期或失败周期未替换旧轨迹时可为 public EmTrajectory PublishedTrajectory { get @@ -37,7 +41,9 @@ public sealed class EmPlanningCoordinator } } - /// Returns whether a caller-supplied time is due for another rolling cycle. + /// 判断调用方提供的时刻是否到达下一次滚动规划周期。 + /// 调用方冻结的当前时间;协调器不读取墙上时钟。 + /// 尚未启动周期或距上次开始已达到配置周期时为 ;否则为 public bool ShouldStartCycle(DateTimeOffset now) { lock (publicationGate) @@ -47,6 +53,10 @@ public sealed class EmPlanningCoordinator } } + /// 异步启动一个 latest-wins 滚动周期,并取消此前仍在执行的周期。 + /// 调用方冻结的规划请求、完整周期身份和 caller-clocked 时刻;其地图、路径和状态版本必须一致。 + /// 调用方取消令牌;会与本周期内部令牌链接。 + /// 完成后给出不可变 ;只有版本和身份仍当前且结果成功完整时其 Published public Task PlanLatestAsync(PlanningCycleInput input, CancellationToken cancellationToken) { if (input == null) @@ -68,7 +78,10 @@ public sealed class EmPlanningCoordinator return Task.Run(() => CompleteCycle(version, input, cycleCancellation)); } - /// Builds a handoff decision from the coordinator's immutable published trajectory snapshot. + /// 从协调器当前已发布的不可变轨迹构建下一周期的安全交接决策。 + /// 下一周期的请求、状态快照和时刻。 + /// 下一周期期望的实际行驶方向。 + /// 同段、同方向、年龄和追踪误差均安全时返回未来轨迹样本;否则返回调用方测量状态和拒绝原因。 public TrajectoryHandoffSelection SelectHandoff(PlanningCycleInput input, TravelDirection expectedDirection) { if (input == null) @@ -81,6 +94,11 @@ public sealed class EmPlanningCoordinator input.Now, input.Request.Configuration); } + /// 执行一次后台纯规划并在锁内比较版本和身份后决定是否发布。 + /// 启动时分配的单调递增周期版本。 + /// 该周期的不可变请求、身份和 caller-clocked 时刻。 + /// 本周期与调用方取消状态链接的令牌源。 + /// 包含原始或 Superseded 规划结果、发布标记和观察器诊断的周期记录。 private PlanningCycleResult CompleteCycle(long version, PlanningCycleInput input, CancellationTokenSource cycleCancellation) { diff --git a/ClumsyPilot/ParkrobTrajplanner/TrajectoryExecution/GearSwitchState.cs b/ClumsyPilot/ParkrobTrajplanner/TrajectoryExecution/GearSwitchState.cs index caa87d7..81699d3 100644 --- a/ClumsyPilot/ParkrobTrajplanner/TrajectoryExecution/GearSwitchState.cs +++ b/ClumsyPilot/ParkrobTrajplanner/TrajectoryExecution/GearSwitchState.cs @@ -1,11 +1,18 @@ namespace MultiWheelC.TrajectoryPlanning.EMPlanner; +/// 轨迹执行器的换向安全状态;所有离开 Following 的状态都禁止非零运动,直到方向已确认。 public enum GearSwitchState { + /// 正常跟随当前方向段的轨迹运动。 Following, + /// 目标方向不同,正在接近精确换向边界。 ApproachingGearSwitch, + /// 位于换向边界并保持零速度,等待满足停稳 dwell。 HoldingZero, + /// 零速 dwell 已满足;本次更新发送一次方向变更请求。 RequestingDirectionChange, + /// 请求已发出,保持制动直到调用方确认实际方向改变。 AwaitingDirectionConfirmation, + /// 到达 Goal 或 RollingSafetyStop,永久保持零运动。 Completed, } diff --git a/ClumsyPilot/ParkrobTrajplanner/TrajectoryExecution/GearSwitchStateMachine.cs b/ClumsyPilot/ParkrobTrajplanner/TrajectoryExecution/GearSwitchStateMachine.cs index f8d5e53..ec53c0a 100644 --- a/ClumsyPilot/ParkrobTrajplanner/TrajectoryExecution/GearSwitchStateMachine.cs +++ b/ClumsyPilot/ParkrobTrajplanner/TrajectoryExecution/GearSwitchStateMachine.cs @@ -3,13 +3,16 @@ using MultiWheelC.TrajectoryPlanning.CoarsePath; namespace MultiWheelC.TrajectoryPlanning.EMPlanner; -/// Pure caller-clocked state machine for a zero-speed direction change. +/// 纯 caller-clocked 的零速换向状态机;只依据调用方时间、测量速度和方向确认推进状态。 public sealed class GearSwitchStateMachine { private readonly double stopSpeedToleranceMetersPerSecond; private readonly TimeSpan zeroSpeedHoldDuration; private DateTimeOffset? zeroSpeedSince; + /// 创建换向状态机。 + /// 判定已停稳的绝对带符号速度上限,单位 m/s。 + /// 发送方向请求前必须连续保持停稳的时长,单位 s。 public GearSwitchStateMachine(double stopSpeedToleranceMetersPerSecond, double zeroSpeedHoldSeconds) { if (!IsNonNegativeFinite(stopSpeedToleranceMetersPerSecond)) @@ -21,8 +24,18 @@ public sealed class GearSwitchStateMachine zeroSpeedHoldDuration = TimeSpan.FromSeconds(zeroSpeedHoldSeconds); } + /// 当前不可回退的换向状态;初始为 public GearSwitchState State { get; private set; } = GearSwitchState.Following; + /// 基于调用方时刻、测量速度和边界语义推进一次换向状态。 + /// 调用方当前时刻,用于计算零速 dwell,不读取系统时钟。 + /// 测量带符号纵向速度,单位 m/s。 + /// 轨迹要求的实际行驶方向。 + /// 调用方确认的当前实际方向。 + /// 调用方是否已确认硬件方向切换完成。 + /// 选中点是否为精确换向接近边界。 + /// 选中点是否为 Goal 或 RollingSafetyStop 终端。 + /// 包含停零、一次换向请求、是否允许轨迹运动和完成语义的不可变状态更新。 public GearSwitchStateUpdate Update(DateTimeOffset now, double measuredSignedSpeed, TravelDirection desiredDirection, TravelDirection currentDirection, bool directionConfirmed, bool atGearSwitchBoundary, bool atTerminal) { @@ -117,6 +130,7 @@ public sealed class GearSwitchStateMachine } } +/// 一次换向状态机更新的不可变安全决策,供执行器映射为零速度/制动或轨迹跟随命令。 public sealed class GearSwitchStateUpdate { internal GearSwitchStateUpdate(GearSwitchState state, bool holdZero, bool requestDirectionChange, @@ -130,14 +144,17 @@ public sealed class GearSwitchStateUpdate Reason = reason ?? string.Empty; } + /// 更新后的换向状态。 public GearSwitchState State { get; } public bool HoldZero { get; } + /// 是否仅在本次更新请求一次方向变更。 public bool RequestDirectionChange { get; } public bool AllowsTrajectoryMotion { get; } + /// 是否因 Goal 或 RollingSafetyStop 完成轨迹并需要持续制动。 public bool IsTrajectoryComplete { get; } public string Reason { get; } diff --git a/ClumsyPilot/ParkrobTrajplanner/TrajectoryExecution/IEmPlanningCycleSink.cs b/ClumsyPilot/ParkrobTrajplanner/TrajectoryExecution/IEmPlanningCycleSink.cs index ce102c2..75bb191 100644 --- a/ClumsyPilot/ParkrobTrajplanner/TrajectoryExecution/IEmPlanningCycleSink.cs +++ b/ClumsyPilot/ParkrobTrajplanner/TrajectoryExecution/IEmPlanningCycleSink.cs @@ -1,7 +1,9 @@ namespace MultiWheelC.TrajectoryPlanning.EMPlanner; -/// Optional observer for completed rolling planning cycles. +/// 滚动规划周期完成后的可选观察器;实现不得依赖其回调来维持发布正确性。 public interface IEmPlanningCycleSink { + /// 接收已经确定的周期完成记录。 + /// 不可变周期结果;观察器异常会被协调器隔离并仅追加诊断。 void OnCycleCompleted(PlanningCycleResult result); } diff --git a/ClumsyPilot/ParkrobTrajplanner/TrajectoryExecution/IVehicleStateProvider.cs b/ClumsyPilot/ParkrobTrajplanner/TrajectoryExecution/IVehicleStateProvider.cs index 6b9276b..9612cef 100644 --- a/ClumsyPilot/ParkrobTrajplanner/TrajectoryExecution/IVehicleStateProvider.cs +++ b/ClumsyPilot/ParkrobTrajplanner/TrajectoryExecution/IVehicleStateProvider.cs @@ -1,7 +1,9 @@ namespace MultiWheelC.TrajectoryPlanning.EMPlanner; -/// Execution-layer source of an immutable caller-owned vehicle-state snapshot. +/// 执行层的车辆状态快照来源;具体实现可在模块外读取定位或硬件,但核心只消费返回的不可变快照。 public interface IVehicleStateProvider { + /// 捕获一次车辆位姿、带符号纵向速度、可选加速度、时刻和序列号快照。 + /// 不可变 ;其位置单位为 m、航向为 rad、速度为 m/s。 VehicleMotionState Capture(); } diff --git a/ClumsyPilot/ParkrobTrajplanner/TrajectoryExecution/PlanningCycleIdentity.cs b/ClumsyPilot/ParkrobTrajplanner/TrajectoryExecution/PlanningCycleIdentity.cs index ba71ea1..083725e 100644 --- a/ClumsyPilot/ParkrobTrajplanner/TrajectoryExecution/PlanningCycleIdentity.cs +++ b/ClumsyPilot/ParkrobTrajplanner/TrajectoryExecution/PlanningCycleIdentity.cs @@ -2,9 +2,15 @@ using System; namespace MultiWheelC.TrajectoryPlanning.EMPlanner; -/// Immutable identity bound to one rolling planning cycle. +/// 绑定一次滚动周期的不可变版本身份;所有字段必须同时匹配,旧结果才可发布。 public sealed class PlanningCycleIdentity : IEquatable { + /// 创建完整周期身份。 + /// 规划地图快照的非负版本 ID。 + /// 平滑参考路径的非空版本 ID。 + /// 调用方捕获的车辆状态非负序列号。 + /// 上一条轨迹 ID;没有时使用空字符串。 + /// 当前前进或倒车方向段的非负索引。 public PlanningCycleIdentity(long mapSnapshotId, string referencePathId, long vehicleStateSequenceId, string previousTrajectoryId, int segmentIndex) { @@ -24,6 +30,7 @@ public sealed class PlanningCycleIdentity : IEquatable SegmentIndex = segmentIndex; } + /// 规划地图快照版本 ID。 public long MapSnapshotId { get; } public string ReferencePathId { get; } @@ -34,6 +41,9 @@ public sealed class PlanningCycleIdentity : IEquatable public int SegmentIndex { get; } + /// 从已冻结的 EM 请求派生发布身份。 + /// 必须携带地图和车辆状态快照的 EM 请求。 + /// 包含请求地图、路径、状态、上一轨迹和方向段字段的不可变身份。 public static PlanningCycleIdentity FromRequest(EmPlanningRequest request) { if (request == null) diff --git a/ClumsyPilot/ParkrobTrajplanner/TrajectoryExecution/PlanningCycleInput.cs b/ClumsyPilot/ParkrobTrajplanner/TrajectoryExecution/PlanningCycleInput.cs index 52b301d..411b80d 100644 --- a/ClumsyPilot/ParkrobTrajplanner/TrajectoryExecution/PlanningCycleInput.cs +++ b/ClumsyPilot/ParkrobTrajplanner/TrajectoryExecution/PlanningCycleInput.cs @@ -2,9 +2,12 @@ using System; namespace MultiWheelC.TrajectoryPlanning.EMPlanner; -/// Caller-captured request and clock value for one rolling cycle. +/// 一次滚动周期的调用方冻结请求、身份和时钟值;构造后不读取外部状态。 public sealed class PlanningCycleInput { + /// 从一次 EM 规划请求和调用方时刻创建周期输入。 + /// 已冻结的 EM 请求;用于派生地图、路径、状态、上一轨迹和方向段身份。 + /// 调用方提供的当前 ;用于周期调度,不由协调器替换。 public PlanningCycleInput(EmPlanningRequest request, DateTimeOffset now) { Request = request ?? throw new ArgumentNullException(nameof(request)); @@ -13,12 +16,16 @@ public sealed class PlanningCycleInput ReplanPeriodSeconds = ReadReplanPeriod(request.Configuration); } + /// 本周期消费的不可变 EM 规划请求快照。 public EmPlanningRequest Request { get; } + /// 派生的完整版本身份,用于拒绝过期发布。 public PlanningCycleIdentity Identity { get; } + /// 调用方冻结的周期开始时刻。 public DateTimeOffset Now { get; } + /// 从请求配置读取的重规划周期,单位 s;无效配置回退为 0.20 s。 public double ReplanPeriodSeconds { get; } private static double ReadReplanPeriod(EmPlannerConfiguration configuration) diff --git a/ClumsyPilot/ParkrobTrajplanner/TrajectoryExecution/PlanningCycleResult.cs b/ClumsyPilot/ParkrobTrajplanner/TrajectoryExecution/PlanningCycleResult.cs index 075b4eb..965ab8f 100644 --- a/ClumsyPilot/ParkrobTrajplanner/TrajectoryExecution/PlanningCycleResult.cs +++ b/ClumsyPilot/ParkrobTrajplanner/TrajectoryExecution/PlanningCycleResult.cs @@ -2,9 +2,15 @@ using System; namespace MultiWheelC.TrajectoryPlanning.EMPlanner; -/// Immutable completion record for one rolling planning cycle. +/// 一次滚动规划周期的不可变完成记录,区分规划结果与是否获准发布。 public sealed class PlanningCycleResult { + /// 创建周期完成记录。 + /// 协调器分配的正数单调周期版本。 + /// 本周期绑定的地图、路径、状态、上一轨迹和方向段身份。 + /// 纯规划服务返回的结果,可能为 Superseded、取消或失败。 + /// 仅当完整成功轨迹仍为当前身份时为 。 + /// 附加的周期或观察器诊断文本。 public PlanningCycleResult(long version, PlanningCycleIdentity identity, EmPlanningResult result, bool published, string diagnostic) { @@ -17,14 +23,19 @@ public sealed class PlanningCycleResult Diagnostic = diagnostic ?? string.Empty; } + /// 本周期的单调递增版本号。 public long Version { get; } + /// 决定该结果是否仍可发布的完整不可变身份。 public PlanningCycleIdentity Identity { get; } + /// 纯 EM 规划结果;失败结果不携带可执行轨迹。 public EmPlanningResult Result { get; } + /// 该结果是否已原子替换协调器的已发布轨迹。 public bool Published { get; } + /// 周期处理或可选观察器产生的诊断文本。 public string Diagnostic { get; } internal PlanningCycleResult WithDiagnostic(string diagnostic) diff --git a/ClumsyPilot/ParkrobTrajplanner/TrajectoryExecution/TrajectoryControlAdapter.cs b/ClumsyPilot/ParkrobTrajplanner/TrajectoryExecution/TrajectoryControlAdapter.cs index 117378b..05b5065 100644 --- a/ClumsyPilot/ParkrobTrajplanner/TrajectoryExecution/TrajectoryControlAdapter.cs +++ b/ClumsyPilot/ParkrobTrajplanner/TrajectoryExecution/TrajectoryControlAdapter.cs @@ -2,9 +2,13 @@ using System; namespace MultiWheelC.TrajectoryPlanning.EMPlanner; -/// Converts immutable execution state into a controller-neutral longitudinal and yaw command. +/// 将不可变执行状态转换为控制器中立的纵向速度和 yaw rate 命令,并在停稳/完成时强制零运动。 public sealed class TrajectoryControlAdapter { + /// 依据选中轨迹点和换向状态创建安全控制命令。 + /// 已验证轨迹中的选中点,提供带符号速度、yaw rate 和方向。 + /// 不可变执行状态;停零、完成或不允许运动时优先于轨迹点运动字段。 + /// 常规跟随时保留轨迹点 m/s 和 rad/s;保持或完成时返回零速度、零 yaw rate 和制动命令。 public TrajectoryControlCommand CreateCommand(EmTrajectoryPoint trajectoryPoint, TrajectoryExecutionState executionState) { diff --git a/ClumsyPilot/ParkrobTrajplanner/TrajectoryExecution/TrajectoryControlCommand.cs b/ClumsyPilot/ParkrobTrajplanner/TrajectoryExecution/TrajectoryControlCommand.cs index 8b29132..a5ed90f 100644 --- a/ClumsyPilot/ParkrobTrajplanner/TrajectoryExecution/TrajectoryControlCommand.cs +++ b/ClumsyPilot/ParkrobTrajplanner/TrajectoryExecution/TrajectoryControlCommand.cs @@ -3,9 +3,19 @@ using MultiWheelC.TrajectoryPlanning.CoarsePath; namespace MultiWheelC.TrajectoryPlanning.EMPlanner; -/// Immutable generic motion command derived from a validated trajectory execution state. +/// +/// 从已验证轨迹执行状态导出的不可变、控制器中立运动命令。 +/// 只表示带符号纵向速度和 yaw rate;不包含横向车体速度、蟹行、原地旋转、UI 或硬件字段。 +/// public sealed class TrajectoryControlCommand { + /// 创建控制器中立命令。 + /// 车体纵向速度,单位 m/s;正数前进、负数倒车,保持制动时必须为 0。 + /// 车辆 yaw rate,单位 rad/s;保持制动或完成时必须为 0。 + /// 命令关联的实际行驶方向。 + /// 仅在零速 dwell 完成后的单个更新周期请求方向变更。 + /// 时调用方必须保持零速度和零 yaw rate。 + /// 到达 Goal 或 RollingSafetyStop 后为 。 public TrajectoryControlCommand(double signedLongitudinalVelocity, double yawRate, TravelDirection direction, bool requestDirectionChange, bool holdBrake, bool isTrajectoryComplete) { @@ -24,14 +34,18 @@ public sealed class TrajectoryControlCommand IsTrajectoryComplete = isTrajectoryComplete; } + /// 权威带符号纵向速度,单位 m/s。 public double SignedLongitudinalVelocity { get; } + /// 逆时针为正的 yaw rate,单位 rad/s。 public double YawRate { get; } public TravelDirection Direction { get; } + /// 是否在本次更新请求一次方向切换。 public bool RequestDirectionChange { get; } + /// 是否要求调用方保持制动,不允许轨迹运动。 public bool HoldBrake { get; } public bool IsTrajectoryComplete { get; } diff --git a/ClumsyPilot/ParkrobTrajplanner/TrajectoryExecution/TrajectoryExecutionState.cs b/ClumsyPilot/ParkrobTrajplanner/TrajectoryExecution/TrajectoryExecutionState.cs index d115fcb..e6209df 100644 --- a/ClumsyPilot/ParkrobTrajplanner/TrajectoryExecution/TrajectoryExecutionState.cs +++ b/ClumsyPilot/ParkrobTrajplanner/TrajectoryExecution/TrajectoryExecutionState.cs @@ -2,7 +2,7 @@ using System; namespace MultiWheelC.TrajectoryPlanning.EMPlanner; -/// Immutable trajectory-execution outcome without controller or hardware coupling. +/// 不耦合控制器或硬件的不可变轨迹执行状态,组合选中点和换向状态机的安全语义。 public sealed class TrajectoryExecutionState { internal TrajectoryExecutionState(EmTrajectoryPoint selectedPoint, GearSwitchStateUpdate gearSwitchUpdate) @@ -19,14 +19,17 @@ public sealed class TrajectoryExecutionState Reason = gearSwitchUpdate.Reason; } + /// 按调用方时刻选中或插值得到的轨迹点;位置为 m、航向为 rad、速度为 m/s。 public EmTrajectoryPoint SelectedPoint { get; } public GearSwitchState GearSwitchState { get; } + /// 是否要求保持零速度和零 yaw rate。 public bool HoldZero { get; } public bool RequestDirectionChange { get; } + /// 状态机是否允许跟随选中点的非零轨迹运动。 public bool AllowsTrajectoryMotion { get; } public bool IsTrajectoryComplete { get; } diff --git a/ClumsyPilot/ParkrobTrajplanner/TrajectoryExecution/TrajectoryExecutor.cs b/ClumsyPilot/ParkrobTrajplanner/TrajectoryExecution/TrajectoryExecutor.cs index bfed9af..9598c97 100644 --- a/ClumsyPilot/ParkrobTrajplanner/TrajectoryExecution/TrajectoryExecutor.cs +++ b/ClumsyPilot/ParkrobTrajplanner/TrajectoryExecution/TrajectoryExecutor.cs @@ -3,18 +3,21 @@ using MultiWheelC.TrajectoryPlanning.CoarsePath; namespace MultiWheelC.TrajectoryPlanning.EMPlanner; -/// Samples immutable EM trajectories and applies the pure zero-speed gear-switch state machine. +/// 按调用方时刻采样不可变 EM 轨迹,并应用纯零速换向状态机;从不在轨迹末点之后外推。 public sealed class TrajectoryExecutor { private readonly TrajectorySampler sampler = new TrajectorySampler(); private readonly TrajectoryControlAdapter controlAdapter = new TrajectoryControlAdapter(); private readonly GearSwitchStateMachine gearSwitchStateMachine; + /// 使用默认 EM 配置创建执行器,默认停车容差和零速 dwell 来自该配置。 public TrajectoryExecutor() : this(EmPlannerConfiguration.CreateDefault()) { } + /// 使用指定 EM 配置创建执行器。 + /// 必须提供纵向停车速度容差(m/s)和零速 dwell(s)的配置快照。 public TrajectoryExecutor(EmPlannerConfiguration configuration) { if (configuration?.Longitudinal == null) @@ -24,8 +27,17 @@ public sealed class TrajectoryExecutor configuration.Longitudinal.ZeroSpeedHoldSeconds); } + /// 最近一次更新产生的不可变执行状态;尚未调用 时为 public TrajectoryExecutionState State { get; private set; } + /// 按调用方时刻选择轨迹点并更新换向状态机。 + /// 调用方当前时刻;相对轨迹生效时间换算为采样秒数。 + /// 车辆测量状态快照;带符号纵向速度单位 m/s,用于判断是否已停稳。 + /// 已发布的完整不可变轨迹;开始前取首点、结束后取末点且不外推。 + /// 下一方向段要求的实际行驶方向。 + /// 调用方确认的当前实际方向。 + /// 硬件/调用方是否已确认方向变更完成。 + /// 包含选中点、换向状态、停零、换向请求和完成标记的不可变执行状态。 public TrajectoryExecutionState Update(DateTimeOffset now, VehicleMotionState measuredState, EmTrajectory trajectory, TravelDirection desiredDirection, TravelDirection currentDirection, bool directionConfirmed) { @@ -45,7 +57,14 @@ public sealed class TrajectoryExecutor return State; } - /// Updates pure execution state and returns its controller-neutral motion command. + /// 更新纯执行状态并返回控制器中立命令。 + /// 调用方当前时刻。 + /// 车辆测量状态快照。 + /// 已发布完整轨迹。 + /// 目标实际行驶方向。 + /// 已确认当前方向。 + /// 是否已确认换向。 + /// 常规跟随时输出轨迹 m/s 与 rad/s;停零、等待确认或完成时输出零运动和制动。 public TrajectoryControlCommand UpdateCommand(DateTimeOffset now, VehicleMotionState measuredState, EmTrajectory trajectory, TravelDirection desiredDirection, TravelDirection currentDirection, bool directionConfirmed) diff --git a/ClumsyPilot/ParkrobTrajplanner/TrajectoryExecution/TrajectoryHandoffSelector.cs b/ClumsyPilot/ParkrobTrajplanner/TrajectoryExecution/TrajectoryHandoffSelector.cs index 3704347..506ecb9 100644 --- a/ClumsyPilot/ParkrobTrajplanner/TrajectoryExecution/TrajectoryHandoffSelector.cs +++ b/ClumsyPilot/ParkrobTrajplanner/TrajectoryExecution/TrajectoryHandoffSelector.cs @@ -3,12 +3,14 @@ using MultiWheelC.TrajectoryPlanning.CoarsePath; namespace MultiWheelC.TrajectoryPlanning.EMPlanner; +/// 下一轮规划起点的来源;只允许安全未来样本或调用方测量状态二选一。 public enum TrajectoryHandoffSource { MeasuredState, PreviousTrajectory, } +/// 拒绝使用已发布轨迹进行交接的稳定原因;拒绝时必须回退到测量状态。 public enum TrajectoryHandoffRejectionReason { None, @@ -25,7 +27,7 @@ public enum TrajectoryHandoffRejectionReason SampleUnavailable, } -/// Immutable handoff decision for the next one-shot planning request. +/// 下一次单次规划的不可变交接决策;不会跨方向段、方向或终端边界复用旧轨迹。 public sealed class TrajectoryHandoffSelection { internal TrajectoryHandoffSelection(TrajectoryHandoffSource source, VehicleMotionState startState, @@ -38,6 +40,7 @@ public sealed class TrajectoryHandoffSelection RejectionReason = rejectionReason; } + /// 交接使用未来旧轨迹样本还是调用方测量状态。 public TrajectoryHandoffSource Source { get; } public VehicleMotionState StartState { get; } @@ -49,12 +52,13 @@ public sealed class TrajectoryHandoffSelection public TrajectoryHandoffRejectionReason RejectionReason { get; } } -/// Selects a future same-segment seed or preserves the caller's measured state. +/// 选择安全的同段同方向未来轨迹种子;任一检查失败时保留调用方测量状态。 public sealed class TrajectoryHandoffSelector { private const double TimeEpsilonSeconds = 1e-9d; private readonly TrajectorySampler sampler; + /// 使用默认同质区间采样器创建交接选择器。 public TrajectoryHandoffSelector() : this(new TrajectorySampler()) { @@ -65,6 +69,14 @@ public sealed class TrajectoryHandoffSelector this.sampler = sampler ?? throw new ArgumentNullException(nameof(sampler)); } + /// 从已发布轨迹选择下一轮规划的未来交接状态。 + /// 最后一条完整已发布轨迹;为 时直接使用测量状态。 + /// 调用方当前车辆状态快照;位置 m、航向 rad、带符号速度 m/s。 + /// 下一轮请求期望的非负方向段索引。 + /// 下一轮请求期望的实际行驶方向。 + /// 调用方当前时刻,用于检查旧轨迹年龄和 lookahead。 + /// 包含最大状态年龄、lookahead 和追踪容差的配置快照。 + /// 只有年龄、追踪、同段同方向、末点和边界检查均通过时返回未来轨迹样本;否则返回测量状态及拒绝原因。 public TrajectoryHandoffSelection Select(EmTrajectory previousTrajectory, VehicleMotionState measuredState, int expectedSegmentIndex, TravelDirection expectedDirection, DateTimeOffset now, EmPlannerConfiguration configuration) diff --git a/ClumsyPilot/ParkrobTrajplanner/TrajectoryExecution/TrajectorySampler.cs b/ClumsyPilot/ParkrobTrajplanner/TrajectoryExecution/TrajectorySampler.cs index 25552bd..7f5e39e 100644 --- a/ClumsyPilot/ParkrobTrajplanner/TrajectoryExecution/TrajectorySampler.cs +++ b/ClumsyPilot/ParkrobTrajplanner/TrajectoryExecution/TrajectorySampler.cs @@ -2,11 +2,16 @@ using System; namespace MultiWheelC.TrajectoryPlanning.EMPlanner; -/// Interpolates an immutable trajectory only inside one homogeneous trajectory interval. +/// 仅在同一方向段、同一方向和同一边界类型的轨迹区间内插值不可变轨迹。 public sealed class TrajectorySampler { private const double TimeEpsilonSeconds = 1e-9d; + /// 按轨迹起点后的时间采样一个同质区间内的轨迹点。 + /// 完整不可变轨迹;其点序列必须按 TimeFromStart 严格递增。 + /// 相对轨迹起点的采样时刻,单位 s。 + /// 成功时为原始点或插值新点;失败时为 。 + /// 时刻位于轨迹范围内且不需跨方向段/方向/边界插值时为 ;区间外或跨边界时为 public bool TrySample(EmTrajectory trajectory, double timeFromStart, out EmTrajectoryPoint sampledPoint) { sampledPoint = null;