打通新版Stanley轨迹跟踪闭环并补充实验测试与数据分析脚本
Co-authored-by: Cursor <cursoragent@cursor.com>
This commit is contained in:
Cursor
@@ -8,6 +8,7 @@ using System.Numerics;
|
||||
using System.Text;
|
||||
using System.Threading;
|
||||
using MyParking.Shared;
|
||||
using MultiWheelC.StateEstimation;
|
||||
|
||||
namespace MultiWheelC
|
||||
{
|
||||
@@ -26,6 +27,26 @@ namespace MultiWheelC
|
||||
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;
|
||||
}
|
||||
|
||||
// C层实验工具：统一采集并保存轨迹跟踪实验数据。
|
||||
@@ -39,6 +60,8 @@ namespace MultiWheelC
|
||||
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 List<TrackingSample> _samples =
|
||||
@@ -50,6 +73,9 @@ namespace MultiWheelC
|
||||
private readonly object _commandSyncRoot =
|
||||
new object();
|
||||
|
||||
private readonly object _stateSyncRoot =
|
||||
new object();
|
||||
|
||||
private readonly Stopwatch _stopwatch =
|
||||
new Stopwatch();
|
||||
|
||||
@@ -63,6 +89,14 @@ namespace MultiWheelC
|
||||
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;
|
||||
|
||||
public TrackingExperimentRecorder(
|
||||
string controllerName,
|
||||
@@ -73,7 +107,9 @@ namespace MultiWheelC
|
||||
float referenceSpeed,
|
||||
float referenceAngularSpeed = 0f,
|
||||
int sampleIntervalMs = 50,
|
||||
float referenceMotionFrameYawDegrees = 0f)
|
||||
float referenceMotionFrameYawDegrees = 0f,
|
||||
float referenceAccelerationMetersPerSecondSquared = 0f,
|
||||
float referenceDecelerationMetersPerSecondSquared = 0f)
|
||||
{
|
||||
if (string.IsNullOrWhiteSpace(controllerName))
|
||||
throw new ArgumentException(
|
||||
@@ -99,6 +135,10 @@ namespace MultiWheelC
|
||||
_referenceAngularSpeed = referenceAngularSpeed;
|
||||
_referenceMotionFrameYawDegrees =
|
||||
referenceMotionFrameYawDegrees;
|
||||
_referenceAccelerationMetersPerSecondSquared =
|
||||
referenceAccelerationMetersPerSecondSquared;
|
||||
_referenceDecelerationMetersPerSecondSquared =
|
||||
referenceDecelerationMetersPerSecondSquared;
|
||||
_sampleIntervalMs = sampleIntervalMs;
|
||||
}
|
||||
|
||||
@@ -162,6 +202,46 @@ namespace MultiWheelC
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 保存新版控制器本周期实际使用的校验后车辆状态，供后台采样线程写入CSV。
|
||||
/// </summary>
|
||||
public void UpdateProcessedState(VehicleState state)
|
||||
{
|
||||
lock (_stateSyncRoot)
|
||||
{
|
||||
_latestProcessedState = state;
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 保存新版控制器本周期实际使用的轨迹投影、误差和参考速度。
|
||||
/// </summary>
|
||||
public void UpdateControlReference(
|
||||
double arcLengthMeters,
|
||||
double referenceSpeedMetersPerSecond,
|
||||
double lateralErrorMeters,
|
||||
double headingErrorRadians,
|
||||
double distanceToTrajectoryMeters,
|
||||
double remainingDistanceMeters)
|
||||
{
|
||||
lock (_stateSyncRoot)
|
||||
{
|
||||
_controlReferenceArcLengthMeters =
|
||||
arcLengthMeters;
|
||||
_controlReferenceSpeedMetersPerSecond =
|
||||
referenceSpeedMetersPerSecond;
|
||||
_controlLateralErrorMeters =
|
||||
lateralErrorMeters;
|
||||
_controlHeadingErrorRadians =
|
||||
headingErrorRadians;
|
||||
_controlDistanceToTrajectoryMeters =
|
||||
distanceToTrajectoryMeters;
|
||||
_controlRemainingDistanceMeters =
|
||||
remainingDistanceMeters;
|
||||
_hasControlReference = true;
|
||||
}
|
||||
}
|
||||
|
||||
// 停止采样并将本次实验保存为CSV；重复调用只保存一次。
|
||||
public void StopAndSave()
|
||||
{
|
||||
@@ -224,6 +304,14 @@ namespace MultiWheelC
|
||||
float commandVx;
|
||||
float commandVy;
|
||||
float commandAngularSpeed;
|
||||
VehicleState? processedState;
|
||||
bool hasControlReference;
|
||||
double controlReferenceArcLengthMeters;
|
||||
double controlReferenceSpeedMetersPerSecond;
|
||||
double controlLateralErrorMeters;
|
||||
double controlHeadingErrorRadians;
|
||||
double controlDistanceToTrajectoryMeters;
|
||||
double controlRemainingDistanceMeters;
|
||||
|
||||
lock (_commandSyncRoot)
|
||||
{
|
||||
@@ -257,6 +345,26 @@ namespace MultiWheelC
|
||||
}
|
||||
}
|
||||
|
||||
lock (_stateSyncRoot)
|
||||
{
|
||||
processedState =
|
||||
_latestProcessedState;
|
||||
hasControlReference =
|
||||
_hasControlReference;
|
||||
controlReferenceArcLengthMeters =
|
||||
_controlReferenceArcLengthMeters;
|
||||
controlReferenceSpeedMetersPerSecond =
|
||||
_controlReferenceSpeedMetersPerSecond;
|
||||
controlLateralErrorMeters =
|
||||
_controlLateralErrorMeters;
|
||||
controlHeadingErrorRadians =
|
||||
_controlHeadingErrorRadians;
|
||||
controlDistanceToTrajectoryMeters =
|
||||
_controlDistanceToTrajectoryMeters;
|
||||
controlRemainingDistanceMeters =
|
||||
_controlRemainingDistanceMeters;
|
||||
}
|
||||
|
||||
var sample = new TrackingSample
|
||||
{
|
||||
ElapsedSeconds =
|
||||
@@ -268,9 +376,50 @@ namespace MultiWheelC
|
||||
CommandVx = commandVx,
|
||||
CommandVy = commandVy,
|
||||
CommandAngularSpeed =
|
||||
commandAngularSpeed
|
||||
commandAngularSpeed,
|
||||
HasProcessedState =
|
||||
processedState.HasValue,
|
||||
HasControlReference =
|
||||
hasControlReference,
|
||||
ControlReferenceArcLengthMeters =
|
||||
controlReferenceArcLengthMeters,
|
||||
ControlReferenceSpeedMetersPerSecond =
|
||||
controlReferenceSpeedMetersPerSecond,
|
||||
ControlLateralErrorMeters =
|
||||
controlLateralErrorMeters,
|
||||
ControlHeadingErrorRadians =
|
||||
controlHeadingErrorRadians,
|
||||
ControlDistanceToTrajectoryMeters =
|
||||
controlDistanceToTrajectoryMeters,
|
||||
ControlRemainingDistanceMeters =
|
||||
controlRemainingDistanceMeters
|
||||
};
|
||||
|
||||
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);
|
||||
@@ -336,7 +485,27 @@ namespace MultiWheelC
|
||||
"ReferenceEndY," +
|
||||
"ReferenceSpeed," +
|
||||
"ReferenceAngularSpeedRadPerSecond," +
|
||||
"ReferenceMotionFrameYawDegrees");
|
||||
"ReferenceMotionFrameYawDegrees," +
|
||||
"ReferenceAccelerationMetersPerSecondSquared," +
|
||||
"ReferenceDecelerationMetersPerSecondSquared," +
|
||||
"HasProcessedState," +
|
||||
"StateTimestampSeconds," +
|
||||
"StateXMeters," +
|
||||
"StateYMeters," +
|
||||
"StateYawRadians," +
|
||||
"StateWorldVxMetersPerSecond," +
|
||||
"StateWorldVyMetersPerSecond," +
|
||||
"StateBodyVxMetersPerSecond," +
|
||||
"StateBodyVyMetersPerSecond," +
|
||||
"StateAngularSpeedRadiansPerSecond," +
|
||||
"StateVelocityEstimateValid," +
|
||||
"HasControlReference," +
|
||||
"ControlReferenceArcLengthMeters," +
|
||||
"ControlReferenceSpeedMetersPerSecond," +
|
||||
"ControlLateralErrorMeters," +
|
||||
"ControlHeadingErrorRadians," +
|
||||
"ControlDistanceToTrajectoryMeters," +
|
||||
"ControlRemainingDistanceMeters");
|
||||
|
||||
foreach (var sample in snapshot)
|
||||
{
|
||||
@@ -363,7 +532,67 @@ namespace MultiWheelC
|
||||
Format(_referenceEnd.Y),
|
||||
Format(_referenceSpeed),
|
||||
Format(_referenceAngularSpeed),
|
||||
Format(_referenceMotionFrameYawDegrees)));
|
||||
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)));
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -392,6 +621,18 @@ namespace MultiWheelC
|
||||
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)
|
||||
{
|
||||
|
||||
Reference in New Issue
Block a user