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;