✨ feat(flyshot): 引入飞拍执行侧最终发送队列构建与校验机制

* 新增 FlyshotExecutionSendSequenceBuilder，负责在运行时前构建最终 8ms 发送队列，并进行离散限幅校验。 * 引入 FlyshotPreparedExecution 类，封装最终发送结果及相关诊断信息。 * 调整 ControllerClientCompatService 和 FanucControllerRuntime，确保运行时直接使用预生成的发送队列，避免临场重采样。 * 更新 TrajectoryResult 和 PlannedExecutionBundle，支持准备好的执行队列。 * 增加单元测试，验证非 1 倍 speedRatio 下的执行行为与预生成队列的使用。
2026-05-09 19:06:49 +08:00
parent f7e2bb0e7b
commit 74761bb5da
11 changed files with 1185 additions and 75 deletions
--- a/src/Flyshot.Core.Planning/Sampling/FlyshotExecutionSendSequenceBuilder.cs
+++ b/src/Flyshot.Core.Planning/Sampling/FlyshotExecutionSendSequenceBuilder.cs
@@ -0,0 +1,161 @@
+using Flyshot.Core.Domain;
+
+namespace Flyshot.Core.Planning.Sampling;
+
+/// <summary>
+/// 负责在飞拍进入运行时前构建最终 8ms 发送队列，并在必要时自动拉长执行时长直到通过离散限幅校验。
+/// </summary>
+public static class FlyshotExecutionSendSequenceBuilder
+{
+    /// <summary>
+    /// 自动拉长时每轮采用的保守倍率缩减系数。
+    /// </summary>
+    private const double StretchFactor = 0.95;
+
+    /// <summary>
+    /// 自动拉长尝试的最大迭代次数。
+    /// </summary>
+    private const int MaxStretchIterations = 16;
+
+    /// <summary>
+    /// 根据规划层稠密轨迹和执行层 speedRatio 构建最终发送队列。
+    /// </summary>
+    /// <param name="robot">机器人关节限值配置。</param>
+    /// <param name="result">规划结果。</param>
+    /// <param name="servoPeriodSeconds">J519 物理发送周期，单位为秒。</param>
+    /// <param name="requestedSpeedRatio">请求的执行倍率。</param>
+    /// <returns>通过离散校验后的飞拍最终发送结果。</returns>
+    public static FlyshotPreparedExecution Build(
+        RobotProfile robot,
+        TrajectoryResult result,
+        double servoPeriodSeconds,
+        double requestedSpeedRatio)
+    {
+        ArgumentNullException.ThrowIfNull(robot);
+        ArgumentNullException.ThrowIfNull(result);
+
+        if (result.DenseJointTrajectory is null)
+        {
+            throw new InvalidOperationException("飞拍执行准备前必须先生成规划层稠密关节轨迹。");
+        }
+
+        if (requestedSpeedRatio <= 0.0 || double.IsNaN(requestedSpeedRatio) || double.IsInfinity(requestedSpeedRatio))
+        {
+            throw new ArgumentOutOfRangeException(nameof(requestedSpeedRatio), "speed_ratio 必须是有限正数。");
+        }
+
+        if (servoPeriodSeconds <= 0.0 || double.IsNaN(servoPeriodSeconds) || double.IsInfinity(servoPeriodSeconds))
+        {
+            throw new ArgumentOutOfRangeException(nameof(servoPeriodSeconds), "伺服周期必须是有限正数。");
+        }
+
+        var effectiveSpeedRatio = requestedSpeedRatio;
+        InvalidOperationException? firstFailure = null;
+
+        for (var iteration = 0; iteration <= MaxStretchIterations; iteration++)
+        {
+            var samples = J519SendTrajectorySampler.SampleDenseJointTrajectory(
+                result.DenseJointTrajectory,
+                result.Duration.TotalSeconds,
+                servoPeriodSeconds,
+                effectiveSpeedRatio);
+
+            try
+            {
+                TrajectoryLimitValidator.ValidateJ519SendSamples(
+                    robot,
+                    samples,
+                    trajectoryName: result.ProgramName);
+
+                return BuildPreparedExecution(
+                    result,
+                    samples,
+                    requestedSpeedRatio,
+                    effectiveSpeedRatio,
+                    iteration);
+            }
+            catch (InvalidOperationException exception)
+            {
+                firstFailure ??= exception;
+                if (iteration >= MaxStretchIterations)
+                {
+                    throw new InvalidOperationException(
+                        $"飞拍最终发送队列离散限幅校验失败，已达到最大自动拉长次数 {MaxStretchIterations}。",
+                        firstFailure);
+                }
+
+                // 只在执行侧进一步保守化 trajectoryTime(sendTime) 映射，不回写规划层轨迹。
+                effectiveSpeedRatio *= StretchFactor;
+            }
+        }
+
+        throw new InvalidOperationException("飞拍最终发送队列构建失败。", firstFailure);
+    }
+
+    /// <summary>
+    /// 将通过校验的 J519 采样点和触发绑定结果封装为稳定的飞拍执行结果。
+    /// </summary>
+    private static FlyshotPreparedExecution BuildPreparedExecution(
+        TrajectoryResult result,
+        IReadOnlyList<J519SendSample> samples,
+        double requestedSpeedRatio,
+        double finalSpeedRatio,
+        int stretchIterationCount)
+    {
+        var preparedSamples = samples.Select(static sample =>
+            new FlyshotPreparedSample(
+                sample.SampleIndex,
+                sample.SendTime,
+                sample.TrajectoryTime,
+                sample.SpeedRatio,
+                sample.JointsDegrees)).ToArray();
+        var sampleLookup = preparedSamples.ToDictionary(static sample => sample.SampleIndex);
+        var triggerBindings = TriggerSampleBinder.Bind(
+            result.TriggerTimeline,
+            samples,
+            result.TriggerSampleIndexOffsetCycles)
+            .Select(binding =>
+                new FlyshotPreparedTriggerBinding(
+                    binding.Trigger,
+                    sampleLookup[binding.Sample.SampleIndex],
+                    binding.SampleIndex,
+                    binding.FoundInWindow))
+            .ToArray();
+        var timingRows = samples.Select(J519SendTrajectorySampler.BuildTimingRow).ToArray();
+        var jerkRows = BuildJerkRows(samples);
+        var finalDurationSeconds = preparedSamples.Length == 0 ? 0.0 : preparedSamples[^1].SendTime;
+
+        return new FlyshotPreparedExecution(
+            preparedSamples,
+            triggerBindings,
+            timingRows,
+            jerkRows,
+            requestedSpeedRatio,
+            finalSpeedRatio,
+            finalDurationSeconds,
+            stretchIterationCount);
+    }
+
+    /// <summary>
+    /// 为最终发送点列构建跃度诊断行，保证导出工件和运行时发送复用同一份数据。
+    /// </summary>
+    private static IReadOnlyList<IReadOnlyList<double>> BuildJerkRows(IReadOnlyList<J519SendSample> samples)
+    {
+        var jerkRows = new List<IReadOnlyList<double>>(Math.Max(0, samples.Count - 1));
+        double[]? previousVelocity = null;
+        double[]? previousAcceleration = null;
+
+        for (var index = 1; index < samples.Count; index++)
+        {
+            jerkRows.Add(J519SendTrajectorySampler.BuildJerkRow(
+                samples[index - 1].SendTime,
+                samples[index].SendTime,
+                samples[index - 1].JointsDegrees,
+                samples[index].JointsDegrees,
+                ref previousVelocity,
+                ref previousAcceleration));
+        }
+
+        return jerkRows;
+    }
+}