✨ feat(fanuc): 改为按状态包驱动 J519 队列发送
* 预生成稠密轨迹 J519 命令队列，等待机器人状态包逐帧出队 * 让 ExecuteTrajectory 在队列实际取完后返回，避免后台发送提前结束 * 新增 ActualSendTiming.txt，区分实发时间与 speed_ratio 采样时间 * 补充 J519 队列、等待完成和实发时间映射相关单元测试 * 同步文档中的 t_send / t_traj / speed_ratio 说明 Co-authored-by: Copilot <copilot@github.com>
This commit is contained in:
@@ -3,6 +3,9 @@ using Flyshot.ControllerClientCompat;
|
||||
using Flyshot.Core.Config;
|
||||
using Flyshot.Runtime.Fanuc;
|
||||
using Flyshot.Runtime.Fanuc.Protocol;
|
||||
using System.Buffers.Binary;
|
||||
using System.Net;
|
||||
using System.Net.Sockets;
|
||||
using System.Reflection;
|
||||
|
||||
namespace Flyshot.Core.Tests;
|
||||
@@ -17,7 +20,7 @@ public sealed class FanucControllerRuntimeDenseTests
|
||||
private const double SmoothPtpJerkShapeCoefficient = 52.5;
|
||||
|
||||
/// <summary>
|
||||
/// 验证真机 J519 发送按 8ms 实发周期、speed_ratio 轨迹时间步进，并输出角度制目标。
|
||||
/// 验证真机 J519 会预生成按 8ms 轨迹映射的命令队列，并输出角度制目标。
|
||||
/// </summary>
|
||||
[Fact]
|
||||
public void ExecuteTrajectory_WithDenseWaypoints_RealMode_ResamplesBySpeedRatioAndConvertsRadiansToDegrees()
|
||||
@@ -59,6 +62,65 @@ public sealed class FanucControllerRuntimeDenseTests
|
||||
Assert.Equal(5, commands.Count);
|
||||
Assert.All(commands, static command => Assert.Equal(0u, command.Sequence));
|
||||
Assert.Equal([0.0, 45.0, 90.0, 135.0, 180.0], commands.Select(static command => command.TargetJoints[0]));
|
||||
Assert.False(j519Client.IsCommandQueueDrainedForTests());
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 验证真机稠密轨迹接口会等待 J519 队列被状态包实际取完后再返回。
|
||||
/// </summary>
|
||||
[Fact]
|
||||
public async Task ExecuteTrajectory_WithDenseWaypoints_RealMode_WaitsForJ519QueueToDrainBeforeReturning()
|
||||
{
|
||||
using var server = new UdpClient(0);
|
||||
using var cts = new CancellationTokenSource(TimeSpan.FromSeconds(5));
|
||||
using var commandClient = new FanucCommandClient();
|
||||
using var stateClient = new FanucStateClient();
|
||||
using var j519Client = new FanucJ519Client();
|
||||
using var runtime = new FanucControllerRuntime(commandClient, stateClient, j519Client);
|
||||
var robot = TestRobotFactory.CreateRobotProfile();
|
||||
var port = ((IPEndPoint)server.Client.LocalEndPoint!).Port;
|
||||
runtime.ResetRobot(robot, "FANUC_LR_Mate_200iD");
|
||||
j519Client.EnableCommandHistoryForTests();
|
||||
await j519Client.ConnectAsync("127.0.0.1", port, cts.Token);
|
||||
var initResult = await server.ReceiveAsync(cts.Token);
|
||||
j519Client.StartMotion();
|
||||
ForceRealModeEnabled(runtime, speedRatio: 1.0);
|
||||
|
||||
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 result = new TrajectoryResult(
|
||||
programName: "wait-drain",
|
||||
method: PlanningMethod.Icsp,
|
||||
isValid: true,
|
||||
duration: TimeSpan.FromSeconds(0.016),
|
||||
shotEvents: Array.Empty<ShotEvent>(),
|
||||
triggerTimeline: Array.Empty<TrajectoryDoEvent>(),
|
||||
artifacts: Array.Empty<TrajectoryArtifact>(),
|
||||
failureReason: null,
|
||||
usedCache: false,
|
||||
originalWaypointCount: 4,
|
||||
plannedWaypointCount: 4,
|
||||
denseJointTrajectory: denseTrajectory);
|
||||
|
||||
var executeTask = Task.Run(() => runtime.ExecuteTrajectory(result, [Math.PI, 0.0, 0.0, 0.0, 0.0, 0.0]), cts.Token);
|
||||
await WaitUntilAsync(() => j519Client.GetCommandHistoryForTests().Count == 3, cts.Token);
|
||||
|
||||
// 只有机器人状态包把队列全部取出后，ExecuteTrajectory 才能向上层返回。
|
||||
Assert.False(executeTask.IsCompleted);
|
||||
|
||||
for (uint sequence = 100; sequence < 103; sequence++)
|
||||
{
|
||||
await SendStatusPacketAsync(server, initResult.RemoteEndPoint, sequence, cts.Token);
|
||||
_ = await server.ReceiveAsync(cts.Token);
|
||||
}
|
||||
|
||||
await executeTask.WaitAsync(TimeSpan.FromSeconds(2), cts.Token);
|
||||
Assert.True(j519Client.IsCommandQueueDrainedForTests());
|
||||
Assert.False(runtime.GetSnapshot().IsInMotion);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
@@ -108,27 +170,47 @@ public sealed class FanucControllerRuntimeDenseTests
|
||||
var commands = j519Client.GetCommandHistoryForTests();
|
||||
var runDir = GetSingleDenseSendRunDirectory(outputRoot);
|
||||
var pointsPath = Path.Combine(runDir, "ActualSendJointTraj.txt");
|
||||
var timingPath = Path.Combine(runDir, "ActualSendTiming.txt");
|
||||
var jerkPath = Path.Combine(runDir, "ActualSendJerkStats.txt");
|
||||
|
||||
Assert.True(File.Exists(pointsPath));
|
||||
Assert.True(File.Exists(timingPath));
|
||||
Assert.True(File.Exists(jerkPath));
|
||||
|
||||
var pointLines = File.ReadAllLines(pointsPath);
|
||||
var timingLines = File.ReadAllLines(timingPath);
|
||||
var jerkLines = File.ReadAllLines(jerkPath);
|
||||
Assert.Equal(commands.Count, pointLines.Length);
|
||||
Assert.Equal(commands.Count, timingLines.Length);
|
||||
Assert.Equal(Math.Max(0, commands.Count - 1), jerkLines.Length);
|
||||
|
||||
var firstColumns = ParseColumns(pointLines[0]);
|
||||
var secondColumns = ParseColumns(pointLines[1]);
|
||||
var lastColumns = ParseColumns(pointLines[^1]);
|
||||
Assert.Equal(9, firstColumns.Length);
|
||||
Assert.Equal(9, lastColumns.Length);
|
||||
Assert.Equal(0.0, firstColumns[0], precision: 6);
|
||||
Assert.Equal(0.008, secondColumns[0], precision: 6);
|
||||
Assert.Equal(180.0, lastColumns[1], precision: 6);
|
||||
|
||||
var firstTimingColumns = ParseColumns(timingLines[0]);
|
||||
var secondTimingColumns = ParseColumns(timingLines[1]);
|
||||
var lastTimingColumns = ParseColumns(timingLines[^1]);
|
||||
Assert.Equal(4, firstTimingColumns.Length);
|
||||
Assert.Equal(0.0, firstTimingColumns[0], precision: 6);
|
||||
Assert.Equal(0.0, firstTimingColumns[1], precision: 6);
|
||||
Assert.Equal(0.0, firstTimingColumns[2], precision: 6);
|
||||
Assert.Equal(0.5, firstTimingColumns[3], precision: 6);
|
||||
Assert.Equal(1.0, secondTimingColumns[0], precision: 6);
|
||||
Assert.Equal(0.008, secondTimingColumns[1], precision: 6);
|
||||
Assert.Equal(0.004, secondTimingColumns[2], precision: 6);
|
||||
Assert.Equal(commands.Count - 1, lastTimingColumns[0], precision: 6);
|
||||
Assert.Equal(0.016, lastTimingColumns[2], precision: 6);
|
||||
|
||||
var firstJerkColumns = ParseColumns(jerkLines[0]);
|
||||
Assert.Equal(10, firstJerkColumns.Length);
|
||||
Assert.Equal(0.0, firstJerkColumns[0], precision: 6);
|
||||
Assert.Equal(0.004, firstJerkColumns[2], precision: 6);
|
||||
Assert.Equal(0.008, firstJerkColumns[2], precision: 6);
|
||||
}
|
||||
finally
|
||||
{
|
||||
@@ -797,6 +879,43 @@ public sealed class FanucControllerRuntimeDenseTests
|
||||
throw new TimeoutException("Timed out waiting for dense trajectory send task to finish.");
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 等待测试条件成立，用于观察后台执行路径是否已经到达指定状态。
|
||||
/// </summary>
|
||||
private static async Task WaitUntilAsync(Func<bool> condition, CancellationToken cancellationToken)
|
||||
{
|
||||
var deadline = DateTimeOffset.UtcNow.AddSeconds(1);
|
||||
while (DateTimeOffset.UtcNow < deadline)
|
||||
{
|
||||
cancellationToken.ThrowIfCancellationRequested();
|
||||
if (condition())
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
await Task.Delay(1, cancellationToken);
|
||||
}
|
||||
|
||||
throw new TimeoutException("Timed out waiting for test condition.");
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 向被测 J519 客户端发送一帧最小状态包，用机器人侧 status sequence 驱动下一帧命令。
|
||||
/// </summary>
|
||||
private static async Task SendStatusPacketAsync(
|
||||
UdpClient server,
|
||||
IPEndPoint clientEndpoint,
|
||||
uint sequence,
|
||||
CancellationToken cancellationToken)
|
||||
{
|
||||
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] = 0b0111;
|
||||
await server.SendAsync(responsePacket, clientEndpoint, cancellationToken);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 获取一次测试执行生成的唯一稠密发送记录目录。
|
||||
/// </summary>
|
||||
|
||||
Reference in New Issue
Block a user