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

✨ feat(fanuc): 优化 J519 实时下发与飞拍起停整形

Browse Source 
- 改为高优先级 J519 接收线程与复用缓冲区发送链路
- 增加稠密执行前的 J519 就绪重试与状态诊断
- 修正程序状态响应字段顺序与 EnableRobot 默认参数
- 为飞拍轨迹补充平滑起停时间轴与首尾整形验证
- 补充真实运行配置、报警窗口与边界对比测试
- 同步更新限值文档、分析脚本与 .NET 8 SDK 固定配置
This commit is contained in:
yunxiao.zhu
2026-05-06 22:37:31 +08:00
parent 783716ff44
commit 70b0ccd414
17 changed files with 1629 additions and 117 deletions
Download Patch File Download Diff File Expand all files Collapse all files

233
tests/Flyshot.Core.Tests/FanucControllerRuntimeDenseTests.cs
View File

@@ -221,6 +221,101 @@ public sealed class FanucControllerRuntimeDenseTests
}
}
/// <summary>
/// 使用运行时 RobotConfig.json 中的真实 UTTC_MS11 轨迹执行一次完整 1x 稠密下发，
/// 并把 0.088s 报警窗口附近的实发时间、关节与跃度摘要落盘，便于继续对照现场报警帧。
/// </summary>
[Fact]
public void ExecuteTrajectory_UttcMs11FromHostRuntimeConfig_RealMode_WritesDenseSendDebugWindowAtOneX()
{
using var commandClient = new FanucCommandClient();
using var stateClient = new FanucStateClient();
using var j519Client = new FanucJ519Client();
using var runtime = new FanucControllerRuntime(commandClient, stateClient, j519Client);
var fixture = LoadUttcMs11RuntimeFixture();
var orchestrator = new ControllerClientTrajectoryOrchestrator();
var bundle = orchestrator.PlanUploadedFlyshot(
fixture.Robot,
fixture.Uploaded,
settings: fixture.Settings,
planningSpeedScale: 1.0);
var outputRoot = Path.Combine(AppContext.BaseDirectory, "Config", "Data", bundle.Result.ProgramName);
var denseSendRoot = Path.Combine(outputRoot, "DenseSend");
var beforeRunDirectories = Directory.Exists(denseSendRoot)
? Directory.GetDirectories(denseSendRoot).ToHashSet(StringComparer.OrdinalIgnoreCase)
: new HashSet<string>(StringComparer.OrdinalIgnoreCase);
runtime.ResetRobot(fixture.Robot, fixture.Robot.Name);
j519Client.EnableCommandHistoryForTests();
ForceRealModeEnabled(runtime, speedRatio: 1.0);
runtime.ExecuteTrajectory(bundle.Result, bundle.Result.DenseJointTrajectory![0].Skip(1).ToArray());
WaitUntilIdle(runtime);
var runDirectory = GetNewDenseSendRunDirectory(outputRoot, beforeRunDirectories);
var pointsPath = Path.Combine(runDirectory, "ActualSendJointTraj.txt");
var timingPath = Path.Combine(runDirectory, "ActualSendTiming.txt");
var jerkPath = Path.Combine(runDirectory, "ActualSendJerkStats.txt");
var summaryPath = Path.Combine(runDirectory, "AlarmWindow_0p088s_Summary.txt");
Assert.True(File.Exists(pointsPath));
Assert.True(File.Exists(timingPath));
Assert.True(File.Exists(jerkPath));
var pointsLines = File.ReadAllLines(pointsPath);
var timingLines = File.ReadAllLines(timingPath);
var jerkLines = File.ReadAllLines(jerkPath);
Assert.NotEmpty(pointsLines);
Assert.NotEmpty(timingLines);
Assert.NotEmpty(jerkLines);
var firstPoint = ParseColumns(pointsLines[0]);
var secondPoint = ParseColumns(pointsLines[1]);
Assert.Equal(0.0, firstPoint[0], precision: 6);
Assert.Equal(0.008, secondPoint[0], precision: 6);
var firstStepJ1 = Math.Abs(secondPoint[1] - firstPoint[1]);
Assert.True(firstStepJ1 > 1e-6, $"UTTC_MS11 实发首步不应被压成 0，actual={firstStepJ1:F9}deg");
const double targetSendTime = 0.088;
const double windowHalfWidth = 0.024;
var summaryLines = new List<string>
{
$"program={bundle.Result.ProgramName}",
$"send_time_target_seconds={targetSendTime:F6}",
$"window_half_width_seconds={windowHalfWidth:F6}",
$"points_path={pointsPath}",
$"timing_path={timingPath}",
$"jerk_path={jerkPath}",
"timing_window:"
};
foreach (var line in timingLines.Select(ParseColumns))
{
var sendTime = line[1];
if (Math.Abs(sendTime - targetSendTime) <= windowHalfWidth + 1e-9)
{
summaryLines.Add(
$"timing send_index={line[0]:F0} send_time={line[1]:F6} trajectory_time={line[2]:F6} speed_ratio={line[3]:F6}");
}
}
summaryLines.Add("jerk_window:");
foreach (var line in jerkLines.Select(ParseColumns))
{
var endTime = line[1];
if (Math.Abs(endTime - targetSendTime) <= windowHalfWidth + 1e-9)
{
summaryLines.Add(
$"jerk end_time={line[1]:F6} dt={line[2]:F6} j1={line[3]:F6} j2={line[4]:F6} j3={line[5]:F6} j4={line[6]:F6} j5={line[7]:F6} j6={line[8]:F6} max_abs={line.Skip(3).Max(static value => Math.Abs(value)):F6}");
}
}
File.WriteAllLines(summaryPath, summaryLines);
Assert.True(File.Exists(summaryPath));
}
/// <summary>
/// 验证 MoveJoint 会按抓包确认的点到点临时轨迹生成稠密 J519 目标，并继续叠加 speed_ratio 重采样。
/// </summary>
@@ -509,6 +604,58 @@ public sealed class FanucControllerRuntimeDenseTests
Assert.Equal([0.12, 0.22, 0.32, 0.42, 0.52, 0.62], snapshot.JointPositions);
}
/// <summary>
/// 验证稠密执行前若 J519 首次未就绪，会先尝试一次 EnableRobot，再在 500ms 后复查状态。
/// </summary>
[Fact]
public void EnsureJ519ReadyForDenseExecutionCore_RetriesEnableRobotOnceBeforePassing()
{
var responses = new Queue<FanucJ519Response?>(
[
CreateJ519Response(status: 0b0001, sequence: 11),
CreateJ519Response(status: 0b0101, sequence: 12)
]);
var enableRobotRetryCount = 0;
var waitCount = 0;
var exception = Record.Exception(
() => FanucControllerRuntime.EnsureJ519ReadyForDenseExecutionCore(
() => responses.Dequeue(),
() => enableRobotRetryCount++,
() => waitCount++));
Assert.Null(exception);
Assert.Equal(1, enableRobotRetryCount);
Assert.Equal(1, waitCount);
}
/// <summary>
/// 验证重试 EnableRobot 之后若 J519 仍未就绪，会继续抛出带状态位的异常。
/// </summary>
[Fact]
public void EnsureJ519ReadyForDenseExecutionCore_ThrowsWhenStillNotReadyAfterRetry()
{
var responses = new Queue<FanucJ519Response?>(
[
CreateJ519Response(status: 0b0000, sequence: 21),
CreateJ519Response(status: 0b0010, sequence: 22)
]);
var enableRobotRetryCount = 0;
var waitCount = 0;
var exception = Assert.Throws<InvalidOperationException>(
() => FanucControllerRuntime.EnsureJ519ReadyForDenseExecutionCore(
() => responses.Dequeue(),
() => enableRobotRetryCount++,
() => waitCount++));
Assert.Equal(1, enableRobotRetryCount);
Assert.Equal(1, waitCount);
Assert.Contains("accept_cmd=False", exception.Message, StringComparison.Ordinal);
Assert.Contains("sysrdy=False", exception.Message, StringComparison.Ordinal);
Assert.Contains("seq=22", exception.Message, StringComparison.Ordinal);
}
/// <summary>
/// 验证 StopMove 在没有任何后台发送任务运行时不会抛出异常。
/// </summary>
@@ -728,6 +875,30 @@ public sealed class FanucControllerRuntimeDenseTests
}
}
/// <summary>
/// 创建用于就绪状态测试的最小 J519 响应。
/// </summary>
/// <param name="status">待注入的 J519 状态位。</param>
/// <param name="sequence">待注入的响应序号。</param>
/// <returns>包含最小字段集合的测试响应。</returns>
private static FanucJ519Response CreateJ519Response(byte status, uint sequence)
{
return new FanucJ519Response(
messageType: 0,
version: 1,
sequence: sequence,
status: status,
readIoType: 0,
readIoIndex: 0,
readIoMask: 0,
readIoValue: 0,
timestamp: 0,
pose: new double[6],
externalAxes: new double[3],
jointDegrees: new double[6],
motorCurrents: new double[6]);
}
private static RobotProfile CreateMoveJointReferenceRobotProfile()
{
return new RobotProfile(
@@ -906,13 +1077,14 @@ public sealed class FanucControllerRuntimeDenseTests
UdpClient server,
IPEndPoint clientEndpoint,
uint sequence,
CancellationToken cancellationToken)
CancellationToken cancellationToken,
byte status = 0b0111)
{
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;
responsePacket[0x0c] = status;
await server.SendAsync(responsePacket, clientEndpoint, cancellationToken);
}
@@ -929,6 +1101,54 @@ public sealed class FanucControllerRuntimeDenseTests
return runDirectories[0];
}
/// <summary>
/// 获取本次测试刚生成的新稠密发送记录目录，避免误读历史运行产物。
/// </summary>
private static string GetNewDenseSendRunDirectory(string outputRoot, IReadOnlySet<string> beforeRunDirectories)
{
var denseSendRoot = Path.Combine(outputRoot, "DenseSend");
Assert.True(Directory.Exists(denseSendRoot));
var newDirectories = Directory.GetDirectories(denseSendRoot)
.Where(path => !beforeRunDirectories.Contains(path))
.ToArray();
Assert.Single(newDirectories);
return newDirectories[0];
}
/// <summary>
/// 加载运行时 Config/RobotConfig.json 中的 UTTC_MS11 轨迹和对应机器人配置。
/// </summary>
private static UttcMs11RuntimeFixture LoadUttcMs11RuntimeFixture()
{
var configPath = Path.Combine(
TestRobotFactory.GetReplacementRoot(),
"src",
"Flyshot.Server.Host",
"bin",
"Debug",
"net8.0",
"Config",
"RobotConfig.json");
var configRoot = Path.GetDirectoryName(configPath)!;
var loaded = new RobotConfigLoader().Load(configPath, configRoot);
var program = loaded.Programs["UTTC_MS11"];
var uploaded = new ControllerClientCompatUploadedTrajectory(
name: program.Name,
waypoints: program.Waypoints.Select(static waypoint => waypoint.Positions),
shotFlags: program.ShotFlags,
offsetValues: program.OffsetValues,
addressGroups: program.AddressGroups.Select(static group => group.Addresses));
var options = new ControllerClientCompatOptions
{
ConfigRoot = configRoot
};
var robot = new ControllerClientCompatRobotCatalog(options, new RobotModelLoader())
.LoadProfile("FANUC_LR_Mate_200iD", loaded.Robot.AccLimitScale, loaded.Robot.JerkLimitScale);
return new UttcMs11RuntimeFixture(configRoot, loaded.Robot, uploaded, robot);
}
/// <summary>
/// 解析空格分隔的纯文本数值列。
/// </summary>
@@ -984,4 +1204,13 @@ public sealed class FanucControllerRuntimeDenseTests
Assert.NotNull(field);
field.SetValue(client, response);
}
/// <summary>
/// 封装运行时 UTTC_MS11 轨迹、配置和机器人模型，避免测试反复拼装。
/// </summary>
private sealed record UttcMs11RuntimeFixture(
string ConfigRoot,
CompatibilityRobotSettings Settings,
ControllerClientCompatUploadedTrajectory Uploaded,
RobotProfile Robot);
}