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

✨ feat(server): 添加服务启动自动初始化机器人功能

Browse Source 
* 在 ControllerClientCompatOptions 中新增 AutoInitOptions 配置模型
* 创建 AutoInitRobotHostedService 实现启动时自动初始化链路
* 在 appsettings.json 的 ControllerClientCompat.AutoInit 节配置
  RobotName 和 RobotIp
* 启动时依次执行 SetUpRobot → SetActiveController → Connect →
  EnableRobot；配置缺失或连接失败不阻止服务启动
* 更新 README.md 和现场工作流文档
This commit is contained in:
yunxiao.zhu
2026-06-22 13:29:04 +08:00
parent 5c5eaf850d
commit ac41d04a54
12 changed files with 397 additions and 35 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -60,15 +60,20 @@ public sealed class FanucControllerRuntimeDenseTests
WaitUntilIdle(runtime);
var commands = j519Client.GetCommandHistoryForTests();
Assert.Equal(3, commands.Count);
Assert.Equal(4, commands.Count);
Assert.All(commands, static command => Assert.Equal(0u, command.Sequence));
var expectedJ1Targets = new[] { 0.0, 0.08, 0.16 };
var actualJ1Targets = commands.Select(static command => command.TargetJoints[0]).ToArray();
var actualJ1Targets = commands.Take(3).Select(static command => command.TargetJoints[0]).ToArray();
Assert.Equal(expectedJ1Targets.Length, actualJ1Targets.Length);
for (var index = 0; index < expectedJ1Targets.Length; index++)
{
Assert.Equal(expectedJ1Targets[index], actualJ1Targets[index], precision: 6);
}
Assert.All(commands.Take(3), static command => Assert.Equal(0, command.LastData));
Assert.Equal(1, commands[^1].LastData);
Assert.Equal(0, commands[^1].WriteIoMask);
Assert.Equal(0, commands[^1].WriteIoValue);
Assert.Equal(0.16, commands[^1].TargetJoints[0], precision: 6);
Assert.False(j519Client.IsCommandQueueDrainedForTests());
}
@@ -114,12 +119,12 @@ public sealed class FanucControllerRuntimeDenseTests
denseJointTrajectory: denseTrajectory);
var executeTask = Task.Run(() => runtime.ExecuteTrajectory(result, [DegreesToRadians(0.16), 0.0, 0.0, 0.0, 0.0, 0.0]), cts.Token);
await WaitUntilAsync(() => j519Client.GetCommandHistoryForTests().Count == 3, cts.Token);
await WaitUntilAsync(() => j519Client.GetCommandHistoryForTests().Count == 4, cts.Token);
// 只有机器人状态包把队列全部取出后，ExecuteTrajectory 才能向上层返回。
Assert.False(executeTask.IsCompleted);
for (uint sequence = 100; sequence < 103; sequence++)
for (uint sequence = 100; sequence < 104; sequence++)
{
await SendStatusPacketAsync(server, initResult.RemoteEndPoint, sequence, cts.Token);
_ = await server.ReceiveAsync(cts.Token);
@@ -190,9 +195,9 @@ public sealed class FanucControllerRuntimeDenseTests
var timingLines = File.ReadAllLines(timingPath);
var jerkLines = File.ReadAllLines(jerkPath);
var shotEventsJson = File.ReadAllText(shotEventsPath);
Assert.Equal(commands.Count, pointLines.Length);
Assert.Equal(commands.Count, timingLines.Length);
Assert.Equal(Math.Max(0, commands.Count - 1), jerkLines.Length);
Assert.Equal(denseTrajectory.Length, pointLines.Length);
Assert.Equal(denseTrajectory.Length, timingLines.Length);
Assert.Equal(Math.Max(0, denseTrajectory.Length - 1), jerkLines.Length);
var firstColumns = ParseColumns(pointLines[0]);
var secondColumns = ParseColumns(pointLines[1]);
@@ -216,7 +221,7 @@ public sealed class FanucControllerRuntimeDenseTests
Assert.Equal(1.0, secondTimingColumns[0], precision: 6);
Assert.Equal(0.008, secondTimingColumns[1], precision: 6);
Assert.Equal(0.008, secondTimingColumns[2], precision: 6);
Assert.Equal(commands.Count - 1, lastTimingColumns[0], precision: 6);
Assert.Equal(denseTrajectory.Length - 1, lastTimingColumns[0], precision: 6);
Assert.Equal(0.016, lastTimingColumns[2], precision: 6);
var firstJerkColumns = ParseColumns(jerkLines[0]);
@@ -395,10 +400,11 @@ public sealed class FanucControllerRuntimeDenseTests
var commands = j519Client.GetCommandHistoryForTests();
Assert.NotEmpty(commands);
Assert.Equal(preparedExecution.Samples.Count, commands.Count);
Assert.Equal(speedRatio, preparedExecution.FinalSpeedRatio, precision: 6);
Assert.Equal(preparedExecution.Samples.Count + 1, commands.Count);
Assert.Equal(speedRatio, preparedExecution.RequestSpeedRatio, precision: 6);
Assert.Equal(0, preparedExecution.StretchIterationCount);
Assert.Equal(preparedExecution.Samples[0].JointsDegrees, commands[0].TargetJoints);
Assert.Equal(1, commands[^1].LastData);
}
finally
{
@@ -469,8 +475,9 @@ public sealed class FanucControllerRuntimeDenseTests
WaitUntilIdle(runtime);
var commands = j519Client.GetCommandHistoryForTests();
Assert.Equal(preparedSamples.Length, commands.Count);
Assert.Equal([1.0, 2.0, 3.0], commands.Select(static command => command.TargetJoints[0]));
Assert.Equal(preparedSamples.Length + 1, commands.Count);
Assert.Equal([1.0, 2.0, 3.0], commands.Take(preparedSamples.Length).Select(static command => command.TargetJoints[0]));
Assert.Equal(1, commands[^1].LastData);
}
/// <summary>
@@ -523,17 +530,57 @@ public sealed class FanucControllerRuntimeDenseTests
WaitUntilIdle(runtime);
var commands = j519Client.GetCommandHistoryForTests();
Assert.Equal(expectedResult.DenseJointTrajectory!.Count, commands.Count);
Assert.Equal(expectedResult.DenseJointTrajectory!.Count + 1, commands.Count);
AssertJointDegreesEqual(startJoints, commands[0].TargetJoints);
AssertJointDegreesEqual(targetJoints, commands[^1].TargetJoints);
Assert.Equal(1, commands[^1].LastData);
Assert.All(commands.Take(commands.Count - 1), static command => Assert.Equal(0, command.LastData));
var middleAlpha = ComputeLineAlpha(commands[commands.Count / 2].TargetJoints, startJoints, targetJoints);
var middleAlpha = ComputeLineAlpha(commands[(commands.Count - 1) / 2].TargetJoints, startJoints, targetJoints);
Assert.InRange(middleAlpha, 0.45, 0.55);
var earlyAlpha = ComputeLineAlpha(commands[Math.Min(5, commands.Count - 1)].TargetJoints, startJoints, targetJoints);
Assert.InRange(earlyAlpha, 0.0, 0.02);
}
/// <summary>
/// 验证关闭命令流结束开关时，MoveJoint 保持旧队列长度和最终目标保持语义。
/// </summary>
[Fact]
public void MoveJoint_RealMode_WithEndCommandStreamDisabled_KeepsLegacyQueueShape()
{
using var commandClient = new FanucCommandClient();
using var stateClient = new FanucStateClient();
using var j519Client = new FanucJ519Client(endCommandStreamAfterQueueDrained: false);
using var runtime = new FanucControllerRuntime(commandClient, stateClient, j519Client);
var service = CreateCompatService(runtime);
var startJoints = new[] { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
var targetJoints = new[] { 0.2, -0.1, 0.05, 0.0, 0.0, 0.1 };
service.SetUpRobot("FANUC_LR_Mate_200iD");
j519Client.EnableCommandHistoryForTests();
ForceRealModeEnabled(runtime, speedRatio: 1.0);
SetPrivateField(runtime, "_jointPositions", startJoints);
var options = new ControllerClientCompatOptions
{
ConfigRoot = TestRobotFactory.GetConfigRoot()
};
var robot = new ControllerClientCompatRobotCatalog(options, new RobotModelLoader())
.LoadProfile("FANUC_LR_Mate_200iD", accLimitScale: 1.0, jerkLimitScale: 1.0);
var expectedResult = MoveJointTrajectoryGenerator.CreateResult(robot, startJoints, targetJoints, speedRatio: 1.0);
service.MoveJoint(targetJoints);
WaitUntilIdle(runtime);
var commands = j519Client.GetCommandHistoryForTests();
Assert.Equal(expectedResult.DenseJointTrajectory!.Count, commands.Count);
Assert.All(commands, static command => Assert.Equal(0, command.LastData));
var currentCommand = j519Client.GetCurrentCommand();
Assert.NotNull(currentCommand);
AssertJointDegreesEqual(targetJoints, currentCommand.TargetJoints);
}
[Fact]
public void MoveJointTrajectoryGenerator_LowerSpeedUsesFixedServoPeriodAndLongerPlannedDuration()
{
@@ -629,6 +676,10 @@ public sealed class FanucControllerRuntimeDenseTests
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.Take(commands.Count - 1), static command => Assert.Equal(0, command.LastData));
Assert.Equal(1, commands[^1].LastData);
Assert.Equal(0, commands[^1].WriteIoMask);
Assert.Equal(0, commands[^1].WriteIoValue);
Assert.All(commands, static command =>
{
Assert.Equal(0.0, command.TargetValues[6], precision: 6);
@@ -677,8 +728,9 @@ public sealed class FanucControllerRuntimeDenseTests
WaitUntilIdle(runtime);
var commands = j519Client.GetCommandHistoryForTests();
Assert.Equal(3, commands.Count);
Assert.Equal(4, commands.Count);
Assert.Equal(0.16, commands[^1].TargetJoints[0], precision: 6);
Assert.Equal(1, commands[^1].LastData);
}
/// <summary>
@@ -782,9 +834,12 @@ public sealed class FanucControllerRuntimeDenseTests
WaitUntilIdle(runtime);
var commands = j519Client.GetCommandHistoryForTests();
Assert.Equal(4, commands.Count);
Assert.Equal([(ushort)0, (ushort)5, (ushort)5, (ushort)5], commands.Select(static command => command.WriteIoMask));
Assert.Equal([(ushort)0, (ushort)5, (ushort)5, (ushort)0], commands.Select(static command => command.WriteIoValue));
Assert.Equal(5, commands.Count);
Assert.Equal([(ushort)0, (ushort)5, (ushort)5, (ushort)5], commands.Take(4).Select(static command => command.WriteIoMask));
Assert.Equal([(ushort)0, (ushort)5, (ushort)5, (ushort)0], commands.Take(4).Select(static command => command.WriteIoValue));
Assert.Equal(1, commands[^1].LastData);
Assert.Equal(0, commands[^1].WriteIoMask);
Assert.Equal(0, commands[^1].WriteIoValue);
}
/// <summary>
@@ -837,9 +892,12 @@ public sealed class FanucControllerRuntimeDenseTests
WaitUntilIdle(runtime);
var commands = j519Client.GetCommandHistoryForTests();
Assert.Equal(4, commands.Count);
Assert.Equal([(ushort)0, (ushort)0, (ushort)1, (ushort)1], commands.Select(static command => command.WriteIoMask));
Assert.Equal([(ushort)0, (ushort)0, (ushort)1, (ushort)0], commands.Select(static command => command.WriteIoValue));
Assert.Equal(5, commands.Count);
Assert.Equal([(ushort)0, (ushort)0, (ushort)1, (ushort)1], commands.Take(4).Select(static command => command.WriteIoMask));
Assert.Equal([(ushort)0, (ushort)0, (ushort)1, (ushort)0], commands.Take(4).Select(static command => command.WriteIoValue));
Assert.Equal(1, commands[^1].LastData);
Assert.Equal(0, commands[^1].WriteIoMask);
Assert.Equal(0, commands[^1].WriteIoValue);
}
/// <summary>

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

@@ -279,6 +279,119 @@ public sealed class FanucJ519ClientTests : IDisposable
Assert.True(client.IsCommandQueueDrainedForTests());
}
/// <summary>
/// 验证启用命令流结束开关后，LastData 结束帧发送完成会进入仅监控状态，不再继续回发保姿命令。
/// </summary>
[Fact]
public async Task LoadCommandQueue_WithEndCommandStreamEnabled_StopsSendingAfterLastDataAndKeepsMonitoring()
{
using var client = new FanucJ519Client();
await client.ConnectAsync("127.0.0.1", Port, _cts.Token);
var initResult = await _server.ReceiveAsync(_cts.Token);
client.StartMotion();
client.LoadCommandQueue(
[
new FanucJ519Command(sequence: 0, targetJoints: [1.0, 0.0, 0.0, 0.0, 0.0, 0.0]),
new FanucJ519Command(sequence: 0, targetJoints: [2.0, 0.0, 0.0, 0.0, 0.0, 0.0], lastData: 1)
]);
await SendStatusPacketAsync(initResult.RemoteEndPoint, sequence: 800);
var first = await _server.ReceiveAsync(_cts.Token);
await SendStatusPacketAsync(initResult.RemoteEndPoint, sequence: 801);
var end = await _server.ReceiveAsync(_cts.Token);
await SendStatusPacketAsync(
initResult.RemoteEndPoint,
sequence: 802,
jointDegrees: [7.0, 8.0, 9.0, 10.0, 11.0, 12.0]);
await Assert.ThrowsAsync<TimeoutException>(
() => _server.ReceiveAsync(_cts.Token).AsTask().WaitAsync(TimeSpan.FromMilliseconds(120)));
var latest = client.GetLatestResponse();
Assert.Equal(FanucJ519Protocol.CommandPacketLength, first.Buffer.Length);
Assert.Equal(FanucJ519Protocol.CommandPacketLength, end.Buffer.Length);
Assert.Equal(0, first.Buffer[0x0c]);
Assert.Equal(1, end.Buffer[0x0c]);
Assert.NotNull(latest);
Assert.Equal(802u, latest.Sequence);
Assert.Equal(7.0, latest.JointDegrees[0], precision: 6);
await client.StopMotionAsync(_cts.Token);
}
/// <summary>
/// 验证禁用命令流结束开关后，即使队列里出现 LastData 也继续沿用旧的最后目标保持行为。
/// </summary>
[Fact]
public async Task LoadCommandQueue_WithEndCommandStreamDisabled_HoldsLastCommandAfterLastData()
{
using var client = new FanucJ519Client(endCommandStreamAfterQueueDrained: false);
await client.ConnectAsync("127.0.0.1", Port, _cts.Token);
var initResult = await _server.ReceiveAsync(_cts.Token);
client.StartMotion();
client.LoadCommandQueue(
[
new FanucJ519Command(sequence: 0, targetJoints: [1.0, 0.0, 0.0, 0.0, 0.0, 0.0]),
new FanucJ519Command(sequence: 0, targetJoints: [2.0, 0.0, 0.0, 0.0, 0.0, 0.0], lastData: 1)
]);
var packets = new List<byte[]>();
for (uint sequence = 810; sequence < 813; sequence++)
{
await SendStatusPacketAsync(initResult.RemoteEndPoint, sequence);
var result = await _server.ReceiveAsync(_cts.Token);
packets.Add(result.Buffer);
}
Assert.Equal([810u, 811u, 812u], packets
.Select(packet => BinaryPrimitives.ReadUInt32BigEndian(packet.AsSpan(0x08, 4)))
.ToArray());
Assert.Equal([1.0f, 2.0f, 2.0f], packets
.Select(packet => BinaryPrimitives.ReadSingleBigEndian(packet.AsSpan(0x1c, 4)))
.ToArray());
await client.StopMotionAsync(_cts.Token);
}
/// <summary>
/// 验证 LastData 后的仅监控状态不会阻止下一轮队列重新开始发送。
/// </summary>
[Fact]
public async Task LoadCommandQueue_AfterLastDataMonitoring_CanStreamNewQueue()
{
using var client = new FanucJ519Client();
await client.ConnectAsync("127.0.0.1", Port, _cts.Token);
var initResult = await _server.ReceiveAsync(_cts.Token);
client.StartMotion();
client.LoadCommandQueue(
[
new FanucJ519Command(sequence: 0, targetJoints: [1.0, 0.0, 0.0, 0.0, 0.0, 0.0]),
new FanucJ519Command(sequence: 0, targetJoints: [2.0, 0.0, 0.0, 0.0, 0.0, 0.0], lastData: 1)
]);
await SendStatusPacketAsync(initResult.RemoteEndPoint, sequence: 820);
_ = await _server.ReceiveAsync(_cts.Token);
await SendStatusPacketAsync(initResult.RemoteEndPoint, sequence: 821);
_ = await _server.ReceiveAsync(_cts.Token);
client.LoadCommandQueue(
[
new FanucJ519Command(sequence: 0, targetJoints: [3.0, 0.0, 0.0, 0.0, 0.0, 0.0]),
new FanucJ519Command(sequence: 0, targetJoints: [4.0, 0.0, 0.0, 0.0, 0.0, 0.0], lastData: 1)
]);
await SendStatusPacketAsync(initResult.RemoteEndPoint, sequence: 822);
var restarted = await _server.ReceiveAsync(_cts.Token);
Assert.Equal(FanucJ519Protocol.CommandPacketLength, restarted.Buffer.Length);
Assert.Equal(822u, BinaryPrimitives.ReadUInt32BigEndian(restarted.Buffer.AsSpan(0x08, 4)));
Assert.Equal(0, restarted.Buffer[0x0c]);
Assert.Equal(3.0f, BinaryPrimitives.ReadSingleBigEndian(restarted.Buffer.AsSpan(0x1c, 4)), precision: 6);
await client.StopMotionAsync(_cts.Token);
}
/// <summary>
/// 验证停止运动后可在同一连接内重启发送，命令序号仍由新的状态包决定。
/// </summary>