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);
}
}

4
tests/Flyshot.Core.Tests/FanucProtocolTests.cs
View File

@@ -111,7 +111,7 @@ public sealed class FanucProtocolTests
Assert.Equal(9, frame.JointOrExtensionValues.Count);
Assert.Equal([2u, 0u, 0u, 1u], frame.TailWords);
Assert.Equal(frame.Pose, frame.CartesianPose);
Assert.Equal(frame.JointOrExtensionValues.Take(6), frame.JointDegrees);
Assert.Equal(frame.JointOrExtensionValues.Take(6), frame.JointRadians);
Assert.Equal(frame.JointOrExtensionValues.Skip(6), frame.ExternalAxes);
Assert.Equal(frame.TailWords, frame.RawTailWords);
Assert.Equal(2u, frame.StatusWord0);
@@ -135,7 +135,7 @@ public sealed class FanucProtocolTests
Assert.Equal(FanucStateProtocol.StateFrameLength, frameBytes.Length);
Assert.Equal(6, frame.CartesianPose.Count);
Assert.Equal(6, frame.JointDegrees.Count);
Assert.Equal(6, frame.JointRadians.Count);
Assert.Equal(3, frame.ExternalAxes.Count);
Assert.Equal([2u, 0u, 0u, 1u], frame.RawTailWords);
}

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

@@ -293,6 +293,92 @@ public sealed class RuntimeOrchestrationTests
Assert.Throws<ArgumentException>(Act);
}
/// <summary>
/// 验证 ExecuteFlyShotTraj(move_to_start=true) 会先执行稠密 PTP 到起点，并等待该段运动完成后再启动飞拍轨迹。
/// </summary>
[Fact]
public void ControllerClientCompatService_ExecuteTrajectoryByName_MoveToStartWaitsBeforeFlyshot()
{
var configRoot = CreateTempConfigRoot();
try
{
var options = new ControllerClientCompatOptions
{
ConfigRoot = configRoot
};
var runtime = new DelayedCompletionControllerRuntime(
initialJointPositions: [0.4, 0.0, 0.0, 0.0, 0.0, 0.0],
firstMotionCompletionDelay: TimeSpan.FromMilliseconds(80));
var service = new ControllerClientCompatService(
options,
new ControllerClientCompatRobotCatalog(options, new RobotModelLoader()),
runtime,
new ControllerClientTrajectoryOrchestrator(),
new RobotConfigLoader());
service.SetUpRobot("FANUC_LR_Mate_200iD");
service.SetActiveController(sim: false);
service.Connect("192.168.10.101");
service.EnableRobot(2);
service.UploadTrajectory(TestRobotFactory.CreateUploadedTrajectoryWithSingleShot());
service.ExecuteTrajectoryByName(
"demo-flyshot",
new FlyshotExecutionOptions(moveToStart: true, method: "icsp", saveTrajectory: false, useCache: false));
Assert.True(runtime.ExecuteCalls.Count >= 2);
Assert.NotNull(runtime.ExecuteCalls[0].Result.DenseJointTrajectory);
Assert.True(runtime.ExecuteCalls[0].Result.DenseJointTrajectory!.Count > 1);
Assert.False(runtime.SecondTrajectoryStartedBeforeFirstMotionCompleted);
}
finally
{
Directory.Delete(configRoot, recursive: true);
}
}
/// <summary>
/// 验证 ExecuteFlyShotTraj(wait=true) 会等待正式飞拍轨迹完成后再返回。
/// </summary>
[Fact]
public void ControllerClientCompatService_ExecuteTrajectoryByName_WaitTrueWaitsForFlyshotCompletion()
{
var configRoot = CreateTempConfigRoot();
try
{
var options = new ControllerClientCompatOptions
{
ConfigRoot = configRoot
};
var runtime = new DelayedCompletionControllerRuntime(
initialJointPositions: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
firstMotionCompletionDelay: TimeSpan.FromMilliseconds(80));
var service = new ControllerClientCompatService(
options,
new ControllerClientCompatRobotCatalog(options, new RobotModelLoader()),
runtime,
new ControllerClientTrajectoryOrchestrator(),
new RobotConfigLoader());
service.SetUpRobot("FANUC_LR_Mate_200iD");
service.SetActiveController(sim: false);
service.Connect("192.168.10.101");
service.EnableRobot(2);
service.UploadTrajectory(TestRobotFactory.CreateUploadedTrajectoryWithSingleShot());
service.ExecuteTrajectoryByName(
"demo-flyshot",
new FlyshotExecutionOptions(moveToStart: false, method: "icsp", saveTrajectory: false, useCache: false, wait: true));
Assert.Single(runtime.ExecuteCalls);
Assert.False(runtime.GetSnapshot().IsInMotion);
}
finally
{
Directory.Delete(configRoot, recursive: true);
}
}
/// <summary>
/// 验证兼容服务初始化机器人时会把 RobotConfig.json 中的 acc_limit / jerk_limit 传给模型加载器。
/// </summary>
@@ -650,3 +736,164 @@ internal sealed class RecordingControllerRuntime : IControllerRuntime
{
}
}
/// <summary>
/// 模拟第一段运动异步完成的测试运行时，用于验证兼容层是否等待 move_to_start 完成。
/// </summary>
internal sealed class DelayedCompletionControllerRuntime : IControllerRuntime
{
private readonly object _lock = new();
private readonly TimeSpan _firstMotionCompletionDelay;
private double[] _jointPositions;
private bool _isEnabled;
private bool _isInMotion;
private bool _firstMotionCompleted;
/// <summary>
/// 初始化可延迟完成第一段运动的测试运行时。
/// </summary>
/// <param name="initialJointPositions">运行时报告的初始关节位置。</param>
/// <param name="firstMotionCompletionDelay">第一段运动完成前保持忙碌的时间。</param>
public DelayedCompletionControllerRuntime(
IReadOnlyList<double> initialJointPositions,
TimeSpan firstMotionCompletionDelay)
{
_jointPositions = initialJointPositions.ToArray();
_firstMotionCompletionDelay = firstMotionCompletionDelay;
}
/// <summary>
/// 获取所有 ExecuteTrajectory 调用记录。
/// </summary>
public List<(TrajectoryResult Result, IReadOnlyList<double> FinalJointPositions)> ExecuteCalls { get; } = [];
/// <summary>
/// 获取第二条轨迹是否在第一段 move_to_start 完成前启动。
/// </summary>
public bool SecondTrajectoryStartedBeforeFirstMotionCompleted { get; private set; }
/// <inheritdoc />
public void ResetRobot(RobotProfile robot, string robotName)
{
}
/// <inheritdoc />
public void SetActiveController(bool sim)
{
}
/// <inheritdoc />
public void Connect(string robotIp)
{
}
/// <inheritdoc />
public void Disconnect()
{
}
/// <inheritdoc />
public void EnableRobot(int bufferSize)
{
_isEnabled = true;
}
/// <inheritdoc />
public void DisableRobot()
{
_isEnabled = false;
}
/// <inheritdoc />
public void StopMove()
{
lock (_lock)
{
_isInMotion = false;
}
}
/// <inheritdoc />
public double GetSpeedRatio() => 1.0;
/// <inheritdoc />
public void SetSpeedRatio(double ratio)
{
}
/// <inheritdoc />
public IReadOnlyList<double> GetTcp() => [0.0, 0.0, 0.0];
/// <inheritdoc />
public void SetTcp(double x, double y, double z)
{
}
/// <inheritdoc />
public bool GetIo(int port, string ioType) => false;
/// <inheritdoc />
public void SetIo(int port, bool value, string ioType)
{
}
/// <inheritdoc />
public IReadOnlyList<double> GetJointPositions()
{
lock (_lock)
{
return _jointPositions.ToArray();
}
}
/// <inheritdoc />
public IReadOnlyList<double> GetPose() => Array.Empty<double>();
/// <inheritdoc />
public ControllerStateSnapshot GetSnapshot()
{
lock (_lock)
{
return new ControllerStateSnapshot(
capturedAt: DateTimeOffset.UtcNow,
connectionState: "Connected",
isEnabled: _isEnabled,
isInMotion: _isInMotion,
speedRatio: 1.0,
jointPositions: _jointPositions.ToArray(),
cartesianPose: Array.Empty<double>(),
activeAlarms: Array.Empty<RuntimeAlarm>());
}
}
/// <inheritdoc />
public void ExecuteTrajectory(TrajectoryResult result, IReadOnlyList<double> finalJointPositions)
{
lock (_lock)
{
ExecuteCalls.Add((result, finalJointPositions.ToArray()));
if (ExecuteCalls.Count == 1)
{
_isInMotion = true;
_ = Task.Run(async () =>
{
await Task.Delay(_firstMotionCompletionDelay).ConfigureAwait(false);
lock (_lock)
{
_jointPositions = finalJointPositions.ToArray();
_isInMotion = false;
_firstMotionCompleted = true;
}
});
return;
}
if (!_firstMotionCompleted)
{
SecondTrajectoryStartedBeforeFirstMotionCompleted = true;
}
_jointPositions = finalJointPositions.ToArray();
}
}
}

3
tests/Flyshot.Server.IntegrationTests/LegacyHttpApiCompatibilityTests.cs
View File

@@ -229,7 +229,8 @@ public sealed class LegacyHttpApiCompatibilityTests(FlyshotServerFactory factory
move_to_start = true,
method = "icsp",
save_traj = true,
use_cache = true
use_cache = true,
wait = true
}))
{
Assert.Equal(HttpStatusCode.OK, executeResponse.StatusCode);