feat: integrate trajectory tracking controller runtime

2026-08-10 13:26:56 +08:00
parent f923affc8f
commit 54ef239231
29 changed files with 5135 additions and 0 deletions
@@ -0,0 +1,123 @@
+using System;
+using MyParking.Shared;
+
+namespace MultiWheelC.Trajectory
+{
+    /// <summary>
+    /// 保存车体中心投影到二维参考轨迹后得到的只读结果。
+    /// </summary>
+    public readonly struct TrajectoryProjection
+    {
+        /// <summary>
+        /// 创建包含轨迹进度、参考状态和跟踪误差的投影结果。
+        /// </summary>
+        public TrajectoryProjection(
+            int segmentStartIndex,
+            TrajectoryPoint referencePoint,
+            double lateralErrorMeters,
+            double headingErrorRadians,
+            double distanceToTrajectoryMeters,
+            double remainingDistanceMeters)
+        {
+            if (segmentStartIndex < 0)
+            {
+                throw new ArgumentOutOfRangeException(
+                    nameof(segmentStartIndex),
+                    "投影线段起点索引不能为负数。");
+            }
+
+            EnsureFinite(
+                lateralErrorMeters,
+                nameof(lateralErrorMeters));
+            EnsureFinite(
+                headingErrorRadians,
+                nameof(headingErrorRadians));
+            EnsureFiniteNonNegative(
+                distanceToTrajectoryMeters,
+                nameof(distanceToTrajectoryMeters));
+            EnsureFiniteNonNegative(
+                remainingDistanceMeters,
+                nameof(remainingDistanceMeters));
+
+            SegmentStartIndex = segmentStartIndex;
+            ReferencePoint = referencePoint;
+            LateralErrorMeters = lateralErrorMeters;
+            HeadingErrorRadians =
+                AngleMath.NormalizeRadians(
+                    headingErrorRadians);
+            DistanceToTrajectoryMeters =
+                distanceToTrajectoryMeters;
+            RemainingDistanceMeters =
+                remainingDistanceMeters;
+        }
+
+        /// <summary>
+        /// 获取投影所在轨迹线段的起点索引，线段终点索引为该值加1。
+        /// </summary>
+        public int SegmentStartIndex { get; }
+
+        /// <summary>
+        /// 获取投影位置插值得到的车体中心参考轨迹点。
+        /// </summary>
+        public TrajectoryPoint ReferencePoint { get; }
+
+        /// <summary>
+        /// 获取有符号横向误差，单位为m，参考轨迹位于车辆左侧时为正。
+        /// </summary>
+        public double LateralErrorMeters { get; }
+
+        /// <summary>
+        /// 获取参考航向减实际车体航向的最短角差，单位为rad，逆时针为正。
+        /// </summary>
+        public double HeadingErrorRadians { get; }
+
+        /// <summary>
+        /// 获取车体中心到投影点的欧氏距离，单位为m。
+        /// </summary>
+        public double DistanceToTrajectoryMeters { get; }
+
+        /// <summary>
+        /// 获取投影位置沿轨迹到终点的剩余弧长，单位为m。
+        /// </summary>
+        public double RemainingDistanceMeters { get; }
+
+        /// <summary>
+        /// 获取投影位置从轨迹起点累计的弧长，单位为m。
+        /// </summary>
+        public double ArcLengthMeters =>
+            ReferencePoint.ArcLengthMeters;
+
+        /// <summary>
+        /// 检查数值是否为有限值。
+        /// </summary>
+        private static void EnsureFinite(
+            double value,
+            string parameterName)
+        {
+            if (double.IsNaN(value) ||
+                double.IsInfinity(value))
+            {
+                throw new ArgumentOutOfRangeException(
+                    parameterName,
+                    "轨迹投影参数必须是有限值。");
+            }
+        }
+
+        /// <summary>
+        /// 检查数值是否为非负有限值。
+        /// </summary>
+        private static void EnsureFiniteNonNegative(
+            double value,
+            string parameterName)
+        {
+            EnsureFinite(value, parameterName);
+
+            if (value < 0.0)
+            {
+                throw new ArgumentOutOfRangeException(
+                    parameterName,
+                    "轨迹投影距离不能为负数。");
+            }
+        }
+    }
+}