697 lines
26 KiB
C#
697 lines
26 KiB
C#
|
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("\"", "\"\"") +
|
|||
|
|
"\"";
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
}
|