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

✨ feat(runtime): 添加轨迹持久化与密集执行链路

Browse Source 
* 新增飞拍轨迹文件存储，支持上传、加载与删除
* 接通 ControllerClientCompat 到运行时的轨迹编排
* 完善 FANUC 命令与 J519 客户端发送链路
* 补充密集轨迹执行、运行时编排和协议客户端测试
* 更新 README 与 AGENTS 中的当前实现状态
This commit is contained in:
yunxiao.zhu
2026-04-26 17:14:17 +08:00
parent a78e6761cb
commit 390d066ece
19 changed files with 1172 additions and 57 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -26,12 +26,12 @@ public sealed class ConfigCompatibilityTests
Assert.Equal(1.0, loaded.Robot.JerkLimitScale);
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>

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

@@ -130,6 +130,27 @@ public sealed class FanucCommandClientTests : IDisposable
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>

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

@@ -0,0 +1,96 @@
using Flyshot.Core.Domain;
using Flyshot.Runtime.Fanuc;
namespace Flyshot.Core.Tests;
/// <summary>
/// 验证 FANUC 控制器运行在稠密轨迹流式执行与 IO 触发上的行为。
/// </summary>
public sealed class FanucControllerRuntimeDenseTests
{
/// <summary>
/// 验证仿真模式下即使传入稠密路点，也会回退到单点同步更新。
/// </summary>
[Fact]
public void ExecuteTrajectory_WithDenseWaypoints_SimulationMode_FallsBackToSinglePoint()
{
var runtime = new FanucControllerRuntime();
var robot = TestRobotFactory.CreateRobotProfile();
runtime.ResetRobot(robot, "FANUC_LR_Mate_200iD");
runtime.SetActiveController(sim: true);
runtime.Connect("192.168.10.101");
runtime.EnableRobot(bufferSize: 2);
var denseTrajectory = new[]
{
new[] { 0.0, 0.1, 0.2, 0.3, 0.4, 0.5, 0.6 },
new[] { 0.008, 0.11, 0.21, 0.31, 0.41, 0.51, 0.61 },
new[] { 0.016, 0.12, 0.22, 0.32, 0.42, 0.52, 0.62 }
};
var result = new TrajectoryResult(
programName: "demo",
method: PlanningMethod.Icsp,
isValid: true,
duration: TimeSpan.FromSeconds(0.016),
shotEvents: Array.Empty<ShotEvent>(),
triggerTimeline: Array.Empty<TrajectoryDoEvent>(),
artifacts: Array.Empty<TrajectoryArtifact>(),
failureReason: null,
usedCache: false,
originalWaypointCount: 4,
plannedWaypointCount: 4,
denseJointTrajectory: denseTrajectory);
runtime.ExecuteTrajectory(result, [0.12, 0.22, 0.32, 0.42, 0.52, 0.62]);
var snapshot = runtime.GetSnapshot();
Assert.False(snapshot.IsInMotion);
Assert.Equal([0.12, 0.22, 0.32, 0.42, 0.52, 0.62], snapshot.JointPositions);
}
/// <summary>
/// 验证 StopMove 在没有任何后台发送任务运行时不会抛出异常。
/// </summary>
[Fact]
public void StopMove_DoesNotThrowWhenNoSendTaskRunning()
{
var runtime = new FanucControllerRuntime();
var robot = TestRobotFactory.CreateRobotProfile();
runtime.ResetRobot(robot, "FANUC_LR_Mate_200iD");
runtime.SetActiveController(sim: true);
runtime.Connect("192.168.10.101");
runtime.EnableRobot(bufferSize: 2);
var exception = Record.Exception(() => runtime.StopMove());
Assert.Null(exception);
Assert.False(runtime.GetSnapshot().IsInMotion);
}
/// <summary>
/// 验证 IO 地址组中的地址号被正确映射为 writeIoValue 的位掩码。
/// </summary>
[Theory]
[InlineData(new[] { 0 }, (ushort)1)]
[InlineData(new[] { 7 }, (ushort)128)]
[InlineData(new[] { 7, 8 }, (ushort)384)] // 128 + 256
[InlineData(new[] { 15 }, (ushort)32768)]
[InlineData(new int[] { }, (ushort)0)]
public void ComputeIoValue_MapsAddressesToBitMask(int[] addresses, ushort expected)
{
var group = new IoAddressGroup(addresses);
var actual = FanucControllerRuntime.ComputeIoValue(group);
Assert.Equal(expected, actual);
}
/// <summary>
/// 验证超过 15 的地址号会被安全忽略，不会溢出位掩码。
/// </summary>
[Fact]
public void ComputeIoValue_IgnoresOutOfRangeAddresses()
{
var group = new IoAddressGroup([0, 16, 7]);
var actual = FanucControllerRuntime.ComputeIoValue(group);
Assert.Equal((ushort)(1 | 128), actual);
}
}

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

@@ -177,4 +177,43 @@ public sealed class FanucJ519ClientTests : IDisposable
using var client = new FanucJ519Client();
Assert.Throws<InvalidOperationException>(() => client.StartMotion());
}
/// <summary>
/// 验证 Stopwatch + SpinWait 发送循环能保持约 8ms 周期抖动在亚毫秒级。
/// </summary>
[Fact]
public async Task StartMotion_MaintainsSubMillisecondPeriod()
{
using var client = new FanucJ519Client();
await client.ConnectAsync("127.0.0.1", Port, _cts.Token);
await _server.ReceiveAsync(_cts.Token); // init
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>();
for (var i = 0; i < 5; i++)
{
var result = await _server.ReceiveAsync(_cts.Token);
timestamps.Add(DateTimeOffset.UtcNow);
}
await client.StopMotionAsync(_cts.Token);
// 计算相邻包间隔并断言最大抖动。
var intervals = new List<TimeSpan>();
for (var i = 1; i < timestamps.Count; i++)
{
intervals.Add(timestamps[i] - timestamps[i - 1]);
}
// 允许 ±2ms 的测量误差（含 UDP 传输和调度延迟）。
Assert.All(intervals, interval =>
{
Assert.True(interval >= TimeSpan.FromMilliseconds(6), $"间隔 {interval.TotalMilliseconds:F2}ms 过短。");
Assert.True(interval <= TimeSpan.FromMilliseconds(10), $"间隔 {interval.TotalMilliseconds:F2}ms 过长。");
});
}
}

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

@@ -1,6 +1,7 @@
using Flyshot.ControllerClientCompat;
using Flyshot.Core.Config;
using Flyshot.Core.Domain;
using Flyshot.Runtime.Common;
using Flyshot.Runtime.Fanuc;
namespace Flyshot.Core.Tests;
@@ -82,6 +83,104 @@ public sealed class RuntimeOrchestrationTests
Assert.Single(bundle.Result.TriggerTimeline);
}
/// <summary>
/// 验证飞拍编排会使用 RobotConfig.json 中的 IO 保持周期。
/// </summary>
[Fact]
public void ControllerClientTrajectoryOrchestrator_PlanUploadedFlyshot_UsesRobotSettingsForHoldCycles()
{
var orchestrator = new ControllerClientTrajectoryOrchestrator();
var robot = TestRobotFactory.CreateRobotProfile();
var uploaded = TestRobotFactory.CreateUploadedTrajectoryWithSingleShot();
var settings = new CompatibilityRobotSettings(
useDo: true,
ioAddresses: [7, 8],
ioKeepCycles: 4,
accLimitScale: 1.0,
jerkLimitScale: 1.0,
adaptIcspTryNum: 5);
var bundle = orchestrator.PlanUploadedFlyshot(robot, uploaded, settings: settings);
var doEvent = Assert.Single(bundle.Result.TriggerTimeline);
Assert.Equal(4, doEvent.HoldCycles);
}
/// <summary>
/// 验证 RobotConfig.json 关闭 use_do 时仍保留 ShotEvent 诊断信息，但不生成伺服 DO 事件。
/// </summary>
[Fact]
public void ControllerClientTrajectoryOrchestrator_PlanUploadedFlyshot_SuppressesDoTimeline_WhenUseDoIsFalse()
{
var orchestrator = new ControllerClientTrajectoryOrchestrator();
var robot = TestRobotFactory.CreateRobotProfile();
var uploaded = TestRobotFactory.CreateUploadedTrajectoryWithSingleShot();
var settings = new CompatibilityRobotSettings(
useDo: false,
ioAddresses: [7, 8],
ioKeepCycles: 4,
accLimitScale: 1.0,
jerkLimitScale: 1.0,
adaptIcspTryNum: 5);
var bundle = orchestrator.PlanUploadedFlyshot(robot, uploaded, settings: settings);
Assert.Single(bundle.Result.ShotEvents);
Assert.Empty(bundle.Result.TriggerTimeline);
}
/// <summary>
/// 验证普通轨迹规划后会生成稠密关节采样序列。
/// </summary>
[Fact]
public void ControllerClientTrajectoryOrchestrator_PlanOrdinaryTrajectory_ReturnsDenseJointTrajectory()
{
var orchestrator = new ControllerClientTrajectoryOrchestrator();
var robot = TestRobotFactory.CreateRobotProfile();
var bundle = orchestrator.PlanOrdinaryTrajectory(
robot,
[
[0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
[0.1, 0.0, 0.0, 0.0, 0.0, 0.0],
[0.2, 0.0, 0.0, 0.0, 0.0, 0.0],
[0.3, 0.0, 0.0, 0.0, 0.0, 0.0]
]);
Assert.NotNull(bundle.Result.DenseJointTrajectory);
Assert.NotEmpty(bundle.Result.DenseJointTrajectory);
// 验证时间单调递增。
var times = bundle.Result.DenseJointTrajectory.Select(static row => row[0]).ToArray();
for (var i = 1; i < times.Length; i++)
{
Assert.True(times[i] > times[i - 1], $"采样时间点应在索引 {i} 处单调递增。");
}
// 验证每行包含时间 + 6 个关节值。
Assert.All(bundle.Result.DenseJointTrajectory, row => Assert.Equal(7, row.Count));
}
/// <summary>
/// 验证飞拍轨迹规划后的稠密采样时间轴与伺服周期一致。
/// </summary>
[Fact]
public void ControllerClientTrajectoryOrchestrator_PlanUploadedFlyshot_DenseTrajectoryUsesServoPeriod()
{
var orchestrator = new ControllerClientTrajectoryOrchestrator();
var robot = TestRobotFactory.CreateRobotProfile();
var uploaded = TestRobotFactory.CreateUploadedTrajectoryWithSingleShot();
var bundle = orchestrator.PlanUploadedFlyshot(robot, uploaded);
Assert.NotNull(bundle.Result.DenseJointTrajectory);
Assert.True(bundle.Result.DenseJointTrajectory.Count > 1);
// 采样周期应为 8ms（伺服周期）。
var firstDt = bundle.Result.DenseJointTrajectory[1][0] - bundle.Result.DenseJointTrajectory[0][0];
Assert.Equal(0.008, firstDt, precision: 3);
}
/// <summary>
/// 验证兼容服务执行普通轨迹时会进入规划链路，而不是直接把最后一个路点写入状态。
/// </summary>
@@ -104,6 +203,73 @@ public sealed class RuntimeOrchestrationTests
Assert.Throws<ArgumentException>(Act);
}
/// <summary>
/// 验证兼容服务初始化机器人时会把 RobotConfig.json 中的 acc_limit / jerk_limit 传给模型加载器。
/// </summary>
[Fact]
public void ControllerClientCompatService_SetUpRobot_AppliesRobotConfigLimitScales()
{
var tempRoot = CreateTempWorkspaceRoot();
try
{
File.WriteAllText(
Path.Combine(tempRoot, "RobotConfig.json"),
"""
{
"robot": {
"use_do": true,
"io_addr": [7, 8],
"io_keep_cycles": 4,
"acc_limit": 0.5,
"jerk_limit": 0.25,
"adapt_icsp_try_num": 3
},
"flying_shots": {}
}
""");
var options = new ControllerClientCompatOptions { WorkspaceRoot = tempRoot };
var runtime = new RecordingControllerRuntime();
var service = new ControllerClientCompatService(
options,
new ControllerClientCompatRobotCatalog(options, new RobotModelLoader()),
runtime,
new ControllerClientTrajectoryOrchestrator(),
new RobotConfigLoader(),
new InMemoryFlyshotTrajectoryStore());
service.SetUpRobot("FANUC_LR_Mate_200iD");
var profile = Assert.IsType<RobotProfile>(runtime.LastRobotProfile);
Assert.Equal(14.905, profile.JointLimits[2].AccelerationLimit, precision: 3);
Assert.Equal(62.115, profile.JointLimits[2].JerkLimit, precision: 3);
}
finally
{
Directory.Delete(tempRoot, recursive: true);
}
}
/// <summary>
/// 创建只包含当前支持机器人模型和 RobotConfig.json 的临时工作区。
/// </summary>
private static string CreateTempWorkspaceRoot()
{
var tempRoot = Path.Combine(Path.GetTempPath(), "flyshot-runtime-tests", Guid.NewGuid().ToString("N"));
var modelDir = Path.Combine(tempRoot, "FlyingShot", "FlyingShot", "Models");
Directory.CreateDirectory(modelDir);
var sourceModel = Path.Combine(
TestRobotFactory.GetWorkspaceRoot(),
"FlyingShot",
"FlyingShot",
"Models",
"LR_Mate_200iD_7L.robot");
File.Copy(sourceModel, Path.Combine(modelDir, "LR_Mate_200iD_7L.robot"));
return tempRoot;
}
}
/// <summary>
@@ -170,14 +336,16 @@ internal static class TestRobotFactory
options,
new ControllerClientCompatRobotCatalog(options, new RobotModelLoader()),
new FanucControllerRuntime(),
new ControllerClientTrajectoryOrchestrator());
new ControllerClientTrajectoryOrchestrator(),
new RobotConfigLoader(),
new InMemoryFlyshotTrajectoryStore());
}
/// <summary>
/// 定位父工作区根目录，供兼容服务加载真实机器人模型。
/// </summary>
/// <returns>父工作区根目录。</returns>
private static string GetWorkspaceRoot()
public static string GetWorkspaceRoot()
{
var current = new DirectoryInfo(AppContext.BaseDirectory);
while (current is not null)
@@ -193,3 +361,126 @@ internal static class TestRobotFactory
throw new DirectoryNotFoundException("Unable to locate the flyshot workspace root.");
}
}
/// <summary>
/// 内存中的轨迹存储实现，用于避免单元测试污染真实文件系统。
/// </summary>
internal sealed class InMemoryFlyshotTrajectoryStore : IFlyshotTrajectoryStore
{
private readonly Dictionary<string, ControllerClientCompatUploadedTrajectory> _store = new(StringComparer.Ordinal);
/// <inheritdoc />
public void Save(string robotName, CompatibilityRobotSettings settings, ControllerClientCompatUploadedTrajectory trajectory)
{
_store[trajectory.Name] = trajectory;
}
/// <inheritdoc />
public void Delete(string robotName, string trajectoryName)
{
_store.Remove(trajectoryName);
}
/// <inheritdoc />
public IReadOnlyDictionary<string, ControllerClientCompatUploadedTrajectory> LoadAll(string robotName, out CompatibilityRobotSettings? settings)
{
settings = null;
return _store;
}
}
/// <summary>
/// 记录 ResetRobot 入参的测试运行时，用于验证兼容服务传递的机器人配置。
/// </summary>
internal sealed class RecordingControllerRuntime : IControllerRuntime
{
/// <summary>
/// 获取最近一次 ResetRobot 收到的机器人配置。
/// </summary>
public RobotProfile? LastRobotProfile { get; private set; }
/// <inheritdoc />
public void ResetRobot(RobotProfile robot, string robotName)
{
LastRobotProfile = robot;
}
/// <inheritdoc />
public void SetActiveController(bool sim)
{
}
/// <inheritdoc />
public void Connect(string robotIp)
{
}
/// <inheritdoc />
public void Disconnect()
{
}
/// <inheritdoc />
public void EnableRobot(int bufferSize)
{
}
/// <inheritdoc />
public void DisableRobot()
{
}
/// <inheritdoc />
public void StopMove()
{
}
/// <inheritdoc />
public double GetSpeedRatio() => 1.0;
/// <inheritdoc />
public void SetSpeedRatio(double ratio)
{
}
/// <inheritdoc />
public IReadOnlyList<double> GetTcp() => [0.0, 0.0, 0.0];
/// <inheritdoc />
public void SetTcp(double x, double y, double z)
{
}
/// <inheritdoc />
public bool GetIo(int port, string ioType) => false;
/// <inheritdoc />
public void SetIo(int port, bool value, string ioType)
{
}
/// <inheritdoc />
public IReadOnlyList<double> GetJointPositions() => Array.Empty<double>();
/// <inheritdoc />
public IReadOnlyList<double> GetPose() => Array.Empty<double>();
/// <inheritdoc />
public ControllerStateSnapshot GetSnapshot()
{
return new ControllerStateSnapshot(
capturedAt: DateTimeOffset.UtcNow,
connectionState: "Connected",
isEnabled: true,
isInMotion: false,
speedRatio: 1.0,
jointPositions: Array.Empty<double>(),
cartesianPose: Array.Empty<double>(),
activeAlarms: Array.Empty<RuntimeAlarm>());
}
/// <inheritdoc />
public void ExecuteTrajectory(TrajectoryResult result, IReadOnlyList<double> finalJointPositions)
{
}
}