feat: integrate trajectory tracking controller runtime

ClumsyPilot/Trajectory/Trajectory2D.cs

@@ -0,0 +1,260 @@
+using System;
+using System.Collections.Generic;
+using System.Collections.ObjectModel;
+using System.Linq;
+using MyParking.Shared;
+
+namespace MultiWheelC.Trajectory
+{
+    /// <summary>
+    /// 保存一条经过基本合法性检查的只读二维参考轨迹。
+    /// </summary>
+    public sealed class Trajectory2D
+    {
+        private const double StartArcLengthToleranceMeters = 1e-9;
+        private const double MinimumSegmentLengthMeters = 1e-6;
+
+        private readonly TrajectoryPoint[] _points;
+        private readonly ReadOnlyCollection<TrajectoryPoint> _readOnlyPoints;
+
+        /// <summary>
+        /// 复制并验证按累计弧长升序排列的参考轨迹点。
+        /// </summary>
+        public Trajectory2D(
+            IEnumerable<TrajectoryPoint> points)
+        {
+            if (points == null)
+            {
+                throw new ArgumentNullException(
+                    nameof(points));
+            }
+
+            _points = points.ToArray();
+
+            if (_points.Length < 2)
+            {
+                throw new ArgumentException(
+                    "二维轨迹至少需要两个轨迹点。",
+                    nameof(points));
+            }
+
+            if (Math.Abs(_points[0].ArcLengthMeters) >
+                StartArcLengthToleranceMeters)
+            {
+                throw new ArgumentException(
+                    "二维轨迹起点的累计弧长必须为0m。",
+                    nameof(points));
+            }
+
+            for (var index = 1;
+                 index < _points.Length;
+                 index++)
+            {
+                ValidateSegment(
+                    _points[index - 1],
+                    _points[index],
+                    index,
+                    nameof(points));
+            }
+
+            _readOnlyPoints =
+                Array.AsReadOnly(_points);
+        }
+
+        /// <summary>
+        /// 获取轨迹点数量。
+        /// </summary>
+        public int Count => _points.Length;
+
+        /// <summary>
+        /// 获取指定索引处的轨迹点。
+        /// </summary>
+        public TrajectoryPoint this[int index] =>
+            _points[index];
+
+        /// <summary>
+        /// 获取不可修改的有序轨迹点集合。
+        /// </summary>
+        public IReadOnlyList<TrajectoryPoint> Points =>
+            _readOnlyPoints;
+
+        /// <summary>
+        /// 获取轨迹起点。
+        /// </summary>
+        public TrajectoryPoint StartPoint =>
+            _points[0];
+
+        /// <summary>
+        /// 获取轨迹终点。
+        /// </summary>
+        public TrajectoryPoint EndPoint =>
+            _points[_points.Length - 1];
+
+        /// <summary>
+        /// 获取轨迹总弧长，单位为m。
+        /// </summary>
+        public double TotalLengthMeters =>
+            EndPoint.ArcLengthMeters;
+
+        /// <summary>
+        /// 根据当前累计弧长计算到轨迹终点的剩余距离。
+        /// </summary>
+        public double GetRemainingDistanceMeters(
+            double arcLengthMeters)
+        {
+            EnsureFinite(
+                arcLengthMeters,
+                nameof(arcLengthMeters));
+
+            if (arcLengthMeters <= 0.0)
+                return TotalLengthMeters;
+
+            if (arcLengthMeters >= TotalLengthMeters)
+                return 0.0;
+
+            return TotalLengthMeters - arcLengthMeters;
+        }
+
+        /// <summary>
+        /// 按累计弧长在线性位置、航向、曲率和参考速度之间插值得到轨迹点。
+        /// </summary>
+        public TrajectoryPoint SampleAtArcLength(
+            double arcLengthMeters)
+        {
+            EnsureFinite(
+                arcLengthMeters,
+                nameof(arcLengthMeters));
+
+            if (arcLengthMeters <= 0.0)
+            {
+                return StartPoint;
+            }
+
+            if (arcLengthMeters >= TotalLengthMeters)
+            {
+                return EndPoint;
+            }
+
+            var segmentStartIndex =
+                FindSegmentStartIndex(
+                    arcLengthMeters);
+            var segmentStart =
+                _points[segmentStartIndex];
+            var segmentEnd =
+                _points[segmentStartIndex + 1];
+            var interpolationRatio =
+                (arcLengthMeters -
+                 segmentStart.ArcLengthMeters) /
+                (segmentEnd.ArcLengthMeters -
+                 segmentStart.ArcLengthMeters);
+
+            return new TrajectoryPoint(
+                arcLengthMeters,
+                new Pose2D(
+                    InterpolationMath.Lerp(
+                        segmentStart.PoseInWorld.XMeters,
+                        segmentEnd.PoseInWorld.XMeters,
+                        interpolationRatio),
+                    InterpolationMath.Lerp(
+                        segmentStart.PoseInWorld.YMeters,
+                        segmentEnd.PoseInWorld.YMeters,
+                        interpolationRatio),
+                    AngleMath.LerpRadians(
+                        segmentStart.PoseInWorld.YawRadians,
+                        segmentEnd.PoseInWorld.YawRadians,
+                        interpolationRatio)),
+                InterpolationMath.Lerp(
+                    segmentStart.CurvaturePerMeter,
+                    segmentEnd.CurvaturePerMeter,
+                    interpolationRatio),
+                InterpolationMath.Lerp(
+                    segmentStart.ReferenceSpeedMetersPerSecond,
+                    segmentEnd.ReferenceSpeedMetersPerSecond,
+                    interpolationRatio));
+        }
+
+        /// <summary>
+        /// 使用二分查找获取包含指定累计弧长的线段起点索引。
+        /// </summary>
+        private int FindSegmentStartIndex(
+            double arcLengthMeters)
+        {
+            var lowerIndex = 0;
+            var upperIndex = _points.Length - 1;
+
+            while (upperIndex - lowerIndex > 1)
+            {
+                var middleIndex =
+                    lowerIndex +
+                    (upperIndex - lowerIndex) / 2;
+
+                if (_points[middleIndex].ArcLengthMeters <=
+                    arcLengthMeters)
+                {
+                    lowerIndex = middleIndex;
+                }
+                else
+                {
+                    upperIndex = middleIndex;
+                }
+            }
+
+            return lowerIndex;
+        }
+
+        /// <summary>
+        /// 检查相邻轨迹点是否构成有效的非零长度有序线段。
+        /// </summary>
+        private static void ValidateSegment(
+            TrajectoryPoint previous,
+            TrajectoryPoint current,
+            int currentIndex,
+            string parameterName)
+        {
+            if (current.ArcLengthMeters <=
+                previous.ArcLengthMeters)
+            {
+                throw new ArgumentException(
+                    $"轨迹点{currentIndex}的累计弧长必须严格大于前一个点。",
+                    parameterName);
+            }
+
+            var deltaX =
+                current.PoseInWorld.XMeters -
+                previous.PoseInWorld.XMeters;
+            var deltaY =
+                current.PoseInWorld.YMeters -
+                previous.PoseInWorld.YMeters;
+            var segmentLengthSquared =
+                deltaX * deltaX +
+                deltaY * deltaY;
+            var minimumLengthSquared =
+                MinimumSegmentLengthMeters *
+                MinimumSegmentLengthMeters;
+
+            if (segmentLengthSquared <
+                minimumLengthSquared)
+            {
+                throw new ArgumentException(
+                    $"轨迹点{currentIndex}与前一个点的位置过近，无法构成有效投影线段。",
+                    parameterName);
+            }
+        }
+
+        /// <summary>
+        /// 检查数值是否为有限值。
+        /// </summary>
+        private static void EnsureFinite(
+            double value,
+            string parameterName)
+        {
+            if (double.IsNaN(value) ||
+                double.IsInfinity(value))
+            {
+                throw new ArgumentOutOfRangeException(
+                    parameterName,
+                    "轨迹弧长必须是有限值。");
+            }
+        }
+    }
+}