First_Dev_Dept/ParkingRobot
10
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

集中停车控制配置并完善终点逼近和周期诊断

Browse Source
This commit is contained in:
yuxiang.shen
2026-08-11 17:46:52 +08:00
parent 33a33af710
commit 3043febd91
30 changed files with 2946 additions and 30 deletions
Download Patch File Download Diff File Expand all files Collapse all files
MultiWheelC/Experiments/CompositeMotionPlanTests.cs
+9
View File
@@ -178,6 +178,7 @@ namespace MultiWheelC
},
TrackingCycleObserver = (index, controller) =>
RecordTrackingCycle(
index,
controller,
controlPointRadiusMeters,
stateProvider),
@@ -281,10 +282,18 @@ namespace MultiWheelC
/// 将轨迹控制周期使用的状态、参考误差和最终GCP命令写入记录器。
/// </summary>
private void RecordTrackingCycle(
int motionSegmentIndex,
ParkingGeometricController controller,
double controlPointRadiusMeters,
WheelFeedbackVehicleStateProvider stateProvider)
{
if (controller.LastCycleTiming.HasValue)
{
_recorder?.RecordControlCycleTiming(
controller.LastCycleTiming.Value,
motionSegmentIndex);
}
if (controller.LastVehicleState.HasValue)
{
_recorder?.UpdateProcessedState(
MultiWheelC/Experiments/NewControllerTrackingTests.cs
+12
View File
@@ -333,6 +333,12 @@ namespace MultiWheelC
double controlPointRadiusMeters,
WheelFeedbackVehicleStateProvider stateProvider)
{
if (controller.LastCycleTiming.HasValue)
{
recorder.RecordControlCycleTiming(
controller.LastCycleTiming.Value);
}
if (controller.LastVehicleState.HasValue)
{
recorder.UpdateProcessedState(
@@ -690,6 +696,12 @@ namespace MultiWheelC
double controlPointRadiusMeters,
WheelFeedbackVehicleStateProvider stateProvider)
{
if (controller.LastCycleTiming.HasValue)
{
recorder.RecordControlCycleTiming(
controller.LastCycleTiming.Value);
}
if (controller.LastVehicleState.HasValue)
{
recorder.UpdateProcessedState(
MultiWheelC/Experiments/TrackingExperimentRecorder.cs
+160
View File
@@ -9,6 +9,7 @@ using System.Text;
using System.Threading;
using CommonUsage.Chassis;
using MyParking.Shared;
using MultiWheelC.Control.Execution;
using MultiWheelC.StateEstimation;
namespace MultiWheelC
@@ -75,6 +76,27 @@ namespace MultiWheelC
// 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;
@@ -91,9 +113,16 @@ namespace MultiWheelC
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();
@@ -180,6 +209,11 @@ namespace MultiWheelC
// 保存成功后的CSV绝对路径；尚未保存时为空。
public string SavedFilePath { get; private set; }
/// <summary>
/// 获取逐控制周期计时CSV的绝对路径；本次实验没有计时数据时为空。
/// </summary>
public string SavedTimingFilePath { get; private set; }
/// <summary>
/// 获取Clumsy当前运行目录下统一保存轨迹实验CSV的文件夹。
/// </summary>
@@ -245,6 +279,24 @@ namespace MultiWheelC
}
}
/// <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>
@@ -374,9 +426,17 @@ namespace MultiWheelC
CaptureSample();
_stopwatch.Stop();
SaveCsv();
SaveControlCycleTimingCsv();
Console.WriteLine(
$"轨迹实验数据已保存：{SavedFilePath}");
if (!string.IsNullOrWhiteSpace(
SavedTimingFilePath))
{
Console.WriteLine(
"控制周期计时数据已保存：" +
SavedTimingFilePath);
}
}
catch
{
@@ -909,6 +969,106 @@ namespace MultiWheelC
}
}
/// <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)
{