First_Dev_Dept/ParkingRobot
10
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

完善Detour状态估计与轨迹跟踪验证

Browse Source
This commit is contained in:
yuxiang.shen
2026-08-19 17:38:17 +08:00
parent d8de901a80
commit 0d5539e595
55 changed files with 4473 additions and 387 deletions
Download Patch File Download Diff File Expand all files Collapse all files
MultiWheelC/Experiments/CompositeMotionPlanTests.cs
+2 -1
View File
@@ -157,7 +157,8 @@ namespace MultiWheelC
(float)AccelerationMetersPerSecondSquared,
referenceDecelerationMetersPerSecondSquared:
(float)DecelerationMetersPerSecondSquared,
diagnosticChassis: chassis);
diagnosticChassis: chassis,
diagnosticStateProvider: stateProvider);
_recorder.Start();
var controlPointRadiusMeters =
MultiWheelC/Experiments/DetourStaticDiagnosticTest.cs
+696
View File
@@ -0,0 +1,696 @@
using System;
using System.Collections.Generic;
using System.Diagnostics;
using System.Globalization;
using System.IO;
using System.Text;
using System.Threading;
using ClumsyCore;
using ClumsyCore.Interfaces;
using ClumsyCore.Pilot;
using CommonUsage.Chassis;
using FundamentalLib;
using MyParking.Shared;
namespace MultiWheelC
{
/// <summary>
/// 从C层测试界面启动定时的Detour静态定位与轮组反馈诊断记录。
/// </summary>
[MovementTest(name = "诊断：Detour静态定位记录")]
public sealed class DetourStaticDiagnosticTest : MovementTest
{
private sealed class DiagnosticSample
{
public double ElapsedSeconds;
public string LocalTimestamp;
public bool DetourReadSucceeded;
public double DetourCallDurationMilliseconds;
public long? DetourTickRaw;
public string DetourTimestamp;
public bool DetourTimestampValid;
public double? DetourDataAgeMilliseconds;
public double? DetourLStep;
public double? DetourXMillimeters;
public double? DetourYMillimeters;
public double? DetourYawDegrees;
public double? LocalDeltaMilliseconds;
public double? DetourTickDeltaMilliseconds;
public double? DeltaXMillimeters;
public double? DeltaYMillimeters;
public double? DeltaYawDegrees;
public double? DeltaPositionMillimeters;
public bool IsRepeatedTick;
public bool IsRepeatedPose;
public bool IsOutOfOrderTick;
public bool WheelReadSucceeded;
public double? WheelBodyVxMetersPerSecond;
public double? WheelBodyVyMetersPerSecond;
public double? WheelBodyOmegaDegreesPerSecond;
public double? ActualSteerLeftFrontDegrees;
public double? ActualSteerLeftRearDegrees;
public double? ActualSteerRightFrontDegrees;
public double? ActualSteerRightRearDegrees;
public double? ActualSpeedLeftFrontMetersPerSecond;
public double? ActualSpeedLeftRearMetersPerSecond;
public double? ActualSpeedRightFrontMetersPerSecond;
public double? ActualSpeedRightRearMetersPerSecond;
public string FailureReason;
}
private readonly object _sampleSyncRoot = new object();
private readonly List<DiagnosticSample> _samples =
new List<DiagnosticSample>();
private readonly Stopwatch _clock = new Stopwatch();
private MultiWheelChassis _chassis;
private Thread _samplingThread;
private volatile bool _sampling;
private int _testRunning;
private int _stopRequested;
private int _sessionId;
private bool _hasPreviousDetourSample;
private double _previousElapsedSeconds;
private long _previousDetourTick;
private double _previousDetourXMillimeters;
private double _previousDetourYMillimeters;
private double _previousDetourYawDegrees;
/// <summary>
/// 获取或设置自动结束前的记录时长，单位为min。
/// </summary>
public double DurationMinutes = 20.0;
/// <summary>
/// 获取或设置本机主动读取Detour的周期，单位为ms。
/// </summary>
public int SampleIntervalMilliseconds = 50;
/// <summary>
/// 获取最近一次静态诊断CSV的完整路径。
/// </summary>
public string SavedFilePath { get; private set; } =
string.Empty;
/// <summary>
/// 停车后开始静态采样，并在到达设定时长时自动保存CSV。
/// </summary>
public override void Test()
{
if (Interlocked.CompareExchange(
ref _testRunning,
1,
0) != 0)
{
Console.WriteLine("Detour静态诊断已经在运行。");
return;
}
var samplingStarted = false;
var completedAutomatically = false;
Exception testFailure = null;
try
{
ValidateSettings();
_chassis =
PilotDefinition.Chassis as MultiWheelChassis;
if (_chassis == null)
{
throw new InvalidOperationException(
"当前底盘不是MultiWheelChassis，无法读取四轮反馈。");
}
var sessionId = ResetSession();
_chassis.PredefinedDriveStop();
_sampling = true;
_clock.Restart();
samplingStarted = true;
_samplingThread = new Thread(
() => SamplingLoop(sessionId))
{
IsBackground = true,
Name = "DetourStaticDiagnostic"
};
_samplingThread.Start();
Console.WriteLine(
$"Detour静态诊断开始：时长={DurationMinutes:F1}min，" +
$"主动读取周期={SampleIntervalMilliseconds}ms，" +
"车辆必须保持静止。");
Hedingben.ToastText(
$"Detour静态诊断开始，预计{DurationMinutes:F1}分钟后自动结束。");
var durationSeconds = DurationMinutes * 60.0;
while (Volatile.Read(ref _stopRequested) == 0 &&
_clock.Elapsed.TotalSeconds < durationSeconds)
{
Thread.Sleep(100);
}
completedAutomatically =
Volatile.Read(ref _stopRequested) == 0;
}
catch (Exception exception)
{
testFailure = exception;
Console.WriteLine(
"Detour静态诊断失败：" +
exception.Message);
}
finally
{
_sampling = false;
_chassis?.PredefinedDriveStop();
if (_samplingThread != null &&
_samplingThread != Thread.CurrentThread)
{
_samplingThread.Join(
Math.Max(
1000,
SampleIntervalMilliseconds * 4));
}
_clock.Stop();
if (samplingStarted)
{
try
{
SaveCsvAndReport(
completedAutomatically,
testFailure);
}
catch (Exception exception)
{
Console.WriteLine(
"Detour静态诊断CSV保存失败：" +
exception.Message);
Hedingben.ToastText(
"Detour静态诊断CSV保存失败：" +
exception.Message);
}
}
_samplingThread = null;
_chassis = null;
Interlocked.Exchange(ref _testRunning, 0);
}
}
/// <summary>
/// 请求提前停止采样；测试线程随后保存已经采集的数据。
/// </summary>
public override void TestStop()
{
Interlocked.Exchange(ref _stopRequested, 1);
_sampling = false;
_chassis?.PredefinedDriveStop();
}
/// <summary>
/// 清除上一次测试的样本、时间基准和输出路径。
/// </summary>
private int ResetSession()
{
lock (_sampleSyncRoot)
{
_samples.Clear();
}
Interlocked.Exchange(ref _stopRequested, 0);
_hasPreviousDetourSample = false;
_previousElapsedSeconds = 0.0;
_previousDetourTick = 0;
_previousDetourXMillimeters = 0.0;
_previousDetourYMillimeters = 0.0;
_previousDetourYawDegrees = 0.0;
SavedFilePath = string.Empty;
return Interlocked.Increment(ref _sessionId);
}
/// <summary>
/// 检查测试时长和主动采样周期是否适合执行。
/// </summary>
private void ValidateSettings()
{
NumericGuard.EnsureFinitePositive(
DurationMinutes,
nameof(DurationMinutes));
if (SampleIntervalMilliseconds < 20 ||
SampleIntervalMilliseconds > 5000)
{
throw new ArgumentOutOfRangeException(
nameof(SampleIntervalMilliseconds),
"Detour主动读取周期必须在20ms到5000ms之间。");
}
}
/// <summary>
/// 按设定周期持续采样，直至测试到时或收到停止请求。
/// </summary>
private void SamplingLoop(int sessionId)
{
while (_sampling &&
sessionId == Volatile.Read(ref _sessionId))
{
CaptureSample(sessionId);
Thread.Sleep(SampleIntervalMilliseconds);
}
}
/// <summary>
/// 采集一帧原始Detour定位、接口耗时和四轮实际反馈。
/// </summary>
private void CaptureSample(int sessionId)
{
var sample = new DiagnosticSample
{
ElapsedSeconds = _clock.Elapsed.TotalSeconds,
LocalTimestamp =
DateTimeOffset.Now.ToString(
"O",
CultureInfo.InvariantCulture),
FailureReason = string.Empty
};
CaptureDetour(sample, sessionId);
if (sessionId != Volatile.Read(ref _sessionId))
{
return;
}
CaptureWheelFeedback(sample);
if (!_sampling ||
sessionId != Volatile.Read(ref _sessionId))
{
return;
}
lock (_sampleSyncRoot)
{
_samples.Add(sample);
}
}
/// <summary>
/// 读取Detour原始字段并计算与上一成功读取之间的时间和位姿差。
/// </summary>
private void CaptureDetour(
DiagnosticSample sample,
int sessionId)
{
var callClock = Stopwatch.StartNew();
try
{
var location =
DetourInterface.getCartLocation();
callClock.Stop();
sample.DetourCallDurationMilliseconds =
callClock.Elapsed.TotalMilliseconds;
sample.DetourReadSucceeded = true;
sample.DetourTickRaw = Convert.ToInt64(
location.tick,
CultureInfo.InvariantCulture);
sample.DetourLStep = Convert.ToDouble(
location.l_step,
CultureInfo.InvariantCulture);
sample.DetourXMillimeters = Convert.ToDouble(
location.x,
CultureInfo.InvariantCulture);
sample.DetourYMillimeters = Convert.ToDouble(
location.y,
CultureInfo.InvariantCulture);
sample.DetourYawDegrees = Convert.ToDouble(
location.th,
CultureInfo.InvariantCulture);
if (sessionId != Volatile.Read(ref _sessionId))
{
return;
}
CaptureDetourTimestamp(sample);
CaptureDetourDelta(sample);
}
catch (Exception exception)
{
callClock.Stop();
sample.DetourCallDurationMilliseconds =
callClock.Elapsed.TotalMilliseconds;
AppendFailure(
sample,
"Detour读取失败：" +
exception.Message);
}
}
/// <summary>
/// 将Detour原始tick按.NET DateTime ticks解释并记录数据年龄。
/// </summary>
private static void CaptureDetourTimestamp(
DiagnosticSample sample)
{
try
{
var detourTime = new DateTime(
sample.DetourTickRaw.Value,
DateTimeKind.Local);
sample.DetourTimestamp =
detourTime.ToString(
"O",
CultureInfo.InvariantCulture);
sample.DetourDataAgeMilliseconds =
(DateTime.Now - detourTime)
.TotalMilliseconds;
sample.DetourTimestampValid = true;
}
catch (ArgumentOutOfRangeException)
{
sample.DetourTimestamp = string.Empty;
}
}
/// <summary>
/// 计算Detour帧间差并更新下一帧使用的原始基准。
/// </summary>
private void CaptureDetourDelta(
DiagnosticSample sample)
{
var tick = sample.DetourTickRaw.Value;
var xMillimeters = sample.DetourXMillimeters.Value;
var yMillimeters = sample.DetourYMillimeters.Value;
var yawDegrees = sample.DetourYawDegrees.Value;
if (_hasPreviousDetourSample)
{
sample.LocalDeltaMilliseconds =
(sample.ElapsedSeconds -
_previousElapsedSeconds) * 1000.0;
sample.DetourTickDeltaMilliseconds =
(tick - _previousDetourTick) /
(double)TimeSpan.TicksPerMillisecond;
sample.DeltaXMillimeters =
xMillimeters - _previousDetourXMillimeters;
sample.DeltaYMillimeters =
yMillimeters - _previousDetourYMillimeters;
sample.DeltaYawDegrees =
AngleMath.ShortestDifferenceDegrees(
yawDegrees,
_previousDetourYawDegrees);
sample.DeltaPositionMillimeters = Math.Sqrt(
sample.DeltaXMillimeters.Value *
sample.DeltaXMillimeters.Value +
sample.DeltaYMillimeters.Value *
sample.DeltaYMillimeters.Value);
sample.IsRepeatedTick =
tick == _previousDetourTick;
sample.IsOutOfOrderTick =
tick < _previousDetourTick;
sample.IsRepeatedPose =
sample.DeltaPositionMillimeters.Value <= 1e-6 &&
Math.Abs(sample.DeltaYawDegrees.Value) <= 1e-9;
}
_hasPreviousDetourSample = true;
_previousElapsedSeconds = sample.ElapsedSeconds;
_previousDetourTick = tick;
_previousDetourXMillimeters = xMillimeters;
_previousDetourYMillimeters = yMillimeters;
_previousDetourYawDegrees = yawDegrees;
}
/// <summary>
/// 读取底盘反算速度与按物理安装位置识别的四轮实际反馈。
/// </summary>
private void CaptureWheelFeedback(
DiagnosticSample sample)
{
try
{
var carSpeed = _chassis.GetCarSpeed(true);
sample.WheelBodyVxMetersPerSecond = carSpeed.Vx;
sample.WheelBodyVyMetersPerSecond = carSpeed.Vy;
// CommonUsage的CarSpeed.Vw以deg/s表达。
sample.WheelBodyOmegaDegreesPerSecond = carSpeed.Vw;
#pragma warning disable CS0612, CS0618
var wheels = _chassis.GetSteerWheels();
#pragma warning restore CS0612, CS0618
var leftFront = FindWheel(wheels, true, true);
var leftRear = FindWheel(wheels, false, true);
var rightFront = FindWheel(wheels, true, false);
var rightRear = FindWheel(wheels, false, false);
if (leftFront == null || leftRear == null ||
rightFront == null || rightRear == null)
{
throw new InvalidOperationException(
"未能按物理安装位置识别四个舵轮。");
}
sample.ActualSteerLeftFrontDegrees =
leftFront.ReadAngle();
sample.ActualSteerLeftRearDegrees =
leftRear.ReadAngle();
sample.ActualSteerRightFrontDegrees =
rightFront.ReadAngle();
sample.ActualSteerRightRearDegrees =
rightRear.ReadAngle();
sample.ActualSpeedLeftFrontMetersPerSecond =
leftFront.ReadSpeed();
sample.ActualSpeedLeftRearMetersPerSecond =
leftRear.ReadSpeed();
sample.ActualSpeedRightFrontMetersPerSecond =
rightFront.ReadSpeed();
sample.ActualSpeedRightRearMetersPerSecond =
rightRear.ReadSpeed();
sample.WheelReadSucceeded = true;
}
catch (Exception exception)
{
AppendFailure(
sample,
"四轮反馈读取失败：" +
exception.Message);
}
}
/// <summary>
/// 根据真实车体X向前、Y向左的物理安装位置查找指定舵轮。
/// </summary>
private static SteerWheel FindWheel(
IReadOnlyList<SteerWheel> wheels,
bool requireFront,
bool requireLeft)
{
foreach (var wheel in wheels)
{
var isFront = wheel.PhysicalPosition.X >= 0f;
var isLeft = wheel.PhysicalPosition.Y >= 0f;
if (isFront == requireFront &&
isLeft == requireLeft)
{
return wheel;
}
}
return null;
}
/// <summary>
/// 追加本帧诊断失败原因且保留先前错误信息。
/// </summary>
private static void AppendFailure(
DiagnosticSample sample,
string reason)
{
sample.FailureReason =
string.IsNullOrWhiteSpace(sample.FailureReason)
? reason
: sample.FailureReason + "；" + reason;
}
/// <summary>
/// 保存采样快照并输出自动结束或手动停止后的摘要提示。
/// </summary>
private void SaveCsvAndReport(
bool completedAutomatically,
Exception testFailure)
{
List<DiagnosticSample> snapshot;
lock (_sampleSyncRoot)
{
snapshot =
new List<DiagnosticSample>(_samples);
}
var outputDirectory = Path.Combine(
AppContext.BaseDirectory,
"DetourStaticDiagnostics");
Directory.CreateDirectory(outputDirectory);
SavedFilePath = Path.Combine(
outputDirectory,
$"{DateTime.Now:yyyyMMdd_HHmmss_fff}_" +
"DetourStaticDiagnostic.csv");
using (var writer = new StreamWriter(
SavedFilePath,
false,
new UTF8Encoding(true)))
{
WriteCsvRow(writer,
"ElapsedSeconds", "LocalTimestamp",
"DetourReadSucceeded", "DetourCallDurationMilliseconds",
"DetourTickRaw", "DetourTimestamp",
"DetourTimestampValid", "DetourDataAgeMilliseconds",
"DetourLStep", "DetourXMillimeters",
"DetourYMillimeters", "DetourYawDegrees",
"LocalDeltaMilliseconds", "DetourTickDeltaMilliseconds",
"DeltaXMillimeters", "DeltaYMillimeters",
"DeltaYawDegrees", "DeltaPositionMillimeters",
"IsRepeatedTick", "IsRepeatedPose", "IsOutOfOrderTick",
"WheelReadSucceeded", "WheelBodyVxMetersPerSecond",
"WheelBodyVyMetersPerSecond",
"WheelBodyOmegaDegreesPerSecond",
"ActualSteerLeftFrontDegrees",
"ActualSteerLeftRearDegrees",
"ActualSteerRightFrontDegrees",
"ActualSteerRightRearDegrees",
"ActualSpeedLeftFrontMetersPerSecond",
"ActualSpeedLeftRearMetersPerSecond",
"ActualSpeedRightFrontMetersPerSecond",
"ActualSpeedRightRearMetersPerSecond",
"FailureReason");
foreach (var sample in snapshot)
{
WriteCsvRow(writer,
sample.ElapsedSeconds, sample.LocalTimestamp,
sample.DetourReadSucceeded,
sample.DetourCallDurationMilliseconds,
sample.DetourTickRaw, sample.DetourTimestamp,
sample.DetourTimestampValid,
sample.DetourDataAgeMilliseconds,
sample.DetourLStep, sample.DetourXMillimeters,
sample.DetourYMillimeters, sample.DetourYawDegrees,
sample.LocalDeltaMilliseconds,
sample.DetourTickDeltaMilliseconds,
sample.DeltaXMillimeters, sample.DeltaYMillimeters,
sample.DeltaYawDegrees,
sample.DeltaPositionMillimeters,
sample.IsRepeatedTick, sample.IsRepeatedPose,
sample.IsOutOfOrderTick,
sample.WheelReadSucceeded,
sample.WheelBodyVxMetersPerSecond,
sample.WheelBodyVyMetersPerSecond,
sample.WheelBodyOmegaDegreesPerSecond,
sample.ActualSteerLeftFrontDegrees,
sample.ActualSteerLeftRearDegrees,
sample.ActualSteerRightFrontDegrees,
sample.ActualSteerRightRearDegrees,
sample.ActualSpeedLeftFrontMetersPerSecond,
sample.ActualSpeedLeftRearMetersPerSecond,
sample.ActualSpeedRightFrontMetersPerSecond,
sample.ActualSpeedRightRearMetersPerSecond,
sample.FailureReason);
}
}
var successfulSamples = 0;
var maximumPositionStepMillimeters = 0.0;
var maximumAbsoluteYawStepDegrees = 0.0;
foreach (var sample in snapshot)
{
if (sample.DetourReadSucceeded)
{
successfulSamples++;
}
maximumPositionStepMillimeters = Math.Max(
maximumPositionStepMillimeters,
sample.DeltaPositionMillimeters ?? 0.0);
maximumAbsoluteYawStepDegrees = Math.Max(
maximumAbsoluteYawStepDegrees,
Math.Abs(sample.DeltaYawDegrees ?? 0.0));
}
var completionReason = testFailure != null
? "因异常提前结束"
: completedAutomatically
? "到达设定时长，已自动结束"
: "收到手动停止请求";
var message =
$"Detour静态诊断{completionReason}；" +
$"样本={snapshot.Count}，有效Detour样本={successfulSamples}，" +
$"最大位置阶跃={maximumPositionStepMillimeters:F2}mm，" +
$"最大航向阶跃={maximumAbsoluteYawStepDegrees:F3}°；" +
$"CSV={SavedFilePath}";
Console.WriteLine(message);
Hedingben.ToastText(message);
}
/// <summary>
/// 使用InvariantCulture格式化并转义一行CSV字段。
/// </summary>
private static void WriteCsvRow(
TextWriter writer,
params object[] values)
{
var fields = new string[values.Length];
for (var index = 0; index < values.Length; index++)
{
fields[index] = FormatCsvValue(values[index]);
}
writer.WriteLine(string.Join(",", fields));
}
/// <summary>
/// 将单个值转换为区域无关且符合CSV转义规则的文本。
/// </summary>
private static string FormatCsvValue(object value)
{
if (value == null)
{
return string.Empty;
}
string text;
if (value is bool boolean)
{
text = boolean ? "1" : "0";
}
else if (value is IFormattable formattable)
{
text = formattable.ToString(
null,
CultureInfo.InvariantCulture);
}
else
{
text = value.ToString();
}
if (text.IndexOfAny(
new[] { ',', '"', '\r', '\n' }) < 0)
{
return text;
}
return "\"" +
text.Replace("\"", "\"\"") +
"\"";
}
}
}
MultiWheelC/Experiments/NewControllerTrackingTests.cs
+59 -5
View File
@@ -231,7 +231,8 @@ namespace MultiWheelC
(float)AccelerationMetersPerSecondSquared,
referenceDecelerationMetersPerSecondSquared:
(float)DecelerationMetersPerSecondSquared,
diagnosticChassis: chassis);
diagnosticChassis: chassis,
diagnosticStateProvider: stateProvider);
_recorder = recorder;
var controlPointRadiusMeters =
@@ -521,7 +522,7 @@ namespace MultiWheelC
/// 从当前Detour位姿开始执行“3m直线—左半圆—3m直线”新版控制器跟踪实验。
/// </summary>
[MovementTest(name = "新版控制器：直线-左半圆-直线轨迹跟踪")]
public sealed class NewControllerStraightSemicircleStraightTest
public class NewControllerStraightSemicircleStraightTest
: MovementTest
{
private const float MillimetersPerMeter = 1000f;
@@ -579,6 +580,24 @@ namespace MultiWheelC
/// </summary>
public double PointSpacingMeters = 0.02;
/// <summary>
/// 获取组合轨迹主运动方向相对车头的夹角，单位为rad。
/// </summary>
protected virtual double MotionDirectionInBodyRadians =>
0.0;
/// <summary>
/// 获取轨迹完成后是否需要将舵轮主动恢复到车头方向。
/// </summary>
protected virtual bool ReturnWheelsForwardAfterCompletion =>
false;
/// <summary>
/// 获取实验记录使用的轨迹基础名称。
/// </summary>
protected virtual string ExperimentTrajectoryBaseName =>
"ProfiledStraightSmoothLeftTurnStraight";
/// <summary>
/// 读取当前位姿、绘制组合轨迹并启动新版轨迹跟踪动作。
/// </summary>
@@ -637,7 +656,8 @@ namespace MultiWheelC
SemicircleMaximumSpeedMetersPerSecond,
AccelerationMetersPerSecondSquared,
DecelerationMetersPerSecondSquared,
PointSpacingMeters);
PointSpacingMeters,
MotionDirectionInBodyRadians);
DrawTrajectory(trajectory);
@@ -650,7 +670,7 @@ namespace MultiWheelC
controllerName: "NewStanleyPid",
trajectoryName:
TrajectoryExperimentInput.BuildTrajectoryName(
"ProfiledStraightSmoothLeftTurnStraight",
ExperimentTrajectoryBaseName,
lateralOffsetMeters),
trialNumber: TrialNumber,
referenceStart: referenceStart,
@@ -658,11 +678,15 @@ namespace MultiWheelC
referenceSpeed:
(float)StraightMaximumSpeedMetersPerSecond,
sampleIntervalMs: 50,
referenceMotionFrameYawDegrees:
(float)AngleMath.RadiansToDegrees(
MotionDirectionInBodyRadians),
referenceAccelerationMetersPerSecondSquared:
(float)AccelerationMetersPerSecondSquared,
referenceDecelerationMetersPerSecondSquared:
(float)DecelerationMetersPerSecondSquared,
diagnosticChassis: chassis);
diagnosticChassis: chassis,
diagnosticStateProvider: stateProvider);
_recorder = recorder;
var controlPointRadiusMeters =
@@ -673,6 +697,10 @@ namespace MultiWheelC
{
Trajectory = trajectory,
StateProvider = _stateProvider,
MotionDirectionInBodyRadians =
MotionDirectionInBodyRadians,
ReturnWheelsForwardAfterCompletion =
ReturnWheelsForwardAfterCompletion,
CycleObserver = controller =>
RecordControlCycle(
recorder,
@@ -894,4 +922,30 @@ namespace MultiWheelC
MillimetersPerMeter));
}
}
/// <summary>
/// 将舵轮准备到车体左前45°，跟踪直线—左半圆—直线轨迹，并在停车后恢复车头方向。
/// </summary>
[MovementTest(name = "新版控制器：45°蟹行直线-左半圆-直线轨迹跟踪")]
public sealed class NewControllerCrab45StraightSemicircleStraightTest
: NewControllerStraightSemicircleStraightTest
{
/// <summary>
/// 使用车体左前45°作为组合轨迹的固定运动方向。
/// </summary>
protected override double MotionDirectionInBodyRadians =>
Math.PI / 4.0;
/// <summary>
/// 蟹行组合轨迹正常完成后主动将四个舵轮恢复到车头方向。
/// </summary>
protected override bool ReturnWheelsForwardAfterCompletion =>
true;
/// <summary>
/// 将45°蟹行组合实验与普通组合轨迹实验的CSV名称明确区分。
/// </summary>
protected override string ExperimentTrajectoryBaseName =>
"ProfiledCrab45StraightSmoothLeftTurnStraight";
}
}
MultiWheelC/Experiments/RotationTests.cs
+47 -1
View File
@@ -7,10 +7,12 @@ using System.Threading;
using ClumsyCore;
using ClumsyCore.Interfaces;
using ClumsyCore.Pilot;
using CommonUsage.Chassis;
using FundamentalLib;
using MDCSToolBox.Clumsy.Movements;
using MDCSToolBox.Clumsy.Pilot;
using MyParking.Shared;
using MultiWheelC.StateEstimation;
namespace MultiWheelC
{
@@ -66,6 +68,26 @@ namespace MultiWheelC
return;
}
var chassis =
PilotDefinition.Chassis as MultiWheelChassis;
if (chassis == null)
{
Console.WriteLine(
"当前底盘不是MultiWheelChassis，无法执行原地旋转测试。");
return;
}
var stateProvider =
ParkingVehicleStateProviderFactory.Create(
chassis);
if (!stateProvider.TryGetState(out _))
{
Console.WriteLine(
"无法读取原地旋转起点状态：" +
stateProvider.LastFailureReason);
return;
}
var rotationCenter =
new Vector2((float)location.x, (float)location.y);
var targetWorldAngle =
@@ -98,7 +120,9 @@ namespace MultiWheelC
referenceSpeed: 0f,
referenceAngularSpeed:
(float)AngleMath.DegreesToRadians(
config.InPlaceRotateMaxSpeed));
config.InPlaceRotateMaxSpeed),
diagnosticChassis: chassis,
diagnosticStateProvider: stateProvider);
_recorder.Start();
try
@@ -108,6 +132,8 @@ namespace MultiWheelC
{
// MultiWheelRotateInPlace接收世界坐标系绝对航向。
AngleTarget = targetWorldAngle,
Chassis = chassis,
StateProvider = stateProvider,
CommandAngularSpeedObserver =
commandAngularSpeed =>
_recorder?.UpdateCommand(
@@ -175,6 +201,26 @@ namespace MultiWheelC
return;
}
var chassis =
PilotDefinition.Chassis as MultiWheelChassis;
if (chassis == null)
{
Console.WriteLine(
"当前底盘不是MultiWheelChassis，无法执行原地旋转测试。");
return;
}
var stateProvider =
ParkingVehicleStateProviderFactory.Create(
chassis);
if (!stateProvider.TryGetState(out _))
{
Console.WriteLine(
"无法读取原地旋转起点状态：" +
stateProvider.LastFailureReason);
return;
}
if (Math.Abs(relativeAngleDegrees) < 1e-3f)
{
Console.WriteLine("旋转角度不能为0，测试已经取消。");
MultiWheelC/Experiments/TestTrajectoryFactory.cs
+16 -7
View File
@@ -125,7 +125,7 @@ namespace MultiWheelC
}
/// <summary>
/// 从当前位姿按速度符号生成“3m直线、沿行进方向平滑左弯180°、3m直线”的轨迹。
/// 从当前位姿沿指定车体运动方向生成“3m直线、平滑左弯180°、3m直线”的轨迹。
/// </summary>
public static Trajectory2D CreateStraightLeftSemicircleStraight(
Pose2D startPoseInWorld,
@@ -136,7 +136,8 @@ namespace MultiWheelC
double semicircleMaximumSpeedMetersPerSecond = 0.25,
double accelerationMetersPerSecondSquared = 0.20,
double decelerationMetersPerSecondSquared = 0.12,
double pointSpacingMeters = 0.02)
double pointSpacingMeters = 0.02,
double motionDirectionInBodyRadians = 0.0)
{
return CreateStraightSmoothLeftTurnStraight(
startPoseInWorld,
@@ -148,11 +149,12 @@ namespace MultiWheelC
semicircleMaximumSpeedMetersPerSecond,
accelerationMetersPerSecondSquared,
decelerationMetersPerSecondSquared,
pointSpacingMeters);
pointSpacingMeters,
motionDirectionInBodyRadians);
}
/// <summary>
/// 按共同速度符号生成“直线、沿行进方向平滑左弯、直线”轨迹，并使总转角严格等于指定角度。
/// 沿指定车体运动方向生成“直线、平滑左弯、直线”轨迹，并使总转角严格等于指定角度。
/// </summary>
public static Trajectory2D CreateStraightSmoothLeftTurnStraight(
Pose2D startPoseInWorld,
@@ -164,7 +166,8 @@ namespace MultiWheelC
double turnMaximumSpeedMetersPerSecond,
double accelerationMetersPerSecondSquared,
double decelerationMetersPerSecondSquared,
double pointSpacingMeters)
double pointSpacingMeters,
double motionDirectionInBodyRadians = 0.0)
{
NumericGuard.EnsureFinite(
startPoseInWorld,
@@ -195,6 +198,9 @@ namespace MultiWheelC
NumericGuard.EnsureFinitePositive(
pointSpacingMeters,
nameof(pointSpacingMeters));
NumericGuard.EnsureFinite(
motionDirectionInBodyRadians,
nameof(motionDirectionInBodyRadians));
if (turnAngleRadians > 2.0 * Math.PI)
{
@@ -276,10 +282,13 @@ namespace MultiWheelC
var points = new List<TrajectoryPoint>(
sampleArcLengths.Count);
var worldMotionStartYawRadians =
startPoseInWorld.YawRadians +
motionDirectionInBodyRadians;
var startCos = Math.Cos(
startPoseInWorld.YawRadians);
worldMotionStartYawRadians);
var startSin = Math.Sin(
startPoseInWorld.YawRadians);
worldMotionStartYawRadians);
var localX = 0.0;
var localY = 0.0;
var localYawRadians = 0.0;
MultiWheelC/Experiments/TrackingExperimentRecorder.cs
+268 -1
View File
@@ -24,6 +24,27 @@ namespace MultiWheelC
public double DetourX;
public double DetourY;
public double DetourTheta;
public long DetourTickRaw;
public double DetourLStep;
// Detour状态估计内部诊断；偏移量仅在跳变候选有效时有意义。
public bool HasDetourStateDiagnostics;
public bool DetourJumpCandidateActive;
public int DetourJumpCandidateConsistentFrameCount;
public double DetourEstimatedShiftDistanceMeters;
public double DetourEstimatedShiftHeadingRadians;
public int DetourAutomaticFrameShiftCount;
public string DetourStateStatusReason;
public double DetourDataAgeMilliseconds;
public double DetourSourceFrameIntervalMilliseconds;
public double DetourMotionPredictionTimestampSeconds;
public bool HasDetourInnovationDiagnostics;
public double DetourPositionInnovationMeters;
public double DetourAllowedPositionInnovationMeters;
public double DetourHeadingInnovationRadians;
public double DetourAllowedHeadingInnovationRadians;
public string DetourLastJumpTriggerReason;
public string DetourStateStatus;
// 车体速度单位为m/s，角速度统一使用rad/s。
public float CommandSpeed;
@@ -62,6 +83,10 @@ namespace MultiWheelC
public double WheelFeedbackRawBodyVyMetersPerSecond;
public double WheelFeedbackFilteredBodyVyMetersPerSecond;
public bool WheelFeedbackVelocityEstimateValid;
public bool HasWheelFeedbackAngularVelocityDiagnostics;
public double WheelFeedbackRawBodyOmegaRadiansPerSecond;
public double WheelFeedbackFilteredBodyOmegaRadiansPerSecond;
public double WheelFeedbackSampleTimestampSeconds;
// 四舵轮机械角使用deg，前后虚拟GCP命令角使用rad。
public bool HasSteeringDiagnostics;
@@ -130,6 +155,8 @@ namespace MultiWheelC
private readonly float _referenceDecelerationMetersPerSecondSquared;
private readonly int _sampleIntervalMs;
private readonly MultiWheelChassis _diagnosticChassis;
private readonly WheelFeedbackVehicleStateProvider
_diagnosticStateProvider;
private readonly List<TrackingSample> _samples =
new List<TrackingSample>();
@@ -203,7 +230,9 @@ namespace MultiWheelC
float referenceMotionFrameYawDegrees = 0f,
float referenceAccelerationMetersPerSecondSquared = 0f,
float referenceDecelerationMetersPerSecondSquared = 0f,
MultiWheelChassis diagnosticChassis = null)
MultiWheelChassis diagnosticChassis = null,
WheelFeedbackVehicleStateProvider
diagnosticStateProvider = null)
{
if (string.IsNullOrWhiteSpace(controllerName))
throw new ArgumentException(
@@ -235,6 +264,7 @@ namespace MultiWheelC
referenceDecelerationMetersPerSecondSquared;
_sampleIntervalMs = sampleIntervalMs;
_diagnosticChassis = diagnosticChassis;
_diagnosticStateProvider = diagnosticStateProvider;
}
// 保存成功后的CSV绝对路径；尚未保存时为空。
@@ -534,6 +564,47 @@ namespace MultiWheelC
var location =
DetourInterface.getCartLocation();
var hasDetourStateDiagnostics = false;
var detourJumpCandidateActive = false;
var detourJumpCandidateConsistentFrameCount = 0;
var detourEstimatedShiftDistanceMeters = 0.0;
var detourEstimatedShiftHeadingRadians = 0.0;
var detourAutomaticFrameShiftCount = 0;
var detourStateStatusReason = string.Empty;
var detourDataAgeMilliseconds = 0.0;
var detourSourceFrameIntervalSeconds = 0.0;
var detourMotionPredictionTimestampSeconds = 0.0;
var hasDetourInnovationDiagnostics = false;
var detourPositionInnovationMeters = 0.0;
var detourAllowedPositionInnovationMeters = 0.0;
var detourHeadingInnovationRadians = 0.0;
var detourAllowedHeadingInnovationRadians = 0.0;
var detourLastJumpTriggerReason = string.Empty;
var detourStateStatus = string.Empty;
if (_diagnosticStateProvider != null)
{
hasDetourStateDiagnostics =
_diagnosticStateProvider
.TryGetLatestDetourDiagnostics(
out detourJumpCandidateActive,
out detourJumpCandidateConsistentFrameCount,
out detourEstimatedShiftDistanceMeters,
out detourEstimatedShiftHeadingRadians,
out detourAutomaticFrameShiftCount,
out detourSourceFrameIntervalSeconds,
out detourMotionPredictionTimestampSeconds,
out hasDetourInnovationDiagnostics,
out detourPositionInnovationMeters,
out detourAllowedPositionInnovationMeters,
out detourHeadingInnovationRadians,
out detourAllowedHeadingInnovationRadians,
out detourLastJumpTriggerReason,
out detourStateStatus,
out detourStateStatusReason,
out detourDataAgeMilliseconds);
}
float commandSpeed;
float commandVx;
float commandVy;
@@ -556,6 +627,10 @@ namespace MultiWheelC
double wheelFeedbackRawBodyVyMetersPerSecond;
double wheelFeedbackFilteredBodyVyMetersPerSecond;
bool wheelFeedbackVelocityEstimateValid;
var hasWheelFeedbackAngularVelocityDiagnostics = false;
var wheelFeedbackRawBodyOmegaRadiansPerSecond = 0.0;
var wheelFeedbackFilteredBodyOmegaRadiansPerSecond = 0.0;
var wheelFeedbackSampleTimestampSeconds = 0.0;
bool hasGcpCommand;
double requestedFrontGcpAngleRadians;
double requestedRearGcpAngleRadians;
@@ -655,6 +730,46 @@ namespace MultiWheelC
_commandRearGcpAngleRadians;
}
// 直接从状态源读取最新完整轮组诊断，使原地自转等没有
// 控制周期回调的实验也能记录原始/滤波Vw及其采样时间。
if (_diagnosticStateProvider != null &&
_diagnosticStateProvider
.TryGetLatestVelocityDiagnostics(
out var directDetourBodyVx,
out var directDetourVelocityValid,
out var directRawWheelBodyVx,
out var directFilteredWheelBodyVx,
out var directRawWheelBodyVy,
out var directFilteredWheelBodyVy,
out var directRawWheelBodyOmega,
out var directFilteredWheelBodyOmega,
out var directWheelSampleTimestampSeconds,
out var directWheelVelocityValid))
{
hasVelocityDiagnostics = true;
detourEstimatedBodyVxMetersPerSecond =
directDetourBodyVx;
detourVelocityEstimateValid =
directDetourVelocityValid;
wheelFeedbackRawBodyVxMetersPerSecond =
directRawWheelBodyVx;
wheelFeedbackFilteredBodyVxMetersPerSecond =
directFilteredWheelBodyVx;
wheelFeedbackRawBodyVyMetersPerSecond =
directRawWheelBodyVy;
wheelFeedbackFilteredBodyVyMetersPerSecond =
directFilteredWheelBodyVy;
wheelFeedbackRawBodyOmegaRadiansPerSecond =
directRawWheelBodyOmega;
wheelFeedbackFilteredBodyOmegaRadiansPerSecond =
directFilteredWheelBodyOmega;
wheelFeedbackSampleTimestampSeconds =
directWheelSampleTimestampSeconds;
wheelFeedbackVelocityEstimateValid =
directWheelVelocityValid;
hasWheelFeedbackAngularVelocityDiagnostics = true;
}
var sample = new TrackingSample
{
ElapsedSeconds =
@@ -662,6 +777,45 @@ namespace MultiWheelC
DetourX = location.x,
DetourY = location.y,
DetourTheta = location.th,
DetourTickRaw = Convert.ToInt64(
location.tick,
CultureInfo.InvariantCulture),
DetourLStep = Convert.ToDouble(
location.l_step,
CultureInfo.InvariantCulture),
HasDetourStateDiagnostics =
hasDetourStateDiagnostics,
DetourJumpCandidateActive =
detourJumpCandidateActive,
DetourJumpCandidateConsistentFrameCount =
detourJumpCandidateConsistentFrameCount,
DetourEstimatedShiftDistanceMeters =
detourEstimatedShiftDistanceMeters,
DetourEstimatedShiftHeadingRadians =
detourEstimatedShiftHeadingRadians,
DetourAutomaticFrameShiftCount =
detourAutomaticFrameShiftCount,
DetourStateStatusReason =
detourStateStatusReason,
DetourDataAgeMilliseconds =
detourDataAgeMilliseconds,
DetourSourceFrameIntervalMilliseconds =
detourSourceFrameIntervalSeconds * 1000.0,
DetourMotionPredictionTimestampSeconds =
detourMotionPredictionTimestampSeconds,
HasDetourInnovationDiagnostics =
hasDetourInnovationDiagnostics,
DetourPositionInnovationMeters =
detourPositionInnovationMeters,
DetourAllowedPositionInnovationMeters =
detourAllowedPositionInnovationMeters,
DetourHeadingInnovationRadians =
detourHeadingInnovationRadians,
DetourAllowedHeadingInnovationRadians =
detourAllowedHeadingInnovationRadians,
DetourLastJumpTriggerReason =
detourLastJumpTriggerReason,
DetourStateStatus = detourStateStatus,
CommandSpeed = commandSpeed,
CommandVx = commandVx,
CommandVy = commandVy,
@@ -703,6 +857,14 @@ namespace MultiWheelC
wheelFeedbackFilteredBodyVyMetersPerSecond,
WheelFeedbackVelocityEstimateValid =
wheelFeedbackVelocityEstimateValid,
HasWheelFeedbackAngularVelocityDiagnostics =
hasWheelFeedbackAngularVelocityDiagnostics,
WheelFeedbackRawBodyOmegaRadiansPerSecond =
wheelFeedbackRawBodyOmegaRadiansPerSecond,
WheelFeedbackFilteredBodyOmegaRadiansPerSecond =
wheelFeedbackFilteredBodyOmegaRadiansPerSecond,
WheelFeedbackSampleTimestampSeconds =
wheelFeedbackSampleTimestampSeconds,
HasGcpCommand = hasGcpCommand,
RequestedFrontGcpAngleRadians =
requestedFrontGcpAngleRadians,
@@ -885,6 +1047,25 @@ namespace MultiWheelC
"DetourX," +
"DetourY," +
"DetourTheta," +
"DetourTickRaw," +
"DetourLStep," +
"HasDetourStateDiagnostics," +
"DetourJumpCandidateActive," +
"DetourJumpCandidateConsistentFrameCount," +
"DetourEstimatedShiftDistanceMeters," +
"DetourEstimatedShiftHeadingRadians," +
"DetourAutomaticFrameShiftCount," +
"DetourStateStatusReason," +
"DetourDataAgeMilliseconds," +
"DetourSourceFrameIntervalMilliseconds," +
"DetourMotionPredictionTimestampSeconds," +
"HasDetourInnovationDiagnostics," +
"DetourPositionInnovationMeters," +
"DetourAllowedPositionInnovationMeters," +
"DetourHeadingInnovationRadians," +
"DetourAllowedHeadingInnovationRadians," +
"DetourLastJumpTriggerReason," +
"DetourStateStatus," +
"CommandSpeed," +
// 保留旧列（deg/s）供历史Python脚本兼容。
"CommandAngularSpeed," +
@@ -928,6 +1109,10 @@ namespace MultiWheelC
"WheelFeedbackRawBodyVyMetersPerSecond," +
"WheelFeedbackFilteredBodyVyMetersPerSecond," +
"WheelFeedbackVelocityEstimateValid," +
"HasWheelFeedbackAngularVelocityDiagnostics," +
"WheelFeedbackRawBodyOmegaRadiansPerSecond," +
"WheelFeedbackFilteredBodyOmegaRadiansPerSecond," +
"WheelFeedbackSampleTimestampSeconds," +
"HasSteeringDiagnostics," +
"TargetSteerLeftFrontDegrees," +
"TargetSteerLeftRearDegrees," +
@@ -959,6 +1144,73 @@ namespace MultiWheelC
Format(sample.DetourX),
Format(sample.DetourY),
Format(sample.DetourTheta),
sample.DetourTickRaw.ToString(
CultureInfo.InvariantCulture),
Format(sample.DetourLStep),
sample.HasDetourStateDiagnostics
? "1"
: "0",
FormatOptionalBoolean(
sample.HasDetourStateDiagnostics,
sample.DetourJumpCandidateActive),
sample.HasDetourStateDiagnostics
? sample
.DetourJumpCandidateConsistentFrameCount
.ToString(
CultureInfo.InvariantCulture)
: string.Empty,
FormatOptional(
sample.HasDetourStateDiagnostics &&
sample.DetourJumpCandidateActive,
sample.DetourEstimatedShiftDistanceMeters),
FormatOptional(
sample.HasDetourStateDiagnostics &&
sample.DetourJumpCandidateActive,
sample.DetourEstimatedShiftHeadingRadians),
sample.HasDetourStateDiagnostics
? sample.DetourAutomaticFrameShiftCount
.ToString(
CultureInfo.InvariantCulture)
: string.Empty,
sample.HasDetourStateDiagnostics
? EscapeCsv(
sample.DetourStateStatusReason)
: string.Empty,
FormatOptional(
sample.HasDetourStateDiagnostics,
sample.DetourDataAgeMilliseconds),
FormatOptional(
sample.HasDetourStateDiagnostics,
sample.DetourSourceFrameIntervalMilliseconds),
FormatOptional(
sample.HasDetourStateDiagnostics,
sample.DetourMotionPredictionTimestampSeconds),
FormatOptionalBoolean(
sample.HasDetourStateDiagnostics,
sample.HasDetourInnovationDiagnostics),
FormatOptional(
sample.HasDetourStateDiagnostics &&
sample.HasDetourInnovationDiagnostics,
sample.DetourPositionInnovationMeters),
FormatOptional(
sample.HasDetourStateDiagnostics &&
sample.HasDetourInnovationDiagnostics,
sample.DetourAllowedPositionInnovationMeters),
FormatOptional(
sample.HasDetourStateDiagnostics &&
sample.HasDetourInnovationDiagnostics,
sample.DetourHeadingInnovationRadians),
FormatOptional(
sample.HasDetourStateDiagnostics &&
sample.HasDetourInnovationDiagnostics,
sample.DetourAllowedHeadingInnovationRadians),
sample.HasDetourStateDiagnostics
? EscapeCsv(
sample.DetourLastJumpTriggerReason)
: string.Empty,
sample.HasDetourStateDiagnostics
? EscapeCsv(sample.DetourStateStatus)
: string.Empty,
Format(sample.CommandSpeed),
Format(
AngleMath.RadiansToDegrees(
@@ -1067,6 +1319,21 @@ namespace MultiWheelC
? "1"
: "0"
: string.Empty,
FormatOptionalBoolean(
sample.HasVelocityDiagnostics,
sample.HasWheelFeedbackAngularVelocityDiagnostics),
FormatOptional(
sample.HasVelocityDiagnostics &&
sample.HasWheelFeedbackAngularVelocityDiagnostics,
sample.WheelFeedbackRawBodyOmegaRadiansPerSecond),
FormatOptional(
sample.HasVelocityDiagnostics &&
sample.HasWheelFeedbackAngularVelocityDiagnostics,
sample.WheelFeedbackFilteredBodyOmegaRadiansPerSecond),
FormatOptional(
sample.HasVelocityDiagnostics &&
sample.HasWheelFeedbackAngularVelocityDiagnostics,
sample.WheelFeedbackSampleTimestampSeconds),
sample.HasSteeringDiagnostics
? "1"
: "0",