yunxiao.zhu
af65ca03a0
- 为 ExecuteFlyShotTraj 补齐 wait 语义，并让 move_to_start 先完成临时 PTP 运动后再启动正式飞拍轨迹 - 将 J519 命令发送改为由机器人 UDP status sequence 驱动， 避免在未收到状态包时主动发周期命令 - 将 10010 状态通道关节字段统一按 JointRadians 命名， 同步更新运行时读取逻辑与协议测试 - 新增 FANUC 10010 状态帧、流运动手册和 Python client 逆向文档，并更新 README 与兼容需求说明 - 补充兼容层编排测试与 HTTP 集成测试，覆盖 wait 和 move_to_start 串行化行为
900 lines
31 KiB
C#
900 lines
31 KiB
C#
using Flyshot.ControllerClientCompat;
|
||
using Flyshot.Core.Config;
|
||
using Flyshot.Core.Domain;
|
||
using Flyshot.Runtime.Common;
|
||
using Flyshot.Runtime.Fanuc;
|
||
using Flyshot.Runtime.Fanuc.Protocol;
|
||
|
||
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>
|
||
/// 验证真机运行时会把 TCP 10010 状态通道健康度映射为可诊断连接状态。
|
||
/// </summary>
|
||
[Theory]
|
||
[InlineData(FanucStateConnectionState.Connected, false, "Connected")]
|
||
[InlineData(FanucStateConnectionState.Connected, true, "StateTimeout")]
|
||
[InlineData(FanucStateConnectionState.TimedOut, true, "StateTimeout")]
|
||
[InlineData(FanucStateConnectionState.Reconnecting, true, "Reconnecting")]
|
||
[InlineData(FanucStateConnectionState.Disconnected, false, "Disconnected")]
|
||
public void FanucControllerRuntime_ResolveRealConnectionState_ReflectsStateChannelHealth(
|
||
FanucStateConnectionState state,
|
||
bool isFrameStale,
|
||
string expected)
|
||
{
|
||
var status = new FanucStateClientStatus(
|
||
state,
|
||
isFrameStale,
|
||
lastFrameAt: null,
|
||
reconnectAttemptCount: 0,
|
||
lastErrorMessage: null);
|
||
|
||
var actual = FanucControllerRuntime.ResolveRealConnectionState(status);
|
||
|
||
Assert.Equal(expected, actual);
|
||
}
|
||
|
||
/// <summary>
|
||
/// 验证只有已连接且未陈旧的 TCP 10010 帧会被 runtime 当作当前机器人状态使用。
|
||
/// </summary>
|
||
[Theory]
|
||
[InlineData(FanucStateConnectionState.Connected, false, true)]
|
||
[InlineData(FanucStateConnectionState.Connected, true, false)]
|
||
[InlineData(FanucStateConnectionState.Reconnecting, false, false)]
|
||
[InlineData(FanucStateConnectionState.TimedOut, false, false)]
|
||
[InlineData(FanucStateConnectionState.Disconnected, false, false)]
|
||
public void FanucControllerRuntime_ShouldUseStateFrame_RequiresConnectedFreshState(
|
||
FanucStateConnectionState state,
|
||
bool isFrameStale,
|
||
bool expected)
|
||
{
|
||
var status = new FanucStateClientStatus(
|
||
state,
|
||
isFrameStale,
|
||
lastFrameAt: null,
|
||
reconnectAttemptCount: 0,
|
||
lastErrorMessage: null);
|
||
|
||
var actual = FanucControllerRuntime.ShouldUseStateFrame(status);
|
||
|
||
Assert.Equal(expected, actual);
|
||
}
|
||
|
||
/// <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>
|
||
/// 验证飞拍规划会把规划限速倍率纳入速度/加速度/Jerk 限制，而不是复用运行时下发倍率。
|
||
/// </summary>
|
||
[Fact]
|
||
public void ControllerClientTrajectoryOrchestrator_PlanUploadedFlyshot_AppliesPlanningSpeedScaleToLimits()
|
||
{
|
||
var orchestrator = new ControllerClientTrajectoryOrchestrator();
|
||
var robot = TestRobotFactory.CreateRobotProfile();
|
||
var uploaded = TestRobotFactory.CreateUploadedTrajectoryWithSingleShot();
|
||
|
||
var fullSpeed = orchestrator.PlanUploadedFlyshot(robot, uploaded, planningSpeedScale: 1.0);
|
||
var halfSpeed = orchestrator.PlanUploadedFlyshot(robot, uploaded, planningSpeedScale: 0.5);
|
||
|
||
Assert.True(
|
||
halfSpeed.Result.Duration.TotalSeconds > fullSpeed.Result.Duration.TotalSeconds * 1.9,
|
||
$"半速规划时长应接近全速的 2 倍，实际 full={fullSpeed.Result.Duration.TotalSeconds}, half={halfSpeed.Result.Duration.TotalSeconds}");
|
||
}
|
||
|
||
/// <summary>
|
||
/// 验证飞拍缓存键包含规划限速倍率，避免降速验证时误用 100% 速度下的规划结果。
|
||
/// </summary>
|
||
[Fact]
|
||
public void ControllerClientTrajectoryOrchestrator_PlanUploadedFlyshot_CacheKeyIncludesPlanningSpeedScale()
|
||
{
|
||
var orchestrator = new ControllerClientTrajectoryOrchestrator();
|
||
var robot = TestRobotFactory.CreateRobotProfile();
|
||
var uploaded = TestRobotFactory.CreateUploadedTrajectoryWithSingleShot();
|
||
var options = new FlyshotExecutionOptions(useCache: true);
|
||
|
||
var fullSpeed = orchestrator.PlanUploadedFlyshot(robot, uploaded, options, planningSpeedScale: 1.0);
|
||
var halfSpeed = orchestrator.PlanUploadedFlyshot(robot, uploaded, options, planningSpeedScale: 0.5);
|
||
|
||
Assert.False(halfSpeed.Result.UsedCache);
|
||
Assert.True(halfSpeed.Result.Duration > fullSpeed.Result.Duration);
|
||
}
|
||
|
||
/// <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>
|
||
/// 验证 ExecuteFlyShotTraj(move_to_start=true) 会先执行稠密 PTP 到起点，并等待该段运动完成后再启动飞拍轨迹。
|
||
/// </summary>
|
||
[Fact]
|
||
public void ControllerClientCompatService_ExecuteTrajectoryByName_MoveToStartWaitsBeforeFlyshot()
|
||
{
|
||
var configRoot = CreateTempConfigRoot();
|
||
try
|
||
{
|
||
var options = new ControllerClientCompatOptions
|
||
{
|
||
ConfigRoot = configRoot
|
||
};
|
||
var runtime = new DelayedCompletionControllerRuntime(
|
||
initialJointPositions: [0.4, 0.0, 0.0, 0.0, 0.0, 0.0],
|
||
firstMotionCompletionDelay: TimeSpan.FromMilliseconds(80));
|
||
var service = new ControllerClientCompatService(
|
||
options,
|
||
new ControllerClientCompatRobotCatalog(options, new RobotModelLoader()),
|
||
runtime,
|
||
new ControllerClientTrajectoryOrchestrator(),
|
||
new RobotConfigLoader());
|
||
|
||
service.SetUpRobot("FANUC_LR_Mate_200iD");
|
||
service.SetActiveController(sim: false);
|
||
service.Connect("192.168.10.101");
|
||
service.EnableRobot(2);
|
||
service.UploadTrajectory(TestRobotFactory.CreateUploadedTrajectoryWithSingleShot());
|
||
|
||
service.ExecuteTrajectoryByName(
|
||
"demo-flyshot",
|
||
new FlyshotExecutionOptions(moveToStart: true, method: "icsp", saveTrajectory: false, useCache: false));
|
||
|
||
Assert.True(runtime.ExecuteCalls.Count >= 2);
|
||
Assert.NotNull(runtime.ExecuteCalls[0].Result.DenseJointTrajectory);
|
||
Assert.True(runtime.ExecuteCalls[0].Result.DenseJointTrajectory!.Count > 1);
|
||
Assert.False(runtime.SecondTrajectoryStartedBeforeFirstMotionCompleted);
|
||
}
|
||
finally
|
||
{
|
||
Directory.Delete(configRoot, recursive: true);
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 验证 ExecuteFlyShotTraj(wait=true) 会等待正式飞拍轨迹完成后再返回。
|
||
/// </summary>
|
||
[Fact]
|
||
public void ControllerClientCompatService_ExecuteTrajectoryByName_WaitTrueWaitsForFlyshotCompletion()
|
||
{
|
||
var configRoot = CreateTempConfigRoot();
|
||
try
|
||
{
|
||
var options = new ControllerClientCompatOptions
|
||
{
|
||
ConfigRoot = configRoot
|
||
};
|
||
var runtime = new DelayedCompletionControllerRuntime(
|
||
initialJointPositions: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
|
||
firstMotionCompletionDelay: TimeSpan.FromMilliseconds(80));
|
||
var service = new ControllerClientCompatService(
|
||
options,
|
||
new ControllerClientCompatRobotCatalog(options, new RobotModelLoader()),
|
||
runtime,
|
||
new ControllerClientTrajectoryOrchestrator(),
|
||
new RobotConfigLoader());
|
||
|
||
service.SetUpRobot("FANUC_LR_Mate_200iD");
|
||
service.SetActiveController(sim: false);
|
||
service.Connect("192.168.10.101");
|
||
service.EnableRobot(2);
|
||
service.UploadTrajectory(TestRobotFactory.CreateUploadedTrajectoryWithSingleShot());
|
||
|
||
service.ExecuteTrajectoryByName(
|
||
"demo-flyshot",
|
||
new FlyshotExecutionOptions(moveToStart: false, method: "icsp", saveTrajectory: false, useCache: false, wait: true));
|
||
|
||
Assert.Single(runtime.ExecuteCalls);
|
||
Assert.False(runtime.GetSnapshot().IsInMotion);
|
||
}
|
||
finally
|
||
{
|
||
Directory.Delete(configRoot, recursive: true);
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 验证兼容服务初始化机器人时会把 RobotConfig.json 中的 acc_limit / jerk_limit 传给模型加载器。
|
||
/// </summary>
|
||
[Fact]
|
||
public void ControllerClientCompatService_SetUpRobot_AppliesRobotConfigLimitScales()
|
||
{
|
||
var configRoot = CreateTempConfigRoot();
|
||
try
|
||
{
|
||
File.WriteAllText(
|
||
Path.Combine(configRoot, "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 { ConfigRoot = configRoot };
|
||
var runtime = new RecordingControllerRuntime();
|
||
var service = new ControllerClientCompatService(
|
||
options,
|
||
new ControllerClientCompatRobotCatalog(options, new RobotModelLoader()),
|
||
runtime,
|
||
new ControllerClientTrajectoryOrchestrator(),
|
||
new RobotConfigLoader());
|
||
|
||
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(configRoot, recursive: true);
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 验证 IsFlyshotTrajectoryValid(saveTrajectory=true) 会把规划后的结果点位导出到 Config/Data/name。
|
||
/// </summary>
|
||
[Fact]
|
||
public void ControllerClientCompatService_IsFlyshotTrajectoryValid_SaveTrajectoryExportsPlannedData()
|
||
{
|
||
var configRoot = CreateTempConfigRoot();
|
||
try
|
||
{
|
||
WriteRobotConfigWithDemoTrajectory(configRoot);
|
||
var options = new ControllerClientCompatOptions { ConfigRoot = configRoot };
|
||
var service = new ControllerClientCompatService(
|
||
options,
|
||
new ControllerClientCompatRobotCatalog(options, new RobotModelLoader()),
|
||
new RecordingControllerRuntime(),
|
||
new ControllerClientTrajectoryOrchestrator(),
|
||
new RobotConfigLoader());
|
||
|
||
service.SetUpRobot("FANUC_LR_Mate_200iD");
|
||
|
||
var valid = service.IsFlyshotTrajectoryValid(
|
||
out var duration,
|
||
"demo-flyshot",
|
||
method: "icsp",
|
||
saveTrajectory: true);
|
||
|
||
var outputDir = Path.Combine(configRoot, "Data", "demo-flyshot");
|
||
Assert.True(valid);
|
||
Assert.True(duration > TimeSpan.Zero);
|
||
Assert.True(File.Exists(Path.Combine(outputDir, "JointTraj.txt")));
|
||
Assert.True(File.Exists(Path.Combine(outputDir, "JointDetialTraj.txt")));
|
||
Assert.True(File.Exists(Path.Combine(outputDir, "CartTraj.txt")));
|
||
Assert.True(File.Exists(Path.Combine(outputDir, "CartDetialTraj.txt")));
|
||
Assert.True(File.Exists(Path.Combine(outputDir, "ShotEvents.json")));
|
||
Assert.NotEmpty(File.ReadAllLines(Path.Combine(outputDir, "JointDetialTraj.txt")));
|
||
Assert.NotEmpty(File.ReadAllLines(Path.Combine(outputDir, "CartDetialTraj.txt")));
|
||
}
|
||
finally
|
||
{
|
||
Directory.Delete(configRoot, recursive: true);
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 创建只包含当前支持机器人模型和 RobotConfig.json 的临时运行配置根。
|
||
/// </summary>
|
||
private static string CreateTempConfigRoot()
|
||
{
|
||
var configRoot = Path.Combine(Path.GetTempPath(), "flyshot-runtime-tests", Guid.NewGuid().ToString("N"), "Config");
|
||
var modelDir = Path.Combine(configRoot, "Models");
|
||
Directory.CreateDirectory(modelDir);
|
||
|
||
var sourceModel = Path.Combine(
|
||
TestRobotFactory.GetReplacementRoot(),
|
||
"Config",
|
||
"Models",
|
||
"LR_Mate_200iD_7L.robot");
|
||
File.Copy(sourceModel, Path.Combine(modelDir, "LR_Mate_200iD_7L.robot"));
|
||
|
||
return configRoot;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 写入包含一条飞拍轨迹的最小 RobotConfig.json，供兼容服务从统一配置恢复轨迹。
|
||
/// </summary>
|
||
/// <param name="configRoot">测试运行配置根。</param>
|
||
private static void WriteRobotConfigWithDemoTrajectory(string configRoot)
|
||
{
|
||
File.WriteAllText(
|
||
Path.Combine(configRoot, "RobotConfig.json"),
|
||
"""
|
||
{
|
||
"robot": {
|
||
"use_do": true,
|
||
"io_addr": [7, 8],
|
||
"io_keep_cycles": 2,
|
||
"acc_limit": 1.0,
|
||
"jerk_limit": 1.0,
|
||
"adapt_icsp_try_num": 5
|
||
},
|
||
"flying_shots": {
|
||
"demo-flyshot": {
|
||
"traj_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]
|
||
],
|
||
"shot_flags": [false, true, false, false],
|
||
"offset_values": [0, 1, 0, 0],
|
||
"addr": [[], [7, 8], [], []]
|
||
}
|
||
}
|
||
}
|
||
""");
|
||
}
|
||
}
|
||
|
||
/// <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
|
||
{
|
||
ConfigRoot = GetConfigRoot()
|
||
};
|
||
|
||
return new ControllerClientCompatService(
|
||
options,
|
||
new ControllerClientCompatRobotCatalog(options, new RobotModelLoader()),
|
||
new FanucControllerRuntime(),
|
||
new ControllerClientTrajectoryOrchestrator(),
|
||
new RobotConfigLoader());
|
||
}
|
||
|
||
/// <summary>
|
||
/// 定位 replacement 仓库内的运行配置根目录。
|
||
/// </summary>
|
||
/// <returns>当前仓库 Config 目录。</returns>
|
||
public static string GetConfigRoot()
|
||
{
|
||
return Path.Combine(GetReplacementRoot(), "Config");
|
||
}
|
||
|
||
/// <summary>
|
||
/// 定位 replacement 仓库根目录，供测试读取仓库内固化配置。
|
||
/// </summary>
|
||
/// <returns>replacement 仓库根目录。</returns>
|
||
public 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>
|
||
/// <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>
|
||
/// 记录 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)
|
||
{
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 模拟第一段运动异步完成的测试运行时，用于验证兼容层是否等待 move_to_start 完成。
|
||
/// </summary>
|
||
internal sealed class DelayedCompletionControllerRuntime : IControllerRuntime
|
||
{
|
||
private readonly object _lock = new();
|
||
private readonly TimeSpan _firstMotionCompletionDelay;
|
||
private double[] _jointPositions;
|
||
private bool _isEnabled;
|
||
private bool _isInMotion;
|
||
private bool _firstMotionCompleted;
|
||
|
||
/// <summary>
|
||
/// 初始化可延迟完成第一段运动的测试运行时。
|
||
/// </summary>
|
||
/// <param name="initialJointPositions">运行时报告的初始关节位置。</param>
|
||
/// <param name="firstMotionCompletionDelay">第一段运动完成前保持忙碌的时间。</param>
|
||
public DelayedCompletionControllerRuntime(
|
||
IReadOnlyList<double> initialJointPositions,
|
||
TimeSpan firstMotionCompletionDelay)
|
||
{
|
||
_jointPositions = initialJointPositions.ToArray();
|
||
_firstMotionCompletionDelay = firstMotionCompletionDelay;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 获取所有 ExecuteTrajectory 调用记录。
|
||
/// </summary>
|
||
public List<(TrajectoryResult Result, IReadOnlyList<double> FinalJointPositions)> ExecuteCalls { get; } = [];
|
||
|
||
/// <summary>
|
||
/// 获取第二条轨迹是否在第一段 move_to_start 完成前启动。
|
||
/// </summary>
|
||
public bool SecondTrajectoryStartedBeforeFirstMotionCompleted { get; private set; }
|
||
|
||
/// <inheritdoc />
|
||
public void ResetRobot(RobotProfile robot, string robotName)
|
||
{
|
||
}
|
||
|
||
/// <inheritdoc />
|
||
public void SetActiveController(bool sim)
|
||
{
|
||
}
|
||
|
||
/// <inheritdoc />
|
||
public void Connect(string robotIp)
|
||
{
|
||
}
|
||
|
||
/// <inheritdoc />
|
||
public void Disconnect()
|
||
{
|
||
}
|
||
|
||
/// <inheritdoc />
|
||
public void EnableRobot(int bufferSize)
|
||
{
|
||
_isEnabled = true;
|
||
}
|
||
|
||
/// <inheritdoc />
|
||
public void DisableRobot()
|
||
{
|
||
_isEnabled = false;
|
||
}
|
||
|
||
/// <inheritdoc />
|
||
public void StopMove()
|
||
{
|
||
lock (_lock)
|
||
{
|
||
_isInMotion = false;
|
||
}
|
||
}
|
||
|
||
/// <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()
|
||
{
|
||
lock (_lock)
|
||
{
|
||
return _jointPositions.ToArray();
|
||
}
|
||
}
|
||
|
||
/// <inheritdoc />
|
||
public IReadOnlyList<double> GetPose() => Array.Empty<double>();
|
||
|
||
/// <inheritdoc />
|
||
public ControllerStateSnapshot GetSnapshot()
|
||
{
|
||
lock (_lock)
|
||
{
|
||
return new ControllerStateSnapshot(
|
||
capturedAt: DateTimeOffset.UtcNow,
|
||
connectionState: "Connected",
|
||
isEnabled: _isEnabled,
|
||
isInMotion: _isInMotion,
|
||
speedRatio: 1.0,
|
||
jointPositions: _jointPositions.ToArray(),
|
||
cartesianPose: Array.Empty<double>(),
|
||
activeAlarms: Array.Empty<RuntimeAlarm>());
|
||
}
|
||
}
|
||
|
||
/// <inheritdoc />
|
||
public void ExecuteTrajectory(TrajectoryResult result, IReadOnlyList<double> finalJointPositions)
|
||
{
|
||
lock (_lock)
|
||
{
|
||
ExecuteCalls.Add((result, finalJointPositions.ToArray()));
|
||
if (ExecuteCalls.Count == 1)
|
||
{
|
||
_isInMotion = true;
|
||
_ = Task.Run(async () =>
|
||
{
|
||
await Task.Delay(_firstMotionCompletionDelay).ConfigureAwait(false);
|
||
lock (_lock)
|
||
{
|
||
_jointPositions = finalJointPositions.ToArray();
|
||
_isInMotion = false;
|
||
_firstMotionCompleted = true;
|
||
}
|
||
});
|
||
return;
|
||
}
|
||
|
||
if (!_firstMotionCompleted)
|
||
{
|
||
SecondTrajectoryStartedBeforeFirstMotionCompleted = true;
|
||
}
|
||
|
||
_jointPositions = finalJointPositions.ToArray();
|
||
}
|
||
}
|
||
}
|