增加倒车测试
This commit is contained in:
@@ -9,6 +9,7 @@ using System.Text;
|
||||
using System.Threading;
|
||||
using CommonUsage.Chassis;
|
||||
using MyParking.Shared;
|
||||
using MultiWheelC.Control.Allocation;
|
||||
using MultiWheelC.Control.Execution;
|
||||
using MultiWheelC.StateEstimation;
|
||||
|
||||
@@ -71,6 +72,13 @@ namespace MultiWheelC
|
||||
public double ActualSteerRightFrontDegrees;
|
||||
public double ActualSteerRightRearDegrees;
|
||||
public bool HasGcpCommand;
|
||||
public double RequestedFrontGcpAngleRadians;
|
||||
public double RequestedRearGcpAngleRadians;
|
||||
public double SentFrontGcpAngleRadians;
|
||||
public double SentRearGcpAngleRadians;
|
||||
public bool FrontGcpRateLimitActive;
|
||||
public bool RearGcpRateLimitActive;
|
||||
// 兼容既有分析脚本：Command角仍表示限速后实际发送角。
|
||||
public double CommandFrontGcpAngleRadians;
|
||||
public double CommandRearGcpAngleRadians;
|
||||
}
|
||||
@@ -86,19 +94,28 @@ namespace MultiWheelC
|
||||
public ControlCycleTimingRecord(
|
||||
double recorderElapsedSeconds,
|
||||
int motionSegmentIndex,
|
||||
ParkingControlCycleTiming timing)
|
||||
ParkingControlCycleTiming timing,
|
||||
GcpMotionCommand? requestedCommand,
|
||||
GcpMotionCommand? sentCommand)
|
||||
{
|
||||
RecorderElapsedSeconds =
|
||||
recorderElapsedSeconds;
|
||||
MotionSegmentIndex = motionSegmentIndex;
|
||||
Timing = timing;
|
||||
RequestedCommand = requestedCommand;
|
||||
SentCommand = sentCommand;
|
||||
}
|
||||
|
||||
public double RecorderElapsedSeconds { get; }
|
||||
public int MotionSegmentIndex { get; }
|
||||
public ParkingControlCycleTiming Timing { get; }
|
||||
public GcpMotionCommand? RequestedCommand { get; }
|
||||
public GcpMotionCommand? SentCommand { get; }
|
||||
}
|
||||
|
||||
private const double GcpAngleComparisonToleranceRadians =
|
||||
1e-9;
|
||||
|
||||
private readonly string _controllerName;
|
||||
private readonly string _trajectoryName;
|
||||
private readonly int _trialNumber;
|
||||
@@ -161,6 +178,12 @@ namespace MultiWheelC
|
||||
private double _wheelFeedbackFilteredBodyVxMetersPerSecond;
|
||||
private bool _wheelFeedbackVelocityEstimateValid;
|
||||
private bool _hasGcpCommand;
|
||||
private double _requestedFrontGcpAngleRadians;
|
||||
private double _requestedRearGcpAngleRadians;
|
||||
private double _sentFrontGcpAngleRadians;
|
||||
private double _sentRearGcpAngleRadians;
|
||||
private bool _frontGcpRateLimitActive;
|
||||
private bool _rearGcpRateLimitActive;
|
||||
private double _commandFrontGcpAngleRadians;
|
||||
private double _commandRearGcpAngleRadians;
|
||||
|
||||
@@ -288,12 +311,16 @@ namespace MultiWheelC
|
||||
/// </summary>
|
||||
public void RecordControlCycleTiming(
|
||||
ParkingControlCycleTiming timing,
|
||||
int motionSegmentIndex = -1)
|
||||
int motionSegmentIndex = -1,
|
||||
GcpMotionCommand? requestedCommand = null,
|
||||
GcpMotionCommand? sentCommand = null)
|
||||
{
|
||||
var record = new ControlCycleTimingRecord(
|
||||
_stopwatch.Elapsed.TotalSeconds,
|
||||
motionSegmentIndex,
|
||||
timing);
|
||||
timing,
|
||||
requestedCommand,
|
||||
sentCommand);
|
||||
|
||||
lock (_controlCycleTimingSyncRoot)
|
||||
{
|
||||
@@ -339,18 +366,37 @@ namespace MultiWheelC
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 保存经过GCP角速度限制后实际交给底盘的前后虚拟控制点转角。
|
||||
/// 保存本周期经过GCP角速度限制前后的前后虚拟控制点转角。
|
||||
/// </summary>
|
||||
public void UpdateGcpCommand(
|
||||
double frontGcpAngleRadians,
|
||||
double rearGcpAngleRadians)
|
||||
double requestedFrontGcpAngleRadians,
|
||||
double requestedRearGcpAngleRadians,
|
||||
double sentFrontGcpAngleRadians,
|
||||
double sentRearGcpAngleRadians)
|
||||
{
|
||||
lock (_stateSyncRoot)
|
||||
{
|
||||
_requestedFrontGcpAngleRadians =
|
||||
requestedFrontGcpAngleRadians;
|
||||
_requestedRearGcpAngleRadians =
|
||||
requestedRearGcpAngleRadians;
|
||||
_sentFrontGcpAngleRadians =
|
||||
sentFrontGcpAngleRadians;
|
||||
_sentRearGcpAngleRadians =
|
||||
sentRearGcpAngleRadians;
|
||||
_frontGcpRateLimitActive =
|
||||
IsGcpRateLimitActive(
|
||||
requestedFrontGcpAngleRadians,
|
||||
sentFrontGcpAngleRadians);
|
||||
_rearGcpRateLimitActive =
|
||||
IsGcpRateLimitActive(
|
||||
requestedRearGcpAngleRadians,
|
||||
sentRearGcpAngleRadians);
|
||||
// 保留原字段语义，避免既有绘图脚本失效。
|
||||
_commandFrontGcpAngleRadians =
|
||||
frontGcpAngleRadians;
|
||||
sentFrontGcpAngleRadians;
|
||||
_commandRearGcpAngleRadians =
|
||||
rearGcpAngleRadians;
|
||||
sentRearGcpAngleRadians;
|
||||
_hasGcpCommand = true;
|
||||
}
|
||||
}
|
||||
@@ -499,6 +545,12 @@ namespace MultiWheelC
|
||||
double wheelFeedbackFilteredBodyVxMetersPerSecond;
|
||||
bool wheelFeedbackVelocityEstimateValid;
|
||||
bool hasGcpCommand;
|
||||
double requestedFrontGcpAngleRadians;
|
||||
double requestedRearGcpAngleRadians;
|
||||
double sentFrontGcpAngleRadians;
|
||||
double sentRearGcpAngleRadians;
|
||||
bool frontGcpRateLimitActive;
|
||||
bool rearGcpRateLimitActive;
|
||||
double commandFrontGcpAngleRadians;
|
||||
double commandRearGcpAngleRadians;
|
||||
|
||||
@@ -569,6 +621,18 @@ namespace MultiWheelC
|
||||
wheelFeedbackVelocityEstimateValid =
|
||||
_wheelFeedbackVelocityEstimateValid;
|
||||
hasGcpCommand = _hasGcpCommand;
|
||||
requestedFrontGcpAngleRadians =
|
||||
_requestedFrontGcpAngleRadians;
|
||||
requestedRearGcpAngleRadians =
|
||||
_requestedRearGcpAngleRadians;
|
||||
sentFrontGcpAngleRadians =
|
||||
_sentFrontGcpAngleRadians;
|
||||
sentRearGcpAngleRadians =
|
||||
_sentRearGcpAngleRadians;
|
||||
frontGcpRateLimitActive =
|
||||
_frontGcpRateLimitActive;
|
||||
rearGcpRateLimitActive =
|
||||
_rearGcpRateLimitActive;
|
||||
commandFrontGcpAngleRadians =
|
||||
_commandFrontGcpAngleRadians;
|
||||
commandRearGcpAngleRadians =
|
||||
@@ -620,6 +684,18 @@ namespace MultiWheelC
|
||||
WheelFeedbackVelocityEstimateValid =
|
||||
wheelFeedbackVelocityEstimateValid,
|
||||
HasGcpCommand = hasGcpCommand,
|
||||
RequestedFrontGcpAngleRadians =
|
||||
requestedFrontGcpAngleRadians,
|
||||
RequestedRearGcpAngleRadians =
|
||||
requestedRearGcpAngleRadians,
|
||||
SentFrontGcpAngleRadians =
|
||||
sentFrontGcpAngleRadians,
|
||||
SentRearGcpAngleRadians =
|
||||
sentRearGcpAngleRadians,
|
||||
FrontGcpRateLimitActive =
|
||||
frontGcpRateLimitActive,
|
||||
RearGcpRateLimitActive =
|
||||
rearGcpRateLimitActive,
|
||||
CommandFrontGcpAngleRadians =
|
||||
commandFrontGcpAngleRadians,
|
||||
CommandRearGcpAngleRadians =
|
||||
@@ -841,7 +917,13 @@ namespace MultiWheelC
|
||||
"ActualSteerRightRearDegrees," +
|
||||
"HasGcpCommand," +
|
||||
"CommandFrontGcpAngleRadians," +
|
||||
"CommandRearGcpAngleRadians");
|
||||
"CommandRearGcpAngleRadians," +
|
||||
"RequestedFrontGcpAngleRadians," +
|
||||
"RequestedRearGcpAngleRadians," +
|
||||
"SentFrontGcpAngleRadians," +
|
||||
"SentRearGcpAngleRadians," +
|
||||
"FrontGcpRateLimitActive," +
|
||||
"RearGcpRateLimitActive");
|
||||
|
||||
foreach (var sample in snapshot)
|
||||
{
|
||||
@@ -992,7 +1074,25 @@ namespace MultiWheelC
|
||||
sample.CommandFrontGcpAngleRadians),
|
||||
FormatOptional(
|
||||
sample.HasGcpCommand,
|
||||
sample.CommandRearGcpAngleRadians)));
|
||||
sample.CommandRearGcpAngleRadians),
|
||||
FormatOptional(
|
||||
sample.HasGcpCommand,
|
||||
sample.RequestedFrontGcpAngleRadians),
|
||||
FormatOptional(
|
||||
sample.HasGcpCommand,
|
||||
sample.RequestedRearGcpAngleRadians),
|
||||
FormatOptional(
|
||||
sample.HasGcpCommand,
|
||||
sample.SentFrontGcpAngleRadians),
|
||||
FormatOptional(
|
||||
sample.HasGcpCommand,
|
||||
sample.SentRearGcpAngleRadians),
|
||||
FormatOptionalBoolean(
|
||||
sample.HasGcpCommand,
|
||||
sample.FrontGcpRateLimitActive),
|
||||
FormatOptionalBoolean(
|
||||
sample.HasGcpCommand,
|
||||
sample.RearGcpRateLimitActive)));
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -1046,7 +1146,14 @@ namespace MultiWheelC
|
||||
"HasStateTimestamp," +
|
||||
"StateTimestampSeconds," +
|
||||
"StateTimestampChanged," +
|
||||
"CycleResult");
|
||||
"CycleResult," +
|
||||
"HasGcpCommand," +
|
||||
"RequestedFrontGcpAngleRadians," +
|
||||
"RequestedRearGcpAngleRadians," +
|
||||
"SentFrontGcpAngleRadians," +
|
||||
"SentRearGcpAngleRadians," +
|
||||
"FrontGcpRateLimitActive," +
|
||||
"RearGcpRateLimitActive");
|
||||
|
||||
foreach (var record in snapshot)
|
||||
{
|
||||
@@ -1060,6 +1167,23 @@ namespace MultiWheelC
|
||||
0.0,
|
||||
timing.TotalCycleMilliseconds -
|
||||
measuredStageMilliseconds);
|
||||
var hasGcpCommand =
|
||||
record.RequestedCommand.HasValue &&
|
||||
record.SentCommand.HasValue;
|
||||
var requestedCommand =
|
||||
record.RequestedCommand.GetValueOrDefault();
|
||||
var sentCommand =
|
||||
record.SentCommand.GetValueOrDefault();
|
||||
var frontRateLimitActive =
|
||||
hasGcpCommand &&
|
||||
IsGcpRateLimitActive(
|
||||
requestedCommand.FrontAngleRadians,
|
||||
sentCommand.FrontAngleRadians);
|
||||
var rearRateLimitActive =
|
||||
hasGcpCommand &&
|
||||
IsGcpRateLimitActive(
|
||||
requestedCommand.RearAngleRadians,
|
||||
sentCommand.RearAngleRadians);
|
||||
|
||||
writer.WriteLine(string.Join(
|
||||
",",
|
||||
@@ -1092,7 +1216,28 @@ namespace MultiWheelC
|
||||
? "1"
|
||||
: "0"
|
||||
: string.Empty,
|
||||
EscapeCsv(timing.Result.ToString())));
|
||||
EscapeCsv(timing.Result.ToString()),
|
||||
hasGcpCommand
|
||||
? "1"
|
||||
: "0",
|
||||
FormatOptional(
|
||||
hasGcpCommand,
|
||||
requestedCommand.FrontAngleRadians),
|
||||
FormatOptional(
|
||||
hasGcpCommand,
|
||||
requestedCommand.RearAngleRadians),
|
||||
FormatOptional(
|
||||
hasGcpCommand,
|
||||
sentCommand.FrontAngleRadians),
|
||||
FormatOptional(
|
||||
hasGcpCommand,
|
||||
sentCommand.RearAngleRadians),
|
||||
FormatOptionalBoolean(
|
||||
hasGcpCommand,
|
||||
frontRateLimitActive),
|
||||
FormatOptionalBoolean(
|
||||
hasGcpCommand,
|
||||
rearRateLimitActive)));
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -1133,6 +1278,33 @@ namespace MultiWheelC
|
||||
: string.Empty;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 在GCP命令有效时将布尔诊断输出为0或1，否则保持CSV空值。
|
||||
/// </summary>
|
||||
private static string FormatOptionalBoolean(
|
||||
bool hasValue,
|
||||
bool value)
|
||||
{
|
||||
return hasValue
|
||||
? value
|
||||
? "1"
|
||||
: "0"
|
||||
: string.Empty;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 判断GCP角速度限制是否实质改变了控制器请求角度。
|
||||
/// </summary>
|
||||
private static bool IsGcpRateLimitActive(
|
||||
double requestedAngleRadians,
|
||||
double sentAngleRadians)
|
||||
{
|
||||
return Math.Abs(
|
||||
requestedAngleRadians -
|
||||
sentAngleRadians) >
|
||||
GcpAngleComparisonToleranceRadians;
|
||||
}
|
||||
|
||||
// 对CSV文本字段进行引号和逗号转义。
|
||||
private static string EscapeCsv(string value)
|
||||
{
|
||||
|
||||
Reference in New Issue
Block a user