From 783716ff44c4cc6e3bc21e34b19b62e9a555f058 Mon Sep 17 00:00:00 2001 From: "yunxiao.zhu" Date: Wed, 6 May 2026 12:57:56 +0800 Subject: [PATCH] =?UTF-8?q?=E2=9C=A8=20feat(fanuc):=20=E6=94=B9=E4=B8=BA?= =?UTF-8?q?=E6=8C=89=E7=8A=B6=E6=80=81=E5=8C=85=E9=A9=B1=E5=8A=A8=20J519?= =?UTF-8?q?=20=E9=98=9F=E5=88=97=E5=8F=91=E9=80=81?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * 预生成稠密轨迹 J519 命令队列，等待机器人状态包逐帧出队 * 让 ExecuteTrajectory 在队列实际取完后返回，避免后台发送提前结束 * 新增 ActualSendTiming.txt，区分实发时间与 speed_ratio 采样时间 * 补充 J519 队列、等待完成和实发时间映射相关单元测试 * 同步文档中的 t_send / t_traj / speed_ratio 说明 Co-authored-by: Copilot --- .gitignore | 1 + AGENTS.md | 2 +- CLAUDE.md | 2 +- README.md | 2 +- ...r-client-api-compatibility-requirements.md | 2 +- ...ntroller-client-api-reverse-engineering.md | 2 +- docs/fanuc-field-runtime-workflow.md | 5 +- docs/fanuc-socket-implementation-plan.md | 6 +- docs/fanuc-stream-motion-manual-notes.md | 2 +- docs/move-joint-jerk-comparison-20260505.md | 4 +- ...lanning-duration-mismatch-investigation.md | 3 +- .../FanucControllerRuntime.cs | 170 ++++++++++-------- .../Protocol/FanucJ519Client.cs | 129 ++++++++++++- .../FanucControllerRuntimeDenseTests.cs | 123 ++++++++++++- .../FanucJ519ClientTests.cs | 42 ++++- 15 files changed, 403 insertions(+), 92 deletions(-) diff --git a/.gitignore b/.gitignore index 5555018..8224f3a 100644 --- a/.gitignore +++ b/.gitignore @@ -397,3 +397,4 @@ FodyWeavers.xsd # JetBrains Rider *.sln.iml Config/Data/* +.dotnet-home/* diff --git a/AGENTS.md b/AGENTS.md index 7cd1852..e6ca917 100644 --- a/AGENTS.md +++ b/AGENTS.md @@ -187,7 +187,7 @@ flyshot-replacement/ - `Flyshot.Runtime.Fanuc` 已固化 `10010 / 10012 / 60015` 基础协议帧编解码，`10010` 状态帧以 `j519 协议.pcap` 和 `Rvbust/uttc-20260428/20260428.pcap` 真机抓包确认为 90B。 - `Flyshot.Runtime.Fanuc` 已将 TCP 10010 的 `pose[6]`、`joint[6]`、`external_axes[3]` 和 `raw_tail_words[4]` 映射为明确状态帧字段，并在状态快照中保留尾部状态字诊断信息。 - `Rvbust/uttc-20260428` 抓包确认 J519 命令目标为关节角 `deg`，而导出 `JointDetialTraj.txt` 为 `rad`；执行链路必须做单位转换。 -- `Rvbust/uttc-20260428` 抓包确认 `speed_ratio=0.7` 体现为 UDP 下发时间轴约 `1.427730x` 拉伸；本抓包机器人侧 `TCP 10012` 未出现 `0x2207 SetSpeedRatio`，不要把速度缩放只建模成单个机器人命令。实发按 `t_traj = k * 0.008 * speed_ratio` 重采样，`UTTC_MS11` 的 `464` 行导出轨迹对应 `1322` 个主运行 J519 包。 +- `Rvbust/uttc-20260428` 抓包确认 `speed_ratio=0.7` 体现为 UDP 下发时间轴约 `1.427730x` 拉伸；本抓包机器人侧 `TCP 10012` 未出现 `0x2207 SetSpeedRatio`，不要把速度缩放只建模成单个机器人命令。J519 实发周期仍为 `t_send = k * 0.008`，原轨迹采样时间为 `t_traj = t_send * speed_ratio`，`UTTC_MS11` 的 `464` 行导出轨迹对应 `1322` 个主运行 J519 包。 - `Rvbust/uttc-20260428` 抓包确认 `UTTC_MS11` 的 17 个 `shot_flags=true` 对应 17 个 UDP IO 脉冲，`io_keep_cycles=2` 对应约两周期清零。 - `Flyshot.Runtime.Fanuc` 已具备基础 Socket 客户端、速度倍率/TCP/IO 参数命令和 J519 周期发送链路；稠密轨迹下发已按 `speed_ratio` 推进轨迹时间，J519 闭环状态判断与现场联调仍需补齐。 - `ExecuteTrajectory` / `ExecuteFlyShotTraj` 已接入 `Planning + Triggering + Runtime`，不再只是兼容层内存赋值。 diff --git a/CLAUDE.md b/CLAUDE.md index a2202d4..3c5cacf 100644 --- a/CLAUDE.md +++ b/CLAUDE.md @@ -153,5 +153,5 @@ flyshot-replacement/ - 解决方案构建已通过。 - `10010` 状态帧以 `j519 协议.pcap` 和 `Rvbust/uttc-20260428/20260428.pcap` 真机抓包确认为 90B。 - `Rvbust/uttc-20260428` 抓包确认 J519 命令目标为关节角 `deg`，而导出 `JointDetialTraj.txt` 为 `rad`；执行链路必须做单位转换。 -- `Rvbust/uttc-20260428` 抓包确认 `speed_ratio=0.7` 体现为 UDP 下发时间轴约 `1.427730x` 拉伸；本抓包机器人侧 `TCP 10012` 未出现 `0x2207 SetSpeedRatio`。实发按 `t_traj = k * 0.008 * speed_ratio` 重采样，`UTTC_MS11` 的 `464` 行导出轨迹对应 `1322` 个主运行 J519 包。 +- `Rvbust/uttc-20260428` 抓包确认 `speed_ratio=0.7` 体现为 UDP 下发时间轴约 `1.427730x` 拉伸；本抓包机器人侧 `TCP 10012` 未出现 `0x2207 SetSpeedRatio`。J519 实发周期仍为 `t_send = k * 0.008`，原轨迹采样时间为 `t_traj = t_send * speed_ratio`，`UTTC_MS11` 的 `464` 行导出轨迹对应 `1322` 个主运行 J519 包。 - `Rvbust/uttc-20260428` 抓包确认 `UTTC_MS11` 的 17 个 `shot_flags=true` 对应 17 个 UDP IO 脉冲，`io_keep_cycles=2` 对应约两周期清零。 diff --git a/README.md b/README.md index 286274c..3182a13 100644 --- a/README.md +++ b/README.md @@ -103,7 +103,7 @@ 5. FANUC UDP 60015 J519 运动链路 - [x] 重新确认 J519 发送节拍与 `FanucControllerRuntime` 稠密轨迹循环的职责边界：`FanucJ519Client` 收到机器人 UDP status 后按该 status sequence 回发命令，`FanucControllerRuntime` 只按轨迹时间更新下一帧命令内容。 - - [x] 执行时将规划输出 `rad` 转为 J519 `deg` 目标，并按当前 `speed_ratio` 调整 8ms 发送索引/时间尺度：第 `k` 个 J519 目标采样 `t_traj = k * 0.008 * speed_ratio`，包数为 `floor(duration / (0.008 * speed_ratio)) + 1`。 + - [x] 执行时将规划输出 `rad` 转为 J519 `deg` 目标，并按当前 `speed_ratio` 调整原轨迹采样时间尺度：第 `k` 个 J519 目标的实发时间为 `t_send = k * 0.008`，采样时间为 `t_traj = t_send * speed_ratio`，包数为 `floor(duration / (0.008 * speed_ratio)) + 1`。 - [x] 补齐 `accept_cmd`、`received_cmd`、`sysrdy`、`rbt_inmotion` 状态位解析与启动前闭环检查；若已有 J519 响应且 `accept_cmd/sysrdy` 未就绪，则拒绝稠密轨迹执行。 - [x] 校验序号递增、状态包 sequence 校准、响应滞后、丢包、停止包和最后一帧语义：UTTC golden tests 覆盖连续 seq、无重复 seq、响应滞后 2 到 8 帧、`lastData=0`；J519 客户端测试覆盖收到 status 后按 status sequence 回发命令和 type 2 状态输出停止包。 - [x] 将飞拍 IO 触发的 `write_io_type/index/mask/value` 与现场控制柜实际 IO 地址逐项对齐；UTTC golden tests 确认 17 个触发点对应 17 个 UDP IO set 脉冲、17 个 clear 帧，mask 集合为 `10/12/14`。 diff --git a/docs/controller-client-api-compatibility-requirements.md b/docs/controller-client-api-compatibility-requirements.md index 74a6ad8..9078c23 100644 --- a/docs/controller-client-api-compatibility-requirements.md +++ b/docs/controller-client-api-compatibility-requirements.md @@ -137,7 +137,7 @@ - `method="icsp"` 与 `method="self-adapt-icsp"` 已接入当前规划器；`method="doubles"` 会被识别但返回显式未实现，不会静默降级成 ICSP。 - `Flyshot.Runtime.Fanuc.Protocol` 已经固化 `10010` 状态帧、`10012` 命令帧和 `60015` J519 数据包的基础编解码，并使用逆向抓包样本覆盖最小测试；`10010` 当前现场确认固定 90B。 - `Flyshot.Runtime.Fanuc` 已具备基础 Socket 客户端、程序启停、速度倍率/TCP/IO 参数命令和 J519 状态包驱动发送链路；稠密轨迹下发已按 `speed_ratio` 推进轨迹时间，并在收到机器人 UDP status 后按该 status sequence 回发命令。真实 R30iB 全流程现场联调仍需执行。 -- 2026-04-28 `UTTC_MS11` 抓包确认 J519 命令目标为 `deg`、导出 `JointDetialTraj.txt` 为 `rad`，`speed_ratio=0.5/0.7/1.0` 分别形成 `1851/1322/926` 个主运行 J519 包；实际执行不发送 464 行导出点，而是按 `floor(duration / (0.008 * speed_ratio)) + 1` 形成 J519 运行包。 +- 2026-04-28 `UTTC_MS11` 抓包确认 J519 命令目标为 `deg`、导出 `JointDetialTraj.txt` 为 `rad`，`speed_ratio=0.5/0.7/1.0` 分别形成 `1851/1322/926` 个主运行 J519 包；实际执行不发送 464 行导出点，而是在 8ms 实发周期上按 `t_traj = t_send * speed_ratio` 采样，包数为 `floor(duration / (0.008 * speed_ratio)) + 1`。 - 宿主已经提供只读 Web 状态页 `/status` 和状态快照 API `/api/status/snapshot`，用于查看兼容层初始化、机器人元数据和运行时快照。 - `MoveJoint` 仍保持旧兼容语义中的直接运动接口，但状态写入已经统一经过运行时，而不是由兼容服务自己维护关节数组。 - `GetNearestIK`、`SetUpRobotFromEnv` 当前已经暴露完整参数形状，但后端求解器 / 环境文件解析仍返回显式未实现。 diff --git a/docs/controller-client-api-reverse-engineering.md b/docs/controller-client-api-reverse-engineering.md index 52a842d..fffaf64 100644 --- a/docs/controller-client-api-reverse-engineering.md +++ b/docs/controller-client-api-reverse-engineering.md @@ -515,7 +515,7 @@ UploadFlyShotTraj(name, waypoints, shot_flags, offset_values, addrs) - `SetSpeedRatio`：`MsgID = 0x2207` - `GetIO`：`MsgID = 0x2208` - `SetIO`：`MsgID = 0x2209` -- 2026-04-28 `UTTC_MS11` 抓包中，`speed_ratio=0.7` 的效果能从 UDP 60015 主运行段时间尺度反推出来，但机器人侧 `TCP 10012` 未出现 `0x2207 SetSpeedRatio`；兼容实现不能只依赖一次 10012 命令来表达执行倍率，还要在 J519 发送时间轴上应用当前倍率。实发规则为 `t_traj = k * 0.008 * speed_ratio`，包数为 `floor(duration / (0.008 * speed_ratio)) + 1`。 +- 2026-04-28 `UTTC_MS11` 抓包中，`speed_ratio=0.7` 的效果能从 UDP 60015 主运行段时间尺度反推出来，但机器人侧 `TCP 10012` 未出现 `0x2207 SetSpeedRatio`；兼容实现不能只依赖一次 10012 命令来表达执行倍率，还要在 J519 发送时间轴上应用当前倍率。J519 实发时间为 `t_send = k * 0.008`，原轨迹采样时间为 `t_traj = t_send * speed_ratio`，包数为 `floor(duration / (0.008 * speed_ratio)) + 1`。 - 飞拍轨迹相关额外字符串线索： - `StartUploadFlyShotTraj` - `EndUploadFlyShotTraj` diff --git a/docs/fanuc-field-runtime-workflow.md b/docs/fanuc-field-runtime-workflow.md index 908b48b..77ce683 100644 --- a/docs/fanuc-field-runtime-workflow.md +++ b/docs/fanuc-field-runtime-workflow.md @@ -53,10 +53,11 @@ POST /set_speedRatio/ { "speed": 0.7 } ``` -真机模式下会通过 `TCP 10012` 下发 `0x2207 SetSpeedRatio`，同时运行时保存当前倍率。`speed_ratio` 是执行期倍率，不参与 `IsFlyShotTrajValid` / `SaveTrajInfo` / `ExecuteFlyShotTraj(save_traj=true)` 的规划时长计算。J519 执行时仍必须按该倍率重采样轨迹时间轴： +真机模式下会通过 `TCP 10012` 下发 `0x2207 SetSpeedRatio`，同时运行时保存当前倍率。`speed_ratio` 是执行期倍率，不参与 `IsFlyShotTrajValid` / `SaveTrajInfo` / `ExecuteFlyShotTraj(save_traj=true)` 的规划时长计算。J519 执行时仍按机器人 8ms 节拍更新目标，`speed_ratio` 只缩放原轨迹采样时间： ```text -t_traj = k * 0.008 * speed_ratio +t_send = k * 0.008 +t_traj = t_send * speed_ratio send_count = floor(duration / (0.008 * speed_ratio)) + 1 ``` diff --git a/docs/fanuc-socket-implementation-plan.md b/docs/fanuc-socket-implementation-plan.md index 7234371..7654301 100644 --- a/docs/fanuc-socket-implementation-plan.md +++ b/docs/fanuc-socket-implementation-plan.md @@ -120,8 +120,8 @@ FanucCommandProtocol / FanucStateProtocol / FanucJ519Protocol (已有，不改 执行注意事项： - 规划层输出关节角为 `rad`，J519 命令 `target[0..5]` 必须转为 `deg`。 -- 发送循环不能只按 `JointDetialTraj` 行号逐行发；需要按当前 `speed_ratio` 对轨迹时间轴做缩放，再采样到约 8ms 的 J519 周期。 -- 实发规则：第 `k` 个 J519 周期采样 `t_traj = k * 0.008 * speed_ratio`，命令包数为 `floor(duration / (0.008 * speed_ratio)) + 1`。`UTTC_MS11` 中 `7.403046 / (0.008 * 0.7) = 1321.9725`，因此主运行实发 `1322` 个运行包，而不是 `JointDetialTraj.txt` 的 `464` 行。 +- 发送循环不能只按 `JointDetialTraj` 行号逐行发；需要保持约 8ms 的 J519 实发周期，同时按当前 `speed_ratio` 对原轨迹时间轴做缩放。 +- 实发规则：第 `k` 个 J519 周期的发送时间为 `t_send = k * 0.008`，轨迹采样时间为 `t_traj = t_send * speed_ratio`，命令包数为 `floor(duration / (0.008 * speed_ratio)) + 1`。`UTTC_MS11` 中 `7.403046 / (0.008 * 0.7) = 1321.9725`，因此主运行实发 `1322` 个运行包，而不是 `JointDetialTraj.txt` 的 `464` 行。 - 飞拍 IO 事件应嵌入 `write_io_type/index/mask/value`，不要用独立 `TCP 10012 SetIO` 模拟拍照触发。 - 响应 `joints_deg` 相对命令目标存在约 7 帧 / 56ms 滞后，闭环判断要容忍该延迟。 @@ -139,7 +139,7 @@ FanucCommandProtocol / FanucStateProtocol / FanucJ519Protocol (已有，不改 - `EnableRobot(bufferSize)` — 走完整 StartProg 序列（Stop→Reset→Status→Start RVBUSTSM），然后启动 J519 - `DisableRobot()` — 停止 J519，发送 StopProg - `Disconnect()` — 断开三条通道 -- `ExecuteTrajectory(result, finalJointPositions)` — 将规划后的稠密路点经 `rad -> deg` 转换，并按 `t_traj = k * 0.008 * speed_ratio` 重采样后，通过 J519 逐周期发送 +- `ExecuteTrajectory(result, finalJointPositions)` — 将规划后的稠密路点经 `rad -> deg` 转换，并按 `t_send = k * 0.008`、`t_traj = t_send * speed_ratio` 重采样后，通过 J519 逐周期发送 - `StopMove()` — 立即停止 J519 发送循环 - `GetSnapshot()` — 优先从 `FanucStateClient` 读取最新状态；若状态通道未连接，回退到内存值 - `GetJointPositions()` / `GetPose()` / `GetTcp()` / `GetSpeedRatio()` / `GetIo()` — 优先从真实通道读取 diff --git a/docs/fanuc-stream-motion-manual-notes.md b/docs/fanuc-stream-motion-manual-notes.md index 6fa01fa..80df8ab 100644 --- a/docs/fanuc-stream-motion-manual-notes.md +++ b/docs/fanuc-stream-motion-manual-notes.md @@ -136,7 +136,7 @@ | --- | --- | | 关节格式下发 | 已实现，当前现场链路默认只使用关节格式 | | `rad -> deg` | 已实现，并由 UTTC J519 golden tests 覆盖 | -| `speed_ratio` 下发时间轴缩放 | 已实现，规则为 `t_traj = k * 0.008 * speed_ratio` | +| `speed_ratio` 下发时间轴缩放 | 已实现，J519 实发时间为 `t_send = k * 0.008`，原轨迹采样时间为 `t_traj = t_send * speed_ratio` | | IO 触发嵌入 J519 命令包 | 已实现，使用 `write_io_type/index/mask/value` | | 速度、加速度、jerk 约束 | 规划层有 `acc_limit / jerk_limit` 等兼容参数，但未从 FANUC `$STMO_GRP` 在线读取，也未实现手册附录中的 20 档速度/负载插值 | | J3 轴定义 | 当前文档未见专门处理；需要确认 `.robot` 模型与现场导出轨迹是否已经采用 FANUC J3 定义 | diff --git a/docs/move-joint-jerk-comparison-20260505.md b/docs/move-joint-jerk-comparison-20260505.md index 5f9f0fa..61a6eaa 100644 --- a/docs/move-joint-jerk-comparison-20260505.md +++ b/docs/move-joint-jerk-comparison-20260505.md @@ -48,13 +48,13 @@ ## 3. ActualSendJerkStats 的单位边界 -`ActualSendJerkStats.txt` 的代码注释写的是 `rad/s^3`，但当前实现里真实输入不是弧度，而是角度： +`ActualSendJerkStats.txt` 的真实输入不是弧度，而是 J519 下发用的角度制关节目标： 1. `SampleDenseJointTrajectoryDegrees(...)` 先把轨迹点从 `rad` 转成 `deg` 2. `BuildDenseSendJointRow(...)` 把这组角度制关节写入 `ActualSendJointTraj.txt` 3. `BuildDenseSendJerkRow(...)` 再直接基于这组角度制关节做三阶差分 -因此当前这份 `ActualSendJerkStats.txt` 的逐轴跃度应按以下方式理解： +2026-05-06 之后，`ActualSendJointTraj.txt` 第一列和 `ActualSendJerkStats.txt` 的 `dt` 都使用 J519 实发时间；若需要查看被 `speed_ratio` 缩放后的原轨迹采样时间，应读取同目录的 `ActualSendTiming.txt`。因此当前这份 `ActualSendJerkStats.txt` 的逐轴跃度应按以下方式理解： - 文本中的数值口径：`deg/s^3` - 若要与 `.robot` / `RobotProfile` 中的 jerk limit 比较，需要先换算为 `rad/s^3` diff --git a/docs/planning-duration-mismatch-investigation.md b/docs/planning-duration-mismatch-investigation.md index 53a1ec3..674c5c2 100644 --- a/docs/planning-duration-mismatch-investigation.md +++ b/docs/planning-duration-mismatch-investigation.md @@ -576,7 +576,8 @@ eol9_eau_90.json SHA256=7F854AA227D842CAE734AFA378FEEFA742D797F99FBE53 它不等同于运行时 `/set_speedRatio/`，也不改变 J519 的 8ms 发送周期。运行阶段仍按： ```text -t_traj = k * 0.008 * speed_ratio +t_send = k * 0.008 +t_traj = t_send * speed_ratio ``` 从已生成轨迹中重采样。 diff --git a/src/Flyshot.Runtime.Fanuc/FanucControllerRuntime.cs b/src/Flyshot.Runtime.Fanuc/FanucControllerRuntime.cs index 0e3353d..570f10f 100644 --- a/src/Flyshot.Runtime.Fanuc/FanucControllerRuntime.cs +++ b/src/Flyshot.Runtime.Fanuc/FanucControllerRuntime.cs @@ -34,7 +34,6 @@ public sealed class FanucControllerRuntime : IControllerRuntime, IDisposable private double[] _pose = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0]; private bool _disposed; private CancellationTokenSource? _sendCts; - private Task? _sendTask; /// /// 初始化 FANUC 控制器运行时。 @@ -271,7 +270,7 @@ public sealed class FanucControllerRuntime : IControllerRuntime, IDisposable var clampedRatio = Math.Clamp(ratio, 0.0, 1.0); if (!IsSimulationMode) { - _commandClient.SetSpeedRatioAsync(clampedRatio).GetAwaiter().GetResult(); + //_commandClient.SetSpeedRatioAsync(clampedRatio).GetAwaiter().GetResult(); } _speedRatio = clampedRatio; @@ -467,6 +466,9 @@ public sealed class FanucControllerRuntime : IControllerRuntime, IDisposable { ArgumentNullException.ThrowIfNull(result); ArgumentNullException.ThrowIfNull(finalJointPositions); + CancellationToken denseSendCancellationToken = default; + CancellationTokenSource? denseSendCancellationSource = null; + var shouldRunDenseTrajectory = false; _logger?.LogInformation( "ExecuteTrajectory 开始: program={ProgramName}, method={Method}, 时长={Duration}s, 稠密采样={HasDense}, 触发事件数={TriggerCount}, speedRatio={SpeedRatio}", @@ -489,29 +491,52 @@ public sealed class FanucControllerRuntime : IControllerRuntime, IDisposable EnsureJ519ReadyForDenseExecution(); - // 真机模式且存在稠密路点：启动后台高精度发送任务。 + // 真机模式且存在稠密路点：准备可被 StopMove 取消的同步发送任务。 _isInMotion = true; _sendCts = new CancellationTokenSource(); - var ct = _sendCts.Token; + denseSendCancellationSource = _sendCts; + denseSendCancellationToken = _sendCts.Token; + shouldRunDenseTrajectory = true; - _sendTask = Task.Run(() => SendDenseTrajectory(result, finalJointPositions, ct), ct); - _logger?.LogInformation("ExecuteTrajectory 已启动后台稠密发送任务"); - return; + _logger?.LogInformation("ExecuteTrajectory 开始同步稠密发送任务"); } - - if (!IsSimulationMode) + else { - // 真机模式无稠密路点：回退到单点收敛。 - var command = new FanucJ519Command( - sequence: 0, - targetJoints: finalJointPositions.Select(j => (double)j).ToArray()); - _j519Client.UpdateCommand(command); - } + if (!IsSimulationMode) + { + // 真机模式无稠密路点：回退到单点收敛。 + var command = new FanucJ519Command( + sequence: 0, + targetJoints: finalJointPositions.Select(j => (double)j).ToArray()); + _j519Client.UpdateCommand(command); + } - _isInMotion = true; - _jointPositions = finalJointPositions.ToArray(); - _isInMotion = false; - _logger?.LogInformation("ExecuteTrajectory 完成(单点模式)"); + _isInMotion = true; + _jointPositions = finalJointPositions.ToArray(); + _isInMotion = false; + _logger?.LogInformation("ExecuteTrajectory 完成(单点模式)"); + } + } + + if (shouldRunDenseTrajectory) + { + try + { + // 稠密轨迹必须等 J519 队列被机器人状态包实际取完后再向上层返回。 + SendDenseTrajectory(result, finalJointPositions, denseSendCancellationToken); + _logger?.LogInformation("ExecuteTrajectory 完成(稠密模式)"); + } + finally + { + lock (_stateLock) + { + if (ReferenceEquals(_sendCts, denseSendCancellationSource)) + { + denseSendCancellationSource?.Dispose(); + _sendCts = null; + } + } + } } } @@ -534,7 +559,7 @@ public sealed class FanucControllerRuntime : IControllerRuntime, IDisposable } /// - /// 后台高精度发送任务：按 J519 周期发送命令，并按 speed_ratio 推进原始轨迹时间。 + /// 稠密轨迹发送任务：预生成完整 J519 命令队列，并等待机器人状态包按 speed_ratio 推进到执行完成。 /// private void SendDenseTrajectory(TrajectoryResult result, IReadOnlyList finalJointPositions, CancellationToken cancellationToken) { @@ -546,24 +571,24 @@ public sealed class FanucControllerRuntime : IControllerRuntime, IDisposable var triggerToleranceSeconds = trajectoryStepSeconds / 2.0; var durationSeconds = result.Duration.TotalSeconds; var sampleCount = CalculateDenseSendSampleCount(durationSeconds, trajectoryStepSeconds); - var periodTicks = (long)(servoPeriodSeconds * Stopwatch.Frequency); _logger?.LogInformation( "SendDenseTrajectory 开始: program={ProgramName}, 采样数={SampleCount}, 时长={Duration}s, speedRatio={SpeedRatio}, 周期={Period}ms, 触发事件数={TriggerCount}", result.ProgramName, sampleCount, durationSeconds, speedRatio, servoPeriodSeconds * 1000, triggers.Count); var stopwatch = Stopwatch.StartNew(); - long nextTick = stopwatch.ElapsedTicks; ushort ioValue = 0; ushort ioMask = 0; int holdRemaining = -1; int segmentIndex = 0; long logInterval = Math.Max(1, sampleCount / 10); int triggerFiredCount = 0; + var commands = new List(); var sentJointRows = new List>(); + var sentTimingRows = new List>(); var sentJerkRows = new List>(); var outputDir = CreateDenseSendOutputDirectory(result.ProgramName); - double? previousTime = null; + double? previousSendTime = null; double[]? previousJoints = null; double[]? previousVelocity = null; double[]? previousAcceleration = null; @@ -573,9 +598,10 @@ public sealed class FanucControllerRuntime : IControllerRuntime, IDisposable for (long sampleIndex = 0; sampleIndex < sampleCount; sampleIndex++) { cancellationToken.ThrowIfCancellationRequested(); - nextTick += periodTicks; - var trajectoryTime = Math.Min(sampleIndex * trajectoryStepSeconds, durationSeconds); + // J519 物理发送周期固定为机器人伺服周期；speedRatio 只缩放原轨迹采样时间。 + var sendTime = sampleIndex * servoPeriodSeconds; + var trajectoryTime = Math.Min(sendTime * speedRatio, durationSeconds); var joints = SampleDenseJointTrajectoryDegrees(denseTrajectory, trajectoryTime, ref segmentIndex); // 递减 IO 保持计数器；若已到期则清零。 @@ -618,14 +644,15 @@ public sealed class FanucControllerRuntime : IControllerRuntime, IDisposable writeIoMask: ioMask, writeIoValue: ioValue); - _j519Client.UpdateCommand(command); - sentJointRows.Add(BuildDenseSendJointRow(trajectoryTime, joints, ioMask, ioValue)); + commands.Add(command); + sentJointRows.Add(BuildDenseSendJointRow(sendTime, joints, ioMask, ioValue)); + sentTimingRows.Add(BuildDenseSendTimingRow(sampleIndex, sendTime, trajectoryTime, speedRatio)); - if (previousTime is not null && previousJoints is not null) + if (previousSendTime is not null && previousJoints is not null) { var jerkRow = BuildDenseSendJerkRow( - previousTime.Value, - trajectoryTime, + previousSendTime.Value, + sendTime, previousJoints, joints, ref previousVelocity, @@ -638,39 +665,38 @@ public sealed class FanucControllerRuntime : IControllerRuntime, IDisposable ioMask = 0; } - previousTime = trajectoryTime; + previousSendTime = sendTime; previousJoints = joints; // 周期性记录进度（Debug 级别，避免高频 Info 日志）。 if (sampleIndex > 0 && sampleIndex % logInterval == 0) { _logger?.LogDebug( - "SendDenseTrajectory 进度: {Percent}% ({Current}/{Total}), time={Time:F4}s", - (int)((double)sampleIndex / sampleCount * 100), sampleIndex, sampleCount, trajectoryTime); - } - - // 高精度忙等待直到下一伺服周期。 - while (stopwatch.ElapsedTicks < nextTick) - { - Thread.SpinWait(1); + "SendDenseTrajectory 进度: {Percent}% ({Current}/{Total}), sendTime={SendTime:F4}s, trajectoryTime={TrajectoryTime:F4}s", + (int)((double)sampleIndex / sampleCount * 100), sampleIndex, sampleCount, sendTime, trajectoryTime); } } + TryWriteDenseSendArtifacts(outputDir, sentJointRows, sentTimingRows, sentJerkRows); + // 上层只负责生成完整目标序列，真实出队节拍交给 J519 状态包驱动。 + _j519Client.LoadCommandQueue(commands); + if (_j519Client.IsConnected) + { + _j519Client.WaitForCommandQueueDrainedAsync(cancellationToken).GetAwaiter().GetResult(); + } _logger?.LogInformation( - "SendDenseTrajectory 正常完成: 采样数={SampleCount}, 触发次数={TriggerFiredCount}, 实际耗时={ElapsedMs}ms", + "SendDenseTrajectory 正常完成: 采样数={SampleCount}, 触发次数={TriggerFiredCount}, 队列装载耗时={ElapsedMs}ms", sampleCount, triggerFiredCount, stopwatch.ElapsedMilliseconds); } catch (OperationCanceledException) { _logger?.LogWarning( - "SendDenseTrajectory 被取消: 已完成 {Percent}% ({Current}/{Total}), 触发次数={TriggerFiredCount}", - (int)((double)(sampleCount > 0 ? 0 : 0) / sampleCount * 100), 0, sampleCount, triggerFiredCount); + "SendDenseTrajectory 被取消: 已生成 {Current}/{Total} 条命令, 触发次数={TriggerFiredCount}", + commands.Count, sampleCount, triggerFiredCount); // 正常取消，轨迹被中断。 } finally { - TryWriteDenseSendArtifacts(outputDir, sentJointRows, sentJerkRows); - lock (_stateLock) { _isInMotion = false; @@ -794,12 +820,12 @@ public sealed class FanucControllerRuntime : IControllerRuntime, IDisposable } /// - /// 构造实际发送点位文本行，格式为 time + 关节角度 + io_mask + io_value。 + /// 构造实际发送点位文本行，格式为 send_time + 关节角度 + io_mask + io_value。 /// - private static IReadOnlyList BuildDenseSendJointRow(double trajectoryTime, IReadOnlyList joints, ushort ioMask, ushort ioValue) + private static IReadOnlyList BuildDenseSendJointRow(double sendTime, IReadOnlyList joints, ushort ioMask, ushort ioValue) { var row = new double[joints.Count + 3]; - row[0] = Math.Round(trajectoryTime, 6); + row[0] = Math.Round(sendTime, 6); for (var index = 0; index < joints.Count; index++) { row[index + 1] = Math.Round(joints[index], 6); @@ -811,18 +837,33 @@ public sealed class FanucControllerRuntime : IControllerRuntime, IDisposable } /// - /// 构造相邻发送点之间的跃度统计，格式为 start_time + end_time + dt + max_abs_jerk + jerk[j1..jn]。 + /// 构造实发时间映射文本行，格式为 sample_index + send_time + trajectory_time + speed_ratio。 /// + private static IReadOnlyList BuildDenseSendTimingRow( + long sampleIndex, + double sendTime, + double trajectoryTime, + double speedRatio) + { + return + [ + sampleIndex, + Math.Round(sendTime, 6), + Math.Round(trajectoryTime, 6), + Math.Round(speedRatio, 6) + ]; + } + /// /// 构造相邻发送点之间的跃度统计行，格式为 start_time + end_time + dt + max_abs_jerk + jerk[j1..jn]。 /// 通过 ref 参数维护前两帧的速度和加速度状态，从而用二阶后向差分近似跃度。 /// /// 上一帧时间戳（秒）。 /// 当前帧时间戳（秒）。 - /// 上一帧关节角（rad）。 - /// 当前帧关节角（rad）。 - /// 上一帧关节速度（rad/s），ref 更新为当前帧速度。 - /// 上一帧关节加速度（rad/s²），ref 更新为当前帧加速度。 + /// 上一帧 J519 角度制关节目标。 + /// 当前帧 J519 角度制关节目标。 + /// 上一帧关节速度，ref 更新为当前帧速度。 + /// 上一帧关节加速度，ref 更新为当前帧加速度。 /// 长度为 jointCount + 4 的行向量。 private static IReadOnlyList BuildDenseSendJerkRow( double previousTime, @@ -881,28 +922,31 @@ public sealed class FanucControllerRuntime : IControllerRuntime, IDisposable row[3] = Math.Round(maxAbsJerk, 6); // 本次步长内所有关节的最大绝对值跃度 for (var index = 0; index < jointCount; index++) { - row[index + 4] = Math.Round(currentJerk[index], 6); // 各关节的跃度值（rad/s³） + row[index + 4] = Math.Round(currentJerk[index], 6); // 各关节的角度制跃度值 } return row; } /// - /// 尝试把实际发送点位和跃度统计写入纯文本文件；若落盘失败，只记录日志，不影响运动主流程。 + /// 尝试把实际发送点位、时间映射和跃度统计写入纯文本文件；若落盘失败，只记录日志，不影响运动主流程。 /// private void TryWriteDenseSendArtifacts( string outputDir, IReadOnlyList> sentJointRows, + IReadOnlyList> sentTimingRows, IReadOnlyList> sentJerkRows) { try { WriteDenseRows(Path.Combine(outputDir, "ActualSendJointTraj.txt"), sentJointRows); + WriteDenseRows(Path.Combine(outputDir, "ActualSendTiming.txt"), sentTimingRows); WriteDenseRows(Path.Combine(outputDir, "ActualSendJerkStats.txt"), sentJerkRows); _logger?.LogInformation( - "SendDenseTrajectory 已写出实际发送记录: outputDir={OutputDir}, pointRows={PointRows}, jerkRows={JerkRows}", + "SendDenseTrajectory 已写出实际发送记录: outputDir={OutputDir}, pointRows={PointRows}, timingRows={TimingRows}, jerkRows={JerkRows}", outputDir, sentJointRows.Count, + sentTimingRows.Count, sentJerkRows.Count); } catch (Exception exception) @@ -936,26 +980,12 @@ public sealed class FanucControllerRuntime : IControllerRuntime, IDisposable } /// - /// 取消并等待当前后台发送任务，避免旧任务与新轨迹并发。 + /// 取消当前稠密发送任务，避免旧轨迹继续向机器人下发。 /// private void CancelSendTaskLocked() { _sendCts?.Cancel(); - if (_sendTask is not null) - { - try - { - _sendTask.Wait(TimeSpan.FromSeconds(2)); - } - catch (AggregateException) - { - // 忽略取消异常。 - } - - _sendTask = null; - } - _sendCts?.Dispose(); _sendCts = null; } diff --git a/src/Flyshot.Runtime.Fanuc/Protocol/FanucJ519Client.cs b/src/Flyshot.Runtime.Fanuc/Protocol/FanucJ519Client.cs index 70a8db3..ff22c0f 100644 --- a/src/Flyshot.Runtime.Fanuc/Protocol/FanucJ519Client.cs +++ b/src/Flyshot.Runtime.Fanuc/Protocol/FanucJ519Client.cs @@ -16,8 +16,13 @@ public sealed class FanucJ519Client : IDisposable private Task? _receiveTask; private FanucJ519Command? _currentCommand; private FanucJ519Command? _lastSentCommand; + // 稠密轨迹执行时预装的命令队列，由机器人状态包节拍逐帧出队。 + private Queue? _commandQueue; + private TaskCompletionSource? _commandQueueDrainedCompletion; private List? _commandHistoryForTests; private FanucJ519Response? _latestResponse; + // 标记 StartMotion 前是否刚装载过新目标，用于区分新命令和上次运动残留目标。 + private bool _hasPendingCommandForStart; private bool _motionStarted; private bool _disposed; @@ -82,13 +87,20 @@ public sealed class FanucJ519Client : IDisposable lock (_commandLock) { - _currentCommand = null; _lastSentCommand = null; if (_motionStarted) { _logger?.LogDebug("J519 StartMotion: 状态包驱动发送已启用"); return; } + + if (!_hasPendingCommandForStart) + { + _currentCommand = null; + CompleteCommandQueueLocked(); + } + + _hasPendingCommandForStart = false; _motionStarted = true; } @@ -112,6 +124,8 @@ public sealed class FanucJ519Client : IDisposable lock (_commandLock) { _motionStarted = false; + _hasPendingCommandForStart = false; + CompleteCommandQueueLocked(); } // FANUC 手册中 packet type=2 表示停止状态包输出；当前保留现场抓包兼容行为。 @@ -130,7 +144,9 @@ public sealed class FanucJ519Client : IDisposable lock (_commandLock) { + CompleteCommandQueueLocked(); _currentCommand = command; + _hasPendingCommandForStart = true; _commandHistoryForTests?.Add(command); } @@ -144,6 +160,65 @@ public sealed class FanucJ519Client : IDisposable } } + /// + /// 装载一整段 J519 命令队列；后续每个可接收命令的机器人状态包会自动取出下一帧。 + /// + /// 按执行顺序排列的 J519 命令列表，至少包含一帧。 + public void LoadCommandQueue(IReadOnlyList commands) + { + ArgumentNullException.ThrowIfNull(commands); + ObjectDisposedException.ThrowIf(_disposed, this); + if (commands.Count == 0) + { + throw new ArgumentException("J519 命令队列至少需要包含一帧。", nameof(commands)); + } + + lock (_commandLock) + { + CompleteCommandQueueLocked(); + _commandQueue = new Queue(commands); + // 队列耗尽后继续保持最后一帧目标，避免运动结束后回落到旧目标或空目标。 + _currentCommand = commands[^1]; + _hasPendingCommandForStart = true; + _commandQueueDrainedCompletion = new TaskCompletionSource(TaskCreationOptions.RunContinuationsAsynchronously); + _commandHistoryForTests?.AddRange(commands); + } + + _logger?.LogInformation("J519 命令队列已装载: count={Count}", commands.Count); + } + + /// + /// 等待当前预装命令队列被状态包全部取出；无队列时立即完成。 + /// + /// 取消令牌。 + /// 表示等待过程的任务。 + internal Task WaitForCommandQueueDrainedAsync(CancellationToken cancellationToken = default) + { + ObjectDisposedException.ThrowIf(_disposed, this); + + Task waitTask; + lock (_commandLock) + { + waitTask = _commandQueueDrainedCompletion?.Task ?? Task.CompletedTask; + } + + return waitTask.WaitAsync(cancellationToken); + } + + /// + /// 判断当前是否没有等待出队的命令；仅供单元测试断言。 + /// + /// 如果队列为空或尚未装载队列，则返回 true。 + internal bool IsCommandQueueDrainedForTests() + { + ObjectDisposedException.ThrowIf(_disposed, this); + + lock (_commandLock) + { + return _commandQueue is null || _commandQueue.Count == 0; + } + } + /// /// 打开命令历史记录，仅供单元测试验证运行时生成的命令序列。 /// @@ -230,7 +305,9 @@ public sealed class FanucJ519Client : IDisposable { _currentCommand = null; _lastSentCommand = null; + CompleteCommandQueueLocked(); _commandHistoryForTests = null; + _hasPendingCommandForStart = false; _motionStarted = false; } @@ -265,6 +342,12 @@ public sealed class FanucJ519Client : IDisposable _receiveTask?.Dispose(); _cts?.Dispose(); _udpClient?.Dispose(); + + lock (_commandLock) + { + CompleteCommandQueueLocked(); + _hasPendingCommandForStart = false; + } } private static FanucJ519Command WithSequence(FanucJ519Command source, uint sequence) @@ -369,11 +452,27 @@ public sealed class FanucJ519Client : IDisposable } FanucJ519Command? command; + var willDrainQueue = false; lock (_commandLock) { - command = !_motionStarted || _currentCommand is null - ? null - : WithSequence(_currentCommand, response.Sequence); + if (!_motionStarted) + { + command = null; + } + else if (_commandQueue is { Count: > 0 } queue) + { + // 状态包是唯一节拍源：每收到一帧可接收状态，才取出下一条目标。 + var queuedCommand = queue.Dequeue(); + _currentCommand = queuedCommand; + willDrainQueue = queue.Count == 0; + command = WithSequence(queuedCommand, response.Sequence); + } + else + { + command = _currentCommand is null + ? null + : WithSequence(_currentCommand, response.Sequence); + } } if (command is null) @@ -383,13 +482,33 @@ public sealed class FanucJ519Client : IDisposable var packet = FanucJ519Protocol.PackCommandPacket(command); await udpClient.SendAsync(packet, cancellationToken).ConfigureAwait(false); - + _logger?.LogDebug("J519 已回发命令包，seq={Seq}", command.Sequence); + _logger?.LogTrace( + "J519 回发命令详情: joints={Joints}, ioMask={IoMask}, ioValue={IoValue}", + command.TargetJoints, + command.WriteIoMask, + command.WriteIoValue); lock (_commandLock) { _lastSentCommand = command; + if (willDrainQueue && _commandQueue is { Count: 0 }) + { + CompleteCommandQueueLocked(); + } } } + /// + /// 清空当前命令队列，并唤醒等待队列结束的运行时任务。 + /// + private void CompleteCommandQueueLocked() + { + _commandQueue?.Clear(); + _commandQueue = null; + _commandQueueDrainedCompletion?.TrySetResult(); + _commandQueueDrainedCompletion = null; + } + /// /// 读取最近一次已成功发送命令的目标关节轴文本，便于状态日志直接对照控制目标。 /// diff --git a/tests/Flyshot.Core.Tests/FanucControllerRuntimeDenseTests.cs b/tests/Flyshot.Core.Tests/FanucControllerRuntimeDenseTests.cs index 006ea1f..4482c4b 100644 --- a/tests/Flyshot.Core.Tests/FanucControllerRuntimeDenseTests.cs +++ b/tests/Flyshot.Core.Tests/FanucControllerRuntimeDenseTests.cs @@ -3,6 +3,9 @@ using Flyshot.ControllerClientCompat; using Flyshot.Core.Config; using Flyshot.Runtime.Fanuc; using Flyshot.Runtime.Fanuc.Protocol; +using System.Buffers.Binary; +using System.Net; +using System.Net.Sockets; using System.Reflection; namespace Flyshot.Core.Tests; @@ -17,7 +20,7 @@ public sealed class FanucControllerRuntimeDenseTests private const double SmoothPtpJerkShapeCoefficient = 52.5; /// - /// 验证真机 J519 发送按 8ms 实发周期、speed_ratio 轨迹时间步进，并输出角度制目标。 + /// 验证真机 J519 会预生成按 8ms 轨迹映射的命令队列，并输出角度制目标。 /// [Fact] public void ExecuteTrajectory_WithDenseWaypoints_RealMode_ResamplesBySpeedRatioAndConvertsRadiansToDegrees() @@ -59,6 +62,65 @@ public sealed class FanucControllerRuntimeDenseTests Assert.Equal(5, commands.Count); Assert.All(commands, static command => Assert.Equal(0u, command.Sequence)); Assert.Equal([0.0, 45.0, 90.0, 135.0, 180.0], commands.Select(static command => command.TargetJoints[0])); + Assert.False(j519Client.IsCommandQueueDrainedForTests()); + } + + /// + /// 验证真机稠密轨迹接口会等待 J519 队列被状态包实际取完后再返回。 + /// + [Fact] + public async Task ExecuteTrajectory_WithDenseWaypoints_RealMode_WaitsForJ519QueueToDrainBeforeReturning() + { + using var server = new UdpClient(0); + using var cts = new CancellationTokenSource(TimeSpan.FromSeconds(5)); + using var commandClient = new FanucCommandClient(); + using var stateClient = new FanucStateClient(); + using var j519Client = new FanucJ519Client(); + using var runtime = new FanucControllerRuntime(commandClient, stateClient, j519Client); + var robot = TestRobotFactory.CreateRobotProfile(); + var port = ((IPEndPoint)server.Client.LocalEndPoint!).Port; + runtime.ResetRobot(robot, "FANUC_LR_Mate_200iD"); + j519Client.EnableCommandHistoryForTests(); + await j519Client.ConnectAsync("127.0.0.1", port, cts.Token); + var initResult = await server.ReceiveAsync(cts.Token); + j519Client.StartMotion(); + ForceRealModeEnabled(runtime, speedRatio: 1.0); + + var denseTrajectory = new[] + { + new[] { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }, + new[] { 0.008, Math.PI / 2.0, 0.0, 0.0, 0.0, 0.0, 0.0 }, + new[] { 0.016, Math.PI, 0.0, 0.0, 0.0, 0.0, 0.0 } + }; + var result = new TrajectoryResult( + programName: "wait-drain", + method: PlanningMethod.Icsp, + isValid: true, + duration: TimeSpan.FromSeconds(0.016), + shotEvents: Array.Empty(), + triggerTimeline: Array.Empty(), + artifacts: Array.Empty(), + failureReason: null, + usedCache: false, + originalWaypointCount: 4, + plannedWaypointCount: 4, + denseJointTrajectory: denseTrajectory); + + var executeTask = Task.Run(() => runtime.ExecuteTrajectory(result, [Math.PI, 0.0, 0.0, 0.0, 0.0, 0.0]), cts.Token); + await WaitUntilAsync(() => j519Client.GetCommandHistoryForTests().Count == 3, cts.Token); + + // 只有机器人状态包把队列全部取出后，ExecuteTrajectory 才能向上层返回。 + Assert.False(executeTask.IsCompleted); + + for (uint sequence = 100; sequence < 103; sequence++) + { + await SendStatusPacketAsync(server, initResult.RemoteEndPoint, sequence, cts.Token); + _ = await server.ReceiveAsync(cts.Token); + } + + await executeTask.WaitAsync(TimeSpan.FromSeconds(2), cts.Token); + Assert.True(j519Client.IsCommandQueueDrainedForTests()); + Assert.False(runtime.GetSnapshot().IsInMotion); } /// @@ -108,27 +170,47 @@ public sealed class FanucControllerRuntimeDenseTests var commands = j519Client.GetCommandHistoryForTests(); var runDir = GetSingleDenseSendRunDirectory(outputRoot); var pointsPath = Path.Combine(runDir, "ActualSendJointTraj.txt"); + var timingPath = Path.Combine(runDir, "ActualSendTiming.txt"); var jerkPath = Path.Combine(runDir, "ActualSendJerkStats.txt"); Assert.True(File.Exists(pointsPath)); + Assert.True(File.Exists(timingPath)); Assert.True(File.Exists(jerkPath)); var pointLines = File.ReadAllLines(pointsPath); + var timingLines = File.ReadAllLines(timingPath); var jerkLines = File.ReadAllLines(jerkPath); Assert.Equal(commands.Count, pointLines.Length); + Assert.Equal(commands.Count, timingLines.Length); Assert.Equal(Math.Max(0, commands.Count - 1), jerkLines.Length); var firstColumns = ParseColumns(pointLines[0]); + var secondColumns = ParseColumns(pointLines[1]); var lastColumns = ParseColumns(pointLines[^1]); Assert.Equal(9, firstColumns.Length); Assert.Equal(9, lastColumns.Length); Assert.Equal(0.0, firstColumns[0], precision: 6); + Assert.Equal(0.008, secondColumns[0], precision: 6); Assert.Equal(180.0, lastColumns[1], precision: 6); + var firstTimingColumns = ParseColumns(timingLines[0]); + var secondTimingColumns = ParseColumns(timingLines[1]); + var lastTimingColumns = ParseColumns(timingLines[^1]); + Assert.Equal(4, firstTimingColumns.Length); + Assert.Equal(0.0, firstTimingColumns[0], precision: 6); + Assert.Equal(0.0, firstTimingColumns[1], precision: 6); + Assert.Equal(0.0, firstTimingColumns[2], precision: 6); + Assert.Equal(0.5, firstTimingColumns[3], precision: 6); + Assert.Equal(1.0, secondTimingColumns[0], precision: 6); + Assert.Equal(0.008, secondTimingColumns[1], precision: 6); + Assert.Equal(0.004, secondTimingColumns[2], precision: 6); + Assert.Equal(commands.Count - 1, lastTimingColumns[0], precision: 6); + Assert.Equal(0.016, lastTimingColumns[2], precision: 6); + var firstJerkColumns = ParseColumns(jerkLines[0]); Assert.Equal(10, firstJerkColumns.Length); Assert.Equal(0.0, firstJerkColumns[0], precision: 6); - Assert.Equal(0.004, firstJerkColumns[2], precision: 6); + Assert.Equal(0.008, firstJerkColumns[2], precision: 6); } finally { @@ -797,6 +879,43 @@ public sealed class FanucControllerRuntimeDenseTests throw new TimeoutException("Timed out waiting for dense trajectory send task to finish."); } + /// + /// 等待测试条件成立，用于观察后台执行路径是否已经到达指定状态。 + /// + private static async Task WaitUntilAsync(Func condition, CancellationToken cancellationToken) + { + var deadline = DateTimeOffset.UtcNow.AddSeconds(1); + while (DateTimeOffset.UtcNow < deadline) + { + cancellationToken.ThrowIfCancellationRequested(); + if (condition()) + { + return; + } + + await Task.Delay(1, cancellationToken); + } + + throw new TimeoutException("Timed out waiting for test condition."); + } + + /// + /// 向被测 J519 客户端发送一帧最小状态包，用机器人侧 status sequence 驱动下一帧命令。 + /// + private static async Task SendStatusPacketAsync( + UdpClient server, + IPEndPoint clientEndpoint, + uint sequence, + CancellationToken cancellationToken) + { + var responsePacket = new byte[FanucJ519Protocol.ResponsePacketLength]; + BinaryPrimitives.WriteUInt32BigEndian(responsePacket.AsSpan(0x00, 4), 0); + BinaryPrimitives.WriteUInt32BigEndian(responsePacket.AsSpan(0x04, 4), 1); + BinaryPrimitives.WriteUInt32BigEndian(responsePacket.AsSpan(0x08, 4), sequence); + responsePacket[0x0c] = 0b0111; + await server.SendAsync(responsePacket, clientEndpoint, cancellationToken); + } + /// /// 获取一次测试执行生成的唯一稠密发送记录目录。 /// diff --git a/tests/Flyshot.Core.Tests/FanucJ519ClientTests.cs b/tests/Flyshot.Core.Tests/FanucJ519ClientTests.cs index 705b4b0..f297bde 100644 --- a/tests/Flyshot.Core.Tests/FanucJ519ClientTests.cs +++ b/tests/Flyshot.Core.Tests/FanucJ519ClientTests.cs @@ -194,6 +194,46 @@ public sealed class FanucJ519ClientTests : IDisposable Assert.All(packets, packet => Assert.Equal(1.0f, BinaryPrimitives.ReadSingleBigEndian(packet.AsSpan(0x1c, 4)), precision: 6)); } + /// + /// 验证预装命令队列会被机器人状态包逐帧出队，队列耗尽后继续保持最后目标。 + /// + [Fact] + public async Task StartMotion_DequeuesPreparedCommandsForStatusPacketsAndHoldsLastCommand() + { + 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]), + new FanucJ519Command(sequence: 0, targetJoints: [3.0, 0.0, 0.0, 0.0, 0.0, 0.0]) + ]); + + var packets = new List(); + for (uint sequence = 700; sequence < 704; sequence++) + { + await SendStatusPacketAsync(initResult.RemoteEndPoint, sequence); + var result = await _server.ReceiveAsync(_cts.Token); + packets.Add(result.Buffer); + } + + await client.StopMotionAsync(_cts.Token); + + var sequences = packets + .Select(packet => BinaryPrimitives.ReadUInt32BigEndian(packet.AsSpan(0x08, 4))) + .ToArray(); + var firstJointTargets = packets + .Select(packet => BinaryPrimitives.ReadSingleBigEndian(packet.AsSpan(0x1c, 4))) + .ToArray(); + + Assert.Equal([700u, 701u, 702u, 703u], sequences); + Assert.Equal([1.0f, 2.0f, 3.0f, 3.0f], firstJointTargets); + Assert.True(client.IsCommandQueueDrainedForTests()); + } + /// /// 验证停止运动后可在同一连接内重启发送，命令序号仍由新的状态包决定。 /// @@ -310,7 +350,7 @@ public sealed class FanucJ519ClientTests : IDisposable logger.Entries, entry => entry.Level == LogLevel.Information && entry.Message.Contains("J519 最后一条发送目标关节轴", StringComparison.Ordinal) - && entry.Message.Contains("1.000, 2.000, 3.000, 4.000, 5.000, 6.000", StringComparison.Ordinal)); + && entry.Message.Contains("1.00000, 2.00000, 3.00000, 4.00000, 5.00000, 6.00000", StringComparison.Ordinal)); } ///