feat(*): 添加 J519 实发重采样与 JSON 机型模型

* 新增 J519 实发采样器,按 8ms 周期生成 timing/jerk 诊断行并完成 rad->deg 转换
* 兼容层产物导出补充 speedRatio,规划编排补齐 smoothStartStopTiming 与日志透传
* 配置与机型加载切换到运行目录 JSON 模型,并补齐 7L 展开模型与相关单元测试
This commit is contained in:
2026-05-07 17:08:32 +08:00
parent 70b0ccd414
commit c6829d214a
26 changed files with 1417 additions and 409 deletions

View File

@@ -0,0 +1,99 @@
using Flyshot.Core.Planning.Sampling;
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>
/// 验证空稠密轨迹会直接暴露为调用错误,避免生成无意义下发点。
/// </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);
}
}