泛化轨迹工厂参数并新增组合运动计划执行器与复合测试

2026-08-07 13:25:47 +08:00
parent 14ca1150e4
commit bc37e71ad0
26 changed files with 874 additions and 92 deletions
@@ -0,0 +1,233 @@
+using System;
+using System.Collections.Generic;
+using ClumsyCore.Interfaces;
+using ClumsyCore.Pilot;
+using MDCSToolBox.Commons.Controllers;
+using MultiWheelC.Control.Execution;
+using MultiWheelC.StateEstimation;
+using MultiWheelC.Trajectory;
+using MyParking.Shared;
+
+namespace MultiWheelC
+{
+    /// <summary>
+    /// 表示组合运动计划中由一种控制方式完整执行的单个动作段。
+    /// </summary>
+    public abstract class MotionPlanSegment
+    {
+    }
+
+    /// <summary>
+    /// 表示使用新版几何控制器跟踪一条连续二维轨迹的动作段。
+    /// </summary>
+    public sealed class TrackMotionPlanSegment : MotionPlanSegment
+    {
+        public TrackMotionPlanSegment(Trajectory2D trajectory)
+        {
+            Trajectory = trajectory ??
+                throw new ArgumentNullException(nameof(trajectory));
+        }
+
+        public Trajectory2D Trajectory { get; }
+
+        /// <summary>
+        /// 获取或设置本段轨迹独立的终点距离容差；为空时沿用轨迹动作默认值。
+        /// </summary>
+        public double? FinishDistanceMeters { get; set; }
+
+        /// <summary>
+        /// 获取或设置本段轨迹独立的停车速度容差；为空时沿用轨迹动作默认值。
+        /// </summary>
+        public double? FinishSpeedMetersPerSecond { get; set; }
+
+        /// <summary>
+        /// 获取或设置本段轨迹独立的终点航向容差；为空时沿用轨迹动作默认值。
+        /// </summary>
+        public double? FinishHeadingToleranceRadians { get; set; }
+    }
+
+    /// <summary>
+    /// 表示车辆停车后原地旋转到指定世界航向的动作段。
+    /// </summary>
+    public sealed class RotateInPlaceMotionPlanSegment
+        : MotionPlanSegment
+    {
+        public RotateInPlaceMotionPlanSegment(
+            double targetYawRadians)
+        {
+            if (double.IsNaN(targetYawRadians) ||
+                double.IsInfinity(targetYawRadians))
+            {
+                throw new ArgumentOutOfRangeException(
+                    nameof(targetYawRadians),
+                    "原地自转目标航向必须是有限值。");
+            }
+
+            TargetYawRadians =
+                AngleMath.NormalizeRadians(targetYawRadians);
+        }
+
+        public double TargetYawRadians { get; }
+    }
+
+    /// <summary>
+    /// 顺序执行连续轨迹和原地自转动作，并在动作段边界完成停车与控制器切换。
+    /// </summary>
+    public sealed class MotionPlanExecutor : MovementDefinition
+    {
+        /// <summary>
+        /// 获取或设置一次性提交并按顺序执行的组合运动计划。
+        /// </summary>
+        public IReadOnlyList<MotionPlanSegment> Segments;
+
+        /// <summary>
+        /// 获取或设置所有动作段共享的车辆状态源；为空时使用Detour状态源。
+        /// </summary>
+        public IVehicleStateProvider StateProvider;
+
+        /// <summary>
+        /// 获取或设置创建每段轨迹动作后应用参数的回调。
+        /// </summary>
+        public Action<TrajectoryTrackingMovement>
+            ConfigureTrackingMovement;
+
+        /// <summary>
+        /// 获取或设置创建每段原地自转动作后应用参数的回调。
+        /// </summary>
+        public Action<MultiWheelRotateInPlace>
+            ConfigureRotationMovement;
+
+        /// <summary>
+        /// 获取或设置动作段开始前的通知，参数依次为索引和动作段。
+        /// </summary>
+        public Action<int, MotionPlanSegment> SegmentStarted;
+
+        /// <summary>
+        /// 获取或设置轨迹段每个有效控制周期后的诊断通知。
+        /// </summary>
+        public Action<int, ParkingGeometricController>
+            TrackingCycleObserver;
+
+        /// <summary>
+        /// 获取或设置自转段角速度命令通知，角速度单位为rad/s。
+        /// </summary>
+        public Action<int, double> RotationCommandObserver;
+
+        /// <summary>
+        /// 按计划顺序执行各动作段，任一动作失败时停止后续动作。
+        /// </summary>
+        public override IEnumerable<bool> Get()
+        {
+            if (Segments == null || Segments.Count == 0)
+            {
+                throw new InvalidOperationException(
+                    "组合运动计划至少需要包含一个动作段。");
+            }
+
+            var stateProvider =
+                StateProvider ??
+                new DetourVehicleStateProvider();
+
+            for (var index = 0;
+                 index < Segments.Count;
+                 index++)
+            {
+                var segment = Segments[index] ??
+                    throw new InvalidOperationException(
+                        $"组合运动计划第{index}段为空。");
+
+                SegmentStarted?.Invoke(index, segment);
+
+                if (segment is TrackMotionPlanSegment track)
+                {
+                    var movement =
+                        new TrajectoryTrackingMovement
+                        {
+                            Trajectory = track.Trajectory,
+                            StateProvider = stateProvider,
+                            CycleObserver = controller =>
+                                TrackingCycleObserver?.Invoke(
+                                    index,
+                                    controller)
+                        };
+                    ConfigureTrackingMovement?.Invoke(movement);
+
+                    // 单段参数后应用，确保中间连接段可以覆盖组合动作的公共配置。
+                    if (track.FinishDistanceMeters.HasValue)
+                    {
+                        movement.FinishDistanceMeters =
+                            track.FinishDistanceMeters.Value;
+                    }
+
+                    if (track.FinishSpeedMetersPerSecond.HasValue)
+                    {
+                        movement.FinishSpeedMetersPerSecond =
+                            track.FinishSpeedMetersPerSecond.Value;
+                    }
+
+                    if (track.FinishHeadingToleranceRadians.HasValue)
+                    {
+                        movement.FinishHeadingToleranceRadians =
+                            track.FinishHeadingToleranceRadians.Value;
+                    }
+
+                    foreach (var keepRunning in movement.Get())
+                    {
+                        yield return keepRunning;
+                    }
+
+                    continue;
+                }
+
+                if (segment is RotateInPlaceMotionPlanSegment rotate)
+                {
+                    var config = PilotDefinition.Conf;
+                    var movement =
+                        new MultiWheelRotateInPlace
+                        {
+                            AngleTarget =
+                                (float)AngleMath.RadiansToDegrees(
+                                    rotate.TargetYawRadians),
+                            StateProvider = stateProvider,
+                            PidparamsRead = () => new PIDParams
+                            {
+                                Kp = config.InPlaceRotateKp,
+                                Ki = config.InPlaceRotateKi,
+                                Kd = config.InPlaceRotateKd,
+                                DeadZone =
+                                    config.InPlaceRotateArriveDeg,
+                                SpeedAccPerSec =
+                                    config.InPlaceRotateAcc,
+                                OutputUpperThreshold =
+                                    config.InPlaceRotateMaxSpeed,
+                                MaxI = config.InPlaceRotateMaxI
+                            },
+                            MinimumAngularSpeedDegreesPerSecond =
+                                config.InPlaceRotateMinimumSpeed,
+                            WheelAlignmentToleranceDegrees =
+                                config.InPlaceRotateWheelAlignDeg,
+                            RotationTimeoutSeconds =
+                                config.InPlaceRotateTimeoutSec,
+                            CommandAngularSpeedObserver =
+                                commandDegreesPerSecond =>
+                                    RotationCommandObserver?.Invoke(
+                                        index,
+                                        AngleMath.DegreesToRadians(
+                                            commandDegreesPerSecond))
+                        };
+                    ConfigureRotationMovement?.Invoke(movement);
+
+                    foreach (var keepRunning in movement.Get())
+                    {
+                        yield return keepRunning;
+                    }
+
+                    continue;
+                }
+
+                throw new NotSupportedException(
+                    $"组合运动计划不支持动作段类型：{segment.GetType().FullName}。");
+            }
+        }
+    }
+}