diff --git a/MedullaAdapter/build/Medulla/plugins/CommonUsage.dll b/MedullaAdapter/build/Medulla/plugins/CommonUsage.dll
index 9ff7b74..92a453a 100644
Binary files a/MedullaAdapter/build/Medulla/plugins/CommonUsage.dll and b/MedullaAdapter/build/Medulla/plugins/CommonUsage.dll differ
diff --git a/MedullaAdapter/build/Medulla/plugins/MedullaAdapter.dll b/MedullaAdapter/build/Medulla/plugins/MedullaAdapter.dll
index fb1eff4..6bb0299 100644
Binary files a/MedullaAdapter/build/Medulla/plugins/MedullaAdapter.dll and b/MedullaAdapter/build/Medulla/plugins/MedullaAdapter.dll differ
diff --git a/MedullaAdapter/build/Medulla/plugins/MedullaAdapter.pdb b/MedullaAdapter/build/Medulla/plugins/MedullaAdapter.pdb
index d91a710..a93d960 100644
Binary files a/MedullaAdapter/build/Medulla/plugins/MedullaAdapter.pdb and b/MedullaAdapter/build/Medulla/plugins/MedullaAdapter.pdb differ
diff --git a/MultiWheelC/Configuration/PilotConfig.ParkingControl.cs b/MultiWheelC/Configuration/PilotConfig.ParkingControl.cs
new file mode 100644
index 0000000..bfbaef3
--- /dev/null
+++ b/MultiWheelC/Configuration/PilotConfig.ParkingControl.cs
@@ -0,0 +1,171 @@
+using ClumsyCore;
+
+namespace MultiWheelC;
+
+///
+/// 定义停车状态估计、轨迹跟踪和原地自转使用的车辆级参数。
+///
+public partial class PilotConfig
+{
+ #region 停车控制-状态估计
+
+ [FieldMember(desc = "停车控制：Detour最大合理线速度(m/s)")]
+ public float ParkingDetourMaximumLinearSpeed = 1.20f;
+
+ [FieldMember(desc = "停车控制：Detour最大合理角速度(deg/s)")]
+ public float ParkingDetourMaximumAngularSpeedDegrees = 45f;
+
+ [FieldMember(desc = "停车控制：Detour位置跳变余量(m)")]
+ public float ParkingDetourPositionJumpMargin = 0.03f;
+
+ [FieldMember(desc = "停车控制：Detour航向跳变余量(deg)")]
+ public float ParkingDetourHeadingJumpMarginDegrees = 5f;
+
+ [FieldMember(desc = "停车控制：Detour速度预测位置残差(m)")]
+ public float ParkingDetourVelocityPositionResidual = 0.04f;
+
+ [FieldMember(desc = "停车控制：Detour速度预测航向残差(deg)")]
+ public float ParkingDetourVelocityHeadingResidualDegrees = 5f;
+
+ [FieldMember(desc = "停车控制：Detour静止确认时间(s)")]
+ public float ParkingDetourStationaryConfirmationSeconds = 0.35f;
+
+ [FieldMember(desc = "停车控制：Detour线速度滤波时间常数(s)")]
+ public float ParkingDetourLinearVelocityFilterSeconds = 0.15f;
+
+ [FieldMember(desc = "停车控制：Detour角速度滤波时间常数(s)")]
+ public float ParkingDetourAngularVelocityFilterSeconds = 0.20f;
+
+ [FieldMember(desc = "停车控制：电机反馈速度滤波时间常数(s)")]
+ public float ParkingWheelVelocityFilterSeconds = 0.10f;
+
+ #endregion
+
+ #region 停车控制-Stanley
+
+ [FieldMember(desc = "停车控制：Stanley横向误差增益(1/s)")]
+ public float ParkingStanleyCrossTrackGain = 0.4f;
+
+ [FieldMember(desc = "停车控制：Stanley航向误差增益")]
+ public float ParkingStanleyHeadingGain = 1.0f;
+
+ [FieldMember(desc = "停车控制：Stanley最低分母速度(m/s)")]
+ public float ParkingStanleyMinimumSpeed = 0.15f;
+
+ [FieldMember(desc = "停车控制：Stanley使用电机实际速度")]
+ public bool ParkingStanleyUseActualSpeed = true;
+
+ [FieldMember(desc = "停车控制：Stanley横向修正上限(deg)")]
+ public float ParkingMaximumCrossTrackCorrectionDegrees = 10f;
+
+ [FieldMember(desc = "停车控制：Stanley航向修正上限(deg)")]
+ public float ParkingMaximumHeadingCorrectionDegrees = 10f;
+
+ #endregion
+
+ #region 停车控制-纵向PID
+
+ [FieldMember(desc = "停车控制：纵向速度Kp")]
+ public float ParkingLongitudinalKp = 0.5f;
+
+ [FieldMember(desc = "停车控制：纵向速度Ki(1/s)")]
+ public float ParkingLongitudinalKi = 0f;
+
+ [FieldMember(desc = "停车控制：纵向速度Kd(s)")]
+ public float ParkingLongitudinalKd = 0f;
+
+ [FieldMember(desc = "停车控制：纵向积分修正上限(m/s)")]
+ public float ParkingMaximumIntegralCorrection = 0.05f;
+
+ [FieldMember(desc = "停车控制：纵向速度误差死区(m/s)")]
+ public float ParkingLongitudinalSpeedErrorDeadband = 0.025f;
+
+ [FieldMember(desc = "停车控制：最大命令速度(m/s)")]
+ public float ParkingMaximumCommandSpeed = 0.50f;
+
+ #endregion
+
+ #region 停车控制-原地自转
+
+ [FieldMember(desc = "停车控制：原地自转Kp")]
+ public float InPlaceRotateKp = 1.05f;
+
+ [FieldMember(desc = "停车控制：原地自转Ki")]
+ public float InPlaceRotateKi = 0f;
+
+ [FieldMember(desc = "停车控制：原地自转Kd")]
+ public float InPlaceRotateKd = 0f;
+
+ [FieldMember(desc = "停车控制：原地自转积分限幅")]
+ public float InPlaceRotateMaxI = 0f;
+
+ [FieldMember(desc = "停车控制：原地自转到位角度容差(deg)")]
+ public float InPlaceRotateArriveDeg = 1.5f;
+
+ [FieldMember(desc = "停车控制：原地自转起转前舵轮对齐容差(deg)")]
+ public float InPlaceRotateWheelAlignDeg = 2f;
+
+ [FieldMember(desc = "停车控制：原地自转最小有效角速度(deg/s)")]
+ public float InPlaceRotateMinimumSpeed = 1f;
+
+ [FieldMember(desc = "停车控制：原地自转最大角速度(deg/s)")]
+ public float InPlaceRotateMaxSpeed = 47.5f;
+
+ [FieldMember(desc = "停车控制：原地自转角加速度(deg/s²)")]
+ public float InPlaceRotateAcc = 60f;
+
+ [FieldMember(desc = "停车控制：原地自转超时(s)")]
+ public float InPlaceRotateTimeoutSec = 15f;
+
+ #endregion
+
+ #region 停车控制-舵轮回正
+
+ [FieldMember(desc = "停车控制：舵轮回正到位容差(deg)")]
+ public float ParkingWheelForwardToleranceDegrees = 2f;
+
+ [FieldMember(desc = "停车控制：舵轮回正稳定确认时间(s)")]
+ public float ParkingWheelForwardStableSeconds = 0.3f;
+
+ [FieldMember(desc = "停车控制：舵轮回正超时(s，0关闭)")]
+ public float ParkingWheelForwardTimeoutSeconds = 10f;
+
+ #endregion
+
+ #region 停车控制-GCP与完成条件
+
+ [FieldMember(desc = "停车控制：最大GCP转角(deg)")]
+ public float ParkingMaximumGcpAngleDegrees = 45f;
+
+ [FieldMember(desc = "停车控制：最大GCP转角速度(deg/s)")]
+ public float ParkingMaximumGcpAngleRateDegreesPerSecond = 15f;
+
+ [FieldMember(desc = "停车控制：终点距离容差(m)")]
+ public float ParkingFinishDistance = 0.03f;
+
+ [FieldMember(desc = "停车控制：终点速度容差(m/s)")]
+ public float ParkingFinishSpeed = 0.02f;
+
+ [FieldMember(desc = "停车控制：终点航向容差(deg)")]
+ public float ParkingFinishHeadingToleranceDegrees = 3f;
+
+ [FieldMember(desc = "停车控制：终点制动预瞄距离(m)")]
+ public float ParkingTerminalBrakingPreview = 0.02f;
+
+ [FieldMember(desc = "停车控制：终点单向逼近范围(m)")]
+ public float ParkingTerminalApproachDistance = 0.10f;
+
+ [FieldMember(desc = "停车控制：终点单向逼近增益(1/s)")]
+ public float ParkingTerminalApproachGain = 0.8f;
+
+ [FieldMember(desc = "停车控制：终点单向逼近最大速度(m/s)")]
+ public float ParkingTerminalMaximumApproachSpeed = 0.05f;
+
+ [FieldMember(desc = "停车控制：最大轨迹偏离距离(m)")]
+ public float ParkingMaximumDistanceToTrajectory = 0.30f;
+
+ [FieldMember(desc = "停车控制：轨迹执行超时(s)")]
+ public float ParkingExecutionTimeoutSeconds = 120f;
+
+ #endregion
+}
diff --git a/MultiWheelC/Control/Execution/ParkingGeometricController.cs b/MultiWheelC/Control/Execution/ParkingGeometricController.cs
index 633db00..015b7d1 100644
--- a/MultiWheelC/Control/Execution/ParkingGeometricController.cs
+++ b/MultiWheelC/Control/Execution/ParkingGeometricController.cs
@@ -1,4 +1,5 @@
using System;
+using System.Diagnostics;
using MultiWheelC.Control.Abstractions;
using MultiWheelC.Control.Allocation;
using MultiWheelC.StateEstimation;
@@ -19,6 +20,52 @@ namespace MultiWheelC.Control.Execution
Faulted = 4
}
+ ///
+ /// 保存一个轨迹控制周期各阶段的高精度耗时和定位帧更新状态。
+ ///
+ public readonly struct ParkingControlCycleTiming
+ {
+ public ParkingControlCycleTiming(
+ long cycleIndex,
+ double cycleIntervalMilliseconds,
+ double stateReadMilliseconds,
+ double projectionMilliseconds,
+ double controllerComputeMilliseconds,
+ double commandSendMilliseconds,
+ double totalCycleMilliseconds,
+ bool hasStateTimestamp,
+ double stateTimestampSeconds,
+ bool stateTimestampChanged,
+ ParkingControlCycleResult result)
+ {
+ CycleIndex = cycleIndex;
+ CycleIntervalMilliseconds =
+ cycleIntervalMilliseconds;
+ StateReadMilliseconds = stateReadMilliseconds;
+ ProjectionMilliseconds = projectionMilliseconds;
+ ControllerComputeMilliseconds =
+ controllerComputeMilliseconds;
+ CommandSendMilliseconds = commandSendMilliseconds;
+ TotalCycleMilliseconds = totalCycleMilliseconds;
+ HasStateTimestamp = hasStateTimestamp;
+ StateTimestampSeconds = stateTimestampSeconds;
+ StateTimestampChanged = stateTimestampChanged;
+ Result = result;
+ }
+
+ public long CycleIndex { get; }
+ public double CycleIntervalMilliseconds { get; }
+ public double StateReadMilliseconds { get; }
+ public double ProjectionMilliseconds { get; }
+ public double ControllerComputeMilliseconds { get; }
+ public double CommandSendMilliseconds { get; }
+ public double TotalCycleMilliseconds { get; }
+ public bool HasStateTimestamp { get; }
+ public double StateTimestampSeconds { get; }
+ public bool StateTimestampChanged { get; }
+ public ParkingControlCycleResult Result { get; }
+ }
+
///
/// 组织状态读取、轨迹投影、横纵向控制、GCP分配和底盘命令执行。
///
@@ -41,6 +88,10 @@ namespace MultiWheelC.Control.Execution
private readonly GcpCommandExecutor _commandExecutor;
private Trajectory2D _trajectory;
+ private double _terminalTravelDirection = 1.0;
+ private long _cycleIndex;
+ private bool _hasPreviousStateTimestamp;
+ private double _previousStateTimestampSeconds;
///
/// 创建具有终点判定和轨迹偏离保护的单车轨迹控制器。
@@ -56,7 +107,10 @@ namespace MultiWheelC.Control.Execution
double finishHeadingToleranceRadians =
3.0 * Math.PI / 180.0,
double maximumDistanceToTrajectoryMeters = 0.30,
- double terminalBrakingPreviewMeters = 0.02)
+ double terminalBrakingPreviewMeters = 0.02,
+ double terminalApproachDistanceMeters = 0.10,
+ double terminalApproachGainPerSecond = 0.8,
+ double maximumTerminalApproachSpeedMetersPerSecond = 0.05)
{
_stateProvider = stateProvider ??
throw new ArgumentNullException(
@@ -89,6 +143,23 @@ namespace MultiWheelC.Control.Execution
EnsureFiniteNonNegative(
terminalBrakingPreviewMeters,
nameof(terminalBrakingPreviewMeters));
+ EnsureFinitePositive(
+ terminalApproachDistanceMeters,
+ nameof(terminalApproachDistanceMeters));
+ EnsureFinitePositive(
+ terminalApproachGainPerSecond,
+ nameof(terminalApproachGainPerSecond));
+ EnsureFinitePositive(
+ maximumTerminalApproachSpeedMetersPerSecond,
+ nameof(maximumTerminalApproachSpeedMetersPerSecond));
+
+ if (terminalApproachDistanceMeters <=
+ finishDistanceMeters)
+ {
+ throw new ArgumentOutOfRangeException(
+ nameof(terminalApproachDistanceMeters),
+ "终点单向逼近范围必须大于终点位置容差。");
+ }
FinishDistanceMeters = finishDistanceMeters;
FinishSpeedMetersPerSecond =
@@ -99,6 +170,12 @@ namespace MultiWheelC.Control.Execution
maximumDistanceToTrajectoryMeters;
TerminalBrakingPreviewMeters =
terminalBrakingPreviewMeters;
+ TerminalApproachDistanceMeters =
+ terminalApproachDistanceMeters;
+ TerminalApproachGainPerSecond =
+ terminalApproachGainPerSecond;
+ MaximumTerminalApproachSpeedMetersPerSecond =
+ maximumTerminalApproachSpeedMetersPerSecond;
}
///
@@ -126,6 +203,21 @@ namespace MultiWheelC.Control.Execution
///
public double TerminalBrakingPreviewMeters { get; }
+ ///
+ /// 获取切换到终点单向低速逼近的最大剩余弧长，单位为m。
+ ///
+ public double TerminalApproachDistanceMeters { get; }
+
+ ///
+ /// 获取由终点纵向剩余距离生成低速参考的比例增益，单位为1/s。
+ ///
+ public double TerminalApproachGainPerSecond { get; }
+
+ ///
+ /// 获取终点单向逼近参考速度允许的最大绝对值，单位为m/s。
+ ///
+ public double MaximumTerminalApproachSpeedMetersPerSecond { get; }
+
///
/// 获取控制器当前是否持有并正在执行一条轨迹。
///
@@ -167,6 +259,11 @@ namespace MultiWheelC.Control.Execution
///
public double? LastControlReferenceSpeedMetersPerSecond { get; private set; }
+ ///
+ /// 获取最近一次控制周期的分阶段耗时诊断；尚未执行时为空。
+ ///
+ public ParkingControlCycleTiming? LastCycleTiming { get; private set; }
+
///
/// 停止当前底盘并从起点开始执行指定二维轨迹。
///
@@ -180,6 +277,8 @@ namespace MultiWheelC.Control.Execution
StopAndResetControllers();
_trajectory = trajectory;
+ _terminalTravelDirection =
+ ResolveTerminalTravelDirection(trajectory);
IsActive = true;
IsCompleted = false;
ClearDiagnostics();
@@ -200,40 +299,110 @@ namespace MultiWheelC.Control.Execution
return ParkingControlCycleResult.Inactive;
}
+ var cycleIndex = ++_cycleIndex;
+ var cycleStartTimestamp =
+ Stopwatch.GetTimestamp();
+ var stateReadMilliseconds = 0.0;
+ var projectionMilliseconds = 0.0;
+ var controllerComputeMilliseconds = 0.0;
+ var commandSendMilliseconds = 0.0;
+ var controllerComputeStartTimestamp = 0L;
+ var controllerComputeCompleted = false;
+ var hasStateTimestamp = false;
+ var stateTimestampSeconds = 0.0;
+ var stateTimestampChanged = false;
+ var cycleResult =
+ ParkingControlCycleResult.Faulted;
+
try
{
- if (!_stateProvider.TryGetState(
- out var vehicleState))
+ var stateReadStartTimestamp =
+ Stopwatch.GetTimestamp();
+ bool stateAvailable;
+ VehicleState vehicleState;
+ try
{
- StopForUnavailableState();
- return ParkingControlCycleResult
+ stateAvailable =
+ _stateProvider.TryGetState(
+ out vehicleState);
+ }
+ finally
+ {
+ stateReadMilliseconds =
+ GetElapsedMilliseconds(
+ stateReadStartTimestamp);
+ }
+
+ if (!stateAvailable)
+ {
+ var stopCommandStartTimestamp =
+ Stopwatch.GetTimestamp();
+ try
+ {
+ StopForUnavailableState();
+ }
+ finally
+ {
+ commandSendMilliseconds =
+ GetElapsedMilliseconds(
+ stopCommandStartTimestamp);
+ }
+
+ cycleResult = ParkingControlCycleResult
.StateUnavailable;
+ return cycleResult;
}
LastVehicleState = vehicleState;
+ hasStateTimestamp = true;
+ stateTimestampSeconds =
+ vehicleState.SampleTimestampSeconds;
+ stateTimestampChanged =
+ !_hasPreviousStateTimestamp ||
+ stateTimestampSeconds !=
+ _previousStateTimestampSeconds;
+ _previousStateTimestampSeconds =
+ stateTimestampSeconds;
+ _hasPreviousStateTimestamp = true;
// 首周期允许全局定位轨迹进度；后续周期仅在上次进度
// 前后有限物理距离内搜索，避免交叉或平行轨迹间跳段。
- var projection = LastProjection.HasValue
- ? TrajectoryProjector.Project(
- _trajectory,
- vehicleState.PoseInWorld,
- LastProjection.Value.ArcLengthMeters,
- ProjectionBackwardSearchDistanceMeters,
- ProjectionForwardSearchDistanceMeters)
- : TrajectoryProjector.Project(
- _trajectory,
- vehicleState.PoseInWorld);
+ var projectionStartTimestamp =
+ Stopwatch.GetTimestamp();
+ TrajectoryProjection projection;
+ try
+ {
+ projection = LastProjection.HasValue
+ ? TrajectoryProjector.Project(
+ _trajectory,
+ vehicleState.PoseInWorld,
+ LastProjection.Value.ArcLengthMeters,
+ ProjectionBackwardSearchDistanceMeters,
+ ProjectionForwardSearchDistanceMeters)
+ : TrajectoryProjector.Project(
+ _trajectory,
+ vehicleState.PoseInWorld);
+ }
+ finally
+ {
+ projectionMilliseconds =
+ GetElapsedMilliseconds(
+ projectionStartTimestamp);
+ }
+
LastProjection = projection;
+ controllerComputeStartTimestamp =
+ Stopwatch.GetTimestamp();
if (projection.DistanceToTrajectoryMeters >
MaximumDistanceToTrajectoryMeters)
{
- return EnterFault(
+ cycleResult = EnterFault(
"车辆距离参考轨迹" +
$"{projection.DistanceToTrajectoryMeters:F3}m，" +
"超过允许值" +
$"{MaximumDistanceToTrajectoryMeters:F3}m。");
+ return cycleResult;
}
if (HasReachedEnd(
@@ -241,7 +410,9 @@ namespace MultiWheelC.Control.Execution
projection))
{
CompleteTrajectory();
- return ParkingControlCycleResult.Completed;
+ cycleResult =
+ ParkingControlCycleResult.Completed;
+ return cycleResult;
}
if (HasStoppedAtUnsatisfiedTerminal(
@@ -249,12 +420,14 @@ namespace MultiWheelC.Control.Execution
projection,
out var terminalFailureReason))
{
- return EnterFault(
+ cycleResult = EnterFault(
terminalFailureReason);
+ return cycleResult;
}
var controlReferenceSpeedMetersPerSecond =
- ResolveReferenceSpeedForControl(
+ ResolveControlReferenceSpeed(
+ vehicleState,
projection);
LastControlReferenceSpeedMetersPerSecond =
controlReferenceSpeedMetersPerSecond;
@@ -272,15 +445,36 @@ namespace MultiWheelC.Control.Execution
commandSpeedMetersPerSecond,
lateralCommand);
- if (!_commandExecutor.Execute(
- gcpCommand,
- deltaTimeSeconds))
+ controllerComputeMilliseconds =
+ GetElapsedMilliseconds(
+ controllerComputeStartTimestamp);
+ controllerComputeCompleted = true;
+
+ var commandSendStartTimestamp =
+ Stopwatch.GetTimestamp();
+ bool commandSucceeded;
+ try
{
- return EnterFault(
+ commandSucceeded =
+ _commandExecutor.Execute(
+ gcpCommand,
+ deltaTimeSeconds);
+ }
+ finally
+ {
+ commandSendMilliseconds =
+ GetElapsedMilliseconds(
+ commandSendStartTimestamp);
+ }
+
+ if (!commandSucceeded)
+ {
+ cycleResult = EnterFault(
string.IsNullOrWhiteSpace(
_commandExecutor.LastFailureReason)
? "GCP底盘命令执行失败。"
: _commandExecutor.LastFailureReason);
+ return cycleResult;
}
LastCommand =
@@ -288,14 +482,42 @@ namespace MultiWheelC.Control.Execution
LastFailureReason = string.Empty;
LastException = null;
- return ParkingControlCycleResult.CommandSent;
+ cycleResult =
+ ParkingControlCycleResult.CommandSent;
+ return cycleResult;
}
catch (Exception exception)
{
- return EnterFault(
+ cycleResult = EnterFault(
"停车机器人轨迹控制周期异常：" +
exception.Message,
exception);
+ return cycleResult;
+ }
+ finally
+ {
+ if (controllerComputeStartTimestamp != 0L &&
+ !controllerComputeCompleted)
+ {
+ controllerComputeMilliseconds =
+ GetElapsedMilliseconds(
+ controllerComputeStartTimestamp);
+ }
+
+ LastCycleTiming =
+ new ParkingControlCycleTiming(
+ cycleIndex,
+ deltaTimeSeconds * 1000.0,
+ stateReadMilliseconds,
+ projectionMilliseconds,
+ controllerComputeMilliseconds,
+ commandSendMilliseconds,
+ GetElapsedMilliseconds(
+ cycleStartTimestamp),
+ hasStateTimestamp,
+ stateTimestampSeconds,
+ stateTimestampChanged,
+ cycleResult);
}
}
@@ -306,11 +528,84 @@ namespace MultiWheelC.Control.Execution
{
StopAndResetControllers();
_trajectory = null;
+ _terminalTravelDirection = 1.0;
IsActive = false;
IsCompleted = false;
ClearDiagnostics();
}
+ ///
+ /// 在普通空间速度规划和终点单向低速逼近之间选择本周期参考速度。
+ ///
+ private double ResolveControlReferenceSpeed(
+ VehicleState vehicleState,
+ TrajectoryProjection projection)
+ {
+ if (projection.RemainingDistanceMeters >
+ TerminalApproachDistanceMeters)
+ {
+ return ResolveReferenceSpeedForControl(
+ projection);
+ }
+
+ return ResolveTerminalApproachSpeed(
+ vehicleState);
+ }
+
+ ///
+ /// 根据终点车头方向上的有符号剩余距离生成只保持原轨迹行驶方向的低速参考。
+ ///
+ private double ResolveTerminalApproachSpeed(
+ VehicleState vehicleState)
+ {
+ var distanceToEndMeters =
+ CalculateDistanceToEndMeters(
+ vehicleState);
+ var headingErrorToEndRadians =
+ CalculateHeadingErrorToEndRadians(
+ vehicleState);
+
+ // 一旦位置和航向已经进入完成容差，先要求纵向停车；
+ // 后续周期在实际速度也满足条件后完成轨迹。
+ if (distanceToEndMeters <=
+ FinishDistanceMeters &&
+ headingErrorToEndRadians <=
+ FinishHeadingToleranceRadians)
+ {
+ return 0.0;
+ }
+
+ var endPose =
+ _trajectory.EndPoint.PoseInWorld;
+ var deltaX =
+ endPose.XMeters -
+ vehicleState.PoseInWorld.XMeters;
+ var deltaY =
+ endPose.YMeters -
+ vehicleState.PoseInWorld.YMeters;
+ var longitudinalErrorMeters =
+ deltaX * Math.Cos(endPose.YawRadians) +
+ deltaY * Math.Sin(endPose.YawRadians);
+ var remainingAlongTravelMeters =
+ _terminalTravelDirection *
+ longitudinalErrorMeters;
+
+ // 到达或越过终点后只停车，禁止生成与原轨迹方向相反的修正速度。
+ if (remainingAlongTravelMeters <= 0.0)
+ {
+ return 0.0;
+ }
+
+ var speedMagnitudeMetersPerSecond =
+ Math.Min(
+ MaximumTerminalApproachSpeedMetersPerSecond,
+ TerminalApproachGainPerSecond *
+ remainingAlongTravelMeters);
+
+ return _terminalTravelDirection *
+ speedMagnitudeMetersPerSecond;
+ }
+
///
/// 在轨迹起步区域内保持最低释放速度，避免空间速度曲线零速固定点。
///
@@ -414,6 +709,33 @@ namespace MultiWheelC.Control.Execution
return currentReferenceSpeed;
}
+ ///
+ /// 从轨迹终点前最后一个非零参考速度解析单向逼近允许的行驶方向。
+ ///
+ private static double ResolveTerminalTravelDirection(
+ Trajectory2D trajectory)
+ {
+ for (var index = trajectory.Count - 1;
+ index >= 0;
+ index--)
+ {
+ var referenceSpeedMetersPerSecond =
+ trajectory[index]
+ .ReferenceSpeedMetersPerSecond;
+
+ if (Math.Abs(referenceSpeedMetersPerSecond) >
+ ZeroReferenceSpeedToleranceMetersPerSecond)
+ {
+ return Math.Sign(
+ referenceSpeedMetersPerSecond);
+ }
+ }
+
+ throw new ArgumentException(
+ "轨迹必须在终点前包含至少一个非零参考速度，才能确定终点单向逼近方向。",
+ nameof(trajectory));
+ }
+
///
/// 根据终点距离、剩余弧长和实际线速度判断轨迹是否完成。
///
@@ -585,14 +907,29 @@ namespace MultiWheelC.Control.Execution
///
private void ClearDiagnostics()
{
+ _cycleIndex = 0;
+ _hasPreviousStateTimestamp = false;
+ _previousStateTimestampSeconds = 0.0;
LastVehicleState = null;
LastProjection = null;
LastCommand = null;
LastControlReferenceSpeedMetersPerSecond = null;
+ LastCycleTiming = null;
LastFailureReason = string.Empty;
LastException = null;
}
+ ///
+ /// 将Stopwatch高精度时间戳差转换为毫秒。
+ ///
+ private static double GetElapsedMilliseconds(
+ long startTimestamp)
+ {
+ return (Stopwatch.GetTimestamp() - startTimestamp) *
+ 1000.0 /
+ Stopwatch.Frequency;
+ }
+
///
/// 检查控制参数是否为正有限值。
///
diff --git a/MultiWheelC/Experiments/CompositeMotionPlanTests.cs b/MultiWheelC/Experiments/CompositeMotionPlanTests.cs
index e335698..2734fe5 100644
--- a/MultiWheelC/Experiments/CompositeMotionPlanTests.cs
+++ b/MultiWheelC/Experiments/CompositeMotionPlanTests.cs
@@ -178,6 +178,7 @@ namespace MultiWheelC
},
TrackingCycleObserver = (index, controller) =>
RecordTrackingCycle(
+ index,
controller,
controlPointRadiusMeters,
stateProvider),
@@ -281,10 +282,18 @@ namespace MultiWheelC
/// 将轨迹控制周期使用的状态、参考误差和最终GCP命令写入记录器。
///
private void RecordTrackingCycle(
+ int motionSegmentIndex,
ParkingGeometricController controller,
double controlPointRadiusMeters,
WheelFeedbackVehicleStateProvider stateProvider)
{
+ if (controller.LastCycleTiming.HasValue)
+ {
+ _recorder?.RecordControlCycleTiming(
+ controller.LastCycleTiming.Value,
+ motionSegmentIndex);
+ }
+
if (controller.LastVehicleState.HasValue)
{
_recorder?.UpdateProcessedState(
diff --git a/MultiWheelC/Experiments/NewControllerTrackingTests.cs b/MultiWheelC/Experiments/NewControllerTrackingTests.cs
index 950d528..62d320a 100644
--- a/MultiWheelC/Experiments/NewControllerTrackingTests.cs
+++ b/MultiWheelC/Experiments/NewControllerTrackingTests.cs
@@ -333,6 +333,12 @@ namespace MultiWheelC
double controlPointRadiusMeters,
WheelFeedbackVehicleStateProvider stateProvider)
{
+ if (controller.LastCycleTiming.HasValue)
+ {
+ recorder.RecordControlCycleTiming(
+ controller.LastCycleTiming.Value);
+ }
+
if (controller.LastVehicleState.HasValue)
{
recorder.UpdateProcessedState(
@@ -690,6 +696,12 @@ namespace MultiWheelC
double controlPointRadiusMeters,
WheelFeedbackVehicleStateProvider stateProvider)
{
+ if (controller.LastCycleTiming.HasValue)
+ {
+ recorder.RecordControlCycleTiming(
+ controller.LastCycleTiming.Value);
+ }
+
if (controller.LastVehicleState.HasValue)
{
recorder.UpdateProcessedState(
diff --git a/MultiWheelC/Experiments/TrackingExperimentRecorder.cs b/MultiWheelC/Experiments/TrackingExperimentRecorder.cs
index 1bbcc21..55a3c5e 100644
--- a/MultiWheelC/Experiments/TrackingExperimentRecorder.cs
+++ b/MultiWheelC/Experiments/TrackingExperimentRecorder.cs
@@ -9,6 +9,7 @@ using System.Text;
using System.Threading;
using CommonUsage.Chassis;
using MyParking.Shared;
+using MultiWheelC.Control.Execution;
using MultiWheelC.StateEstimation;
namespace MultiWheelC
@@ -75,6 +76,27 @@ namespace MultiWheelC
// C层实验工具：统一采集并保存轨迹跟踪实验数据。
public sealed class TrackingExperimentRecorder
{
+ ///
+ /// 保存一次控制周期计时及其所属组合运动段索引。
+ ///
+ private readonly struct ControlCycleTimingRecord
+ {
+ public ControlCycleTimingRecord(
+ double recorderElapsedSeconds,
+ int motionSegmentIndex,
+ ParkingControlCycleTiming timing)
+ {
+ RecorderElapsedSeconds =
+ recorderElapsedSeconds;
+ MotionSegmentIndex = motionSegmentIndex;
+ Timing = timing;
+ }
+
+ public double RecorderElapsedSeconds { get; }
+ public int MotionSegmentIndex { get; }
+ public ParkingControlCycleTiming Timing { get; }
+ }
+
private readonly string _controllerName;
private readonly string _trajectoryName;
private readonly int _trialNumber;
@@ -91,9 +113,16 @@ namespace MultiWheelC
private readonly List _samples =
new List();
+ private readonly List
+ _controlCycleTimings =
+ new List();
+
private readonly object _sampleSyncRoot =
new object();
+ private readonly object _controlCycleTimingSyncRoot =
+ new object();
+
private readonly object _commandSyncRoot =
new object();
@@ -180,6 +209,11 @@ namespace MultiWheelC
// 保存成功后的CSV绝对路径；尚未保存时为空。
public string SavedFilePath { get; private set; }
+ ///
+ /// 获取逐控制周期计时CSV的绝对路径；本次实验没有计时数据时为空。
+ ///
+ public string SavedTimingFilePath { get; private set; }
+
///
/// 获取Clumsy当前运行目录下统一保存轨迹实验CSV的文件夹。
///
@@ -245,6 +279,24 @@ namespace MultiWheelC
}
}
+ ///
+ /// 将一个真实控制周期的分阶段耗时追加到内存，实验结束后统一保存。
+ ///
+ public void RecordControlCycleTiming(
+ ParkingControlCycleTiming timing,
+ int motionSegmentIndex = -1)
+ {
+ var record = new ControlCycleTimingRecord(
+ _stopwatch.Elapsed.TotalSeconds,
+ motionSegmentIndex,
+ timing);
+
+ lock (_controlCycleTimingSyncRoot)
+ {
+ _controlCycleTimings.Add(record);
+ }
+ }
+
///
/// 保存新版控制器本周期实际使用的校验后车辆状态，供后台采样线程写入CSV。
///
@@ -374,9 +426,17 @@ namespace MultiWheelC
CaptureSample();
_stopwatch.Stop();
SaveCsv();
+ SaveControlCycleTimingCsv();
Console.WriteLine(
$"轨迹实验数据已保存：{SavedFilePath}");
+ if (!string.IsNullOrWhiteSpace(
+ SavedTimingFilePath))
+ {
+ Console.WriteLine(
+ "控制周期计时数据已保存：" +
+ SavedTimingFilePath);
+ }
}
catch
{
@@ -909,6 +969,106 @@ namespace MultiWheelC
}
}
+ ///
+ /// 将逐控制周期的内存计时数据保存为独立CSV，不受后台采样周期限制。
+ ///
+ private void SaveControlCycleTimingCsv()
+ {
+ List snapshot;
+
+ lock (_controlCycleTimingSyncRoot)
+ {
+ snapshot =
+ new List(
+ _controlCycleTimings);
+ }
+
+ if (snapshot.Count == 0)
+ {
+ SavedTimingFilePath = null;
+ return;
+ }
+
+ SavedTimingFilePath = Path.Combine(
+ Path.GetDirectoryName(SavedFilePath) ??
+ DefaultOutputDirectory,
+ Path.GetFileNameWithoutExtension(
+ SavedFilePath) +
+ "_timing.csv");
+
+ using (var writer = new StreamWriter(
+ SavedTimingFilePath,
+ false,
+ new UTF8Encoding(true)))
+ {
+ writer.WriteLine(
+ "ControlCycleEndElapsedSeconds," +
+ "ControllerName," +
+ "TrajectoryName," +
+ "TrialNumber," +
+ "MotionSegmentIndex," +
+ "ControlCycleIndex," +
+ "ControlCycleIntervalMs," +
+ "StateReadMs," +
+ "ProjectionMs," +
+ "ControllerComputeMs," +
+ "CommandSendMs," +
+ "OtherMs," +
+ "TotalCycleMs," +
+ "HasStateTimestamp," +
+ "StateTimestampSeconds," +
+ "StateTimestampChanged," +
+ "CycleResult");
+
+ foreach (var record in snapshot)
+ {
+ var timing = record.Timing;
+ var measuredStageMilliseconds =
+ timing.StateReadMilliseconds +
+ timing.ProjectionMilliseconds +
+ timing.ControllerComputeMilliseconds +
+ timing.CommandSendMilliseconds;
+ var otherMilliseconds = Math.Max(
+ 0.0,
+ timing.TotalCycleMilliseconds -
+ measuredStageMilliseconds);
+
+ writer.WriteLine(string.Join(
+ ",",
+ Format(record.RecorderElapsedSeconds),
+ EscapeCsv(_controllerName),
+ EscapeCsv(_trajectoryName),
+ _trialNumber.ToString(
+ CultureInfo.InvariantCulture),
+ record.MotionSegmentIndex.ToString(
+ CultureInfo.InvariantCulture),
+ timing.CycleIndex.ToString(
+ CultureInfo.InvariantCulture),
+ Format(
+ timing.CycleIntervalMilliseconds),
+ Format(timing.StateReadMilliseconds),
+ Format(timing.ProjectionMilliseconds),
+ Format(
+ timing.ControllerComputeMilliseconds),
+ Format(timing.CommandSendMilliseconds),
+ Format(otherMilliseconds),
+ Format(timing.TotalCycleMilliseconds),
+ timing.HasStateTimestamp
+ ? "1"
+ : "0",
+ FormatOptional(
+ timing.HasStateTimestamp,
+ timing.StateTimestampSeconds),
+ timing.HasStateTimestamp
+ ? timing.StateTimestampChanged
+ ? "1"
+ : "0"
+ : string.Empty,
+ EscapeCsv(timing.Result.ToString())));
+ }
+ }
+ }
+
// 将文件名中的非法字符替换为下划线。
private static string SanitizeFileName(string value)
{
diff --git a/MultiWheelC/Movements/TrajectoryTrackingMovement.cs b/MultiWheelC/Movements/TrajectoryTrackingMovement.cs
index 795d939..d7fd0b0 100644
--- a/MultiWheelC/Movements/TrajectoryTrackingMovement.cs
+++ b/MultiWheelC/Movements/TrajectoryTrackingMovement.cs
@@ -132,6 +132,21 @@ namespace MultiWheelC
///
public double? TerminalBrakingPreviewMeters;
+ ///
+ /// 获取或设置本次动作进入终点单向低速逼近的剩余弧长覆盖值，单位为m；为空时读取车辆配置。
+ ///
+ public double? TerminalApproachDistanceMeters;
+
+ ///
+ /// 获取或设置本次动作由终点纵向剩余距离生成低速参考的比例增益覆盖值，单位为1/s；为空时读取车辆配置。
+ ///
+ public double? TerminalApproachGainPerSecond;
+
+ ///
+ /// 获取或设置本次动作终点单向逼近参考速度的最大绝对值覆盖值，单位为m/s；为空时读取车辆配置。
+ ///
+ public double? MaximumTerminalApproachSpeedMetersPerSecond;
+
///
/// 获取或设置本次动作的最大轨迹偏离距离覆盖值，单位为m；为空时读取车辆配置。
///
@@ -212,6 +227,15 @@ namespace MultiWheelC
var terminalBrakingPreviewMeters =
TerminalBrakingPreviewMeters ??
config.ParkingTerminalBrakingPreview;
+ var terminalApproachDistanceMeters =
+ TerminalApproachDistanceMeters ??
+ config.ParkingTerminalApproachDistance;
+ var terminalApproachGainPerSecond =
+ TerminalApproachGainPerSecond ??
+ config.ParkingTerminalApproachGain;
+ var maximumTerminalApproachSpeedMetersPerSecond =
+ MaximumTerminalApproachSpeedMetersPerSecond ??
+ config.ParkingTerminalMaximumApproachSpeed;
var maximumDistanceToTrajectoryMeters =
MaximumDistanceToTrajectoryMeters ??
config.ParkingMaximumDistanceToTrajectory;
@@ -305,7 +329,10 @@ namespace MultiWheelC
finishSpeedMetersPerSecond,
finishHeadingToleranceRadians,
maximumDistanceToTrajectoryMeters,
- terminalBrakingPreviewMeters);
+ terminalBrakingPreviewMeters,
+ terminalApproachDistanceMeters,
+ terminalApproachGainPerSecond,
+ maximumTerminalApproachSpeedMetersPerSecond);
var clock = Stopwatch.StartNew();
var previousCycleSeconds =
@@ -342,10 +369,9 @@ namespace MultiWheelC
Controller.ExecuteCycle(
deltaTimeSeconds);
- if (Controller.LastVehicleState.HasValue)
- {
- CycleObserver?.Invoke(Controller);
- }
+ // 诊断观察器按真实控制周期触发，即使本周期状态不可用，
+ // 也允许记录状态读取和主动停车所消耗的时间。
+ CycleObserver?.Invoke(Controller);
if (result ==
ParkingControlCycleResult.Completed)
diff --git a/MultiWheelC/Old/AGV.cs b/MultiWheelC/Old/AGV.cs
new file mode 100644
index 0000000..fe3b55a
--- /dev/null
+++ b/MultiWheelC/Old/AGV.cs
@@ -0,0 +1,21 @@
+// using ClumsyCore;
+// using MDCSToolBox.Clumsy.AgvInterfaces;
+// using MDCSToolBox.Clumsy.MotionControllers;
+
+// namespace MultiWheelC
+// {
+// public class AGV : MultiWheelInterface
+// {
+// public override AbstractGeometricController GetController()
+// => new ChassisController().Get();
+// public override MultiWheelMagTracker GetMagController()
+// => new MultiWheelMagTracker();
+// public override NaiveMagnetController GetNaiveMagnetController()
+// => new NaiveMagnetController();
+
+// public void Sleep(float seconds)
+// {
+// new DriveTask(new Sleep { Second = seconds }.Get()).Wait();
+// }
+// }
+// }
diff --git a/MultiWheelC/Old/ChassisController.cs b/MultiWheelC/Old/ChassisController.cs
new file mode 100644
index 0000000..3652948
--- /dev/null
+++ b/MultiWheelC/Old/ChassisController.cs
@@ -0,0 +1,45 @@
+// using ClumsyCore;
+// using ClumsyCore.Pilot;
+// using MDCSToolBox.Clumsy.MotionControllers;
+// using MDCSToolBox.Clumsy.Movements;
+// using MDCSToolBox.Clumsy.Pilot;
+
+// namespace MultiWheelC;
+
+// public class ChassisController : MovementDefinition
+// {
+// public float BaseSpeed = Configuration.conf.basicSpeed;
+
+// // 创建单车几何跟踪控制器（直接控本车底盘，不走多车 Auto 通道）
+// public override MultiWheelGeometricController Get()
+// {
+// return new MultiWheelGeometricController
+// {
+// Chassis = BasicPilotBase.Chassis,
+// BaseSpeed = BaseSpeed,
+// SlowDistance = PilotDefinition.Conf.SlowDistance,
+// SlowingPow = PilotDefinition.Conf.SlowingPow,
+// FinishDistance = PilotDefinition.Conf.FinishDistance,
+// FinishSpeed = PilotDefinition.Conf.FinishSpeed,
+// FirstThAccuracy = PilotDefinition.Conf.FirstThAccuracy,
+// FirstRotateSpeedFac = PilotDefinition.Conf.FirstRotateSpeedFac,
+// FirstRotateMaxSpeed = PilotDefinition.Conf.FirstRotateMaxSpeed,
+// NotContinuousAngle = PilotDefinition.Conf.NotContinuousAngle,
+// DebugMode = PilotDefinition.Conf.MotionDebugPrint,
+// DebugCurvature = PilotDefinition.Conf.DebugCurvature,
+// PowerSteeringLookAhead = PilotDefinition.Conf.PowerSteeringLookAhead,
+// SpeedLookAhead = PilotDefinition.Conf.SpeedLookAhead,
+// SpeedLookAheadCurveDiff = PilotDefinition.Conf.SpeedLookAheadCurveDiff,
+// SpeedLookBackCurveDiff = PilotDefinition.Conf.SpeedLookBackCurveDiff,
+// SpeedLimitCurveDiffMin = PilotDefinition.Conf.SpeedLimitCurveDiffMin,
+// SpeedLimitCurveMin = PilotDefinition.Conf.SpeedLimitCurveMin,
+// MaxRotateSpeed = PilotDefinition.Conf.MaxRotateSpeedCurveLimit,
+// MaxRotateAcc = PilotDefinition.Conf.MaxRotateAccCurveLimit,
+// GcpThetaThreshold = PilotDefinition.Conf.GcpThetaThreshold,
+// DthLinearFac = PilotDefinition.Conf.DthLinearFac,
+// DthLinearThreshold = PilotDefinition.Conf.DthLinearThreshold,
+// BiasFac = PilotDefinition.Conf.BiasFac,
+// BiasThreshold = PilotDefinition.Conf.BiasThreshold,
+// };
+// }
+// }
\ No newline at end of file
diff --git a/MultiWheelC/Old/ClampTests.cs b/MultiWheelC/Old/ClampTests.cs
new file mode 100644
index 0000000..b789a68
--- /dev/null
+++ b/MultiWheelC/Old/ClampTests.cs
@@ -0,0 +1,90 @@
+
+
+using System;
+using ClumsyCore;
+using ClumsyCore.Pilot;
+using FundamentalLib;
+using MDCSToolBox.Clumsy.Movements;
+using MDCSToolBox.Clumsy.Pilot;
+
+namespace MultiWheelC
+{
+ public abstract class ClampMovementTestBase : MovementTest
+ {
+ public float TimeoutSeconds = 30f; // 动作超时时间，单位s。
+
+ private DriveTask _task;
+ protected abstract bool Close { get; }
+
+ // 根据派生测试类型驱动左右夹臂同步夹紧或打开。
+ public override void Test()
+ {
+ var leftTarget = Close
+ ? PilotDefinition.Self.LeftArmUpperPos
+ : PilotDefinition.Self.LeftArmLowerPos;
+ var rightTarget = Close
+ ? PilotDefinition.Self.RightArmUpperPos
+ : PilotDefinition.Self.RightArmLowerPos;
+
+ if (float.IsNaN(leftTarget) ||
+ float.IsInfinity(leftTarget) ||
+ float.IsNaN(rightTarget) ||
+ float.IsInfinity(rightTarget))
+ {
+ Console.WriteLine(
+ "夹臂目标位置无效，取消夹臂运动测试。");
+ return;
+ }
+
+ // 防止重复点击时上一项夹臂任务仍在运行。
+ TestStop();
+ Console.WriteLine(
+ $"开始夹臂{(Close ? "夹紧" : "打开")}测试：" +
+ $"左目标={leftTarget}，右目标={rightTarget}");
+
+ var task = new DriveTask(
+ new ClampToTarget
+ {
+ LeftClampTarget = leftTarget,
+ RightClampTarget = rightTarget,
+ TimeoutSeconds = TimeoutSeconds
+ }.Get());
+ _task = task;
+
+ try
+ {
+ task.Wait();
+ }
+ finally
+ {
+ PilotDefinition.Self.SpeedLeftArm = 0f;
+ PilotDefinition.Self.SpeedRightArm = 0f;
+ if (ReferenceEquals(_task, task))
+ _task = null;
+ }
+ }
+
+ // 停止夹臂任务并立即清零左右夹臂下发速度。
+ public override void TestStop()
+ {
+ _task?.Stop();
+ _task = null;
+ PilotDefinition.Self.SpeedLeftArm = 0f;
+ PilotDefinition.Self.SpeedRightArm = 0f;
+ }
+ }
+
+ [MovementTest(name = "夹臂关闭测试")]
+ public sealed class TestClampCloseMovement
+ : ClampMovementTestBase
+ {
+ protected override bool Close => false;
+ }
+
+ [MovementTest(name = "夹臂启动测试")]
+ public sealed class TestClampOpenMovement
+ : ClampMovementTestBase
+ {
+ protected override bool Close => true;
+ }
+}
diff --git a/MultiWheelC/Old/ClampToTarget.cs b/MultiWheelC/Old/ClampToTarget.cs
new file mode 100644
index 0000000..d6e06bc
--- /dev/null
+++ b/MultiWheelC/Old/ClampToTarget.cs
@@ -0,0 +1,91 @@
+using System;
+using System.Collections.Generic;
+using ClumsyCore.Pilot;
+using MDCSToolBox.Commons.Controllers;
+
+namespace MultiWheelC
+{
+ public class ClampToTarget : MovementDefinition
+ {
+ public float LeftClampTarget;
+ public float RightClampTarget;
+ public float MaxClampSpeed = PilotDefinition.Conf.MaxClampSpeed;
+ public float ClampKp = PilotDefinition.Conf.ClampControlKp;
+ public float ClampKi = PilotDefinition.Conf.ClampControlKi;
+ public float ClampKd = PilotDefinition.Conf.ClampControlKd;
+ public float ClampMaxI = PilotDefinition.Conf.ClampControlMaxI;
+ public float ClampSpeedAcc = PilotDefinition.Conf.ClampControlSpeedAcc;
+ public float ClampDeadZone = PilotDefinition.Conf.ClampControlDeadZone;
+ public float TimeoutSeconds = 30f;
+ private PIDController leftpid, rightpid;
+
+ // C层单车业务：驱动左右夹臂运动到夹紧或松开目标。
+ public override IEnumerable Get()
+ {
+ try
+ {
+ leftpid = new PIDController(
+ () => PilotDefinition.Self.ActualPosLeftArm,
+ ClampKp, ClampKi, ClampKd, ClampMaxI,
+ ClampDeadZone, MaxClampSpeed)
+ {
+ SpeedAccPerSec = ClampSpeedAcc
+ };
+
+ rightpid = new PIDController(
+ () => PilotDefinition.Self.ActualPosRightArm,
+ ClampKp, ClampKi, ClampKd, ClampMaxI,
+ ClampDeadZone, MaxClampSpeed)
+ {
+ SpeedAccPerSec = ClampSpeedAcc
+ };
+
+ var startTime = DateTime.UtcNow;
+ while (true)
+ {
+ if (TimeoutSeconds > 0f &&
+ (DateTime.UtcNow - startTime).TotalSeconds >
+ TimeoutSeconds)
+ {
+ Console.WriteLine(
+ $"夹臂运动超时({TimeoutSeconds:F1}s)，" +
+ "停止左右夹臂。");
+ yield break;
+ }
+
+ var leftspeed =
+ leftpid.GetResponse(LeftClampTarget);
+ var rightspeed =
+ rightpid.GetResponse(RightClampTarget);
+ Console.WriteLine(
+ $"left arm speed:{leftspeed} " +
+ $"right arm speed:{rightspeed}");
+
+ PilotDefinition.Self.SpeedLeftArm = leftspeed;
+ PilotDefinition.Self.SpeedRightArm = rightspeed;
+
+ var leftArrived = leftpid.IsArrived();
+ var rightArrived = rightpid.IsArrived();
+ if (leftArrived)
+ PilotDefinition.Self.SpeedLeftArm = 0f;
+ if (rightArrived)
+ PilotDefinition.Self.SpeedRightArm = 0f;
+
+ if (leftArrived && rightArrived)
+ break;
+
+ yield return true;
+ }
+
+ Console.WriteLine(
+ $"left clamp to target:{LeftClampTarget} " +
+ $"right clamp to target:{RightClampTarget}");
+ }
+ finally
+ {
+ PilotDefinition.Self.SpeedLeftArm = 0f;
+ PilotDefinition.Self.SpeedRightArm = 0f;
+ }
+ }
+ }
+}
diff --git a/MultiWheelC/Old/CrabMotionFrameTracker.cs b/MultiWheelC/Old/CrabMotionFrameTracker.cs
new file mode 100644
index 0000000..b3a575d
--- /dev/null
+++ b/MultiWheelC/Old/CrabMotionFrameTracker.cs
@@ -0,0 +1,817 @@
+// using ClumsyCore;
+// using ClumsyCore.DTools;
+// using ClumsyCore.Interfaces;
+// using ClumsyCore.Pilot;
+// using CommonUsage.Chassis;
+// using MyParking.Shared;
+// using System;
+// using System.Collections.Generic;
+// using System.Numerics;
+
+// namespace MultiWheelC
+// {
+// // C层单车测试：在可配置的运动坐标系中统一跟踪直线、圆弧或S型曲线。
+// public sealed class CrabMotionFrameTracker : MovementDefinition
+// {
+// public enum ReferencePathKind
+// {
+// Straight = 0,
+// LeftArc = 1,
+// SCurve = 2
+// }
+
+// public enum ChassisCommandBackend
+// {
+// SendXYThSpeed = 0,
+// SendMotion = 1
+// }
+
+// public ReferencePathKind PathKind;
+// public ChassisCommandBackend CommandBackend =
+// ChassisCommandBackend.SendMotion;
+// public Vector2 StartPosition;
+// public double InitialBodyYawRadians;
+// public float LengthMillimeters = 4000f;
+// public float RadiusMillimeters = 2000f;
+// public float SCurveLateralOffsetMillimeters = 400f;
+// public double ArcSweepRadians = Math.PI / 2.0;
+// public float CruiseSpeed = 0.2f;
+// public float SlowDistanceMillimeters = 600f;
+// public float FinishDistanceMillimeters = 30f;
+// public float MinimumSpeed = 0.04f;
+// public double LateralGainPerSecond = 0.8;
+// public double MaximumLateralCorrection = 0.12;
+// public double HeadingGainPerSecond = 1.5;
+// public double MaximumAngularSpeedRadiansPerSecond =
+// AngleMath.DegreesToRadians(30.0);
+// public double MaximumVirtualSteeringRadians =
+// AngleMath.DegreesToRadians(30.0);
+// public float WheelAlignmentToleranceDegrees = 2f;
+// public float WheelAlignmentStableSeconds = 0.3f;
+// public float WheelAlignmentTimeoutSeconds = 10f;
+// public float TrackingTimeoutSeconds = 60f;
+// public Action CommandObserver;
+
+// // 运动坐标系相对车体坐标系的朝向：普通模式为0，蟹行为π/2。
+// public double MotionFrameYawInBodyRadians = Math.PI / 2.0;
+// private double _lastSCurveProgress;
+
+// public override IEnumerable Get()
+// {
+// ValidateParameters();
+
+// var chassis =
+// PilotDefinition.Chassis as MultiWheelChassis;
+// if (chassis == null)
+// throw new InvalidOperationException(
+// "当前底盘不是MultiWheelChassis，无法执行运动坐标系轨迹测试。");
+
+// var adapter = new MultiWheelChassisAdapter(
+// chassis,
+// PilotDefinition.Self.CarNum);
+// adapter.ResetToBodyFrame();
+
+// var lastCommandTime = DateTime.Now;
+
+// try
+// {
+// // 模式切换阶段只转舵轮，驱动速度始终保持为零。
+// var alignmentStarted = DateTime.Now;
+// DateTime? stableSince = null;
+// while (true)
+// {
+// if (!adapter.PrepareParallelDirection(
+// MotionFrameYawInBodyRadians))
+// throw new InvalidOperationException(
+// "无法生成运动坐标系对应的舵轮准备姿态。");
+
+// var aligned =
+// adapter.AreParallelWheelsAligned(
+// MotionFrameYawInBodyRadians,
+// AngleMath.DegreesToRadians(
+// WheelAlignmentToleranceDegrees));
+
+// if (aligned)
+// {
+// if (stableSince == null)
+// stableSince = DateTime.Now;
+
+// if ((DateTime.Now - stableSince.Value)
+// .TotalSeconds >=
+// WheelAlignmentStableSeconds)
+// break;
+// }
+// else
+// {
+// stableSince = null;
+// }
+
+// if ((DateTime.Now - alignmentStarted)
+// .TotalSeconds >
+// WheelAlignmentTimeoutSeconds)
+// throw new TimeoutException(
+// "舵轮在限定时间内未稳定到达运动坐标系初始方向。");
+
+// yield return true;
+// }
+
+// if (CommandBackend ==
+// ChassisCommandBackend.SendMotion)
+// {
+// // 舵轮已按真实机械角度完成预对齐；
+// // 现在由Shared适配层激活SendMotion虚拟运动坐标系。
+// adapter.ActivateMotionFrame(
+// MotionFrameYawInBodyRadians);
+// }
+
+// var trackingStarted = DateTime.Now;
+// while (true)
+// {
+// if ((DateTime.Now - trackingStarted)
+// .TotalSeconds >
+// TrackingTimeoutSeconds)
+// throw new TimeoutException(
+// "蟹行轨迹在限定时间内未完成。");
+
+// var location =
+// DetourInterface.getCartLocation();
+// if (!IsFinite(location.x) ||
+// !IsFinite(location.y) ||
+// !IsFinite(location.th))
+// throw new InvalidOperationException(
+// "蟹行轨迹测试期间Detour位姿无效。");
+
+// var currentPosition = new Vector2(
+// (float)location.x,
+// (float)location.y);
+// var currentBodyYaw =
+// AngleMath.DegreesToRadians(location.th);
+
+// CalculateReference(
+// currentPosition,
+// out var tangentYaw,
+// out var referencePoint,
+// out var remainingMillimeters,
+// out var referenceCurvature);
+
+// if (remainingMillimeters <=
+// FinishDistanceMillimeters)
+// break;
+
+// var speed =
+// CalculateSpeed(remainingMillimeters);
+// var tangent = new Vector2(
+// (float)Math.Cos(tangentYaw),
+// (float)Math.Sin(tangentYaw));
+// var leftNormal = new Vector2(
+// -tangent.Y,
+// tangent.X);
+// var positionError =
+// currentPosition - referencePoint;
+// var lateralErrorMeters =
+// Vector2.Dot(
+// positionError,
+// leftNormal) / 1000.0;
+// var normalCorrection =
+// Limit(
+// -LateralGainPerSecond *
+// lateralErrorMeters,
+// MaximumLateralCorrection);
+
+// // 先在世界坐标中组合切向速度与横向纠偏速度。
+// var worldVx =
+// tangent.X * speed +
+// leftNormal.X * (float)normalCorrection;
+// var worldVy =
+// tangent.Y * speed +
+// leftNormal.Y * (float)normalCorrection;
+
+// // 将世界速度表达为当前蟹行运动坐标系速度。
+// var motionYaw =
+// currentBodyYaw +
+// MotionFrameYawInBodyRadians;
+// var motionCos = Math.Cos(motionYaw);
+// var motionSin = Math.Sin(motionYaw);
+// var vxInMotion =
+// motionCos * worldVx +
+// motionSin * worldVy;
+// var vyInMotion =
+// -motionSin * worldVx +
+// motionCos * worldVy;
+
+// var desiredBodyYaw =
+// tangentYaw -
+// MotionFrameYawInBodyRadians;
+// var headingError =
+// AngleMath.ShortestDifferenceRadians(
+// desiredBodyYaw,
+// currentBodyYaw);
+// var omega =
+// speed * referenceCurvature +
+// HeadingGainPerSecond * headingError;
+// omega = Limit(
+// omega,
+// MaximumAngularSpeedRadiansPerSecond);
+
+// var now = DateTime.Now;
+// var interval = now - lastCommandTime;
+// lastCommandTime = now;
+
+// bool commandAccepted;
+// Twist2D bodyTwist;
+// if (CommandBackend ==
+// ChassisCommandBackend.SendMotion)
+// {
+// // 运动坐标系相对车体系旋转+90°：
+// // 运动系正向速度会转换成车体系+Y速度。
+// bodyTwist =
+// FrameTransform2D
+// .TransformTwistAtSamePoint(
+// new Pose2D(
+// 0.0,
+// 0.0,
+// MotionFrameYawInBodyRadians),
+// new Twist2D(
+// vxInMotion,
+// vyInMotion,
+// omega));
+
+// // 将运动坐标系原点和前后几何控制点处的速度，
+// // 转换为SendMotion需要的前后轴方向。
+// var controlPointRadiusMeters =
+// Math.Max(
+// chassis.ControlPointRadius /
+// 1000.0,
+// 0.001);
+// var frontVelocityY =
+// vyInMotion +
+// omega *
+// controlPointRadiusMeters;
+// var rearVelocityY =
+// vyInMotion -
+// omega *
+// controlPointRadiusMeters;
+// var frontSteeringRadians =
+// Math.Atan2(
+// frontVelocityY,
+// vxInMotion);
+// var rearSteeringRadians =
+// Math.Atan2(
+// rearVelocityY,
+// vxInMotion);
+
+// // 蟹行测试绕过M层ManualControl并直接调用SendMotion，
+// // 因此需要在C层同步应用蟹行虚拟几何比例和转向符号。
+// if (IsCrabMotionFrame())
+// {
+// var geometryRatio =
+// adapter.HalfTrackWidthMeters /
+// adapter.HalfWheelBaseMeters;
+
+// frontSteeringRadians =
+// ConvertToCrabSteering(
+// frontSteeringRadians,
+// geometryRatio);
+// rearSteeringRadians =
+// ConvertToCrabSteering(
+// rearSteeringRadians,
+// geometryRatio);
+// }
+
+// var frontThetaDegrees =
+// (float)AngleMath.RadiansToDegrees(
+// frontSteeringRadians);
+// var rearThetaDegrees =
+// (float)AngleMath.RadiansToDegrees(
+// rearSteeringRadians);
+// var motionSpeed =
+// (float)Math.Sqrt(
+// vxInMotion * vxInMotion +
+// vyInMotion * vyInMotion);
+
+// commandAccepted =
+// chassis.SendMotion(
+// motionSpeed,
+// frontThetaDegrees,
+// rearThetaDegrees,
+// interval);
+// }
+// else if (CommandBackend ==
+// ChassisCommandBackend
+// .SendXYThSpeed)
+// {
+// // 安全XYTh后端根据舵角误差统一压低驱动轮速。
+// bodyTwist =
+// FrameTransform2D
+// .TransformTwistAtSamePoint(
+// new Pose2D(
+// 0.0,
+// 0.0,
+// MotionFrameYawInBodyRadians),
+// new Twist2D(
+// vxInMotion,
+// vyInMotion,
+// omega));
+// var command = new ChassisCommand(
+// PilotDefinition.Self.CarNum,
+// bodyTwist);
+// commandAccepted =
+// adapter.Send(
+// command,
+// interval);
+// }
+// else
+// {
+// throw new InvalidOperationException(
+// $"不支持的底盘命令后端：{CommandBackend}。");
+// }
+
+// if (!commandAccepted)
+// throw new InvalidOperationException(
+// "运动坐标系轨迹底盘解算失败：" +
+// chassis
+// .LastMotionDecomposeFailureReason);
+
+// CommandObserver?.Invoke(
+// (float)bodyTwist.VxMetersPerSecond,
+// (float)bodyTwist.VyMetersPerSecond,
+// (float)bodyTwist
+// .OmegaRadiansPerSecond);
+
+// yield return true;
+// }
+// }
+// finally
+// {
+// adapter.StopImmediately();
+// if (CommandBackend ==
+// ChassisCommandBackend.SendMotion)
+// {
+// // 测试退出后恢复真实车体坐标系，避免影响后续测试。
+// adapter.ResetToBodyFrame();
+// }
+// CommandObserver?.Invoke(0f, 0f, 0f);
+// }
+
+// yield return false;
+// }
+
+// // 判断当前运动坐标系是否为车体左侧朝前的蟹行坐标系。
+// private bool IsCrabMotionFrame()
+// {
+// return Math.Abs(
+// AngleMath.ShortestDifferenceRadians(
+// Math.PI / 2.0,
+// MotionFrameYawInBodyRadians)) <
+// 1e-6;
+// }
+
+// // 按车体几何比例缩小蟹行转角。
+// // +90°运动坐标系已经完成方向映射，此处不能再次反号。
+// private double ConvertToCrabSteering(
+// double normalSteeringRadians,
+// double geometryRatio)
+// {
+// var crabSteeringRadians =
+// Math.Atan(
+// geometryRatio *
+// Math.Tan(
+// normalSteeringRadians));
+
+// return Limit(
+// crabSteeringRadians,
+// MaximumVirtualSteeringRadians);
+// }
+
+// // 计算当前点在直线或圆弧上的参考点、切线和剩余距离。
+// private void CalculateReference(
+// Vector2 currentPosition,
+// out double tangentYaw,
+// out Vector2 referencePoint,
+// out float remainingMillimeters,
+// out double curvaturePerMeter)
+// {
+// var initialMotionYaw =
+// InitialBodyYawRadians +
+// MotionFrameYawInBodyRadians;
+
+// if (PathKind == ReferencePathKind.Straight)
+// {
+// var tangent = new Vector2(
+// (float)Math.Cos(initialMotionYaw),
+// (float)Math.Sin(initialMotionYaw));
+// var relative = currentPosition - StartPosition;
+// var progress =
+// Vector2.Dot(relative, tangent);
+// var clampedProgress =
+// Math.Max(
+// 0f,
+// Math.Min(progress, LengthMillimeters));
+
+// tangentYaw = initialMotionYaw;
+// referencePoint =
+// StartPosition +
+// tangent * clampedProgress;
+// remainingMillimeters =
+// Math.Max(
+// 0f,
+// LengthMillimeters - progress);
+// curvaturePerMeter = 0.0;
+// return;
+// }
+
+// if (PathKind == ReferencePathKind.SCurve)
+// {
+// CalculateSCurveReference(
+// currentPosition,
+// initialMotionYaw,
+// out tangentYaw,
+// out referencePoint,
+// out remainingMillimeters,
+// out curvaturePerMeter);
+// return;
+// }
+
+// var center = GetArcCenter();
+// var startRadialYaw =
+// initialMotionYaw - Math.PI / 2.0;
+// var radial = currentPosition - center;
+// var currentRadialYaw =
+// Math.Atan2(radial.Y, radial.X);
+// var progressRadians =
+// AngleMath.NormalizeRadians(
+// currentRadialYaw - startRadialYaw);
+
+// // 测试圆弧只有+90°，起点附近的轻微负噪声按0处理。
+// if (progressRadians < 0.0)
+// progressRadians = 0.0;
+
+// var clampedProgressRadians =
+// Math.Min(
+// progressRadians,
+// ArcSweepRadians);
+// var referenceRadialYaw =
+// startRadialYaw +
+// clampedProgressRadians;
+// referencePoint = center + new Vector2(
+// RadiusMillimeters *
+// (float)Math.Cos(referenceRadialYaw),
+// RadiusMillimeters *
+// (float)Math.Sin(referenceRadialYaw));
+// tangentYaw =
+// referenceRadialYaw + Math.PI / 2.0;
+// remainingMillimeters =
+// (float)Math.Max(
+// 0.0,
+// (ArcSweepRadians - progressRadians) *
+// RadiusMillimeters);
+// curvaturePerMeter =
+// 1000.0 / RadiusMillimeters;
+// }
+
+// // 通过离散最近点和解析导数计算两段三次贝塞尔S曲线的参考状态。
+// private void CalculateSCurveReference(
+// Vector2 currentPosition,
+// double initialMotionYaw,
+// out double tangentYaw,
+// out Vector2 referencePoint,
+// out float remainingMillimeters,
+// out double curvaturePerMeter)
+// {
+// const int nearestPointSamples = 200;
+// var searchStart =
+// Math.Max(
+// 0.0,
+// _lastSCurveProgress - 0.02);
+// var bestProgress = _lastSCurveProgress;
+// var bestDistanceSquared = double.MaxValue;
+
+// for (var i = 0;
+// i <= nearestPointSamples;
+// i++)
+// {
+// var progress =
+// searchStart +
+// (1.0 - searchStart) *
+// i / nearestPointSamples;
+// EvaluateSCurve(
+// progress,
+// out var localPoint,
+// out _,
+// out _);
+// var worldPoint =
+// LocalPathPointToWorld(
+// localPoint,
+// initialMotionYaw);
+// var distanceSquared =
+// Vector2.DistanceSquared(
+// currentPosition,
+// worldPoint);
+
+// if (distanceSquared <
+// bestDistanceSquared)
+// {
+// bestDistanceSquared =
+// distanceSquared;
+// bestProgress = progress;
+// }
+// }
+
+// // 轨迹进度不允许因定位噪声倒退，防止控制目标跳回上一段曲线。
+// _lastSCurveProgress =
+// Math.Max(
+// _lastSCurveProgress,
+// bestProgress);
+// EvaluateSCurve(
+// _lastSCurveProgress,
+// out var bestLocalPoint,
+// out var firstDerivative,
+// out var secondDerivative);
+// referencePoint =
+// LocalPathPointToWorld(
+// bestLocalPoint,
+// initialMotionYaw);
+// tangentYaw =
+// initialMotionYaw +
+// Math.Atan2(
+// firstDerivative.Y,
+// firstDerivative.X);
+
+// var derivativeMagnitude =
+// Math.Sqrt(
+// firstDerivative.X *
+// firstDerivative.X +
+// firstDerivative.Y *
+// firstDerivative.Y);
+// if (derivativeMagnitude < 1e-6)
+// {
+// curvaturePerMeter = 0.0;
+// }
+// else
+// {
+// // 导数单位为mm，乘1000后将曲率从1/mm转换成1/m。
+// curvaturePerMeter =
+// (firstDerivative.X *
+// secondDerivative.Y -
+// firstDerivative.Y *
+// secondDerivative.X) *
+// 1000.0 /
+// Math.Pow(
+// derivativeMagnitude,
+// 3.0);
+// }
+
+// remainingMillimeters =
+// ApproximateSCurveRemainingLength(
+// _lastSCurveProgress);
+// }
+
+// // 计算与普通4m S型测试完全一致的三段三次贝塞尔完整S曲线。
+// private void EvaluateSCurve(
+// double progress,
+// out Vector2 point,
+// out Vector2 firstDerivative,
+// out Vector2 secondDerivative)
+// {
+// progress =
+// Math.Max(
+// 0.0,
+// Math.Min(progress, 1.0));
+
+// Vector2 p0;
+// Vector2 p1;
+// Vector2 p2;
+// Vector2 p3;
+// double t;
+
+// if (progress <= 0.25)
+// {
+// t = progress * 4.0;
+// p0 = new Vector2(0f, 0f);
+// p1 = new Vector2(
+// LengthMillimeters / 12f,
+// 0f);
+// p2 = new Vector2(
+// LengthMillimeters / 6f,
+// SCurveLateralOffsetMillimeters);
+// p3 = new Vector2(
+// LengthMillimeters * 0.25f,
+// SCurveLateralOffsetMillimeters);
+// }
+// else if (progress <= 0.75)
+// {
+// t = (progress - 0.25) * 2.0;
+// p0 = new Vector2(
+// LengthMillimeters * 0.25f,
+// SCurveLateralOffsetMillimeters);
+// p1 = new Vector2(
+// LengthMillimeters / 3f,
+// SCurveLateralOffsetMillimeters);
+// p2 = new Vector2(
+// LengthMillimeters * 2f / 3f,
+// -SCurveLateralOffsetMillimeters);
+// p3 = new Vector2(
+// LengthMillimeters * 0.75f,
+// -SCurveLateralOffsetMillimeters);
+// }
+// else
+// {
+// t = (progress - 0.75) * 4.0;
+// p0 = new Vector2(
+// LengthMillimeters * 0.75f,
+// -SCurveLateralOffsetMillimeters);
+// p1 = new Vector2(
+// LengthMillimeters * 5f / 6f,
+// -SCurveLateralOffsetMillimeters);
+// p2 = new Vector2(
+// LengthMillimeters * 11f / 12f,
+// 0f);
+// p3 = new Vector2(
+// LengthMillimeters,
+// 0f);
+// }
+
+// var oneMinusT = 1.0 - t;
+// point =
+// p0 * (float)(
+// oneMinusT *
+// oneMinusT *
+// oneMinusT) +
+// p1 * (float)(
+// 3.0 *
+// oneMinusT *
+// oneMinusT *
+// t) +
+// p2 * (float)(
+// 3.0 *
+// oneMinusT *
+// t *
+// t) +
+// p3 * (float)(t * t * t);
+// firstDerivative =
+// (p1 - p0) *
+// (float)(
+// 3.0 *
+// oneMinusT *
+// oneMinusT) +
+// (p2 - p1) *
+// (float)(
+// 6.0 *
+// oneMinusT *
+// t) +
+// (p3 - p2) *
+// (float)(3.0 * t * t);
+// secondDerivative =
+// (p2 - 2f * p1 + p0) *
+// (float)(6.0 * oneMinusT) +
+// (p3 - 2f * p2 + p1) *
+// (float)(6.0 * t);
+// }
+
+// // 通过分段采样估算从当前S曲线进度到终点的实际弧长。
+// private float ApproximateSCurveRemainingLength(
+// double startProgress)
+// {
+// const int lengthSamples = 100;
+// EvaluateSCurve(
+// startProgress,
+// out var previousPoint,
+// out _,
+// out _);
+// var length = 0f;
+
+// for (var i = 1;
+// i <= lengthSamples;
+// i++)
+// {
+// var progress =
+// startProgress +
+// (1.0 - startProgress) *
+// i / lengthSamples;
+// EvaluateSCurve(
+// progress,
+// out var point,
+// out _,
+// out _);
+// length +=
+// Vector2.Distance(
+// previousPoint,
+// point);
+// previousPoint = point;
+// }
+
+// return length;
+// }
+
+// // 将以初始蟹行方向为X轴的局部路径点转换到Detour世界坐标。
+// private Vector2 LocalPathPointToWorld(
+// Vector2 localPoint,
+// double initialMotionYaw)
+// {
+// var cos =
+// (float)Math.Cos(initialMotionYaw);
+// var sin =
+// (float)Math.Sin(initialMotionYaw);
+
+// return StartPosition + new Vector2(
+// localPoint.X * cos -
+// localPoint.Y * sin,
+// localPoint.X * sin +
+// localPoint.Y * cos);
+// }
+
+// // 获取蟹行左转圆弧圆心；它位于初始运动方向的左侧。
+// public Vector2 GetArcCenter()
+// {
+// var initialMotionYaw =
+// InitialBodyYawRadians +
+// MotionFrameYawInBodyRadians;
+// return StartPosition + new Vector2(
+// -RadiusMillimeters *
+// (float)Math.Sin(initialMotionYaw),
+// RadiusMillimeters *
+// (float)Math.Cos(initialMotionYaw));
+// }
+
+// // 获取圆弧测试的理论终点。
+// public Vector2 GetArcDestination()
+// {
+// var initialMotionYaw =
+// InitialBodyYawRadians +
+// MotionFrameYawInBodyRadians;
+// var startRadialYaw =
+// initialMotionYaw - Math.PI / 2.0;
+// var endRadialYaw =
+// startRadialYaw + ArcSweepRadians;
+// var center = GetArcCenter();
+
+// return center + new Vector2(
+// RadiusMillimeters *
+// (float)Math.Cos(endRadialYaw),
+// RadiusMillimeters *
+// (float)Math.Sin(endRadialYaw));
+// }
+
+// // 根据剩余路径长度生成终点减速速度。
+// private float CalculateSpeed(
+// float remainingMillimeters)
+// {
+// if (remainingMillimeters >=
+// SlowDistanceMillimeters)
+// return CruiseSpeed;
+
+// var ratio =
+// remainingMillimeters /
+// Math.Max(
+// SlowDistanceMillimeters,
+// 1f);
+// return Math.Max(
+// MinimumSpeed,
+// CruiseSpeed * ratio);
+// }
+
+// private void ValidateParameters()
+// {
+// if (CruiseSpeed <= 0f ||
+// !IsFinite(CruiseSpeed) ||
+// LengthMillimeters <= 0f ||
+// !IsFinite(LengthMillimeters) ||
+// RadiusMillimeters <= 0f ||
+// !IsFinite(RadiusMillimeters) ||
+// SCurveLateralOffsetMillimeters <= 0f ||
+// !IsFinite(
+// SCurveLateralOffsetMillimeters) ||
+// ArcSweepRadians <= 0.0 ||
+// !IsFinite(ArcSweepRadians) ||
+// SlowDistanceMillimeters <= 0f ||
+// !IsFinite(SlowDistanceMillimeters) ||
+// FinishDistanceMillimeters < 0f ||
+// !IsFinite(FinishDistanceMillimeters) ||
+// TrackingTimeoutSeconds <= 0f ||
+// !IsFinite(TrackingTimeoutSeconds) ||
+// MaximumVirtualSteeringRadians <= 0.0 ||
+// MaximumVirtualSteeringRadians >=
+// Math.PI / 2.0 ||
+// !IsFinite(
+// MaximumVirtualSteeringRadians))
+// throw new ArgumentOutOfRangeException(
+// "蟹行轨迹测试参数无效。");
+// }
+
+// private static double Limit(
+// double value,
+// double absoluteLimit)
+// {
+// return Math.Max(
+// -absoluteLimit,
+// Math.Min(value, absoluteLimit));
+// }
+
+// private static bool IsFinite(double value)
+// {
+// return
+// !double.IsNaN(value) &&
+// !double.IsInfinity(value);
+// }
+// }
+// }
diff --git a/MultiWheelC/Old/DriverMovements.cs b/MultiWheelC/Old/DriverMovements.cs
new file mode 100644
index 0000000..5102c28
--- /dev/null
+++ b/MultiWheelC/Old/DriverMovements.cs
@@ -0,0 +1,123 @@
+// using System;
+// using System.Collections.Generic;
+// using System.Threading;
+// using ClumsyCore.Pilot;
+
+// namespace MultiWheelC
+// {
+// public class Sleep : MovementDefinition
+// {
+// public float Second = 2f;
+
+// public override IEnumerable Get()
+// {
+// if (Second <= 0)
+// {
+// yield return false;
+// yield break;
+// }
+
+// var endTime = DateTime.UtcNow.AddSeconds(Second);
+// while (DateTime.UtcNow < endTime)
+// {
+// Thread.Sleep(50);
+// yield return true;
+// }
+
+// yield return false;
+// }
+// }
+
+// public class DriverAble : MovementDefinition
+// {
+// public int WaitTimeoutMs = 2000;
+// public int PollIntervalMs = 50;
+
+// // C层单车硬件：请求全部驱动轮复位并恢复使能。
+// public override IEnumerable Get()
+// {
+// PilotDefinition.Self.ResetFromC = true;
+
+// try
+// {
+// var start = DateTime.Now;
+// var timeoutMs = Math.Max(0, WaitTimeoutMs);
+// var pollMs = Math.Max(1, PollIntervalMs);
+
+// // 至少保留一个调度周期，确保M层能收到复位请求。
+// yield return true;
+
+// while (!PilotDefinition.Self.WheelAbleState &&
+// (DateTime.Now - start).TotalMilliseconds < timeoutMs)
+// {
+// Thread.Sleep(pollMs);
+// yield return true;
+// }
+// }
+// finally
+// {
+// PilotDefinition.Self.ResetFromC = false;
+// }
+// }
+// }
+// public class DriverDisable : MovementDefinition
+// {
+// public int WaitTimeoutMs = 3000;
+// public int PollIntervalMs = 20;
+
+// // C层单车硬件：请求驱动轮退出使能，并等待M层状态反馈。
+// public override IEnumerable Get()
+// {
+// var timeoutMs = Math.Max(0, WaitTimeoutMs);
+// var pollMs = Math.Max(1, PollIntervalMs);
+// var startTime = DateTime.UtcNow;
+// var success = false;
+
+// PilotDefinition.Self.DisableFromC = true;
+
+// try
+// {
+// // 至少保持一个C层调度周期，确保M层能收到下使能请求。
+// yield return true;
+
+// success = !PilotDefinition.Self.WheelAbleState;
+
+// while (!success &&
+// (DateTime.UtcNow - startTime).TotalMilliseconds <
+// timeoutMs)
+// {
+// Thread.Sleep(pollMs);
+
+// success =
+// !PilotDefinition.Self.WheelAbleState;
+
+// if (!success)
+// {
+// yield return true;
+// }
+// }
+// }
+// finally
+// {
+// // 无论正常完成、超时、异常还是任务被停止，都撤销请求。
+// PilotDefinition.Self.DisableFromC = false;
+// }
+// if (success)
+// {
+// Console.WriteLine(
+// $"驱动器下使能完成，" +
+// $"WheelAbleState=" +
+// $"{PilotDefinition.Self.WheelAbleState}");
+// }
+// else
+// {
+// Console.WriteLine(
+// $"驱动器下使能超时，" +
+// $"WheelAbleState=" +
+// $"{PilotDefinition.Self.WheelAbleState}，" +
+// $"等待{timeoutMs}ms");
+// }
+// yield return false;
+// }
+// }
+// }
diff --git a/MultiWheelC/Old/DstTracker.cs b/MultiWheelC/Old/DstTracker.cs
new file mode 100644
index 0000000..3a41061
--- /dev/null
+++ b/MultiWheelC/Old/DstTracker.cs
@@ -0,0 +1,55 @@
+// using System;
+// using System.Collections.Generic;
+// using System.Drawing;
+// using System.Numerics;
+// using ClumsyCore;
+// using ClumsyCore.DTools;
+// using ClumsyCore.Pilot;
+// using CommonUsage.Chassis;
+// using MDCSToolBox.Clumsy.Tracks;
+
+// namespace MultiWheelC
+// {
+// //在世界坐标系下，从路径起点追踪到终点并停车
+// public class DstTracker : MovementDefinition
+// {
+// public Vector2 Src;
+// public Vector2 Dst;
+// // 本次轨迹的巡航速度上限，单位m/s。
+// public float MaxSpeed = PilotDefinition.Conf.DstTrackerMaxSpeed;
+// public float CarDirectionBias = 0f;
+// public Painter Painter = UI.GetPainter("DstTracker");
+// public override IEnumerable Get()
+// {
+// var chassis = (MultiWheelChassis)PilotDefinition.Chassis;
+// DriveTask task = null;
+// try
+// {
+// Console.WriteLine($"DstTracker src:({Src.X:F2}, {Src.Y:F2}) dst:({Dst.X:F2}, {Dst.Y:F2})");
+// Painter.DrawLine(Color.Cyan, Src.X, Src.Y, Dst.X, Dst.Y, width: 3);
+
+// var tracker = new ChassisController
+// {
+// BaseSpeed = MaxSpeed
+// }.Get();
+// // 要求路径末端速度下降到零。
+// tracker.FinishSpeed = 0f;
+// var linePath = new LineTrack(Src, Dst)
+// {
+// CarDirectionBias = CarDirectionBias,
+// Speed = MaxSpeed
+// };
+// tracker.AddTrack(linePath);
+// task = new DriveTask(tracker.Track());
+// task.Wait();
+// yield return false;
+// }
+// finally
+// {
+// task?.Stop();
+// chassis.PredefinedDriveStop();
+// }
+// }
+
+// }
+// }
diff --git a/MultiWheelC/Old/LineTracking.cs b/MultiWheelC/Old/LineTracking.cs
new file mode 100644
index 0000000..829be73
--- /dev/null
+++ b/MultiWheelC/Old/LineTracking.cs
@@ -0,0 +1,178 @@
+// using System;
+// using System.Collections.Generic;
+// using System.Drawing;
+// using System.Numerics;
+// using ClumsyCore;
+// using ClumsyCore.DTools;
+// using ClumsyCore.Interfaces;
+// using ClumsyCore.Pilot;
+// using CommonUsage.Chassis;
+// using FundamentalLib;
+// using MDCSToolBox.Clumsy.Tracks;
+// using MDCSToolBox.Commons.Controllers;
+// using MyParking.Shared;
+
+// namespace MultiWheelC
+// {
+// // C层单车底盘：按照车轮里程行驶指定的相对距离。
+// public class LineTracking : MovementDefinition
+// {
+// // 相对动作启动位置的行驶距离，单位mm。
+// // 正数表示前进，负数表示后退。
+// public float TargetDistance;
+// public float MaxSpeed = PilotDefinition.Conf.LineTrackMaxSpeed;
+// public float Kp = PilotDefinition.Conf.LineTrackKp;
+// public float Ki = PilotDefinition.Conf.LineTrackKi;
+// public float Kd = PilotDefinition.Conf.LineTrackKd;
+// public float DeadZone = PilotDefinition.Conf.LineTrackDeadZone;
+// public int SrcId = -1;
+// public int DstId = -1;
+// public Action LeaveSrcFunction;
+// // 接近目标后是否保留速度，交给下一个动作接管。
+// public bool EnableHandover;
+// // 进入动作衔接的剩余距离，单位mm。
+// public float HandoverDistance = 80f;
+// // HandoverSpeed小于0时，使用MaxSpeed的此比例。
+// public float HandoverSpeedRatio = 0.5f;
+// // 大于等于0时，直接作为衔接速度，单位m/s。
+// public float HandoverSpeed = -1f;
+// public float MinHandoverSpeed = 0.05f;
+// private PIDController _pid;
+// // 读取当前单车直线行驶里程，单位mm。
+// private static float ReadPosition()
+// {
+// return
+// (PilotDefinition.Self.LFLActualPos + PilotDefinition.Self.LFRActualPos) / 2f;
+// }
+
+// // 根据动作启动位置和目标距离执行直线里程闭环。
+// public override IEnumerable Get()
+// {
+// if (float.IsNaN(TargetDistance) || float.IsInfinity(TargetDistance))
+// {
+// throw new ArgumentOutOfRangeException(
+// nameof(TargetDistance),
+// "目标行驶距离必须是有限值。");
+// }
+
+// if (float.IsNaN(MaxSpeed) || float.IsInfinity(MaxSpeed) || MaxSpeed <= 0f)
+// {
+// throw new ArgumentOutOfRangeException(
+// nameof(MaxSpeed),
+// "最大速度必须是大于零的有限值。");
+// }
+// var chassis = (MultiWheelChassis)PilotDefinition.Chassis;
+// // 每次启动动作时重新读取起始编码器位置。
+// var startPosition = ReadPosition();
+// // PID仍然控制绝对编码器位置，但绝对目标由动作自动计算。
+// var targetPosition = startPosition + TargetDistance;
+// _pid = new PIDController(ReadPosition, Kp, Ki, Kd, 0, DeadZone, MaxSpeed)
+// {
+// SpeedAccPerSec = Math.Abs(MaxSpeed) / 2f
+// };
+// var handoverRequested = false;
+// var keepHandoverSpeed = false;
+// DLog.Log(
+// $"直线里程动作：" +
+// $"起点={startPosition:F1}mm，" +
+// $"距离={TargetDistance:F1}mm，" +
+// $"目标={targetPosition:F1}mm",
+// "straight_line");
+// try
+// {
+// while (true)
+// {
+// var currentPosition = ReadPosition();
+// var remainingDistance = targetPosition - currentPosition;
+// // 接近目标后，保留一定速度交给后续动作。
+// if (EnableHandover && Math.Abs(remainingDistance) <= Math.Max(1f, HandoverDistance))
+// {
+// var direction = Math.Sign(remainingDistance);
+// if (direction == 0)
+// {
+// direction = Math.Sign(TargetDistance);
+// }
+// var requestedSpeed = HandoverSpeed >= 0f ? Math.Abs(HandoverSpeed) : Math.Abs(MaxSpeed) * HandoverSpeedRatio;
+// var maximumSpeed = Math.Abs(MaxSpeed);
+// var minimumSpeed = Math.Min(Math.Abs(MinHandoverSpeed), maximumSpeed);
+// var limitedSpeed = Math.Max(minimumSpeed, Math.Min(requestedSpeed, maximumSpeed));
+// var handoverSpeed = limitedSpeed * direction;
+// chassis.SendXYThSpeed(handoverSpeed, 0f, 0f);
+// handoverRequested = true;
+// // 保持一个调度周期，让速度命令实际生效。
+// yield return true;
+// break;
+// }
+// var speed = _pid.GetResponse(targetPosition);
+// chassis.SendXYThSpeed(speed, 0f, 0f);
+// if (_pid.IsArrived())
+// {
+// break;
+// }
+// yield return true;
+// }
+// if (SrcId != -1 &&
+// LeaveSrcFunction != null)
+// {
+// LeaveSrcFunction(SrcId);
+// DLog.Log($"释放放车点{SrcId}", "straight_line");
+// }
+// // 只有正常完成动作衔接时才允许保留非零速度。
+// keepHandoverSpeed = handoverRequested;
+// }
+// finally
+// {
+// // 普通完成、人工停止或异常退出时都必须停车。
+// if (!keepHandoverSpeed)
+// {
+// chassis.SendXYThSpeed(0f, 0f, 0f);
+// }
+// }
+// yield return false;
+// }
+// }
+
+
+
+
+// //直线行走基于detour
+// public class LineTracking_based_detour : MovementDefinition
+// {
+// public float LineDistance = 1000f;
+// public int SrcId = -1;
+// public int DstId = -1;
+// public Action LeaveSrcFunction = null;
+// public Painter painter = UI.GetPainter("Line", false);
+// // C层单车轨迹：执行早期版本的两点直线跟踪动作。
+// public override IEnumerable Get()
+// {
+// var curpose = DetourInterface.getCartLocation();
+// Console.WriteLine($"curpose.th:{curpose.th}");
+// var src = new Vector2((float)curpose.x, (float)curpose.y);
+// var headingRadians =
+// AngleMath.DegreesToRadians(curpose.th);
+// var dst = new Vector2(
+// (float)(curpose.x +
+// LineDistance * Math.Cos(headingRadians)),
+// (float)(curpose.y +
+// LineDistance * Math.Sin(headingRadians)));
+// // var dst = new Vector2((float)curpose.x + LineDistance * (float)Math.Cos(curpose.th),
+// // (float)curpose.y + LineDistance * (float)Math.Sin(curpose.th));
+// Console.WriteLine($"src:{src.X} {src.Y}");
+// Console.WriteLine($"dst:{dst.X} {dst.Y}");
+// painter.DrawLine(Color.Green, src.X, src.Y, dst.X, dst.Y, width: 3);
+
+// var tracker = new ChassisController().Get();
+// var linePath = new LineTrack(src, dst) { CarDirectionBias = LineDistance > 0 ? 0 : 180 };
+// tracker.AddTrack(linePath);
+// var _dt = new DriveTask(tracker.Track());
+// _dt.Wait();
+// if (SrcId != -1 && LeaveSrcFunction != null)
+// {
+// LeaveSrcFunction(SrcId);
+// DLog.Log($"释放放车点{SrcId}", "straight_line");
+// }
+// yield return false;
+// }
+// }
+// }
diff --git a/MultiWheelC/Old/PathTrackingTests.cs b/MultiWheelC/Old/PathTrackingTests.cs
new file mode 100644
index 0000000..4c52cde
--- /dev/null
+++ b/MultiWheelC/Old/PathTrackingTests.cs
@@ -0,0 +1,635 @@
+
+
+// using System;
+// using System.Collections.Generic;
+// using System.Numerics;
+// using System.Threading;
+// using ClumsyCore;
+// using ClumsyCore.Interfaces;
+// using ClumsyCore.Pilot;
+// using FundamentalLib;
+// using MDCSToolBox.Clumsy.Movements;
+// using MDCSToolBox.Clumsy.Pilot;
+// using MDCSToolBox.Clumsy.Tracks;
+// using MyParking.Shared;
+
+// namespace MultiWheelC
+// {
+// [MovementTest(name = "SendMotion：连续前进4m")]
+// public class TestForward4m : MovementTest
+// {
+// public float DistanceMillimeters = 4000f; // 测试距离，单位mm。
+// public float CruiseSpeed = 0.3f; // 巡航速度上限，单位m/s。
+// public int TrialNumber = 1; // 重复实验编号。
+// private DriveTask _task;
+// private TrackingExperimentRecorder _recorder;
+// // 从当前Detour位置沿车头方向生成4m连续直线并记录测试数据。
+// public override void Test()
+// {
+// if (!MovementTestPreparation.AreWheelsForward())
+// {
+// return;
+// }
+
+// var location = DetourInterface.getCartLocation();
+// if (double.IsNaN(location.x) ||
+// double.IsInfinity(location.x) ||
+// double.IsNaN(location.y) ||
+// double.IsInfinity(location.y) ||
+// double.IsNaN(location.th) ||
+// double.IsInfinity(location.th))
+// {
+// Console.WriteLine(
+// "Detour当前位姿无效，取消连续前进4m测试。");
+// return;
+// }
+// var source = new Vector2((float)location.x, (float)location.y);
+// // Detour航向单位是度，三角函数需要弧度。
+// var headingRadians =
+// AngleMath.DegreesToRadians(location.th);
+// var destination = new Vector2(
+// source.X + DistanceMillimeters * (float)Math.Cos(headingRadians),
+// source.Y + DistanceMillimeters * (float)Math.Sin(headingRadians));
+// _recorder =
+// new TrackingExperimentRecorder(
+// controllerName: "LegacyGeometricController",
+// trajectoryName: "LegacyStraight4m",
+// trialNumber: TrialNumber,
+// referenceStart: source,
+// referenceEnd: destination,
+// referenceSpeed: CruiseSpeed);
+// _recorder.Start();
+// try
+// {
+// _task = new DriveTask(
+// new DstTracker
+// {
+// Src = source,
+// Dst = destination,
+// CarDirectionBias = 0f,
+// MaxSpeed = CruiseSpeed
+// }.Get());
+// _task.Wait();
+// // 保留少量停车后数据，便于观察速度是否回到零。
+// Thread.Sleep(300);
+// }
+// finally
+// {
+// _task?.Stop();
+// _recorder?.UpdateCommand(0f, 0f);
+// _recorder?.StopAndSave();
+// _task = null;
+// _recorder = null;
+// }
+// }
+
+// public override void TestStop()
+// {
+// _task?.Stop();
+// _recorder?.UpdateCommand(0f, 0f);
+// _recorder?.StopAndSave();
+// }
+// }
+
+// [MovementTest(name = "SendMotion：左转90°半径2m圆弧")]
+// public class TestArcMovement : MovementTest
+// {
+// public float RadiusMillimeters = 2000f; // 左转圆的半径，单位mm。
+// public float CruiseSpeed = 0.3f; // 圆周运动速度上限，单位m/s。
+// public int TrialNumber = 1; // 重复实验编号。
+
+// private DriveTask _task;
+// private TrackingExperimentRecorder _recorder;
+
+// // 从当前位姿开始，沿半径2m的圆弧向左转弯90°。
+// public override void Test()
+// {
+// if (float.IsNaN(RadiusMillimeters) ||
+// float.IsInfinity(RadiusMillimeters) ||
+// RadiusMillimeters <= 0f ||
+// float.IsNaN(CruiseSpeed) ||
+// float.IsInfinity(CruiseSpeed) ||
+// CruiseSpeed <= 0f)
+// {
+// Console.WriteLine("圆弧运动测试参数无效。");
+// return;
+// }
+
+// if (!MovementTestPreparation.AreWheelsForward())
+// {
+// return;
+// }
+
+// var location = DetourInterface.getCartLocation();
+// if (double.IsNaN(location.x) ||
+// double.IsInfinity(location.x) ||
+// double.IsNaN(location.y) ||
+// double.IsInfinity(location.y) ||
+// double.IsNaN(location.th) ||
+// double.IsInfinity(location.th))
+// {
+// Console.WriteLine(
+// "Detour当前位姿无效，取消圆弧运动测试。");
+// return;
+// }
+
+// var source =
+// new Vector2((float)location.x, (float)location.y);
+// var headingRadians =
+// AngleMath.DegreesToRadians(location.th);
+
+// // 根据世界航向求车体左法向，左转圆心位于车辆左侧。
+// var center = new Vector2(
+// source.X -
+// RadiusMillimeters *
+// (float)Math.Sin(headingRadians),
+// source.Y +
+// RadiusMillimeters *
+// (float)Math.Cos(headingRadians));
+
+// // 从圆心指向车辆起点的极角，比车辆切线航向小90°。
+// var startRadialAngleDegrees =
+// (float)location.th - 90f;
+
+// var controller = new ChassisController
+// {
+// BaseSpeed = CruiseSpeed
+// }.Get();
+// controller.FinishSpeed = 0f;
+
+// var arc = new CircularArcTrack(
+// center,
+// RadiusMillimeters,
+// startRadialAngleDegrees,
+// startRadialAngleDegrees + 90f,
+// direction: 1)
+// {
+// Speed = CruiseSpeed,
+// CarDirectionBias = 0f
+// };
+
+// // 左转90°后，圆心到终点的径向方向等于起始车头方向。
+// var destination = center + new Vector2(
+// RadiusMillimeters *
+// (float)Math.Cos(headingRadians),
+// RadiusMillimeters *
+// (float)Math.Sin(headingRadians));
+
+// if (!controller.AddTrack(arc, "LeftArc90Degrees"))
+// {
+// Console.WriteLine(
+// "左转90°圆弧轨迹添加失败，取消测试。");
+// return;
+// }
+
+// _recorder = new TrackingExperimentRecorder(
+// controllerName: "LegacyGeometricController",
+// trajectoryName:
+// $"LegacyLeftArc90_R{RadiusMillimeters:0}mm",
+// trialNumber: TrialNumber,
+// referenceStart: source,
+// referenceEnd: destination,
+// referenceSpeed: CruiseSpeed);
+// _recorder.Start();
+
+// try
+// {
+// _task = new DriveTask(controller.Track());
+// _task.Wait();
+
+// // 保留少量停车后的样本，用于观察速度是否回到零。
+// Thread.Sleep(300);
+// }
+// finally
+// {
+// _task?.Stop();
+// _recorder?.UpdateCommand(0f, 0f);
+// _recorder?.StopAndSave();
+// _task = null;
+// _recorder = null;
+// }
+// }
+
+// // 停止圆弧运动并保存当前已经采集的实验数据。
+// public override void TestStop()
+// {
+// _task?.Stop();
+// _recorder?.UpdateCommand(0f, 0f);
+// _recorder?.StopAndSave();
+// }
+// }
+
+// [MovementTest(name = "SendMotion：蟹行直线4m")]
+// public class TestCrabForward4m : MovementTest
+// {
+// public float DistanceMillimeters = 4000f;
+// public float CruiseSpeed = 0.2f;
+// public int TrialNumber = 1;
+
+// private DriveTask _task;
+// private TrackingExperimentRecorder _recorder;
+
+// // 将车体左侧作为运动前向，沿直线蟹行4m并记录Detour实验数据。
+// public override void Test()
+// {
+// if (!TryReadStartPose(
+// out var source,
+// out var bodyYawRadians))
+// return;
+
+// var motionYaw =
+// bodyYawRadians + Math.PI / 2.0;
+// var destination = new Vector2(
+// source.X +
+// DistanceMillimeters *
+// (float)Math.Cos(motionYaw),
+// source.Y +
+// DistanceMillimeters *
+// (float)Math.Sin(motionYaw));
+
+// var tracker = new CrabMotionFrameTracker
+// {
+// CommandBackend =
+// CrabMotionFrameTracker
+// .ChassisCommandBackend
+// .SendMotion,
+// PathKind =
+// CrabMotionFrameTracker
+// .ReferencePathKind.Straight,
+// StartPosition = source,
+// InitialBodyYawRadians =
+// bodyYawRadians,
+// LengthMillimeters =
+// DistanceMillimeters,
+// CruiseSpeed = CruiseSpeed
+// };
+
+// _recorder = new TrackingExperimentRecorder(
+// controllerName:
+// "CrabSendMotionTracker",
+// trajectoryName:
+// "CrabStraight4m",
+// trialNumber: TrialNumber,
+// referenceStart: source,
+// referenceEnd: destination,
+// referenceSpeed: CruiseSpeed,
+// referenceMotionFrameYawDegrees: 90f);
+// tracker.CommandObserver =
+// (vx, vy, omega) =>
+// _recorder?.UpdateBodyCommand(
+// vx,
+// vy,
+// omega);
+// _recorder.Start();
+
+// try
+// {
+// _task = new DriveTask(tracker.Get());
+// _task.Wait();
+// Thread.Sleep(300);
+// }
+// finally
+// {
+// _task?.Stop();
+// _recorder?.UpdateBodyCommand(
+// 0f,
+// 0f,
+// 0f);
+// _recorder?.StopAndSave();
+// _task = null;
+// _recorder = null;
+// }
+// }
+
+// public override void TestStop()
+// {
+// _task?.Stop();
+// _recorder?.UpdateBodyCommand(
+// 0f,
+// 0f,
+// 0f);
+// _recorder?.StopAndSave();
+// }
+
+// // 读取并校验测试开始时的Detour世界位姿。
+// private static bool TryReadStartPose(
+// out Vector2 source,
+// out double bodyYawRadians)
+// {
+// var location =
+// DetourInterface.getCartLocation();
+// if (double.IsNaN(location.x) ||
+// double.IsInfinity(location.x) ||
+// double.IsNaN(location.y) ||
+// double.IsInfinity(location.y) ||
+// double.IsNaN(location.th) ||
+// double.IsInfinity(location.th))
+// {
+// Console.WriteLine(
+// "Detour当前位姿无效，取消蟹行直线测试。");
+// source = Vector2.Zero;
+// bodyYawRadians = 0.0;
+// return false;
+// }
+
+// source = new Vector2(
+// (float)location.x,
+// (float)location.y);
+// bodyYawRadians =
+// AngleMath.DegreesToRadians(location.th);
+// return true;
+// }
+// }
+
+// [MovementTest(name = "SendMotion：蟹行左转90°半径2m圆弧")]
+// public class TestCrabLeftArc90 : MovementTest
+// {
+// public float RadiusMillimeters = 2000f;
+// public float CruiseSpeed = 0.2f;
+// public int TrialNumber = 1;
+
+// private DriveTask _task;
+// private TrackingExperimentRecorder _recorder;
+
+// // 将车体左侧作为运动前向，沿半径2m的左转圆弧运动90°。
+// public override void Test()
+// {
+// var location =
+// DetourInterface.getCartLocation();
+// if (double.IsNaN(location.x) ||
+// double.IsInfinity(location.x) ||
+// double.IsNaN(location.y) ||
+// double.IsInfinity(location.y) ||
+// double.IsNaN(location.th) ||
+// double.IsInfinity(location.th))
+// {
+// Console.WriteLine(
+// "Detour当前位姿无效，取消蟹行圆弧测试。");
+// return;
+// }
+
+// var source = new Vector2(
+// (float)location.x,
+// (float)location.y);
+// var bodyYawRadians =
+// AngleMath.DegreesToRadians(location.th);
+// var tracker = new CrabMotionFrameTracker
+// {
+// CommandBackend =
+// CrabMotionFrameTracker
+// .ChassisCommandBackend
+// .SendMotion,
+// PathKind =
+// CrabMotionFrameTracker
+// .ReferencePathKind.LeftArc,
+// StartPosition = source,
+// InitialBodyYawRadians =
+// bodyYawRadians,
+// RadiusMillimeters =
+// RadiusMillimeters,
+// ArcSweepRadians = Math.PI / 2.0,
+// CruiseSpeed = CruiseSpeed
+// };
+// var destination =
+// tracker.GetArcDestination();
+
+// _recorder = new TrackingExperimentRecorder(
+// controllerName:
+// "CrabSendMotionTracker",
+// trajectoryName:
+// $"CrabLeftArc90_R{RadiusMillimeters:0}mm",
+// trialNumber: TrialNumber,
+// referenceStart: source,
+// referenceEnd: destination,
+// referenceSpeed: CruiseSpeed,
+// referenceMotionFrameYawDegrees: 90f);
+// tracker.CommandObserver =
+// (vx, vy, omega) =>
+// _recorder?.UpdateBodyCommand(
+// vx,
+// vy,
+// omega);
+// _recorder.Start();
+
+// try
+// {
+// _task = new DriveTask(tracker.Get());
+// _task.Wait();
+// Thread.Sleep(300);
+// }
+// finally
+// {
+// _task?.Stop();
+// _recorder?.UpdateBodyCommand(
+// 0f,
+// 0f,
+// 0f);
+// _recorder?.StopAndSave();
+// _task = null;
+// _recorder = null;
+// }
+// }
+
+// public override void TestStop()
+// {
+// _task?.Stop();
+// _recorder?.UpdateBodyCommand(
+// 0f,
+// 0f,
+// 0f);
+// _recorder?.StopAndSave();
+// }
+// }
+
+// [MovementTest(name = "SendMotion：4m S型曲线")]
+// public class TestSCurve4m : MovementTest
+// {
+// public float LengthMillimeters = 4000f; // S型曲线纵向长度，单位mm。
+// public float LateralOffsetMillimeters = 400f; // S型曲线左右两侧的最大偏移，单位mm。
+// public float CruiseSpeed = 0.3f; // 首次实车测试建议使用0.3m/s。
+// public int TrialNumber = 1; // 重复实验编号。
+
+// private DriveTask _task;
+// private TrackingExperimentRecorder _recorder;
+
+// // 从当前Detour位姿开始，沿车头方向跟踪先左偏、再右偏并最终回中的完整S型曲线。
+// public override void Test()
+// {
+// if (float.IsNaN(LengthMillimeters) ||
+// float.IsInfinity(LengthMillimeters) ||
+// LengthMillimeters <= 0f ||
+// float.IsNaN(LateralOffsetMillimeters) ||
+// float.IsInfinity(LateralOffsetMillimeters) ||
+// LateralOffsetMillimeters <= 0f ||
+// float.IsNaN(CruiseSpeed) ||
+// float.IsInfinity(CruiseSpeed) ||
+// CruiseSpeed <= 0f)
+// {
+// Console.WriteLine("S型曲线测试参数无效。");
+// return;
+// }
+
+// if (!MovementTestPreparation.AreWheelsForward())
+// return;
+
+// var location = DetourInterface.getCartLocation();
+// if (double.IsNaN(location.x) ||
+// double.IsInfinity(location.x) ||
+// double.IsNaN(location.y) ||
+// double.IsInfinity(location.y) ||
+// double.IsNaN(location.th) ||
+// double.IsInfinity(location.th))
+// {
+// Console.WriteLine(
+// "Detour当前位姿无效，取消4m S型曲线测试。");
+// return;
+// }
+
+// var source =
+// new Vector2((float)location.x, (float)location.y);
+// var headingRadians =
+// AngleMath.DegreesToRadians(location.th);
+// var length = LengthMillimeters;
+// var offset = LateralOffsetMillimeters;
+
+// // 三段三次贝塞尔依次经过左侧峰值、中心线和右侧峰值，
+// // 起点、两个峰值和终点的切线均沿初始前向，连接处没有折角。
+// var firstControlPoints = new List
+// {
+// LocalToWorld(source, headingRadians, 0f, 0f),
+// LocalToWorld(
+// source, headingRadians,
+// length / 12f, 0f),
+// LocalToWorld(
+// source, headingRadians,
+// length / 6f, offset),
+// LocalToWorld(
+// source, headingRadians,
+// length * 0.25f, offset)
+// };
+// var secondControlPoints = new List
+// {
+// LocalToWorld(
+// source, headingRadians,
+// length * 0.25f, offset),
+// LocalToWorld(
+// source, headingRadians,
+// length / 3f, offset),
+// LocalToWorld(
+// source, headingRadians,
+// length * 2f / 3f, -offset),
+// LocalToWorld(
+// source, headingRadians,
+// length * 0.75f, -offset)
+// };
+// var thirdControlPoints = new List
+// {
+// LocalToWorld(
+// source, headingRadians,
+// length * 0.75f, -offset),
+// LocalToWorld(
+// source, headingRadians,
+// length * 5f / 6f, -offset),
+// LocalToWorld(
+// source, headingRadians,
+// length * 11f / 12f, 0f),
+// LocalToWorld(
+// source, headingRadians,
+// length, 0f)
+// };
+
+// var firstTrack = new BezierTrack(firstControlPoints)
+// {
+// Speed = CruiseSpeed,
+// CarDirectionBias = 0f
+// };
+// var secondTrack = new BezierTrack(secondControlPoints)
+// {
+// Speed = CruiseSpeed,
+// CarDirectionBias = 0f
+// };
+// var thirdTrack = new BezierTrack(thirdControlPoints)
+// {
+// Speed = CruiseSpeed,
+// CarDirectionBias = 0f
+// };
+
+// var controller = new ChassisController
+// {
+// BaseSpeed = CruiseSpeed
+// }.Get();
+// controller.FinishSpeed = 0f;
+
+// if (!controller.AddTrack(
+// firstTrack,
+// "SCurve4m-Part1") ||
+// !controller.AddTrack(
+// secondTrack,
+// "SCurve4m-Part2") ||
+// !controller.AddTrack(
+// thirdTrack,
+// "SCurve4m-Part3"))
+// {
+// Console.WriteLine(
+// "4m S型曲线轨迹添加失败，取消测试。");
+// return;
+// }
+
+// var destination =
+// LocalToWorld(
+// source,
+// headingRadians,
+// length,
+// 0f);
+// _recorder = new TrackingExperimentRecorder(
+// controllerName: "LegacyGeometricController",
+// trajectoryName:
+// $"LegacySCurve4m_A{LateralOffsetMillimeters:0}mm",
+// trialNumber: TrialNumber,
+// referenceStart: source,
+// referenceEnd: destination,
+// referenceSpeed: CruiseSpeed);
+// _recorder.Start();
+
+// try
+// {
+// _task = new DriveTask(controller.Track());
+// _task.Wait();
+
+// // 保留少量停止后的数据，用于观察速度是否回到零。
+// Thread.Sleep(300);
+// }
+// finally
+// {
+// _task?.Stop();
+// _recorder?.UpdateCommand(0f, 0f);
+// _recorder?.StopAndSave();
+// _task = null;
+// _recorder = null;
+// }
+// }
+
+// // 停止S型曲线测试并保存当前已经采集的数据。
+// public override void TestStop()
+// {
+// _task?.Stop();
+// _recorder?.UpdateCommand(0f, 0f);
+// _recorder?.StopAndSave();
+// }
+
+// // 将车体起点局部坐标转换为Detour世界坐标，X向前、Y向左。
+// private static Vector2 LocalToWorld(
+// Vector2 origin,
+// double headingRadians,
+// float localX,
+// float localY)
+// {
+// var cos = (float)Math.Cos(headingRadians);
+// var sin = (float)Math.Sin(headingRadians);
+
+// return new Vector2(
+// origin.X + localX * cos - localY * sin,
+// origin.Y + localX * sin + localY * cos);
+// }
+// }
+// }
diff --git a/MultiWheelC/StateEstimation/ParkingVehicleStateProviderFactory.cs b/MultiWheelC/StateEstimation/ParkingVehicleStateProviderFactory.cs
new file mode 100644
index 0000000..19474e4
--- /dev/null
+++ b/MultiWheelC/StateEstimation/ParkingVehicleStateProviderFactory.cs
@@ -0,0 +1,66 @@
+using System;
+using CommonUsage.Chassis;
+using MyParking.Shared;
+
+namespace MultiWheelC.StateEstimation
+{
+ ///
+ /// 根据车载配置创建Detour位姿过滤与电机反馈纵向速度组合的停车状态源。
+ ///
+ public static class ParkingVehicleStateProviderFactory
+ {
+ ///
+ /// 使用当前车辆运行配置创建停车轨迹控制的默认状态源。
+ ///
+ public static WheelFeedbackVehicleStateProvider Create(
+ MultiWheelChassis chassis)
+ {
+ return Create(
+ chassis,
+ PilotDefinition.Conf);
+ }
+
+ ///
+ /// 使用指定配置快照创建停车轨迹控制的默认状态源。
+ ///
+ public static WheelFeedbackVehicleStateProvider Create(
+ MultiWheelChassis chassis,
+ PilotConfig config)
+ {
+ if (chassis == null)
+ {
+ throw new ArgumentNullException(nameof(chassis));
+ }
+
+ if (config == null)
+ {
+ throw new ArgumentNullException(nameof(config));
+ }
+
+ var velocityEstimator =
+ new VelocityEstimator2D(
+ config.ParkingDetourLinearVelocityFilterSeconds,
+ config.ParkingDetourAngularVelocityFilterSeconds);
+ var detourStateProvider =
+ new DetourVehicleStateProvider(
+ velocityEstimator,
+ config.ParkingDetourMaximumLinearSpeed,
+ AngleMath.DegreesToRadians(
+ config
+ .ParkingDetourMaximumAngularSpeedDegrees),
+ config.ParkingDetourPositionJumpMargin,
+ AngleMath.DegreesToRadians(
+ config.ParkingDetourHeadingJumpMarginDegrees),
+ config.ParkingDetourVelocityPositionResidual,
+ AngleMath.DegreesToRadians(
+ config
+ .ParkingDetourVelocityHeadingResidualDegrees),
+ config.ParkingDetourStationaryConfirmationSeconds);
+
+ return new WheelFeedbackVehicleStateProvider(
+ detourStateProvider,
+ chassis,
+ config.ParkingWheelVelocityFilterSeconds);
+ }
+ }
+}
diff --git a/MultiWheelC/build/Clumsy/CommonUsage.dll b/MultiWheelC/build/Clumsy/CommonUsage.dll
index 9ff7b74..92a453a 100644
Binary files a/MultiWheelC/build/Clumsy/CommonUsage.dll and b/MultiWheelC/build/Clumsy/CommonUsage.dll differ
diff --git a/MultiWheelC/build/Clumsy/MultiWheelC.dll b/MultiWheelC/build/Clumsy/MultiWheelC.dll
index a19ff33..4dcd48e 100644
Binary files a/MultiWheelC/build/Clumsy/MultiWheelC.dll and b/MultiWheelC/build/Clumsy/MultiWheelC.dll differ
diff --git a/MultiWheelC/build/Clumsy/MultiWheelC.pdb b/MultiWheelC/build/Clumsy/MultiWheelC.pdb
index d982861..71e5e30 100644
Binary files a/MultiWheelC/build/Clumsy/MultiWheelC.pdb and b/MultiWheelC/build/Clumsy/MultiWheelC.pdb differ
diff --git a/Shared/Validation/NumericGuard.cs b/Shared/Validation/NumericGuard.cs
new file mode 100644
index 0000000..cf559bd
--- /dev/null
+++ b/Shared/Validation/NumericGuard.cs
@@ -0,0 +1,80 @@
+using System;
+
+namespace MyParking.Shared
+{
+ ///
+ /// 统一检查跨层数值和二维位姿是否由满足基本范围要求的有限值组成。
+ ///
+ public static class NumericGuard
+ {
+ ///
+ /// 确保指定浮点数不是NaN或无穷大。
+ ///
+ public static void EnsureFinite(
+ double value,
+ string parameterName)
+ {
+ if (double.IsNaN(value) ||
+ double.IsInfinity(value))
+ {
+ throw new ArgumentOutOfRangeException(
+ parameterName,
+ "参数必须是有限值。");
+ }
+ }
+
+ ///
+ /// 确保指定浮点数是非负有限值。
+ ///
+ public static void EnsureFiniteNonNegative(
+ double value,
+ string parameterName)
+ {
+ EnsureFinite(value, parameterName);
+
+ if (value < 0.0)
+ {
+ throw new ArgumentOutOfRangeException(
+ parameterName,
+ "参数必须是非负有限值。");
+ }
+ }
+
+ ///
+ /// 确保指定浮点数是正有限值。
+ ///
+ public static void EnsureFinitePositive(
+ double value,
+ string parameterName)
+ {
+ EnsureFinite(value, parameterName);
+
+ if (value <= 0.0)
+ {
+ throw new ArgumentOutOfRangeException(
+ parameterName,
+ "参数必须是正有限值。");
+ }
+ }
+
+ ///
+ /// 确保二维位姿的位置和航向均为有限值。
+ ///
+ public static void EnsureFinite(
+ Pose2D pose,
+ string parameterName)
+ {
+ if (double.IsNaN(pose.XMeters) ||
+ double.IsInfinity(pose.XMeters) ||
+ double.IsNaN(pose.YMeters) ||
+ double.IsInfinity(pose.YMeters) ||
+ double.IsNaN(pose.YawRadians) ||
+ double.IsInfinity(pose.YawRadians))
+ {
+ throw new ArgumentOutOfRangeException(
+ parameterName,
+ "二维位姿必须由有限值组成。");
+ }
+ }
+ }
+}
diff --git a/output/C/CommonUsage.dll b/output/C/CommonUsage.dll
index 9ff7b74..92a453a 100644
Binary files a/output/C/CommonUsage.dll and b/output/C/CommonUsage.dll differ
diff --git a/output/C/MultiWheelC.dll b/output/C/MultiWheelC.dll
index a19ff33..4dcd48e 100644
Binary files a/output/C/MultiWheelC.dll and b/output/C/MultiWheelC.dll differ
diff --git a/output/C/MultiWheelC.pdb b/output/C/MultiWheelC.pdb
index d982861..71e5e30 100644
Binary files a/output/C/MultiWheelC.pdb and b/output/C/MultiWheelC.pdb differ
diff --git a/output/M/CommonUsage.dll b/output/M/CommonUsage.dll
index 9ff7b74..92a453a 100644
Binary files a/output/M/CommonUsage.dll and b/output/M/CommonUsage.dll differ
diff --git a/output/M/MedullaAdapter.dll b/output/M/MedullaAdapter.dll
index fb1eff4..6bb0299 100644
Binary files a/output/M/MedullaAdapter.dll and b/output/M/MedullaAdapter.dll differ
diff --git a/output/M/MedullaAdapter.pdb b/output/M/MedullaAdapter.pdb
index d91a710..a93d960 100644
Binary files a/output/M/MedullaAdapter.pdb and b/output/M/MedullaAdapter.pdb differ
diff --git a/ref/CommonUsage.dll b/ref/CommonUsage.dll
index 9ff7b74..92a453a 100644
Binary files a/ref/CommonUsage.dll and b/ref/CommonUsage.dll differ