yunxiao.zhu
390d066ece
* 新增飞拍轨迹文件存储，支持上传、加载与删除 * 接通 ControllerClientCompat 到运行时的轨迹编排 * 完善 FANUC 命令与 J519 客户端发送链路 * 补充密集轨迹执行、运行时编排和协议客户端测试 * 更新 README 与 AGENTS 中的当前实现状态
487 lines
16 KiB
C#
487 lines
16 KiB
C#
using Flyshot.ControllerClientCompat;
|
||
using Flyshot.Core.Config;
|
||
using Flyshot.Core.Domain;
|
||
using Flyshot.Runtime.Common;
|
||
using Flyshot.Runtime.Fanuc;
|
||
|
||
namespace Flyshot.Core.Tests;
|
||
|
||
/// <summary>
|
||
/// 验证最小运行时编排链路会把规划结果交给控制器运行时，而不是停留在兼容层内存状态。
|
||
/// </summary>
|
||
public sealed class RuntimeOrchestrationTests
|
||
{
|
||
/// <summary>
|
||
/// 验证 FANUC 最小运行时执行轨迹后会更新状态快照与最终关节位置。
|
||
/// </summary>
|
||
[Fact]
|
||
public void FanucControllerRuntime_ExecuteTrajectory_UpdatesSnapshotAndFinalJointPositions()
|
||
{
|
||
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 result = new TrajectoryResult(
|
||
programName: "demo",
|
||
method: PlanningMethod.Icsp,
|
||
isValid: true,
|
||
duration: TimeSpan.FromSeconds(1.2),
|
||
shotEvents: Array.Empty<ShotEvent>(),
|
||
triggerTimeline: Array.Empty<TrajectoryDoEvent>(),
|
||
artifacts: Array.Empty<TrajectoryArtifact>(),
|
||
failureReason: null,
|
||
usedCache: false,
|
||
originalWaypointCount: 4,
|
||
plannedWaypointCount: 4);
|
||
|
||
runtime.ExecuteTrajectory(result, [1.0, 2.0, 3.0, 4.0, 5.0, 6.0]);
|
||
|
||
var snapshot = runtime.GetSnapshot();
|
||
Assert.Equal("Connected", snapshot.ConnectionState);
|
||
Assert.False(snapshot.IsInMotion);
|
||
Assert.Equal([1.0, 2.0, 3.0, 4.0, 5.0, 6.0], snapshot.JointPositions);
|
||
}
|
||
|
||
/// <summary>
|
||
/// 验证普通轨迹会先进入 ICSP 规划，并沿用 ICSP 对示教点数量的约束。
|
||
/// </summary>
|
||
[Fact]
|
||
public void ControllerClientTrajectoryOrchestrator_PlanOrdinaryTrajectory_RejectsThreeTeachPoints()
|
||
{
|
||
var orchestrator = new ControllerClientTrajectoryOrchestrator();
|
||
var robot = TestRobotFactory.CreateRobotProfile();
|
||
|
||
void Act() =>
|
||
orchestrator.PlanOrdinaryTrajectory(
|
||
robot,
|
||
[
|
||
[0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
|
||
[0.5, 0.0, 0.0, 0.0, 0.0, 0.0],
|
||
[1.0, 0.0, 0.0, 0.0, 0.0, 0.0]
|
||
]);
|
||
|
||
Assert.Throws<ArgumentException>(Act);
|
||
}
|
||
|
||
/// <summary>
|
||
/// 验证已上传飞拍轨迹会经过 self-adapt-icsp 并生成拍照触发时间轴。
|
||
/// </summary>
|
||
[Fact]
|
||
public void ControllerClientTrajectoryOrchestrator_PlanUploadedFlyshot_BuildsShotTimeline()
|
||
{
|
||
var orchestrator = new ControllerClientTrajectoryOrchestrator();
|
||
var robot = TestRobotFactory.CreateRobotProfile();
|
||
var uploaded = TestRobotFactory.CreateUploadedTrajectoryWithSingleShot();
|
||
|
||
var bundle = orchestrator.PlanUploadedFlyshot(robot, uploaded);
|
||
|
||
Assert.True(bundle.Result.IsValid);
|
||
Assert.Single(bundle.Result.ShotEvents);
|
||
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>
|
||
[Fact]
|
||
public void ControllerClientCompatService_ExecuteTrajectory_RejectsThreeTeachPointsAfterPlanningIsIntroduced()
|
||
{
|
||
var service = TestRobotFactory.CreateCompatService();
|
||
service.SetUpRobot("FANUC_LR_Mate_200iD");
|
||
service.SetActiveController(sim: true);
|
||
service.Connect("192.168.10.101");
|
||
service.EnableRobot(2);
|
||
|
||
void Act() =>
|
||
service.ExecuteTrajectory(
|
||
[
|
||
[0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
|
||
[0.5, 0.0, 0.0, 0.0, 0.0, 0.0],
|
||
[1.0, 0.0, 0.0, 0.0, 0.0, 0.0]
|
||
]);
|
||
|
||
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>
|
||
/// 为运行时编排测试构造稳定的最小领域对象。
|
||
/// </summary>
|
||
internal static class TestRobotFactory
|
||
{
|
||
/// <summary>
|
||
/// 构造六轴测试机器人配置，避免运行时测试依赖真实 .robot 文件。
|
||
/// </summary>
|
||
/// <returns>可用于规划和运行时状态校验的机器人配置。</returns>
|
||
public static RobotProfile CreateRobotProfile()
|
||
{
|
||
return new RobotProfile(
|
||
name: "TestRobot",
|
||
modelPath: "Models/Test.robot",
|
||
degreesOfFreedom: 6,
|
||
jointLimits: Enumerable.Range(1, 6)
|
||
.Select(static index => new JointLimit($"J{index}", 10.0, 20.0, 100.0))
|
||
.ToArray(),
|
||
jointCouplings: Array.Empty<JointCoupling>(),
|
||
servoPeriod: TimeSpan.FromMilliseconds(8),
|
||
triggerPeriod: TimeSpan.FromMilliseconds(8));
|
||
}
|
||
|
||
/// <summary>
|
||
/// 构造一条含单个拍照点的上传飞拍轨迹。
|
||
/// </summary>
|
||
/// <returns>可用于触发时间轴测试的上传轨迹。</returns>
|
||
public static ControllerClientCompatUploadedTrajectory CreateUploadedTrajectoryWithSingleShot()
|
||
{
|
||
return new ControllerClientCompatUploadedTrajectory(
|
||
name: "demo-flyshot",
|
||
waypoints:
|
||
[
|
||
[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]
|
||
],
|
||
shotFlags: [false, true, false, false],
|
||
offsetValues: [0, 1, 0, 0],
|
||
addressGroups:
|
||
[
|
||
Array.Empty<int>(),
|
||
[7, 8],
|
||
Array.Empty<int>(),
|
||
Array.Empty<int>()
|
||
]);
|
||
}
|
||
|
||
/// <summary>
|
||
/// 构造一份真实依赖注入等价的兼容服务，覆盖运行时和编排器协作。
|
||
/// </summary>
|
||
/// <returns>可执行 ControllerClient 兼容语义的服务实例。</returns>
|
||
public static ControllerClientCompatService CreateCompatService()
|
||
{
|
||
var options = new ControllerClientCompatOptions
|
||
{
|
||
WorkspaceRoot = GetWorkspaceRoot()
|
||
};
|
||
|
||
return new ControllerClientCompatService(
|
||
options,
|
||
new ControllerClientCompatRobotCatalog(options, new RobotModelLoader()),
|
||
new FanucControllerRuntime(),
|
||
new ControllerClientTrajectoryOrchestrator(),
|
||
new RobotConfigLoader(),
|
||
new InMemoryFlyshotTrajectoryStore());
|
||
}
|
||
|
||
/// <summary>
|
||
/// 定位父工作区根目录，供兼容服务加载真实机器人模型。
|
||
/// </summary>
|
||
/// <returns>父工作区根目录。</returns>
|
||
public static string GetWorkspaceRoot()
|
||
{
|
||
var current = new DirectoryInfo(AppContext.BaseDirectory);
|
||
while (current is not null)
|
||
{
|
||
if (File.Exists(Path.Combine(current.FullName, "FlyshotReplacement.sln")))
|
||
{
|
||
return Path.GetFullPath(Path.Combine(current.FullName, ".."));
|
||
}
|
||
|
||
current = current.Parent;
|
||
}
|
||
|
||
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)
|
||
{
|
||
}
|
||
}
|