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

Merge commit '0952958af7685646cdbf2a4907c6e2ae59d4d352'

Browse Source
This commit is contained in:
yunxiao.zhu
2026-05-28 10:44:03 +08:00
parent 4eeaa3fef3
commit 35bdb08d11
144 changed files with 33730 additions and 392 deletions
Download Patch File Download Diff File Expand all files Collapse all files

285
tests/Flyshot.Core.Tests/ConfigCompatibilityTests.cs
View File

@@ -1,37 +1,39 @@
using Flyshot.Core.Config;
using Flyshot.Core.Domain;
namespace Flyshot.Core.Tests;
/// <summary>
/// 锁定 Task 3 的兼容输入行为，确保旧配置、.robot 元数据和路径策略都能被稳定加载。
/// 锁定 Task 3 的兼容输入行为，确保旧配置、JSON 模型元数据和路径策略都能被稳定加载。
/// </summary>
public sealed class ConfigCompatibilityTests
{
/// <summary>
/// 验证现有 RobotConfig.json 能被加载，并保持关键机器人参数与飞拍程序内容不变。
/// 验证现有旧样本配置能被加载，并保持关键机器人参数与飞拍程序内容不变。
/// </summary>
[Fact]
public void RobotConfigLoader_LoadsLegacyRobotConfig_AndPreservesPrograms()
{
var workspaceRoot = GetWorkspaceRoot();
var configPath = Path.Combine(workspaceRoot, "Rvbust", "EOL10_EAU_0", "RobotConfig.json");
var configPath = Path.Combine(workspaceRoot, "Rvbust", "EOL10_EAU_0", "EOL10_EAU_0.json");
var loaded = new RobotConfigLoader().Load(configPath);
Assert.True(loaded.Robot.UseDo);
Assert.Equal([7, 8], loaded.Robot.IoAddresses);
Assert.Equal(2, loaded.Robot.IoKeepCycles);
Assert.Equal(7, loaded.Robot.TriggerSampleIndexOffsetCycles);
Assert.Equal(1.0, loaded.Robot.AccLimitScale);
Assert.Equal(1.0, loaded.Robot.JerkLimitScale);
Assert.Equal(1.0, loaded.Robot.PlanningSpeedScale);
Assert.True(loaded.Robot.SmoothStartStopTiming);
Assert.Equal(5, loaded.Robot.AdaptIcspTryNum);
var program = Assert.Contains("001", loaded.Programs);
Assert.Equal("001", program.Name);
Assert.Equal(5, program.Waypoints.Count);
Assert.Equal(3, program.ShotWaypointCount);
var program = Assert.Contains("EOL10_EAU_0", loaded.Programs);
Assert.Equal("EOL10_EAU_0", program.Name);
Assert.Equal(45, program.Waypoints.Count);
Assert.Equal(42, program.ShotWaypointCount);
Assert.Empty(program.AddressGroups[0].Addresses);
Assert.Equal([8, 7], program.AddressGroups[1].Addresses);
Assert.Equal([4, 3], program.AddressGroups[1].Addresses);
}
/// <summary>
@@ -69,8 +71,12 @@ public sealed class ConfigCompatibilityTests
Assert.False(loaded.Robot.UseDo);
Assert.Empty(loaded.Robot.IoAddresses);
Assert.Equal(3, loaded.Robot.IoKeepCycles);
Assert.Equal(7, loaded.Robot.TriggerSampleIndexOffsetCycles);
Assert.Equal(0.5, loaded.Robot.AccLimitScale);
Assert.Equal(0.25, loaded.Robot.JerkLimitScale);
Assert.Equal(1.0, loaded.Robot.PlanningSpeedScale);
Assert.Equal(1.0, loaded.Robot.SpeedRatio);
Assert.True(loaded.Robot.SmoothStartStopTiming);
Assert.Equal([0, 0, 0], program.OffsetValues);
Assert.All(program.AddressGroups, group => Assert.Empty(group.Addresses));
}
@@ -81,13 +87,173 @@ public sealed class ConfigCompatibilityTests
}
/// <summary>
/// 验证 .robot 解析会保留 Joint3 对 Joint2 的 couple 元数据，并构造规划侧可直接消费的 RobotProfile。
/// 验证 RobotConfig.json 可以显式配置运行时默认速度倍率，供 MoveJoint 和后续执行链路初始化使用。
/// </summary>
[Fact]
public void RobotConfigLoader_LoadsRuntimeSpeedRatio()
{
var tempRoot = CreateTempDirectory();
try
{
var configPath = Path.Combine(tempRoot, "legacy.json");
File.WriteAllText(
configPath,
"""
{
"robot": {
"use_do": true,
"io_keep_cycles": 2,
"acc_limit": 1.0,
"jerk_limit": 1.0,
"speed_ratio": 0.65
},
"flying_shots": {
"demo": {
"traj_waypoints": [[0, 1], [2, 3], [4, 5], [6, 7]],
"shot_flags": [false, false, false, false]
}
}
}
""");
var loaded = new RobotConfigLoader().Load(configPath);
Assert.Equal(0.65, loaded.Robot.SpeedRatio, precision: 6);
}
finally
{
Directory.Delete(tempRoot, recursive: true);
}
}
/// <summary>
/// 验证 RobotConfig.json 可以显式配置规划限速倍率，且该倍率独立于运行时 J519 速度倍率。
/// </summary>
[Fact]
public void RobotConfigLoader_LoadsPlanningSpeedScale()
{
var tempRoot = CreateTempDirectory();
try
{
var configPath = Path.Combine(tempRoot, "legacy.json");
File.WriteAllText(
configPath,
"""
{
"robot": {
"use_do": true,
"io_keep_cycles": 2,
"acc_limit": 1.0,
"jerk_limit": 1.0,
"planning_speed_scale": 0.742277
},
"flying_shots": {
"demo": {
"traj_waypoints": [[0, 1], [2, 3], [4, 5], [6, 7]],
"shot_flags": [false, false, false, false]
}
}
}
""");
var loaded = new RobotConfigLoader().Load(configPath);
Assert.Equal(0.742277, loaded.Robot.PlanningSpeedScale, precision: 6);
}
finally
{
Directory.Delete(tempRoot, recursive: true);
}
}
/// <summary>
/// 验证 RobotConfig.json 可以显式配置触发绑定后的命令 sample 后移周期数。
/// </summary>
[Fact]
public void RobotConfigLoader_LoadsTriggerSampleIndexOffsetCycles()
{
var tempRoot = CreateTempDirectory();
try
{
var configPath = Path.Combine(tempRoot, "legacy.json");
File.WriteAllText(
configPath,
"""
{
"robot": {
"use_do": true,
"io_keep_cycles": 2,
"trigger_sample_index_offset_cycles": 8,
"acc_limit": 1.0,
"jerk_limit": 1.0
},
"flying_shots": {
"demo": {
"traj_waypoints": [[0, 1], [2, 3], [4, 5], [6, 7]],
"shot_flags": [false, false, false, false]
}
}
}
""");
var loaded = new RobotConfigLoader().Load(configPath);
Assert.Equal(8, loaded.Robot.TriggerSampleIndexOffsetCycles);
}
finally
{
Directory.Delete(tempRoot, recursive: true);
}
}
/// <summary>
/// 验证 RobotConfig.json 可以关闭飞拍执行前的二次平滑起停时间重映射。
/// </summary>
[Fact]
public void RobotConfigLoader_LoadsSmoothStartStopTimingSwitch()
{
var tempRoot = CreateTempDirectory();
try
{
var configPath = Path.Combine(tempRoot, "legacy.json");
File.WriteAllText(
configPath,
"""
{
"robot": {
"use_do": true,
"io_keep_cycles": 2,
"acc_limit": 1.0,
"jerk_limit": 1.0,
"smooth_start_stop_timing": false
},
"flying_shots": {
"demo": {
"traj_waypoints": [[0, 1], [2, 3], [4, 5], [6, 7]],
"shot_flags": [false, false, false, false]
}
}
}
""");
var loaded = new RobotConfigLoader().Load(configPath);
Assert.False(loaded.Robot.SmoothStartStopTiming);
}
finally
{
Directory.Delete(tempRoot, recursive: true);
}
}
/// <summary>
/// 验证 JSON 模型解析会保留 Joint3 对 Joint2 的 couple 元数据，并构造规划侧可直接消费的 RobotProfile。
/// </summary>
[Fact]
public void RobotModelLoader_LoadsRobotProfile_WithJointLimitsAndCoupling()
{
var workspaceRoot = GetWorkspaceRoot();
var modelPath = Path.Combine(workspaceRoot, "FlyingShot", "FlyingShot", "Models", "LR_Mate_200iD_7L.robot");
var replacementRoot = GetReplacementRoot();
var modelPath = Path.Combine(replacementRoot, "Config", "Models", "LR_Mate_200iD_7L.json");
var profile = new RobotModelLoader().LoadProfile(modelPath);
@@ -105,13 +271,13 @@ public sealed class ConfigCompatibilityTests
}
/// <summary>
/// 验证 RobotConfig 中的 acc_limit 和 jerk_limit 乘子会正确叠加到模型关节限制上。
/// 验证 RobotConfig 中的 acc_limit 和 jerk_limit 乘子会正确叠加到 JSON 模型关节限制上。
/// </summary>
[Fact]
public void RobotModelLoader_AppliesAccelerationAndJerkScales()
{
var workspaceRoot = GetWorkspaceRoot();
var modelPath = Path.Combine(workspaceRoot, "FlyingShot", "FlyingShot", "Models", "LR_Mate_200iD_7L.robot");
var replacementRoot = GetReplacementRoot();
var modelPath = Path.Combine(replacementRoot, "Config", "Models", "LR_Mate_200iD_7L.json");
var profile = new RobotModelLoader().LoadProfile(modelPath, accLimitScale: 0.5, jerkLimitScale: 0.25);
@@ -120,10 +286,57 @@ public sealed class ConfigCompatibilityTests
}
/// <summary>
/// 验证路径兼容层既能补旧目录候选，也能按平台策略生成默认用户数据目录。
/// 验证 JSON 模型可一次解析后同时生成规划约束视图和运动学几何视图。
/// </summary>
[Fact]
public void PathCompatibility_ResolvesLegacyCandidates_AndBuildsUserDataRoots()
public void RobotModelLoader_LoadsProfileAndKinematics_FromSingleParse()
{
var replacementRoot = GetReplacementRoot();
var modelPath = Path.Combine(replacementRoot, "Config", "Models", "LR_Mate_200iD_7L.json");
var loaded = new RobotModelLoader().LoadProfileAndKinematics(modelPath, accLimitScale: 0.5, jerkLimitScale: 0.25);
Assert.Equal("FANUC_LR_Mate_200iD_7L", loaded.Profile.Name);
Assert.Equal(modelPath, loaded.Profile.ModelPath);
Assert.Equal(6, loaded.Profile.DegreesOfFreedom);
Assert.Equal(14.905, loaded.Profile.JointLimits[2].AccelerationLimit, precision: 3);
Assert.Equal(62.115, loaded.Profile.JointLimits[2].JerkLimit, precision: 3);
Assert.Equal("FANUC_LR_Mate_200iD_7L", loaded.KinematicsModel.Name);
Assert.True(loaded.KinematicsModel.Joints.Count >= loaded.Profile.DegreesOfFreedom);
Assert.Contains(loaded.KinematicsModel.Joints, static joint => joint.Name == "Joint3" && joint.CoupleMaster == "Joint2");
}
/// <summary>
/// 验证路径兼容层只从当前服务配置目录解析相对配置，并按平台策略生成默认用户数据目录。
/// </summary>
[Fact]
public void PathCompatibility_ResolvesConfigDirectoryOnly_AndBuildsUserDataRoots()
{
var tempRoot = CreateTempDirectory();
try
{
var configPath = Path.Combine(tempRoot, "Config", "sample.json");
Directory.CreateDirectory(Path.GetDirectoryName(configPath)!);
File.WriteAllText(configPath, "{}");
var resolved = PathCompatibility.ResolveConfigPath("sample.json", tempRoot);
Assert.Equal(configPath, resolved);
Assert.Equal("/home/tester/.Rvbust/Data", PathCompatibility.BuildUserDataRoot("/home/tester", CompatibilityPathStyle.Posix));
Assert.Equal(@"C:\Users\tester\.Rvbust\Data", PathCompatibility.BuildUserDataRoot(@"C:\Users\tester", CompatibilityPathStyle.Windows));
}
finally
{
Directory.Delete(tempRoot, recursive: true);
}
}
/// <summary>
/// 验证旧父工作区候选路径存在时也不会被相对配置解析隐式命中。
/// </summary>
[Fact]
public void PathCompatibility_DoesNotResolveLegacyWorkspaceFallbacks()
{
var tempRoot = CreateTempDirectory();
try
@@ -132,11 +345,9 @@ public sealed class ConfigCompatibilityTests
Directory.CreateDirectory(Path.GetDirectoryName(legacyConfigPath)!);
File.WriteAllText(legacyConfigPath, "{}");
var resolved = PathCompatibility.ResolveConfigPath("sample.json", tempRoot);
var exception = Assert.Throws<FileNotFoundException>(() => PathCompatibility.ResolveConfigPath("sample.json", tempRoot));
Assert.Equal(legacyConfigPath, resolved);
Assert.Equal("/home/tester/.Rvbust/Data", PathCompatibility.BuildUserDataRoot("/home/tester", CompatibilityPathStyle.Posix));
Assert.Equal(@"C:\Users\tester\.Rvbust\Data", PathCompatibility.BuildUserDataRoot(@"C:\Users\tester", CompatibilityPathStyle.Windows));
Assert.Equal("sample.json", exception.FileName);
}
finally
{
@@ -144,6 +355,19 @@ public sealed class ConfigCompatibilityTests
}
}
/// <summary>
/// 验证默认加载配置时使用当前 replacement 仓库内的 Config/RobotConfig.json。
/// </summary>
[Fact]
public void RobotConfigLoader_LoadsRepositoryConfigFromReplacementConfigDirectory()
{
var replacementRoot = GetReplacementRoot();
var loaded = new RobotConfigLoader().Load("RobotConfig.json");
Assert.Equal(Path.Combine(replacementRoot, "Config", "RobotConfig.json"), loaded.SourcePath);
}
/// <summary>
/// 定位当前工作区根目录，便于复用父仓库中的真实样本。
/// </summary>
@@ -164,6 +388,25 @@ public sealed class ConfigCompatibilityTests
throw new DirectoryNotFoundException("Unable to locate the flyshot workspace root.");
}
/// <summary>
/// 定位 replacement 仓库根目录，供测试读取仓库内固化配置。
/// </summary>
private static string GetReplacementRoot()
{
var current = new DirectoryInfo(AppContext.BaseDirectory);
while (current is not null)
{
if (File.Exists(Path.Combine(current.FullName, "FlyshotReplacement.sln")))
{
return current.FullName;
}
current = current.Parent;
}
throw new DirectoryNotFoundException("Unable to locate the flyshot replacement root.");
}
/// <summary>
/// 创建当前测试专用的临时目录，避免不同测试之间相互污染。
/// </summary>

153
tests/Flyshot.Core.Tests/ControllerClientCompatConfigRootTests.cs Normal file
View File

@@ -0,0 +1,153 @@
using Flyshot.ControllerClientCompat;
using Flyshot.Core.Config;
namespace Flyshot.Core.Tests;
/// <summary>
/// 验证 ControllerClient 兼容层默认围绕运行目录 Config 读写配置和轨迹文件。
/// </summary>
public sealed class ControllerClientCompatConfigRootTests
{
/// <summary>
/// 验证路径兼容层优先命中运行目录 Config 下的 RobotConfig.json，而不是旧仓库根目录候选。
/// </summary>
[Fact]
public void PathCompatibility_ResolvesRuntimeConfigBeforeLegacyCandidates()
{
var runtimeRoot = CreateTempDirectory();
try
{
var configPath = Path.Combine(runtimeRoot, "Config", "RobotConfig.json");
var legacyPath = Path.Combine(runtimeRoot, "RobotConfig.json");
Directory.CreateDirectory(Path.GetDirectoryName(configPath)!);
File.WriteAllText(configPath, "{}");
File.WriteAllText(legacyPath, "{}");
var resolved = PathCompatibility.ResolveConfigPath("RobotConfig.json", runtimeRoot);
Assert.Equal(configPath, resolved);
}
finally
{
Directory.Delete(runtimeRoot, recursive: true);
}
}
/// <summary>
/// 验证机器人目录优先从显式 ConfigRoot/Models 加载现场 JSON 模型文件。
/// </summary>
[Fact]
public void ControllerClientCompatRobotCatalog_LoadsModelFromConfigRootModels()
{
var configRoot = CreateTempConfigRoot();
try
{
CopySampleRobotModel(configRoot);
var options = new ControllerClientCompatOptions { ConfigRoot = configRoot };
var catalog = new ControllerClientCompatRobotCatalog(options, new RobotModelLoader());
var profile = catalog.LoadProfile("FANUC_LR_Mate_200iD");
Assert.Equal(Path.Combine(configRoot, "Models", "LR_Mate_200iD_7L.json"), profile.ModelPath);
}
finally
{
Directory.Delete(configRoot, recursive: true);
}
}
/// <summary>
/// 验证 JSON 轨迹存储保存、加载和删除都落在 ConfigRoot/RobotConfig.json。
/// </summary>
[Fact]
public void JsonFlyshotTrajectoryStore_PersistsTrajectoriesInRobotConfigJson()
{
var configRoot = CreateTempConfigRoot();
try
{
var options = new ControllerClientCompatOptions { ConfigRoot = configRoot };
var store = new JsonFlyshotTrajectoryStore(options, new RobotConfigLoader());
var settings = new CompatibilityRobotSettings(
useDo: true,
ioAddresses: [7, 8],
ioKeepCycles: 2,
triggerSampleIndexOffsetCycles: 7,
accLimitScale: 1.0,
jerkLimitScale: 1.0,
adaptIcspTryNum: 5,
speedRatio: 0.65);
var trajectory = TestRobotFactory.CreateUploadedTrajectoryWithSingleShot();
store.Save("FANUC_LR_Mate_200iD", settings, trajectory);
var expectedPath = Path.Combine(configRoot, "RobotConfig.json");
Assert.True(File.Exists(expectedPath), $"应在运行目录 Config 下创建统一配置文件: {expectedPath}");
Assert.False(Directory.Exists(Path.Combine(configRoot, "TrajectoryStore")), "不应再创建独立轨迹存储目录。");
var loaded = store.LoadAll("FANUC_LR_Mate_200iD", out var loadedSettings);
Assert.NotNull(loadedSettings);
Assert.Equal(7, loadedSettings.TriggerSampleIndexOffsetCycles);
Assert.Equal(0.65, loadedSettings.SpeedRatio, precision: 6);
Assert.Contains(trajectory.Name, loaded);
store.Delete("FANUC_LR_Mate_200iD", trajectory.Name);
var afterDelete = store.LoadAll("FANUC_LR_Mate_200iD", out _);
Assert.Empty(afterDelete);
}
finally
{
Directory.Delete(configRoot, recursive: true);
}
}
/// <summary>
/// 创建测试专用的运行目录 Config 根，避免污染真实输出目录。
/// </summary>
private static string CreateTempConfigRoot()
{
var root = Path.Combine(Path.GetTempPath(), "flyshot-config-root-tests", Guid.NewGuid().ToString("N"), "Config");
Directory.CreateDirectory(root);
return root;
}
/// <summary>
/// 创建测试专用的临时目录。
/// </summary>
private static string CreateTempDirectory()
{
var root = Path.Combine(Path.GetTempPath(), "flyshot-config-root-tests", Guid.NewGuid().ToString("N"));
Directory.CreateDirectory(root);
return root;
}
/// <summary>
/// 复制仓库内已固化的现场机器人 JSON 模型到临时 Config/Models 目录。
/// </summary>
private static void CopySampleRobotModel(string configRoot)
{
var modelDir = Path.Combine(configRoot, "Models");
Directory.CreateDirectory(modelDir);
File.Copy(
Path.Combine(GetReplacementRoot(), "Config", "Models", "LR_Mate_200iD_7L.json"),
Path.Combine(modelDir, "LR_Mate_200iD_7L.json"));
}
/// <summary>
/// 定位 replacement 仓库根目录，供测试读取仓库内固化样本。
/// </summary>
private static string GetReplacementRoot()
{
var current = new DirectoryInfo(AppContext.BaseDirectory);
while (current is not null)
{
if (File.Exists(Path.Combine(current.FullName, "FlyshotReplacement.sln")))
{
return current.FullName;
}
current = current.Parent;
}
throw new DirectoryNotFoundException("Unable to locate the flyshot replacement root.");
}
}

58
tests/Flyshot.Core.Tests/DomainModelTests.cs
View File

@@ -1,5 +1,6 @@
using System.Text.Json;
using Flyshot.Core.Domain;
using Flyshot.Core.Planning.Sampling;
namespace Flyshot.Core.Tests;
@@ -114,6 +115,27 @@ public sealed class DomainModelTests
Assert.Empty(snapshot.JointPositions);
Assert.Empty(snapshot.CartesianPose);
Assert.Empty(snapshot.ActiveAlarms);
Assert.Empty(snapshot.StateTailWords);
}
/// <summary>
/// 验证控制器快照会保留 TCP 10010 尾部状态字作为诊断字段。
/// </summary>
[Fact]
public void ControllerStateSnapshot_CopiesStateTailWordsForDiagnostics()
{
var snapshot = new ControllerStateSnapshot(
capturedAt: DateTimeOffset.Parse("2026-04-23T10:00:00+08:00"),
connectionState: "Connected",
isEnabled: true,
isInMotion: false,
speedRatio: 1.0,
stateTailWords: [2u, 0u, 0u, 1u]);
var json = JsonSerializer.Serialize(snapshot);
Assert.Equal([2u, 0u, 0u, 1u], snapshot.StateTailWords);
Assert.Contains("\"stateTailWords\":[2,0,0,1]", json);
}
/// <summary>
@@ -143,7 +165,8 @@ public sealed class DomainModelTests
triggerTime: 0.5,
offsetCycles: 0,
holdCycles: 1,
addressGroup: new IoAddressGroup(new[] { 100 }))
addressGroup: new IoAddressGroup(new[] { 100 }),
referenceJointsDegrees: new[] { 12.5, -3.0 })
},
artifacts: new[]
{
@@ -155,7 +178,8 @@ public sealed class DomainModelTests
failureReason: null,
usedCache: true,
originalWaypointCount: 4,
plannedWaypointCount: 5);
plannedWaypointCount: 5,
triggerSampleIndexOffsetCycles: 7);
var json = JsonSerializer.Serialize(result);
@@ -163,6 +187,36 @@ public sealed class DomainModelTests
Assert.Contains("\"method\":\"SelfAdaptIcsp\"", json);
Assert.Contains("\"kind\":\"JointDenseTrajectory\"", json);
Assert.Contains("\"usedCache\":true", json);
Assert.Contains("\"triggerSampleIndexOffsetCycles\":7", json);
Assert.Contains("\"referenceJointsDegrees\":[12.5,-3]", json);
}
/// <summary>
/// 验证触发绑定允许在最佳 sample 基础上继续向后偏移固定命令周期。
/// </summary>
[Fact]
public void TriggerSampleBinder_Bind_AppliesConfiguredSampleIndexOffset()
{
var trigger = new TrajectoryDoEvent(
waypointIndex: 1,
triggerTime: 0.008,
offsetCycles: 0,
holdCycles: 2,
addressGroup: new IoAddressGroup([2, 4]),
referenceJointsDegrees: [10.0, 20.0]);
var samples = new[]
{
new J519SendSample(sampleIndex: 0, sendTime: 0.0, trajectoryTime: 0.0, speedRatio: 1.0, jointsDegrees: [0.0, 0.0]),
new J519SendSample(sampleIndex: 1, sendTime: 0.008, trajectoryTime: 0.008, speedRatio: 1.0, jointsDegrees: [10.0, 20.0]),
new J519SendSample(sampleIndex: 2, sendTime: 0.016, trajectoryTime: 0.016, speedRatio: 1.0, jointsDegrees: [11.0, 21.0]),
new J519SendSample(sampleIndex: 3, sendTime: 0.024, trajectoryTime: 0.024, speedRatio: 1.0, jointsDegrees: [12.0, 22.0])
};
var binding = Assert.Single(Flyshot.Core.Planning.Sampling.TriggerSampleBinder.Bind([trigger], samples, sampleIndexOffsetCycles: 2));
Assert.True(binding.FoundInWindow);
Assert.Equal(3, binding.SampleIndex);
Assert.Equal(0.024, binding.Sample.SendTime, precision: 6);
}
/// <summary>

319
tests/Flyshot.Core.Tests/FanucCommandClientTests.cs Normal file
View File

@@ -0,0 +1,319 @@
using System.Net;
using System.Net.Sockets;
using Flyshot.Runtime.Fanuc.Protocol;
namespace Flyshot.Core.Tests;
/// <summary>
/// 验证 FANUC TCP 10012 命令客户端的帧收发与响应解析。
/// </summary>
public sealed class FanucCommandClientTests : IDisposable
{
private readonly TcpListener _listener;
private readonly CancellationTokenSource _cts = new();
/// <summary>
/// 在随机可用端口启动本地模拟控制器。
/// </summary>
public FanucCommandClientTests()
{
_listener = new TcpListener(IPAddress.Loopback, 0);
_listener.Start();
}
/// <summary>
/// 获取分配给本地模拟控制器的端口。
/// </summary>
private int Port => ((IPEndPoint)_listener.LocalEndpoint).Port;
/// <summary>
/// 清理模拟控制器和取消源。
/// </summary>
public void Dispose()
{
_cts.Cancel();
_listener.Stop();
_cts.Dispose();
}
/// <summary>
/// 验证命令客户端可以连接本地模拟控制器。
/// </summary>
[Fact]
public async Task ConnectAsync_ConnectsToLocalListener()
{
using var client = new FanucCommandClient();
var acceptTask = _listener.AcceptTcpClientAsync();
await client.ConnectAsync("127.0.0.1", Port, _cts.Token);
Assert.True(client.IsConnected);
// 确保模拟侧也完成握手
await acceptTask.WaitAsync(TimeSpan.FromSeconds(2), _cts.Token);
}
/// <summary>
/// 验证 StopProgram 命令帧与抓包样本一致，并能解析成功响应。
/// </summary>
[Fact]
public async Task StopProgramAsync_SendsCorrectFrameAndParsesSuccess()
{
using var client = new FanucCommandClient();
var handlerTask = RunSingleResponseControllerAsync(
Convert.FromHexString("646f7a0000001a0000210300000008525642555354534d7a6f64"),
Convert.FromHexString("646f7a0000001200002103000000007a6f64"),
_cts.Token);
await client.ConnectAsync("127.0.0.1", Port, _cts.Token);
var response = await client.StopProgramAsync("RVBUSTSM", _cts.Token);
Assert.True(response.IsSuccess);
Assert.Equal(FanucCommandMessageIds.StopProgram, response.MessageId);
await handlerTask.WaitAsync(TimeSpan.FromSeconds(2), _cts.Token);
}
/// <summary>
/// 验证 ResetRobot 空命令帧能正确发送并解析结果响应。
/// </summary>
[Fact]
public async Task ResetRobotAsync_SendsEmptyCommandAndParsesResponse()
{
using var client = new FanucCommandClient();
var expectedFrame = FanucCommandProtocol.PackEmptyCommand(FanucCommandMessageIds.ResetRobot);
var responseFrame = Convert.FromHexString("646f7a0000001200002100000000007a6f64");
var handlerTask = RunSingleResponseControllerAsync(expectedFrame, responseFrame, _cts.Token);
await client.ConnectAsync("127.0.0.1", Port, _cts.Token);
var response = await client.ResetRobotAsync(_cts.Token);
Assert.True(response.IsSuccess);
Assert.Equal(FanucCommandMessageIds.ResetRobot, response.MessageId);
await handlerTask.WaitAsync(TimeSpan.FromSeconds(2), _cts.Token);
}
/// <summary>
/// 验证 GetProgramStatus 命令帧能正确发送并解析程序状态响应。
/// </summary>
[Fact]
public async Task GetProgramStatusAsync_SendsFrameAndParsesStatusResponse()
{
using var client = new FanucCommandClient();
var expectedFrame = FanucCommandProtocol.PackProgramCommand(FanucCommandMessageIds.GetProgramStatus, "RVBUSTSM");
var responseFrame = Convert.FromHexString("646f7a000000160000200300000001000000007a6f64");
var handlerTask = RunSingleResponseControllerAsync(expectedFrame, responseFrame, _cts.Token);
await client.ConnectAsync("127.0.0.1", Port, _cts.Token);
var response = await client.GetProgramStatusAsync("RVBUSTSM", _cts.Token);
Assert.True(response.IsSuccess);
Assert.Equal(FanucCommandMessageIds.GetProgramStatus, response.MessageId);
Assert.Equal(1u, response.ProgramStatus);
await handlerTask.WaitAsync(TimeSpan.FromSeconds(2), _cts.Token);
}
/// <summary>
/// 验证 StartProgram 命令帧能正确发送并解析成功响应。
/// </summary>
[Fact]
public async Task StartProgramAsync_SendsCorrectFrameAndParsesSuccess()
{
using var client = new FanucCommandClient();
var expectedFrame = FanucCommandProtocol.PackProgramCommand(FanucCommandMessageIds.StartProgram, "RVBUSTSM");
var responseFrame = Convert.FromHexString("646f7a0000001200002102000000007a6f64");
var handlerTask = RunSingleResponseControllerAsync(expectedFrame, responseFrame, _cts.Token);
await client.ConnectAsync("127.0.0.1", Port, _cts.Token);
var response = await client.StartProgramAsync("RVBUSTSM", _cts.Token);
Assert.True(response.IsSuccess);
Assert.Equal(FanucCommandMessageIds.StartProgram, response.MessageId);
await handlerTask.WaitAsync(TimeSpan.FromSeconds(2), _cts.Token);
}
/// <summary>
/// 验证 GetSpeedRatio 发送空业务体命令，并按 ratio_int / 100.0 解析倍率。
/// </summary>
[Fact]
public async Task GetSpeedRatioAsync_SendsFrameAndParsesRatio()
{
using var client = new FanucCommandClient();
var handlerTask = RunSingleResponseControllerAsync(
FanucCommandProtocol.PackGetSpeedRatioCommand(),
FanucCommandProtocol.PackFrame(FanucCommandMessageIds.GetSpeedRatio, Convert.FromHexString("0000005a00000000")),
_cts.Token);
await client.ConnectAsync("127.0.0.1", Port, _cts.Token);
var response = await client.GetSpeedRatioAsync(_cts.Token);
Assert.True(response.IsSuccess);
Assert.Equal(0.9, response.Ratio, precision: 6);
await handlerTask.WaitAsync(TimeSpan.FromSeconds(2), _cts.Token);
}
/// <summary>
/// 验证 SetSpeedRatio 会把 double 倍率夹到 0..100 的整数百分比后下发。
/// </summary>
[Fact]
public async Task SetSpeedRatioAsync_SendsClampedPercentAndParsesSuccess()
{
using var client = new FanucCommandClient();
var handlerTask = RunSingleResponseControllerAsync(
FanucCommandProtocol.PackSetSpeedRatioCommand(2.0),
FanucCommandProtocol.PackFrame(FanucCommandMessageIds.SetSpeedRatio, Convert.FromHexString("00000000")),
_cts.Token);
await client.ConnectAsync("127.0.0.1", Port, _cts.Token);
var response = await client.SetSpeedRatioAsync(2.0, _cts.Token);
Assert.True(response.IsSuccess);
await handlerTask.WaitAsync(TimeSpan.FromSeconds(2), _cts.Token);
}
/// <summary>
/// 验证 GetTcp 会发送 tcp_id 请求，并解析 result_code + tcp_id + 7 个 float 位姿。
/// </summary>
[Fact]
public async Task GetTcpAsync_SendsFrameAndParsesPose()
{
using var client = new FanucCommandClient();
var handlerTask = RunSingleResponseControllerAsync(
FanucCommandProtocol.PackGetTcpCommand(1),
FanucCommandProtocol.PackFrame(
FanucCommandMessageIds.GetTcp,
Convert.FromHexString("00000000000000013f80000040000000404000000000000000000000000000003f800000")),
_cts.Token);
await client.ConnectAsync("127.0.0.1", Port, _cts.Token);
var response = await client.GetTcpAsync(1, _cts.Token);
Assert.True(response.IsSuccess);
Assert.Equal([1.0, 2.0, 3.0, 0.0, 0.0, 0.0, 1.0], response.Pose);
await handlerTask.WaitAsync(TimeSpan.FromSeconds(2), _cts.Token);
}
/// <summary>
/// 验证 SetTcp 会按 tcp_id + 7 个 float 位姿下发并解析结果码。
/// </summary>
[Fact]
public async Task SetTcpAsync_SendsFrameAndParsesSuccess()
{
using var client = new FanucCommandClient();
var handlerTask = RunSingleResponseControllerAsync(
FanucCommandProtocol.PackSetTcpCommand(1, [1.0, 2.0, 3.0, 0.0, 0.0, 0.0, 1.0]),
FanucCommandProtocol.PackFrame(FanucCommandMessageIds.SetTcp, Convert.FromHexString("00000000")),
_cts.Token);
await client.ConnectAsync("127.0.0.1", Port, _cts.Token);
var response = await client.SetTcpAsync(1, [1.0, 2.0, 3.0, 0.0, 0.0, 0.0, 1.0], _cts.Token);
Assert.True(response.IsSuccess);
await handlerTask.WaitAsync(TimeSpan.FromSeconds(2), _cts.Token);
}
/// <summary>
/// 验证 GetIo 会按 io_type、io_index 顺序请求，并解析 float IO 值。
/// </summary>
[Fact]
public async Task GetIoAsync_SendsFrameAndParsesValue()
{
using var client = new FanucCommandClient();
var handlerTask = RunSingleResponseControllerAsync(
FanucCommandProtocol.PackGetIoCommand(FanucIoTypes.DigitalOutput, 7),
FanucCommandProtocol.PackFrame(FanucCommandMessageIds.GetIo, Convert.FromHexString("000000003f800000")),
_cts.Token);
await client.ConnectAsync("127.0.0.1", Port, _cts.Token);
var response = await client.GetIoAsync(7, "DO", _cts.Token);
Assert.True(response.IsSuccess);
Assert.True(response.Value);
await handlerTask.WaitAsync(TimeSpan.FromSeconds(2), _cts.Token);
}
/// <summary>
/// 验证 SetIo 会按 io_type、io_index、float value 顺序下发并解析结果码。
/// </summary>
[Fact]
public async Task SetIoAsync_SendsFrameAndParsesSuccess()
{
using var client = new FanucCommandClient();
var handlerTask = RunSingleResponseControllerAsync(
FanucCommandProtocol.PackSetIoCommand(FanucIoTypes.DigitalOutput, 7, true),
FanucCommandProtocol.PackFrame(FanucCommandMessageIds.SetIo, Convert.FromHexString("00000000")),
_cts.Token);
await client.ConnectAsync("127.0.0.1", Port, _cts.Token);
var response = await client.SetIoAsync(7, true, "DO", _cts.Token);
Assert.True(response.IsSuccess);
await handlerTask.WaitAsync(TimeSpan.FromSeconds(2), _cts.Token);
}
/// <summary>
/// 验证命令响应 result_code 非零时，客户端会抛出可诊断异常而不是让上层误判成功。
/// </summary>
[Fact]
public async Task StopProgramAsync_NonZeroResultCode_ThrowsDiagnosticException()
{
using var client = new FanucCommandClient();
var handlerTask = RunSingleResponseControllerAsync(
FanucCommandProtocol.PackProgramCommand(FanucCommandMessageIds.StopProgram, "RVBUSTSM"),
Convert.FromHexString("646f7a00000012000021030000002a7a6f64"),
_cts.Token);
await client.ConnectAsync("127.0.0.1", Port, _cts.Token);
var exception = await Assert.ThrowsAsync<InvalidOperationException>(
() => client.StopProgramAsync("RVBUSTSM", _cts.Token));
Assert.Contains("0x2103", exception.Message);
Assert.Contains("42", exception.Message);
await handlerTask.WaitAsync(TimeSpan.FromSeconds(2), _cts.Token);
}
/// <summary>
/// 验证在连接前调用命令会抛出 InvalidOperationException。
/// </summary>
[Fact]
public async Task SendProgramCommandAsync_BeforeConnect_Throws()
{
using var client = new FanucCommandClient();
await Assert.ThrowsAsync<InvalidOperationException>(
() => client.StopProgramAsync("RVBUSTSM", _cts.Token));
}
/// <summary>
/// 启动模拟控制器，接收一条请求帧并比对期望内容，然后返回预设响应。
/// </summary>
private async Task RunSingleResponseControllerAsync(
byte[] expectedFrame,
byte[] responseFrame,
CancellationToken cancellationToken)
{
using var controller = await _listener.AcceptTcpClientAsync(cancellationToken);
await using var stream = controller.GetStream();
var buffer = new byte[expectedFrame.Length];
await ReadExactAsync(stream, buffer, cancellationToken);
Assert.Equal(expectedFrame, buffer);
await stream.WriteAsync(responseFrame, cancellationToken);
}
/// <summary>
/// 从流中精确读取指定长度的字节。
/// </summary>
private static async Task ReadExactAsync(NetworkStream stream, byte[] buffer, CancellationToken cancellationToken)
{
var totalRead = 0;
while (totalRead < buffer.Length)
{
var read = await stream.ReadAsync(buffer.AsMemory(totalRead), cancellationToken);
if (read == 0)
{
throw new IOException("模拟控制器读取到 EOF。");
}
totalRead += read;
}
}
}

1543
tests/Flyshot.Core.Tests/FanucControllerRuntimeDenseTests.cs Normal file
View File

File diff suppressed because it is too large Load Diff

32
tests/Flyshot.Core.Tests/FanucControllerRuntimeLoggingRegistrationTests.cs Normal file
View File

@@ -0,0 +1,32 @@
using Flyshot.Runtime.Common;
using Flyshot.Runtime.Fanuc;
using Microsoft.Extensions.DependencyInjection;
using System.Reflection;
namespace Flyshot.Core.Tests;
/// <summary>
/// 验证 FANUC 运行时通过依赖注入解析时，能够稳定拿到日志依赖。
/// </summary>
public sealed class FanucControllerRuntimeLoggingRegistrationTests
{
/// <summary>
/// 验证宿主按 IControllerRuntime 解析 FANUC 运行时时，不会因为可选日志参数而退回到无日志实例。
/// </summary>
[Fact]
public void ServiceProvider_Resolves_FanucControllerRuntime_WithLogger()
{
var services = new ServiceCollection();
services.AddLogging();
services.AddSingleton<IControllerRuntime, FanucControllerRuntime>();
using var serviceProvider = services.BuildServiceProvider();
var runtime = serviceProvider.GetRequiredService<IControllerRuntime>();
var concreteRuntime = Assert.IsType<FanucControllerRuntime>(runtime);
var loggerField = typeof(FanucControllerRuntime).GetField("_logger", BindingFlags.Instance | BindingFlags.NonPublic);
Assert.NotNull(loggerField);
Assert.NotNull(loggerField!.GetValue(concreteRuntime));
}
}

546
tests/Flyshot.Core.Tests/FanucJ519ClientTests.cs Normal file
View File

@@ -0,0 +1,546 @@
using System.Buffers.Binary;
using System.Net;
using System.Net.Sockets;
using Flyshot.Runtime.Fanuc.Protocol;
using Microsoft.Extensions.Logging;
namespace Flyshot.Core.Tests;
/// <summary>
/// 验证 FANUC UDP 60015 J519 运动客户端的初始化、状态包驱动发送与响应解析。
/// </summary>
public sealed class FanucJ519ClientTests : IDisposable
{
private readonly UdpClient _server;
private readonly CancellationTokenSource _cts = new();
/// <summary>
/// 在随机可用端口启动本地 UDP 模拟控制器。
/// </summary>
public FanucJ519ClientTests()
{
_server = new UdpClient(0);
}
/// <summary>
/// 获取分配给本地模拟控制器的端口。
/// </summary>
private int Port => ((IPEndPoint)_server.Client.LocalEndPoint!).Port;
/// <summary>
/// 清理模拟控制器和取消源。
/// </summary>
public void Dispose()
{
_cts.Cancel();
_server.Dispose();
_cts.Dispose();
}
/// <summary>
/// 验证连接时会发送初始化包。
/// </summary>
[Fact]
public async Task ConnectAsync_SendsInitPacket()
{
using var client = new FanucJ519Client();
var receiveTask = _server.ReceiveAsync(_cts.Token);
await client.ConnectAsync("127.0.0.1", Port, _cts.Token);
Assert.True(client.IsConnected);
var result = await receiveTask.AsTask().WaitAsync(TimeSpan.FromSeconds(2), _cts.Token);
Assert.Equal(FanucJ519Protocol.ControlPacketLength, result.Buffer.Length);
Assert.Equal(Convert.FromHexString("0000000000000001"), result.Buffer);
}
/// <summary>
/// 验证启动运动后必须等到状态包到达，不能由上位机本地 8ms 循环主动发命令。
/// </summary>
[Fact]
public async Task StartMotion_WaitsForStatusPacketBeforeSendingCommand()
{
using var client = new FanucJ519Client();
await client.ConnectAsync("127.0.0.1", Port, _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();
// 机器人尚未回状态包时，上位机不应自行发 64B command packet。
await Assert.ThrowsAsync<TimeoutException>(
() => _server.ReceiveAsync(_cts.Token).AsTask().WaitAsync(TimeSpan.FromMilliseconds(120)));
await client.StopMotionAsync(_cts.Token);
}
/// <summary>
/// 验证停止运动时会发送结束包。
/// </summary>
[Fact]
public async Task StopMotionAsync_SendsEndPacket()
{
using var client = new FanucJ519Client();
await client.ConnectAsync("127.0.0.1", Port, _cts.Token);
// 接收并丢弃初始化包。
await _server.ReceiveAsync(_cts.Token);
await client.StopMotionAsync(_cts.Token);
// 服务器应该收到结束包。
var endResult = await _server.ReceiveAsync(_cts.Token);
Assert.Equal(FanucJ519Protocol.ControlPacketLength, endResult.Buffer.Length);
Assert.Equal(Convert.FromHexString("0000000200000001"), endResult.Buffer);
}
/// <summary>
/// 验证响应解析和最新响应缓存。
/// </summary>
[Fact]
public async Task GetLatestResponse_ParsesIncomingResponse()
{
using var client = new FanucJ519Client();
await client.ConnectAsync("127.0.0.1", Port, _cts.Token);
// 接收初始化包，获取客户端端点。
var initResult = await _server.ReceiveAsync(_cts.Token);
var clientEndpoint = initResult.RemoteEndPoint;
// 构造 132B 响应包并发送回客户端。
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), 5);
responsePacket[0x0c] = 15; // 所有状态位为真。
BinaryPrimitives.WriteUInt32BigEndian(responsePacket.AsSpan(0x14, 4), 999u);
BinaryPrimitives.WriteSingleBigEndian(responsePacket.AsSpan(0x18, 4), 10.0f);
BinaryPrimitives.WriteSingleBigEndian(responsePacket.AsSpan(0x3c, 4), 0.5f);
BinaryPrimitives.WriteSingleBigEndian(responsePacket.AsSpan(0x60, 4), 1.0f);
await _server.SendAsync(responsePacket, clientEndpoint, _cts.Token);
// 给接收循环留出时间。
await Task.Delay(200, _cts.Token);
var latest = client.GetLatestResponse();
Assert.NotNull(latest);
Assert.Equal(5u, latest.Sequence);
Assert.True(latest.AcceptsCommand);
Assert.True(latest.ReceivedCommand);
Assert.True(latest.SystemReady);
Assert.True(latest.RobotInMotion);
Assert.Equal(999u, latest.Timestamp);
Assert.Equal(10.0, latest.Pose[0], precision: 6);
Assert.Equal(0.5, latest.JointDegrees[0], precision: 6);
Assert.Equal(1.0, latest.MotorCurrents[0], precision: 6);
}
/// <summary>
/// 验证收到状态包后，下一帧命令使用该状态包的序号。
/// </summary>
[Fact]
public async Task StartMotion_UsesLatestStatusSequenceForFirstCommand()
{
using var client = new FanucJ519Client();
await client.ConnectAsync("127.0.0.1", Port, _cts.Token);
var initResult = await _server.ReceiveAsync(_cts.Token);
var command = new FanucJ519Command(sequence: 1, targetJoints: [1.0, 0.0, 0.0, 0.0, 0.0, 0.0]);
client.UpdateCommand(command);
client.StartMotion();
await SendStatusPacketAsync(initResult.RemoteEndPoint, sequence: 100);
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>
/// 验证直角坐标命令会把 Data format 写为 0，并按通用目标槽位写入 X/Y/Z/W/P/R。
/// </summary>
[Fact]
public void PackCommandPacket_WritesCartesianDataFormatAndTargetValues()
{
var command = new FanucJ519Command(
sequence: 7,
targetValues: [100.0, 200.0, 300.0, 1.0, 2.0, 3.0, 0.0, 0.0, 0.0],
dataStyle: 0);
var packet = FanucJ519Protocol.PackCommandPacket(command);
Assert.Equal(0, packet[0x12]);
Assert.Equal(100.0f, BinaryPrimitives.ReadSingleBigEndian(packet.AsSpan(0x1c, 4)));
Assert.Equal(200.0f, BinaryPrimitives.ReadSingleBigEndian(packet.AsSpan(0x20, 4)));
Assert.Equal(300.0f, BinaryPrimitives.ReadSingleBigEndian(packet.AsSpan(0x24, 4)));
Assert.Equal(1.0f, BinaryPrimitives.ReadSingleBigEndian(packet.AsSpan(0x28, 4)));
Assert.Equal(2.0f, BinaryPrimitives.ReadSingleBigEndian(packet.AsSpan(0x2c, 4)));
Assert.Equal(3.0f, BinaryPrimitives.ReadSingleBigEndian(packet.AsSpan(0x30, 4)));
}
/// <summary>
/// 验证配置 J519 buffer size 后，实际回发命令序号会在状态包序号基础上增加该缓冲深度。
/// </summary>
[Fact]
public async Task StartMotion_AddsConfiguredBufferSizeToStatusSequence()
{
using var client = new FanucJ519Client();
await client.ConnectAsync("127.0.0.1", Port, _cts.Token);
var initResult = await _server.ReceiveAsync(_cts.Token);
client.SetSequenceBufferSize(8);
client.UpdateCommand(new FanucJ519Command(sequence: 1, targetJoints: [1.0, 0.0, 0.0, 0.0, 0.0, 0.0]));
client.StartMotion();
await SendStatusPacketAsync(initResult.RemoteEndPoint, sequence: 100);
var result = await _server.ReceiveAsync(_cts.Token);
Assert.Equal(FanucJ519Protocol.CommandPacketLength, result.Buffer.Length);
Assert.Equal(108u, BinaryPrimitives.ReadUInt32BigEndian(result.Buffer.AsSpan(0x08, 4)));
await client.StopMotionAsync(_cts.Token);
}
/// <summary>
/// 验证连续状态包会逐包驱动命令发送，并使用各自的状态包序号。
/// </summary>
[Fact]
public async Task StartMotion_SendsOneCommandForEachStatusPacketWithMatchingSequence()
{
using var client = new FanucJ519Client();
await client.ConnectAsync("127.0.0.1", Port, _cts.Token);
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 (uint sequence = 100; sequence < 104; sequence++)
{
await SendStatusPacketAsync(initResult.RemoteEndPoint, sequence);
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([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_DequeuesPreparedCommandsForStatusPacketsAndHoldsLastCommand()
{
using var client = new FanucJ519Client();
await client.ConnectAsync("127.0.0.1", Port, _cts.Token);
var initResult = await _server.ReceiveAsync(_cts.Token);
client.StartMotion();
client.LoadCommandQueue(
[
new FanucJ519Command(sequence: 0, targetJoints: [1.0, 0.0, 0.0, 0.0, 0.0, 0.0]),
new FanucJ519Command(sequence: 0, targetJoints: [2.0, 0.0, 0.0, 0.0, 0.0, 0.0]),
new FanucJ519Command(sequence: 0, targetJoints: [3.0, 0.0, 0.0, 0.0, 0.0, 0.0])
]);
var packets = new List<byte[]>();
for (uint sequence = 700; sequence < 704; sequence++)
{
await SendStatusPacketAsync(initResult.RemoteEndPoint, sequence);
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();
var firstJointTargets = packets
.Select(packet => BinaryPrimitives.ReadSingleBigEndian(packet.AsSpan(0x1c, 4)))
.ToArray();
Assert.Equal([700u, 701u, 702u, 703u], sequences);
Assert.Equal([1.0f, 2.0f, 3.0f, 3.0f], firstJointTargets);
Assert.True(client.IsCommandQueueDrainedForTests());
}
/// <summary>
/// 验证停止运动后可在同一连接内重启发送，命令序号仍由新的状态包决定。
/// </summary>
[Fact]
public async Task StartMotion_CanRestartAfterStopMotionAndUseNewStatusSequence()
{
using var client = new FanucJ519Client();
await client.ConnectAsync("127.0.0.1", Port, _cts.Token);
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(200u, 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();
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.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);
}
/// <summary>
/// 验证没有显式目标时，会使用最近一帧状态反馈里的关节角持续构造 hold 命令。
/// </summary>
[Fact]
public async Task StartMotion_WithoutExplicitCommand_HoldsLatestResponseJointDegrees()
{
using var client = new FanucJ519Client();
await client.ConnectAsync("127.0.0.1", Port, _cts.Token);
var initResult = await _server.ReceiveAsync(_cts.Token);
client.StartMotion();
await SendStatusPacketAsync(
initResult.RemoteEndPoint,
sequence: 401,
jointDegrees: [10.0, 20.0, 30.0, 40.0, 50.0, 60.0]);
var holdPacket = await _server.ReceiveAsync(_cts.Token).AsTask().WaitAsync(TimeSpan.FromSeconds(1), _cts.Token);
Assert.Equal(FanucJ519Protocol.CommandPacketLength, holdPacket.Buffer.Length);
Assert.Equal(401u, BinaryPrimitives.ReadUInt32BigEndian(holdPacket.Buffer.AsSpan(0x08, 4)));
Assert.Equal(10.0f, BinaryPrimitives.ReadSingleBigEndian(holdPacket.Buffer.AsSpan(0x1c, 4)), precision: 6);
Assert.Equal(20.0f, BinaryPrimitives.ReadSingleBigEndian(holdPacket.Buffer.AsSpan(0x20, 4)), precision: 6);
Assert.Equal(60.0f, BinaryPrimitives.ReadSingleBigEndian(holdPacket.Buffer.AsSpan(0x30, 4)), precision: 6);
await client.StopMotionAsync(_cts.Token);
}
/// <summary>
/// 验证存在显式目标时，状态反馈生成的 hold 命令不会覆盖当前待发送目标。
/// </summary>
[Fact]
public async Task StartMotion_WithExplicitCommand_PrefersExplicitTargetOverHoldCommand()
{
using var client = new FanucJ519Client();
await client.ConnectAsync("127.0.0.1", Port, _cts.Token);
var initResult = await _server.ReceiveAsync(_cts.Token);
client.UpdateCommand(new FanucJ519Command(sequence: 1, targetJoints: [1.0, 2.0, 3.0, 4.0, 5.0, 6.0]));
client.StartMotion();
await SendStatusPacketAsync(
initResult.RemoteEndPoint,
sequence: 402,
jointDegrees: [10.0, 20.0, 30.0, 40.0, 50.0, 60.0]);
var commandPacket = await _server.ReceiveAsync(_cts.Token).AsTask().WaitAsync(TimeSpan.FromSeconds(1), _cts.Token);
Assert.Equal(FanucJ519Protocol.CommandPacketLength, commandPacket.Buffer.Length);
Assert.Equal(402u, BinaryPrimitives.ReadUInt32BigEndian(commandPacket.Buffer.AsSpan(0x08, 4)));
Assert.Equal(1.0f, BinaryPrimitives.ReadSingleBigEndian(commandPacket.Buffer.AsSpan(0x1c, 4)), precision: 6);
Assert.Equal(2.0f, BinaryPrimitives.ReadSingleBigEndian(commandPacket.Buffer.AsSpan(0x20, 4)), precision: 6);
Assert.Equal(6.0f, BinaryPrimitives.ReadSingleBigEndian(commandPacket.Buffer.AsSpan(0x30, 4)), precision: 6);
await client.StopMotionAsync(_cts.Token);
}
/// <summary>
/// 验证在连接前调用 StartMotion 会抛出 InvalidOperationException。
/// </summary>
[Fact]
public void StartMotion_BeforeConnect_Throws()
{
using var client = new FanucJ519Client();
Assert.Throws<InvalidOperationException>(() => client.StartMotion());
}
/// <summary>
/// 验证状态包驱动发送能持续输出命令包。
/// </summary>
[Fact]
public async Task StartMotion_MaintainsStatusDrivenCommandStream()
{
using var client = new FanucJ519Client();
await client.ConnectAsync("127.0.0.1", Port, _cts.Token);
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);
client.StartMotion();
// 收集 5 个命令包到达时间戳和序号。
var timestamps = new List<DateTimeOffset>();
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)));
timestamps.Add(DateTimeOffset.UtcNow);
}
await client.StopMotionAsync(_cts.Token);
Assert.Equal([500u, 501u, 502u, 503u, 504u], sequences);
// 计算相邻包间隔并使用 CI 安全的宽松边界验证周期流仍在推进。
var intervals = new List<TimeSpan>();
for (var i = 1; i < timestamps.Count; i++)
{
intervals.Add(timestamps[i] - timestamps[i - 1]);
}
Assert.All(intervals, interval =>
{
Assert.True(interval > TimeSpan.Zero, $"间隔 {interval.TotalMilliseconds:F2}ms 必须为正。");
Assert.True(interval <= TimeSpan.FromMilliseconds(30), $"间隔 {interval.TotalMilliseconds:F2}ms 过长。");
});
}
/// <summary>
/// 验证状态变化日志会附带最近一次实际发送的目标关节轴，便于联调时对照控制目标。
/// </summary>
[Fact]
public async Task ReceiveLoop_LogsLastSentTargetJointsWhenStatusChanges()
{
var logger = new CapturingLogger<FanucJ519Client>();
using var client = new FanucJ519Client(logger);
await client.ConnectAsync("127.0.0.1", Port, _cts.Token);
var initResult = await _server.ReceiveAsync(_cts.Token);
client.UpdateCommand(new FanucJ519Command(sequence: 1, targetJoints: [1.0, 2.0, 3.0, 4.0, 5.0, 6.0]));
client.StartMotion();
await SendStatusPacketAsync(initResult.RemoteEndPoint, sequence: 42);
_ = await _server.ReceiveAsync(_cts.Token);
await Task.Delay(200, _cts.Token);
Assert.Contains(
logger.Entries,
entry => entry.Level == LogLevel.Information
&& entry.Message.Contains("J519 最后一条发送目标关节轴", StringComparison.Ordinal)
&& entry.Message.Contains("1.00000, 2.00000, 3.00000, 4.00000, 5.00000, 6.00000", StringComparison.Ordinal));
}
/// <summary>
/// 向被测 J519 客户端发送一帧最小状态包，用机器人侧 status sequence 驱动下一帧命令。
/// </summary>
private async Task SendStatusPacketAsync(
IPEndPoint clientEndpoint,
uint sequence,
IReadOnlyList<double>? jointDegrees = null)
{
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;
if (jointDegrees is not null)
{
for (var index = 0; index < Math.Min(6, jointDegrees.Count); index++)
{
BinaryPrimitives.WriteSingleBigEndian(
responsePacket.AsSpan(0x3c + index * sizeof(float), sizeof(float)),
(float)jointDegrees[index]);
}
}
await _server.SendAsync(responsePacket, clientEndpoint, _cts.Token);
}
/// <summary>
/// 收集测试过程中的结构化日志，便于断言运行期输出内容。
/// </summary>
private sealed class CapturingLogger<T> : ILogger<T>
{
/// <summary>
/// 获取已记录的日志条目。
/// </summary>
public List<LogEntry> Entries { get; } = [];
/// <summary>
/// 开始日志作用域；当前测试无需作用域，直接返回空对象。
/// </summary>
public IDisposable BeginScope<TState>(TState state)
where TState : notnull
{
return NullScope.Instance;
}
/// <summary>
/// 指示所有日志级别均启用，便于测试完整捕获输出。
/// </summary>
public bool IsEnabled(LogLevel logLevel)
{
return true;
}
/// <summary>
/// 记录一条格式化后的日志消息。
/// </summary>
public void Log<TState>(
LogLevel logLevel,
EventId eventId,
TState state,
Exception? exception,
Func<TState, Exception?, string> formatter)
{
Entries.Add(new LogEntry(logLevel, formatter(state, exception)));
}
/// <summary>
/// 表示一次日志记录。
/// </summary>
public sealed record LogEntry(LogLevel Level, string Message);
/// <summary>
/// 提供空日志作用域，避免测试中额外分配。
/// </summary>
private sealed class NullScope : IDisposable
{
/// <summary>
/// 获取单例空作用域。
/// </summary>
public static NullScope Instance { get; } = new();
/// <summary>
/// 释放空作用域；无需实际动作。
/// </summary>
public void Dispose()
{
}
}
}
}

238
tests/Flyshot.Core.Tests/FanucProtocolTests.cs Normal file
View File

@@ -0,0 +1,238 @@
using System.Buffers.Binary;
using Flyshot.Runtime.Fanuc.Protocol;
namespace Flyshot.Core.Tests;
/// <summary>
/// 验证 FANUC 真机三条通信链路的二进制协议基础与逆向抓包样本一致。
/// </summary>
public sealed class FanucProtocolTests
{
/// <summary>
/// 验证 TCP 10012 程序命令封包与抓包中的 StopProg("RVBUSTSM") 完全一致。
/// </summary>
[Fact]
public void CommandProtocol_PacksCapturedStopProgramFrame()
{
var frame = FanucCommandProtocol.PackProgramCommand(FanucCommandMessageIds.StopProgram, "RVBUSTSM");
Assert.Equal(
Convert.FromHexString("646f7a0000001a0000210300000008525642555354534d7a6f64"),
frame);
}
/// <summary>
/// 验证 TCP 10012 短响应和程序状态响应可以按抓包字段解析。
/// </summary>
[Fact]
public void CommandProtocol_ParsesCapturedResponses()
{
var stopResponse = FanucCommandProtocol.ParseResultResponse(
Convert.FromHexString("646f7a0000001200002103000000007a6f64"));
var statusResponse = FanucCommandProtocol.ParseProgramStatusResponse(
Convert.FromHexString("646f7a000000160000200300000001000000007a6f64"));
Assert.Equal(FanucCommandMessageIds.StopProgram, stopResponse.MessageId);
Assert.True(stopResponse.IsSuccess);
Assert.Equal(FanucCommandMessageIds.GetProgramStatus, statusResponse.MessageId);
Assert.True(statusResponse.IsSuccess);
Assert.Equal(1u, statusResponse.ProgramStatus);
}
/// <summary>
/// 验证 TCP 10012 的速度倍率、TCP 和 IO 请求体字段顺序与逆向文档一致。
/// </summary>
[Fact]
public void CommandProtocol_PacksParameterCommandBodies()
{
var setTcpFrame = FanucCommandProtocol.PackSetTcpCommand(1, [1.0, 2.0, 3.0, 0.0, 0.0, 0.0, 1.0]);
Assert.Equal(
Convert.FromHexString("646f7a0000000e000022067a6f64"),
FanucCommandProtocol.PackGetSpeedRatioCommand());
Assert.Equal(
Convert.FromHexString("646f7a0000001200002207000000507a6f64"),
FanucCommandProtocol.PackSetSpeedRatioCommand(0.8));
Assert.Equal(
Convert.FromHexString("646f7a0000001200002200000000017a6f64"),
FanucCommandProtocol.PackGetTcpCommand(1));
Assert.Equal(
Convert.FromHexString("646f7a000000160000220800000002000000077a6f64"),
FanucCommandProtocol.PackGetIoCommand(FanucIoTypes.DigitalOutput, 7));
Assert.Equal(
Convert.FromHexString("646f7a0000001a0000220900000002000000073f8000007a6f64"),
FanucCommandProtocol.PackSetIoCommand(FanucIoTypes.DigitalOutput, 7, true));
Assert.Equal(FanucCommandMessageIds.SetTcp, BinaryPrimitives.ReadUInt32BigEndian(setTcpFrame.AsSpan(7, 4)));
Assert.Equal(1u, BinaryPrimitives.ReadUInt32BigEndian(setTcpFrame.AsSpan(11, 4)));
Assert.Equal(1.0f, BinaryPrimitives.ReadSingleBigEndian(setTcpFrame.AsSpan(15, 4)));
Assert.Equal(1.0f, BinaryPrimitives.ReadSingleBigEndian(setTcpFrame.AsSpan(39, 4)));
}
/// <summary>
/// 验证 TCP 10012 参数响应解析使用各自不同的字段顺序。
/// </summary>
[Fact]
public void CommandProtocol_ParsesParameterResponses()
{
var speedRatioResponse = FanucCommandProtocol.ParseSpeedRatioResponse(
FanucCommandProtocol.PackFrame(
FanucCommandMessageIds.GetSpeedRatio,
Convert.FromHexString("0000005000000000")));
var tcpResponse = FanucCommandProtocol.ParseTcpResponse(
FanucCommandProtocol.PackFrame(
FanucCommandMessageIds.GetTcp,
Convert.FromHexString("00000000000000013f80000040000000404000000000000000000000000000003f800000")));
var ioResponse = FanucCommandProtocol.ParseIoResponse(
FanucCommandProtocol.PackFrame(
FanucCommandMessageIds.GetIo,
Convert.FromHexString("000000003f800000")));
Assert.True(speedRatioResponse.IsSuccess);
Assert.Equal(0.8, speedRatioResponse.Ratio, precision: 6);
Assert.True(tcpResponse.IsSuccess);
Assert.Equal(1u, tcpResponse.TcpId);
Assert.Equal([1.0, 2.0, 3.0, 0.0, 0.0, 0.0, 1.0], tcpResponse.Pose);
Assert.True(ioResponse.IsSuccess);
Assert.True(ioResponse.Value);
Assert.Equal(1.0, ioResponse.NumericValue, precision: 6);
}
/// <summary>
/// 验证 TCP 10010 状态帧可以从抓包样本解析出尾部状态槽位。
/// </summary>
[Fact]
public void StateProtocol_ParsesCapturedStateFrame()
{
var frame = FanucStateProtocol.ParseFrame(Convert.FromHexString(
"646f7a0000005a000000004388a23243f1ed7f43e9de6bc265031ec2b33cc3c278e0153f8742f53c3f128dbc929529bc7861d63cb0184c3c1ca1a7000000000000000000000000000000020000000000000000000000017a6f64"));
Assert.Equal(0u, frame.MessageId);
Assert.Equal(6, frame.Pose.Count);
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.JointRadians);
Assert.Equal(frame.JointOrExtensionValues.Skip(6), frame.ExternalAxes);
Assert.Equal(frame.TailWords, frame.RawTailWords);
Assert.Equal(2u, frame.StatusWord0);
Assert.Equal(0u, frame.StatusWord1);
Assert.Equal(0u, frame.StatusWord2);
Assert.Equal(1u, frame.StatusWord3);
}
/// <summary>
/// 验证 pcap 中多条唯一 TCP 10010 状态帧都符合固定 90B 布局。
/// </summary>
[Theory]
[InlineData("646f7a0000005a0000000040eac85a43b2ef4043aba8e9421ed9c1c2828105c2ed981f3fbdbda0bed4764ebe92aacc3efd9f0a3f317ce9be5d4580000000000000000000000000000000020000000000000000000000017a6f64")]
[InlineData("646f7a0000005a00000000415aab64440a5302439adef542b39739c293c441431d50423fcdb7003d862fe3beca5730bf60eab23f148e403f89269d000000000000000000000000000000020000000000000000000000017a6f64")]
[InlineData("646f7a0000005a000000004221b6f9440b9ce043a129ac42b292bac29cba78431bddcb3fc743213d90268dbeba5351bf64bc1b3f0cbdf73f826864000000000000000000000000000000020000000000000000000000017a6f64")]
public void StateProtocol_ParsesMultipleCapturedPcapFrames(string frameHex)
{
var frameBytes = Convert.FromHexString(frameHex);
var frame = FanucStateProtocol.ParseFrame(frameBytes);
Assert.Equal(FanucStateProtocol.StateFrameLength, frameBytes.Length);
Assert.Equal(6, frame.CartesianPose.Count);
Assert.Equal(6, frame.JointRadians.Count);
Assert.Equal(3, frame.ExternalAxes.Count);
Assert.Equal([2u, 0u, 0u, 1u], frame.RawTailWords);
}
/// <summary>
/// 验证 TCP 10010 状态帧会拒绝损坏的长度和 magic，避免后台循环缓存坏帧。
/// </summary>
[Fact]
public void StateProtocol_RejectsMalformedStateFrames()
{
var validFrame = Convert.FromHexString(
"646f7a0000005a000000004388a23243f1ed7f43e9de6bc265031ec2b33cc3c278e0153f8742f53c3f128dbc929529bc7861d63cb0184c3c1ca1a7000000000000000000000000000000020000000000000000000000017a6f64");
var wrongMagic = validFrame.ToArray();
wrongMagic[0] = 0;
var wrongLength = validFrame.ToArray();
wrongLength[6] = 0x59;
Assert.Throws<InvalidDataException>(() => FanucStateProtocol.ParseFrame(validFrame.AsSpan(0, validFrame.Length - 1)));
Assert.Throws<InvalidDataException>(() => FanucStateProtocol.ParseFrame(wrongMagic));
Assert.Throws<InvalidDataException>(() => FanucStateProtocol.ParseFrame(wrongLength));
}
/// <summary>
/// 验证 UDP 60015 的 J519 初始化、结束和命令包字段布局。
/// </summary>
[Fact]
public void J519Protocol_PacksControlAndCommandPackets()
{
var command = new FanucJ519Command(
sequence: 2,
targetJoints: [1.0, 2.0, 3.0, 4.0, 5.0, 6.0]);
var packet = FanucJ519Protocol.PackCommandPacket(command);
Assert.Equal(Convert.FromHexString("0000000000000001"), FanucJ519Protocol.PackInitPacket());
Assert.Equal(Convert.FromHexString("0000000200000001"), FanucJ519Protocol.PackEndPacket());
Assert.Equal(FanucJ519Protocol.CommandPacketLength, packet.Length);
Assert.Equal(1u, BinaryPrimitives.ReadUInt32BigEndian(packet.AsSpan(0x00, 4)));
Assert.Equal(1u, BinaryPrimitives.ReadUInt32BigEndian(packet.AsSpan(0x04, 4)));
Assert.Equal(2u, BinaryPrimitives.ReadUInt32BigEndian(packet.AsSpan(0x08, 4)));
Assert.Equal(2, packet[0x0d]);
Assert.Equal(1, packet[0x12]);
Assert.Equal(1.0f, BinaryPrimitives.ReadSingleBigEndian(packet.AsSpan(0x1c, 4)));
Assert.Equal(6.0f, BinaryPrimitives.ReadSingleBigEndian(packet.AsSpan(0x30, 4)));
Assert.Equal(0.0f, BinaryPrimitives.ReadSingleBigEndian(packet.AsSpan(0x38, 4)));
}
/// <summary>
/// 验证实时发送路径可以复用命令对象，并用机器人状态包序号覆盖出包序号。
/// </summary>
[Fact]
public void J519Protocol_PacksCommandWithOverrideSequenceIntoReusableBuffer()
{
var command = new FanucJ519Command(
sequence: 2,
targetJoints: [1.0, 2.0, 3.0, 4.0, 5.0, 6.0]);
var packet = new byte[FanucJ519Protocol.CommandPacketLength];
FanucJ519Protocol.PackCommandPacket(command, sequence: 456, packet);
Assert.Equal(456u, BinaryPrimitives.ReadUInt32BigEndian(packet.AsSpan(0x08, 4)));
Assert.Equal(1.0f, BinaryPrimitives.ReadSingleBigEndian(packet.AsSpan(0x1c, 4)));
Assert.Equal(6.0f, BinaryPrimitives.ReadSingleBigEndian(packet.AsSpan(0x30, 4)));
}
/// <summary>
/// 验证 UDP 60015 的 132 字节响应包字段可以被解析成状态位和关节反馈。
/// </summary>
[Fact]
public void J519Protocol_ParsesResponsePacket()
{
var packet = new byte[FanucJ519Protocol.ResponsePacketLength];
BinaryPrimitives.WriteUInt32BigEndian(packet.AsSpan(0x00, 4), 0);
BinaryPrimitives.WriteUInt32BigEndian(packet.AsSpan(0x04, 4), 1);
BinaryPrimitives.WriteUInt32BigEndian(packet.AsSpan(0x08, 4), 12);
packet[0x0c] = 15;
packet[0x0d] = 2;
BinaryPrimitives.WriteUInt16BigEndian(packet.AsSpan(0x0e, 2), 1);
BinaryPrimitives.WriteUInt16BigEndian(packet.AsSpan(0x10, 2), 255);
BinaryPrimitives.WriteUInt16BigEndian(packet.AsSpan(0x12, 2), 10);
BinaryPrimitives.WriteUInt32BigEndian(packet.AsSpan(0x14, 4), 1234);
BinaryPrimitives.WriteSingleBigEndian(packet.AsSpan(0x18, 4), 100.5f);
BinaryPrimitives.WriteSingleBigEndian(packet.AsSpan(0x3c, 4), 1.25f);
BinaryPrimitives.WriteSingleBigEndian(packet.AsSpan(0x60, 4), 2.5f);
var response = FanucJ519Protocol.ParseResponse(packet);
Assert.Equal(12u, response.Sequence);
Assert.Equal(15, response.Status);
Assert.True(response.AcceptsCommand);
Assert.True(response.ReceivedCommand);
Assert.True(response.SystemReady);
Assert.True(response.RobotInMotion);
Assert.Equal(10, response.ReadIoValue);
Assert.Equal(1234u, response.Timestamp);
Assert.Equal(100.5, response.Pose[0], precision: 6);
Assert.Equal(1.25, response.JointDegrees[0], precision: 6);
Assert.Equal(2.5, response.MotorCurrents[0], precision: 6);
}
}

263
tests/Flyshot.Core.Tests/FanucStateClientTests.cs Normal file
View File

@@ -0,0 +1,263 @@
using System.Buffers.Binary;
using System.Net;
using System.Net.Sockets;
using Flyshot.Runtime.Fanuc.Protocol;
namespace Flyshot.Core.Tests;
/// <summary>
/// 验证 FANUC TCP 10010 状态通道客户端的后台接收与缓存能力。
/// </summary>
public sealed class FanucStateClientTests : IDisposable
{
private readonly TcpListener _listener;
private readonly CancellationTokenSource _cts = new();
/// <summary>
/// 在随机可用端口启动本地模拟控制器。
/// </summary>
public FanucStateClientTests()
{
_listener = new TcpListener(IPAddress.Loopback, 0);
_listener.Start();
}
/// <summary>
/// 获取分配给本地模拟控制器的端口。
/// </summary>
private int Port => ((IPEndPoint)_listener.LocalEndpoint).Port;
/// <summary>
/// 清理模拟控制器和取消源。
/// </summary>
public void Dispose()
{
_cts.Cancel();
_listener.Stop();
_cts.Dispose();
}
/// <summary>
/// 验证状态客户端可以连接本地模拟控制器。
/// </summary>
[Fact]
public async Task ConnectAsync_ConnectsToLocalListener()
{
using var client = new FanucStateClient();
var acceptTask = _listener.AcceptTcpClientAsync();
await client.ConnectAsync("127.0.0.1", Port, _cts.Token);
Assert.True(client.IsConnected);
await acceptTask.WaitAsync(TimeSpan.FromSeconds(2), _cts.Token);
}
/// <summary>
/// 验证后台循环能正确解析抓包样本状态帧。
/// </summary>
[Fact]
public async Task GetLatestFrame_ReceivesAndParsesCapturedStateFrame()
{
using var client = new FanucStateClient();
var capturedFrame = Convert.FromHexString(
"646f7a0000005a000000004388a23243f1ed7f43e9de6bc265031ec2b33cc3c278e0153f8742f53c3f128dbc929529bc7861d63cb0184c3c1ca1a7000000000000000000000000000000020000000000000000000000017a6f64");
var handlerTask = RunStreamingControllerAsync(capturedFrame, _cts.Token);
await client.ConnectAsync("127.0.0.1", Port, _cts.Token);
// 给后台循环留出接收和解析的时间。
await Task.Delay(200, _cts.Token);
var latest = client.GetLatestFrame();
Assert.NotNull(latest);
Assert.Equal(0u, latest.MessageId);
Assert.Equal(6, latest.Pose.Count);
Assert.Equal(9, latest.JointOrExtensionValues.Count);
Assert.Equal([2u, 0u, 0u, 1u], latest.TailWords);
client.Disconnect();
await handlerTask.WaitAsync(TimeSpan.FromSeconds(2), _cts.Token);
}
/// <summary>
/// 验证状态客户端在连接前调用 GetLatestFrame 返回 null。
/// </summary>
[Fact]
public void GetLatestFrame_BeforeConnect_ReturnsNull()
{
using var client = new FanucStateClient();
Assert.Null(client.GetLatestFrame());
}
/// <summary>
/// 验证 Disconnect 后最新帧被清空。
/// </summary>
[Fact]
public async Task Disconnect_ClearsLatestFrame()
{
using var client = new FanucStateClient();
var capturedFrame = CapturedStateFrame();
var handlerTask = RunStreamingControllerAsync(capturedFrame, _cts.Token);
await client.ConnectAsync("127.0.0.1", Port, _cts.Token);
await Task.Delay(200, _cts.Token);
Assert.NotNull(client.GetLatestFrame());
client.Disconnect();
Assert.Null(client.GetLatestFrame());
await handlerTask.WaitAsync(TimeSpan.FromSeconds(2), _cts.Token);
}
/// <summary>
/// 验证状态通道长时间收不到完整帧时会标记陈旧并触发重连。
/// </summary>
[Fact]
public async Task GetStatus_MarksFrameStaleAndReconnectsWhenFrameTimesOut()
{
using var client = new FanucStateClient(new FanucStateClientOptions
{
FrameTimeout = TimeSpan.FromMilliseconds(100),
ReconnectInitialDelay = TimeSpan.FromMilliseconds(20),
ReconnectMaxDelay = TimeSpan.FromMilliseconds(50),
ConnectTimeout = TimeSpan.FromSeconds(1),
});
var acceptTask = _listener.AcceptTcpClientAsync(_cts.Token);
await client.ConnectAsync("127.0.0.1", Port, _cts.Token);
using var controller = await acceptTask.AsTask().WaitAsync(TimeSpan.FromSeconds(2), _cts.Token);
await WaitUntilAsync(
() => client.GetStatus().ReconnectAttemptCount > 0,
TimeSpan.FromSeconds(2),
_cts.Token);
var status = client.GetStatus();
Assert.True(status.IsFrameStale);
Assert.True(status.State is FanucStateConnectionState.TimedOut or FanucStateConnectionState.Reconnecting or FanucStateConnectionState.Connected);
Assert.NotNull(status.LastErrorMessage);
Assert.Contains("超时", status.LastErrorMessage);
}
/// <summary>
/// 验证状态通道在控制柜主动断开后可以退避重连并接收新连接上的状态帧。
/// </summary>
[Fact]
public async Task ReceiveLoop_ReconnectsAfterEofAndKeepsReceivingFrames()
{
using var client = new FanucStateClient(new FanucStateClientOptions
{
FrameTimeout = TimeSpan.FromMilliseconds(500),
ReconnectInitialDelay = TimeSpan.FromMilliseconds(20),
ReconnectMaxDelay = TimeSpan.FromMilliseconds(50),
ConnectTimeout = TimeSpan.FromSeconds(1),
});
var firstFrame = CapturedStateFrame(1);
var secondFrame = CapturedStateFrame(2);
var handlerTask = RunReconnectControllerAsync(firstFrame, secondFrame, _cts.Token);
await client.ConnectAsync("127.0.0.1", Port, _cts.Token);
await WaitUntilAsync(
() => client.GetLatestFrame()?.MessageId == 2u,
TimeSpan.FromSeconds(2),
_cts.Token);
var status = client.GetStatus();
Assert.Equal(FanucStateConnectionState.Connected, status.State);
Assert.True(status.ReconnectAttemptCount >= 1);
client.Disconnect();
await handlerTask.WaitAsync(TimeSpan.FromSeconds(2), _cts.Token);
}
/// <summary>
/// 启动模拟控制器，持续发送状态帧流。
/// </summary>
private async Task RunStreamingControllerAsync(byte[] frames, CancellationToken cancellationToken)
{
using var controller = await _listener.AcceptTcpClientAsync(cancellationToken);
await using var stream = controller.GetStream();
try
{
while (!cancellationToken.IsCancellationRequested)
{
await stream.WriteAsync(frames, cancellationToken);
await Task.Delay(50, cancellationToken);
}
}
catch (OperationCanceledException)
{
// 正常取消。
}
catch (IOException)
{
// 客户端断开。
}
}
/// <summary>
/// 启动模拟控制器：第一条连接发一帧后主动断开，第二条连接持续发送新帧。
/// </summary>
private async Task RunReconnectControllerAsync(byte[] firstFrame, byte[] secondFrame, CancellationToken cancellationToken)
{
using (var firstController = await _listener.AcceptTcpClientAsync(cancellationToken))
{
await using var firstStream = firstController.GetStream();
await firstStream.WriteAsync(firstFrame, cancellationToken);
}
using var secondController = await _listener.AcceptTcpClientAsync(cancellationToken);
await using var secondStream = secondController.GetStream();
try
{
while (!cancellationToken.IsCancellationRequested)
{
await secondStream.WriteAsync(secondFrame, cancellationToken);
await Task.Delay(50, cancellationToken);
}
}
catch (OperationCanceledException)
{
// 正常取消。
}
catch (IOException)
{
// 客户端断开。
}
}
/// <summary>
/// 构造来自 j519 抓包的状态帧，并按测试需要覆写 message_id。
/// </summary>
private static byte[] CapturedStateFrame(uint messageId = 0)
{
var frame = Convert.FromHexString(
"646f7a0000005a000000004388a23243f1ed7f43e9de6bc265031ec2b33cc3c278e0153f8742f53c3f128dbc929529bc7861d63cb0184c3c1ca1a7000000000000000000000000000000020000000000000000000000017a6f64");
BinaryPrimitives.WriteUInt32BigEndian(frame.AsSpan(7, 4), messageId);
return frame;
}
/// <summary>
/// 等待异步后台循环达到预期状态，超时后让测试明确失败。
/// </summary>
private static async Task WaitUntilAsync(Func<bool> predicate, TimeSpan timeout, CancellationToken cancellationToken)
{
var deadline = DateTimeOffset.UtcNow + timeout;
while (DateTimeOffset.UtcNow < deadline)
{
if (predicate())
{
return;
}
await Task.Delay(20, cancellationToken);
}
Assert.True(predicate(), "等待状态通道后台循环达到预期状态超时。");
}
}

2
tests/Flyshot.Core.Tests/Flyshot.Core.Tests.csproj
View File

@@ -23,10 +23,12 @@
</ItemGroup>
<ItemGroup>
<ProjectReference Include="..\..\src\Flyshot.ControllerClientCompat\Flyshot.ControllerClientCompat.csproj" />
<ProjectReference Include="..\..\src\Flyshot.Core.Config\Flyshot.Core.Config.csproj" />
<ProjectReference Include="..\..\src\Flyshot.Core.Domain\Flyshot.Core.Domain.csproj" />
<ProjectReference Include="..\..\src\Flyshot.Core.Planning\Flyshot.Core.Planning.csproj" />
<ProjectReference Include="..\..\src\Flyshot.Core.Triggering\Flyshot.Core.Triggering.csproj" />
<ProjectReference Include="..\..\src\Flyshot.Runtime.Fanuc\Flyshot.Runtime.Fanuc.csproj" />
</ItemGroup>
</Project>

233
tests/Flyshot.Core.Tests/J519SendTrajectorySamplerTests.cs Normal file
View File

@@ -0,0 +1,233 @@
using Flyshot.Core.Planning.Sampling;
using Flyshot.Core.Domain;
using Flyshot.Core.Planning;
namespace Flyshot.Core.Tests;
/// <summary>
/// 验证 J519 实发重采样器在离线导出和运行时下发之间保持一致的时间轴语义。
/// </summary>
public sealed class J519SendTrajectorySamplerTests
{
/// <summary>
/// 验证 speed_ratio 只缩放轨迹时间，物理发送时间仍按固定伺服周期推进。
/// </summary>
[Fact]
public void SampleDenseJointTrajectory_MapsSendTimeToScaledTrajectoryTimeAndDegrees()
{
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 samples = J519SendTrajectorySampler.SampleDenseJointTrajectory(
denseTrajectory,
durationSeconds: 0.016,
servoPeriodSeconds: 0.008,
speedRatio: 0.5);
Assert.Equal(5, samples.Count);
Assert.Equal(0, samples[0].SampleIndex);
Assert.Equal(0.0, samples[0].SendTime, precision: 6);
Assert.Equal(0.0, samples[0].TrajectoryTime, precision: 6);
Assert.Equal(0.0, samples[0].JointsDegrees[0], precision: 6);
Assert.Equal(1, samples[1].SampleIndex);
Assert.Equal(0.008, samples[1].SendTime, precision: 6);
Assert.Equal(0.004, samples[1].TrajectoryTime, precision: 6);
Assert.Equal(45.0, samples[1].JointsDegrees[0], precision: 6);
Assert.Equal(4, samples[^1].SampleIndex);
Assert.Equal(0.032, samples[^1].SendTime, precision: 6);
Assert.Equal(0.016, samples[^1].TrajectoryTime, precision: 6);
Assert.Equal(180.0, samples[^1].JointsDegrees[0], precision: 6);
}
/// <summary>
/// 验证飞拍 speed_ratio 安全队列从连续规划样条直接采样，而不是先把规划轨迹离散成稠密点后再线性插值。
/// </summary>
[Fact]
public void SamplePlannedTrajectory_DirectlyEvaluatesContinuousSplineForSpeedRatio()
{
var trajectory = CreateSingleJointTrajectory();
var samples = J519SendTrajectorySampler.SamplePlannedTrajectory(
trajectory,
servoPeriodSeconds: 0.008,
speedRatio: 0.5);
var sample = Assert.Single(samples.Where(static item => Math.Abs(item.TrajectoryTime - 0.004) < 1e-9));
Assert.Equal(0.008, sample.SendTime, precision: 6);
Assert.Equal(0.004, sample.TrajectoryTime, precision: 6);
Assert.NotEqual(45.0, sample.JointsDegrees[0], precision: 3);
var expected = new CubicSplineInterpolator(
trajectory.WaypointTimes.ToArray(),
trajectory.PlannedWaypoints.Select(static waypoint => waypoint.Positions.ToArray()).ToArray())
.Evaluate(0.004)[0] * 180.0 / Math.PI;
Assert.Equal(expected, sample.JointsDegrees[0], precision: 6);
}
/// <summary>
/// 验证 speed_ratio 等价于执行侧时间轴拉长：原轨迹 0.8 倍执行应与时间整体拉长 1/0.8 的轨迹 1 倍执行一致。
/// </summary>
[Fact]
public void SamplePlannedTrajectory_SpeedRatioMatchesUniformPlanningTimeStretch()
{
var trajectory = CreateSingleJointTrajectory();
var stretchedTrajectory = StretchTrajectoryTime(trajectory, stretchFactor: 1.0 / 0.8);
var ratioSamples = J519SendTrajectorySampler.SamplePlannedTrajectory(
trajectory,
servoPeriodSeconds: 0.008,
speedRatio: 0.8);
var stretchedSamples = J519SendTrajectorySampler.SamplePlannedTrajectory(
stretchedTrajectory,
servoPeriodSeconds: 0.008,
speedRatio: 1.0);
Assert.Equal(stretchedSamples.Count, ratioSamples.Count);
for (var index = 0; index < ratioSamples.Count; index++)
{
Assert.Equal(stretchedSamples[index].SendTime, ratioSamples[index].SendTime, precision: 6);
Assert.Equal(stretchedSamples[index].TrajectoryTime * 0.8, ratioSamples[index].TrajectoryTime, precision: 6);
Assert.Equal(stretchedSamples[index].JointsDegrees[0], ratioSamples[index].JointsDegrees[0], precision: 6);
}
}
/// <summary>
/// 验证空稠密轨迹会直接暴露为调用错误，避免生成无意义下发点。
/// </summary>
[Fact]
public void SampleDenseJointTrajectory_RejectsEmptyDenseTrajectory()
{
var exception = Assert.Throws<InvalidOperationException>(() =>
J519SendTrajectorySampler.SampleDenseJointTrajectory(
Array.Empty<IReadOnlyList<double>>(),
durationSeconds: 0.016,
servoPeriodSeconds: 0.008,
speedRatio: 1.0));
Assert.Contains("稠密关节轨迹为空", exception.Message);
}
/// <summary>
/// 验证非法 speed_ratio 会在公共入口统一拦截。
/// </summary>
[Theory]
[InlineData(0.0)]
[InlineData(double.NaN)]
[InlineData(double.PositiveInfinity)]
public void SampleDenseJointTrajectory_RejectsInvalidSpeedRatio(double speedRatio)
{
var denseTrajectory = new[]
{
new[] { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }
};
Assert.Throws<ArgumentOutOfRangeException>(() =>
J519SendTrajectorySampler.SampleDenseJointTrajectory(
denseTrajectory,
durationSeconds: 0.0,
servoPeriodSeconds: 0.008,
speedRatio: speedRatio));
}
/// <summary>
/// 验证公共诊断行格式与既有 ActualSendTiming 文件保持一致。
/// </summary>
[Fact]
public void BuildTimingRow_UsesLegacyActualSendColumnOrder()
{
var row = J519SendTrajectorySampler.BuildTimingRow(new J519SendSample(
sampleIndex: 2,
sendTime: 0.016,
trajectoryTime: 0.008,
speedRatio: 0.5,
jointsDegrees: [90.0, 0.0, 0.0, 0.0, 0.0, 0.0]));
Assert.Equal([2.0, 0.016, 0.008, 0.5], row);
}
/// <summary>
/// 验证倍率诊断行在保留旧 4 列的同时追加请求倍率和历史改写次数。
/// </summary>
[Fact]
public void BuildTimingRow_WithRatioDiagnostics_AppendsRequestedRatioAndIterationCount()
{
var sample = new J519SendSample(
sampleIndex: 2,
sendTime: 0.016,
trajectoryTime: 0.0076,
speedRatio: 0.475,
jointsDegrees: [90.0, 0.0, 0.0, 0.0, 0.0, 0.0]);
var row = J519SendTrajectorySampler.BuildTimingRow(
sample,
requestedSpeedRatio: 0.5,
stretchIterationCount: 1);
Assert.Equal([2.0, 0.016, 0.0076, 0.475, 0.5, 1.0], row);
}
/// <summary>
/// 构造一个最小单关节规划轨迹，便于验证 speed_ratio 直接样条采样语义。
/// </summary>
private static PlannedTrajectory CreateSingleJointTrajectory()
{
var robot = new RobotProfile(
name: "TestRobot",
modelPath: "Models/Test.robot",
degreesOfFreedom: 1,
jointLimits: [new JointLimit("J1", 100.0, 1000.0, 10000.0)],
jointCouplings: Array.Empty<JointCoupling>(),
servoPeriod: TimeSpan.FromMilliseconds(8),
triggerPeriod: TimeSpan.FromMilliseconds(8));
var program = new FlyshotProgram(
name: "spline-sample",
waypoints:
[
new JointWaypoint([0.0]),
new JointWaypoint([Math.PI / 2.0]),
new JointWaypoint([Math.PI])
],
shotFlags: [false, false, false],
offsetValues: [0, 0, 0],
addressGroups:
[
new IoAddressGroup(Array.Empty<int>()),
new IoAddressGroup(Array.Empty<int>()),
new IoAddressGroup(Array.Empty<int>())
]);
return new PlannedTrajectory(
robot: robot,
originalProgram: program,
plannedWaypoints: program.Waypoints,
waypointTimes: [0.0, 0.008, 0.016],
segmentDurations: [0.008, 0.008],
segmentScales: [1.0, 1.0],
method: PlanningMethod.Icsp,
iterations: 1,
threshold: 0.0);
}
/// <summary>
/// 构造统一拉长时间轴后的轨迹，模拟更低 planning_speed_scale 生成的等几何时间结果。
/// </summary>
private static PlannedTrajectory StretchTrajectoryTime(PlannedTrajectory trajectory, double stretchFactor)
{
return new PlannedTrajectory(
robot: trajectory.Robot,
originalProgram: trajectory.OriginalProgram,
plannedWaypoints: trajectory.PlannedWaypoints,
waypointTimes: trajectory.WaypointTimes.Select(time => time * stretchFactor).ToArray(),
segmentDurations: trajectory.SegmentDurations.Select(duration => duration * stretchFactor).ToArray(),
segmentScales: trajectory.SegmentScales.Select(scale => scale / stretchFactor).ToArray(),
method: trajectory.Method,
iterations: trajectory.Iterations,
threshold: trajectory.Threshold);
}
}

21
tests/Flyshot.Core.Tests/OfflinePlanTests.cs
View File

@@ -2,7 +2,6 @@ using Flyshot.Core.Config;
using Flyshot.Core.Domain;
using Flyshot.Core.Planning;
using Flyshot.Core.Planning.Export;
using Flyshot.Core.Planning.Kinematics;
using Flyshot.Core.Planning.Sampling;
using Flyshot.Core.Triggering;
using Xunit.Abstractions;
@@ -24,26 +23,30 @@ public sealed class OfflinePlanTests
/// 使用预设参数生成离线轨迹，输出到 analysis/output/dotnet/。
/// </summary>
[Theory]
[InlineData("Rvbust/EOL9 EAU 90/eol9_eau_90.json", "FlyingShot/FlyingShot/Models/LR_Mate_200iD_7L.robot", "EOL9_EAU_90", false, 1.0)]
[InlineData("Rvbust/EOL9 EAU 90/eol9_eau_90.json", "FlyingShot/FlyingShot/Models/LR_Mate_200iD_7L.robot", "EOL9_EAU_90", true, 0.9)]
[InlineData("Rvbust/EOL10_EAU_0/EOL10_EAU_0.json", "FlyingShot/FlyingShot/Models/LR_Mate_200iD_7L.robot", "EOL10_EAU_0", false, 1.0)]
[InlineData("Rvbust/EOL9 EAU 90/eol9_eau_90.json", "EOL9_EAU_90", false, 1.0)]
[InlineData("Rvbust/EOL9 EAU 90/eol9_eau_90.json", "EOL9_EAU_90", true, 0.9)]
[InlineData("Rvbust/EOL10_EAU_0/EOL10_EAU_0.json", "EOL10_EAU_0", false, 1.0)]
public void GenerateTrajectory_MatchesPythonDemo(
string configPath,
string robotModelPath,
string trajName,
bool useSelfAdapt,
double speedRatio)
{
var workspaceRoot = GetWorkspaceRoot();
var resolvedConfigPath = Path.Combine(workspaceRoot, configPath);
var outputDir = Path.Combine(workspaceRoot, "analysis", "output", "dotnet", $"{trajName}_sr{speedRatio:F2}_{(useSelfAdapt ? "adapt" : "icsp")}");
Directory.CreateDirectory(outputDir);
// 1. 加载配置和模型。
var loadedConfig = new RobotConfigLoader().Load(configPath, repoRoot: workspaceRoot);
var loadedConfig = new RobotConfigLoader().Load(resolvedConfigPath, repoRoot: workspaceRoot);
var program = loadedConfig.Programs[trajName];
var resolvedRobotModelPath = Path.Combine(workspaceRoot, robotModelPath);
var baseProfile = new RobotModelLoader().LoadProfile(resolvedRobotModelPath, loadedConfig.Robot.AccLimitScale, loadedConfig.Robot.JerkLimitScale);
var kinematicsModel = new RobotModelLoader().LoadKinematicsModel(resolvedRobotModelPath);
var resolvedRobotModelPath = Path.Combine(workspaceRoot, "flyshot-replacement", "Config", "Models", "LR_Mate_200iD_7L.json");
var loadedModel = new RobotModelLoader().LoadProfileAndKinematics(
resolvedRobotModelPath,
loadedConfig.Robot.AccLimitScale,
loadedConfig.Robot.JerkLimitScale);
var baseProfile = loadedModel.Profile;
var kinematicsModel = loadedModel.KinematicsModel;
// 2. 应用 speed_ratio 缩放。
var scaledProfile = ScaleRobotProfile(baseProfile, speedRatio);

267
tests/Flyshot.Core.Tests/PlanningCompatibilityTests.cs
View File

@@ -1,6 +1,7 @@
using Flyshot.Core.Config;
using Flyshot.Core.Domain;
using Flyshot.Core.Planning;
using Flyshot.Core.Planning.Sampling;
using Flyshot.Core.Triggering;
namespace Flyshot.Core.Tests;
@@ -34,6 +35,30 @@ public sealed class PlanningCompatibilityTests
Assert.All(trajectory.WaypointTimes.Zip(trajectory.WaypointTimes.Skip(1)), pair => Assert.True(pair.Second > pair.First));
}
/// <summary>
/// 验证普通 ICSP 在最终最优解仍超限时会显式失败，而不是返回不可执行轨迹。
/// </summary>
[Fact]
public void ICspPlanner_Throws_WhenFinalGlobalScaleExceedsOne()
{
var request = new TrajectoryRequest(
robot: CreateRobotProfile([0.1], [0.1], [0.1]),
program: CreateProgram(
new[]
{
new[] { 0.0 },
new[] { 10.0 },
new[] { 20.0 },
new[] { 30.0 }
}),
method: PlanningMethod.Icsp);
var planner = new ICspPlanner(maxIterations: 0);
var exception = Assert.Throws<InvalidOperationException>(() => planner.Plan(request));
Assert.Contains("global_scale", exception.Message);
}
/// <summary>
/// 验证 speed09 风格的大跳变样本在 self-adapt-icsp 下会通过补中点收敛。
/// </summary>
@@ -42,7 +67,7 @@ public sealed class PlanningCompatibilityTests
{
var workspaceRoot = GetWorkspaceRoot();
var configPath = Path.Combine(workspaceRoot, "Rvbust", "EOL9 EAU 90", "eol9_eau_90.json");
var modelPath = Path.Combine(workspaceRoot, "FlyingShot", "FlyingShot", "Models", "LR_Mate_200iD_7L.robot");
var modelPath = Path.Combine(workspaceRoot, "flyshot-replacement", "Config", "Models", "LR_Mate_200iD_7L.json");
var config = new RobotConfigLoader().Load(configPath);
var baseProfile = new RobotModelLoader().LoadProfile(modelPath, config.Robot.AccLimitScale, config.Robot.JerkLimitScale);
@@ -145,6 +170,241 @@ public sealed class PlanningCompatibilityTests
Assert.Equal(1, doEvent.WaypointIndex);
Assert.Equal(1, doEvent.OffsetCycles);
Assert.Equal(2, doEvent.HoldCycles);
Assert.NotNull(doEvent.ReferenceJointsDegrees);
Assert.Equal(RadiansToDegrees(1.0), doEvent.ReferenceJointsDegrees![0], precision: 6);
Assert.Equal(0.0, doEvent.ReferenceJointsDegrees![1], precision: 6);
}
/// <summary>
/// 验证时间轴会把示教点关节角保存到 DO 事件里，供运行时在近时窗内做最小关节差绑定。
/// </summary>
[Fact]
public void ShotTimelineBuilder_PopulatesReferenceJointsDegreesForRuntimeBinding()
{
var robot = CreateRobotProfile([1, 1], [1, 1], [1, 1]);
var program = new FlyshotProgram(
name: "demo",
waypoints:
[
new JointWaypoint([0.0, 0.0]),
new JointWaypoint([Math.PI / 6.0, -Math.PI / 4.0])
],
shotFlags: [false, true],
offsetValues: [0, 0],
addressGroups:
[
new IoAddressGroup(Array.Empty<int>()),
new IoAddressGroup([1])
]);
var trajectory = new PlannedTrajectory(
robot: robot,
originalProgram: program,
plannedWaypoints: program.Waypoints,
waypointTimes: [0.0, 0.5],
segmentDurations: [0.5],
segmentScales: [1.0],
method: PlanningMethod.Icsp,
iterations: 1,
threshold: 0.0);
var timeline = new ShotTimelineBuilder(new WaypointTimestampResolver())
.Build(trajectory, holdCycles: 1, samplePeriod: TimeSpan.FromMilliseconds(8));
var trigger = Assert.Single(timeline.TriggerTimeline);
Assert.NotNull(trigger.ReferenceJointsDegrees);
Assert.Equal(30.0, trigger.ReferenceJointsDegrees![0], precision: 6);
Assert.Equal(-45.0, trigger.ReferenceJointsDegrees![1], precision: 6);
}
/// <summary>
/// 验证稠密轨迹生成后会复核离散加速度，避免采样结果绕过 ICSP 段 scale 校验。
/// </summary>
[Fact]
public void TrajectoryLimitValidator_Throws_WhenDenseTrajectoryAccelerationExceedsLimit()
{
var robot = CreateRobotProfile([100.0], [10.0], [1000.0]);
var exception = Assert.Throws<InvalidOperationException>(() =>
TrajectoryLimitValidator.ValidateDenseJointTrajectory(
robot,
[
[0.0, 0.0],
[0.008, 0.0],
[0.016, 0.01]
],
trajectoryName: "dense-acceleration-check"));
Assert.Contains("加速度", exception.Message);
Assert.Contains("dense-acceleration-check", exception.Message);
}
/// <summary>
/// 验证飞拍链路临时关闭 jerk 复核时，仍保留速度/加速度校验，但不会再被 jerk 单独阻断。
/// </summary>
[Fact]
public void TrajectoryLimitValidator_CanSkipJerkValidation_ForFlyshotPath()
{
var robot = CreateRobotProfile([100.0], [1000.0], [10.0]);
IReadOnlyList<IReadOnlyList<double>> rows =
[
[0.0, 0.0],
[0.008, 0.0],
[0.016, 0.01],
[0.024, 0.02]
];
var exception = Assert.Throws<InvalidOperationException>(() =>
TrajectoryLimitValidator.ValidateDenseJointTrajectory(
robot,
rows,
trajectoryName: "dense-jerk-check"));
Assert.Contains("Jerk", exception.Message);
TrajectoryLimitValidator.ValidateDenseJointTrajectory(
robot,
rows,
trajectoryName: "dense-jerk-check-skip",
validateJerk: false);
}
/// <summary>
/// 验证飞拍最终 J519 队列的 jerk 硬约束使用严格限值，不再沿用历史上放大 4 倍的宽容口径。
/// </summary>
[Fact]
public void TrajectoryLimitValidator_ThrowsForJ519Jerk_WhenStrictFinalQueueValidationIsEnabled()
{
var robot = CreateRobotProfile([100.0], [1000.0], [100.0]);
var samples = new[]
{
new J519SendSample(0, 0.000, 0.000, 1.0, [0.0]),
new J519SendSample(1, 0.008, 0.008, 1.0, [0.0]),
new J519SendSample(2, 0.016, 0.016, 1.0, [0.003754]),
new J519SendSample(3, 0.024, 0.024, 1.0, [0.007508])
};
var exception = Assert.Throws<InvalidOperationException>(() =>
TrajectoryLimitValidator.ValidateJ519SendSamples(
robot,
samples,
trajectoryName: "strict-final-j519",
validateJerk: true));
Assert.Contains("Jerk", exception.Message);
Assert.Contains("strict-final-j519", exception.Message);
}
/// <summary>
/// 验证飞拍最终发送队列在请求倍率下严格 jerk 超限时直接拒绝执行，而不是自动改写 speed_ratio。
/// </summary>
[Fact]
public void FlyshotExecutionSendSequenceBuilder_ThrowsInsteadOfRewritingRequestedSpeedRatio()
{
var robot = CreateRobotProfile([100.0], [10000.0], [800.0]);
var program = CreateProgram(
[
[0.0],
[0.001]
]);
var trajectory = new PlannedTrajectory(
robot: robot,
originalProgram: program,
plannedWaypoints: program.Waypoints,
waypointTimes: [0.0, 0.024],
segmentDurations: [0.024],
segmentScales: [1.0],
method: PlanningMethod.Icsp,
iterations: 1,
threshold: 0.0);
var result = new TrajectoryResult(
programName: "strict-builder",
method: PlanningMethod.Icsp,
isValid: true,
duration: TimeSpan.FromSeconds(0.024),
shotEvents: Array.Empty<ShotEvent>(),
triggerTimeline: Array.Empty<TrajectoryDoEvent>(),
artifacts: Array.Empty<TrajectoryArtifact>(),
failureReason: null,
usedCache: false,
originalWaypointCount: 2,
plannedWaypointCount: 2);
var initialSamples = J519SendTrajectorySampler.SamplePlannedTrajectory(
trajectory,
servoPeriodSeconds: 0.008,
speedRatio: 1.0);
Assert.Throws<InvalidOperationException>(() =>
TrajectoryLimitValidator.ValidateJ519SendSamples(
robot,
initialSamples,
trajectoryName: "strict-builder-initial",
validateJerk: true));
var exception = Assert.Throws<InvalidOperationException>(() =>
FlyshotExecutionSendSequenceBuilder.Build(
robot,
trajectory,
result,
servoPeriodSeconds: 0.008,
requestedSpeedRatio: 1.0));
Assert.Contains("speed_ratio=1.000000", exception.Message);
Assert.Contains("planning_speed_scale", exception.Message);
}
/// <summary>
/// 验证飞拍最终发送队列通过严格校验时，请求 speed_ratio 会原样成为最终倍率。
/// </summary>
[Fact]
public void FlyshotExecutionSendSequenceBuilder_KeepsRequestedSpeedRatioWhenQueuePasses()
{
var robot = CreateRobotProfile([100.0], [10000.0], [10000.0]);
var program = CreateProgram(
[
[0.0],
[0.001]
]);
var trajectory = new PlannedTrajectory(
robot: robot,
originalProgram: program,
plannedWaypoints: program.Waypoints,
waypointTimes: [0.0, 0.024],
segmentDurations: [0.024],
segmentScales: [1.0],
method: PlanningMethod.Icsp,
iterations: 1,
threshold: 0.0);
var result = new TrajectoryResult(
programName: "rewrite-speedratio-builder",
method: PlanningMethod.Icsp,
isValid: true,
duration: TimeSpan.FromSeconds(0.024),
shotEvents: Array.Empty<ShotEvent>(),
triggerTimeline: Array.Empty<TrajectoryDoEvent>(),
artifacts: Array.Empty<TrajectoryArtifact>(),
failureReason: null,
usedCache: false,
originalWaypointCount: 2,
plannedWaypointCount: 2);
var prepared = FlyshotExecutionSendSequenceBuilder.Build(
robot,
trajectory,
result,
servoPeriodSeconds: 0.008,
requestedSpeedRatio: 0.8);
Assert.Equal(0.8, prepared.RequestSpeedRatio, precision: 6);
Assert.Equal(0.8, prepared.FinalSpeedRatio, precision: 6);
Assert.Equal(0, prepared.StretchIterationCount);
Assert.All(prepared.Samples, sample => Assert.Equal(0.8, sample.SpeedRatio, precision: 6));
Assert.All(prepared.TimingRows, row =>
{
Assert.Equal(0.8, row[3], precision: 6);
Assert.Equal(0.8, row[4], precision: 6);
Assert.Equal(0.0, row[5], precision: 6);
});
}
/// <summary>
@@ -224,4 +484,9 @@ public sealed class PlanningCompatibilityTests
throw new DirectoryNotFoundException("Unable to locate the flyshot workspace root.");
}
private static double RadiansToDegrees(double radians)
{
return radians * 180.0 / Math.PI;
}
}

2586
tests/Flyshot.Core.Tests/RuntimeOrchestrationTests.cs Normal file
View File

File diff suppressed because it is too large Load Diff

526
tests/Flyshot.Core.Tests/UttcJ519GoldenTests.cs Normal file
View File

@@ -0,0 +1,526 @@
using System.Buffers.Binary;
using System.Globalization;
using Flyshot.Runtime.Fanuc.Protocol;
namespace Flyshot.Core.Tests;
/// <summary>
/// 使用 2026-04-28 UTTC 真实抓包验证 J519 主运行点位与 JointDetialTraj 重采样规则一致。
/// </summary>
public sealed class UttcJ519GoldenTests
{
private const int JointCount = 6;
private const int RobotJ519Port = 60015;
private const double ServoPeriodSeconds = 0.008;
public static IEnumerable<object[]> SpeedSweepCases()
{
yield return ["2026042802-0.5.pcap", 0.5, 1851, 14.800309];
yield return ["2026042802-0.7.pcap", 0.7, 1322, 10.568313];
yield return ["2026042802-1.pcap", 1.0, 926, 7.400125];
}
/// <summary>
/// 验证 speed=0.5/0.7/1.0 三份真实抓包都符合当前运行时采用的发送点生成规则。
/// </summary>
[Theory]
[MemberData(nameof(SpeedSweepCases))]
public void CapturedJ519Run_ReplaysJointDetailTrajectoryWithSpeedRatio(
string pcapFileName,
double speedRatio,
int expectedPointCount,
double expectedSendDurationSeconds)
{
var repositoryRoot = FindRepositoryRoot();
var pcapPath = Path.Combine(repositoryRoot, "Rvbust", "uttc-20260428", pcapFileName);
var jointDetailPath = Path.Combine(repositoryRoot, "Rvbust", "uttc-20260428", "Data", "JointDetialTraj.txt");
var packets = ParsePcapUdp(pcapPath);
var hostPort = DetectHostJ519Port(packets);
var commands = ParseJ519Commands(packets, hostPort);
var responses = ParseJ519Responses(packets, hostPort);
var responseSegment = LongestStatusSegment(responses, status: 15);
var jointRows = ReadJointDetail(jointDetailPath);
var expected = GenerateExpectedPoints(jointRows, speedRatio);
var commandBySequence = new Dictionary<uint, CapturedJ519Command>();
var duplicateSequenceCount = 0;
foreach (var command in commands)
{
if (!commandBySequence.TryAdd(command.Sequence, command))
{
duplicateSequenceCount++;
}
}
var (startSequence, windowRms) = FindBestCommandWindow(commandBySequence, expected, responseSegment, searchRadius: 32);
var actualRun = Enumerable
.Range(0, expected.Length)
.Select(index => commandBySequence[startSequence + (uint)index])
.ToArray();
var comparison = Compare(actualRun, expected);
Assert.Equal(464, jointRows.Length);
Assert.Equal(expectedPointCount, expected.Length);
Assert.Equal(expectedPointCount, actualRun.Length);
Assert.Equal(0, duplicateSequenceCount);
Assert.Equal(17, comparison.IoSetPulses);
Assert.Equal(17, comparison.IoClearFrames);
Assert.Equal(
new ushort[] { 10, 12, 14 },
actualRun
.Where(static command => command.WriteIoMask != 0)
.Select(static command => command.WriteIoMask)
.Distinct()
.Order()
.ToArray());
Assert.True(responseSegment.Length >= expectedPointCount - 1, $"status=15 segment too short: {responseSegment.Length}");
Assert.InRange((long)responseSegment[0].Sequence - startSequence, 2, 8);
Assert.All(actualRun, static command => Assert.Equal(0, command.LastData));
for (var index = 0; index < actualRun.Length; index++)
{
Assert.Equal(startSequence + (uint)index, actualRun[index].Sequence);
}
Assert.True(windowRms < 0.012, $"J519 global RMS {windowRms:F9} deg exceeds tolerance.");
Assert.True(comparison.GlobalMaxAbsDeg < 0.07, $"J519 max abs diff {comparison.GlobalMaxAbsDeg:F9} deg exceeds tolerance.");
var sendDuration = actualRun[^1].TimeRelativeSeconds - actualRun[0].TimeRelativeSeconds;
Assert.InRange(sendDuration, expectedSendDurationSeconds - 0.04, expectedSendDurationSeconds + 0.04);
}
private static string FindRepositoryRoot()
{
var directory = new DirectoryInfo(AppContext.BaseDirectory);
while (directory is not null)
{
var jointDetailPath = Path.Combine(directory.FullName, "Rvbust", "uttc-20260428", "Data", "JointDetialTraj.txt");
if (File.Exists(jointDetailPath))
{
return directory.FullName;
}
directory = directory.Parent;
}
throw new DirectoryNotFoundException("Cannot locate repository root containing Rvbust/uttc-20260428/Data/JointDetialTraj.txt.");
}
private static IReadOnlyList<UdpPacket> ParsePcapUdp(string path)
{
using var stream = File.OpenRead(path);
var header = new byte[24];
stream.ReadExactly(header);
var magic = header.AsSpan(0, 4);
var bigEndian = false;
var timestampScale = 1_000_000.0;
if (magic.SequenceEqual(new byte[] { 0xd4, 0xc3, 0xb2, 0xa1 }))
{
bigEndian = false;
}
else if (magic.SequenceEqual(new byte[] { 0xa1, 0xb2, 0xc3, 0xd4 }))
{
bigEndian = true;
}
else if (magic.SequenceEqual(new byte[] { 0x4d, 0x3c, 0xb2, 0xa1 }))
{
bigEndian = false;
timestampScale = 1_000_000_000.0;
}
else if (magic.SequenceEqual(new byte[] { 0xa1, 0xb2, 0x3c, 0x4d }))
{
bigEndian = true;
timestampScale = 1_000_000_000.0;
}
else
{
throw new InvalidDataException($"Unsupported pcap magic: {Convert.ToHexString(header.AsSpan(0, 4))}");
}
var linkType = ReadUInt32(header.AsSpan(20, 4), bigEndian);
if (linkType != 1)
{
throw new InvalidDataException($"Only Ethernet pcap is supported, got linktype {linkType}.");
}
var packets = new List<UdpPacket>();
var recordHeader = new byte[16];
double? firstTimestamp = null;
var frameNumber = 0;
while (ReadFullOrEnd(stream, recordHeader))
{
frameNumber++;
var tsSec = ReadUInt32(recordHeader.AsSpan(0, 4), bigEndian);
var tsFraction = ReadUInt32(recordHeader.AsSpan(4, 4), bigEndian);
var includedLength = ReadUInt32(recordHeader.AsSpan(8, 4), bigEndian);
var packet = new byte[includedLength];
stream.ReadExactly(packet);
var timestamp = tsSec + (tsFraction / timestampScale);
firstTimestamp ??= timestamp;
var udp = ParseEthernetIpv4Udp(packet, frameNumber, timestamp - firstTimestamp.Value);
if (udp is not null)
{
packets.Add(udp);
}
}
return packets;
}
private static bool ReadFullOrEnd(Stream stream, byte[] buffer)
{
var offset = 0;
while (offset < buffer.Length)
{
var read = stream.Read(buffer, offset, buffer.Length - offset);
if (read == 0)
{
if (offset == 0)
{
return false;
}
throw new EndOfStreamException("Truncated pcap record header.");
}
offset += read;
}
return true;
}
private static UdpPacket? ParseEthernetIpv4Udp(byte[] packet, int frameNumber, double timeRelativeSeconds)
{
if (packet.Length < 14)
{
return null;
}
var offset = 14;
var etherType = BinaryPrimitives.ReadUInt16BigEndian(packet.AsSpan(12, 2));
if (etherType == 0x8100)
{
if (packet.Length < 18)
{
return null;
}
etherType = BinaryPrimitives.ReadUInt16BigEndian(packet.AsSpan(16, 2));
offset = 18;
}
if (etherType != 0x0800 || packet.Length < offset + 20)
{
return null;
}
var versionIhl = packet[offset];
var version = versionIhl >> 4;
var ihl = (versionIhl & 0x0f) * 4;
if (version != 4 || ihl < 20 || packet.Length < offset + ihl)
{
return null;
}
var protocol = packet[offset + 9];
if (protocol != 17)
{
return null;
}
var totalLength = BinaryPrimitives.ReadUInt16BigEndian(packet.AsSpan(offset + 2, 2));
var udpOffset = offset + ihl;
if (packet.Length < udpOffset + 8)
{
return null;
}
var sourcePort = BinaryPrimitives.ReadUInt16BigEndian(packet.AsSpan(udpOffset, 2));
var destinationPort = BinaryPrimitives.ReadUInt16BigEndian(packet.AsSpan(udpOffset + 2, 2));
var udpLength = BinaryPrimitives.ReadUInt16BigEndian(packet.AsSpan(udpOffset + 4, 2));
var payloadOffset = udpOffset + 8;
var payloadLength = Math.Max(0, Math.Min(udpLength - 8, totalLength - ihl - 8));
if (packet.Length < payloadOffset + payloadLength)
{
return null;
}
return new UdpPacket(
frameNumber,
timeRelativeSeconds,
sourcePort,
destinationPort,
packet.AsSpan(payloadOffset, payloadLength).ToArray());
}
private static ushort DetectHostJ519Port(IEnumerable<UdpPacket> packets)
{
return packets
.Where(static packet => packet.DestinationPort == RobotJ519Port && packet.Payload.Length == FanucJ519Protocol.CommandPacketLength)
.GroupBy(static packet => packet.SourcePort)
.OrderByDescending(static group => group.Count())
.Select(static group => group.Key)
.First();
}
private static CapturedJ519Command[] ParseJ519Commands(IEnumerable<UdpPacket> packets, ushort hostPort)
{
return packets
.Where(packet =>
packet.SourcePort == hostPort
&& packet.DestinationPort == RobotJ519Port
&& packet.Payload.Length == FanucJ519Protocol.CommandPacketLength)
.Select(ParseCommand)
.Where(static command => command is not null)
.Cast<CapturedJ519Command>()
.ToArray();
}
private static CapturedJ519Command? ParseCommand(UdpPacket packet)
{
var payload = packet.Payload;
var messageType = BinaryPrimitives.ReadUInt32BigEndian(payload.AsSpan(0x00, 4));
var version = BinaryPrimitives.ReadUInt32BigEndian(payload.AsSpan(0x04, 4));
if (messageType != 1 || version != 1)
{
return null;
}
var targets = new double[9];
for (var index = 0; index < targets.Length; index++)
{
targets[index] = BinaryPrimitives.ReadSingleBigEndian(payload.AsSpan(0x1c + (index * 4), 4));
}
return new CapturedJ519Command(
packet.FrameNumber,
packet.TimeRelativeSeconds,
BinaryPrimitives.ReadUInt32BigEndian(payload.AsSpan(0x08, 4)),
payload[0x0c],
BinaryPrimitives.ReadUInt16BigEndian(payload.AsSpan(0x16, 2)),
BinaryPrimitives.ReadUInt16BigEndian(payload.AsSpan(0x18, 2)),
targets);
}
private static FanucJ519Response[] ParseJ519Responses(IEnumerable<UdpPacket> packets, ushort hostPort)
{
return packets
.Where(packet =>
packet.SourcePort == RobotJ519Port
&& packet.DestinationPort == hostPort
&& packet.Payload.Length == FanucJ519Protocol.ResponsePacketLength)
.Select(packet => FanucJ519Protocol.ParseResponse(packet.Payload))
.ToArray();
}
private static FanucJ519Response[] LongestStatusSegment(IEnumerable<FanucJ519Response> responses, byte status)
{
var best = new List<FanucJ519Response>();
var current = new List<FanucJ519Response>();
foreach (var response in responses)
{
if (response.Status == status)
{
current.Add(response);
continue;
}
if (current.Count > best.Count)
{
best = current;
}
current = [];
}
return (current.Count > best.Count ? current : best).ToArray();
}
private static JointRow[] ReadJointDetail(string path)
{
return File.ReadLines(path)
.Where(static line => !string.IsNullOrWhiteSpace(line) && !line.TrimStart().StartsWith('#'))
.Select(static line =>
{
var values = line
.Split((char[]?)null, StringSplitOptions.RemoveEmptyEntries)
.Select(value => double.Parse(value, CultureInfo.InvariantCulture))
.ToArray();
return new JointRow(values[0], values.Skip(1).Take(JointCount).ToArray());
})
.ToArray();
}
private static ExpectedPoint[] GenerateExpectedPoints(IReadOnlyList<JointRow> rows, double speedRatio)
{
var durationSeconds = rows[^1].TimeSeconds;
var trajectoryStepSeconds = ServoPeriodSeconds * speedRatio;
var sampleCount = (int)Math.Floor((durationSeconds / trajectoryStepSeconds) + 1e-9) + 1;
var points = new ExpectedPoint[sampleCount];
var segmentIndex = 0;
for (var index = 0; index < sampleCount; index++)
{
var trajectoryTime = Math.Min(index * trajectoryStepSeconds, durationSeconds);
var jointsRad = Interpolate(rows, trajectoryTime, ref segmentIndex);
points[index] = new ExpectedPoint(
index,
trajectoryTime,
jointsRad.Select(static value => value * 180.0 / Math.PI).ToArray());
}
return points;
}
private static double[] Interpolate(IReadOnlyList<JointRow> rows, double trajectoryTime, ref int segmentIndex)
{
if (rows.Count == 1 || trajectoryTime <= rows[0].TimeSeconds)
{
return rows[0].JointsRad.ToArray();
}
var lastIndex = rows.Count - 1;
if (trajectoryTime >= rows[lastIndex].TimeSeconds)
{
return rows[lastIndex].JointsRad.ToArray();
}
while (segmentIndex < lastIndex - 1 && rows[segmentIndex + 1].TimeSeconds < trajectoryTime)
{
segmentIndex++;
}
var start = rows[segmentIndex];
var end = rows[segmentIndex + 1];
var duration = end.TimeSeconds - start.TimeSeconds;
var alpha = duration <= 0.0 ? 0.0 : (trajectoryTime - start.TimeSeconds) / duration;
var joints = new double[JointCount];
for (var index = 0; index < joints.Length; index++)
{
joints[index] = start.JointsRad[index] + ((end.JointsRad[index] - start.JointsRad[index]) * alpha);
}
return joints;
}
private static (uint StartSequence, double RmsDeg) FindBestCommandWindow(
IReadOnlyDictionary<uint, CapturedJ519Command> commandBySequence,
IReadOnlyList<ExpectedPoint> expected,
IReadOnlyList<FanucJ519Response> responseSegment,
int searchRadius)
{
if (responseSegment.Count == 0)
{
throw new InvalidDataException("No status=15 response segment found.");
}
var responseStartSequence = (long)responseSegment[0].Sequence;
uint? bestStartSequence = null;
var bestRms = double.PositiveInfinity;
for (var startSequence = responseStartSequence - searchRadius; startSequence <= responseStartSequence + searchRadius; startSequence++)
{
if (startSequence < 0)
{
continue;
}
var differences = new List<double>(expected.Count * JointCount);
var completeWindow = true;
for (var index = 0; index < expected.Count; index++)
{
var sequence = (uint)(startSequence + index);
if (!commandBySequence.TryGetValue(sequence, out var command))
{
completeWindow = false;
break;
}
for (var joint = 0; joint < JointCount; joint++)
{
differences.Add(command.TargetDegrees[joint] - expected[index].JointsDeg[joint]);
}
}
if (!completeWindow)
{
continue;
}
var rms = Rms(differences);
if (rms < bestRms)
{
bestRms = rms;
bestStartSequence = (uint)startSequence;
}
}
if (bestStartSequence is null)
{
throw new InvalidDataException("No complete command window found near the status=15 response segment.");
}
return (bestStartSequence.Value, bestRms);
}
private static ComparisonSummary Compare(IReadOnlyList<CapturedJ519Command> actual, IReadOnlyList<ExpectedPoint> expected)
{
var differences = new List<double>(actual.Count * JointCount);
var maxAbs = 0.0;
for (var index = 0; index < actual.Count; index++)
{
for (var joint = 0; joint < JointCount; joint++)
{
var difference = actual[index].TargetDegrees[joint] - expected[index].JointsDeg[joint];
differences.Add(difference);
maxAbs = Math.Max(maxAbs, Math.Abs(difference));
}
}
var ioSetPulses = actual.Count(command => command.WriteIoMask != 0 && command.WriteIoValue != 0);
var ioClearFrames = actual.Count(command => command.WriteIoMask != 0 && command.WriteIoValue == 0);
return new ComparisonSummary(Rms(differences), maxAbs, ioSetPulses, ioClearFrames);
}
private static double Rms(IEnumerable<double> values)
{
var sum = 0.0;
var count = 0;
foreach (var value in values)
{
sum += value * value;
count++;
}
return count == 0 ? 0.0 : Math.Sqrt(sum / count);
}
private static uint ReadUInt32(ReadOnlySpan<byte> value, bool bigEndian)
{
return bigEndian ? BinaryPrimitives.ReadUInt32BigEndian(value) : BinaryPrimitives.ReadUInt32LittleEndian(value);
}
private sealed record UdpPacket(
int FrameNumber,
double TimeRelativeSeconds,
ushort SourcePort,
ushort DestinationPort,
byte[] Payload);
private sealed record CapturedJ519Command(
int FrameNumber,
double TimeRelativeSeconds,
uint Sequence,
byte LastData,
ushort WriteIoMask,
ushort WriteIoValue,
IReadOnlyList<double> TargetDegrees);
private sealed record JointRow(double TimeSeconds, IReadOnlyList<double> JointsRad);
private sealed record ExpectedPoint(int Index, double TrajectoryTimeSeconds, IReadOnlyList<double> JointsDeg);
private sealed record ComparisonSummary(double GlobalRmsDeg, double GlobalMaxAbsDeg, int IoSetPulses, int IoClearFrames);
}