xsxiaosa/FlyShotHost
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

✨ feat(fanuc): 打通飞拍轨迹完整执行链路

Browse Source 
* 增加 J519 稠密发送采样校验与保姿回发逻辑
* 调整 saveTrajectory 导出与 sequence buffer 行为
* 补充 10010 解析脚本、ICSP 说明和回归测试
This commit is contained in:
yunxiao.zhu
2026-05-08 13:25:02 +08:00
parent c6829d214a
commit 1779067b5c
13 changed files with 1363 additions and 33 deletions
Download Patch File Download Diff File Expand all files Collapse all files

94
tests/Flyshot.Core.Tests/FanucJ519ClientTests.cs
View File

@@ -163,6 +163,29 @@ public sealed class FanucJ519ClientTests : IDisposable
await client.StopMotionAsync(_cts.Token);
}
/// <summary>
/// 验证配置 J519 buffer size 后，实际回发命令序号会在状态包序号基础上增加该缓冲深度。
/// </summary>
[Fact]
public async Task StartMotion_AddsConfiguredBufferSizeToStatusSequence()
{
using var client = new FanucJ519Client();
await client.ConnectAsync("127.0.0.1", Port, _cts.Token);
var initResult = await _server.ReceiveAsync(_cts.Token);
client.SetSequenceBufferSize(8);
client.UpdateCommand(new FanucJ519Command(sequence: 1, targetJoints: [1.0, 0.0, 0.0, 0.0, 0.0, 0.0]));
client.StartMotion();
await SendStatusPacketAsync(initResult.RemoteEndPoint, sequence: 100);
var result = await _server.ReceiveAsync(_cts.Token);
Assert.Equal(FanucJ519Protocol.CommandPacketLength, result.Buffer.Length);
Assert.Equal(108u, BinaryPrimitives.ReadUInt32BigEndian(result.Buffer.AsSpan(0x08, 4)));
await client.StopMotionAsync(_cts.Token);
}
/// <summary>
/// 验证连续状态包会逐包驱动命令发送，并使用各自的状态包序号。
/// </summary>
@@ -274,6 +297,61 @@ public sealed class FanucJ519ClientTests : IDisposable
await client.StopMotionAsync(_cts.Token);
}
/// <summary>
/// 验证没有显式目标时，会使用最近一帧状态反馈里的关节角持续构造 hold 命令。
/// </summary>
[Fact]
public async Task StartMotion_WithoutExplicitCommand_HoldsLatestResponseJointDegrees()
{
using var client = new FanucJ519Client();
await client.ConnectAsync("127.0.0.1", Port, _cts.Token);
var initResult = await _server.ReceiveAsync(_cts.Token);
client.StartMotion();
await SendStatusPacketAsync(
initResult.RemoteEndPoint,
sequence: 401,
jointDegrees: [10.0, 20.0, 30.0, 40.0, 50.0, 60.0]);
var holdPacket = await _server.ReceiveAsync(_cts.Token).AsTask().WaitAsync(TimeSpan.FromSeconds(1), _cts.Token);
Assert.Equal(FanucJ519Protocol.CommandPacketLength, holdPacket.Buffer.Length);
Assert.Equal(401u, BinaryPrimitives.ReadUInt32BigEndian(holdPacket.Buffer.AsSpan(0x08, 4)));
Assert.Equal(10.0f, BinaryPrimitives.ReadSingleBigEndian(holdPacket.Buffer.AsSpan(0x1c, 4)), precision: 6);
Assert.Equal(20.0f, BinaryPrimitives.ReadSingleBigEndian(holdPacket.Buffer.AsSpan(0x20, 4)), precision: 6);
Assert.Equal(60.0f, BinaryPrimitives.ReadSingleBigEndian(holdPacket.Buffer.AsSpan(0x30, 4)), precision: 6);
await client.StopMotionAsync(_cts.Token);
}
/// <summary>
/// 验证存在显式目标时，状态反馈生成的 hold 命令不会覆盖当前待发送目标。
/// </summary>
[Fact]
public async Task StartMotion_WithExplicitCommand_PrefersExplicitTargetOverHoldCommand()
{
using var client = new FanucJ519Client();
await client.ConnectAsync("127.0.0.1", Port, _cts.Token);
var initResult = await _server.ReceiveAsync(_cts.Token);
client.UpdateCommand(new FanucJ519Command(sequence: 1, targetJoints: [1.0, 2.0, 3.0, 4.0, 5.0, 6.0]));
client.StartMotion();
await SendStatusPacketAsync(
initResult.RemoteEndPoint,
sequence: 402,
jointDegrees: [10.0, 20.0, 30.0, 40.0, 50.0, 60.0]);
var commandPacket = await _server.ReceiveAsync(_cts.Token).AsTask().WaitAsync(TimeSpan.FromSeconds(1), _cts.Token);
Assert.Equal(FanucJ519Protocol.CommandPacketLength, commandPacket.Buffer.Length);
Assert.Equal(402u, BinaryPrimitives.ReadUInt32BigEndian(commandPacket.Buffer.AsSpan(0x08, 4)));
Assert.Equal(1.0f, BinaryPrimitives.ReadSingleBigEndian(commandPacket.Buffer.AsSpan(0x1c, 4)), precision: 6);
Assert.Equal(2.0f, BinaryPrimitives.ReadSingleBigEndian(commandPacket.Buffer.AsSpan(0x20, 4)), precision: 6);
Assert.Equal(6.0f, BinaryPrimitives.ReadSingleBigEndian(commandPacket.Buffer.AsSpan(0x30, 4)), precision: 6);
await client.StopMotionAsync(_cts.Token);
}
/// <summary>
/// 验证在连接前调用 StartMotion 会抛出 InvalidOperationException。
/// </summary>
@@ -356,13 +434,27 @@ public sealed class FanucJ519ClientTests : IDisposable
/// <summary>
/// 向被测 J519 客户端发送一帧最小状态包，用机器人侧 status sequence 驱动下一帧命令。
/// </summary>
private async Task SendStatusPacketAsync(IPEndPoint clientEndpoint, uint sequence)
private async Task SendStatusPacketAsync(
IPEndPoint clientEndpoint,
uint sequence,
IReadOnlyList<double>? jointDegrees = null)
{
var responsePacket = new byte[FanucJ519Protocol.ResponsePacketLength];
BinaryPrimitives.WriteUInt32BigEndian(responsePacket.AsSpan(0x00, 4), 0);
BinaryPrimitives.WriteUInt32BigEndian(responsePacket.AsSpan(0x04, 4), 1);
BinaryPrimitives.WriteUInt32BigEndian(responsePacket.AsSpan(0x08, 4), sequence);
responsePacket[0x0c] = 15;
if (jointDegrees is not null)
{
for (var index = 0; index < Math.Min(6, jointDegrees.Count); index++)
{
BinaryPrimitives.WriteSingleBigEndian(
responsePacket.AsSpan(0x3c + index * sizeof(float), sizeof(float)),
(float)jointDegrees[index]);
}
}
await _server.SendAsync(responsePacket, clientEndpoint, _cts.Token);
}

23
tests/Flyshot.Core.Tests/PlanningCompatibilityTests.cs
View File

@@ -1,6 +1,7 @@
using Flyshot.Core.Config;
using Flyshot.Core.Domain;
using Flyshot.Core.Planning;
using Flyshot.Core.Planning.Sampling;
using Flyshot.Core.Triggering;
namespace Flyshot.Core.Tests;
@@ -171,6 +172,28 @@ public sealed class PlanningCompatibilityTests
Assert.Equal(2, doEvent.HoldCycles);
}
/// <summary>
/// 验证稠密轨迹生成后会复核离散加速度，避免采样结果绕过 ICSP 段 scale 校验。
/// </summary>
[Fact]
public void TrajectoryLimitValidator_Throws_WhenDenseTrajectoryAccelerationExceedsLimit()
{
var robot = CreateRobotProfile([100.0], [10.0], [1000.0]);
var exception = Assert.Throws<InvalidOperationException>(() =>
TrajectoryLimitValidator.ValidateDenseJointTrajectory(
robot,
[
[0.0, 0.0],
[0.008, 0.0],
[0.016, 0.01]
],
trajectoryName: "dense-acceleration-check"));
Assert.Contains("加速度", exception.Message);
Assert.Contains("dense-acceleration-check", exception.Message);
}
/// <summary>
/// 构造一个最小 RobotProfile，便于规划层单元测试聚焦在时间轴逻辑上。
/// </summary>

90
tests/Flyshot.Core.Tests/RuntimeOrchestrationTests.cs
View File

@@ -914,7 +914,7 @@ public sealed class RuntimeOrchestrationTests
}
/// <summary>
/// 验证 SaveTrajectoryInfo 会同时导出按 J519 8ms 实发周期重采样的点位，并应用当前 speed_ratio。
/// 验证 SaveTrajectoryInfo 会同时导出按 J519 8ms 实发周期重采样的点位，并按执行侧稠密轨迹时长应用当前 speed_ratio。
/// </summary>
[Fact]
public void ControllerClientCompatService_SaveTrajectoryInfo_ExportsActualSendRowsWithSpeedRatio()
@@ -945,8 +945,10 @@ public sealed class RuntimeOrchestrationTests
var pointRows = File.ReadAllLines(pointsPath).Select(ParseSpaceSeparatedDoubles).ToArray();
var timingRows = File.ReadAllLines(timingPath).Select(ParseSpaceSeparatedDoubles).ToArray();
var duration = double.Parse(File.ReadLines(Path.Combine(outputDir, "JointTraj.txt")).Last().Split(' ')[0], CultureInfo.InvariantCulture);
var expectedRows = (int)Math.Ceiling(Math.Max(0.0, (duration / (0.008 * 0.5)) - 1e-9)) + 1;
var executionDuration = double.Parse(
File.ReadLines(Path.Combine(outputDir, "JointDetialTraj.txt")).Last().Split(' ')[0],
CultureInfo.InvariantCulture);
var expectedRows = (int)Math.Ceiling(Math.Max(0.0, (executionDuration / (0.008 * 0.5)) - 1e-9)) + 1;
Assert.Equal(expectedRows, pointRows.Length);
Assert.Equal(expectedRows, timingRows.Length);
@@ -961,6 +963,54 @@ public sealed class RuntimeOrchestrationTests
}
}
/// <summary>
/// 验证 saveTrajectory 导出的 JointDetialTraj.txt 来自执行侧 8ms 稠密轨迹的 16ms 视图，
/// 而不是再次从 PlannedTrajectory 独立重采样。
/// </summary>
[Fact]
public void FlyshotTrajectoryArtifactWriter_WriteUploadedFlyshot_JointDetailUsesExecutionDenseDownsample()
{
var fixture = LoadUttcMs11RuntimeFixture();
var configRoot = CreateTempConfigRoot();
try
{
var options = new ControllerClientCompatOptions { ConfigRoot = configRoot };
var writer = new FlyshotTrajectoryArtifactWriter(options, new RobotModelLoader());
var orchestrator = new ControllerClientTrajectoryOrchestrator();
var bundle = orchestrator.PlanUploadedFlyshot(
fixture.Robot,
fixture.Uploaded,
settings: EnableSmoothStartStopTiming(fixture.Settings),
planningSpeedScale: 1.0);
writer.WriteUploadedFlyshot("UTTC_MS11", fixture.Robot, bundle, speedRatio: 1.0);
var outputDir = Path.Combine(configRoot, "Data", "UTTC_MS11");
var exportedRows = File.ReadAllLines(Path.Combine(outputDir, "JointDetialTraj.txt"))
.Select(ParseSpaceSeparatedDoubles)
.ToArray();
var expectedRows = DownsampleDenseRows(
bundle.Result.DenseJointTrajectory!,
samplePeriodSeconds: 0.016)
.Select(static row => row.ToArray())
.ToArray();
Assert.Equal(expectedRows.Length, exportedRows.Length);
for (var rowIndex = 0; rowIndex < expectedRows.Length; rowIndex++)
{
Assert.Equal(expectedRows[rowIndex].Length, exportedRows[rowIndex].Length);
for (var columnIndex = 0; columnIndex < expectedRows[rowIndex].Length; columnIndex++)
{
Assert.Equal(expectedRows[rowIndex][columnIndex], exportedRows[rowIndex][columnIndex], precision: 6);
}
}
}
finally
{
Directory.Delete(configRoot, recursive: true);
}
}
/// <summary>
/// 创建只包含当前支持机器人 JSON 模型和 RobotConfig.json 的临时运行配置根。
/// </summary>
@@ -1276,6 +1326,40 @@ public sealed class RuntimeOrchestrationTests
.ToArray();
}
/// <summary>
/// 将 8ms 执行稠密轨迹按目标周期抽稀为低频视图，并始终保留终点。
/// </summary>
private static IReadOnlyList<IReadOnlyList<double>> DownsampleDenseRows(
IReadOnlyList<IReadOnlyList<double>> denseRows,
double samplePeriodSeconds)
{
var result = new List<IReadOnlyList<double>>();
var epsilon = 1e-6;
var nextSampleTime = 0.0;
foreach (var row in denseRows)
{
var sampleTime = row[0];
if (sampleTime + epsilon < nextSampleTime)
{
continue;
}
if (Math.Abs(sampleTime - nextSampleTime) <= epsilon || sampleTime.Equals(0.0))
{
result.Add(row);
nextSampleTime += samplePeriodSeconds;
}
}
if (result.Count == 0 || !ReferenceEquals(result[^1], denseRows[^1]))
{
result.Add(denseRows[^1]);
}
return result;
}
/// <summary>
/// 用真实可运行轨迹前 4 个采样点拟合局部三次曲线，返回相对首点时间的系数。
/// 曲线形式为 p(t)=c3*t^3+c2*t^2+c1*t+c0，单位保持输入文件的角度制。