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

MultiWheelC/Experiments/TrackingExperimentRecorder.cs

@@ -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)
         {