1157 lines
46 KiB
C#
1157 lines
46 KiB
C#
using ClumsyCore.Interfaces;
|
||
using System;
|
||
using System.Collections.Generic;
|
||
using System.Diagnostics;
|
||
using System.Globalization;
|
||
using System.IO;
|
||
using System.Numerics;
|
||
using System.Text;
|
||
using System.Threading;
|
||
using CommonUsage.Chassis;
|
||
using MyParking.Shared;
|
||
using MultiWheelC.Control.Execution;
|
||
using MultiWheelC.StateEstimation;
|
||
|
||
namespace MultiWheelC
|
||
{
|
||
// C层实验数据：保存一个采样时刻的定位与控制命令。
|
||
public sealed class TrackingSample
|
||
{
|
||
public double ElapsedSeconds;
|
||
|
||
// Detour位置单位为mm，航向单位为deg。
|
||
public double DetourX;
|
||
public double DetourY;
|
||
public double DetourTheta;
|
||
|
||
// 车体速度单位为m/s，角速度统一使用rad/s。
|
||
public float CommandSpeed;
|
||
public float CommandVx;
|
||
public float CommandVy;
|
||
public float CommandAngularSpeed;
|
||
|
||
// 新版状态估计统一使用SI单位；无有效控制器状态时HasProcessedState为false。
|
||
public bool HasProcessedState;
|
||
public double StateTimestampSeconds;
|
||
public double StateXmeters;
|
||
public double StateYMeters;
|
||
public double StateYawRadians;
|
||
public double StateWorldVxMetersPerSecond;
|
||
public double StateWorldVyMetersPerSecond;
|
||
public double StateBodyVxMetersPerSecond;
|
||
public double StateBodyVyMetersPerSecond;
|
||
public double StateAngularSpeedRadiansPerSecond;
|
||
public bool StateVelocityEstimateValid;
|
||
public bool HasControlReference;
|
||
public double ControlReferenceArcLengthMeters;
|
||
public double ControlReferenceSpeedMetersPerSecond;
|
||
public double ControlLateralErrorMeters;
|
||
public double ControlHeadingErrorRadians;
|
||
public double ControlDistanceToTrajectoryMeters;
|
||
public double ControlRemainingDistanceMeters;
|
||
public double CurvaturePreviewDistanceMeters;
|
||
public double FeedforwardCurvaturePerMeter;
|
||
|
||
// 并列保存Detour速度与轮速解算速度，避免StateBodyVx的数据来源产生歧义。
|
||
public bool HasVelocityDiagnostics;
|
||
public double DetourEstimatedBodyVxMetersPerSecond;
|
||
public bool DetourVelocityEstimateValid;
|
||
public double WheelFeedbackRawBodyVxMetersPerSecond;
|
||
public double WheelFeedbackFilteredBodyVxMetersPerSecond;
|
||
public bool WheelFeedbackVelocityEstimateValid;
|
||
|
||
// 四舵轮机械角使用deg，前后虚拟GCP命令角使用rad。
|
||
public bool HasSteeringDiagnostics;
|
||
public double TargetSteerLeftFrontDegrees;
|
||
public double TargetSteerLeftRearDegrees;
|
||
public double TargetSteerRightFrontDegrees;
|
||
public double TargetSteerRightRearDegrees;
|
||
public double ActualSteerLeftFrontDegrees;
|
||
public double ActualSteerLeftRearDegrees;
|
||
public double ActualSteerRightFrontDegrees;
|
||
public double ActualSteerRightRearDegrees;
|
||
public bool HasGcpCommand;
|
||
public double CommandFrontGcpAngleRadians;
|
||
public double CommandRearGcpAngleRadians;
|
||
}
|
||
|
||
// C层实验工具：统一采集并保存轨迹跟踪实验数据。
|
||
public sealed class TrackingExperimentRecorder
|
||
{
|
||
/// <summary>
|
||
/// 保存一次控制周期计时及其所属组合运动段索引。
|
||
/// </summary>
|
||
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;
|
||
private readonly Vector2 _referenceStart;
|
||
private readonly Vector2 _referenceEnd;
|
||
private readonly float _referenceSpeed;
|
||
private readonly float _referenceAngularSpeed;
|
||
private readonly float _referenceMotionFrameYawDegrees;
|
||
private readonly float _referenceAccelerationMetersPerSecondSquared;
|
||
private readonly float _referenceDecelerationMetersPerSecondSquared;
|
||
private readonly int _sampleIntervalMs;
|
||
private readonly MultiWheelChassis _diagnosticChassis;
|
||
|
||
private readonly List<TrackingSample> _samples =
|
||
new List<TrackingSample>();
|
||
|
||
private readonly List<ControlCycleTimingRecord>
|
||
_controlCycleTimings =
|
||
new List<ControlCycleTimingRecord>();
|
||
|
||
private readonly object _sampleSyncRoot =
|
||
new object();
|
||
|
||
private readonly object _controlCycleTimingSyncRoot =
|
||
new object();
|
||
|
||
private readonly object _commandSyncRoot =
|
||
new object();
|
||
|
||
private readonly object _stateSyncRoot =
|
||
new object();
|
||
|
||
private readonly Stopwatch _stopwatch =
|
||
new Stopwatch();
|
||
|
||
private Thread _worker;
|
||
private volatile bool _running;
|
||
private int _started;
|
||
private int _saved;
|
||
|
||
private bool _hasExternalCommand;
|
||
private float _externalCommandSpeed;
|
||
private float _externalCommandVx;
|
||
private float _externalCommandVy;
|
||
private float _externalCommandAngularSpeed;
|
||
private VehicleState? _latestProcessedState;
|
||
private bool _hasControlReference;
|
||
private double _controlReferenceArcLengthMeters;
|
||
private double _controlReferenceSpeedMetersPerSecond;
|
||
private double _controlLateralErrorMeters;
|
||
private double _controlHeadingErrorRadians;
|
||
private double _controlDistanceToTrajectoryMeters;
|
||
private double _controlRemainingDistanceMeters;
|
||
private double _curvaturePreviewDistanceMeters;
|
||
private double _feedforwardCurvaturePerMeter;
|
||
private bool _hasVelocityDiagnostics;
|
||
private double _detourEstimatedBodyVxMetersPerSecond;
|
||
private bool _detourVelocityEstimateValid;
|
||
private double _wheelFeedbackRawBodyVxMetersPerSecond;
|
||
private double _wheelFeedbackFilteredBodyVxMetersPerSecond;
|
||
private bool _wheelFeedbackVelocityEstimateValid;
|
||
private bool _hasGcpCommand;
|
||
private double _commandFrontGcpAngleRadians;
|
||
private double _commandRearGcpAngleRadians;
|
||
|
||
public TrackingExperimentRecorder(
|
||
string controllerName,
|
||
string trajectoryName,
|
||
int trialNumber,
|
||
Vector2 referenceStart,
|
||
Vector2 referenceEnd,
|
||
float referenceSpeed,
|
||
float referenceAngularSpeed = 0f,
|
||
int sampleIntervalMs = 50,
|
||
float referenceMotionFrameYawDegrees = 0f,
|
||
float referenceAccelerationMetersPerSecondSquared = 0f,
|
||
float referenceDecelerationMetersPerSecondSquared = 0f,
|
||
MultiWheelChassis diagnosticChassis = null)
|
||
{
|
||
if (string.IsNullOrWhiteSpace(controllerName))
|
||
throw new ArgumentException(
|
||
"控制器名称不能为空。",
|
||
nameof(controllerName));
|
||
|
||
if (string.IsNullOrWhiteSpace(trajectoryName))
|
||
throw new ArgumentException(
|
||
"轨迹名称不能为空。",
|
||
nameof(trajectoryName));
|
||
|
||
if (sampleIntervalMs <= 0)
|
||
throw new ArgumentOutOfRangeException(
|
||
nameof(sampleIntervalMs),
|
||
"采样周期必须大于零。");
|
||
|
||
_controllerName = controllerName;
|
||
_trajectoryName = trajectoryName;
|
||
_trialNumber = trialNumber;
|
||
_referenceStart = referenceStart;
|
||
_referenceEnd = referenceEnd;
|
||
_referenceSpeed = referenceSpeed;
|
||
_referenceAngularSpeed = referenceAngularSpeed;
|
||
_referenceMotionFrameYawDegrees =
|
||
referenceMotionFrameYawDegrees;
|
||
_referenceAccelerationMetersPerSecondSquared =
|
||
referenceAccelerationMetersPerSecondSquared;
|
||
_referenceDecelerationMetersPerSecondSquared =
|
||
referenceDecelerationMetersPerSecondSquared;
|
||
_sampleIntervalMs = sampleIntervalMs;
|
||
_diagnosticChassis = diagnosticChassis;
|
||
}
|
||
|
||
// 保存成功后的CSV绝对路径；尚未保存时为空。
|
||
public string SavedFilePath { get; private set; }
|
||
|
||
/// <summary>
|
||
/// 获取逐控制周期计时CSV的绝对路径；本次实验没有计时数据时为空。
|
||
/// </summary>
|
||
public string SavedTimingFilePath { get; private set; }
|
||
|
||
/// <summary>
|
||
/// 获取Clumsy当前运行目录下统一保存轨迹实验CSV的文件夹。
|
||
/// </summary>
|
||
public static string DefaultOutputDirectory =>
|
||
Path.Combine(
|
||
AppContext.BaseDirectory,
|
||
"TrackingExperiments");
|
||
|
||
// 启动后台采样线程。
|
||
public void Start()
|
||
{
|
||
if (Interlocked.Exchange(ref _started, 1) != 0)
|
||
return;
|
||
|
||
_stopwatch.Restart();
|
||
_running = true;
|
||
|
||
// 立即保存起点静止状态，避免第一帧被后台线程延迟。
|
||
CaptureSample();
|
||
|
||
_worker = new Thread(SamplingLoop)
|
||
{
|
||
IsBackground = true,
|
||
Name = "TrackingExperimentRecorder"
|
||
};
|
||
_worker.Start();
|
||
}
|
||
|
||
// 供Stanley/LQR控制器主动写入本周期最终速度命令。
|
||
// 调用后优先记录该命令，不再使用底盘反解值。
|
||
public void UpdateCommand(
|
||
float commandSpeed,
|
||
float commandAngularSpeed)
|
||
{
|
||
lock (_commandSyncRoot)
|
||
{
|
||
_externalCommandSpeed = commandSpeed;
|
||
_externalCommandVx = commandSpeed;
|
||
_externalCommandVy = 0f;
|
||
_externalCommandAngularSpeed =
|
||
commandAngularSpeed;
|
||
_hasExternalCommand = true;
|
||
}
|
||
}
|
||
|
||
// 供全向、蟹行和曲线控制器写入完整车体速度命令。
|
||
public void UpdateBodyCommand(
|
||
float commandVx,
|
||
float commandVy,
|
||
float commandAngularSpeed)
|
||
{
|
||
lock (_commandSyncRoot)
|
||
{
|
||
_externalCommandVx = commandVx;
|
||
_externalCommandVy = commandVy;
|
||
_externalCommandSpeed =
|
||
(float)Math.Sqrt(
|
||
commandVx * commandVx +
|
||
commandVy * commandVy);
|
||
_externalCommandAngularSpeed =
|
||
commandAngularSpeed;
|
||
_hasExternalCommand = true;
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 将一个真实控制周期的分阶段耗时追加到内存，实验结束后统一保存。
|
||
/// </summary>
|
||
public void RecordControlCycleTiming(
|
||
ParkingControlCycleTiming timing,
|
||
int motionSegmentIndex = -1)
|
||
{
|
||
var record = new ControlCycleTimingRecord(
|
||
_stopwatch.Elapsed.TotalSeconds,
|
||
motionSegmentIndex,
|
||
timing);
|
||
|
||
lock (_controlCycleTimingSyncRoot)
|
||
{
|
||
_controlCycleTimings.Add(record);
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 保存新版控制器本周期实际使用的校验后车辆状态，供后台采样线程写入CSV。
|
||
/// </summary>
|
||
public void UpdateProcessedState(VehicleState state)
|
||
{
|
||
lock (_stateSyncRoot)
|
||
{
|
||
_latestProcessedState = state;
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 保存同一控制周期的Detour纵向速度以及轮速解算的原始和滤波纵向速度。
|
||
/// </summary>
|
||
public void UpdateVelocityDiagnostics(
|
||
double detourEstimatedBodyVxMetersPerSecond,
|
||
bool detourVelocityEstimateValid,
|
||
double wheelFeedbackRawBodyVxMetersPerSecond,
|
||
double wheelFeedbackFilteredBodyVxMetersPerSecond,
|
||
bool wheelFeedbackVelocityEstimateValid)
|
||
{
|
||
lock (_stateSyncRoot)
|
||
{
|
||
_detourEstimatedBodyVxMetersPerSecond =
|
||
detourEstimatedBodyVxMetersPerSecond;
|
||
_detourVelocityEstimateValid =
|
||
detourVelocityEstimateValid;
|
||
_wheelFeedbackRawBodyVxMetersPerSecond =
|
||
wheelFeedbackRawBodyVxMetersPerSecond;
|
||
_wheelFeedbackFilteredBodyVxMetersPerSecond =
|
||
wheelFeedbackFilteredBodyVxMetersPerSecond;
|
||
_wheelFeedbackVelocityEstimateValid =
|
||
wheelFeedbackVelocityEstimateValid;
|
||
_hasVelocityDiagnostics = true;
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 保存经过GCP角速度限制后实际交给底盘的前后虚拟控制点转角。
|
||
/// </summary>
|
||
public void UpdateGcpCommand(
|
||
double frontGcpAngleRadians,
|
||
double rearGcpAngleRadians)
|
||
{
|
||
lock (_stateSyncRoot)
|
||
{
|
||
_commandFrontGcpAngleRadians =
|
||
frontGcpAngleRadians;
|
||
_commandRearGcpAngleRadians =
|
||
rearGcpAngleRadians;
|
||
_hasGcpCommand = true;
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 清除上一轨迹段的GCP命令，避免停车或原地自转阶段沿用旧角度。
|
||
/// </summary>
|
||
public void ClearGcpCommand()
|
||
{
|
||
lock (_stateSyncRoot)
|
||
{
|
||
_hasGcpCommand = false;
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 保存新版控制器本周期实际使用的轨迹投影、误差和参考速度。
|
||
/// </summary>
|
||
public void UpdateControlReference(
|
||
double arcLengthMeters,
|
||
double referenceSpeedMetersPerSecond,
|
||
double lateralErrorMeters,
|
||
double headingErrorRadians,
|
||
double distanceToTrajectoryMeters,
|
||
double remainingDistanceMeters,
|
||
double curvaturePreviewDistanceMeters,
|
||
double feedforwardCurvaturePerMeter)
|
||
{
|
||
lock (_stateSyncRoot)
|
||
{
|
||
_controlReferenceArcLengthMeters =
|
||
arcLengthMeters;
|
||
_controlReferenceSpeedMetersPerSecond =
|
||
referenceSpeedMetersPerSecond;
|
||
_controlLateralErrorMeters =
|
||
lateralErrorMeters;
|
||
_controlHeadingErrorRadians =
|
||
headingErrorRadians;
|
||
_controlDistanceToTrajectoryMeters =
|
||
distanceToTrajectoryMeters;
|
||
_controlRemainingDistanceMeters =
|
||
remainingDistanceMeters;
|
||
_curvaturePreviewDistanceMeters =
|
||
curvaturePreviewDistanceMeters;
|
||
_feedforwardCurvaturePerMeter =
|
||
feedforwardCurvaturePerMeter;
|
||
_hasControlReference = true;
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 清除上一轨迹段参考量，避免停车或原地自转期间沿用已经结束的投影结果。
|
||
/// </summary>
|
||
public void ClearControlReference()
|
||
{
|
||
lock (_stateSyncRoot)
|
||
{
|
||
_hasControlReference = false;
|
||
}
|
||
}
|
||
|
||
// 停止采样并将本次实验保存为CSV；重复调用只保存一次。
|
||
public void StopAndSave()
|
||
{
|
||
if (Volatile.Read(ref _started) == 0)
|
||
return;
|
||
|
||
if (Interlocked.Exchange(ref _saved, 1) != 0)
|
||
return;
|
||
|
||
try
|
||
{
|
||
_running = false;
|
||
|
||
if (_worker != null &&
|
||
_worker != Thread.CurrentThread)
|
||
{
|
||
_worker.Join(
|
||
Math.Max(1000, _sampleIntervalMs * 4));
|
||
}
|
||
|
||
// 保存停止时刻的最后一帧。
|
||
CaptureSample();
|
||
_stopwatch.Stop();
|
||
SaveCsv();
|
||
SaveControlCycleTimingCsv();
|
||
|
||
Console.WriteLine(
|
||
$"轨迹实验数据已保存：{SavedFilePath}");
|
||
if (!string.IsNullOrWhiteSpace(
|
||
SavedTimingFilePath))
|
||
{
|
||
Console.WriteLine(
|
||
"控制周期计时数据已保存：" +
|
||
SavedTimingFilePath);
|
||
}
|
||
}
|
||
catch
|
||
{
|
||
// 保存失败后允许调用者再次尝试。
|
||
Interlocked.Exchange(ref _saved, 0);
|
||
throw;
|
||
}
|
||
}
|
||
|
||
// 按固定周期采集Detour位姿和控制命令。
|
||
private void SamplingLoop()
|
||
{
|
||
while (_running)
|
||
{
|
||
Thread.Sleep(_sampleIntervalMs);
|
||
|
||
if (!_running)
|
||
break;
|
||
|
||
CaptureSample();
|
||
}
|
||
}
|
||
|
||
// 采集一帧Detour位姿和控制命令。
|
||
private void CaptureSample()
|
||
{
|
||
try
|
||
{
|
||
var location =
|
||
DetourInterface.getCartLocation();
|
||
|
||
float commandSpeed;
|
||
float commandVx;
|
||
float commandVy;
|
||
float commandAngularSpeed;
|
||
VehicleState? processedState;
|
||
bool hasControlReference;
|
||
double controlReferenceArcLengthMeters;
|
||
double controlReferenceSpeedMetersPerSecond;
|
||
double controlLateralErrorMeters;
|
||
double controlHeadingErrorRadians;
|
||
double controlDistanceToTrajectoryMeters;
|
||
double controlRemainingDistanceMeters;
|
||
double curvaturePreviewDistanceMeters;
|
||
double feedforwardCurvaturePerMeter;
|
||
bool hasVelocityDiagnostics;
|
||
double detourEstimatedBodyVxMetersPerSecond;
|
||
bool detourVelocityEstimateValid;
|
||
double wheelFeedbackRawBodyVxMetersPerSecond;
|
||
double wheelFeedbackFilteredBodyVxMetersPerSecond;
|
||
bool wheelFeedbackVelocityEstimateValid;
|
||
bool hasGcpCommand;
|
||
double commandFrontGcpAngleRadians;
|
||
double commandRearGcpAngleRadians;
|
||
|
||
lock (_commandSyncRoot)
|
||
{
|
||
if (_hasExternalCommand)
|
||
{
|
||
commandSpeed =
|
||
_externalCommandSpeed;
|
||
commandVx =
|
||
_externalCommandVx;
|
||
commandVy =
|
||
_externalCommandVy;
|
||
commandAngularSpeed =
|
||
_externalCommandAngularSpeed;
|
||
}
|
||
else
|
||
{
|
||
var command =
|
||
PilotDefinition.Chassis
|
||
.GetCarSpeed(false);
|
||
|
||
commandVx = command.Vx;
|
||
commandVy = command.Vy;
|
||
// CommonUsage.GetCarSpeed().Vw的单位为deg/s，
|
||
// 记录器内部统一转换为rad/s。
|
||
commandAngularSpeed =
|
||
(float)AngleMath.DegreesToRadians(
|
||
command.Vw);
|
||
commandSpeed = (float)Math.Sqrt(
|
||
commandVx * commandVx +
|
||
commandVy * commandVy);
|
||
}
|
||
}
|
||
|
||
lock (_stateSyncRoot)
|
||
{
|
||
processedState =
|
||
_latestProcessedState;
|
||
hasControlReference =
|
||
_hasControlReference;
|
||
controlReferenceArcLengthMeters =
|
||
_controlReferenceArcLengthMeters;
|
||
controlReferenceSpeedMetersPerSecond =
|
||
_controlReferenceSpeedMetersPerSecond;
|
||
controlLateralErrorMeters =
|
||
_controlLateralErrorMeters;
|
||
controlHeadingErrorRadians =
|
||
_controlHeadingErrorRadians;
|
||
controlDistanceToTrajectoryMeters =
|
||
_controlDistanceToTrajectoryMeters;
|
||
controlRemainingDistanceMeters =
|
||
_controlRemainingDistanceMeters;
|
||
curvaturePreviewDistanceMeters =
|
||
_curvaturePreviewDistanceMeters;
|
||
feedforwardCurvaturePerMeter =
|
||
_feedforwardCurvaturePerMeter;
|
||
hasVelocityDiagnostics =
|
||
_hasVelocityDiagnostics;
|
||
detourEstimatedBodyVxMetersPerSecond =
|
||
_detourEstimatedBodyVxMetersPerSecond;
|
||
detourVelocityEstimateValid =
|
||
_detourVelocityEstimateValid;
|
||
wheelFeedbackRawBodyVxMetersPerSecond =
|
||
_wheelFeedbackRawBodyVxMetersPerSecond;
|
||
wheelFeedbackFilteredBodyVxMetersPerSecond =
|
||
_wheelFeedbackFilteredBodyVxMetersPerSecond;
|
||
wheelFeedbackVelocityEstimateValid =
|
||
_wheelFeedbackVelocityEstimateValid;
|
||
hasGcpCommand = _hasGcpCommand;
|
||
commandFrontGcpAngleRadians =
|
||
_commandFrontGcpAngleRadians;
|
||
commandRearGcpAngleRadians =
|
||
_commandRearGcpAngleRadians;
|
||
}
|
||
|
||
var sample = new TrackingSample
|
||
{
|
||
ElapsedSeconds =
|
||
_stopwatch.Elapsed.TotalSeconds,
|
||
DetourX = location.x,
|
||
DetourY = location.y,
|
||
DetourTheta = location.th,
|
||
CommandSpeed = commandSpeed,
|
||
CommandVx = commandVx,
|
||
CommandVy = commandVy,
|
||
CommandAngularSpeed =
|
||
commandAngularSpeed,
|
||
HasProcessedState =
|
||
processedState.HasValue,
|
||
HasControlReference =
|
||
hasControlReference,
|
||
ControlReferenceArcLengthMeters =
|
||
controlReferenceArcLengthMeters,
|
||
ControlReferenceSpeedMetersPerSecond =
|
||
controlReferenceSpeedMetersPerSecond,
|
||
ControlLateralErrorMeters =
|
||
controlLateralErrorMeters,
|
||
ControlHeadingErrorRadians =
|
||
controlHeadingErrorRadians,
|
||
ControlDistanceToTrajectoryMeters =
|
||
controlDistanceToTrajectoryMeters,
|
||
ControlRemainingDistanceMeters =
|
||
controlRemainingDistanceMeters,
|
||
CurvaturePreviewDistanceMeters =
|
||
curvaturePreviewDistanceMeters,
|
||
FeedforwardCurvaturePerMeter =
|
||
feedforwardCurvaturePerMeter,
|
||
HasVelocityDiagnostics =
|
||
hasVelocityDiagnostics,
|
||
DetourEstimatedBodyVxMetersPerSecond =
|
||
detourEstimatedBodyVxMetersPerSecond,
|
||
DetourVelocityEstimateValid =
|
||
detourVelocityEstimateValid,
|
||
WheelFeedbackRawBodyVxMetersPerSecond =
|
||
wheelFeedbackRawBodyVxMetersPerSecond,
|
||
WheelFeedbackFilteredBodyVxMetersPerSecond =
|
||
wheelFeedbackFilteredBodyVxMetersPerSecond,
|
||
WheelFeedbackVelocityEstimateValid =
|
||
wheelFeedbackVelocityEstimateValid,
|
||
HasGcpCommand = hasGcpCommand,
|
||
CommandFrontGcpAngleRadians =
|
||
commandFrontGcpAngleRadians,
|
||
CommandRearGcpAngleRadians =
|
||
commandRearGcpAngleRadians
|
||
};
|
||
|
||
CaptureSteeringDiagnostics(sample);
|
||
|
||
if (processedState.HasValue)
|
||
{
|
||
var state = processedState.Value;
|
||
sample.StateTimestampSeconds =
|
||
state.SampleTimestampSeconds;
|
||
sample.StateXmeters =
|
||
state.PoseInWorld.XMeters;
|
||
sample.StateYMeters =
|
||
state.PoseInWorld.YMeters;
|
||
sample.StateYawRadians =
|
||
state.PoseInWorld.YawRadians;
|
||
sample.StateWorldVxMetersPerSecond =
|
||
state.TwistInWorld.VxMetersPerSecond;
|
||
sample.StateWorldVyMetersPerSecond =
|
||
state.TwistInWorld.VyMetersPerSecond;
|
||
sample.StateBodyVxMetersPerSecond =
|
||
state.TwistInBody.VxMetersPerSecond;
|
||
sample.StateBodyVyMetersPerSecond =
|
||
state.TwistInBody.VyMetersPerSecond;
|
||
sample.StateAngularSpeedRadiansPerSecond =
|
||
state.TwistInBody.OmegaRadiansPerSecond;
|
||
sample.StateVelocityEstimateValid =
|
||
state.HasValidVelocityEstimate;
|
||
}
|
||
|
||
lock (_sampleSyncRoot)
|
||
{
|
||
_samples.Add(sample);
|
||
}
|
||
}
|
||
catch (Exception ex)
|
||
{
|
||
// 单帧读取失败不应终止车辆控制或整个记录线程。
|
||
Console.WriteLine(
|
||
$"轨迹实验采样失败：{ex.Message}");
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 按舵轮物理安装位置记录四轮目标角和实际反馈角。
|
||
/// </summary>
|
||
private void CaptureSteeringDiagnostics(
|
||
TrackingSample sample)
|
||
{
|
||
if (_diagnosticChassis == null)
|
||
{
|
||
return;
|
||
}
|
||
|
||
#pragma warning disable CS0612, CS0618
|
||
var wheels = _diagnosticChassis.GetSteerWheels();
|
||
#pragma warning restore CS0612, CS0618
|
||
|
||
var leftFront = FindWheel(
|
||
wheels,
|
||
requireFront: true,
|
||
requireLeft: true);
|
||
var leftRear = FindWheel(
|
||
wheels,
|
||
requireFront: false,
|
||
requireLeft: true);
|
||
var rightFront = FindWheel(
|
||
wheels,
|
||
requireFront: true,
|
||
requireLeft: false);
|
||
var rightRear = FindWheel(
|
||
wheels,
|
||
requireFront: false,
|
||
requireLeft: false);
|
||
|
||
if (leftFront == null ||
|
||
leftRear == null ||
|
||
rightFront == null ||
|
||
rightRear == null)
|
||
{
|
||
return;
|
||
}
|
||
|
||
sample.HasSteeringDiagnostics = true;
|
||
sample.TargetSteerLeftFrontDegrees =
|
||
leftFront.GetSendAngle();
|
||
sample.TargetSteerLeftRearDegrees =
|
||
leftRear.GetSendAngle();
|
||
sample.TargetSteerRightFrontDegrees =
|
||
rightFront.GetSendAngle();
|
||
sample.TargetSteerRightRearDegrees =
|
||
rightRear.GetSendAngle();
|
||
sample.ActualSteerLeftFrontDegrees =
|
||
leftFront.ReadAngle();
|
||
sample.ActualSteerLeftRearDegrees =
|
||
leftRear.ReadAngle();
|
||
sample.ActualSteerRightFrontDegrees =
|
||
rightFront.ReadAngle();
|
||
sample.ActualSteerRightRearDegrees =
|
||
rightRear.ReadAngle();
|
||
}
|
||
|
||
/// <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;
|
||
}
|
||
|
||
// 将内存中的采样数据写入CSV。
|
||
private void SaveCsv()
|
||
{
|
||
List<TrackingSample> snapshot;
|
||
|
||
lock (_sampleSyncRoot)
|
||
{
|
||
snapshot =
|
||
new List<TrackingSample>(_samples);
|
||
}
|
||
|
||
var outputDirectory =
|
||
DefaultOutputDirectory;
|
||
|
||
Directory.CreateDirectory(outputDirectory);
|
||
|
||
var fileName =
|
||
$"{DateTime.Now:yyyyMMdd_HHmmss_fff}_" +
|
||
$"{SanitizeFileName(_controllerName)}_" +
|
||
$"{SanitizeFileName(_trajectoryName)}_" +
|
||
$"Trial{_trialNumber}.csv";
|
||
|
||
SavedFilePath = Path.Combine(
|
||
outputDirectory,
|
||
fileName);
|
||
|
||
using (var writer = new StreamWriter(
|
||
SavedFilePath,
|
||
false,
|
||
new UTF8Encoding(true)))
|
||
{
|
||
writer.WriteLine(
|
||
"ElapsedSeconds," +
|
||
"ControllerName," +
|
||
"TrajectoryName," +
|
||
"TrialNumber," +
|
||
"DetourX," +
|
||
"DetourY," +
|
||
"DetourTheta," +
|
||
"CommandSpeed," +
|
||
// 保留旧列（deg/s）供历史Python脚本兼容。
|
||
"CommandAngularSpeed," +
|
||
"CommandAngularSpeedRadPerSecond," +
|
||
"CommandVx," +
|
||
"CommandVy," +
|
||
"ReferenceStartX," +
|
||
"ReferenceStartY," +
|
||
"ReferenceEndX," +
|
||
"ReferenceEndY," +
|
||
"ReferenceSpeed," +
|
||
"ReferenceAngularSpeedRadPerSecond," +
|
||
"ReferenceMotionFrameYawDegrees," +
|
||
"ReferenceAccelerationMetersPerSecondSquared," +
|
||
"ReferenceDecelerationMetersPerSecondSquared," +
|
||
"HasProcessedState," +
|
||
"StateTimestampSeconds," +
|
||
"StateXMeters," +
|
||
"StateYMeters," +
|
||
"StateYawRadians," +
|
||
"StateWorldVxMetersPerSecond," +
|
||
"StateWorldVyMetersPerSecond," +
|
||
"StateBodyVxMetersPerSecond," +
|
||
"StateBodyVyMetersPerSecond," +
|
||
"StateAngularSpeedRadiansPerSecond," +
|
||
"StateVelocityEstimateValid," +
|
||
"HasControlReference," +
|
||
"ControlReferenceArcLengthMeters," +
|
||
"ControlReferenceSpeedMetersPerSecond," +
|
||
"ControlLateralErrorMeters," +
|
||
"ControlHeadingErrorRadians," +
|
||
"ControlDistanceToTrajectoryMeters," +
|
||
"ControlRemainingDistanceMeters," +
|
||
"CurvaturePreviewDistanceMeters," +
|
||
"FeedforwardCurvaturePerMeter," +
|
||
"HasVelocityDiagnostics," +
|
||
"DetourEstimatedBodyVxMetersPerSecond," +
|
||
"DetourVelocityEstimateValid," +
|
||
"WheelFeedbackRawBodyVxMetersPerSecond," +
|
||
"WheelFeedbackFilteredBodyVxMetersPerSecond," +
|
||
"WheelFeedbackVelocityEstimateValid," +
|
||
"HasSteeringDiagnostics," +
|
||
"TargetSteerLeftFrontDegrees," +
|
||
"TargetSteerLeftRearDegrees," +
|
||
"TargetSteerRightFrontDegrees," +
|
||
"TargetSteerRightRearDegrees," +
|
||
"ActualSteerLeftFrontDegrees," +
|
||
"ActualSteerLeftRearDegrees," +
|
||
"ActualSteerRightFrontDegrees," +
|
||
"ActualSteerRightRearDegrees," +
|
||
"HasGcpCommand," +
|
||
"CommandFrontGcpAngleRadians," +
|
||
"CommandRearGcpAngleRadians");
|
||
|
||
foreach (var sample in snapshot)
|
||
{
|
||
writer.WriteLine(string.Join(
|
||
",",
|
||
Format(sample.ElapsedSeconds),
|
||
EscapeCsv(_controllerName),
|
||
EscapeCsv(_trajectoryName),
|
||
_trialNumber.ToString(
|
||
CultureInfo.InvariantCulture),
|
||
Format(sample.DetourX),
|
||
Format(sample.DetourY),
|
||
Format(sample.DetourTheta),
|
||
Format(sample.CommandSpeed),
|
||
Format(
|
||
AngleMath.RadiansToDegrees(
|
||
sample.CommandAngularSpeed)),
|
||
Format(sample.CommandAngularSpeed),
|
||
Format(sample.CommandVx),
|
||
Format(sample.CommandVy),
|
||
Format(_referenceStart.X),
|
||
Format(_referenceStart.Y),
|
||
Format(_referenceEnd.X),
|
||
Format(_referenceEnd.Y),
|
||
Format(_referenceSpeed),
|
||
Format(_referenceAngularSpeed),
|
||
Format(_referenceMotionFrameYawDegrees),
|
||
Format(
|
||
_referenceAccelerationMetersPerSecondSquared),
|
||
Format(
|
||
_referenceDecelerationMetersPerSecondSquared),
|
||
sample.HasProcessedState
|
||
? "1"
|
||
: "0",
|
||
FormatOptional(
|
||
sample.HasProcessedState,
|
||
sample.StateTimestampSeconds),
|
||
FormatOptional(
|
||
sample.HasProcessedState,
|
||
sample.StateXmeters),
|
||
FormatOptional(
|
||
sample.HasProcessedState,
|
||
sample.StateYMeters),
|
||
FormatOptional(
|
||
sample.HasProcessedState,
|
||
sample.StateYawRadians),
|
||
FormatOptional(
|
||
sample.HasProcessedState,
|
||
sample.StateWorldVxMetersPerSecond),
|
||
FormatOptional(
|
||
sample.HasProcessedState,
|
||
sample.StateWorldVyMetersPerSecond),
|
||
FormatOptional(
|
||
sample.HasProcessedState,
|
||
sample.StateBodyVxMetersPerSecond),
|
||
FormatOptional(
|
||
sample.HasProcessedState,
|
||
sample.StateBodyVyMetersPerSecond),
|
||
FormatOptional(
|
||
sample.HasProcessedState,
|
||
sample.StateAngularSpeedRadiansPerSecond),
|
||
sample.HasProcessedState
|
||
? sample.StateVelocityEstimateValid
|
||
? "1"
|
||
: "0"
|
||
: string.Empty,
|
||
sample.HasControlReference
|
||
? "1"
|
||
: "0",
|
||
FormatOptional(
|
||
sample.HasControlReference,
|
||
sample.ControlReferenceArcLengthMeters),
|
||
FormatOptional(
|
||
sample.HasControlReference,
|
||
sample.ControlReferenceSpeedMetersPerSecond),
|
||
FormatOptional(
|
||
sample.HasControlReference,
|
||
sample.ControlLateralErrorMeters),
|
||
FormatOptional(
|
||
sample.HasControlReference,
|
||
sample.ControlHeadingErrorRadians),
|
||
FormatOptional(
|
||
sample.HasControlReference,
|
||
sample.ControlDistanceToTrajectoryMeters),
|
||
FormatOptional(
|
||
sample.HasControlReference,
|
||
sample.ControlRemainingDistanceMeters),
|
||
FormatOptional(
|
||
sample.HasControlReference,
|
||
sample.CurvaturePreviewDistanceMeters),
|
||
FormatOptional(
|
||
sample.HasControlReference,
|
||
sample.FeedforwardCurvaturePerMeter),
|
||
sample.HasVelocityDiagnostics
|
||
? "1"
|
||
: "0",
|
||
FormatOptional(
|
||
sample.HasVelocityDiagnostics,
|
||
sample.DetourEstimatedBodyVxMetersPerSecond),
|
||
sample.HasVelocityDiagnostics
|
||
? sample.DetourVelocityEstimateValid
|
||
? "1"
|
||
: "0"
|
||
: string.Empty,
|
||
FormatOptional(
|
||
sample.HasVelocityDiagnostics,
|
||
sample.WheelFeedbackRawBodyVxMetersPerSecond),
|
||
FormatOptional(
|
||
sample.HasVelocityDiagnostics,
|
||
sample.WheelFeedbackFilteredBodyVxMetersPerSecond),
|
||
sample.HasVelocityDiagnostics
|
||
? sample.WheelFeedbackVelocityEstimateValid
|
||
? "1"
|
||
: "0"
|
||
: string.Empty,
|
||
sample.HasSteeringDiagnostics
|
||
? "1"
|
||
: "0",
|
||
FormatOptional(
|
||
sample.HasSteeringDiagnostics,
|
||
sample.TargetSteerLeftFrontDegrees),
|
||
FormatOptional(
|
||
sample.HasSteeringDiagnostics,
|
||
sample.TargetSteerLeftRearDegrees),
|
||
FormatOptional(
|
||
sample.HasSteeringDiagnostics,
|
||
sample.TargetSteerRightFrontDegrees),
|
||
FormatOptional(
|
||
sample.HasSteeringDiagnostics,
|
||
sample.TargetSteerRightRearDegrees),
|
||
FormatOptional(
|
||
sample.HasSteeringDiagnostics,
|
||
sample.ActualSteerLeftFrontDegrees),
|
||
FormatOptional(
|
||
sample.HasSteeringDiagnostics,
|
||
sample.ActualSteerLeftRearDegrees),
|
||
FormatOptional(
|
||
sample.HasSteeringDiagnostics,
|
||
sample.ActualSteerRightFrontDegrees),
|
||
FormatOptional(
|
||
sample.HasSteeringDiagnostics,
|
||
sample.ActualSteerRightRearDegrees),
|
||
sample.HasGcpCommand
|
||
? "1"
|
||
: "0",
|
||
FormatOptional(
|
||
sample.HasGcpCommand,
|
||
sample.CommandFrontGcpAngleRadians),
|
||
FormatOptional(
|
||
sample.HasGcpCommand,
|
||
sample.CommandRearGcpAngleRadians)));
|
||
}
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 将逐控制周期的内存计时数据保存为独立CSV，不受后台采样周期限制。
|
||
/// </summary>
|
||
private void SaveControlCycleTimingCsv()
|
||
{
|
||
List<ControlCycleTimingRecord> snapshot;
|
||
|
||
lock (_controlCycleTimingSyncRoot)
|
||
{
|
||
snapshot =
|
||
new List<ControlCycleTimingRecord>(
|
||
_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)
|
||
{
|
||
var result = value;
|
||
|
||
foreach (var invalidCharacter in
|
||
Path.GetInvalidFileNameChars())
|
||
{
|
||
result = result.Replace(
|
||
invalidCharacter,
|
||
'_');
|
||
}
|
||
|
||
return result;
|
||
}
|
||
|
||
// 按固定小数格式输出数值，避免系统区域设置改变CSV格式。
|
||
private static string Format(double value)
|
||
{
|
||
return value.ToString(
|
||
"0.######",
|
||
CultureInfo.InvariantCulture);
|
||
}
|
||
|
||
/// <summary>
|
||
/// 在新版状态尚未产生时为空，否则按统一小数格式输出状态数值。
|
||
/// </summary>
|
||
private static string FormatOptional(
|
||
bool hasValue,
|
||
double value)
|
||
{
|
||
return hasValue
|
||
? Format(value)
|
||
: string.Empty;
|
||
}
|
||
|
||
// 对CSV文本字段进行引号和逗号转义。
|
||
private static string EscapeCsv(string value)
|
||
{
|
||
if (value == null)
|
||
return string.Empty;
|
||
|
||
if (!value.Contains(",") &&
|
||
!value.Contains("\"") &&
|
||
!value.Contains("\r") &&
|
||
!value.Contains("\n"))
|
||
{
|
||
return value;
|
||
}
|
||
|
||
return
|
||
"\"" +
|
||
value.Replace("\"", "\"\"") +
|
||
"\"";
|
||
}
|
||
}
|
||
}
|