✨ feat(runtime): 添加轨迹持久化与密集执行链路
* 新增飞拍轨迹文件存储，支持上传、加载与删除 * 接通 ControllerClientCompat 到运行时的轨迹编排 * 完善 FANUC 命令与 J519 客户端发送链路 * 补充密集轨迹执行、运行时编排和协议客户端测试 * 更新 README 与 AGENTS 中的当前实现状态
This commit is contained in:
@@ -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)
|
||||
{
|
||||
}
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user