diff --git a/README.md b/README.md
index 35de859..a0499e7 100644
--- a/README.md
+++ b/README.md
@@ -24,6 +24,7 @@
- 2026-05-11 对 `UTTC_MS11` 的独立 `ConfigRoot` 重新生成确认：flyshot 链路若继续用离散 `jerk` 作为阻断条件，会把新结果拉长到约 `16.576s / 33.152s`；只保留速度、加速度校验并跳过 flyshot jerk 阻断后，新 `1.0x / 0.5x` 时长回落到约 `7.552s / 15.104s`，与旧抓包主段 `7.408s / 14.808s` 的差异已很小，当前按可忽略处理。详见 `docs/uttc-ms11-jerk-validation-decision-20260511.md`。
- `MoveJoint` 已按 `2026042802-mvpoint*.pcap` 复刻为点到点临时轨迹：当前关节到目标关节的关节空间直线，五次 smoothstep 起停，按 `status=15` 运动窗口复现 `40/55/77` 点，并由 J519 层完成 `rad -> deg` 下发。
- 单程序只对应一台机器人，上传/删除/恢复飞拍轨迹统一读写运行目录 `Config/RobotConfig.json`，不再创建独立轨迹存储文件。
+- 服务启动时支持自动初始化机器人：在 `appsettings.json` 的 `ControllerClientCompat.AutoInit` 节配置 `RobotName` 和 `RobotIp`，启动后会自动执行 `SetUpRobot → SetActiveController → Connect → EnableRobot`；配置缺失或连接失败不会阻止服务启动。
单位约定总览：
@@ -71,6 +72,7 @@
- [x] 补齐 `Get/SetSpeedRatio`、`Get/SetTCP`、`Get/SetIO` 真机命令体与响应解析
- [x] 保留新 HTTP 接口路线，明确不再实现旧 `50001/TCP+JSON` 网关
- [x] 将飞拍轨迹持久化收敛到运行目录 `Config/RobotConfig.json`
+- [x] 服务启动自动初始化机器人：通过 `ControllerClientCompat.AutoInit` 配置节指定 `RobotName` 和 `RobotIp`，启动时自动执行初始化链路
剩余 Todo：
diff --git a/docs/fanuc-field-runtime-workflow.md b/docs/fanuc-field-runtime-workflow.md
index 77ce683..b616689 100644
--- a/docs/fanuc-field-runtime-workflow.md
+++ b/docs/fanuc-field-runtime-workflow.md
@@ -159,7 +159,8 @@ POST /disconnect_robot/
真机模式下：
-- `StopMove()` 取消当前稠密轨迹生成任务并停止 J519 状态包驱动发送。
+- 正常飞拍、MoveJoint、MovePose 结束时，默认在 J519 队列末尾追加 `LastData=1` 结束帧；结束帧发出后进入仅监控状态，继续接收 J519 状态包，不再回发保姿命令。
+- `StopMove()` 取消当前稠密轨迹生成任务并走强制停止路径，可停止 J519 状态包驱动发送。
- `DisableRobot()` 发送 J519 packet type 2 状态输出停止包，然后 `StopProg("RVBUSTSM")`。
- `Disconnect()` 关闭状态、命令和 J519 三条通道，并清理本地运行状态。
@@ -178,5 +179,5 @@ POST /disconnect_robot/
- 主运行窗口命令序号连续，无重复 seq；J519 客户端单元测试覆盖按最新 status sequence 回发命令。
- 响应 `status=15` 段覆盖主运行窗口，响应相对命令滞后 2 到 8 帧。
- 实发点位相对重采样期望的全局 RMS 小于 `0.012deg`，最大绝对误差小于 `0.07deg`。
-- `lastData=0`，结束运动当前依赖 J519 packet type 2 状态输出停止包；`../j519 协议.pcap` 中另有 1 个 `LastData=1` 后紧跟 type 2 的样本，停止语义后续单独验证。
+- 历史抓包主运行窗口 `lastData=0`；当前实现默认在生成的新 J519 队列末尾追加一帧 `LastData=1` 结束命令，随后保留状态监控且不发送 packet type 2。packet type 2 仅用于禁用、异常停止、断开等强制停止路径；如现场需要旧保持行为，可通过内部开关关闭结束帧路径。
- IO 脉冲数量和 mask 集合 `10/12/14` 与抓包一致。
diff --git a/docs/fanuc-stream-motion-manual-notes.md b/docs/fanuc-stream-motion-manual-notes.md
index 80df8ab..63919d9 100644
--- a/docs/fanuc-stream-motion-manual-notes.md
+++ b/docs/fanuc-stream-motion-manual-notes.md
@@ -82,7 +82,7 @@
文档把它定义为“停止状态包输出”的控制包，不是命令流正常终止的首选动作。命令流结束应通过 command packet 的 `Last data=1` 表达。
-当前实现：`FanucJ519Client.StopMotionAsync()` 当前会停止状态包驱动发送并发送 packet type `2`，而稠密轨迹执行期间保持 `LastData=0`。这是与 FANUC 文档最明显的语义差异之一；已有多数 UTTC 抓包显示主运行窗口 `LastData=0`，但 `../j519 协议.pcap` 中存在 1 个 `LastData=1` 后紧跟 packet type `2` 的样本，后续应单独校准停止语义。
+当前实现：`FanucJ519Client.StopMotionAsync()` 仍保留 packet type `2` 作为禁用、异常中断或断开前的强制停止路径。正常稠密运动结束由内部开关 `EndCommandStreamAfterQueueDrained` 控制，默认会在队列末尾追加一帧 `LastData=1` 的结束命令；该帧发出后客户端进入仅监控状态，继续接收 J519 状态包用于位置监控，不再持续回发保姿命令。关闭该开关时保留旧行为：队列结束后继续保持最后目标或反馈位置。`LastData` 是单帧 command packet 字段，不是全局状态；下一轮运动重新装载的新队列会从 `LastData=0` 的普通命令帧开始。
## 4. 通信时序重点
@@ -100,7 +100,7 @@
1. `FanucJ519Client` 已改为收到机器人 132B status 包后立即回发当前命令，不再由上位机本地固定 8ms 发送循环主动发包。
2. 命令包 sequence 已按刚收到的 status packet sequence 写入，避免第一帧从本地 `0` 起步。
3. `FanucControllerRuntime.ExecuteTrajectory()` 启动前会检查已有 J519 响应中的 `AcceptsCommand` 和 `SystemReady`；但如果还没收到状态包，则会放行，后续命令仍要等第一帧 status 到达才会发出。
-4. 当前稠密轨迹结束不发送 `LastData=1`，而是依赖停止 J519 状态包驱动发送和 packet type `2` stop 控制包。
+4. 默认新路径会在稠密轨迹队列末尾追加独立的 `LastData=1` 结束帧；结束后只监控状态，不发送 packet type `2`。旧保持路径仍可通过内部开关回退。
序号和节拍已经按手册方向校准；停止语义仍需在真实 R30iB 联调中继续确认。
@@ -220,13 +220,13 @@
主要差异/风险：
1. 当前未实现命令缓冲预填，也未读取 `$PKT_STACK / $START_MOVE`。
-2. 当前停止运动依赖 packet type `2` stop 控制包，没有稳定发送 `LastData=1` 的最后 command packet；这与手册标准结束语义不同。
+2. 默认正常运动结束已经通过追加 `LastData=1` 结束帧靠近手册语义；packet type `2` 仍仅作为强制停止状态输出路径。
3. 当前未实现 packet type `3` 的速度/加速度/jerk 限制表查询，也未实现 payload/speed 20 档动态限制算法。
4. 当前没有自动校验 `AUTO / 100% OVERRIDE / brake control / resume offset / payload` 等控制器前置状态。
5. 当前没有报警码读取和 `MOTN-* / PRIO-*` 映射。
建议后续联调优先级：
-1. 验证运动结束是否必须补 `LastData=1`；如果当前 stop 控制包能稳定工作，也应在文档中标为现场兼容路径，而不是手册标准路径。
+1. 联调验证 `LastData=1` 后机器人是否稳定退出命令消费并继续输出状态包；如现场程序仍要求持续命令包，可通过内部开关回退旧保持路径。
2. 抓一次报警现场包，确认 `MOTN-600/603/617` 等是否能从包序号与状态位直接定位。
3. 如果后续追求更稳的真实机运行，补 packet type `3` 限制表查询，并把规划器的速度、加速度、jerk 校验与 FANUC 手册算法靠近。
diff --git a/global.json b/global.json
index 6661367..e1d0a90 100644
--- a/global.json
+++ b/global.json
@@ -1,6 +1,6 @@
{
"sdk": {
- "version": "8.0.420",
- "rollForward": "disable"
+ "version": "8.0.422",
+ "rollForward": "latestPatch"
}
}
diff --git a/src/Flyshot.ControllerClientCompat/ControllerClientCompatOptions.cs b/src/Flyshot.ControllerClientCompat/ControllerClientCompatOptions.cs
index 100aa18..27b26e6 100644
--- a/src/Flyshot.ControllerClientCompat/ControllerClientCompatOptions.cs
+++ b/src/Flyshot.ControllerClientCompat/ControllerClientCompatOptions.cs
@@ -20,6 +20,11 @@ public sealed class ControllerClientCompatOptions
///
public string? WorkspaceRoot { get; set; }
+ ///
+ /// 获取或设置服务启动时自动初始化机器人的配置；为 null 时跳过自动初始化。
+ ///
+ public AutoInitOptions? AutoInit { get; set; }
+
///
/// 解析运行配置根目录，确保运行时默认不再依赖源码仓库位置。
///
@@ -44,3 +49,19 @@ public sealed class ControllerClientCompatOptions
: Path.GetFullPath(WorkspaceRoot);
}
}
+
+///
+/// 表示服务启动时自动初始化机器人的配置项。
+///
+public sealed class AutoInitOptions
+{
+ ///
+ /// 获取或设置机器人名称，用于加载机器人配置文件（如 LR_Mate_200iD）。
+ ///
+ public string RobotName { get; set; } = string.Empty;
+
+ ///
+ /// 获取或设置机器人 IP 地址，用于连接真机或仿真控制器。
+ ///
+ public string RobotIp { get; set; } = string.Empty;
+}
diff --git a/src/Flyshot.Runtime.Fanuc/FanucControllerRuntime.cs b/src/Flyshot.Runtime.Fanuc/FanucControllerRuntime.cs
index b8a86b1..dcebca9 100644
--- a/src/Flyshot.Runtime.Fanuc/FanucControllerRuntime.cs
+++ b/src/Flyshot.Runtime.Fanuc/FanucControllerRuntime.cs
@@ -802,6 +802,18 @@ public sealed class FanucControllerRuntime : IControllerRuntime, IDisposable
sample.TrajectoryTime);
}
}
+ if (_j519Client.EndCommandStreamAfterQueueDrained && commands.Count > 0)
+ {
+ // 正常结束命令流时追加独立结束帧，避免把最后一个真实轨迹采样点和结束语义混在一起。
+ commands.Add(new FanucJ519Command(
+ sequence: 0,
+ targetJoints: commands[^1].TargetJoints,
+ lastData: 1,
+ writeIoType: 2,
+ writeIoIndex: 1,
+ writeIoMask: 0,
+ writeIoValue: 0));
+ }
TryWriteDenseSendArtifacts(result, outputDir, sentJointRows, sentTimingRows, sentJerkRows, triggerBindings);
// 上层只负责生成完整目标序列，真实出队节拍交给 J519 状态包驱动。
@@ -865,6 +877,19 @@ public sealed class FanucControllerRuntime : IControllerRuntime, IDisposable
commands.Add(command);
sentPoseRows.Add(BuildDenseSendPoseRow(row[0], pose));
}
+ if (_j519Client.EndCommandStreamAfterQueueDrained && commands.Count > 0)
+ {
+ // 直角轨迹结束帧沿用最终目标位姿，并明确清空 IO 输出。
+ commands.Add(new FanucJ519Command(
+ sequence: 0,
+ targetValues: commands[^1].TargetValues,
+ lastData: 1,
+ dataStyle: 0,
+ writeIoType: 2,
+ writeIoIndex: 1,
+ writeIoMask: 0,
+ writeIoValue: 0));
+ }
TryWriteDenseCartesianSendArtifacts(outputDir, sentPoseRows);
diff --git a/src/Flyshot.Runtime.Fanuc/Protocol/FanucJ519Client.cs b/src/Flyshot.Runtime.Fanuc/Protocol/FanucJ519Client.cs
index 7f033c3..7600380 100644
--- a/src/Flyshot.Runtime.Fanuc/Protocol/FanucJ519Client.cs
+++ b/src/Flyshot.Runtime.Fanuc/Protocol/FanucJ519Client.cs
@@ -12,6 +12,10 @@ public sealed class FanucJ519Client : IDisposable
private readonly object _commandLock = new();
private readonly object _responseLock = new();
private readonly ILogger? _logger;
+ ///
+ /// 指示预装队列发出 LastData 结束帧后是否进入仅监控状态，而不是继续发送保姿命令。
+ ///
+ private readonly bool _endCommandStreamAfterQueueDrained;
private UdpClient? _udpClient;
private CancellationTokenSource? _cts;
private Thread? _receiveThread;
@@ -25,8 +29,14 @@ public sealed class FanucJ519Client : IDisposable
private long _slowSendCount;
private long _maxReceiveToSendTicks;
private uint _sequenceBufferSize;
- // 标记 StartMotion 前是否刚装载过新目标，用于区分新命令和上次运动残留目标。
+ ///
+ /// 在 StartMotion 前是否有待发送的新命令；如果为 true 则 StartMotion 不会清空当前目标，而是继续发送新目标。
+ ///
private bool _hasPendingCommandForStart;
+ ///
+ /// 标记命令流已通过 LastData 正常结束；该状态下仅接收状态包，不继续回发 hold 命令。
+ ///
+ private bool _monitorOnlyAfterLastData;
private bool _motionStarted;
private bool _disposed;
@@ -35,13 +45,22 @@ public sealed class FanucJ519Client : IDisposable
///
public bool IsConnected => _udpClient is not null;
+ ///
+ /// 获取正常队列结束时是否通过 LastData 结束命令流并保留状态监控。
+ ///
+ internal bool EndCommandStreamAfterQueueDrained => _endCommandStreamAfterQueueDrained;
+
///
/// 初始化 FANUC J519 客户端。
///
/// 日志记录器；允许 null，供无日志场景使用。
- public FanucJ519Client(ILogger? logger = null)
+ /// 队列结束帧发出后是否停止命令流并保留状态监控。
+ public FanucJ519Client(
+ ILogger? logger = null,
+ bool endCommandStreamAfterQueueDrained = true)
{
_logger = logger;
+ _endCommandStreamAfterQueueDrained = endCommandStreamAfterQueueDrained;
}
///
@@ -113,6 +132,7 @@ public sealed class FanucJ519Client : IDisposable
}
_hasPendingCommandForStart = false;
+ _monitorOnlyAfterLastData = false;
_motionStarted = true;
}
@@ -156,6 +176,7 @@ public sealed class FanucJ519Client : IDisposable
{
_motionStarted = false;
_hasPendingCommandForStart = false;
+ _monitorOnlyAfterLastData = false;
CompleteCommandQueueLocked();
}
@@ -178,6 +199,7 @@ public sealed class FanucJ519Client : IDisposable
CompleteCommandQueueLocked();
_currentCommand = command;
_hasPendingCommandForStart = true;
+ _monitorOnlyAfterLastData = false;
_commandHistoryForTests?.Add(command);
}
@@ -211,6 +233,8 @@ public sealed class FanucJ519Client : IDisposable
// 队列耗尽后继续保持最后一帧目标，避免运动结束后回落到旧目标或空目标。
_currentCommand = commands[^1];
_hasPendingCommandForStart = true;
+ _monitorOnlyAfterLastData = false;
+ _motionStarted = true;
_commandQueueDrainedCompletion = new TaskCompletionSource(TaskCreationOptions.RunContinuationsAsynchronously);
_commandHistoryForTests?.AddRange(commands);
}
@@ -337,6 +361,7 @@ public sealed class FanucJ519Client : IDisposable
CompleteCommandQueueLocked();
_commandHistoryForTests = null;
_hasPendingCommandForStart = false;
+ _monitorOnlyAfterLastData = false;
_motionStarted = false;
}
@@ -375,6 +400,7 @@ public sealed class FanucJ519Client : IDisposable
{
CompleteCommandQueueLocked();
_hasPendingCommandForStart = false;
+ _monitorOnlyAfterLastData = false;
}
}
@@ -526,9 +552,15 @@ public sealed class FanucJ519Client : IDisposable
{
FanucJ519Command? command;
var willDrainQueue = false;
+ var suppressHoldCommand = false;
lock (_commandLock)
{
- if (!_motionStarted)
+ if (_monitorOnlyAfterLastData)
+ {
+ command = null;
+ suppressHoldCommand = true;
+ }
+ else if (!_motionStarted)
{
command = null;
}
@@ -546,7 +578,7 @@ public sealed class FanucJ519Client : IDisposable
}
}
- if (command is null)
+ if (command is null && !suppressHoldCommand)
{
command = TryBuildHoldCommandFromLatestResponse(response);
}
@@ -574,7 +606,14 @@ public sealed class FanucJ519Client : IDisposable
lock (_commandLock)
{
_lastSentCommand = command;
- if (willDrainQueue && _commandQueue is { Count: 0 })
+ if (_endCommandStreamAfterQueueDrained && command.LastData == 1)
+ {
+ // LastData 是命令流的正常结束语义；结束后保留状态接收，但不继续回发保姿包。
+ _monitorOnlyAfterLastData = true;
+ _motionStarted = false;
+ CompleteCommandQueueLocked();
+ }
+ else if (willDrainQueue && _commandQueue is { Count: 0 })
{
CompleteCommandQueueLocked();
}
diff --git a/src/Flyshot.Server.Host/Program.cs b/src/Flyshot.Server.Host/Program.cs
index 6e313ff..e49f392 100644
--- a/src/Flyshot.Server.Host/Program.cs
+++ b/src/Flyshot.Server.Host/Program.cs
@@ -1,6 +1,7 @@
using Flyshot.ControllerClientCompat;
using Flyshot.Server.Host;
using Flyshot.Server.Host.Middleware;
+using Flyshot.Server.Host.Services;
using Microsoft.Extensions.Options;
using Microsoft.OpenApi.Models;
using NLog.Web;
@@ -23,6 +24,10 @@ try
builder.Services.Configure(builder.Configuration.GetSection("Swagger"));
builder.Services.Configure(builder.Configuration.GetSection("Cors"));
builder.Services.AddControllerClientCompat(builder.Configuration);
+
+ // 注册服务启动时自动初始化机器人的托管服务；配置缺失时自动跳过。
+ builder.Services.AddHostedService();
+
builder.Services.AddControllers();
builder.Services.AddEndpointsApiExplorer();
diff --git a/src/Flyshot.Server.Host/Services/AutoInitRobotHostedService.cs b/src/Flyshot.Server.Host/Services/AutoInitRobotHostedService.cs
new file mode 100644
index 0000000..266710f
--- /dev/null
+++ b/src/Flyshot.Server.Host/Services/AutoInitRobotHostedService.cs
@@ -0,0 +1,94 @@
+using Flyshot.ControllerClientCompat;
+using Microsoft.Extensions.Options;
+
+namespace Flyshot.Server.Host.Services;
+
+///
+/// 服务启动时自动初始化机器人的托管服务。
+/// 从配置文件读取 robot_name 和 robot_ip，依次执行 SetUpRobot、SetActiveController、Connect、EnableRobot。
+/// 初始化失败不会阻止服务启动，仅记录警告日志。
+///
+public sealed class AutoInitRobotHostedService : IHostedService
+{
+ private readonly IControllerClientCompatService _compatService;
+ private readonly ControllerClientCompatOptions _options;
+ private readonly ILogger _logger;
+
+ ///
+ /// 初始化自动初始化机器人托管服务的实例。
+ ///
+ /// 控制器兼容服务，用于执行机器人初始化操作。
+ /// 控制器兼容层配置，包含自动初始化参数。
+ /// 日志记录器。
+ public AutoInitRobotHostedService(
+ IControllerClientCompatService compatService,
+ IOptions options,
+ ILogger logger)
+ {
+ _compatService = compatService;
+ _options = options.Value;
+ _logger = logger;
+ }
+
+ ///
+ /// 服务启动时执行自动初始化。若配置中未指定 AutoInit 或参数为空，则跳过。
+ ///
+ /// 取消令牌。
+ /// 异步任务。
+ public Task StartAsync(CancellationToken cancellationToken)
+ {
+ var autoInit = _options.AutoInit;
+
+ // 未配置 AutoInit 或参数为空时，跳过自动初始化
+ if (autoInit == null
+ || string.IsNullOrWhiteSpace(autoInit.RobotName)
+ || string.IsNullOrWhiteSpace(autoInit.RobotIp))
+ {
+ _logger.LogInformation("未配置 AutoInit，跳过服务启动自动初始化。");
+ return Task.CompletedTask;
+ }
+
+ _logger.LogInformation(
+ "开始自动初始化机器人: robot_name={RobotName}, robot_ip={RobotIp}",
+ autoInit.RobotName, autoInit.RobotIp);
+
+ try
+ {
+ // 步骤1：加载机器人配置
+ _compatService.SetUpRobot(autoInit.RobotName);
+ if (!_compatService.IsSetUp)
+ {
+ _logger.LogWarning("自动初始化失败: SetUpRobot 返回 IsSetUp=false，robot_name={RobotName}", autoInit.RobotName);
+ return Task.CompletedTask;
+ }
+
+ // 步骤2：设置控制器模式（默认非仿真）
+ _compatService.SetActiveController(sim: false);
+
+ // 步骤3：连接机器人控制器
+ _compatService.Connect(autoInit.RobotIp);
+
+ // 步骤4：使能机器人（上伺服）
+ _compatService.EnableRobot(8);
+
+ _logger.LogInformation("自动初始化成功: robot_name={RobotName}", autoInit.RobotName);
+ }
+ catch (Exception ex)
+ {
+ // 初始化失败不阻止服务启动，仅记录警告
+ _logger.LogWarning(ex, "自动初始化机器人失败，服务将继续启动。robot_name={RobotName}", autoInit.RobotName);
+ }
+
+ return Task.CompletedTask;
+ }
+
+ ///
+ /// 服务停止时的清理操作。当前无需额外清理。
+ ///
+ /// 取消令牌。
+ /// 异步任务。
+ public Task StopAsync(CancellationToken cancellationToken)
+ {
+ return Task.CompletedTask;
+ }
+}
diff --git a/src/Flyshot.Server.Host/appsettings.json b/src/Flyshot.Server.Host/appsettings.json
index e8d68f9..bae74e8 100644
--- a/src/Flyshot.Server.Host/appsettings.json
+++ b/src/Flyshot.Server.Host/appsettings.json
@@ -6,7 +6,11 @@
}
},
"ControllerClientCompat": {
- "ServerVersion": "flyshot-replacement-controller-client-compat/0.1.0"
+ "ServerVersion": "flyshot-replacement-controller-client-compat/0.1.0",
+ "AutoInit": {
+ "RobotName": "LR_Mate_200iD",
+ "RobotIp": "127.0.0.1"
+ }
},
"Swagger": {
"Enabled": true,
diff --git a/tests/Flyshot.Core.Tests/FanucControllerRuntimeDenseTests.cs b/tests/Flyshot.Core.Tests/FanucControllerRuntimeDenseTests.cs
index 02d35a0..183f92c 100644
--- a/tests/Flyshot.Core.Tests/FanucControllerRuntimeDenseTests.cs
+++ b/tests/Flyshot.Core.Tests/FanucControllerRuntimeDenseTests.cs
@@ -60,15 +60,20 @@ public sealed class FanucControllerRuntimeDenseTests
WaitUntilIdle(runtime);
var commands = j519Client.GetCommandHistoryForTests();
- Assert.Equal(3, commands.Count);
+ Assert.Equal(4, commands.Count);
Assert.All(commands, static command => Assert.Equal(0u, command.Sequence));
var expectedJ1Targets = new[] { 0.0, 0.08, 0.16 };
- var actualJ1Targets = commands.Select(static command => command.TargetJoints[0]).ToArray();
+ var actualJ1Targets = commands.Take(3).Select(static command => command.TargetJoints[0]).ToArray();
Assert.Equal(expectedJ1Targets.Length, actualJ1Targets.Length);
for (var index = 0; index < expectedJ1Targets.Length; index++)
{
Assert.Equal(expectedJ1Targets[index], actualJ1Targets[index], precision: 6);
}
+ Assert.All(commands.Take(3), static command => Assert.Equal(0, command.LastData));
+ Assert.Equal(1, commands[^1].LastData);
+ Assert.Equal(0, commands[^1].WriteIoMask);
+ Assert.Equal(0, commands[^1].WriteIoValue);
+ Assert.Equal(0.16, commands[^1].TargetJoints[0], precision: 6);
Assert.False(j519Client.IsCommandQueueDrainedForTests());
}
@@ -114,12 +119,12 @@ public sealed class FanucControllerRuntimeDenseTests
denseJointTrajectory: denseTrajectory);
var executeTask = Task.Run(() => runtime.ExecuteTrajectory(result, [DegreesToRadians(0.16), 0.0, 0.0, 0.0, 0.0, 0.0]), cts.Token);
- await WaitUntilAsync(() => j519Client.GetCommandHistoryForTests().Count == 3, cts.Token);
+ await WaitUntilAsync(() => j519Client.GetCommandHistoryForTests().Count == 4, cts.Token);
// 只有机器人状态包把队列全部取出后，ExecuteTrajectory 才能向上层返回。
Assert.False(executeTask.IsCompleted);
- for (uint sequence = 100; sequence < 103; sequence++)
+ for (uint sequence = 100; sequence < 104; sequence++)
{
await SendStatusPacketAsync(server, initResult.RemoteEndPoint, sequence, cts.Token);
_ = await server.ReceiveAsync(cts.Token);
@@ -190,9 +195,9 @@ public sealed class FanucControllerRuntimeDenseTests
var timingLines = File.ReadAllLines(timingPath);
var jerkLines = File.ReadAllLines(jerkPath);
var shotEventsJson = File.ReadAllText(shotEventsPath);
- Assert.Equal(commands.Count, pointLines.Length);
- Assert.Equal(commands.Count, timingLines.Length);
- Assert.Equal(Math.Max(0, commands.Count - 1), jerkLines.Length);
+ Assert.Equal(denseTrajectory.Length, pointLines.Length);
+ Assert.Equal(denseTrajectory.Length, timingLines.Length);
+ Assert.Equal(Math.Max(0, denseTrajectory.Length - 1), jerkLines.Length);
var firstColumns = ParseColumns(pointLines[0]);
var secondColumns = ParseColumns(pointLines[1]);
@@ -216,7 +221,7 @@ public sealed class FanucControllerRuntimeDenseTests
Assert.Equal(1.0, secondTimingColumns[0], precision: 6);
Assert.Equal(0.008, secondTimingColumns[1], precision: 6);
Assert.Equal(0.008, secondTimingColumns[2], precision: 6);
- Assert.Equal(commands.Count - 1, lastTimingColumns[0], precision: 6);
+ Assert.Equal(denseTrajectory.Length - 1, lastTimingColumns[0], precision: 6);
Assert.Equal(0.016, lastTimingColumns[2], precision: 6);
var firstJerkColumns = ParseColumns(jerkLines[0]);
@@ -395,10 +400,11 @@ public sealed class FanucControllerRuntimeDenseTests
var commands = j519Client.GetCommandHistoryForTests();
Assert.NotEmpty(commands);
- Assert.Equal(preparedExecution.Samples.Count, commands.Count);
- Assert.Equal(speedRatio, preparedExecution.FinalSpeedRatio, precision: 6);
+ Assert.Equal(preparedExecution.Samples.Count + 1, commands.Count);
+ Assert.Equal(speedRatio, preparedExecution.RequestSpeedRatio, precision: 6);
Assert.Equal(0, preparedExecution.StretchIterationCount);
Assert.Equal(preparedExecution.Samples[0].JointsDegrees, commands[0].TargetJoints);
+ Assert.Equal(1, commands[^1].LastData);
}
finally
{
@@ -469,8 +475,9 @@ public sealed class FanucControllerRuntimeDenseTests
WaitUntilIdle(runtime);
var commands = j519Client.GetCommandHistoryForTests();
- Assert.Equal(preparedSamples.Length, commands.Count);
- Assert.Equal([1.0, 2.0, 3.0], commands.Select(static command => command.TargetJoints[0]));
+ Assert.Equal(preparedSamples.Length + 1, commands.Count);
+ Assert.Equal([1.0, 2.0, 3.0], commands.Take(preparedSamples.Length).Select(static command => command.TargetJoints[0]));
+ Assert.Equal(1, commands[^1].LastData);
}
///
@@ -523,17 +530,57 @@ public sealed class FanucControllerRuntimeDenseTests
WaitUntilIdle(runtime);
var commands = j519Client.GetCommandHistoryForTests();
- Assert.Equal(expectedResult.DenseJointTrajectory!.Count, commands.Count);
+ Assert.Equal(expectedResult.DenseJointTrajectory!.Count + 1, commands.Count);
AssertJointDegreesEqual(startJoints, commands[0].TargetJoints);
AssertJointDegreesEqual(targetJoints, commands[^1].TargetJoints);
+ Assert.Equal(1, commands[^1].LastData);
+ Assert.All(commands.Take(commands.Count - 1), static command => Assert.Equal(0, command.LastData));
- var middleAlpha = ComputeLineAlpha(commands[commands.Count / 2].TargetJoints, startJoints, targetJoints);
+ var middleAlpha = ComputeLineAlpha(commands[(commands.Count - 1) / 2].TargetJoints, startJoints, targetJoints);
Assert.InRange(middleAlpha, 0.45, 0.55);
var earlyAlpha = ComputeLineAlpha(commands[Math.Min(5, commands.Count - 1)].TargetJoints, startJoints, targetJoints);
Assert.InRange(earlyAlpha, 0.0, 0.02);
}
+ ///
+ /// 验证关闭命令流结束开关时，MoveJoint 保持旧队列长度和最终目标保持语义。
+ ///
+ [Fact]
+ public void MoveJoint_RealMode_WithEndCommandStreamDisabled_KeepsLegacyQueueShape()
+ {
+ using var commandClient = new FanucCommandClient();
+ using var stateClient = new FanucStateClient();
+ using var j519Client = new FanucJ519Client(endCommandStreamAfterQueueDrained: false);
+ using var runtime = new FanucControllerRuntime(commandClient, stateClient, j519Client);
+ var service = CreateCompatService(runtime);
+ var startJoints = new[] { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
+ var targetJoints = new[] { 0.2, -0.1, 0.05, 0.0, 0.0, 0.1 };
+
+ service.SetUpRobot("FANUC_LR_Mate_200iD");
+ j519Client.EnableCommandHistoryForTests();
+ ForceRealModeEnabled(runtime, speedRatio: 1.0);
+ SetPrivateField(runtime, "_jointPositions", startJoints);
+
+ var options = new ControllerClientCompatOptions
+ {
+ ConfigRoot = TestRobotFactory.GetConfigRoot()
+ };
+ var robot = new ControllerClientCompatRobotCatalog(options, new RobotModelLoader())
+ .LoadProfile("FANUC_LR_Mate_200iD", accLimitScale: 1.0, jerkLimitScale: 1.0);
+ var expectedResult = MoveJointTrajectoryGenerator.CreateResult(robot, startJoints, targetJoints, speedRatio: 1.0);
+
+ service.MoveJoint(targetJoints);
+ WaitUntilIdle(runtime);
+
+ var commands = j519Client.GetCommandHistoryForTests();
+ Assert.Equal(expectedResult.DenseJointTrajectory!.Count, commands.Count);
+ Assert.All(commands, static command => Assert.Equal(0, command.LastData));
+ var currentCommand = j519Client.GetCurrentCommand();
+ Assert.NotNull(currentCommand);
+ AssertJointDegreesEqual(targetJoints, currentCommand.TargetJoints);
+ }
+
[Fact]
public void MoveJointTrajectoryGenerator_LowerSpeedUsesFixedServoPeriodAndLongerPlannedDuration()
{
@@ -629,6 +676,10 @@ public sealed class FanucControllerRuntimeDenseTests
Assert.All(commands, static command => Assert.Equal(0, command.DataStyle));
AssertPoseEqual(startPose, commands[0].TargetValues.Take(6).ToArray());
AssertPoseEqual(targetPose, commands[^1].TargetValues.Take(6).ToArray());
+ Assert.All(commands.Take(commands.Count - 1), static command => Assert.Equal(0, command.LastData));
+ Assert.Equal(1, commands[^1].LastData);
+ Assert.Equal(0, commands[^1].WriteIoMask);
+ Assert.Equal(0, commands[^1].WriteIoValue);
Assert.All(commands, static command =>
{
Assert.Equal(0.0, command.TargetValues[6], precision: 6);
@@ -677,8 +728,9 @@ public sealed class FanucControllerRuntimeDenseTests
WaitUntilIdle(runtime);
var commands = j519Client.GetCommandHistoryForTests();
- Assert.Equal(3, commands.Count);
+ Assert.Equal(4, commands.Count);
Assert.Equal(0.16, commands[^1].TargetJoints[0], precision: 6);
+ Assert.Equal(1, commands[^1].LastData);
}
///
@@ -782,9 +834,12 @@ public sealed class FanucControllerRuntimeDenseTests
WaitUntilIdle(runtime);
var commands = j519Client.GetCommandHistoryForTests();
- Assert.Equal(4, commands.Count);
- Assert.Equal([(ushort)0, (ushort)5, (ushort)5, (ushort)5], commands.Select(static command => command.WriteIoMask));
- Assert.Equal([(ushort)0, (ushort)5, (ushort)5, (ushort)0], commands.Select(static command => command.WriteIoValue));
+ Assert.Equal(5, commands.Count);
+ Assert.Equal([(ushort)0, (ushort)5, (ushort)5, (ushort)5], commands.Take(4).Select(static command => command.WriteIoMask));
+ Assert.Equal([(ushort)0, (ushort)5, (ushort)5, (ushort)0], commands.Take(4).Select(static command => command.WriteIoValue));
+ Assert.Equal(1, commands[^1].LastData);
+ Assert.Equal(0, commands[^1].WriteIoMask);
+ Assert.Equal(0, commands[^1].WriteIoValue);
}
///
@@ -837,9 +892,12 @@ public sealed class FanucControllerRuntimeDenseTests
WaitUntilIdle(runtime);
var commands = j519Client.GetCommandHistoryForTests();
- Assert.Equal(4, commands.Count);
- Assert.Equal([(ushort)0, (ushort)0, (ushort)1, (ushort)1], commands.Select(static command => command.WriteIoMask));
- Assert.Equal([(ushort)0, (ushort)0, (ushort)1, (ushort)0], commands.Select(static command => command.WriteIoValue));
+ Assert.Equal(5, commands.Count);
+ Assert.Equal([(ushort)0, (ushort)0, (ushort)1, (ushort)1], commands.Take(4).Select(static command => command.WriteIoMask));
+ Assert.Equal([(ushort)0, (ushort)0, (ushort)1, (ushort)0], commands.Take(4).Select(static command => command.WriteIoValue));
+ Assert.Equal(1, commands[^1].LastData);
+ Assert.Equal(0, commands[^1].WriteIoMask);
+ Assert.Equal(0, commands[^1].WriteIoValue);
}
///
diff --git a/tests/Flyshot.Core.Tests/FanucJ519ClientTests.cs b/tests/Flyshot.Core.Tests/FanucJ519ClientTests.cs
index f026b2b..aff1c16 100644
--- a/tests/Flyshot.Core.Tests/FanucJ519ClientTests.cs
+++ b/tests/Flyshot.Core.Tests/FanucJ519ClientTests.cs
@@ -279,6 +279,119 @@ public sealed class FanucJ519ClientTests : IDisposable
Assert.True(client.IsCommandQueueDrainedForTests());
}
+ ///
+ /// 验证启用命令流结束开关后，LastData 结束帧发送完成会进入仅监控状态，不再继续回发保姿命令。
+ ///
+ [Fact]
+ public async Task LoadCommandQueue_WithEndCommandStreamEnabled_StopsSendingAfterLastDataAndKeepsMonitoring()
+ {
+ 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], lastData: 1)
+ ]);
+
+ await SendStatusPacketAsync(initResult.RemoteEndPoint, sequence: 800);
+ var first = await _server.ReceiveAsync(_cts.Token);
+ await SendStatusPacketAsync(initResult.RemoteEndPoint, sequence: 801);
+ var end = await _server.ReceiveAsync(_cts.Token);
+ await SendStatusPacketAsync(
+ initResult.RemoteEndPoint,
+ sequence: 802,
+ jointDegrees: [7.0, 8.0, 9.0, 10.0, 11.0, 12.0]);
+
+ await Assert.ThrowsAsync(
+ () => _server.ReceiveAsync(_cts.Token).AsTask().WaitAsync(TimeSpan.FromMilliseconds(120)));
+
+ var latest = client.GetLatestResponse();
+ Assert.Equal(FanucJ519Protocol.CommandPacketLength, first.Buffer.Length);
+ Assert.Equal(FanucJ519Protocol.CommandPacketLength, end.Buffer.Length);
+ Assert.Equal(0, first.Buffer[0x0c]);
+ Assert.Equal(1, end.Buffer[0x0c]);
+ Assert.NotNull(latest);
+ Assert.Equal(802u, latest.Sequence);
+ Assert.Equal(7.0, latest.JointDegrees[0], precision: 6);
+
+ await client.StopMotionAsync(_cts.Token);
+ }
+
+ ///
+ /// 验证禁用命令流结束开关后，即使队列里出现 LastData 也继续沿用旧的最后目标保持行为。
+ ///
+ [Fact]
+ public async Task LoadCommandQueue_WithEndCommandStreamDisabled_HoldsLastCommandAfterLastData()
+ {
+ using var client = new FanucJ519Client(endCommandStreamAfterQueueDrained: false);
+ 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], lastData: 1)
+ ]);
+
+ var packets = new List();
+ for (uint sequence = 810; sequence < 813; sequence++)
+ {
+ await SendStatusPacketAsync(initResult.RemoteEndPoint, sequence);
+ var result = await _server.ReceiveAsync(_cts.Token);
+ packets.Add(result.Buffer);
+ }
+
+ Assert.Equal([810u, 811u, 812u], packets
+ .Select(packet => BinaryPrimitives.ReadUInt32BigEndian(packet.AsSpan(0x08, 4)))
+ .ToArray());
+ Assert.Equal([1.0f, 2.0f, 2.0f], packets
+ .Select(packet => BinaryPrimitives.ReadSingleBigEndian(packet.AsSpan(0x1c, 4)))
+ .ToArray());
+
+ await client.StopMotionAsync(_cts.Token);
+ }
+
+ ///
+ /// 验证 LastData 后的仅监控状态不会阻止下一轮队列重新开始发送。
+ ///
+ [Fact]
+ public async Task LoadCommandQueue_AfterLastDataMonitoring_CanStreamNewQueue()
+ {
+ 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], lastData: 1)
+ ]);
+ await SendStatusPacketAsync(initResult.RemoteEndPoint, sequence: 820);
+ _ = await _server.ReceiveAsync(_cts.Token);
+ await SendStatusPacketAsync(initResult.RemoteEndPoint, sequence: 821);
+ _ = await _server.ReceiveAsync(_cts.Token);
+
+ client.LoadCommandQueue(
+ [
+ new FanucJ519Command(sequence: 0, targetJoints: [3.0, 0.0, 0.0, 0.0, 0.0, 0.0]),
+ new FanucJ519Command(sequence: 0, targetJoints: [4.0, 0.0, 0.0, 0.0, 0.0, 0.0], lastData: 1)
+ ]);
+ await SendStatusPacketAsync(initResult.RemoteEndPoint, sequence: 822);
+ var restarted = await _server.ReceiveAsync(_cts.Token);
+
+ Assert.Equal(FanucJ519Protocol.CommandPacketLength, restarted.Buffer.Length);
+ Assert.Equal(822u, BinaryPrimitives.ReadUInt32BigEndian(restarted.Buffer.AsSpan(0x08, 4)));
+ Assert.Equal(0, restarted.Buffer[0x0c]);
+ Assert.Equal(3.0f, BinaryPrimitives.ReadSingleBigEndian(restarted.Buffer.AsSpan(0x1c, 4)), precision: 6);
+
+ await client.StopMotionAsync(_cts.Token);
+ }
+
///
/// 验证停止运动后可在同一连接内重启发送，命令序号仍由新的状态包决定。
///