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

@@ -2,23 +2,153 @@ using System.Net.Sockets;
namespace Flyshot.Runtime.Fanuc.Protocol;
/// <summary>
/// 表示 FANUC TCP 10010 状态通道客户端的连接阶段。
/// </summary>
public enum FanucStateConnectionState
{
/// <summary>
/// 状态通道未连接。
/// </summary>
Disconnected,
/// <summary>
/// 状态通道正在建立连接。
/// </summary>
Connecting,
/// <summary>
/// 状态通道已连接并由后台循环接收状态帧。
/// </summary>
Connected,
/// <summary>
/// 状态通道在限定时间内没有收到完整状态帧。
/// </summary>
TimedOut,
/// <summary>
/// 状态通道正在按退避策略重新连接。
/// </summary>
Reconnecting,
}
/// <summary>
/// 定义 FANUC TCP 10010 状态通道的超时和重连参数。
/// </summary>
public sealed class FanucStateClientOptions
{
/// <summary>
/// 获取或设置接收一帧完整 90B 状态帧允许的最长时间。
/// </summary>
public TimeSpan FrameTimeout { get; init; } = TimeSpan.FromMilliseconds(250);
/// <summary>
/// 获取或设置初始重连等待时间。
/// </summary>
public TimeSpan ReconnectInitialDelay { get; init; } = TimeSpan.FromMilliseconds(100);
/// <summary>
/// 获取或设置重连等待时间的上限。
/// </summary>
public TimeSpan ReconnectMaxDelay { get; init; } = TimeSpan.FromSeconds(2);
/// <summary>
/// 获取或设置单次 TCP 建连允许的最长时间。
/// </summary>
public TimeSpan ConnectTimeout { get; init; } = TimeSpan.FromSeconds(2);
}
/// <summary>
/// 表示 FANUC TCP 10010 状态通道客户端的当前诊断状态。
/// </summary>
public sealed class FanucStateClientStatus
{
/// <summary>
/// 初始化状态通道诊断状态。
/// </summary>
public FanucStateClientStatus(
FanucStateConnectionState state,
bool isFrameStale,
DateTimeOffset? lastFrameAt,
long reconnectAttemptCount,
string? lastErrorMessage)
{
State = state;
IsFrameStale = isFrameStale;
LastFrameAt = lastFrameAt;
ReconnectAttemptCount = reconnectAttemptCount;
LastErrorMessage = lastErrorMessage;
}
/// <summary>
/// 获取状态通道当前连接阶段。
/// </summary>
public FanucStateConnectionState State { get; }
/// <summary>
/// 获取最近缓存状态帧是否已经超过状态帧超时窗口。
/// </summary>
public bool IsFrameStale { get; }
/// <summary>
/// 获取最近一次成功解析状态帧的 UTC 时间。
/// </summary>
public DateTimeOffset? LastFrameAt { get; }
/// <summary>
/// 获取后台循环发起重连的累计次数。
/// </summary>
public long ReconnectAttemptCount { get; }
/// <summary>
/// 获取最近一次状态通道异常的诊断文本。
/// </summary>
public string? LastErrorMessage { get; }
}
/// <summary>
/// FANUC TCP 10010 状态通道客户端,持续接收状态帧并缓存最新快照。
/// </summary>
public sealed class FanucStateClient : IDisposable
{
private readonly object _stateLock = new();
private readonly FanucStateClientOptions _options;
private TcpClient? _tcpClient;
private NetworkStream? _stream;
private CancellationTokenSource? _receiveCts;
private Task? _receiveTask;
private FanucStateFrame? _latestFrame;
private FanucStateConnectionState _connectionState = FanucStateConnectionState.Disconnected;
private DateTimeOffset? _lastConnectedAt;
private DateTimeOffset? _lastFrameAt;
private long _reconnectAttemptCount;
private string? _lastErrorMessage;
private bool _disposed;
/// <summary>
/// 使用默认状态通道参数初始化客户端。
/// </summary>
public FanucStateClient()
: this(new FanucStateClientOptions())
{
}
/// <summary>
/// 使用指定状态通道参数初始化客户端。
/// </summary>
/// <param name="options">超时和重连参数。</param>
public FanucStateClient(FanucStateClientOptions options)
{
ArgumentNullException.ThrowIfNull(options);
ValidateOptions(options);
_options = options;
}
/// <summary>
/// 获取当前是否已建立连接。
/// </summary>
public bool IsConnected => _tcpClient?.Connected ?? false;
public bool IsConnected => GetStatus().State == FanucStateConnectionState.Connected;
/// <summary>
/// 建立到 FANUC 控制柜 TCP 10010 状态通道的连接并启动后台接收循环。
@@ -35,17 +165,44 @@ public sealed class FanucStateClient : IDisposable
throw new ArgumentException("IP 不能为空。", nameof(ip));
}
if (_tcpClient is not null)
if (_receiveTask is not null)
{
throw new InvalidOperationException("状态通道已经连接,请先 Disconnect。");
}
_tcpClient = new TcpClient { NoDelay = true };
await _tcpClient.ConnectAsync(ip, port, cancellationToken).ConfigureAwait(false);
_stream = _tcpClient.GetStream();
_receiveCts = new CancellationTokenSource();
_receiveTask = Task.Run(() => ReceiveLoopAsync(_receiveCts.Token), _receiveCts.Token);
using var linkedCts = CancellationTokenSource.CreateLinkedTokenSource(cancellationToken, _receiveCts.Token);
lock (_stateLock)
{
_connectionState = FanucStateConnectionState.Connecting;
_latestFrame = null;
_lastConnectedAt = null;
_lastFrameAt = null;
_reconnectAttemptCount = 0;
_lastErrorMessage = null;
}
try
{
await OpenConnectionAsync(ip, port, linkedCts.Token).ConfigureAwait(false);
}
catch
{
CloseCurrentConnection();
lock (_stateLock)
{
_connectionState = FanucStateConnectionState.Disconnected;
}
_receiveCts.Dispose();
_receiveCts = null;
throw;
}
_receiveTask = Task.Run(
() => ReceiveAndReconnectLoopAsync(ip, port, _receiveCts.Token),
_receiveCts.Token);
}
/// <summary>
@@ -55,31 +212,7 @@ public sealed class FanucStateClient : IDisposable
{
ObjectDisposedException.ThrowIf(_disposed, this);
_receiveCts?.Cancel();
try
{
_receiveTask?.Wait(TimeSpan.FromSeconds(2));
}
catch (AggregateException)
{
// 后台循环可能因取消而抛出 OperationCanceledException忽略即可。
}
_receiveTask?.Dispose();
_receiveTask = null;
_receiveCts?.Dispose();
_receiveCts = null;
_stream?.Dispose();
_stream = null;
_tcpClient?.Dispose();
_tcpClient = null;
lock (_stateLock)
{
_latestFrame = null;
}
Shutdown(clearLatestFrame: true);
}
/// <summary>
@@ -96,6 +229,25 @@ public sealed class FanucStateClient : IDisposable
}
}
/// <summary>
/// 获取状态通道当前诊断状态。
/// </summary>
/// <returns>状态通道诊断快照。</returns>
public FanucStateClientStatus GetStatus()
{
ObjectDisposedException.ThrowIf(_disposed, this);
lock (_stateLock)
{
return new FanucStateClientStatus(
_connectionState,
IsFrameStaleLocked(DateTimeOffset.UtcNow),
_lastFrameAt,
_reconnectAttemptCount,
_lastErrorMessage);
}
}
/// <summary>
/// 释放客户端资源。
/// </summary>
@@ -107,7 +259,226 @@ public sealed class FanucStateClient : IDisposable
}
_disposed = true;
Shutdown(clearLatestFrame: true);
}
/// <summary>
/// 后台循环:持续接收状态帧;断线、超时或坏帧后进入退避重连。
/// </summary>
private async Task ReceiveAndReconnectLoopAsync(string ip, int port, CancellationToken cancellationToken)
{
var reconnectDelay = _options.ReconnectInitialDelay;
while (!cancellationToken.IsCancellationRequested)
{
try
{
await ReceiveCurrentConnectionAsync(cancellationToken).ConfigureAwait(false);
reconnectDelay = _options.ReconnectInitialDelay;
}
catch (OperationCanceledException) when (cancellationToken.IsCancellationRequested)
{
return;
}
catch (TimeoutException ex)
{
MarkReceiveFailure(FanucStateConnectionState.TimedOut, ex.Message);
}
catch (Exception ex) when (ex is IOException or InvalidDataException or SocketException or ObjectDisposedException)
{
MarkReceiveFailure(FanucStateConnectionState.Reconnecting, ex.Message);
}
CloseCurrentConnection();
if (cancellationToken.IsCancellationRequested)
{
return;
}
reconnectDelay = await ReconnectWithBackoffAsync(ip, port, reconnectDelay, cancellationToken).ConfigureAwait(false);
}
}
/// <summary>
/// 从当前连接中持续读取状态帧,直到连接异常或被取消。
/// </summary>
private async Task ReceiveCurrentConnectionAsync(CancellationToken cancellationToken)
{
NetworkStream stream;
lock (_stateLock)
{
stream = _stream ?? throw new IOException("状态通道未连接。");
}
var buffer = new byte[FanucStateProtocol.StateFrameLength];
while (!cancellationToken.IsCancellationRequested)
{
await ReadExactAsync(stream, buffer, cancellationToken).ConfigureAwait(false);
var frame = FanucStateProtocol.ParseFrame(buffer);
lock (_stateLock)
{
_latestFrame = frame;
_lastFrameAt = DateTimeOffset.UtcNow;
_connectionState = FanucStateConnectionState.Connected;
_lastErrorMessage = null;
}
}
}
/// <summary>
/// 从流中精确读取固定长度字节,超过帧超时窗口则抛出超时异常。
/// </summary>
private async Task ReadExactAsync(NetworkStream stream, byte[] buffer, CancellationToken cancellationToken)
{
using var timeoutCts = CancellationTokenSource.CreateLinkedTokenSource(cancellationToken);
timeoutCts.CancelAfter(_options.FrameTimeout);
var totalRead = 0;
try
{
while (totalRead < buffer.Length)
{
var read = await stream.ReadAsync(
buffer.AsMemory(totalRead, buffer.Length - totalRead),
timeoutCts.Token).ConfigureAwait(false);
if (read == 0)
{
throw new IOException("状态通道已断开,读取到 EOF。");
}
totalRead += read;
}
}
catch (OperationCanceledException) when (!cancellationToken.IsCancellationRequested)
{
throw new TimeoutException("状态通道接收超时,未在限定时间内收到完整 90B 状态帧。");
}
}
/// <summary>
/// 打开 TCP 状态通道并更新连接状态。
/// </summary>
private async Task OpenConnectionAsync(string ip, int port, CancellationToken cancellationToken)
{
var tcpClient = new TcpClient { NoDelay = true };
try
{
using var timeoutCts = CancellationTokenSource.CreateLinkedTokenSource(cancellationToken);
timeoutCts.CancelAfter(_options.ConnectTimeout);
await tcpClient.ConnectAsync(ip, port, timeoutCts.Token).ConfigureAwait(false);
lock (_stateLock)
{
_tcpClient = tcpClient;
_stream = tcpClient.GetStream();
_lastConnectedAt = DateTimeOffset.UtcNow;
_connectionState = FanucStateConnectionState.Connected;
}
}
catch (OperationCanceledException) when (!cancellationToken.IsCancellationRequested)
{
tcpClient.Dispose();
throw new TimeoutException("状态通道建连超时。");
}
catch
{
tcpClient.Dispose();
throw;
}
}
/// <summary>
/// 按退避策略循环尝试重新连接,并返回下一次异常后的退避时间。
/// </summary>
private async Task<TimeSpan> ReconnectWithBackoffAsync(
string ip,
int port,
TimeSpan reconnectDelay,
CancellationToken cancellationToken)
{
var nextDelay = reconnectDelay;
while (!cancellationToken.IsCancellationRequested)
{
lock (_stateLock)
{
_connectionState = FanucStateConnectionState.Reconnecting;
}
await Task.Delay(nextDelay, cancellationToken).ConfigureAwait(false);
lock (_stateLock)
{
_reconnectAttemptCount++;
}
try
{
await OpenConnectionAsync(ip, port, cancellationToken).ConfigureAwait(false);
return _options.ReconnectInitialDelay;
}
catch (OperationCanceledException) when (cancellationToken.IsCancellationRequested)
{
throw;
}
catch (Exception ex) when (ex is SocketException or IOException or TimeoutException)
{
CloseCurrentConnection();
lock (_stateLock)
{
_connectionState = FanucStateConnectionState.Reconnecting;
_lastErrorMessage = ex.Message;
}
nextDelay = IncreaseReconnectDelay(nextDelay);
}
}
return nextDelay;
}
/// <summary>
/// 关闭当前 TCP 连接,不清除最新状态帧,供重连路径保留诊断数据。
/// </summary>
private void CloseCurrentConnection()
{
NetworkStream? stream;
TcpClient? tcpClient;
lock (_stateLock)
{
stream = _stream;
tcpClient = _tcpClient;
_stream = null;
_tcpClient = null;
}
stream?.Dispose();
tcpClient?.Dispose();
}
/// <summary>
/// 记录接收异常并更新状态通道连接阶段。
/// </summary>
private void MarkReceiveFailure(FanucStateConnectionState state, string message)
{
lock (_stateLock)
{
_connectionState = state;
_lastErrorMessage = message;
}
}
/// <summary>
/// 关闭后台循环和 socket 资源。
/// </summary>
private void Shutdown(bool clearLatestFrame)
{
_receiveCts?.Cancel();
CloseCurrentConnection();
try
{
@@ -115,74 +486,76 @@ public sealed class FanucStateClient : IDisposable
}
catch (AggregateException)
{
// 忽略取消异常
// 后台循环可能因取消而抛出 OperationCanceledException忽略即可
}
_receiveTask?.Dispose();
_receiveTask = null;
_receiveCts?.Dispose();
_stream?.Dispose();
_tcpClient?.Dispose();
}
_receiveCts = null;
/// <summary>
/// 后台循环:持续从流中读取固定长度状态帧并更新缓存。
/// </summary>
private async Task ReceiveLoopAsync(CancellationToken cancellationToken)
{
if (_stream is null)
lock (_stateLock)
{
return;
}
var buffer = new byte[FanucStateProtocol.StateFrameLength];
try
{
while (!cancellationToken.IsCancellationRequested)
_connectionState = FanucStateConnectionState.Disconnected;
_lastConnectedAt = null;
_lastErrorMessage = null;
_reconnectAttemptCount = 0;
if (clearLatestFrame)
{
await ReadExactAsync(buffer, cancellationToken).ConfigureAwait(false);
var frame = FanucStateProtocol.ParseFrame(buffer);
lock (_stateLock)
{
_latestFrame = frame;
}
_latestFrame = null;
_lastFrameAt = null;
}
}
catch (OperationCanceledException)
{
// 正常取消,无需处理。
}
catch (IOException)
{
// 连接断开,退出循环。
}
catch (InvalidDataException)
{
// 解析到异常帧,退出循环由上层重连。
}
}
/// <summary>
/// 从流中精确读取固定长度字节
/// 判断缓存帧是否已经不能代表当前控制柜状态
/// </summary>
private async Task ReadExactAsync(byte[] buffer, CancellationToken cancellationToken)
private bool IsFrameStaleLocked(DateTimeOffset now)
{
if (_stream is null)
if (_latestFrame is null)
{
throw new InvalidOperationException("状态通道未连接。");
return _connectionState is FanucStateConnectionState.TimedOut or FanucStateConnectionState.Reconnecting
|| _reconnectAttemptCount > 0
|| (_lastConnectedAt.HasValue && now - _lastConnectedAt.Value > _options.FrameTimeout);
}
var totalRead = 0;
while (totalRead < buffer.Length)
{
var read = await _stream.ReadAsync(buffer.AsMemory(totalRead), cancellationToken).ConfigureAwait(false);
if (read == 0)
{
throw new IOException("状态通道已断开,读取到 EOF。");
}
return _lastFrameAt.HasValue && now - _lastFrameAt.Value > _options.FrameTimeout;
}
totalRead += read;
/// <summary>
/// 计算下一轮重连等待时间。
/// </summary>
private TimeSpan IncreaseReconnectDelay(TimeSpan currentDelay)
{
var doubledMilliseconds = Math.Max(currentDelay.TotalMilliseconds * 2.0, _options.ReconnectInitialDelay.TotalMilliseconds);
var cappedMilliseconds = Math.Min(doubledMilliseconds, _options.ReconnectMaxDelay.TotalMilliseconds);
return TimeSpan.FromMilliseconds(cappedMilliseconds);
}
/// <summary>
/// 校验状态通道参数,避免后台循环使用无效时间窗口。
/// </summary>
private static void ValidateOptions(FanucStateClientOptions options)
{
ValidatePositive(options.FrameTimeout, nameof(options.FrameTimeout));
ValidatePositive(options.ReconnectInitialDelay, nameof(options.ReconnectInitialDelay));
ValidatePositive(options.ReconnectMaxDelay, nameof(options.ReconnectMaxDelay));
ValidatePositive(options.ConnectTimeout, nameof(options.ConnectTimeout));
if (options.ReconnectMaxDelay < options.ReconnectInitialDelay)
{
throw new ArgumentOutOfRangeException(nameof(options), "最大重连等待时间不能小于初始重连等待时间。");
}
}
/// <summary>
/// 校验时间参数必须为正值。
/// </summary>
private static void ValidatePositive(TimeSpan value, string parameterName)
{
if (value <= TimeSpan.Zero)
{
throw new ArgumentOutOfRangeException(parameterName, "时间参数必须大于 0。");
}
}
}