完善Detour状态估计与轨迹跟踪验证
This commit is contained in:
@@ -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("\"", "\"\"") +
|
||||
"\"";
|
||||
}
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user