diff --git a/AGENTS.md b/AGENTS.md index 1fe51ff..3b6240e 100644 --- a/AGENTS.md +++ b/AGENTS.md @@ -8,7 +8,7 @@ - 使用 `C# + .NET 8` - 提供跨平台独立服务端 -- 兼容现有 `50001/TCP+JSON` 上层接入语义 +- 以新的 ASP.NET Core HTTP API 作为唯一上层接口 - 重写轨迹生成、触发时序、FANUC 控制链路和状态监控 - Windows / Linux 都能运行完整服务端 - 只支持当前现场这套组合 @@ -18,6 +18,7 @@ - GUI 桌面程序 - 多机器人同时控制 - 面向多控制柜的通用平台化框架 +- 恢复旧 `50001/TCP+JSON` 网关 ## 2. 代码与资料边界 @@ -91,6 +92,7 @@ flyshot-replacement/ ### 4.2 实现约束 +- 旧 `ControllerClient` 资料只作为接口语义参考；运行时入口以新 HTTP API 为准，不恢复旧 `50001/TCP+JSON` 网关。 - 旧协议兼容以“语义兼容”为主，不追求二进制逐字节一致。 - 轨迹规划必须与底层 Socket / HTTP / Web UI 解耦。 - 领域层不允许引用 ASP.NET Core、Socket、文件系统 API。 @@ -169,8 +171,9 @@ flyshot-replacement/ - `Flyshot.Server.Host` 已提供最小 `/healthz`。 - 最小集成测试已通过。 - 解决方案构建已通过。 -- HTTP-only `ControllerClientCompat` 已覆盖旧 HTTP 控制器后端的主要兼容语义。 -- `Flyshot.Core.Planning` 已落地 `icsp` 与 `self-adapt-icsp` 的最小规划链路。 +- 新 HTTP API / HTTP-only `ControllerClientCompat` 已覆盖旧 HTTP 控制器后端的主要兼容语义。 +- `Flyshot.Core.Planning` 已落地 `icsp` 与 `self-adapt-icsp`，并已完成旧系统导出轨迹对齐。 - `Flyshot.Core.Triggering` 已能从 `shot_flags / offset_values / addr` 生成触发时间轴。 -- `Flyshot.Runtime.Fanuc` 已提供状态型最小运行时骨架，供兼容服务执行规划结果。 +- `Flyshot.Runtime.Fanuc` 已固化 `10010 / 10012 / 60015` 基础协议帧编解码，`10010` 状态帧以 `j519 协议.pcap` 真机抓包确认为 90B。 +- `Flyshot.Runtime.Fanuc` 已具备基础 Socket 客户端和 J519 周期发送链路，但速度倍率、TCP、IO、J519 闭环与现场联调仍需补齐。 - `ExecuteTrajectory` / `ExecuteFlyShotTraj` 已接入 `Planning + Triggering + Runtime`，不再只是兼容层内存赋值。 diff --git a/README.md b/README.md index 066d2d4..4edd75e 100644 --- a/README.md +++ b/README.md @@ -4,7 +4,7 @@ 当前目标： -- 兼容现有 `50001/TCP+JSON` 上层接入语义 +- 以新的 ASP.NET Core HTTP API 作为唯一上层接口 - 重写轨迹生成、触发时序和 FANUC 实时控制链路 - 提供 Web 状态监控页面 - 在 Windows 和 Linux 上运行完整后台服务 @@ -13,9 +13,12 @@ - 这是长期运行的无头后台服务，不是 GUI 桌面程序。 - 第一版仅面向当前现场组合，后续再扩展机型与控制柜适配。 -- 当前仓库内已经移除宿主中的 `50001/TCP+JSON` 监听实现；现阶段只保留 ASP.NET Core HTTP 控制器层，以及其后端 `Flyshot.ControllerClientCompat` 兼容服务。 -- `ExecuteTrajectory` 与 `ExecuteFlyShotTraj` 已经接入 `Planning + Triggering + Runtime` 最小链路；Web 状态页已通过 `/status` 和 `/api/status/snapshot` 暴露当前兼容层与运行时状态；`Flyshot.Runtime.Fanuc` 已固化 `10010 / 10012 / 60015` 基础协议帧编解码，但运行时仍是状态型骨架，尚未完成真机 Socket 联调。 -- `50001/TCP+JSON` 的真实兼容入口如果后续需要恢复，必须基于 `docs/controller-client-api-compatibility-requirements.md` 与 `docs/controller-client-api-reverse-engineering.md` 重新评估，而不是直接把旧的 TCP 网关方向接回宿主。 +- 当前仓库不再恢复旧 `50001/TCP+JSON` 监听入口；旧 `ControllerClient` 逆向资料只作为接口语义参考，不作为运行时目标。 +- 宿主只保留 ASP.NET Core HTTP 控制器层，以及其后端 `Flyshot.ControllerClientCompat` 兼容服务。 +- `ExecuteTrajectory` 与 `ExecuteFlyShotTraj` 已经接入 `Planning + Triggering + Runtime` 链路；Web 状态页已通过 `/status` 和 `/api/status/snapshot` 暴露当前兼容层与运行时状态。 +- `Flyshot.Core.Planning` 的 ICSP / self-adapt-icsp 轨迹已经完成旧系统导出轨迹对齐；`doubles` 仍未实现。 +- `Flyshot.Runtime.Fanuc` 已固化 `10010 / 10012 / 60015` 基础协议帧编解码。`10010` 状态通道以 `j519 协议.pcap` 真机抓包确认为 90B 固定帧。 +- 真机 Socket 客户端已具备基础连接、程序启停和 J519 周期发送能力，但速度倍率、TCP、IO、J519 闭环和现场联调仍需补齐。 开发约定： @@ -23,15 +26,58 @@ - 当前仓库内的 `@` 引用主要覆盖本仓库文件；引用父目录资料时，请直接写相对路径，如 `../analysis/ICSP_algorithm_reverse_analysis.md`。 - 父目录中的 `analysis/`、`FlyingShot/`、`RobotController/`、`RPS/` 主要作为逆向参考资料和样本来源，新实现默认只落地在当前仓库。 -当前 Todo： +当前已完成： - [x] 初始化独立仓库 - [x] 创建 `dotnet 8` 解决方案骨架 - [x] 打通最小宿主与 `/healthz` - [x] 建立领域模型与模块边界 - [x] 落地配置兼容与机器人模型解析 -- [x] 落地轨迹规划与飞拍触发时间轴 -- [x] 将 `ExecuteTrajectory` / `ExecuteFlyShotTraj` 接入最小 FANUC 运行时骨架 +- [x] 落地 ICSP / self-adapt-icsp 轨迹规划与飞拍触发时间轴 +- [x] 完成 ICSP 轨迹导出结果与旧系统对齐 +- [x] 将 `ExecuteTrajectory` / `ExecuteFlyShotTraj` 接入 FANUC 运行时链路 - [x] 落地 Web 状态页 -- [x] 固化 `10010 / 10012 / 60015` FANUC 基础协议帧编解码 -- [ ] 落地真实 `10010 / 10012 / 60015` FANUC Socket 通讯与现场联调 +- [x] 固化 `10010 / 10012 / 60015` FANUC 基础协议帧编解码，确认 `10010` 状态帧为 90B +- [x] 使用本地 TCP/UDP 模拟器覆盖命令通道、状态通道和 J519 基础收发 +- [x] 保留新 HTTP 接口路线，明确不再实现旧 `50001/TCP+JSON` 网关 + +剩余 Todo： + +1. 配置与测试基线 + - [x] 修正 `ConfigCompatibilityTests` 当前样本路径漂移：`Rvbust/EOL10_EAU_0/RobotConfig.json` 不再包含 `001`，应改用稳定样本或更新断言。 + - [x] 将 `RobotConfig.json` 中的 `use_do`、`io_keep_cycles`、`acc_limit`、`jerk_limit`、`adapt_icsp_try_num` 全部贯通到规划和执行链路。 + - [ ] 为新 HTTP API 补一份当前现场调用顺序文档，替代旧 `ControllerClient` 工作流。 + +2. 轨迹规划 + - [ ] 补齐 ICSP 最终 `global_scale > 1.0` 失败判定，避免未收敛轨迹被当作有效结果执行。 + - [x] 将 self-adapt-icsp 的补点次数改为使用配置中的 `adapt_icsp_try_num`。 + - [ ] 如果现场仍需要 `method="doubles"`，实现 `TrajectoryDoubleS` 等价规划；否则在 HTTP 文档中明确标为不支持。 + - [ ] 把已完成对齐的旧系统轨迹样本固化为 golden tests，防止后续重构破坏轨迹一致性。 + +3. FANUC TCP 10012 命令通道 + - [ ] 补齐 `GetSpeedRatio` / `SetSpeedRatio` 真机命令体与响应解析。 + - [ ] 补齐 `GetTCP` / `SetTCP` 真机命令体与响应解析。 + - [ ] 补齐 `GetIO` / `SetIO` 真机命令体与响应解析。 + - [x] 所有命令响应必须检查 `result_code`，失败时返回可诊断错误，而不是只更新本地缓存。 + +4. FANUC TCP 10010 状态通道 + - [ ] 用 `j519 协议.pcap` 中的 90B 真机状态帧扩充状态解析测试样本。 + - [ ] 明确 `pose[6]`、`joint_or_ext[9]`、尾部状态字的字段语义，并映射到 `ControllerStateSnapshot`。 + - [ ] 补充断线、异常帧、超时和重连策略。 + +5. FANUC UDP 60015 J519 运动链路 + - [ ] 重新确认 J519 发送循环与 `FanucControllerRuntime` 稠密轨迹循环的职责边界，避免双重节拍或命令覆盖。 + - [ ] 补齐 `accept_cmd`、`received_cmd`、`sysrdy`、`rbt_inmotion` 状态位闭环检查。 + - [ ] 校验序号递增、响应滞后、丢包、停止包和最后一帧语义。 + - [ ] 将飞拍 IO 触发的 `write_io_type/index/mask/value` 与现场控制柜实际 IO 地址逐项对齐。 + +6. 真机联调与运行安全 + - [ ] 在真实 R30iB + `RVBUSTSM` 程序上验证 `Connect -> EnableRobot -> ExecuteFlyShotTraj -> StopMove -> DisableRobot -> Disconnect` 全流程。 + - [ ] 增加急停、伺服未就绪、程序未启动、网络断开、控制柜拒收命令等故障路径处理。 + - [ ] 给 HTTP 执行接口增加运行互斥、执行中拒绝重复轨迹、取消和超时控制。 + - [ ] 增加运行日志、协议摘要日志和状态快照导出，便于现场排查。 + +7. 发布与部署 + - [ ] 固化 Windows / Linux 启动脚本和 systemd 服务配置。 + - [ ] 补充生产配置模板、端口说明和现场部署检查表。 + - [ ] 给 Web 状态页增加真机连接、程序状态、J519 状态位和最近报警显示。 diff --git a/TrajectoryStore/FANUC_LR_Mate_200iD_trajectories.json b/TrajectoryStore/FANUC_LR_Mate_200iD_trajectories.json new file mode 100644 index 0000000..998e6ab --- /dev/null +++ b/TrajectoryStore/FANUC_LR_Mate_200iD_trajectories.json @@ -0,0 +1,11 @@ +{ + "robot": { + "use_do": false, + "io_addr": [], + "io_keep_cycles": 2, + "acc_limit": 1, + "jerk_limit": 1, + "adapt_icsp_try_num": 5 + }, + "flying_shots": {} +} \ No newline at end of file diff --git a/src/Flyshot.ControllerClientCompat/ControllerClientCompatRobotCatalog.cs b/src/Flyshot.ControllerClientCompat/ControllerClientCompatRobotCatalog.cs index e99a69c..6ab5e35 100644 --- a/src/Flyshot.ControllerClientCompat/ControllerClientCompatRobotCatalog.cs +++ b/src/Flyshot.ControllerClientCompat/ControllerClientCompatRobotCatalog.cs @@ -37,8 +37,10 @@ public sealed class ControllerClientCompatRobotCatalog /// 根据旧客户端的机器人名称加载对应模型。 /// /// 旧客户端传入的机器人名称。 + /// RobotConfig.json 中的加速度倍率。 + /// RobotConfig.json 中的 jerk 倍率。 /// 兼容层加载出的机器人模型。 - public RobotProfile LoadProfile(string robotName) + public RobotProfile LoadProfile(string robotName, double accLimitScale = 1.0, double jerkLimitScale = 1.0) { if (string.IsNullOrWhiteSpace(robotName)) { @@ -52,7 +54,7 @@ public sealed class ControllerClientCompatRobotCatalog var workspaceRoot = ResolveWorkspaceRoot(); var modelPath = Path.Combine(workspaceRoot, modelRelativePath); - return _robotModelLoader.LoadProfile(modelPath); + return _robotModelLoader.LoadProfile(modelPath, accLimitScale, jerkLimitScale); } /// diff --git a/src/Flyshot.ControllerClientCompat/ControllerClientCompatService.cs b/src/Flyshot.ControllerClientCompat/ControllerClientCompatService.cs index ef55b1c..4ef93af 100644 --- a/src/Flyshot.ControllerClientCompat/ControllerClientCompatService.cs +++ b/src/Flyshot.ControllerClientCompat/ControllerClientCompatService.cs @@ -1,3 +1,4 @@ +using Flyshot.Core.Config; using Flyshot.Core.Domain; using Flyshot.Runtime.Common; @@ -14,8 +15,11 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi private readonly ControllerClientCompatRobotCatalog _robotCatalog; private readonly IControllerRuntime _runtime; private readonly ControllerClientTrajectoryOrchestrator _trajectoryOrchestrator; + private readonly RobotConfigLoader _configLoader; + private readonly IFlyshotTrajectoryStore _trajectoryStore; private RobotProfile? _activeRobotProfile; private string? _configuredRobotName; + private CompatibilityRobotSettings? _robotSettings; private string? _connectedServerIp; private int _connectedServerPort; private bool _showTcp = true; @@ -29,16 +33,22 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi /// 机器人模型目录。 /// 控制器运行时。 /// 轨迹规划与触发编排器。 + /// 旧版 RobotConfig.json 加载器。 + /// 已上传轨迹持久化存储。 public ControllerClientCompatService( ControllerClientCompatOptions options, ControllerClientCompatRobotCatalog robotCatalog, IControllerRuntime runtime, - ControllerClientTrajectoryOrchestrator trajectoryOrchestrator) + ControllerClientTrajectoryOrchestrator trajectoryOrchestrator, + RobotConfigLoader configLoader, + IFlyshotTrajectoryStore trajectoryStore) { _options = options ?? throw new ArgumentNullException(nameof(options)); _robotCatalog = robotCatalog ?? throw new ArgumentNullException(nameof(robotCatalog)); _runtime = runtime ?? throw new ArgumentNullException(nameof(runtime)); _trajectoryOrchestrator = trajectoryOrchestrator ?? throw new ArgumentNullException(nameof(trajectoryOrchestrator)); + _configLoader = configLoader ?? throw new ArgumentNullException(nameof(configLoader)); + _trajectoryStore = trajectoryStore ?? throw new ArgumentNullException(nameof(trajectoryStore)); } /// @@ -97,7 +107,11 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi /// public void SetUpRobot(string robotName) { - var robotProfile = _robotCatalog.LoadProfile(robotName); + var robotSettings = TryLoadRobotSettings() ?? CreateDefaultRobotSettings(); + var robotProfile = _robotCatalog.LoadProfile( + robotName, + robotSettings.AccLimitScale, + robotSettings.JerkLimitScale); lock (_stateLock) { @@ -106,6 +120,14 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi _activeRobotProfile = robotProfile; _uploadedTrajectories.Clear(); _runtime.ResetRobot(robotProfile, robotName); + _robotSettings = robotSettings; + + // 从持久化存储恢复该机器人名下之前已上传的轨迹。 + var savedTrajectories = _trajectoryStore.LoadAll(robotName, out _); + foreach (var saved in savedTrajectories) + { + _uploadedTrajectories[saved.Key] = saved.Value; + } } } @@ -361,6 +383,10 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi { EnsureRuntimeEnabled(); _uploadedTrajectories[trajectory.Name] = trajectory; + + var robotName = _configuredRobotName ?? throw new InvalidOperationException("Robot has not been setup."); + var settings = _robotSettings ?? CreateDefaultRobotSettings(); + _trajectoryStore.Save(robotName, settings, trajectory); } } @@ -398,7 +424,7 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi } // 已上传飞拍轨迹必须按调用方指定 method 生成 shot timeline 后再交给运行时。 - var bundle = _trajectoryOrchestrator.PlanUploadedFlyshot(robot, trajectory, options); + var bundle = _trajectoryOrchestrator.PlanUploadedFlyshot(robot, trajectory, options, RequireRobotSettings()); if (options.MoveToStart) { _runtime.ExecuteTrajectory(CreateImmediateMoveResult(), bundle.PlannedTrajectory.PlannedWaypoints[0].Positions); @@ -425,11 +451,16 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi throw new InvalidOperationException("FlyShot trajectory does not exist."); } - // 当前阶段没有落地文件导出，先通过 saveTrajectory=true 走规划校验，避免静默接受非法参数。 + // 先通过规划校验避免静默接受非法参数，同时把轨迹信息强制刷写到本地 JSON。 _ = _trajectoryOrchestrator.PlanUploadedFlyshot( robot, trajectory, - new FlyshotExecutionOptions(saveTrajectory: true, method: method)); + new FlyshotExecutionOptions(saveTrajectory: true, method: method), + RequireRobotSettings()); + + var robotName = _configuredRobotName ?? throw new InvalidOperationException("Robot has not been setup."); + var settings = _robotSettings ?? CreateDefaultRobotSettings(); + _trajectoryStore.Save(robotName, settings, trajectory); } } @@ -452,7 +483,8 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi var bundle = _trajectoryOrchestrator.PlanUploadedFlyshot( robot, trajectory, - new FlyshotExecutionOptions(method: method, saveTrajectory: saveTrajectory)); + new FlyshotExecutionOptions(method: method, saveTrajectory: saveTrajectory), + RequireRobotSettings()); duration = bundle.Result.Duration; return bundle.Result.IsValid; @@ -473,6 +505,9 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi { throw new InvalidOperationException("DeleteFlyShotTraj failed"); } + + var robotName = _configuredRobotName ?? throw new InvalidOperationException("Robot has not been setup."); + _trajectoryStore.Delete(robotName, name); } } @@ -503,6 +538,15 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi return _activeRobotProfile ?? throw new InvalidOperationException("Robot has not been setup."); } + /// + /// 获取当前机器人兼容配置；未加载旧配置时回退到现场默认值。 + /// + /// 当前机器人配置。 + private CompatibilityRobotSettings RequireRobotSettings() + { + return _robotSettings ?? CreateDefaultRobotSettings(); + } + /// /// 校验机器人已经完成初始化。 /// @@ -542,4 +586,61 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi originalWaypointCount: 1, plannedWaypointCount: 1); } + + /// + /// 尝试从工作区加载旧版 RobotConfig.json 获取机器人配置；失败时返回 null。 + /// + /// 加载到的机器人配置，或 null。 + private CompatibilityRobotSettings? TryLoadRobotSettings() + { + try + { + var workspaceRoot = !string.IsNullOrWhiteSpace(_options.WorkspaceRoot) + ? Path.GetFullPath(_options.WorkspaceRoot) + : ResolveWorkspaceRootFromBaseDirectory(); + + var configPath = PathCompatibility.ResolveConfigPath("RobotConfig.json", workspaceRoot); + var loaded = _configLoader.Load(configPath, workspaceRoot); + return loaded.Robot; + } + catch + { + return null; + } + } + + /// + /// 构造与旧现场默认行为一致的机器人兼容配置。 + /// + /// 默认机器人配置。 + private static CompatibilityRobotSettings CreateDefaultRobotSettings() + { + return new CompatibilityRobotSettings( + useDo: false, + ioAddresses: Array.Empty(), + ioKeepCycles: 2, + accLimitScale: 1.0, + jerkLimitScale: 1.0, + adaptIcspTryNum: 5); + } + + /// + /// 从当前程序基目录向上搜索 FlyshotReplacement.sln 以推断工作区根目录。 + /// + /// 父工作区根目录。 + private static string ResolveWorkspaceRootFromBaseDirectory() + { + var current = new DirectoryInfo(AppContext.BaseDirectory); + while (current is not null) + { + if (File.Exists(Path.Combine(current.FullName, "FlyshotReplacement.sln"))) + { + return Path.GetFullPath(Path.Combine(current.FullName, "..")); + } + + current = current.Parent; + } + + throw new DirectoryNotFoundException("Unable to locate the flyshot workspace root."); + } } diff --git a/src/Flyshot.ControllerClientCompat/ControllerClientCompatServiceCollectionExtensions.cs b/src/Flyshot.ControllerClientCompat/ControllerClientCompatServiceCollectionExtensions.cs index 5e7a44a..0e2945f 100644 --- a/src/Flyshot.ControllerClientCompat/ControllerClientCompatServiceCollectionExtensions.cs +++ b/src/Flyshot.ControllerClientCompat/ControllerClientCompatServiceCollectionExtensions.cs @@ -29,8 +29,10 @@ public static class ControllerClientCompatServiceCollectionExtensions services.AddSingleton(static serviceProvider => serviceProvider.GetRequiredService>().Value); services.AddSingleton(); + services.AddSingleton(); services.AddSingleton(); services.AddSingleton(); + services.AddSingleton(); services.AddSingleton(); services.AddSingleton(); diff --git a/src/Flyshot.ControllerClientCompat/ControllerClientTrajectoryOrchestrator.cs b/src/Flyshot.ControllerClientCompat/ControllerClientTrajectoryOrchestrator.cs index 793f9af..a7013ec 100644 --- a/src/Flyshot.ControllerClientCompat/ControllerClientTrajectoryOrchestrator.cs +++ b/src/Flyshot.ControllerClientCompat/ControllerClientTrajectoryOrchestrator.cs @@ -1,5 +1,7 @@ +using Flyshot.Core.Config; using Flyshot.Core.Domain; using Flyshot.Core.Planning; +using Flyshot.Core.Planning.Sampling; using Flyshot.Core.Triggering; namespace Flyshot.ControllerClientCompat; @@ -59,11 +61,13 @@ public sealed class ControllerClientTrajectoryOrchestrator public PlannedExecutionBundle PlanUploadedFlyshot( RobotProfile robot, ControllerClientCompatUploadedTrajectory uploaded, - FlyshotExecutionOptions? options = null) + FlyshotExecutionOptions? options = null, + CompatibilityRobotSettings? settings = null) { ArgumentNullException.ThrowIfNull(robot); ArgumentNullException.ThrowIfNull(uploaded); options ??= new FlyshotExecutionOptions(); + settings ??= CreateDefaultRobotSettings(); var program = CreateProgram( name: uploaded.Name, @@ -73,7 +77,7 @@ public sealed class ControllerClientTrajectoryOrchestrator addressGroups: uploaded.AddressGroups); var method = ParseFlyshotMethod(options.Method); - var cacheKey = CreateFlyshotCacheKey(robot, uploaded, options); + var cacheKey = CreateFlyshotCacheKey(robot, uploaded, options, settings); if (options.UseCache && _flyshotCache.TryGetValue(cacheKey, out var cachedBundle)) { // 命中缓存时只替换 TrajectoryResult 的 usedCache 标志，规划轨迹和触发时间轴保持不可变复用。 @@ -91,11 +95,12 @@ public sealed class ControllerClientTrajectoryOrchestrator saveTrajectoryArtifacts: options.SaveTrajectory, useCache: options.UseCache); - var plannedTrajectory = PlanByMethod(request, method); + var plannedTrajectory = PlanByMethod(request, method, settings); var shotTimeline = _shotTimelineBuilder.Build( plannedTrajectory, - holdCycles: 0, - samplePeriod: robot.ServoPeriod); + holdCycles: settings.IoKeepCycles, + samplePeriod: robot.ServoPeriod, + useDo: settings.UseDo); var result = CreateResult(plannedTrajectory, shotTimeline, usedCache: false); var bundle = new PlannedExecutionBundle(plannedTrajectory, shotTimeline, result); @@ -146,12 +151,12 @@ public sealed class ControllerClientTrajectoryOrchestrator /// 规划请求。 /// 规划方法。 /// 规划轨迹。 - private PlannedTrajectory PlanByMethod(TrajectoryRequest request, PlanningMethod method) + private PlannedTrajectory PlanByMethod(TrajectoryRequest request, PlanningMethod method, CompatibilityRobotSettings? settings = null) { return method switch { PlanningMethod.Icsp => _icspPlanner.Plan(request), - PlanningMethod.SelfAdaptIcsp => _selfAdaptIcspPlanner.Plan(request), + PlanningMethod.SelfAdaptIcsp => _selfAdaptIcspPlanner.Plan(request, settings?.AdaptIcspTryNum ?? 5), PlanningMethod.Doubles => throw new NotSupportedException("doubles 轨迹规划尚未落地。"), _ => throw new ArgumentOutOfRangeException(nameof(method), method, "未知轨迹规划方法。") }; @@ -182,7 +187,8 @@ public sealed class ControllerClientTrajectoryOrchestrator private static string CreateFlyshotCacheKey( RobotProfile robot, ControllerClientCompatUploadedTrajectory uploaded, - FlyshotExecutionOptions options) + FlyshotExecutionOptions options, + CompatibilityRobotSettings settings) { var hash = new HashCode(); hash.Add(robot.Name, StringComparer.Ordinal); @@ -190,6 +196,9 @@ public sealed class ControllerClientTrajectoryOrchestrator hash.Add(NormalizeMethod(options.Method), StringComparer.Ordinal); hash.Add(options.MoveToStart); hash.Add(options.SaveTrajectory); + hash.Add(settings.UseDo); + hash.Add(settings.IoKeepCycles); + hash.Add(settings.AdaptIcspTryNum); foreach (var waypoint in uploaded.Waypoints) { @@ -220,6 +229,21 @@ public sealed class ControllerClientTrajectoryOrchestrator return hash.ToHashCode().ToString("X8"); } + /// + /// 构造编排器直接调用时的默认兼容配置，保持既有单元测试中的 DO 生成行为。 + /// + /// 默认机器人兼容配置。 + private static CompatibilityRobotSettings CreateDefaultRobotSettings() + { + return new CompatibilityRobotSettings( + useDo: true, + ioAddresses: Array.Empty(), + ioKeepCycles: 0, + accLimitScale: 1.0, + jerkLimitScale: 1.0, + adaptIcspTryNum: 5); + } + /// /// 把兼容层输入数组转换成领域层 FlyshotProgram。 /// @@ -252,6 +276,10 @@ public sealed class ControllerClientTrajectoryOrchestrator /// 运行时执行结果描述。 private static TrajectoryResult CreateResult(PlannedTrajectory plannedTrajectory, ShotTimeline shotTimeline, bool usedCache) { + var denseJointTrajectory = TrajectorySampler.SampleJointTrajectory( + plannedTrajectory, + samplePeriod: plannedTrajectory.Robot.ServoPeriod.TotalSeconds); + return new TrajectoryResult( programName: plannedTrajectory.OriginalProgram.Name, method: plannedTrajectory.Method, @@ -263,6 +291,7 @@ public sealed class ControllerClientTrajectoryOrchestrator failureReason: null, usedCache: usedCache, originalWaypointCount: plannedTrajectory.OriginalWaypointCount, - plannedWaypointCount: plannedTrajectory.PlannedWaypointCount); + plannedWaypointCount: plannedTrajectory.PlannedWaypointCount, + denseJointTrajectory: denseJointTrajectory); } } diff --git a/src/Flyshot.ControllerClientCompat/FlyshotTrajectoryStore.cs b/src/Flyshot.ControllerClientCompat/FlyshotTrajectoryStore.cs new file mode 100644 index 0000000..1cbcce6 --- /dev/null +++ b/src/Flyshot.ControllerClientCompat/FlyshotTrajectoryStore.cs @@ -0,0 +1,231 @@ +using System.Text.Json; +using System.Text.Json.Nodes; +using Flyshot.Core.Config; +using Flyshot.Core.Domain; + +namespace Flyshot.ControllerClientCompat; + +/// +/// 定义已上传飞拍轨迹的持久化存储契约。 +/// +public interface IFlyshotTrajectoryStore +{ + /// + /// 将单条轨迹持久化到本地 JSON，同时更新所属机器人配置段。 + /// + /// 当前已初始化的机器人名称。 + /// 当前机器人级兼容配置。 + /// 要保存的已上传轨迹。 + void Save(string robotName, CompatibilityRobotSettings settings, ControllerClientCompatUploadedTrajectory trajectory); + + /// + /// 从本地 JSON 删除指定名称的轨迹。 + /// + /// 当前已初始化的机器人名称。 + /// 要删除的轨迹名称。 + void Delete(string robotName, string trajectoryName); + + /// + /// 加载指定机器人名下所有已持久化的轨迹，并回传保存时的机器人配置。 + /// + /// 当前已初始化的机器人名称。 + /// 输出保存时的机器人配置；若文件不存在或解析失败则为 null。 + /// 按轨迹名称索引的已上传轨迹集合。 + IReadOnlyDictionary LoadAll(string robotName, out CompatibilityRobotSettings? settings); +} + +/// +/// 使用与旧版 RobotConfig.json 一致的 JSON 格式持久化飞拍轨迹和机器人配置。 +/// +public sealed class JsonFlyshotTrajectoryStore : IFlyshotTrajectoryStore +{ + private readonly ControllerClientCompatOptions _options; + private readonly RobotConfigLoader _configLoader; + + /// + /// 初始化基于 JSON 文件的轨迹存储。 + /// + /// 兼容层基础配置，用于定位工作区根目录。 + /// 旧版 RobotConfig.json 加载器，用于反序列化已保存的轨迹。 + public JsonFlyshotTrajectoryStore(ControllerClientCompatOptions options, RobotConfigLoader configLoader) + { + _options = options ?? throw new ArgumentNullException(nameof(options)); + _configLoader = configLoader ?? throw new ArgumentNullException(nameof(configLoader)); + } + + /// + public void Save(string robotName, CompatibilityRobotSettings settings, ControllerClientCompatUploadedTrajectory trajectory) + { + ArgumentNullException.ThrowIfNull(settings); + ArgumentNullException.ThrowIfNull(trajectory); + + var path = ResolveStorePath(robotName); + var directory = Path.GetDirectoryName(path)!; + Directory.CreateDirectory(directory); + + JsonObject root; + if (File.Exists(path)) + { + var existingJson = File.ReadAllText(path); + root = JsonNode.Parse(existingJson)?.AsObject() ?? new JsonObject(); + } + else + { + root = new JsonObject(); + } + + // 更新 robot 配置段，保持与旧版 RobotConfig.json 字段名一致。 + root["robot"] = SerializeRobotSettings(settings); + + // 确保 flying_shots 节点存在。 + if (!root.TryGetPropertyValue("flying_shots", out var flyingShotsNode) || flyingShotsNode is not JsonObject flyingShotsObj) + { + flyingShotsObj = new JsonObject(); + root["flying_shots"] = flyingShotsObj; + } + + flyingShotsObj[trajectory.Name] = SerializeTrajectory(trajectory); + + var writeOptions = new JsonSerializerOptions + { + WriteIndented = true, + PropertyNamingPolicy = JsonNamingPolicy.SnakeCaseLower + }; + File.WriteAllText(path, root.ToJsonString(writeOptions)); + } + + /// + public void Delete(string robotName, string trajectoryName) + { + if (string.IsNullOrWhiteSpace(trajectoryName)) + { + throw new ArgumentException("轨迹名称不能为空。", nameof(trajectoryName)); + } + + var path = ResolveStorePath(robotName); + if (!File.Exists(path)) + { + return; + } + + var existingJson = File.ReadAllText(path); + var root = JsonNode.Parse(existingJson)?.AsObject(); + if (root is null) + { + return; + } + + if (root.TryGetPropertyValue("flying_shots", out var flyingShotsNode) && flyingShotsNode is JsonObject flyingShotsObj) + { + flyingShotsObj.Remove(trajectoryName); + + var writeOptions = new JsonSerializerOptions + { + WriteIndented = true, + PropertyNamingPolicy = JsonNamingPolicy.SnakeCaseLower + }; + File.WriteAllText(path, root.ToJsonString(writeOptions)); + } + } + + /// + public IReadOnlyDictionary LoadAll(string robotName, out CompatibilityRobotSettings? settings) + { + var path = ResolveStorePath(robotName); + if (!File.Exists(path)) + { + settings = null; + return new Dictionary(StringComparer.Ordinal); + } + + try + { + var workspaceRoot = ResolveWorkspaceRoot(); + var loaded = _configLoader.Load(path, workspaceRoot); + settings = loaded.Robot; + + var dict = new Dictionary(StringComparer.Ordinal); + foreach (var program in loaded.Programs) + { + var traj = new ControllerClientCompatUploadedTrajectory( + name: program.Value.Name, + waypoints: program.Value.Waypoints.Select(static wp => wp.Positions), + shotFlags: program.Value.ShotFlags, + offsetValues: program.Value.OffsetValues, + addressGroups: program.Value.AddressGroups.Select(static g => g.Addresses)); + dict[program.Key] = traj; + } + + return dict; + } + catch + { + settings = null; + return new Dictionary(StringComparer.Ordinal); + } + } + + /// + /// 把机器人兼容配置序列化为 JSON 对象，字段名与旧版 RobotConfig.json 一致。 + /// + private static JsonObject SerializeRobotSettings(CompatibilityRobotSettings settings) + { + return new JsonObject + { + ["use_do"] = JsonValue.Create(settings.UseDo), + ["io_addr"] = JsonSerializer.SerializeToNode(settings.IoAddresses), + ["io_keep_cycles"] = JsonValue.Create(settings.IoKeepCycles), + ["acc_limit"] = JsonValue.Create(settings.AccLimitScale), + ["jerk_limit"] = JsonValue.Create(settings.JerkLimitScale), + ["adapt_icsp_try_num"] = JsonValue.Create(settings.AdaptIcspTryNum) + }; + } + + /// + /// 把已上传轨迹序列化为 JSON 对象，字段名与旧版 RobotConfig.json 的 flying_shots 节点一致。 + /// + private static JsonObject SerializeTrajectory(ControllerClientCompatUploadedTrajectory trajectory) + { + return new JsonObject + { + ["traj_waypoints"] = JsonSerializer.SerializeToNode(trajectory.Waypoints), + ["shot_flags"] = JsonSerializer.SerializeToNode(trajectory.ShotFlags), + ["offset_values"] = JsonSerializer.SerializeToNode(trajectory.OffsetValues), + ["addr"] = JsonSerializer.SerializeToNode(trajectory.AddressGroups) + }; + } + + /// + /// 解析当前机器人对应的持久化文件路径。 + /// + private string ResolveStorePath(string robotName) + { + var workspaceRoot = ResolveWorkspaceRoot(); + var storeDir = Path.Combine(workspaceRoot, "flyshot-replacement", "TrajectoryStore"); + return Path.Combine(storeDir, $"{robotName}_trajectories.json"); + } + + /// + /// 解析父工作区根目录，优先使用显式配置。 + /// + private string ResolveWorkspaceRoot() + { + if (!string.IsNullOrWhiteSpace(_options.WorkspaceRoot)) + { + return Path.GetFullPath(_options.WorkspaceRoot); + } + + var current = new DirectoryInfo(AppContext.BaseDirectory); + while (current is not null) + { + if (File.Exists(Path.Combine(current.FullName, "FlyshotReplacement.sln"))) + { + return Path.GetFullPath(Path.Combine(current.FullName, "..")); + } + + current = current.Parent; + } + + throw new DirectoryNotFoundException("Unable to locate the flyshot workspace root."); + } +} diff --git a/src/Flyshot.Core.Domain/TrajectoryResult.cs b/src/Flyshot.Core.Domain/TrajectoryResult.cs index e0ada1c..7c55e6c 100644 --- a/src/Flyshot.Core.Domain/TrajectoryResult.cs +++ b/src/Flyshot.Core.Domain/TrajectoryResult.cs @@ -21,7 +21,8 @@ public sealed class TrajectoryResult string? failureReason, bool usedCache, int originalWaypointCount, - int plannedWaypointCount) + int plannedWaypointCount, + IEnumerable>? denseJointTrajectory = null) { if (string.IsNullOrWhiteSpace(programName)) { @@ -51,6 +52,7 @@ public sealed class TrajectoryResult var copiedShotEvents = shotEvents.ToArray(); var copiedTriggerTimeline = triggerTimeline.ToArray(); var copiedArtifacts = artifacts.ToArray(); + var copiedDenseJointTrajectory = denseJointTrajectory?.Select(static row => row.ToArray()).ToArray(); ProgramName = programName; Method = method; @@ -63,6 +65,7 @@ public sealed class TrajectoryResult UsedCache = usedCache; OriginalWaypointCount = originalWaypointCount; PlannedWaypointCount = plannedWaypointCount; + DenseJointTrajectory = copiedDenseJointTrajectory; } /// @@ -130,6 +133,13 @@ public sealed class TrajectoryResult /// [JsonPropertyName("plannedWaypointCount")] public int PlannedWaypointCount { get; } + + /// + /// Gets the dense joint trajectory samples where each row is [time, j1, j2, ...]. + /// Null when dense sampling was not performed (e.g. simulation fallback). + /// + [JsonPropertyName("denseJointTrajectory")] + public IReadOnlyList>? DenseJointTrajectory { get; } } /// diff --git a/src/Flyshot.Core.Triggering/ShotTimelineBuilder.cs b/src/Flyshot.Core.Triggering/ShotTimelineBuilder.cs index f95778e..7d91b67 100644 --- a/src/Flyshot.Core.Triggering/ShotTimelineBuilder.cs +++ b/src/Flyshot.Core.Triggering/ShotTimelineBuilder.cs @@ -25,8 +25,9 @@ public sealed class ShotTimelineBuilder /// 规划后的轨迹（含补中点信息和机器人配置）。 /// IO 保持周期数（对应原系统的 io_keep_cycles）。 /// 稠密采样周期，用于离散化 sample_index 和 sample_time。 + /// 是否生成可注入伺服流的 DO 事件。 /// 包含 ShotEvent 和 TrajectoryDoEvent 的触发时间轴。 - public ShotTimeline Build(PlannedTrajectory trajectory, int holdCycles, TimeSpan samplePeriod) + public ShotTimeline Build(PlannedTrajectory trajectory, int holdCycles, TimeSpan samplePeriod, bool useDo = true) { ArgumentNullException.ThrowIfNull(trajectory); @@ -69,12 +70,16 @@ public sealed class ShotTimelineBuilder sampleTime: sampleTime, addressGroup: addressGroup)); - triggerTimeline.Add(new TrajectoryDoEvent( - waypointIndex: i, - triggerTime: triggerTime, - offsetCycles: program.OffsetValues[i], - holdCycles: holdCycles, - addressGroup: addressGroup)); + if (useDo) + { + // use_do=false 时保留 ShotEvent 诊断信息，但不向运行时下发 IO 脉冲。 + triggerTimeline.Add(new TrajectoryDoEvent( + waypointIndex: i, + triggerTime: triggerTime, + offsetCycles: program.OffsetValues[i], + holdCycles: holdCycles, + addressGroup: addressGroup)); + } } return new ShotTimeline(shotEvents, triggerTimeline); diff --git a/src/Flyshot.Runtime.Fanuc/FanucControllerRuntime.cs b/src/Flyshot.Runtime.Fanuc/FanucControllerRuntime.cs index 782331b..5b45b10 100644 --- a/src/Flyshot.Runtime.Fanuc/FanucControllerRuntime.cs +++ b/src/Flyshot.Runtime.Fanuc/FanucControllerRuntime.cs @@ -1,3 +1,4 @@ +using System.Diagnostics; using Flyshot.Core.Domain; using Flyshot.Runtime.Common; using Flyshot.Runtime.Fanuc.Protocol; @@ -12,9 +13,9 @@ public sealed class FanucControllerRuntime : IControllerRuntime, IDisposable { private readonly object _stateLock = new(); private readonly Dictionary<(string IoType, int Port), bool> _ioValues = new(); - private readonly FanucCommandClient _commandClient = new(); - private readonly FanucStateClient _stateClient = new(); - private readonly FanucJ519Client _j519Client = new(); + private readonly FanucCommandClient _commandClient; + private readonly FanucStateClient _stateClient; + private readonly FanucJ519Client _j519Client; private RobotProfile? _robot; private string? _robotName; @@ -28,6 +29,28 @@ public sealed class FanucControllerRuntime : IControllerRuntime, IDisposable private double[] _jointPositions = Array.Empty(); 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 控制器运行时。 + /// + public FanucControllerRuntime() + { + _commandClient = new FanucCommandClient(); + _stateClient = new FanucStateClient(); + _j519Client = new FanucJ519Client(); + } + + /// + /// 供测试注入 mock 客户端的内部构造函数。 + /// + internal FanucControllerRuntime(FanucCommandClient commandClient, FanucStateClient stateClient, FanucJ519Client j519Client) + { + _commandClient = commandClient; + _stateClient = stateClient; + _j519Client = j519Client; + } /// public void ResetRobot(RobotProfile robot, string robotName) @@ -106,6 +129,7 @@ public sealed class FanucControllerRuntime : IControllerRuntime, IDisposable lock (_stateLock) { EnsureRobotSetup(); + CancelSendTaskLocked(); DisconnectClients(); _connectedRobotIp = null; _isEnabled = false; @@ -149,6 +173,7 @@ public sealed class FanucControllerRuntime : IControllerRuntime, IDisposable lock (_stateLock) { EnsureRobotSetup(); + CancelSendTaskLocked(); if (!IsSimulationMode) { _j519Client.StopMotionAsync().GetAwaiter().GetResult(); @@ -166,6 +191,7 @@ public sealed class FanucControllerRuntime : IControllerRuntime, IDisposable lock (_stateLock) { EnsureRobotSetup(); + CancelSendTaskLocked(); if (!IsSimulationMode) { _j519Client.StopMotionAsync().GetAwaiter().GetResult(); @@ -347,11 +373,21 @@ public sealed class FanucControllerRuntime : IControllerRuntime, IDisposable EnsureEnabled(); EnsureValidTrajectory(result); EnsureJointCount(finalJointPositions.Count); + CancelSendTaskLocked(); + + if (!IsSimulationMode && result.DenseJointTrajectory is not null) + { + // 真机模式且存在稠密路点：启动后台高精度发送任务。 + _isInMotion = true; + _sendCts = new CancellationTokenSource(); + var ct = _sendCts.Token; + _sendTask = Task.Run(() => SendDenseTrajectory(result, finalJointPositions, ct), ct); + return; + } if (!IsSimulationMode) { - // 真机模式：通过 J519 发送最终关节目标。 - // TODO: 后续接入稠密路点流，当前先发送单点收敛。 + // 真机模式无稠密路点：回退到单点收敛。 var command = new FanucJ519Command( sequence: 0, targetJoints: finalJointPositions.Select(j => (double)j).ToArray()); @@ -375,12 +411,138 @@ public sealed class FanucControllerRuntime : IControllerRuntime, IDisposable } _disposed = true; + CancelSendTaskLocked(); DisconnectClients(); _commandClient.Dispose(); _stateClient.Dispose(); _j519Client.Dispose(); } + /// + /// 后台高精度发送任务：按伺服周期遍历稠密路点并注入 IO 触发。 + /// + private void SendDenseTrajectory(TrajectoryResult result, IReadOnlyList finalJointPositions, CancellationToken cancellationToken) + { + var denseTrajectory = result.DenseJointTrajectory!; + var triggers = result.TriggerTimeline; + var servoPeriodSeconds = _robot!.ServoPeriod.TotalSeconds; + var halfServoPeriod = servoPeriodSeconds / 2.0; + var periodTicks = (long)(servoPeriodSeconds * Stopwatch.Frequency); + + var stopwatch = Stopwatch.StartNew(); + long nextTick = stopwatch.ElapsedTicks; + uint sequence = 0; + ushort ioValue = 0; + int holdRemaining = -1; + + try + { + foreach (var row in denseTrajectory) + { + cancellationToken.ThrowIfCancellationRequested(); + nextTick += periodTicks; + + double t = row[0]; + var joints = row.Skip(1).Select(static v => (double)v).ToArray(); + + // 递减 IO 保持计数器；若已到期则清零。 + if (holdRemaining > 0) + { + holdRemaining--; + } + else if (holdRemaining == 0) + { + ioValue = 0; + holdRemaining = -1; + } + + // 检查当前周期是否有新的触发事件。 + if (holdRemaining < 0) + { + foreach (var trigger in triggers) + { + if (Math.Abs(t - trigger.TriggerTime) < halfServoPeriod) + { + ioValue = ComputeIoValue(trigger.AddressGroup); + holdRemaining = trigger.HoldCycles; + break; + } + } + } + + var command = new FanucJ519Command( + sequence: sequence++, + targetJoints: joints, + writeIoType: 2, + writeIoIndex: 1, + writeIoMask: 255, + writeIoValue: ioValue); + + _j519Client.UpdateCommand(command); + + // 高精度忙等待直到下一伺服周期。 + while (stopwatch.ElapsedTicks < nextTick) + { + Thread.SpinWait(1); + } + } + } + catch (OperationCanceledException) + { + // 正常取消，轨迹被中断。 + } + finally + { + lock (_stateLock) + { + _isInMotion = false; + _jointPositions = finalJointPositions.ToArray(); + } + } + } + + /// + /// 取消并等待当前后台发送任务，避免旧任务与新轨迹并发。 + /// + private void CancelSendTaskLocked() + { + _sendCts?.Cancel(); + + if (_sendTask is not null) + { + try + { + _sendTask.Wait(TimeSpan.FromSeconds(2)); + } + catch (AggregateException) + { + // 忽略取消异常。 + } + + _sendTask = null; + } + + _sendCts?.Dispose(); + _sendCts = null; + } + + /// + /// 把 IO 地址组中的地址号映射为 writeIoValue 的位掩码。 + /// + internal static ushort ComputeIoValue(IoAddressGroup group) + { + ushort value = 0; + foreach (var addr in group.Addresses) + { + if (addr is >= 0 and < 16) + { + value |= (ushort)(1 << addr); + } + } + + return value; + } + /// /// 判断当前是否处于仿真模式；若尚未选择控制器则抛出异常。 /// diff --git a/src/Flyshot.Runtime.Fanuc/Flyshot.Runtime.Fanuc.csproj b/src/Flyshot.Runtime.Fanuc/Flyshot.Runtime.Fanuc.csproj index e637074..707afe4 100644 --- a/src/Flyshot.Runtime.Fanuc/Flyshot.Runtime.Fanuc.csproj +++ b/src/Flyshot.Runtime.Fanuc/Flyshot.Runtime.Fanuc.csproj @@ -6,6 +6,12 @@ enable + + + <_Parameter1>Flyshot.Core.Tests + + + diff --git a/src/Flyshot.Runtime.Fanuc/Protocol/FanucCommandClient.cs b/src/Flyshot.Runtime.Fanuc/Protocol/FanucCommandClient.cs index d4e0523..ad44b79 100644 --- a/src/Flyshot.Runtime.Fanuc/Protocol/FanucCommandClient.cs +++ b/src/Flyshot.Runtime.Fanuc/Protocol/FanucCommandClient.cs @@ -96,7 +96,7 @@ public sealed class FanucCommandClient : IDisposable { var frame = FanucCommandProtocol.PackProgramCommand(messageId, programName); var response = await SendRawFrameAsync(frame, cancellationToken).ConfigureAwait(false); - return FanucCommandProtocol.ParseResultResponse(response); + return EnsureSuccess(FanucCommandProtocol.ParseResultResponse(response)); } /// @@ -119,7 +119,7 @@ public sealed class FanucCommandClient : IDisposable { var frame = FanucCommandProtocol.PackEmptyCommand(FanucCommandMessageIds.ResetRobot); var response = await SendRawFrameAsync(frame, cancellationToken).ConfigureAwait(false); - return FanucCommandProtocol.ParseResultResponse(response); + return EnsureSuccess(FanucCommandProtocol.ParseResultResponse(response)); } /// @@ -132,7 +132,7 @@ public sealed class FanucCommandClient : IDisposable { var frame = FanucCommandProtocol.PackProgramCommand(FanucCommandMessageIds.GetProgramStatus, programName); var response = await SendRawFrameAsync(frame, cancellationToken).ConfigureAwait(false); - return FanucCommandProtocol.ParseProgramStatusResponse(response); + return EnsureSuccess(FanucCommandProtocol.ParseProgramStatusResponse(response)); } /// @@ -186,6 +186,41 @@ public sealed class FanucCommandClient : IDisposable } } + /// + /// 校验普通命令响应结果码，失败时抛出包含消息号和结果码的诊断异常。 + /// + private static FanucCommandResultResponse EnsureSuccess(FanucCommandResultResponse response) + { + if (!response.IsSuccess) + { + throw CreateCommandFailureException(response.MessageId, response.ResultCode); + } + + return response; + } + + /// + /// 校验程序状态响应结果码，失败时抛出包含消息号和结果码的诊断异常。 + /// + private static FanucProgramStatusResponse EnsureSuccess(FanucProgramStatusResponse response) + { + if (!response.IsSuccess) + { + throw CreateCommandFailureException(response.MessageId, response.ResultCode); + } + + return response; + } + + /// + /// 构造包含 FANUC 命令上下文的失败异常。 + /// + private static InvalidOperationException CreateCommandFailureException(uint messageId, uint resultCode) + { + return new InvalidOperationException( + $"FANUC command 0x{messageId:X4} failed with result_code {resultCode}."); + } + /// /// 从流中读取一条完整的 doz/zod 响应帧。 /// diff --git a/src/Flyshot.Runtime.Fanuc/Protocol/FanucJ519Client.cs b/src/Flyshot.Runtime.Fanuc/Protocol/FanucJ519Client.cs index 81da460..0a10926 100644 --- a/src/Flyshot.Runtime.Fanuc/Protocol/FanucJ519Client.cs +++ b/src/Flyshot.Runtime.Fanuc/Protocol/FanucJ519Client.cs @@ -1,3 +1,4 @@ +using System.Diagnostics; using System.Net.Sockets; namespace Flyshot.Runtime.Fanuc.Protocol; @@ -123,6 +124,20 @@ public sealed class FanucJ519Client : IDisposable } } + /// + /// 获取最近一次通过 UpdateCommand 设置的 J519 命令；供测试断言使用。 + /// + /// 当前 J519 命令或 null。 + internal FanucJ519Command? GetCurrentCommand() + { + ObjectDisposedException.ThrowIf(_disposed, this); + + lock (_commandLock) + { + return _currentCommand; + } + } + /// /// 获取最近一次解析的 J519 响应；若尚未收到任何响应则返回 null。 /// @@ -225,7 +240,7 @@ public sealed class FanucJ519Client : IDisposable } /// - /// 后台发送循环：约 8ms 周期发送当前命令。 + /// 后台发送循环：以 Stopwatch + SpinWait 实现高精度 8ms 周期发送当前命令。 /// private async Task SendLoopAsync(CancellationToken cancellationToken) { @@ -234,13 +249,17 @@ public sealed class FanucJ519Client : IDisposable return; } - // 使用 8ms 周期近似 125Hz 伺服频率。 - using var timer = new PeriodicTimer(TimeSpan.FromMilliseconds(8)); + // 8ms 伺服周期，对应 125Hz。 + var periodTicks = (long)(0.008 * Stopwatch.Frequency); + var stopwatch = Stopwatch.StartNew(); + long nextTick = stopwatch.ElapsedTicks; try { - while (await timer.WaitForNextTickAsync(cancellationToken).ConfigureAwait(false)) + while (!cancellationToken.IsCancellationRequested) { + nextTick += periodTicks; + FanucJ519Command? command; lock (_commandLock) { @@ -252,6 +271,12 @@ public sealed class FanucJ519Client : IDisposable var packet = FanucJ519Protocol.PackCommandPacket(command); await _udpClient.SendAsync(packet, cancellationToken).ConfigureAwait(false); } + + // 高精度忙等待直到下一周期，避免 PeriodicTimer 的 ±15ms 抖动。 + while (stopwatch.ElapsedTicks < nextTick) + { + Thread.SpinWait(1); + } } } catch (OperationCanceledException) diff --git a/tests/Flyshot.Core.Tests/ConfigCompatibilityTests.cs b/tests/Flyshot.Core.Tests/ConfigCompatibilityTests.cs index ac3b989..083be66 100644 --- a/tests/Flyshot.Core.Tests/ConfigCompatibilityTests.cs +++ b/tests/Flyshot.Core.Tests/ConfigCompatibilityTests.cs @@ -26,12 +26,12 @@ public sealed class ConfigCompatibilityTests Assert.Equal(1.0, loaded.Robot.JerkLimitScale); Assert.Equal(5, loaded.Robot.AdaptIcspTryNum); - var program = Assert.Contains("001", loaded.Programs); - Assert.Equal("001", program.Name); - Assert.Equal(5, program.Waypoints.Count); - Assert.Equal(3, program.ShotWaypointCount); + var program = Assert.Contains("EOL10_EAU_0", loaded.Programs); + Assert.Equal("EOL10_EAU_0", program.Name); + Assert.Equal(45, program.Waypoints.Count); + Assert.Equal(42, program.ShotWaypointCount); Assert.Empty(program.AddressGroups[0].Addresses); - Assert.Equal([8, 7], program.AddressGroups[1].Addresses); + Assert.Equal([4, 3], program.AddressGroups[1].Addresses); } /// diff --git a/tests/Flyshot.Core.Tests/FanucCommandClientTests.cs b/tests/Flyshot.Core.Tests/FanucCommandClientTests.cs index c1c0ce7..fd98477 100644 --- a/tests/Flyshot.Core.Tests/FanucCommandClientTests.cs +++ b/tests/Flyshot.Core.Tests/FanucCommandClientTests.cs @@ -130,6 +130,27 @@ public sealed class FanucCommandClientTests : IDisposable await handlerTask.WaitAsync(TimeSpan.FromSeconds(2), _cts.Token); } + /// + /// 验证命令响应 result_code 非零时，客户端会抛出可诊断异常而不是让上层误判成功。 + /// + [Fact] + public async Task StopProgramAsync_NonZeroResultCode_ThrowsDiagnosticException() + { + using var client = new FanucCommandClient(); + var handlerTask = RunSingleResponseControllerAsync( + FanucCommandProtocol.PackProgramCommand(FanucCommandMessageIds.StopProgram, "RVBUSTSM"), + Convert.FromHexString("646f7a00000012000021030000002a7a6f64"), + _cts.Token); + + await client.ConnectAsync("127.0.0.1", Port, _cts.Token); + var exception = await Assert.ThrowsAsync( + () => client.StopProgramAsync("RVBUSTSM", _cts.Token)); + + Assert.Contains("0x2103", exception.Message); + Assert.Contains("42", exception.Message); + await handlerTask.WaitAsync(TimeSpan.FromSeconds(2), _cts.Token); + } + /// /// 验证在连接前调用命令会抛出 InvalidOperationException。 /// diff --git a/tests/Flyshot.Core.Tests/FanucControllerRuntimeDenseTests.cs b/tests/Flyshot.Core.Tests/FanucControllerRuntimeDenseTests.cs new file mode 100644 index 0000000..73af296 --- /dev/null +++ b/tests/Flyshot.Core.Tests/FanucControllerRuntimeDenseTests.cs @@ -0,0 +1,96 @@ +using Flyshot.Core.Domain; +using Flyshot.Runtime.Fanuc; + +namespace Flyshot.Core.Tests; + +/// +/// 验证 FANUC 控制器运行在稠密轨迹流式执行与 IO 触发上的行为。 +/// +public sealed class FanucControllerRuntimeDenseTests +{ + /// + /// 验证仿真模式下即使传入稠密路点，也会回退到单点同步更新。 + /// + [Fact] + public void ExecuteTrajectory_WithDenseWaypoints_SimulationMode_FallsBackToSinglePoint() + { + var runtime = new FanucControllerRuntime(); + var robot = TestRobotFactory.CreateRobotProfile(); + runtime.ResetRobot(robot, "FANUC_LR_Mate_200iD"); + runtime.SetActiveController(sim: true); + runtime.Connect("192.168.10.101"); + runtime.EnableRobot(bufferSize: 2); + + var denseTrajectory = new[] + { + new[] { 0.0, 0.1, 0.2, 0.3, 0.4, 0.5, 0.6 }, + new[] { 0.008, 0.11, 0.21, 0.31, 0.41, 0.51, 0.61 }, + new[] { 0.016, 0.12, 0.22, 0.32, 0.42, 0.52, 0.62 } + }; + + var result = new TrajectoryResult( + programName: "demo", + 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); + + runtime.ExecuteTrajectory(result, [0.12, 0.22, 0.32, 0.42, 0.52, 0.62]); + + var snapshot = runtime.GetSnapshot(); + Assert.False(snapshot.IsInMotion); + Assert.Equal([0.12, 0.22, 0.32, 0.42, 0.52, 0.62], snapshot.JointPositions); + } + + /// + /// 验证 StopMove 在没有任何后台发送任务运行时不会抛出异常。 + /// + [Fact] + public void StopMove_DoesNotThrowWhenNoSendTaskRunning() + { + var runtime = new FanucControllerRuntime(); + var robot = TestRobotFactory.CreateRobotProfile(); + runtime.ResetRobot(robot, "FANUC_LR_Mate_200iD"); + runtime.SetActiveController(sim: true); + runtime.Connect("192.168.10.101"); + runtime.EnableRobot(bufferSize: 2); + + var exception = Record.Exception(() => runtime.StopMove()); + Assert.Null(exception); + Assert.False(runtime.GetSnapshot().IsInMotion); + } + + /// + /// 验证 IO 地址组中的地址号被正确映射为 writeIoValue 的位掩码。 + /// + [Theory] + [InlineData(new[] { 0 }, (ushort)1)] + [InlineData(new[] { 7 }, (ushort)128)] + [InlineData(new[] { 7, 8 }, (ushort)384)] // 128 + 256 + [InlineData(new[] { 15 }, (ushort)32768)] + [InlineData(new int[] { }, (ushort)0)] + public void ComputeIoValue_MapsAddressesToBitMask(int[] addresses, ushort expected) + { + var group = new IoAddressGroup(addresses); + var actual = FanucControllerRuntime.ComputeIoValue(group); + Assert.Equal(expected, actual); + } + + /// + /// 验证超过 15 的地址号会被安全忽略，不会溢出位掩码。 + /// + [Fact] + public void ComputeIoValue_IgnoresOutOfRangeAddresses() + { + var group = new IoAddressGroup([0, 16, 7]); + var actual = FanucControllerRuntime.ComputeIoValue(group); + Assert.Equal((ushort)(1 | 128), actual); + } +} diff --git a/tests/Flyshot.Core.Tests/FanucJ519ClientTests.cs b/tests/Flyshot.Core.Tests/FanucJ519ClientTests.cs index d453581..c98788e 100644 --- a/tests/Flyshot.Core.Tests/FanucJ519ClientTests.cs +++ b/tests/Flyshot.Core.Tests/FanucJ519ClientTests.cs @@ -177,4 +177,43 @@ public sealed class FanucJ519ClientTests : IDisposable using var client = new FanucJ519Client(); Assert.Throws(() => client.StartMotion()); } + + /// + /// 验证 Stopwatch + SpinWait 发送循环能保持约 8ms 周期抖动在亚毫秒级。 + /// + [Fact] + public async Task StartMotion_MaintainsSubMillisecondPeriod() + { + using var client = new FanucJ519Client(); + await client.ConnectAsync("127.0.0.1", Port, _cts.Token); + await _server.ReceiveAsync(_cts.Token); // init + + var command = new FanucJ519Command(sequence: 1, targetJoints: [1.0, 2.0, 3.0, 4.0, 5.0, 6.0]); + client.UpdateCommand(command); + client.StartMotion(); + + // 收集 5 个命令包到达时间戳。 + var timestamps = new List(); + for (var i = 0; i < 5; i++) + { + var result = await _server.ReceiveAsync(_cts.Token); + timestamps.Add(DateTimeOffset.UtcNow); + } + + await client.StopMotionAsync(_cts.Token); + + // 计算相邻包间隔并断言最大抖动。 + var intervals = new List(); + for (var i = 1; i < timestamps.Count; i++) + { + intervals.Add(timestamps[i] - timestamps[i - 1]); + } + + // 允许 ±2ms 的测量误差（含 UDP 传输和调度延迟）。 + Assert.All(intervals, interval => + { + Assert.True(interval >= TimeSpan.FromMilliseconds(6), $"间隔 {interval.TotalMilliseconds:F2}ms 过短。"); + Assert.True(interval <= TimeSpan.FromMilliseconds(10), $"间隔 {interval.TotalMilliseconds:F2}ms 过长。"); + }); + } } diff --git a/tests/Flyshot.Core.Tests/RuntimeOrchestrationTests.cs b/tests/Flyshot.Core.Tests/RuntimeOrchestrationTests.cs index f82519d..b169955 100644 --- a/tests/Flyshot.Core.Tests/RuntimeOrchestrationTests.cs +++ b/tests/Flyshot.Core.Tests/RuntimeOrchestrationTests.cs @@ -1,6 +1,7 @@ using Flyshot.ControllerClientCompat; using Flyshot.Core.Config; using Flyshot.Core.Domain; +using Flyshot.Runtime.Common; using Flyshot.Runtime.Fanuc; namespace Flyshot.Core.Tests; @@ -82,6 +83,104 @@ public sealed class RuntimeOrchestrationTests Assert.Single(bundle.Result.TriggerTimeline); } + /// + /// 验证飞拍编排会使用 RobotConfig.json 中的 IO 保持周期。 + /// + [Fact] + public void ControllerClientTrajectoryOrchestrator_PlanUploadedFlyshot_UsesRobotSettingsForHoldCycles() + { + var orchestrator = new ControllerClientTrajectoryOrchestrator(); + var robot = TestRobotFactory.CreateRobotProfile(); + var uploaded = TestRobotFactory.CreateUploadedTrajectoryWithSingleShot(); + var settings = new CompatibilityRobotSettings( + useDo: true, + ioAddresses: [7, 8], + ioKeepCycles: 4, + accLimitScale: 1.0, + jerkLimitScale: 1.0, + adaptIcspTryNum: 5); + + var bundle = orchestrator.PlanUploadedFlyshot(robot, uploaded, settings: settings); + + var doEvent = Assert.Single(bundle.Result.TriggerTimeline); + Assert.Equal(4, doEvent.HoldCycles); + } + + /// + /// 验证 RobotConfig.json 关闭 use_do 时仍保留 ShotEvent 诊断信息，但不生成伺服 DO 事件。 + /// + [Fact] + public void ControllerClientTrajectoryOrchestrator_PlanUploadedFlyshot_SuppressesDoTimeline_WhenUseDoIsFalse() + { + var orchestrator = new ControllerClientTrajectoryOrchestrator(); + var robot = TestRobotFactory.CreateRobotProfile(); + var uploaded = TestRobotFactory.CreateUploadedTrajectoryWithSingleShot(); + var settings = new CompatibilityRobotSettings( + useDo: false, + ioAddresses: [7, 8], + ioKeepCycles: 4, + accLimitScale: 1.0, + jerkLimitScale: 1.0, + adaptIcspTryNum: 5); + + var bundle = orchestrator.PlanUploadedFlyshot(robot, uploaded, settings: settings); + + Assert.Single(bundle.Result.ShotEvents); + Assert.Empty(bundle.Result.TriggerTimeline); + } + + /// + /// 验证普通轨迹规划后会生成稠密关节采样序列。 + /// + [Fact] + public void ControllerClientTrajectoryOrchestrator_PlanOrdinaryTrajectory_ReturnsDenseJointTrajectory() + { + var orchestrator = new ControllerClientTrajectoryOrchestrator(); + var robot = TestRobotFactory.CreateRobotProfile(); + + var bundle = orchestrator.PlanOrdinaryTrajectory( + robot, + [ + [0.0, 0.0, 0.0, 0.0, 0.0, 0.0], + [0.1, 0.0, 0.0, 0.0, 0.0, 0.0], + [0.2, 0.0, 0.0, 0.0, 0.0, 0.0], + [0.3, 0.0, 0.0, 0.0, 0.0, 0.0] + ]); + + Assert.NotNull(bundle.Result.DenseJointTrajectory); + Assert.NotEmpty(bundle.Result.DenseJointTrajectory); + + // 验证时间单调递增。 + var times = bundle.Result.DenseJointTrajectory.Select(static row => row[0]).ToArray(); + for (var i = 1; i < times.Length; i++) + { + Assert.True(times[i] > times[i - 1], $"采样时间点应在索引 {i} 处单调递增。"); + } + + // 验证每行包含时间 + 6 个关节值。 + Assert.All(bundle.Result.DenseJointTrajectory, row => Assert.Equal(7, row.Count)); + } + + /// + /// 验证飞拍轨迹规划后的稠密采样时间轴与伺服周期一致。 + /// + [Fact] + public void ControllerClientTrajectoryOrchestrator_PlanUploadedFlyshot_DenseTrajectoryUsesServoPeriod() + { + var orchestrator = new ControllerClientTrajectoryOrchestrator(); + var robot = TestRobotFactory.CreateRobotProfile(); + var uploaded = TestRobotFactory.CreateUploadedTrajectoryWithSingleShot(); + + var bundle = orchestrator.PlanUploadedFlyshot(robot, uploaded); + + Assert.NotNull(bundle.Result.DenseJointTrajectory); + Assert.True(bundle.Result.DenseJointTrajectory.Count > 1); + + // 采样周期应为 8ms（伺服周期）。 + var firstDt = bundle.Result.DenseJointTrajectory[1][0] - bundle.Result.DenseJointTrajectory[0][0]; + Assert.Equal(0.008, firstDt, precision: 3); + } + /// /// 验证兼容服务执行普通轨迹时会进入规划链路，而不是直接把最后一个路点写入状态。 /// @@ -104,6 +203,73 @@ public sealed class RuntimeOrchestrationTests Assert.Throws(Act); } + + /// + /// 验证兼容服务初始化机器人时会把 RobotConfig.json 中的 acc_limit / jerk_limit 传给模型加载器。 + /// + [Fact] + public void ControllerClientCompatService_SetUpRobot_AppliesRobotConfigLimitScales() + { + var tempRoot = CreateTempWorkspaceRoot(); + try + { + File.WriteAllText( + Path.Combine(tempRoot, "RobotConfig.json"), + """ + { + "robot": { + "use_do": true, + "io_addr": [7, 8], + "io_keep_cycles": 4, + "acc_limit": 0.5, + "jerk_limit": 0.25, + "adapt_icsp_try_num": 3 + }, + "flying_shots": {} + } + """); + + var options = new ControllerClientCompatOptions { WorkspaceRoot = tempRoot }; + var runtime = new RecordingControllerRuntime(); + var service = new ControllerClientCompatService( + options, + new ControllerClientCompatRobotCatalog(options, new RobotModelLoader()), + runtime, + new ControllerClientTrajectoryOrchestrator(), + new RobotConfigLoader(), + new InMemoryFlyshotTrajectoryStore()); + + service.SetUpRobot("FANUC_LR_Mate_200iD"); + + var profile = Assert.IsType(runtime.LastRobotProfile); + Assert.Equal(14.905, profile.JointLimits[2].AccelerationLimit, precision: 3); + Assert.Equal(62.115, profile.JointLimits[2].JerkLimit, precision: 3); + } + finally + { + Directory.Delete(tempRoot, recursive: true); + } + } + + /// + /// 创建只包含当前支持机器人模型和 RobotConfig.json 的临时工作区。 + /// + private static string CreateTempWorkspaceRoot() + { + var tempRoot = Path.Combine(Path.GetTempPath(), "flyshot-runtime-tests", Guid.NewGuid().ToString("N")); + var modelDir = Path.Combine(tempRoot, "FlyingShot", "FlyingShot", "Models"); + Directory.CreateDirectory(modelDir); + + var sourceModel = Path.Combine( + TestRobotFactory.GetWorkspaceRoot(), + "FlyingShot", + "FlyingShot", + "Models", + "LR_Mate_200iD_7L.robot"); + File.Copy(sourceModel, Path.Combine(modelDir, "LR_Mate_200iD_7L.robot")); + + return tempRoot; + } } /// @@ -170,14 +336,16 @@ internal static class TestRobotFactory options, new ControllerClientCompatRobotCatalog(options, new RobotModelLoader()), new FanucControllerRuntime(), - new ControllerClientTrajectoryOrchestrator()); + new ControllerClientTrajectoryOrchestrator(), + new RobotConfigLoader(), + new InMemoryFlyshotTrajectoryStore()); } /// /// 定位父工作区根目录，供兼容服务加载真实机器人模型。 /// /// 父工作区根目录。 - private static string GetWorkspaceRoot() + public static string GetWorkspaceRoot() { var current = new DirectoryInfo(AppContext.BaseDirectory); while (current is not null) @@ -193,3 +361,126 @@ internal static class TestRobotFactory throw new DirectoryNotFoundException("Unable to locate the flyshot workspace root."); } } + +/// +/// 内存中的轨迹存储实现，用于避免单元测试污染真实文件系统。 +/// +internal sealed class InMemoryFlyshotTrajectoryStore : IFlyshotTrajectoryStore +{ + private readonly Dictionary _store = new(StringComparer.Ordinal); + + /// + public void Save(string robotName, CompatibilityRobotSettings settings, ControllerClientCompatUploadedTrajectory trajectory) + { + _store[trajectory.Name] = trajectory; + } + + /// + public void Delete(string robotName, string trajectoryName) + { + _store.Remove(trajectoryName); + } + + /// + public IReadOnlyDictionary LoadAll(string robotName, out CompatibilityRobotSettings? settings) + { + settings = null; + return _store; + } +} + +/// +/// 记录 ResetRobot 入参的测试运行时，用于验证兼容服务传递的机器人配置。 +/// +internal sealed class RecordingControllerRuntime : IControllerRuntime +{ + /// + /// 获取最近一次 ResetRobot 收到的机器人配置。 + /// + public RobotProfile? LastRobotProfile { get; private set; } + + /// + public void ResetRobot(RobotProfile robot, string robotName) + { + LastRobotProfile = robot; + } + + /// + public void SetActiveController(bool sim) + { + } + + /// + public void Connect(string robotIp) + { + } + + /// + public void Disconnect() + { + } + + /// + public void EnableRobot(int bufferSize) + { + } + + /// + public void DisableRobot() + { + } + + /// + public void StopMove() + { + } + + /// + public double GetSpeedRatio() => 1.0; + + /// + public void SetSpeedRatio(double ratio) + { + } + + /// + public IReadOnlyList GetTcp() => [0.0, 0.0, 0.0]; + + /// + public void SetTcp(double x, double y, double z) + { + } + + /// + public bool GetIo(int port, string ioType) => false; + + /// + public void SetIo(int port, bool value, string ioType) + { + } + + /// + public IReadOnlyList GetJointPositions() => Array.Empty(); + + /// + public IReadOnlyList GetPose() => Array.Empty(); + + /// + public ControllerStateSnapshot GetSnapshot() + { + return new ControllerStateSnapshot( + capturedAt: DateTimeOffset.UtcNow, + connectionState: "Connected", + isEnabled: true, + isInMotion: false, + speedRatio: 1.0, + jointPositions: Array.Empty(), + cartesianPose: Array.Empty(), + activeAlarms: Array.Empty()); + } + + /// + public void ExecuteTrajectory(TrajectoryResult result, IReadOnlyList finalJointPositions) + { + } +}