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

✨ feat(*): 完善 FANUC J519 闭环、MoveJoint 与现场抓包验证

Browse Source 
* 划分 J519 发送循环与稠密轨迹循环职责边界，
  FanucJ519Client 负责 UDP 周期发送，
  FanucControllerRuntime 按轨迹时间更新下一帧命令
* 执行时将规划输出 rad 转为 J519 deg 目标，
  并按 speed_ratio 调整 8ms 发送时间尺度
* 补齐 accept_cmd/received_cmd/sysrdy/rbt_inmotion
  状态位解析与启动前闭环检查
* MoveJoint 改为关节空间直线 + smoothstep 进度
  的临时 PTP 稠密轨迹，按 status=15 运动窗口复现
* 新增 UTTC 2026-04-28 三份抓包 golden tests，
  覆盖 0.5/0.7/1.0 speed_ratio 下的 J519 命令、
  IO 脉冲与响应滞后
* 状态通道补充超时重连策略与退避逻辑
* TCP 10012 命令响应统一检查 result_code
* 状态页扩展 J519 状态位与快照诊断信息
* 新增 docs/fanuc-field-runtime-workflow.md 现场工作流
* 补充 LR Mate 200iD 模型、RobotConfig.json 与 workpiece
This commit is contained in:
yunxiao.zhu
2026-04-29 01:03:18 +08:00
parent 0292e077ff
commit 0724efebed
25 changed files with 1986 additions and 60 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -72,7 +72,7 @@ public sealed class FanucJ519ClientTests : IDisposable
// 接收至少一个命令包。
var commandResult = await _server.ReceiveAsync(_cts.Token);
Assert.Equal(FanucJ519Protocol.CommandPacketLength, commandResult.Buffer.Length);
Assert.Equal(1u, BinaryPrimitives.ReadUInt32BigEndian(commandResult.Buffer.AsSpan(0x08, 4)));
Assert.Equal(0u, BinaryPrimitives.ReadUInt32BigEndian(commandResult.Buffer.AsSpan(0x08, 4)));
await client.StopMotionAsync(_cts.Token);
}
@@ -156,18 +156,86 @@ public sealed class FanucJ519ClientTests : IDisposable
client.StartMotion();
var result1 = await _server.ReceiveAsync(_cts.Token);
Assert.Equal(1u, BinaryPrimitives.ReadUInt32BigEndian(result1.Buffer.AsSpan(0x08, 4)));
Assert.Equal(0u, BinaryPrimitives.ReadUInt32BigEndian(result1.Buffer.AsSpan(0x08, 4)));
Assert.Equal(1.0f, BinaryPrimitives.ReadSingleBigEndian(result1.Buffer.AsSpan(0x1c, 4)), precision: 6);
client.UpdateCommand(command2);
var result2 = await _server.ReceiveAsync(_cts.Token);
Assert.Equal(2u, BinaryPrimitives.ReadUInt32BigEndian(result2.Buffer.AsSpan(0x08, 4)));
Assert.Equal(1u, BinaryPrimitives.ReadUInt32BigEndian(result2.Buffer.AsSpan(0x08, 4)));
Assert.Equal(2.0f, BinaryPrimitives.ReadSingleBigEndian(result2.Buffer.AsSpan(0x1c, 4)), precision: 6);
await client.StopMotionAsync(_cts.Token);
}
/// <summary>
/// 验证重复保持同一命令时实际 J519 包序号仍按客户端全局递增。
/// </summary>
[Fact]
public async Task StartMotion_IncrementsSequenceForRepeatedHoldPackets()
{
using var client = new FanucJ519Client();
await client.ConnectAsync("127.0.0.1", Port, _cts.Token);
await _server.ReceiveAsync(_cts.Token); // init
var command = new FanucJ519Command(sequence: 99, targetJoints: [1.0, 0.0, 0.0, 0.0, 0.0, 0.0]);
client.UpdateCommand(command);
client.StartMotion();
var packets = new List<byte[]>();
for (var index = 0; index < 4; index++)
{
var result = await _server.ReceiveAsync(_cts.Token);
packets.Add(result.Buffer);
}
await client.StopMotionAsync(_cts.Token);
var sequences = packets
.Select(packet => BinaryPrimitives.ReadUInt32BigEndian(packet.AsSpan(0x08, 4)))
.ToArray();
Assert.Equal([0u, 1u, 2u, 3u], sequences);
Assert.All(packets, packet => Assert.Equal(1.0f, BinaryPrimitives.ReadSingleBigEndian(packet.AsSpan(0x1c, 4)), precision: 6));
}
/// <summary>
/// 验证停止运动后可在同一连接内重启发送，且包序号不重置。
/// </summary>
[Fact]
public async Task StartMotion_CanRestartAfterStopMotionWithoutResettingSequence()
{
using var client = new FanucJ519Client();
await client.ConnectAsync("127.0.0.1", Port, _cts.Token);
await _server.ReceiveAsync(_cts.Token); // init
client.UpdateCommand(new FanucJ519Command(sequence: 10, targetJoints: [1.0, 0.0, 0.0, 0.0, 0.0, 0.0]));
client.StartMotion();
var first = await _server.ReceiveAsync(_cts.Token);
var firstSequence = BinaryPrimitives.ReadUInt32BigEndian(first.Buffer.AsSpan(0x08, 4));
Assert.Equal(0u, firstSequence);
await client.StopMotionAsync(_cts.Token);
byte[] packet;
do
{
packet = (await _server.ReceiveAsync(_cts.Token)).Buffer;
}
while (packet.Length != FanucJ519Protocol.ControlPacketLength);
client.UpdateCommand(new FanucJ519Command(sequence: 20, targetJoints: [2.0, 0.0, 0.0, 0.0, 0.0, 0.0]));
client.StartMotion();
var restarted = await _server.ReceiveAsync(_cts.Token).AsTask().WaitAsync(TimeSpan.FromSeconds(1), _cts.Token);
Assert.Equal(FanucJ519Protocol.CommandPacketLength, restarted.Buffer.Length);
Assert.True(BinaryPrimitives.ReadUInt32BigEndian(restarted.Buffer.AsSpan(0x08, 4)) > firstSequence);
Assert.Equal(2.0f, BinaryPrimitives.ReadSingleBigEndian(restarted.Buffer.AsSpan(0x1c, 4)), precision: 6);
await client.StopMotionAsync(_cts.Token);
}
/// <summary>
/// 验证在连接前调用 StartMotion 会抛出 InvalidOperationException。
/// </summary>
@@ -179,10 +247,10 @@ public sealed class FanucJ519ClientTests : IDisposable
}
/// <summary>
/// 验证 Stopwatch + SpinWait 发送循环能保持约 8ms 周期抖动在亚毫秒级。
/// 验证发送循环能持续按协议周期输出命令包。
/// </summary>
[Fact]
public async Task StartMotion_MaintainsSubMillisecondPeriod()
public async Task StartMotion_MaintainsPeriodicCommandStream()
{
using var client = new FanucJ519Client();
await client.ConnectAsync("127.0.0.1", Port, _cts.Token);
@@ -192,28 +260,32 @@ public sealed class FanucJ519ClientTests : IDisposable
client.UpdateCommand(command);
client.StartMotion();
// 收集 5 个命令包到达时间戳。
// 收集 5 个命令包到达时间戳和序号。
var timestamps = new List<DateTimeOffset>();
var sequences = new List<uint>();
for (var i = 0; i < 5; i++)
{
var result = await _server.ReceiveAsync(_cts.Token);
Assert.Equal(FanucJ519Protocol.CommandPacketLength, result.Buffer.Length);
sequences.Add(BinaryPrimitives.ReadUInt32BigEndian(result.Buffer.AsSpan(0x08, 4)));
timestamps.Add(DateTimeOffset.UtcNow);
}
await client.StopMotionAsync(_cts.Token);
// 计算相邻包间隔并断言最大抖动。
Assert.Equal([0u, 1u, 2u, 3u, 4u], sequences);
// 计算相邻包间隔并使用 CI 安全的宽松边界验证周期流仍在推进。
var intervals = new List<TimeSpan>();
for (var i = 1; i < timestamps.Count; i++)
{
intervals.Add(timestamps[i] - timestamps[i - 1]);
}
// 允许 ±2ms 的测量误差（含 UDP 传输和调度延迟）。
Assert.All(intervals, interval =>
{
Assert.True(interval >= TimeSpan.FromMilliseconds(6), $"间隔 {interval.TotalMilliseconds:F2}ms 过短。");
Assert.True(interval <= TimeSpan.FromMilliseconds(10), $"间隔 {interval.TotalMilliseconds:F2}ms 过长。");
Assert.True(interval > TimeSpan.Zero, $"间隔 {interval.TotalMilliseconds:F2}ms 必须为正。");
Assert.True(interval <= TimeSpan.FromMilliseconds(30), $"间隔 {interval.TotalMilliseconds:F2}ms 过长。");
});
}
}