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

✨ feat(compat): 补齐飞拍执行等待与 FANUC 状态驱动链路

Browse Source 
- 为 ExecuteFlyShotTraj 补齐 wait 语义，并让 move_to_start
  先完成临时 PTP 运动后再启动正式飞拍轨迹
- 将 J519 命令发送改为由机器人 UDP status sequence 驱动，
  避免在未收到状态包时主动发周期命令
- 将 10010 状态通道关节字段统一按 JointRadians 命名，
  同步更新运行时读取逻辑与协议测试
- 新增 FANUC 10010 状态帧、流运动手册和 Python client
  逆向文档，并更新 README 与兼容需求说明
- 补充兼容层编排测试与 HTTP 集成测试，覆盖 wait 和
  move_to_start 串行化行为
This commit is contained in:
yunxiao.zhu
2026-05-03 19:29:31 +08:00
parent 91c1494cde
commit af65ca03a0
17 changed files with 1694 additions and 214 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -6,7 +6,7 @@ using Flyshot.Runtime.Fanuc.Protocol;
namespace Flyshot.Core.Tests;
/// <summary>
/// 验证 FANUC UDP 60015 J519 运动客户端的初始化、周期发送与响应解析。
/// 验证 FANUC UDP 60015 J519 运动客户端的初始化、状态包驱动发送与响应解析。
/// </summary>
public sealed class FanucJ519ClientTests : IDisposable
{
@@ -54,25 +54,24 @@ public sealed class FanucJ519ClientTests : IDisposable
}
/// <summary>
/// 验证启动运动后能按周期发送命令包。
/// 验证启动运动后必须等到状态包到达，不能由上位机本地 8ms 循环主动发命令。
/// </summary>
[Fact]
public async Task StartMotion_SendsPeriodicCommands()
public async Task StartMotion_WaitsForStatusPacketBeforeSendingCommand()
{
using var client = new FanucJ519Client();
await client.ConnectAsync("127.0.0.1", Port, _cts.Token);
// 接收并丢弃初始化包。
var initResult = await _server.ReceiveAsync(_cts.Token);
await _server.ReceiveAsync(_cts.Token);
var command = new FanucJ519Command(sequence: 1, targetJoints: [1.0, 2.0, 3.0, 4.0, 5.0, 6.0]);
client.UpdateCommand(command);
client.StartMotion();
// 接收至少一个命令包。
var commandResult = await _server.ReceiveAsync(_cts.Token);
Assert.Equal(FanucJ519Protocol.CommandPacketLength, commandResult.Buffer.Length);
Assert.Equal(0u, BinaryPrimitives.ReadUInt32BigEndian(commandResult.Buffer.AsSpan(0x08, 4)));
// 机器人尚未回状态包时，上位机不应自行发 64B command packet。
await Assert.ThrowsAsync<TimeoutException>(
() => _server.ReceiveAsync(_cts.Token).AsTask().WaitAsync(TimeSpan.FromMilliseconds(120)));
await client.StopMotionAsync(_cts.Token);
}
@@ -140,51 +139,47 @@ public sealed class FanucJ519ClientTests : IDisposable
}
/// <summary>
/// 验证 UpdateCommand 替换当前命令后下一周期发送新命令。
/// 验证收到状态包后，下一帧命令使用该状态包的序号。
/// </summary>
[Fact]
public async Task UpdateCommand_ReplacesCurrentCommand()
public async Task StartMotion_UsesLatestStatusSequenceForFirstCommand()
{
using var client = new FanucJ519Client();
await client.ConnectAsync("127.0.0.1", Port, _cts.Token);
await _server.ReceiveAsync(_cts.Token); // init
var initResult = await _server.ReceiveAsync(_cts.Token);
var command1 = new FanucJ519Command(sequence: 1, targetJoints: [1.0, 0.0, 0.0, 0.0, 0.0, 0.0]);
var command2 = new FanucJ519Command(sequence: 2, targetJoints: [2.0, 0.0, 0.0, 0.0, 0.0, 0.0]);
client.UpdateCommand(command1);
var command = new FanucJ519Command(sequence: 1, targetJoints: [1.0, 0.0, 0.0, 0.0, 0.0, 0.0]);
client.UpdateCommand(command);
client.StartMotion();
var result1 = await _server.ReceiveAsync(_cts.Token);
Assert.Equal(0u, BinaryPrimitives.ReadUInt32BigEndian(result1.Buffer.AsSpan(0x08, 4)));
Assert.Equal(1.0f, BinaryPrimitives.ReadSingleBigEndian(result1.Buffer.AsSpan(0x1c, 4)), precision: 6);
await SendStatusPacketAsync(initResult.RemoteEndPoint, sequence: 100);
client.UpdateCommand(command2);
var result2 = await _server.ReceiveAsync(_cts.Token);
Assert.Equal(1u, BinaryPrimitives.ReadUInt32BigEndian(result2.Buffer.AsSpan(0x08, 4)));
Assert.Equal(2.0f, BinaryPrimitives.ReadSingleBigEndian(result2.Buffer.AsSpan(0x1c, 4)), precision: 6);
var result = await _server.ReceiveAsync(_cts.Token);
Assert.Equal(FanucJ519Protocol.CommandPacketLength, result.Buffer.Length);
Assert.Equal(100u, BinaryPrimitives.ReadUInt32BigEndian(result.Buffer.AsSpan(0x08, 4)));
Assert.Equal(1.0f, BinaryPrimitives.ReadSingleBigEndian(result.Buffer.AsSpan(0x1c, 4)), precision: 6);
await client.StopMotionAsync(_cts.Token);
}
/// <summary>
/// 验证重复保持同一命令时实际 J519 包序号仍按客户端全局递增。
/// 验证连续状态包会逐包驱动命令发送，并使用各自的状态包序号。
/// </summary>
[Fact]
public async Task StartMotion_IncrementsSequenceForRepeatedHoldPackets()
public async Task StartMotion_SendsOneCommandForEachStatusPacketWithMatchingSequence()
{
using var client = new FanucJ519Client();
await client.ConnectAsync("127.0.0.1", Port, _cts.Token);
await _server.ReceiveAsync(_cts.Token); // init
var initResult = await _server.ReceiveAsync(_cts.Token);
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++)
for (uint sequence = 100; sequence < 104; sequence++)
{
await SendStatusPacketAsync(initResult.RemoteEndPoint, sequence);
var result = await _server.ReceiveAsync(_cts.Token);
packets.Add(result.Buffer);
}
@@ -194,26 +189,27 @@ public sealed class FanucJ519ClientTests : IDisposable
var sequences = packets
.Select(packet => BinaryPrimitives.ReadUInt32BigEndian(packet.AsSpan(0x08, 4)))
.ToArray();
Assert.Equal([0u, 1u, 2u, 3u], sequences);
Assert.Equal([100u, 101u, 102u, 103u], 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()
public async Task StartMotion_CanRestartAfterStopMotionAndUseNewStatusSequence()
{
using var client = new FanucJ519Client();
await client.ConnectAsync("127.0.0.1", Port, _cts.Token);
await _server.ReceiveAsync(_cts.Token); // init
var initResult = await _server.ReceiveAsync(_cts.Token);
client.UpdateCommand(new FanucJ519Command(sequence: 10, targetJoints: [1.0, 0.0, 0.0, 0.0, 0.0, 0.0]));
client.StartMotion();
await SendStatusPacketAsync(initResult.RemoteEndPoint, sequence: 200);
var first = await _server.ReceiveAsync(_cts.Token);
var firstSequence = BinaryPrimitives.ReadUInt32BigEndian(first.Buffer.AsSpan(0x08, 4));
Assert.Equal(0u, firstSequence);
Assert.Equal(200u, firstSequence);
await client.StopMotionAsync(_cts.Token);
@@ -227,10 +223,11 @@ public sealed class FanucJ519ClientTests : IDisposable
client.UpdateCommand(new FanucJ519Command(sequence: 20, targetJoints: [2.0, 0.0, 0.0, 0.0, 0.0, 0.0]));
client.StartMotion();
await SendStatusPacketAsync(initResult.RemoteEndPoint, sequence: 300);
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(300u, BinaryPrimitives.ReadUInt32BigEndian(restarted.Buffer.AsSpan(0x08, 4)));
Assert.Equal(2.0f, BinaryPrimitives.ReadSingleBigEndian(restarted.Buffer.AsSpan(0x1c, 4)), precision: 6);
await client.StopMotionAsync(_cts.Token);
@@ -247,14 +244,14 @@ public sealed class FanucJ519ClientTests : IDisposable
}
/// <summary>
/// 验证发送循环能持续按协议周期输出命令包。
/// 验证状态包驱动发送能持续输出命令包。
/// </summary>
[Fact]
public async Task StartMotion_MaintainsPeriodicCommandStream()
public async Task StartMotion_MaintainsStatusDrivenCommandStream()
{
using var client = new FanucJ519Client();
await client.ConnectAsync("127.0.0.1", Port, _cts.Token);
await _server.ReceiveAsync(_cts.Token); // init
var initResult = await _server.ReceiveAsync(_cts.Token);
var command = new FanucJ519Command(sequence: 1, targetJoints: [1.0, 2.0, 3.0, 4.0, 5.0, 6.0]);
client.UpdateCommand(command);
@@ -265,6 +262,7 @@ public sealed class FanucJ519ClientTests : IDisposable
var sequences = new List<uint>();
for (var i = 0; i < 5; i++)
{
await SendStatusPacketAsync(initResult.RemoteEndPoint, sequence: (uint)(500 + i));
var result = await _server.ReceiveAsync(_cts.Token);
Assert.Equal(FanucJ519Protocol.CommandPacketLength, result.Buffer.Length);
sequences.Add(BinaryPrimitives.ReadUInt32BigEndian(result.Buffer.AsSpan(0x08, 4)));
@@ -273,7 +271,7 @@ public sealed class FanucJ519ClientTests : IDisposable
await client.StopMotionAsync(_cts.Token);
Assert.Equal([0u, 1u, 2u, 3u, 4u], sequences);
Assert.Equal([500u, 501u, 502u, 503u, 504u], sequences);
// 计算相邻包间隔并使用 CI 安全的宽松边界验证周期流仍在推进。
var intervals = new List<TimeSpan>();
@@ -288,4 +286,17 @@ public sealed class FanucJ519ClientTests : IDisposable
Assert.True(interval <= TimeSpan.FromMilliseconds(30), $"间隔 {interval.TotalMilliseconds:F2}ms 过长。");
});
}
/// <summary>
/// 向被测 J519 客户端发送一帧最小状态包，用机器人侧 status sequence 驱动下一帧命令。
/// </summary>
private async Task SendStatusPacketAsync(IPEndPoint clientEndpoint, uint sequence)
{
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;
await _server.SendAsync(responsePacket, clientEndpoint, _cts.Token);
}
}