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

✨ feat(fanuc): 添加直角坐标点动功能与相关接口

Browse Source 
* 新增 `MovePose` 方法，支持以直角坐标执行点到点移动。
* 引入 `LegacyCartesianPoseRequest` 类，处理直角位姿请求体的解析与验证。
* 更新 `LegacyHttpApiController`，实现 `/move_pose/` 路由以支持新功能。
* 增强状态快照元数据，提供机器人初始化状态与已上传轨迹信息。
* 更新前端状态页面，增加直角坐标点动控制面板与步长设置选项。
* 相关文档与测试用例同步更新，确保新功能的完整性与稳定性。
This commit is contained in:
yunxiao.zhu
2026-05-14 17:46:42 +08:00
parent d120ef2a39
commit 2cd42f04e5
22 changed files with 2062 additions and 104 deletions
Download Patch File Download Diff File Expand all files Collapse all files

68
tests/Flyshot.Core.Tests/FanucControllerRuntimeDenseTests.cs
View File

@@ -555,6 +555,30 @@ public sealed class FanucControllerRuntimeDenseTests
Assert.True(speed05.Duration.TotalSeconds >= ExpectedSmoothPtpDuration(robot, startJoints, targetJoints, speedRatio: 0.5));
}
/// <summary>
/// 验证 MovePose 低速倍率仍保持固定伺服周期，并通过拉长时长降低直角运动速度。
/// </summary>
[Fact]
public void MovePoseTrajectoryGenerator_LowerSpeedUsesFixedServoPeriodAndLongerPlannedDuration()
{
var servoPeriod = TimeSpan.FromMilliseconds(8);
var startPose = new[] { 100.0, 200.0, 300.0, 1.0, 2.0, 3.0 };
var targetPose = new[] { 140.0, 260.0, 330.0, 8.0, 10.0, 12.0 };
var fullSpeed = MovePoseTrajectoryGenerator.CreateResult(startPose, targetPose, servoPeriod, speedRatio: 1.0);
var speed07 = MovePoseTrajectoryGenerator.CreateResult(startPose, targetPose, servoPeriod, speedRatio: 0.7);
var speed05 = MovePoseTrajectoryGenerator.CreateResult(startPose, targetPose, servoPeriod, speedRatio: 0.5);
Assert.True(speed07.DenseCartesianTrajectory!.Count > fullSpeed.DenseCartesianTrajectory!.Count);
Assert.True(speed05.DenseCartesianTrajectory!.Count > speed07.DenseCartesianTrajectory!.Count);
AssertDenseRowsUseServoPeriod(fullSpeed.DenseCartesianTrajectory, servoPeriod.TotalSeconds);
AssertDenseRowsUseServoPeriod(speed07.DenseCartesianTrajectory, servoPeriod.TotalSeconds);
AssertDenseRowsUseServoPeriod(speed05.DenseCartesianTrajectory, servoPeriod.TotalSeconds);
AssertPoseEqual(startPose, fullSpeed.DenseCartesianTrajectory[0].Skip(1).ToArray());
AssertPoseEqual(targetPose, fullSpeed.DenseCartesianTrajectory[^1].Skip(1).ToArray());
Assert.True(MovePoseTrajectoryGenerator.SatisfiesDefaultCartesianLimits(speed05.DenseCartesianTrajectory, speedRatio: 0.5));
}
[Fact]
public void MoveJoint_RealMode_LeavesFinalTargetForHoldStreaming()
{
@@ -578,6 +602,41 @@ public sealed class FanucControllerRuntimeDenseTests
AssertJointDegreesEqual(targetJoints, currentCommand.TargetJoints);
}
/// <summary>
/// 验证 MovePose 会生成直角坐标 J519 队列，并使用 Data format=0 下发 X/Y/Z/W/P/R。
/// </summary>
[Fact]
public void MovePose_RealMode_GeneratesCartesianJ519Queue()
{
using var commandClient = new FanucCommandClient();
using var stateClient = new FanucStateClient();
using var j519Client = new FanucJ519Client();
using var runtime = new FanucControllerRuntime(commandClient, stateClient, j519Client);
var service = CreateCompatService(runtime);
var startPose = new[] { 100.0, 200.0, 300.0, 1.0, 2.0, 3.0 };
var targetPose = new[] { 110.0, 220.0, 315.0, 4.0, 5.0, 6.0 };
service.SetUpRobot("FANUC_LR_Mate_200iD");
j519Client.EnableCommandHistoryForTests();
ForceRealModeEnabled(runtime, speedRatio: 1.0);
SetPrivateField(runtime, "_pose", startPose);
service.MovePose(targetPose);
WaitUntilIdle(runtime);
var commands = j519Client.GetCommandHistoryForTests();
Assert.NotEmpty(commands);
Assert.All(commands, static command => Assert.Equal(0, command.DataStyle));
AssertPoseEqual(startPose, commands[0].TargetValues.Take(6).ToArray());
AssertPoseEqual(targetPose, commands[^1].TargetValues.Take(6).ToArray());
Assert.All(commands, static command =>
{
Assert.Equal(0.0, command.TargetValues[6], precision: 6);
Assert.Equal(0.0, command.TargetValues[7], precision: 6);
Assert.Equal(0.0, command.TargetValues[8], precision: 6);
});
}
/// <summary>
/// 验证运行时稠密发送不再依赖当前 speed_ratio；倍率合法性应在上游规划/生成阶段处理。
/// </summary>
@@ -1103,6 +1162,15 @@ public sealed class FanucControllerRuntimeDenseTests
}
}
private static void AssertPoseEqual(IReadOnlyList<double> expected, IReadOnlyList<double> actual)
{
Assert.Equal(expected.Count, actual.Count);
for (var index = 0; index < expected.Count; index++)
{
Assert.Equal(expected[index], actual[index], precision: 6);
}
}
/// <summary>
/// 创建用于就绪状态测试的最小 J519 响应。
/// </summary>

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

@@ -163,6 +163,28 @@ public sealed class FanucJ519ClientTests : IDisposable
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>

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

@@ -956,6 +956,111 @@ public sealed class RuntimeOrchestrationTests
}
}
/// <summary>
/// 验证飞拍执行阻塞在运行时时，状态页元数据快照仍能通过短锁快速返回。
/// </summary>
[Fact]
public async Task ControllerClientCompatService_GetStatusSnapshotMetadata_DoesNotWaitForRunningFlyshot()
{
var configRoot = CreateTempConfigRoot();
try
{
WriteRobotConfigWithDemoTrajectory(configRoot);
var options = new ControllerClientCompatOptions
{
ConfigRoot = configRoot
};
var runtime = new BlockingExecutionControllerRuntime([0.0, 0.0, 0.0, 0.0, 0.0, 0.0]);
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());
var executing = Task.Run(() => service.ExecuteTrajectoryByName(
"demo-flyshot",
new FlyshotExecutionOptions(moveToStart: false, method: "icsp", saveTrajectory: false, useCache: false, wait: true)));
Assert.True(runtime.WaitForExecutionStarted(TimeSpan.FromSeconds(2)));
var metadataTask = Task.Run(() => service.GetStatusSnapshotMetadata());
var completed = await Task.WhenAny(metadataTask, Task.Delay(TimeSpan.FromMilliseconds(150)));
runtime.ReleaseExecution();
await executing;
Assert.Same(metadataTask, completed);
var metadata = await metadataTask;
Assert.True(metadata.IsSetup);
Assert.Equal("FANUC_LR_Mate_200iD", metadata.RobotName);
Assert.Equal(["demo-flyshot"], metadata.UploadedTrajectories);
}
finally
{
Directory.Delete(configRoot, recursive: true);
}
}
/// <summary>
/// 验证两个飞拍执行命令必须串行进入 runtime，避免 J519 队列被并发执行覆盖。
/// </summary>
[Fact]
public async Task ControllerClientCompatService_ExecuteTrajectoryByName_SerializesConcurrentExecutionCommands()
{
var configRoot = CreateTempConfigRoot();
try
{
WriteRobotConfigWithDemoTrajectory(configRoot);
var options = new ControllerClientCompatOptions
{
ConfigRoot = configRoot
};
var runtime = new BlockingExecutionControllerRuntime([0.0, 0.0, 0.0, 0.0, 0.0, 0.0]);
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());
var first = Task.Run(() => service.ExecuteTrajectoryByName(
"demo-flyshot",
new FlyshotExecutionOptions(moveToStart: false, method: "icsp", saveTrajectory: false, useCache: false, wait: true)));
Assert.True(runtime.WaitForExecutionStarted(TimeSpan.FromSeconds(2)));
var second = Task.Run(() => service.ExecuteTrajectoryByName(
"demo-flyshot",
new FlyshotExecutionOptions(moveToStart: false, method: "icsp", saveTrajectory: false, useCache: false, wait: true)));
await Task.Delay(TimeSpan.FromMilliseconds(100));
Assert.Equal(1, runtime.ExecuteCallCount);
runtime.ReleaseExecution();
await first;
Assert.True(runtime.WaitForExecutionStarted(TimeSpan.FromSeconds(2), expectedCallCount: 2));
runtime.ReleaseExecution();
await second;
Assert.Equal(2, runtime.ExecuteCallCount);
}
finally
{
Directory.Delete(configRoot, recursive: true);
}
}
/// <summary>
/// 验证飞拍链路在进入运行时前就会准备最终发送队列，而不是把 speedRatio 重采样留给运行时临场处理。
/// </summary>
@@ -2005,6 +2110,12 @@ internal sealed class RecordingControllerRuntime : IControllerRuntime
{
LastExecutedResult = result;
}
/// <inheritdoc />
public void ExecuteCartesianTrajectory(TrajectoryResult result, IReadOnlyList<double> finalPose)
{
LastExecutedResult = result;
}
}
/// <summary>
@@ -2166,6 +2277,182 @@ internal sealed class DelayedCompletionControllerRuntime : IControllerRuntime
_jointPositions = finalJointPositions.ToArray();
}
}
/// <inheritdoc />
public void ExecuteCartesianTrajectory(TrajectoryResult result, IReadOnlyList<double> finalPose)
{
}
}
/// <summary>
/// 模拟 runtime 执行入口长期占用的测试运行时，用于验证兼容层锁边界。
/// </summary>
internal sealed class BlockingExecutionControllerRuntime : IControllerRuntime
{
private readonly object _lock = new();
private readonly ManualResetEventSlim _executionStarted = new(false);
private readonly ManualResetEventSlim _releaseExecution = new(false);
private readonly double[] _jointPositions;
private bool _isEnabled;
private bool _isInMotion;
private int _executeCallCount;
/// <summary>
/// 初始化一份会阻塞 ExecuteTrajectory 的测试运行时。
/// </summary>
/// <param name="initialJointPositions">初始关节反馈。</param>
public BlockingExecutionControllerRuntime(IReadOnlyList<double> initialJointPositions)
{
_jointPositions = initialJointPositions.ToArray();
}
/// <summary>
/// 获取 runtime 执行入口被调用的次数。
/// </summary>
public int ExecuteCallCount
{
get
{
lock (_lock)
{
return _executeCallCount;
}
}
}
/// <summary>
/// 等待指定序号的执行调用进入 runtime。
/// </summary>
/// <param name="timeout">最长等待时间。</param>
/// <param name="expectedCallCount">期望已经进入的执行次数。</param>
/// <returns>是否在超时前等到。</returns>
public bool WaitForExecutionStarted(TimeSpan timeout, int expectedCallCount = 1)
{
var deadline = DateTimeOffset.UtcNow.Add(timeout);
while (DateTimeOffset.UtcNow < deadline)
{
lock (_lock)
{
if (_executeCallCount >= expectedCallCount)
{
return true;
}
}
_executionStarted.Wait(TimeSpan.FromMilliseconds(10));
}
return false;
}
/// <summary>
/// 释放当前阻塞的执行调用。
/// </summary>
public void ReleaseExecution()
{
_releaseExecution.Set();
}
public void ResetRobot(RobotProfile robot, string robotName)
{
}
public void SetActiveController(bool sim)
{
}
public void Connect(string robotIp)
{
}
public void Disconnect()
{
}
public void EnableRobot(int bufferSize)
{
_isEnabled = true;
}
public void DisableRobot()
{
_isEnabled = false;
}
public void StopMove()
{
lock (_lock)
{
_isInMotion = false;
}
ReleaseExecution();
}
public double GetSpeedRatio() => 1.0;
public void SetSpeedRatio(double ratio)
{
}
public IReadOnlyList<double> GetTcp() => [0.0, 0.0, 0.0];
public void SetTcp(double x, double y, double z)
{
}
public bool GetIo(int port, string ioType) => false;
public void SetIo(int port, bool value, string ioType)
{
}
public IReadOnlyList<double> GetJointPositions()
{
return _jointPositions.ToArray();
}
public IReadOnlyList<double> GetPose() => [0.0, 0.0, 0.0, 0.0, 0.0, 0.0];
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>());
}
}
public void ExecuteTrajectory(TrajectoryResult result, IReadOnlyList<double> finalJointPositions)
{
lock (_lock)
{
_executeCallCount++;
_isInMotion = true;
_executionStarted.Set();
_releaseExecution.Reset();
}
_releaseExecution.Wait();
lock (_lock)
{
_isInMotion = false;
_executionStarted.Reset();
}
}
public void ExecuteCartesianTrajectory(TrajectoryResult result, IReadOnlyList<double> finalPose)
{
ExecuteTrajectory(result, _jointPositions);
}
}
/// <summary>
@@ -2292,4 +2579,8 @@ internal sealed class StickyFeedbackControllerRuntime : IControllerRuntime
_jointPositions = finalJointPositions.ToArray();
}
}
public void ExecuteCartesianTrajectory(TrajectoryResult result, IReadOnlyList<double> finalPose)
{
}
}