ParkingRobot/MultiWheelC/Movements/MotionPlanExecutor.cs

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())
                    {
                        if (!keepRunning)
                        {
                            break;
                        }

                        yield return true;
                    }

                    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())
                    {
                        if (!keepRunning)
                        {
                            break;
                        }

                        yield return true;
                    }

                    continue;
                }

                throw new NotSupportedException(
                    $"组合运动计划不支持动作段类型：{segment.GetType().FullName}。");
            }

            // 所有子动作均已完成后，才向外层DriveTask发送组合计划结束信号。
            yield return false;
        }
    }
}