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

Merge commit '0952958af7685646cdbf2a4907c6e2ae59d4d352'

Browse Source
This commit is contained in:
yunxiao.zhu
2026-05-28 10:44:03 +08:00
parent 4eeaa3fef3
commit 35bdb08d11
144 changed files with 33730 additions and 392 deletions
Download Patch File Download Diff File Expand all files Collapse all files

654
src/Flyshot.Runtime.Fanuc/Protocol/FanucJ519Client.cs Normal file
View File

@@ -0,0 +1,654 @@
using System.Diagnostics;
using System.Net.Sockets;
using Microsoft.Extensions.Logging;
namespace Flyshot.Runtime.Fanuc.Protocol;
/// <summary>
/// FANUC UDP 60015 J519/ICSP 伺服运动客户端，提供状态包驱动的命令发送与响应接收能力。
/// </summary>
public sealed class FanucJ519Client : IDisposable
{
private readonly object _commandLock = new();
private readonly object _responseLock = new();
private readonly ILogger<FanucJ519Client>? _logger;
private UdpClient? _udpClient;
private CancellationTokenSource? _cts;
private Thread? _receiveThread;
private FanucJ519Command? _currentCommand;
private FanucJ519Command? _lastSentCommand;
// 稠密轨迹执行时预装的命令队列，由机器人状态包节拍逐帧出队。
private Queue<FanucJ519Command>? _commandQueue;
private TaskCompletionSource? _commandQueueDrainedCompletion;
private List<FanucJ519Command>? _commandHistoryForTests;
private FanucJ519Response? _latestResponse;
private long _slowSendCount;
private long _maxReceiveToSendTicks;
private uint _sequenceBufferSize;
// 标记 StartMotion 前是否刚装载过新目标，用于区分新命令和上次运动残留目标。
private bool _hasPendingCommandForStart;
private bool _motionStarted;
private bool _disposed;
/// <summary>
/// 获取当前是否已创建 UDP 套接字。
/// </summary>
public bool IsConnected => _udpClient is not null;
/// <summary>
/// 初始化 FANUC J519 客户端。
/// </summary>
/// <param name="logger">日志记录器；允许 null，供无日志场景使用。</param>
public FanucJ519Client(ILogger<FanucJ519Client>? logger = null)
{
_logger = logger;
}
/// <summary>
/// 建立到 FANUC 控制柜 UDP 60015 运动通道的连接并启动接收循环。
/// </summary>
/// <param name="ip">控制柜 IP 地址。</param>
/// <param name="port">运动通道端口，默认 60015。</param>
/// <param name="cancellationToken">取消令牌。</param>
public async Task ConnectAsync(string ip, int port = 60015, CancellationToken cancellationToken = default)
{
ObjectDisposedException.ThrowIf(_disposed, this);
if (string.IsNullOrWhiteSpace(ip))
{
throw new ArgumentException("IP 不能为空。", nameof(ip));
}
if (_udpClient is not null)
{
throw new InvalidOperationException("J519 通道已经连接，请先 Disconnect。");
}
_logger?.LogInformation("J519 ConnectAsync: {Ip}:{Port}", ip, port);
_udpClient = new UdpClient();
_udpClient.Connect(ip, port);
ConfigureRealtimeSocket(_udpClient.Client);
ConfigureProcessPriority();
// 发送初始化包。
await _udpClient.SendAsync(FanucJ519Protocol.PackInitPacket(), cancellationToken).ConfigureAwait(false);
_logger?.LogInformation("J519 初始化包已发送");
_cts = new CancellationTokenSource();
_receiveThread = new Thread(ReceiveLoop)
{
IsBackground = true,
Name = "J519 UDP realtime loop",
Priority = ThreadPriority.Highest
};
_receiveThread.Start();
}
/// <summary>
/// 启动 J519 命令发送许可；实际发包由机器人状态包节拍驱动。
/// </summary>
public void StartMotion()
{
ObjectDisposedException.ThrowIf(_disposed, this);
if (_udpClient is null)
{
throw new InvalidOperationException("J519 通道未连接。");
}
lock (_commandLock)
{
_lastSentCommand = null;
if (_motionStarted)
{
_logger?.LogDebug("J519 StartMotion: 状态包驱动发送已启用");
return;
}
if (!_hasPendingCommandForStart)
{
_currentCommand = null;
CompleteCommandQueueLocked();
}
_hasPendingCommandForStart = false;
_motionStarted = true;
}
_logger?.LogInformation("J519 StartMotion: 已启用状态包驱动发送");
}
/// <summary>
/// 配置状态包驱动回发时附加到机器人 sequence 的前视缓冲深度。
/// </summary>
/// <param name="bufferSize">要附加到状态序号上的缓冲深度。</param>
public void SetSequenceBufferSize(int bufferSize)
{
ObjectDisposedException.ThrowIf(_disposed, this);
if (bufferSize < 0)
{
throw new ArgumentOutOfRangeException(nameof(bufferSize), "J519 sequence buffer size 不能为负数。");
}
lock (_commandLock)
{
_sequenceBufferSize = (uint)bufferSize;
}
}
/// <summary>
/// 发送状态输出停止包并停止 J519 命令发送。
/// </summary>
public async Task StopMotionAsync(CancellationToken cancellationToken = default)
{
ObjectDisposedException.ThrowIf(_disposed, this);
if (_udpClient is null)
{
return;
}
_logger?.LogInformation("J519 StopMotionAsync: 停止状态包驱动发送");
lock (_commandLock)
{
_motionStarted = false;
_hasPendingCommandForStart = false;
CompleteCommandQueueLocked();
}
// FANUC 手册中 packet type=2 表示停止状态包输出；当前保留现场抓包兼容行为。
await _udpClient.SendAsync(FanucJ519Protocol.PackEndPacket(), cancellationToken).ConfigureAwait(false);
_logger?.LogInformation("J519 StopMotionAsync: 状态输出停止包已发送");
}
/// <summary>
/// 原子更新下一周期要发送的 J519 命令。
/// </summary>
/// <param name="command">新的 J519 命令。</param>
public void UpdateCommand(FanucJ519Command command)
{
ArgumentNullException.ThrowIfNull(command);
ObjectDisposedException.ThrowIf(_disposed, this);
lock (_commandLock)
{
CompleteCommandQueueLocked();
_currentCommand = command;
_hasPendingCommandForStart = true;
_commandHistoryForTests?.Add(command);
}
if (_logger?.IsEnabled(LogLevel.Debug) == true)
{
//_logger.LogDebug(
// "J519 UpdateCommand: joints={Joints}, ioMask={IoMask}, ioValue={IoValue}",
// command.TargetJoints,
// command.WriteIoMask,
// command.WriteIoValue);
}
}
/// <summary>
/// 装载一整段 J519 命令队列；后续每个可接收命令的机器人状态包会自动取出下一帧。
/// </summary>
/// <param name="commands">按执行顺序排列的 J519 命令列表，至少包含一帧。</param>
public void LoadCommandQueue(IReadOnlyList<FanucJ519Command> commands)
{
ArgumentNullException.ThrowIfNull(commands);
ObjectDisposedException.ThrowIf(_disposed, this);
if (commands.Count == 0)
{
throw new ArgumentException("J519 命令队列至少需要包含一帧。", nameof(commands));
}
lock (_commandLock)
{
CompleteCommandQueueLocked();
_commandQueue = new Queue<FanucJ519Command>(commands);
// 队列耗尽后继续保持最后一帧目标，避免运动结束后回落到旧目标或空目标。
_currentCommand = commands[^1];
_hasPendingCommandForStart = true;
_commandQueueDrainedCompletion = new TaskCompletionSource(TaskCreationOptions.RunContinuationsAsynchronously);
_commandHistoryForTests?.AddRange(commands);
}
_logger?.LogInformation("J519 命令队列已装载: count={Count}", commands.Count);
_logger?.LogInformation("开始运动前向机器人发送的sequence={Sequence}",_lastSentCommand?.Sequence ?? 0);
}
/// <summary>
/// 等待当前预装命令队列被状态包全部取出；无队列时立即完成。
/// </summary>
/// <param name="cancellationToken">取消令牌。</param>
/// <returns>表示等待过程的任务。</returns>
internal Task WaitForCommandQueueDrainedAsync(CancellationToken cancellationToken = default)
{
ObjectDisposedException.ThrowIf(_disposed, this);
Task waitTask;
lock (_commandLock)
{
waitTask = _commandQueueDrainedCompletion?.Task ?? Task.CompletedTask;
}
return waitTask.WaitAsync(cancellationToken);
}
/// 判断当前是否没有等待出队的命令；仅供单元测试断言。
/// </summary>
/// <returns>如果队列为空或尚未装载队列，则返回 true。</returns>
internal bool IsCommandQueueDrainedForTests()
{
ObjectDisposedException.ThrowIf(_disposed, this);
lock (_commandLock)
{
return _commandQueue is null || _commandQueue.Count == 0;
}
}
/// <summary>
/// 打开命令历史记录，仅供单元测试验证运行时生成的命令序列。
/// </summary>
internal void EnableCommandHistoryForTests()
{
ObjectDisposedException.ThrowIf(_disposed, this);
lock (_commandLock)
{
_commandHistoryForTests = [];
}
}
/// <summary>
/// 获取测试记录的命令历史。
/// </summary>
/// <returns>命令历史快照。</returns>
internal IReadOnlyList<FanucJ519Command> GetCommandHistoryForTests()
{
ObjectDisposedException.ThrowIf(_disposed, this);
lock (_commandLock)
{
return _commandHistoryForTests?.ToArray() ?? Array.Empty<FanucJ519Command>();
}
}
/// <summary>
/// 获取最近一次通过 UpdateCommand 设置的 J519 命令；供测试断言使用。
/// </summary>
/// <returns>当前 J519 命令或 null。</returns>
internal FanucJ519Command? GetCurrentCommand()
{
ObjectDisposedException.ThrowIf(_disposed, this);
lock (_commandLock)
{
return _currentCommand;
}
}
/// <summary>
/// 获取最近一次解析的 J519 响应；若尚未收到任何响应则返回 null。
/// </summary>
/// <returns>最新 J519 响应或 null。</returns>
public FanucJ519Response? GetLatestResponse()
{
ObjectDisposedException.ThrowIf(_disposed, this);
lock (_responseLock)
{
return _latestResponse;
}
}
/// <summary>
/// 断开 J519 通道并释放资源。
/// </summary>
public void Disconnect()
{
ObjectDisposedException.ThrowIf(_disposed, this);
_cts?.Cancel();
try
{
_udpClient?.Dispose();
_receiveThread?.Join(TimeSpan.FromSeconds(1));
}
catch (ObjectDisposedException)
{
// 忽略释放期间的套接字关闭异常。
}
_receiveThread = null;
_cts?.Dispose();
_cts = null;
_udpClient = null;
lock (_commandLock)
{
_currentCommand = null;
_lastSentCommand = null;
CompleteCommandQueueLocked();
_commandHistoryForTests = null;
_hasPendingCommandForStart = false;
_motionStarted = false;
}
lock (_responseLock)
{
_latestResponse = null;
}
}
/// <summary>
/// 释放客户端资源。
/// </summary>
public void Dispose()
{
if (_disposed)
{
return;
}
_disposed = true;
_cts?.Cancel();
try
{
_udpClient?.Dispose();
_receiveThread?.Join(TimeSpan.FromSeconds(1));
}
catch (ObjectDisposedException)
{
// 忽略释放期间的套接字关闭异常。
}
_cts?.Dispose();
lock (_commandLock)
{
CompleteCommandQueueLocked();
_hasPendingCommandForStart = false;
}
}
private static void ConfigureProcessPriority()
{
try
{
var process = Process.GetCurrentProcess();
if (process.PriorityClass < ProcessPriorityClass.High)
{
process.PriorityClass = ProcessPriorityClass.High;
}
}
catch (Exception)
{
// 某些部署环境不允许提升进程优先级；实时链路仍按普通优先级运行。
}
}
/// <summary>
/// 配置 J519 UDP 套接字的低延迟参数。
/// </summary>
/// <param name="socket">已连接 FANUC 60015 的 UDP 套接字。</param>
private void ConfigureRealtimeSocket(Socket socket)
{
socket.ReceiveBufferSize = 1024 * 1024;
socket.SendBufferSize = 1024 * 1024;
try
{
// DSCP EF(46) 标记低延迟流量，是否生效取决于现场网卡、交换机和控制柜网络策略。
socket.SetSocketOption(SocketOptionLevel.IP, SocketOptionName.TypeOfService, 0xB8);
}
catch (SocketException ex)
{
_logger?.LogWarning(ex, "J519 UDP 套接字无法设置 DSCP EF 优先级标记");
}
}
/// <summary>
/// 后台接收循环：在专用高优先级线程中同步接收 132B 响应并立即回发命令。
/// </summary>
private void ReceiveLoop()
{
var udpClient = _udpClient;
var cancellationToken = _cts?.Token ?? CancellationToken.None;
if (udpClient is null)
{
return;
}
_logger?.LogInformation("J519 ReceiveLoop 启动");
long receiveCount = 0;
FanucJ519Response? lastLoggedResponse = null;
var receiveBuffer = new byte[FanucJ519Protocol.ResponsePacketLength];
var commandBuffer = new byte[FanucJ519Protocol.CommandPacketLength];
try
{
while (!cancellationToken.IsCancellationRequested)
{
var received = udpClient.Client.Receive(receiveBuffer);
if (received == FanucJ519Protocol.ResponsePacketLength)
{
var receiveTicks = Stopwatch.GetTimestamp();
var response = FanucJ519Protocol.ParseResponse(receiveBuffer);
// 先按状态包节拍回发命令，再做低频日志处理，减少受信周期内的非必要工作。
if (response.AcceptsCommand)
{
SendCommandForStatus(udpClient.Client, response, commandBuffer, receiveTicks);
}
lock (_responseLock)
{
_latestResponse = response;
}
receiveCount++;
// 仅在状态变化时记录 Info，避免高频日志。
if (lastLoggedResponse is null
|| lastLoggedResponse.Status != response.Status
|| lastLoggedResponse.RobotInMotion != response.RobotInMotion
|| lastLoggedResponse.SystemReady != response.SystemReady
|| lastLoggedResponse.AcceptsCommand != response.AcceptsCommand)
{
_logger?.LogInformation(
"J519 响应: status=0x{Status:X2}, seq={Seq}, accept={Accept}, received={received}, SystemReady={SystemReady}, RobotInMotion={RobotInMotion}, pose=[{Pose}], joints=[{Joints}]",
response.Status,
response.Sequence,
response.AcceptsCommand,
response.ReceivedCommand,
response.SystemReady,
response.RobotInMotion,
string.Join(", ", response.Pose.Select(v => v.ToString("F3"))),
string.Join(", ", response.JointDegrees.Take(6).Select(v => v.ToString("F3"))));
var lastSentTargetJoints = GetLastSentTargetJointsLogText();
_logger?.LogInformation("J519 最后一条发送目标关节轴: joints=[{Joints}]", lastSentTargetJoints);
lastLoggedResponse = response;
// 如果状态从AcceptsCommand true 变为false,说明机器人报错,清空队列
if (!response.AcceptsCommand)
{
lock (_commandLock)
{
_currentCommand = null;
CompleteCommandQueueLocked();
}
_logger?.LogWarning("J519 接收状态包显示机器人不可接受命令，已清空命令队列");
}
}
else if (receiveCount % 1000 == 0)
{
var maxReceiveToSendMs = Stopwatch.GetElapsedTime(0, Interlocked.Read(ref _maxReceiveToSendTicks)).TotalMilliseconds;
_logger?.LogDebug(
"J519 已接收 {Count} 个响应包，receive-to-send 最大耗时约 {MaxMs:F3}ms，超过 0.5ms 次数 {SlowCount}",
receiveCount,
maxReceiveToSendMs,
Interlocked.Read(ref _slowSendCount));
}
}
}
}
catch (OperationCanceledException)
{
_logger?.LogInformation("J519 ReceiveLoop 正常取消，共接收 {Count} 个包", receiveCount);
}
catch (ObjectDisposedException)
{
_logger?.LogInformation("J519 ReceiveLoop 因 UDP 释放退出，共接收 {Count} 个包", receiveCount);
}
catch (SocketException ex) when (cancellationToken.IsCancellationRequested)
{
_logger?.LogInformation(ex, "J519 ReceiveLoop 因取消关闭套接字退出，共接收 {Count} 个包", receiveCount);
}
}
/// <summary>
/// 按机器人状态包的 sequence 立即回发当前 J519 命令。
/// </summary>
/// <param name="socket">已连接的 UDP 套接字。</param>
/// <param name="response">刚收到的机器人状态包。</param>
/// <param name="commandBuffer">可复用的 64B 命令包缓冲区。</param>
/// <param name="receiveTicks">收到状态包后的时间戳。</param>
private void SendCommandForStatus(Socket socket, FanucJ519Response response, byte[] commandBuffer, long receiveTicks)
{
FanucJ519Command? command;
var willDrainQueue = false;
lock (_commandLock)
{
if (!_motionStarted)
{
command = null;
}
else if (_commandQueue is { Count: > 0 } queue)
{
// 状态包是唯一节拍源：每收到一帧可接收状态，才取出下一条目标。
var queuedCommand = queue.Dequeue();
_currentCommand = queuedCommand;
willDrainQueue = queue.Count == 0;
command = queuedCommand;
}
else
{
command = _currentCommand;
}
}
if (command is null)
{
command = TryBuildHoldCommandFromLatestResponse(response);
}
if (command is null)
{
return;
}
uint sequenceToSend;
lock (_commandLock)
{
sequenceToSend = response.Sequence + _sequenceBufferSize;
}
FanucJ519Protocol.PackCommandPacket(command, sequenceToSend, commandBuffer);
socket.Send(commandBuffer);
TrackReceiveToSendLatency(receiveTicks);
// _logger?.LogDebug("J519 已回发命令包，seq={Seq}", sequence);
// _logger?.LogDebug(
// "J519 回发命令详情: joints={Joints}, ioMask={IoMask}, ioValue={IoValue}",
// command.TargetJoints,
// command.WriteIoMask,
// command.WriteIoValue);
lock (_commandLock)
{
_lastSentCommand = command;
if (willDrainQueue && _commandQueue is { Count: 0 })
{
CompleteCommandQueueLocked();
}
}
}
/// <summary>
/// 当当前没有显式目标时，使用最近一帧状态反馈关节角构造保姿命令，维持机器人当前位置。
/// </summary>
/// <param name="response">当前收到的机器人状态包。</param>
/// <returns>可用于保姿的临时 J519 命令；若反馈关节不足则返回 null。</returns>
private static FanucJ519Command? TryBuildHoldCommandFromLatestResponse(FanucJ519Response response)
{
if (response.JointDegrees.Count < 6)
{
return null;
}
// 无运动目标时，持续回发机器人当前反馈关节，保持伺服流与机器人当前位置一致。
return new FanucJ519Command(
sequence: response.Sequence,
targetJoints:
[
response.JointDegrees[0],
response.JointDegrees[1],
response.JointDegrees[2],
response.JointDegrees[3],
response.JointDegrees[4],
response.JointDegrees[5]
]);
}
/// <summary>
/// 记录状态包到命令包发出的最大耗时和慢发送次数，供低频诊断日志观察调度抖动。
/// </summary>
/// <param name="receiveTicks">收到状态包后的时间戳。</param>
private void TrackReceiveToSendLatency(long receiveTicks)
{
var elapsedTicks = Stopwatch.GetTimestamp() - receiveTicks;
var currentMax = Interlocked.Read(ref _maxReceiveToSendTicks);
while (elapsedTicks > currentMax
&& Interlocked.CompareExchange(ref _maxReceiveToSendTicks, elapsedTicks, currentMax) != currentMax)
{
currentMax = Interlocked.Read(ref _maxReceiveToSendTicks);
}
if (Stopwatch.GetElapsedTime(0, elapsedTicks) > TimeSpan.FromMilliseconds(0.5))
{
Interlocked.Increment(ref _slowSendCount);
}
}
/// <summary>
/// 清空当前命令队列，并唤醒等待队列结束的运行时任务。
/// </summary>
private void CompleteCommandQueueLocked()
{
_commandQueue?.Clear();
_commandQueue = null;
_commandQueueDrainedCompletion?.TrySetResult();
_commandQueueDrainedCompletion = null;
}
/// <summary>
/// 读取最近一次已成功发送命令的目标关节轴文本，便于状态日志直接对照控制目标。
/// </summary>
/// <returns>格式化后的目标关节轴文本；如果尚未发送命令则返回占位符。</returns>
private string GetLastSentTargetJointsLogText()
{
lock (_commandLock)
{
return _lastSentCommand is null
? "n/a"
: string.Join(", ", _lastSentCommand.TargetJoints.Take(6).Select(v => v.ToString("F5")));
}
}
}