feat(runtime): 完善 FANUC 命令参数与状态通道重连

* 在 FanucCommandProtocol/Client 中补齐速度倍率、TCP 位姿和
  IO 的封包/解析,并引入 FanucIoTypes 字符串到枚举映射
* FanucControllerRuntime 在非仿真模式下接入真机命令通道,本地
  缓存仅作为兜底,TCP 操作扩展为 7 维 Pose
* FanucStateClient 增加帧超时检测、退避自动重连和诊断状态接口,
  超时或重连期间不再把陈旧帧当作当前机器人状态
* FanucStateProtocol 锁定 90B 帧字段为 pose[6]、joint[6]、
  external_axes[3] 和 raw_tail_words[4],并保留状态字诊断槽位
* ICspPlanner 增加 global_scale > 1.0 失败判定,self-adapt-icsp
  内部禁用该判定以保留补点重试链路
* 同步更新 README/AGENTS/计划文档的 todo 状态和实现说明
This commit is contained in:
2026-04-27 00:18:50 +08:00
parent 390d066ece
commit 69fa3edd89
18 changed files with 1631 additions and 122 deletions

View File

@@ -114,6 +114,27 @@ public sealed class DomainModelTests
Assert.Empty(snapshot.JointPositions);
Assert.Empty(snapshot.CartesianPose);
Assert.Empty(snapshot.ActiveAlarms);
Assert.Empty(snapshot.StateTailWords);
}
/// <summary>
/// 验证控制器快照会保留 TCP 10010 尾部状态字作为诊断字段。
/// </summary>
[Fact]
public void ControllerStateSnapshot_CopiesStateTailWordsForDiagnostics()
{
var snapshot = new ControllerStateSnapshot(
capturedAt: DateTimeOffset.Parse("2026-04-23T10:00:00+08:00"),
connectionState: "Connected",
isEnabled: true,
isInMotion: false,
speedRatio: 1.0,
stateTailWords: [2u, 0u, 0u, 1u]);
var json = JsonSerializer.Serialize(snapshot);
Assert.Equal([2u, 0u, 0u, 1u], snapshot.StateTailWords);
Assert.Contains("\"stateTailWords\":[2,0,0,1]", json);
}
/// <summary>

View File

@@ -130,6 +130,125 @@ public sealed class FanucCommandClientTests : IDisposable
await handlerTask.WaitAsync(TimeSpan.FromSeconds(2), _cts.Token);
}
/// <summary>
/// 验证 GetSpeedRatio 发送空业务体命令,并按 ratio_int / 100.0 解析倍率。
/// </summary>
[Fact]
public async Task GetSpeedRatioAsync_SendsFrameAndParsesRatio()
{
using var client = new FanucCommandClient();
var handlerTask = RunSingleResponseControllerAsync(
FanucCommandProtocol.PackGetSpeedRatioCommand(),
FanucCommandProtocol.PackFrame(FanucCommandMessageIds.GetSpeedRatio, Convert.FromHexString("0000005a00000000")),
_cts.Token);
await client.ConnectAsync("127.0.0.1", Port, _cts.Token);
var response = await client.GetSpeedRatioAsync(_cts.Token);
Assert.True(response.IsSuccess);
Assert.Equal(0.9, response.Ratio, precision: 6);
await handlerTask.WaitAsync(TimeSpan.FromSeconds(2), _cts.Token);
}
/// <summary>
/// 验证 SetSpeedRatio 会把 double 倍率夹到 0..100 的整数百分比后下发。
/// </summary>
[Fact]
public async Task SetSpeedRatioAsync_SendsClampedPercentAndParsesSuccess()
{
using var client = new FanucCommandClient();
var handlerTask = RunSingleResponseControllerAsync(
FanucCommandProtocol.PackSetSpeedRatioCommand(2.0),
FanucCommandProtocol.PackFrame(FanucCommandMessageIds.SetSpeedRatio, Convert.FromHexString("00000000")),
_cts.Token);
await client.ConnectAsync("127.0.0.1", Port, _cts.Token);
var response = await client.SetSpeedRatioAsync(2.0, _cts.Token);
Assert.True(response.IsSuccess);
await handlerTask.WaitAsync(TimeSpan.FromSeconds(2), _cts.Token);
}
/// <summary>
/// 验证 GetTcp 会发送 tcp_id 请求,并解析 result_code + tcp_id + 7 个 float 位姿。
/// </summary>
[Fact]
public async Task GetTcpAsync_SendsFrameAndParsesPose()
{
using var client = new FanucCommandClient();
var handlerTask = RunSingleResponseControllerAsync(
FanucCommandProtocol.PackGetTcpCommand(1),
FanucCommandProtocol.PackFrame(
FanucCommandMessageIds.GetTcp,
Convert.FromHexString("00000000000000013f80000040000000404000000000000000000000000000003f800000")),
_cts.Token);
await client.ConnectAsync("127.0.0.1", Port, _cts.Token);
var response = await client.GetTcpAsync(1, _cts.Token);
Assert.True(response.IsSuccess);
Assert.Equal([1.0, 2.0, 3.0, 0.0, 0.0, 0.0, 1.0], response.Pose);
await handlerTask.WaitAsync(TimeSpan.FromSeconds(2), _cts.Token);
}
/// <summary>
/// 验证 SetTcp 会按 tcp_id + 7 个 float 位姿下发并解析结果码。
/// </summary>
[Fact]
public async Task SetTcpAsync_SendsFrameAndParsesSuccess()
{
using var client = new FanucCommandClient();
var handlerTask = RunSingleResponseControllerAsync(
FanucCommandProtocol.PackSetTcpCommand(1, [1.0, 2.0, 3.0, 0.0, 0.0, 0.0, 1.0]),
FanucCommandProtocol.PackFrame(FanucCommandMessageIds.SetTcp, Convert.FromHexString("00000000")),
_cts.Token);
await client.ConnectAsync("127.0.0.1", Port, _cts.Token);
var response = await client.SetTcpAsync(1, [1.0, 2.0, 3.0, 0.0, 0.0, 0.0, 1.0], _cts.Token);
Assert.True(response.IsSuccess);
await handlerTask.WaitAsync(TimeSpan.FromSeconds(2), _cts.Token);
}
/// <summary>
/// 验证 GetIo 会按 io_type、io_index 顺序请求,并解析 float IO 值。
/// </summary>
[Fact]
public async Task GetIoAsync_SendsFrameAndParsesValue()
{
using var client = new FanucCommandClient();
var handlerTask = RunSingleResponseControllerAsync(
FanucCommandProtocol.PackGetIoCommand(FanucIoTypes.DigitalOutput, 7),
FanucCommandProtocol.PackFrame(FanucCommandMessageIds.GetIo, Convert.FromHexString("000000003f800000")),
_cts.Token);
await client.ConnectAsync("127.0.0.1", Port, _cts.Token);
var response = await client.GetIoAsync(7, "DO", _cts.Token);
Assert.True(response.IsSuccess);
Assert.True(response.Value);
await handlerTask.WaitAsync(TimeSpan.FromSeconds(2), _cts.Token);
}
/// <summary>
/// 验证 SetIo 会按 io_type、io_index、float value 顺序下发并解析结果码。
/// </summary>
[Fact]
public async Task SetIoAsync_SendsFrameAndParsesSuccess()
{
using var client = new FanucCommandClient();
var handlerTask = RunSingleResponseControllerAsync(
FanucCommandProtocol.PackSetIoCommand(FanucIoTypes.DigitalOutput, 7, true),
FanucCommandProtocol.PackFrame(FanucCommandMessageIds.SetIo, Convert.FromHexString("00000000")),
_cts.Token);
await client.ConnectAsync("127.0.0.1", Port, _cts.Token);
var response = await client.SetIoAsync(7, true, "DO", _cts.Token);
Assert.True(response.IsSuccess);
await handlerTask.WaitAsync(TimeSpan.FromSeconds(2), _cts.Token);
}
/// <summary>
/// 验证命令响应 result_code 非零时,客户端会抛出可诊断异常而不是让上层误判成功。
/// </summary>

View File

@@ -39,6 +39,64 @@ public sealed class FanucProtocolTests
Assert.Equal(1u, statusResponse.ProgramStatus);
}
/// <summary>
/// 验证 TCP 10012 的速度倍率、TCP 和 IO 请求体字段顺序与逆向文档一致。
/// </summary>
[Fact]
public void CommandProtocol_PacksParameterCommandBodies()
{
var setTcpFrame = FanucCommandProtocol.PackSetTcpCommand(1, [1.0, 2.0, 3.0, 0.0, 0.0, 0.0, 1.0]);
Assert.Equal(
Convert.FromHexString("646f7a0000000e000022067a6f64"),
FanucCommandProtocol.PackGetSpeedRatioCommand());
Assert.Equal(
Convert.FromHexString("646f7a0000001200002207000000507a6f64"),
FanucCommandProtocol.PackSetSpeedRatioCommand(0.8));
Assert.Equal(
Convert.FromHexString("646f7a0000001200002200000000017a6f64"),
FanucCommandProtocol.PackGetTcpCommand(1));
Assert.Equal(
Convert.FromHexString("646f7a000000160000220800000002000000077a6f64"),
FanucCommandProtocol.PackGetIoCommand(FanucIoTypes.DigitalOutput, 7));
Assert.Equal(
Convert.FromHexString("646f7a0000001a0000220900000002000000073f8000007a6f64"),
FanucCommandProtocol.PackSetIoCommand(FanucIoTypes.DigitalOutput, 7, true));
Assert.Equal(FanucCommandMessageIds.SetTcp, BinaryPrimitives.ReadUInt32BigEndian(setTcpFrame.AsSpan(7, 4)));
Assert.Equal(1u, BinaryPrimitives.ReadUInt32BigEndian(setTcpFrame.AsSpan(11, 4)));
Assert.Equal(1.0f, BinaryPrimitives.ReadSingleBigEndian(setTcpFrame.AsSpan(15, 4)));
Assert.Equal(1.0f, BinaryPrimitives.ReadSingleBigEndian(setTcpFrame.AsSpan(39, 4)));
}
/// <summary>
/// 验证 TCP 10012 参数响应解析使用各自不同的字段顺序。
/// </summary>
[Fact]
public void CommandProtocol_ParsesParameterResponses()
{
var speedRatioResponse = FanucCommandProtocol.ParseSpeedRatioResponse(
FanucCommandProtocol.PackFrame(
FanucCommandMessageIds.GetSpeedRatio,
Convert.FromHexString("0000005000000000")));
var tcpResponse = FanucCommandProtocol.ParseTcpResponse(
FanucCommandProtocol.PackFrame(
FanucCommandMessageIds.GetTcp,
Convert.FromHexString("00000000000000013f80000040000000404000000000000000000000000000003f800000")));
var ioResponse = FanucCommandProtocol.ParseIoResponse(
FanucCommandProtocol.PackFrame(
FanucCommandMessageIds.GetIo,
Convert.FromHexString("000000003f800000")));
Assert.True(speedRatioResponse.IsSuccess);
Assert.Equal(0.8, speedRatioResponse.Ratio, precision: 6);
Assert.True(tcpResponse.IsSuccess);
Assert.Equal(1u, tcpResponse.TcpId);
Assert.Equal([1.0, 2.0, 3.0, 0.0, 0.0, 0.0, 1.0], tcpResponse.Pose);
Assert.True(ioResponse.IsSuccess);
Assert.True(ioResponse.Value);
Assert.Equal(1.0, ioResponse.NumericValue, precision: 6);
}
/// <summary>
/// 验证 TCP 10010 状态帧可以从抓包样本解析出尾部状态槽位。
/// </summary>
@@ -52,6 +110,52 @@ public sealed class FanucProtocolTests
Assert.Equal(6, frame.Pose.Count);
Assert.Equal(9, frame.JointOrExtensionValues.Count);
Assert.Equal([2u, 0u, 0u, 1u], frame.TailWords);
Assert.Equal(frame.Pose, frame.CartesianPose);
Assert.Equal(frame.JointOrExtensionValues.Take(6), frame.JointDegrees);
Assert.Equal(frame.JointOrExtensionValues.Skip(6), frame.ExternalAxes);
Assert.Equal(frame.TailWords, frame.RawTailWords);
Assert.Equal(2u, frame.StatusWord0);
Assert.Equal(0u, frame.StatusWord1);
Assert.Equal(0u, frame.StatusWord2);
Assert.Equal(1u, frame.StatusWord3);
}
/// <summary>
/// 验证 pcap 中多条唯一 TCP 10010 状态帧都符合固定 90B 布局。
/// </summary>
[Theory]
[InlineData("646f7a0000005a0000000040eac85a43b2ef4043aba8e9421ed9c1c2828105c2ed981f3fbdbda0bed4764ebe92aacc3efd9f0a3f317ce9be5d4580000000000000000000000000000000020000000000000000000000017a6f64")]
[InlineData("646f7a0000005a00000000415aab64440a5302439adef542b39739c293c441431d50423fcdb7003d862fe3beca5730bf60eab23f148e403f89269d000000000000000000000000000000020000000000000000000000017a6f64")]
[InlineData("646f7a0000005a000000004221b6f9440b9ce043a129ac42b292bac29cba78431bddcb3fc743213d90268dbeba5351bf64bc1b3f0cbdf73f826864000000000000000000000000000000020000000000000000000000017a6f64")]
public void StateProtocol_ParsesMultipleCapturedPcapFrames(string frameHex)
{
var frameBytes = Convert.FromHexString(frameHex);
var frame = FanucStateProtocol.ParseFrame(frameBytes);
Assert.Equal(FanucStateProtocol.StateFrameLength, frameBytes.Length);
Assert.Equal(6, frame.CartesianPose.Count);
Assert.Equal(6, frame.JointDegrees.Count);
Assert.Equal(3, frame.ExternalAxes.Count);
Assert.Equal([2u, 0u, 0u, 1u], frame.RawTailWords);
}
/// <summary>
/// 验证 TCP 10010 状态帧会拒绝损坏的长度和 magic避免后台循环缓存坏帧。
/// </summary>
[Fact]
public void StateProtocol_RejectsMalformedStateFrames()
{
var validFrame = Convert.FromHexString(
"646f7a0000005a000000004388a23243f1ed7f43e9de6bc265031ec2b33cc3c278e0153f8742f53c3f128dbc929529bc7861d63cb0184c3c1ca1a7000000000000000000000000000000020000000000000000000000017a6f64");
var wrongMagic = validFrame.ToArray();
wrongMagic[0] = 0;
var wrongLength = validFrame.ToArray();
wrongLength[6] = 0x59;
Assert.Throws<InvalidDataException>(() => FanucStateProtocol.ParseFrame(validFrame.AsSpan(0, validFrame.Length - 1)));
Assert.Throws<InvalidDataException>(() => FanucStateProtocol.ParseFrame(wrongMagic));
Assert.Throws<InvalidDataException>(() => FanucStateProtocol.ParseFrame(wrongLength));
}
/// <summary>

View File

@@ -1,3 +1,4 @@
using System.Buffers.Binary;
using System.Net;
using System.Net.Sockets;
using Flyshot.Runtime.Fanuc.Protocol;
@@ -96,8 +97,7 @@ public sealed class FanucStateClientTests : IDisposable
public async Task Disconnect_ClearsLatestFrame()
{
using var client = new FanucStateClient();
var capturedFrame = Convert.FromHexString(
"646f7a0000005a000000004388a23243f1ed7f43e9de6bc265031ec2b33cc3c278e0153f8742f53c3f128dbc929529bc7861d63cb0184c3c1ca1a7000000000000000000000000000000020000000000000000000000017a6f64");
var capturedFrame = CapturedStateFrame();
var handlerTask = RunStreamingControllerAsync(capturedFrame, _cts.Token);
@@ -110,6 +110,69 @@ public sealed class FanucStateClientTests : IDisposable
await handlerTask.WaitAsync(TimeSpan.FromSeconds(2), _cts.Token);
}
/// <summary>
/// 验证状态通道长时间收不到完整帧时会标记陈旧并触发重连。
/// </summary>
[Fact]
public async Task GetStatus_MarksFrameStaleAndReconnectsWhenFrameTimesOut()
{
using var client = new FanucStateClient(new FanucStateClientOptions
{
FrameTimeout = TimeSpan.FromMilliseconds(100),
ReconnectInitialDelay = TimeSpan.FromMilliseconds(20),
ReconnectMaxDelay = TimeSpan.FromMilliseconds(50),
ConnectTimeout = TimeSpan.FromSeconds(1),
});
var acceptTask = _listener.AcceptTcpClientAsync(_cts.Token);
await client.ConnectAsync("127.0.0.1", Port, _cts.Token);
using var controller = await acceptTask.AsTask().WaitAsync(TimeSpan.FromSeconds(2), _cts.Token);
await WaitUntilAsync(
() => client.GetStatus().ReconnectAttemptCount > 0,
TimeSpan.FromSeconds(2),
_cts.Token);
var status = client.GetStatus();
Assert.True(status.IsFrameStale);
Assert.True(status.State is FanucStateConnectionState.TimedOut or FanucStateConnectionState.Reconnecting or FanucStateConnectionState.Connected);
Assert.NotNull(status.LastErrorMessage);
Assert.Contains("超时", status.LastErrorMessage);
}
/// <summary>
/// 验证状态通道在控制柜主动断开后可以退避重连并接收新连接上的状态帧。
/// </summary>
[Fact]
public async Task ReceiveLoop_ReconnectsAfterEofAndKeepsReceivingFrames()
{
using var client = new FanucStateClient(new FanucStateClientOptions
{
FrameTimeout = TimeSpan.FromMilliseconds(500),
ReconnectInitialDelay = TimeSpan.FromMilliseconds(20),
ReconnectMaxDelay = TimeSpan.FromMilliseconds(50),
ConnectTimeout = TimeSpan.FromSeconds(1),
});
var firstFrame = CapturedStateFrame(1);
var secondFrame = CapturedStateFrame(2);
var handlerTask = RunReconnectControllerAsync(firstFrame, secondFrame, _cts.Token);
await client.ConnectAsync("127.0.0.1", Port, _cts.Token);
await WaitUntilAsync(
() => client.GetLatestFrame()?.MessageId == 2u,
TimeSpan.FromSeconds(2),
_cts.Token);
var status = client.GetStatus();
Assert.Equal(FanucStateConnectionState.Connected, status.State);
Assert.True(status.ReconnectAttemptCount >= 1);
client.Disconnect();
await handlerTask.WaitAsync(TimeSpan.FromSeconds(2), _cts.Token);
}
/// <summary>
/// 启动模拟控制器,持续发送状态帧流。
/// </summary>
@@ -135,4 +198,66 @@ public sealed class FanucStateClientTests : IDisposable
// 客户端断开。
}
}
/// <summary>
/// 启动模拟控制器:第一条连接发一帧后主动断开,第二条连接持续发送新帧。
/// </summary>
private async Task RunReconnectControllerAsync(byte[] firstFrame, byte[] secondFrame, CancellationToken cancellationToken)
{
using (var firstController = await _listener.AcceptTcpClientAsync(cancellationToken))
{
await using var firstStream = firstController.GetStream();
await firstStream.WriteAsync(firstFrame, cancellationToken);
}
using var secondController = await _listener.AcceptTcpClientAsync(cancellationToken);
await using var secondStream = secondController.GetStream();
try
{
while (!cancellationToken.IsCancellationRequested)
{
await secondStream.WriteAsync(secondFrame, cancellationToken);
await Task.Delay(50, cancellationToken);
}
}
catch (OperationCanceledException)
{
// 正常取消。
}
catch (IOException)
{
// 客户端断开。
}
}
/// <summary>
/// 构造来自 j519 抓包的状态帧,并按测试需要覆写 message_id。
/// </summary>
private static byte[] CapturedStateFrame(uint messageId = 0)
{
var frame = Convert.FromHexString(
"646f7a0000005a000000004388a23243f1ed7f43e9de6bc265031ec2b33cc3c278e0153f8742f53c3f128dbc929529bc7861d63cb0184c3c1ca1a7000000000000000000000000000000020000000000000000000000017a6f64");
BinaryPrimitives.WriteUInt32BigEndian(frame.AsSpan(7, 4), messageId);
return frame;
}
/// <summary>
/// 等待异步后台循环达到预期状态,超时后让测试明确失败。
/// </summary>
private static async Task WaitUntilAsync(Func<bool> predicate, TimeSpan timeout, CancellationToken cancellationToken)
{
var deadline = DateTimeOffset.UtcNow + timeout;
while (DateTimeOffset.UtcNow < deadline)
{
if (predicate())
{
return;
}
await Task.Delay(20, cancellationToken);
}
Assert.True(predicate(), "等待状态通道后台循环达到预期状态超时。");
}
}

View File

@@ -34,6 +34,30 @@ public sealed class PlanningCompatibilityTests
Assert.All(trajectory.WaypointTimes.Zip(trajectory.WaypointTimes.Skip(1)), pair => Assert.True(pair.Second > pair.First));
}
/// <summary>
/// 验证普通 ICSP 在最终最优解仍超限时会显式失败,而不是返回不可执行轨迹。
/// </summary>
[Fact]
public void ICspPlanner_Throws_WhenFinalGlobalScaleExceedsOne()
{
var request = new TrajectoryRequest(
robot: CreateRobotProfile([0.1], [0.1], [0.1]),
program: CreateProgram(
new[]
{
new[] { 0.0 },
new[] { 10.0 },
new[] { 20.0 },
new[] { 30.0 }
}),
method: PlanningMethod.Icsp);
var planner = new ICspPlanner(maxIterations: 0);
var exception = Assert.Throws<InvalidOperationException>(() => planner.Plan(request));
Assert.Contains("global_scale", exception.Message);
}
/// <summary>
/// 验证 speed09 风格的大跳变样本在 self-adapt-icsp 下会通过补中点收敛。
/// </summary>

View File

@@ -3,6 +3,7 @@ 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;
@@ -45,6 +46,58 @@ public sealed class RuntimeOrchestrationTests
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>