feat: 发布 EM 轨迹规划首个版本

2026-08-11 20:35:59 +08:00
parent 569de5f13c
commit 1903e71fc1
522 changed files with 4188 additions and 119188 deletions
@@ -0,0 +1,47 @@
+using System;
+using System.Collections.Generic;
+using System.Globalization;
+
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+internal sealed class LongitudinalSolveTrace
+{
+    private const int MaximumEntries = 12;
+    private readonly List<string> _entries = new List<string>(MaximumEntries);
+
+    internal void Add(string phase, int callOrdinal, int anchorUpdateIndex, double trustScale,
+        TimeSpan remainingBudget, TimeSpan remainingAfterReserve, TimeSpan elapsed, QpSolveResult result,
+        double anchorObjective, double candidateObjective, string rejection)
+    {
+        if (_entries.Count >= MaximumEntries)
+            throw new InvalidOperationException("Longitudinal solve trace exceeded the twelve-call cap.");
+        string status = result == null ? "null" : result.Status.ToString();
+        int iterations = result == null ? -1 : result.Iterations;
+        double primal = result == null ? double.NaN : result.PrimalResidual;
+        double dual = result == null ? double.NaN : result.DualResidual;
+        _entries.Add("phase:" + phase +
+            ",call:" + callOrdinal.ToString(CultureInfo.InvariantCulture) +
+            ",anchor:" + anchorUpdateIndex.ToString(CultureInfo.InvariantCulture) +
+            ",scale:" + trustScale.ToString("R", CultureInfo.InvariantCulture) +
+            ",budgetMs:" + remainingBudget.TotalMilliseconds.ToString("F3", CultureInfo.InvariantCulture) +
+            ",remainingAfterReserveMs:" + remainingAfterReserve.TotalMilliseconds.ToString(
+                "F3", CultureInfo.InvariantCulture) +
+            ",elapsedMs:" + elapsed.TotalMilliseconds.ToString("F3", CultureInfo.InvariantCulture) +
+            ",status:" + status +
+            ",iterations:" + iterations.ToString(CultureInfo.InvariantCulture) +
+            ",primal:" + primal.ToString("R", CultureInfo.InvariantCulture) +
+            ",dual:" + dual.ToString("R", CultureInfo.InvariantCulture) +
+            ",anchorObj:" + anchorObjective.ToString("R", CultureInfo.InvariantCulture) +
+            ",candidateObj:" + candidateObjective.ToString("R", CultureInfo.InvariantCulture) +
+            ",rejection:" + Sanitize(rejection));
+    }
+
+    internal string Format() => string.Join("|", _entries);
+
+    private static string Sanitize(string text)
+    {
+        if (string.IsNullOrEmpty(text))
+            return "none";
+        return text.Replace(';', '/').Replace('|', '/').Replace('\r', '/').Replace('\n', '/');
+    }
+}