控制器测试横向跟踪，增加记录
This commit is contained in:
@@ -7,6 +7,7 @@ using System.IO;
|
||||
using System.Numerics;
|
||||
using System.Text;
|
||||
using System.Threading;
|
||||
using CommonUsage.Chassis;
|
||||
using MyParking.Shared;
|
||||
using MultiWheelC.StateEstimation;
|
||||
|
||||
@@ -47,6 +48,28 @@ namespace MultiWheelC
|
||||
public double ControlHeadingErrorRadians;
|
||||
public double ControlDistanceToTrajectoryMeters;
|
||||
public double ControlRemainingDistanceMeters;
|
||||
|
||||
// 并列保存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层实验工具：统一采集并保存轨迹跟踪实验数据。
|
||||
@@ -63,6 +86,7 @@ namespace MultiWheelC
|
||||
private readonly float _referenceAccelerationMetersPerSecondSquared;
|
||||
private readonly float _referenceDecelerationMetersPerSecondSquared;
|
||||
private readonly int _sampleIntervalMs;
|
||||
private readonly MultiWheelChassis _diagnosticChassis;
|
||||
|
||||
private readonly List<TrackingSample> _samples =
|
||||
new List<TrackingSample>();
|
||||
@@ -97,6 +121,15 @@ namespace MultiWheelC
|
||||
private double _controlHeadingErrorRadians;
|
||||
private double _controlDistanceToTrajectoryMeters;
|
||||
private double _controlRemainingDistanceMeters;
|
||||
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,
|
||||
@@ -109,7 +142,8 @@ namespace MultiWheelC
|
||||
int sampleIntervalMs = 50,
|
||||
float referenceMotionFrameYawDegrees = 0f,
|
||||
float referenceAccelerationMetersPerSecondSquared = 0f,
|
||||
float referenceDecelerationMetersPerSecondSquared = 0f)
|
||||
float referenceDecelerationMetersPerSecondSquared = 0f,
|
||||
MultiWheelChassis diagnosticChassis = null)
|
||||
{
|
||||
if (string.IsNullOrWhiteSpace(controllerName))
|
||||
throw new ArgumentException(
|
||||
@@ -140,6 +174,7 @@ namespace MultiWheelC
|
||||
_referenceDecelerationMetersPerSecondSquared =
|
||||
referenceDecelerationMetersPerSecondSquared;
|
||||
_sampleIntervalMs = sampleIntervalMs;
|
||||
_diagnosticChassis = diagnosticChassis;
|
||||
}
|
||||
|
||||
// 保存成功后的CSV绝对路径；尚未保存时为空。
|
||||
@@ -221,6 +256,60 @@ namespace MultiWheelC
|
||||
}
|
||||
}
|
||||
|
||||
/// <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>
|
||||
@@ -331,6 +420,15 @@ namespace MultiWheelC
|
||||
double controlHeadingErrorRadians;
|
||||
double controlDistanceToTrajectoryMeters;
|
||||
double controlRemainingDistanceMeters;
|
||||
bool hasVelocityDiagnostics;
|
||||
double detourEstimatedBodyVxMetersPerSecond;
|
||||
bool detourVelocityEstimateValid;
|
||||
double wheelFeedbackRawBodyVxMetersPerSecond;
|
||||
double wheelFeedbackFilteredBodyVxMetersPerSecond;
|
||||
bool wheelFeedbackVelocityEstimateValid;
|
||||
bool hasGcpCommand;
|
||||
double commandFrontGcpAngleRadians;
|
||||
double commandRearGcpAngleRadians;
|
||||
|
||||
lock (_commandSyncRoot)
|
||||
{
|
||||
@@ -382,6 +480,23 @@ namespace MultiWheelC
|
||||
_controlDistanceToTrajectoryMeters;
|
||||
controlRemainingDistanceMeters =
|
||||
_controlRemainingDistanceMeters;
|
||||
hasVelocityDiagnostics =
|
||||
_hasVelocityDiagnostics;
|
||||
detourEstimatedBodyVxMetersPerSecond =
|
||||
_detourEstimatedBodyVxMetersPerSecond;
|
||||
detourVelocityEstimateValid =
|
||||
_detourVelocityEstimateValid;
|
||||
wheelFeedbackRawBodyVxMetersPerSecond =
|
||||
_wheelFeedbackRawBodyVxMetersPerSecond;
|
||||
wheelFeedbackFilteredBodyVxMetersPerSecond =
|
||||
_wheelFeedbackFilteredBodyVxMetersPerSecond;
|
||||
wheelFeedbackVelocityEstimateValid =
|
||||
_wheelFeedbackVelocityEstimateValid;
|
||||
hasGcpCommand = _hasGcpCommand;
|
||||
commandFrontGcpAngleRadians =
|
||||
_commandFrontGcpAngleRadians;
|
||||
commandRearGcpAngleRadians =
|
||||
_commandRearGcpAngleRadians;
|
||||
}
|
||||
|
||||
var sample = new TrackingSample
|
||||
@@ -411,9 +526,28 @@ namespace MultiWheelC
|
||||
ControlDistanceToTrajectoryMeters =
|
||||
controlDistanceToTrajectoryMeters,
|
||||
ControlRemainingDistanceMeters =
|
||||
controlRemainingDistanceMeters
|
||||
controlRemainingDistanceMeters,
|
||||
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;
|
||||
@@ -452,6 +586,90 @@ namespace MultiWheelC
|
||||
}
|
||||
}
|
||||
|
||||
/// <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()
|
||||
{
|
||||
@@ -523,7 +741,25 @@ namespace MultiWheelC
|
||||
"ControlLateralErrorMeters," +
|
||||
"ControlHeadingErrorRadians," +
|
||||
"ControlDistanceToTrajectoryMeters," +
|
||||
"ControlRemainingDistanceMeters");
|
||||
"ControlRemainingDistanceMeters," +
|
||||
"HasVelocityDiagnostics," +
|
||||
"DetourEstimatedBodyVxMetersPerSecond," +
|
||||
"DetourVelocityEstimateValid," +
|
||||
"WheelFeedbackRawBodyVxMetersPerSecond," +
|
||||
"WheelFeedbackFilteredBodyVxMetersPerSecond," +
|
||||
"WheelFeedbackVelocityEstimateValid," +
|
||||
"HasSteeringDiagnostics," +
|
||||
"TargetSteerLeftFrontDegrees," +
|
||||
"TargetSteerLeftRearDegrees," +
|
||||
"TargetSteerRightFrontDegrees," +
|
||||
"TargetSteerRightRearDegrees," +
|
||||
"ActualSteerLeftFrontDegrees," +
|
||||
"ActualSteerLeftRearDegrees," +
|
||||
"ActualSteerRightFrontDegrees," +
|
||||
"ActualSteerRightRearDegrees," +
|
||||
"HasGcpCommand," +
|
||||
"CommandFrontGcpAngleRadians," +
|
||||
"CommandRearGcpAngleRadians");
|
||||
|
||||
foreach (var sample in snapshot)
|
||||
{
|
||||
@@ -610,7 +846,65 @@ namespace MultiWheelC
|
||||
sample.ControlDistanceToTrajectoryMeters),
|
||||
FormatOptional(
|
||||
sample.HasControlReference,
|
||||
sample.ControlRemainingDistanceMeters)));
|
||||
sample.ControlRemainingDistanceMeters),
|
||||
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)));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user