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/tests/Flyshot.Core.Tests/J519SendTrajectorySamplerTests.cs
+++ b/tests/Flyshot.Core.Tests/J519SendTrajectorySamplerTests.cs
@@ -0,0 +1,233 @@
+using Flyshot.Core.Planning.Sampling;
+using Flyshot.Core.Domain;
+using Flyshot.Core.Planning;
+
+namespace Flyshot.Core.Tests;
+
+/// <summary>
+/// 验证 J519 实发重采样器在离线导出和运行时下发之间保持一致的时间轴语义。
+/// </summary>
+public sealed class J519SendTrajectorySamplerTests
+{
+    /// <summary>
+    /// 验证 speed_ratio 只缩放轨迹时间，物理发送时间仍按固定伺服周期推进。
+    /// </summary>
+    [Fact]
+    public void SampleDenseJointTrajectory_MapsSendTimeToScaledTrajectoryTimeAndDegrees()
+    {
+        var denseTrajectory = new[]
+        {
+            new[] { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 },
+            new[] { 0.008, Math.PI / 2.0, 0.0, 0.0, 0.0, 0.0, 0.0 },
+            new[] { 0.016, Math.PI, 0.0, 0.0, 0.0, 0.0, 0.0 }
+        };
+
+        var samples = J519SendTrajectorySampler.SampleDenseJointTrajectory(
+            denseTrajectory,
+            durationSeconds: 0.016,
+            servoPeriodSeconds: 0.008,
+            speedRatio: 0.5);
+
+        Assert.Equal(5, samples.Count);
+        Assert.Equal(0, samples[0].SampleIndex);
+        Assert.Equal(0.0, samples[0].SendTime, precision: 6);
+        Assert.Equal(0.0, samples[0].TrajectoryTime, precision: 6);
+        Assert.Equal(0.0, samples[0].JointsDegrees[0], precision: 6);
+
+        Assert.Equal(1, samples[1].SampleIndex);
+        Assert.Equal(0.008, samples[1].SendTime, precision: 6);
+        Assert.Equal(0.004, samples[1].TrajectoryTime, precision: 6);
+        Assert.Equal(45.0, samples[1].JointsDegrees[0], precision: 6);
+
+        Assert.Equal(4, samples[^1].SampleIndex);
+        Assert.Equal(0.032, samples[^1].SendTime, precision: 6);
+        Assert.Equal(0.016, samples[^1].TrajectoryTime, precision: 6);
+        Assert.Equal(180.0, samples[^1].JointsDegrees[0], precision: 6);
+    }
+
+    /// <summary>
+    /// 验证飞拍 speed_ratio 安全队列从连续规划样条直接采样，而不是先把规划轨迹离散成稠密点后再线性插值。
+    /// </summary>
+    [Fact]
+    public void SamplePlannedTrajectory_DirectlyEvaluatesContinuousSplineForSpeedRatio()
+    {
+        var trajectory = CreateSingleJointTrajectory();
+
+        var samples = J519SendTrajectorySampler.SamplePlannedTrajectory(
+            trajectory,
+            servoPeriodSeconds: 0.008,
+            speedRatio: 0.5);
+
+        var sample = Assert.Single(samples.Where(static item => Math.Abs(item.TrajectoryTime - 0.004) < 1e-9));
+        Assert.Equal(0.008, sample.SendTime, precision: 6);
+        Assert.Equal(0.004, sample.TrajectoryTime, precision: 6);
+        Assert.NotEqual(45.0, sample.JointsDegrees[0], precision: 3);
+
+        var expected = new CubicSplineInterpolator(
+            trajectory.WaypointTimes.ToArray(),
+            trajectory.PlannedWaypoints.Select(static waypoint => waypoint.Positions.ToArray()).ToArray())
+            .Evaluate(0.004)[0] * 180.0 / Math.PI;
+        Assert.Equal(expected, sample.JointsDegrees[0], precision: 6);
+    }
+
+    /// <summary>
+    /// 验证 speed_ratio 等价于执行侧时间轴拉长：原轨迹 0.8 倍执行应与时间整体拉长 1/0.8 的轨迹 1 倍执行一致。
+    /// </summary>
+    [Fact]
+    public void SamplePlannedTrajectory_SpeedRatioMatchesUniformPlanningTimeStretch()
+    {
+        var trajectory = CreateSingleJointTrajectory();
+        var stretchedTrajectory = StretchTrajectoryTime(trajectory, stretchFactor: 1.0 / 0.8);
+
+        var ratioSamples = J519SendTrajectorySampler.SamplePlannedTrajectory(
+            trajectory,
+            servoPeriodSeconds: 0.008,
+            speedRatio: 0.8);
+        var stretchedSamples = J519SendTrajectorySampler.SamplePlannedTrajectory(
+            stretchedTrajectory,
+            servoPeriodSeconds: 0.008,
+            speedRatio: 1.0);
+
+        Assert.Equal(stretchedSamples.Count, ratioSamples.Count);
+        for (var index = 0; index < ratioSamples.Count; index++)
+        {
+            Assert.Equal(stretchedSamples[index].SendTime, ratioSamples[index].SendTime, precision: 6);
+            Assert.Equal(stretchedSamples[index].TrajectoryTime * 0.8, ratioSamples[index].TrajectoryTime, precision: 6);
+            Assert.Equal(stretchedSamples[index].JointsDegrees[0], ratioSamples[index].JointsDegrees[0], precision: 6);
+        }
+    }
+
+    /// <summary>
+    /// 验证空稠密轨迹会直接暴露为调用错误，避免生成无意义下发点。
+    /// </summary>
+    [Fact]
+    public void SampleDenseJointTrajectory_RejectsEmptyDenseTrajectory()
+    {
+        var exception = Assert.Throws<InvalidOperationException>(() =>
+            J519SendTrajectorySampler.SampleDenseJointTrajectory(
+                Array.Empty<IReadOnlyList<double>>(),
+                durationSeconds: 0.016,
+                servoPeriodSeconds: 0.008,
+                speedRatio: 1.0));
+
+        Assert.Contains("稠密关节轨迹为空", exception.Message);
+    }
+
+    /// <summary>
+    /// 验证非法 speed_ratio 会在公共入口统一拦截。
+    /// </summary>
+    [Theory]
+    [InlineData(0.0)]
+    [InlineData(double.NaN)]
+    [InlineData(double.PositiveInfinity)]
+    public void SampleDenseJointTrajectory_RejectsInvalidSpeedRatio(double speedRatio)
+    {
+        var denseTrajectory = new[]
+        {
+            new[] { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }
+        };
+
+        Assert.Throws<ArgumentOutOfRangeException>(() =>
+            J519SendTrajectorySampler.SampleDenseJointTrajectory(
+                denseTrajectory,
+                durationSeconds: 0.0,
+                servoPeriodSeconds: 0.008,
+                speedRatio: speedRatio));
+    }
+
+    /// <summary>
+    /// 验证公共诊断行格式与既有 ActualSendTiming 文件保持一致。
+    /// </summary>
+    [Fact]
+    public void BuildTimingRow_UsesLegacyActualSendColumnOrder()
+    {
+        var row = J519SendTrajectorySampler.BuildTimingRow(new J519SendSample(
+            sampleIndex: 2,
+            sendTime: 0.016,
+            trajectoryTime: 0.008,
+            speedRatio: 0.5,
+            jointsDegrees: [90.0, 0.0, 0.0, 0.0, 0.0, 0.0]));
+
+        Assert.Equal([2.0, 0.016, 0.008, 0.5], row);
+    }
+
+    /// <summary>
+    /// 验证倍率诊断行在保留旧 4 列的同时追加请求倍率和历史改写次数。
+    /// </summary>
+    [Fact]
+    public void BuildTimingRow_WithRatioDiagnostics_AppendsRequestedRatioAndIterationCount()
+    {
+        var sample = new J519SendSample(
+            sampleIndex: 2,
+            sendTime: 0.016,
+            trajectoryTime: 0.0076,
+            speedRatio: 0.475,
+            jointsDegrees: [90.0, 0.0, 0.0, 0.0, 0.0, 0.0]);
+
+        var row = J519SendTrajectorySampler.BuildTimingRow(
+            sample,
+            requestedSpeedRatio: 0.5,
+            stretchIterationCount: 1);
+
+        Assert.Equal([2.0, 0.016, 0.0076, 0.475, 0.5, 1.0], row);
+    }
+
+    /// <summary>
+    /// 构造一个最小单关节规划轨迹，便于验证 speed_ratio 直接样条采样语义。
+    /// </summary>
+    private static PlannedTrajectory CreateSingleJointTrajectory()
+    {
+        var robot = new RobotProfile(
+            name: "TestRobot",
+            modelPath: "Models/Test.robot",
+            degreesOfFreedom: 1,
+            jointLimits: [new JointLimit("J1", 100.0, 1000.0, 10000.0)],
+            jointCouplings: Array.Empty<JointCoupling>(),
+            servoPeriod: TimeSpan.FromMilliseconds(8),
+            triggerPeriod: TimeSpan.FromMilliseconds(8));
+        var program = new FlyshotProgram(
+            name: "spline-sample",
+            waypoints:
+            [
+                new JointWaypoint([0.0]),
+                new JointWaypoint([Math.PI / 2.0]),
+                new JointWaypoint([Math.PI])
+            ],
+            shotFlags: [false, false, false],
+            offsetValues: [0, 0, 0],
+            addressGroups:
+            [
+                new IoAddressGroup(Array.Empty<int>()),
+                new IoAddressGroup(Array.Empty<int>()),
+                new IoAddressGroup(Array.Empty<int>())
+            ]);
+
+        return new PlannedTrajectory(
+            robot: robot,
+            originalProgram: program,
+            plannedWaypoints: program.Waypoints,
+            waypointTimes: [0.0, 0.008, 0.016],
+            segmentDurations: [0.008, 0.008],
+            segmentScales: [1.0, 1.0],
+            method: PlanningMethod.Icsp,
+            iterations: 1,
+            threshold: 0.0);
+    }
+
+    /// <summary>
+    /// 构造统一拉长时间轴后的轨迹，模拟更低 planning_speed_scale 生成的等几何时间结果。
+    /// </summary>
+    private static PlannedTrajectory StretchTrajectoryTime(PlannedTrajectory trajectory, double stretchFactor)
+    {
+        return new PlannedTrajectory(
+            robot: trajectory.Robot,
+            originalProgram: trajectory.OriginalProgram,
+            plannedWaypoints: trajectory.PlannedWaypoints,
+            waypointTimes: trajectory.WaypointTimes.Select(time => time * stretchFactor).ToArray(),
+            segmentDurations: trajectory.SegmentDurations.Select(duration => duration * stretchFactor).ToArray(),
+            segmentScales: trajectory.SegmentScales.Select(scale => scale / stretchFactor).ToArray(),
+            method: trajectory.Method,
+            iterations: trajectory.Iterations,
+            threshold: trajectory.Threshold);
+    }
+}