Merge commit '0952958af7685646cdbf2a4907c6e2ae59d4d352'

2026-05-28 10:44:03 +08:00
parent 4eeaa3fef3
commit 35bdb08d11
144 changed files with 33730 additions and 392 deletions
--- a/src/Flyshot.Core.Planning/Sampling/FlyshotExecutionSendSequenceBuilder.cs
+++ b/src/Flyshot.Core.Planning/Sampling/FlyshotExecutionSendSequenceBuilder.cs
@@ -0,0 +1,146 @@
+using Flyshot.Core.Domain;
+
+namespace Flyshot.Core.Planning.Sampling;
+
+/// <summary>
+/// 负责在飞拍进入运行时前构建最终 8ms 发送队列，并执行发送口径的离散限幅校验。
+/// </summary>
+public static class FlyshotExecutionSendSequenceBuilder
+{
+    /// <summary>
+    /// 根据连续规划轨迹构建最终发送队列；请求 speedRatio 只作为诊断信息保留。
+    /// </summary>
+    /// <param name="robot">机器人关节限值配置。</param>
+    /// <param name="plannedTrajectory">规划阶段产出的连续轨迹。</param>
+    /// <param name="result">规划结果。</param>
+    /// <param name="servoPeriodSeconds">J519 物理发送周期，单位为秒。</param>
+    /// <param name="requestedSpeedRatio">请求的执行倍率，必须已经在规划阶段折算进轨迹。</param>
+    /// <param name="samplingSpeedRatio">最终队列采样倍率；为空时使用 1.0，表示直接采样已调速后的规划轨迹。</param>
+    /// <returns>通过离散校验后的飞拍最终发送结果。</returns>
+    public static FlyshotPreparedExecution Build(
+        RobotProfile robot,
+        PlannedTrajectory plannedTrajectory,
+        TrajectoryResult result,
+        double servoPeriodSeconds,
+        double requestedSpeedRatio,
+        double? samplingSpeedRatio = null)
+    {
+        ArgumentNullException.ThrowIfNull(robot);
+        ArgumentNullException.ThrowIfNull(plannedTrajectory);
+        ArgumentNullException.ThrowIfNull(result);
+
+        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 finalSpeedRatio = samplingSpeedRatio ?? 1.0;
+        if (finalSpeedRatio <= 0.0 || double.IsNaN(finalSpeedRatio) || double.IsInfinity(finalSpeedRatio))
+        {
+            throw new ArgumentOutOfRangeException(nameof(samplingSpeedRatio), "最终队列采样倍率必须是有限正数。");
+        }
+
+        var samples = J519SendTrajectorySampler.SamplePlannedTrajectory(
+            plannedTrajectory,
+            servoPeriodSeconds,
+            finalSpeedRatio);
+
+        try
+        {
+            TrajectoryLimitValidator.ValidateJ519SendSamples(
+                robot,
+                samples,
+                trajectoryName: result.ProgramName,
+                validateJerk: false,
+                strictJerkTolerance: true);
+        }
+        catch (InvalidOperationException exception)
+        {
+            throw new InvalidOperationException(
+                $"飞拍最终发送队列在请求 speed_ratio={requestedSpeedRatio:F6} 下离散限幅校验失败。" +
+                "请降低 planning_speed_scale 或 speed_ratio 后重新规划；执行侧不会在发送阶段临场重采样。",
+                exception);
+        }
+
+        return BuildPreparedExecution(
+            result,
+            samples,
+            requestedSpeedRatio,
+            finalSpeedRatio,
+            stretchIterationCount: 0);
+    }
+
+    /// <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(sample => J519SendTrajectorySampler.BuildTimingRow(sample, requestedSpeedRatio, stretchIterationCount))
+            .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;
+    }
+}