yunxiao.zhu
e0162b72b3
* 为 FANUC 命令、状态和 J519 客户端注入独立日志记录器 * 补充不可接收命令状态下队列完成等待的回归测试 * 调整 J519 命令流结束后的状态处理语义
694 lines
25 KiB
C#
694 lines
25 KiB
C#
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;
|
||
/// <summary>
|
||
/// 指示预装队列发出 LastData 结束帧后是否进入仅监控状态，而不是继续发送保姿命令。
|
||
/// </summary>
|
||
private readonly bool _endCommandStreamAfterQueueDrained;
|
||
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;
|
||
/// <summary>
|
||
/// 在 StartMotion 前是否有待发送的新命令；如果为 true 则 StartMotion 不会清空当前目标，而是继续发送新目标。
|
||
/// </summary>
|
||
private bool _hasPendingCommandForStart;
|
||
/// <summary>
|
||
/// 标记命令流已通过 LastData 正常结束；该状态下仅接收状态包，不继续回发 hold 命令。
|
||
/// </summary>
|
||
private bool _monitorOnlyAfterLastData;
|
||
private bool _motionStarted;
|
||
private bool _disposed;
|
||
|
||
/// <summary>
|
||
/// 获取当前是否已创建 UDP 套接字。
|
||
/// </summary>
|
||
public bool IsConnected => _udpClient is not null;
|
||
|
||
/// <summary>
|
||
/// 获取正常队列结束时是否通过 LastData 结束命令流并保留状态监控。
|
||
/// </summary>
|
||
internal bool EndCommandStreamAfterQueueDrained => _endCommandStreamAfterQueueDrained;
|
||
|
||
/// <summary>
|
||
/// 初始化 FANUC J519 客户端。
|
||
/// </summary>
|
||
/// <param name="logger">日志记录器；允许 null，供无日志场景使用。</param>
|
||
/// <param name="endCommandStreamAfterQueueDrained">队列结束帧发出后是否停止命令流并保留状态监控。</param>
|
||
public FanucJ519Client(
|
||
ILogger<FanucJ519Client>? logger = null,
|
||
bool endCommandStreamAfterQueueDrained = true)
|
||
{
|
||
_logger = logger;
|
||
_endCommandStreamAfterQueueDrained = endCommandStreamAfterQueueDrained;
|
||
}
|
||
|
||
/// <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;
|
||
_monitorOnlyAfterLastData = 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;
|
||
_monitorOnlyAfterLastData = 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;
|
||
_monitorOnlyAfterLastData = false;
|
||
_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;
|
||
_monitorOnlyAfterLastData = false;
|
||
_motionStarted = 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;
|
||
_monitorOnlyAfterLastData = 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;
|
||
_monitorOnlyAfterLastData = 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;
|
||
var suppressHoldCommand = false;
|
||
lock (_commandLock)
|
||
{
|
||
if (_monitorOnlyAfterLastData)
|
||
{
|
||
command = null;
|
||
suppressHoldCommand = true;
|
||
}
|
||
else 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 && !suppressHoldCommand)
|
||
{
|
||
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 (_endCommandStreamAfterQueueDrained && command.LastData == 1)
|
||
{
|
||
// LastData 是命令流的正常结束语义；结束后保留状态接收，但不继续回发保姿包。
|
||
// _monitorOnlyAfterLastData = true;
|
||
// _motionStarted = false;
|
||
CompleteCommandQueueLocked();
|
||
}
|
||
else 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")));
|
||
}
|
||
}
|
||
}
|