diff --git a/.claude/settings.local.json b/.claude/settings.local.json index 7e69559..2c991b3 100644 --- a/.claude/settings.local.json +++ b/.claude/settings.local.json @@ -9,7 +9,9 @@ "Bash(DOTNET_CLI_HOME=/tmp NUGET_PACKAGES=/tmp/nuget-packages dotnet test tests/Flyshot.Server.IntegrationTests/Flyshot.Server.IntegrationTests.csproj --no-build -v minimal)", "Bash(python -c \"import json; json.load\\(open\\('.vscode/launch.json'\\)\\); json.load\\(open\\('.vscode/tasks.json'\\)\\); print\\('JSON valid.'\\)\")", "Bash(python -c \"import json; json.load\\(open\\('.vscode/launch.json', encoding='utf-8'\\)\\); json.load\\(open\\('.vscode/tasks.json', encoding='utf-8'\\)\\); print\\('JSON valid.'\\)\")", - "Bash(/bin/bash -lc 'DOTNET_CLI_HOME=/tmp NUGET_PACKAGES=/tmp/nuget-packages dotnet build FlyshotReplacement.sln --no-restore -v minimal')" + "Bash(/bin/bash -lc 'DOTNET_CLI_HOME=/tmp NUGET_PACKAGES=/tmp/nuget-packages dotnet build FlyshotReplacement.sln --no-restore -v minimal')", + "Bash(/bin/bash -lc 'DOTNET_CLI_HOME=/tmp NUGET_PACKAGES=/tmp/nuget-packages dotnet test tests/Flyshot.Core.Tests/Flyshot.Core.Tests.csproj -v minimal')", + "Bash(/bin/bash -lc 'DOTNET_CLI_HOME=/tmp NUGET_PACKAGES=/tmp/nuget-packages dotnet test tests/Flyshot.Server.IntegrationTests/Flyshot.Server.IntegrationTests.csproj -v minimal')" ] } } diff --git a/README.md b/README.md index f66a7f7..1019a1d 100644 --- a/README.md +++ b/README.md @@ -18,9 +18,10 @@ - `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` 和 `Rvbust/uttc-20260428/20260428.pcap` 真机抓包确认为 90B 固定帧。 -- 2026-04-28 UTTC 抓包确认：UDP 60015 命令 `target[0..5]` 为关节角度制 `deg`，`JointDetialTraj.txt` 为弧度制 `rad`，`speed_ratio=0.7` 体现为 UDP 下发时间轴约 `1.427730x` 拉伸。 +- 2026-04-28 UTTC 抓包确认：UDP 60015 命令 `target[0..5]` 为关节角度制 `deg`，`JointDetialTraj.txt` 为弧度制 `rad`，`speed_ratio=0.7` 体现为 UDP 下发时间轴约 `1.427730x` 拉伸；2026-04-30 实体机确认 `speed_ratio` 不影响生成的 `JointTraj.txt` 规划时长，当前实际生成约 `7.4s` 轨迹。 - 真机 Socket 客户端已具备基础连接、程序启停、速度倍率/TCP/IO 参数命令和 J519 周期发送能力；稠密轨迹下发已按 `speed_ratio` 做执行时间缩放，并已用 0.5/0.7/1.0 三份 UTTC 抓包固化 J519 golden tests。真实 R30iB 全流程现场联调仍需执行。 - `MoveJoint` 已按 `2026042802-mvpoint*.pcap` 复刻为点到点临时轨迹：当前关节到目标关节的关节空间直线，五次 smoothstep 起停，按 `status=15` 运动窗口复现 `40/55/77` 点，并由 J519 层完成 `rad -> deg` 下发。 +- 单程序只对应一台机器人，上传/删除/恢复飞拍轨迹统一读写运行目录 `Config/RobotConfig.json`，不再创建独立轨迹存储文件。 开发约定： @@ -44,12 +45,14 @@ - [x] 使用本地 TCP/UDP 模拟器覆盖命令通道、状态通道和 J519 基础收发 - [x] 补齐 `Get/SetSpeedRatio`、`Get/SetTCP`、`Get/SetIO` 真机命令体与响应解析 - [x] 保留新 HTTP 接口路线，明确不再实现旧 `50001/TCP+JSON` 网关 +- [x] 将飞拍轨迹持久化收敛到运行目录 `Config/RobotConfig.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` 全部贯通到规划和执行链路。 + - [x] 将上传飞拍轨迹统一保存到运行目录 `Config/RobotConfig.json` 的 `flying_shots` 节点。 - [x] 为新 HTTP API 补一份当前现场调用顺序文档，替代旧 `ControllerClient` 工作流：见 `docs/fanuc-field-runtime-workflow.md`。 2. 轨迹规划 @@ -58,7 +61,7 @@ - [ ] 如果现场仍需要 `method="doubles"`，实现 `TrajectoryDoubleS` 等价规划；否则在 HTTP 文档中明确标为不支持。 - [ ] 把已完成对齐的旧系统轨迹样本固化为 golden tests，防止后续重构破坏轨迹一致性。 - [x] 将 `Rvbust/uttc-20260428/Data/JointDetialTraj.txt` 固化为 J519 golden 样本：输入为 `rad`，下发为 `deg`，并按 `speed_ratio` 拉伸时间轴；覆盖 `2026042802-0.5/0.7/1.pcap`。 - - [ ] 补齐 `save_traj` / `SaveTrajInfo` 的规划结果导出，将稠密关节轨迹、笛卡尔轨迹和 ShotEvents 写入可诊断 artifacts。 + - [x] 补齐飞拍 `save_traj` / `SaveTrajInfo` 的规划结果导出，将关节关键点、稠密关节轨迹、笛卡尔关键点、稠密笛卡尔轨迹和 ShotEvents 写入 `Config/Data/`。 3. FANUC TCP 10012 命令通道 - [x] 补齐 `GetSpeedRatio` / `SetSpeedRatio` 真机命令体与响应解析。 @@ -82,6 +85,7 @@ 6. 真机联调与运行安全 - [ ] 在真实 R30iB + `RVBUSTSM` 程序上验证 `Connect -> EnableRobot -> ExecuteFlyShotTraj -> StopMove -> DisableRobot -> Disconnect` 全流程。 + - [x] 实体机复核运行速度对轨迹生成时间的影响：`speed_ratio` 不影响 `IsFlyshotTrajectoryValid` / `SaveTrajectoryInfo` 生成的 `JointTraj.txt` 规划时长，当前实际生成约 `7.4s` 轨迹；运行阶段仅 J519 下发时长和包数按 `speed_ratio` 拉伸，UTTC_MS11 参考值为约 `7.4s` 与 `10.56s`。 - [ ] 增加急停、伺服未就绪、程序未启动、网络断开、控制柜拒收命令等故障路径处理。 - [ ] 给 HTTP 执行接口增加运行互斥、执行中拒绝重复轨迹、取消和超时控制。 - [ ] 增加运行日志、协议摘要日志和状态快照导出，便于现场排查。 diff --git a/src/Flyshot.ControllerClientCompat/ControllerClientCompatService.cs b/src/Flyshot.ControllerClientCompat/ControllerClientCompatService.cs index fd2629a..8f3b20f 100644 --- a/src/Flyshot.ControllerClientCompat/ControllerClientCompatService.cs +++ b/src/Flyshot.ControllerClientCompat/ControllerClientCompatService.cs @@ -17,7 +17,8 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi private readonly IControllerRuntime _runtime; private readonly ControllerClientTrajectoryOrchestrator _trajectoryOrchestrator; private readonly RobotConfigLoader _configLoader; - private readonly IFlyshotTrajectoryStore _trajectoryStore; + private readonly FlyshotTrajectoryArtifactWriter _artifactWriter; + private readonly JsonFlyshotTrajectoryStore _trajectoryStore; private readonly ILogger? _logger; private RobotProfile? _activeRobotProfile; private string? _configuredRobotName; @@ -36,7 +37,8 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi /// 控制器运行时。 /// 轨迹规划与触发编排器。 /// 旧版 RobotConfig.json 加载器。 - /// 已上传轨迹持久化存储。 + /// saveTrajectory 规划结果点位导出器。 + /// 统一 RobotConfig.json 持久化存储；为空时按配置根目录创建默认实例。 /// 日志记录器；允许测试直接构造时传入 null。 public ControllerClientCompatService( ControllerClientCompatOptions options, @@ -44,7 +46,8 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi IControllerRuntime runtime, ControllerClientTrajectoryOrchestrator trajectoryOrchestrator, RobotConfigLoader configLoader, - IFlyshotTrajectoryStore trajectoryStore, + FlyshotTrajectoryArtifactWriter? artifactWriter = null, + JsonFlyshotTrajectoryStore? trajectoryStore = null, ILogger? logger = null) { _options = options ?? throw new ArgumentNullException(nameof(options)); @@ -52,7 +55,8 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi _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)); + _artifactWriter = artifactWriter ?? new FlyshotTrajectoryArtifactWriter(_options, new RobotModelLoader()); + _trajectoryStore = trajectoryStore ?? new JsonFlyshotTrajectoryStore(_options, _configLoader); _logger = logger; } @@ -374,7 +378,7 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi EnsureJointVector(jointPositions, robot.DegreesOfFreedom, nameof(jointPositions)); var speedRatio = _runtime.GetSnapshot().SpeedRatio; - var moveResult = MoveJointTrajectoryGenerator.CreateResult(robot, currentJointPositions, jointPositions, speedRatio); + var moveResult = MoveJointTrajectoryGenerator.CreateResult(robot, currentJointPositions, jointPositions, speedRatio, _logger); _logger?.LogInformation( "MoveJoint 规划完成: 当前速度倍率={SpeedRatio}, 规划时长={Duration}s, 采样点数={SampleCount}", speedRatio, @@ -407,13 +411,15 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi EnsureRuntimeEnabled(); // 普通轨迹必须按调用方指定 method 规划，再把规划结果交给运行时执行。 - var bundle = _trajectoryOrchestrator.PlanOrdinaryTrajectory(robot, waypoints, options); + var planningSpeedScale = RequireRobotSettings().PlanningSpeedScale; + var bundle = _trajectoryOrchestrator.PlanOrdinaryTrajectory(robot, waypoints, options, planningSpeedScale); _logger?.LogInformation( - "ExecuteTrajectory 规划完成: method={Method}, 时长={Duration}s, 有效={IsValid}, 采样点数={SampleCount}", + "ExecuteTrajectory 规划完成: method={Method}, 时长={Duration}s, 有效={IsValid}, 采样点数={SampleCount}, planningSpeedScale={PlanningSpeedScale}", bundle.Result.Method, bundle.Result.Duration.TotalSeconds, bundle.Result.IsValid, - bundle.Result.DenseJointTrajectory?.Count ?? 0); + bundle.Result.DenseJointTrajectory?.Count ?? 0, + planningSpeedScale); var finalJointPositions = bundle.PlannedTrajectory.PlannedWaypoints[^1].Positions; _runtime.ExecuteTrajectory(bundle.Result, finalJointPositions); } @@ -495,14 +501,17 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi } // 已上传飞拍轨迹必须按调用方指定 method 生成 shot timeline 后再交给运行时。 - var bundle = _trajectoryOrchestrator.PlanUploadedFlyshot(robot, trajectory, options, RequireRobotSettings()); + var settings = RequireRobotSettings(); + var bundle = _trajectoryOrchestrator.PlanUploadedFlyshot(robot, trajectory, options, settings, settings.PlanningSpeedScale); + ExportFlyshotArtifactsIfRequested(name, options.SaveTrajectory, robot, bundle); _logger?.LogInformation( - "ExecuteTrajectoryByName 规划完成: name={Name}, method={Method}, 时长={Duration}s, 触发事件数={TriggerCount}, 使用缓存={UsedCache}", + "ExecuteTrajectoryByName 规划完成: name={Name}, method={Method}, 时长={Duration}s, 触发事件数={TriggerCount}, 使用缓存={UsedCache}, planningSpeedScale={PlanningSpeedScale}", name, bundle.Result.Method, bundle.Result.Duration.TotalSeconds, bundle.Result.TriggerTimeline.Count, - bundle.Result.UsedCache); + bundle.Result.UsedCache, + settings.PlanningSpeedScale); if (options.MoveToStart) { @@ -537,11 +546,14 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi } // 先通过规划校验避免静默接受非法参数，同时把轨迹信息强制刷写到本地 JSON。 - _ = _trajectoryOrchestrator.PlanUploadedFlyshot( + var planningSettings = RequireRobotSettings(); + var bundle = _trajectoryOrchestrator.PlanUploadedFlyshot( robot, trajectory, new FlyshotExecutionOptions(saveTrajectory: true, method: method), - RequireRobotSettings()); + planningSettings, + planningSettings.PlanningSpeedScale); + ExportFlyshotArtifactsIfRequested(name, saveTrajectory: true, robot, bundle); var robotName = _configuredRobotName ?? throw new InvalidOperationException("Robot has not been setup."); var settings = _robotSettings ?? CreateDefaultRobotSettings(); @@ -570,11 +582,14 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi throw new InvalidOperationException("FlyShot trajectory does not exist."); } + var planningSettings = RequireRobotSettings(); var bundle = _trajectoryOrchestrator.PlanUploadedFlyshot( robot, trajectory, new FlyshotExecutionOptions(method: method, saveTrajectory: saveTrajectory), - RequireRobotSettings()); + planningSettings, + planningSettings.PlanningSpeedScale); + ExportFlyshotArtifactsIfRequested(name, saveTrajectory, robot, bundle); duration = bundle.Result.Duration; _logger?.LogInformation( @@ -708,6 +723,27 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi plannedWaypointCount: 1); } + /// + /// 根据 saveTrajectory 参数把规划结果点位写入运行目录 Config/Data/name。 + /// + /// 飞拍轨迹名称。 + /// 是否导出规划结果点位。 + /// 当前机器人模型。 + /// 规划结果包。 + private void ExportFlyshotArtifactsIfRequested( + string name, + bool saveTrajectory, + RobotProfile robot, + PlannedExecutionBundle bundle) + { + if (!saveTrajectory) + { + return; + } + + _artifactWriter.WriteUploadedFlyshot(name, robot, bundle); + } + /// /// 尝试从配置根目录加载 RobotConfig.json 获取机器人配置；失败时返回 null。 /// diff --git a/src/Flyshot.ControllerClientCompat/ControllerClientCompatServiceCollectionExtensions.cs b/src/Flyshot.ControllerClientCompat/ControllerClientCompatServiceCollectionExtensions.cs index 0e2945f..fc09530 100644 --- a/src/Flyshot.ControllerClientCompat/ControllerClientCompatServiceCollectionExtensions.cs +++ b/src/Flyshot.ControllerClientCompat/ControllerClientCompatServiceCollectionExtensions.cs @@ -32,7 +32,8 @@ public static class ControllerClientCompatServiceCollectionExtensions services.AddSingleton(); 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 f0b1148..164359c 100644 --- a/src/Flyshot.ControllerClientCompat/ControllerClientTrajectoryOrchestrator.cs +++ b/src/Flyshot.ControllerClientCompat/ControllerClientTrajectoryOrchestrator.cs @@ -38,15 +38,17 @@ public sealed class ControllerClientTrajectoryOrchestrator public PlannedExecutionBundle PlanOrdinaryTrajectory( RobotProfile robot, IReadOnlyList> waypoints, - TrajectoryExecutionOptions? options = null) + TrajectoryExecutionOptions? options = null, + double planningSpeedScale = 1.0) { ArgumentNullException.ThrowIfNull(robot); ArgumentNullException.ThrowIfNull(waypoints); options ??= new TrajectoryExecutionOptions(); + var planningRobot = ApplyPlanningSpeedScale(robot, planningSpeedScale); _logger?.LogInformation( - "PlanOrdinaryTrajectory 开始: 路点数={WaypointCount}, method={Method}", - waypoints.Count, options.Method); + "PlanOrdinaryTrajectory 开始: 路点数={WaypointCount}, method={Method}, planningSpeedScale={PlanningSpeedScale}", + waypoints.Count, options.Method, planningSpeedScale); var program = CreateProgram( name: "ordinary-trajectory", @@ -57,7 +59,7 @@ public sealed class ControllerClientTrajectoryOrchestrator var method = ParseOrdinaryMethod(options.Method); var request = new TrajectoryRequest( - robot: robot, + robot: planningRobot, program: program, method: method, saveTrajectoryArtifacts: options.SaveTrajectory); @@ -84,16 +86,19 @@ public sealed class ControllerClientTrajectoryOrchestrator RobotProfile robot, ControllerClientCompatUploadedTrajectory uploaded, FlyshotExecutionOptions? options = null, - CompatibilityRobotSettings? settings = null) + CompatibilityRobotSettings? settings = null, + double? planningSpeedScale = null) { ArgumentNullException.ThrowIfNull(robot); ArgumentNullException.ThrowIfNull(uploaded); options ??= new FlyshotExecutionOptions(); settings ??= CreateDefaultRobotSettings(); + var effectivePlanningSpeedScale = planningSpeedScale ?? settings.PlanningSpeedScale; + var planningRobot = ApplyPlanningSpeedScale(robot, effectivePlanningSpeedScale); _logger?.LogInformation( - "PlanUploadedFlyshot 开始: name={Name}, waypoints={WaypointCount}, method={Method}, useCache={UseCache}", - uploaded.Name, uploaded.Waypoints.Count, options.Method, options.UseCache); + "PlanUploadedFlyshot 开始: name={Name}, waypoints={WaypointCount}, method={Method}, useCache={UseCache}, planningSpeedScale={PlanningSpeedScale}", + uploaded.Name, uploaded.Waypoints.Count, options.Method, options.UseCache, effectivePlanningSpeedScale); var program = CreateProgram( name: uploaded.Name, @@ -103,7 +108,7 @@ public sealed class ControllerClientTrajectoryOrchestrator addressGroups: uploaded.AddressGroups); var method = ParseFlyshotMethod(options.Method); - var cacheKey = CreateFlyshotCacheKey(robot, uploaded, options, settings); + var cacheKey = CreateFlyshotCacheKey(planningRobot, uploaded, options, settings, effectivePlanningSpeedScale); if (options.UseCache && _flyshotCache.TryGetValue(cacheKey, out var cachedBundle)) { _logger?.LogInformation("PlanUploadedFlyshot 命中缓存: name={Name}, cacheKey={CacheKey}", uploaded.Name, cacheKey); @@ -115,7 +120,7 @@ public sealed class ControllerClientTrajectoryOrchestrator } var request = new TrajectoryRequest( - robot: robot, + robot: planningRobot, program: program, method: method, moveToStart: options.MoveToStart, @@ -126,7 +131,7 @@ public sealed class ControllerClientTrajectoryOrchestrator var shotTimeline = _shotTimelineBuilder.Build( plannedTrajectory, holdCycles: settings.IoKeepCycles, - samplePeriod: robot.ServoPeriod, + samplePeriod: planningRobot.ServoPeriod, useDo: settings.UseDo); var result = CreateResult(plannedTrajectory, shotTimeline, usedCache: false); var bundle = new PlannedExecutionBundle(plannedTrajectory, shotTimeline, result); @@ -219,10 +224,12 @@ public sealed class ControllerClientTrajectoryOrchestrator RobotProfile robot, ControllerClientCompatUploadedTrajectory uploaded, FlyshotExecutionOptions options, - CompatibilityRobotSettings settings) + CompatibilityRobotSettings settings, + double planningSpeedScale) { var hash = new HashCode(); hash.Add(robot.Name, StringComparer.Ordinal); + hash.Add(planningSpeedScale); hash.Add(uploaded.Name, StringComparer.Ordinal); hash.Add(NormalizeMethod(options.Method), StringComparer.Ordinal); hash.Add(options.MoveToStart); @@ -231,6 +238,14 @@ public sealed class ControllerClientTrajectoryOrchestrator hash.Add(settings.IoKeepCycles); hash.Add(settings.AdaptIcspTryNum); + foreach (var limit in robot.JointLimits) + { + hash.Add(limit.JointName, StringComparer.Ordinal); + hash.Add(limit.VelocityLimit); + hash.Add(limit.AccelerationLimit); + hash.Add(limit.JerkLimit); + } + foreach (var waypoint in uploaded.Waypoints) { foreach (var value in waypoint) @@ -275,6 +290,43 @@ public sealed class ControllerClientTrajectoryOrchestrator adaptIcspTryNum: 5); } + /// + /// 按规划全局速度倍率生成规划专用机器人约束。 + /// + /// 原始机器人约束。 + /// 规划阶段的全局速度倍率，1.0 表示不额外缩放。 + /// 已按速度倍率缩放后的规划机器人约束。 + private static RobotProfile ApplyPlanningSpeedScale(RobotProfile robot, double planningSpeedScale) + { + if (double.IsNaN(planningSpeedScale) || double.IsInfinity(planningSpeedScale) || planningSpeedScale <= 0.0) + { + throw new ArgumentOutOfRangeException(nameof(planningSpeedScale), "规划速度倍率必须是有限正数。"); + } + + if (Math.Abs(planningSpeedScale - 1.0) < 1e-12) + { + return robot; + } + + // RVBUST 规划阶段会用独立限速倍率缩放有效限制；运行时 speedRatio 仍只负责 J519 下发重采样。 + var scaledLimits = robot.JointLimits + .Select(limit => new JointLimit( + limit.JointName, + limit.VelocityLimit * planningSpeedScale, + limit.AccelerationLimit * planningSpeedScale * planningSpeedScale, + limit.JerkLimit * planningSpeedScale * planningSpeedScale * planningSpeedScale)) + .ToArray(); + + return new RobotProfile( + name: robot.Name, + modelPath: robot.ModelPath, + degreesOfFreedom: robot.DegreesOfFreedom, + jointLimits: scaledLimits, + jointCouplings: robot.JointCouplings, + servoPeriod: robot.ServoPeriod, + triggerPeriod: robot.TriggerPeriod); + } + /// /// 把兼容层输入数组转换成领域层 FlyshotProgram。 /// diff --git a/src/Flyshot.ControllerClientCompat/FlyshotTrajectoryArtifactWriter.cs b/src/Flyshot.ControllerClientCompat/FlyshotTrajectoryArtifactWriter.cs new file mode 100644 index 0000000..d25312c --- /dev/null +++ b/src/Flyshot.ControllerClientCompat/FlyshotTrajectoryArtifactWriter.cs @@ -0,0 +1,136 @@ +using Flyshot.Core.Config; +using Flyshot.Core.Domain; +using Flyshot.Core.Planning; +using Flyshot.Core.Planning.Export; +using Flyshot.Core.Planning.Kinematics; +using Flyshot.Core.Planning.Sampling; +using Microsoft.Extensions.Logging; + +namespace Flyshot.ControllerClientCompat; + +/// +/// 负责把 saveTrajectory 生成的规划结果点位写入运行目录 Config/Data。 +/// +public sealed class FlyshotTrajectoryArtifactWriter +{ + /// + /// 旧 Data 明细点位文件使用的默认采样周期，单位为秒。 + /// + private const double LegacyDetailSamplePeriodSeconds = 0.016; + + private readonly ControllerClientCompatOptions _options; + private readonly RobotModelLoader _robotModelLoader; + private readonly ILogger? _logger; + + /// + /// 初始化规划结果点位导出器。 + /// + /// 兼容层基础配置，用于定位运行配置根目录。 + /// 机器人模型加载器，用于生成笛卡尔点位。 + /// 日志记录器；允许 null。 + public FlyshotTrajectoryArtifactWriter( + ControllerClientCompatOptions options, + RobotModelLoader robotModelLoader, + ILogger? logger = null) + { + _options = options ?? throw new ArgumentNullException(nameof(options)); + _robotModelLoader = robotModelLoader ?? throw new ArgumentNullException(nameof(robotModelLoader)); + _logger = logger; + } + + /// + /// 将飞拍规划结果导出到 Config/Data/name。 + /// + /// 飞拍轨迹名称。 + /// 当前机器人配置。 + /// 规划结果包。 + public void WriteUploadedFlyshot(string trajectoryName, RobotProfile robot, PlannedExecutionBundle bundle) + { + if (string.IsNullOrWhiteSpace(trajectoryName)) + { + throw new ArgumentException("轨迹名称不能为空。", nameof(trajectoryName)); + } + + ArgumentNullException.ThrowIfNull(robot); + ArgumentNullException.ThrowIfNull(bundle); + + var outputDir = Path.Combine(_options.ResolveConfigRoot(), "Data", SanitizeDirectoryName(trajectoryName)); + Directory.CreateDirectory(outputDir); + + // 明细文件对齐旧 Data 目录的 16ms 采样；运行时 J519 仍可使用自己的 8ms 伺服采样。 + var kinematicsModel = _robotModelLoader.LoadKinematicsModel(robot.ModelPath); + var jointTrajectory = BuildJointRows(bundle.PlannedTrajectory); + var jointDetailTrajectory = TrajectorySampler.SampleJointTrajectory( + bundle.PlannedTrajectory, + samplePeriod: LegacyDetailSamplePeriodSeconds); + var cartTrajectory = BuildCartesianRows(bundle.PlannedTrajectory, kinematicsModel); + var cartDetailTrajectory = TrajectorySampler.SampleCartesianTrajectory( + bundle.PlannedTrajectory, + kinematicsModel, + samplePeriod: LegacyDetailSamplePeriodSeconds); + + TrajectoryExporter.WriteJointTrajectory(Path.Combine(outputDir, "JointTraj.txt"), jointTrajectory); + TrajectoryExporter.WriteJointDenseTrajectory(Path.Combine(outputDir, "JointDetialTraj.txt"), jointDetailTrajectory); + TrajectoryExporter.WriteCartesianTrajectory(Path.Combine(outputDir, "CartTraj.txt"), cartTrajectory); + TrajectoryExporter.WriteCartesianDenseTrajectory(Path.Combine(outputDir, "CartDetialTraj.txt"), cartDetailTrajectory); + TrajectoryExporter.WriteShotEvents(Path.Combine(outputDir, "ShotEvents.json"), bundle.ShotTimeline.ShotEvents); + + _logger?.LogInformation( + "saveTrajectory 已导出规划点位: name={TrajectoryName}, outputDir={OutputDir}, jointRows={JointRows}, detailRows={DetailRows}", + trajectoryName, + outputDir, + jointTrajectory.Count, + jointDetailTrajectory.Count); + } + + /// + /// 构造 JointTraj.txt 行数据，格式为 time + 关节弧度。 + /// + private static IReadOnlyList> BuildJointRows(PlannedTrajectory trajectory) + { + var rows = new List>(trajectory.PlannedWaypoints.Count); + for (var index = 0; index < trajectory.PlannedWaypoints.Count; index++) + { + var row = new List(trajectory.PlannedWaypoints[index].Positions.Count + 1) + { + Math.Round(trajectory.WaypointTimes[index], 6) + }; + row.AddRange(trajectory.PlannedWaypoints[index].Positions.Select(static value => Math.Round(value, 6))); + rows.Add(row); + } + + return rows; + } + + /// + /// 构造 CartTraj.txt 行数据，格式为 time + x/y/z/qx/qy/qz/qw。 + /// + private static IReadOnlyList> BuildCartesianRows( + PlannedTrajectory trajectory, + RobotKinematicsModel kinematicsModel) + { + var rows = new List>(trajectory.PlannedWaypoints.Count); + for (var index = 0; index < trajectory.PlannedWaypoints.Count; index++) + { + var pose = RobotKinematics.ForwardKinematics(kinematicsModel, trajectory.PlannedWaypoints[index].Positions.ToArray()); + var row = new List(pose.Length + 1) + { + Math.Round(trajectory.WaypointTimes[index], 6) + }; + row.AddRange(pose.Select(static value => Math.Round(value, 6))); + rows.Add(row); + } + + return rows; + } + + /// + /// 将轨迹名转换为可用目录名，避免 HTTP 输入中的路径字符污染输出目录。 + /// + private static string SanitizeDirectoryName(string name) + { + var invalidChars = Path.GetInvalidFileNameChars(); + var chars = name.Select(ch => invalidChars.Contains(ch) ? '_' : ch).ToArray(); + return new string(chars); + } +} diff --git a/src/Flyshot.ControllerClientCompat/FlyshotTrajectoryStore.cs b/src/Flyshot.ControllerClientCompat/FlyshotTrajectoryStore.cs index 68b7e43..29444a2 100644 --- a/src/Flyshot.ControllerClientCompat/FlyshotTrajectoryStore.cs +++ b/src/Flyshot.ControllerClientCompat/FlyshotTrajectoryStore.cs @@ -7,38 +7,9 @@ using Microsoft.Extensions.Logging; namespace Flyshot.ControllerClientCompat; /// -/// 定义已上传飞拍轨迹的持久化存储契约。 +/// 使用运行目录 Config/RobotConfig.json 持久化单机器人飞拍轨迹和机器人配置。 /// -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 格式在运行目录 Config 中持久化飞拍轨迹和机器人配置。 -/// -public sealed class JsonFlyshotTrajectoryStore : IFlyshotTrajectoryStore +public sealed class JsonFlyshotTrajectoryStore { private readonly ControllerClientCompatOptions _options; private readonly RobotConfigLoader _configLoader; @@ -57,19 +28,24 @@ public sealed class JsonFlyshotTrajectoryStore : IFlyshotTrajectoryStore _logger = logger; } - /// + /// + /// 将单条轨迹持久化到统一 RobotConfig.json，同时更新机器人配置段。 + /// + /// 当前已初始化的机器人名称，仅用于日志诊断。 + /// 当前机器人级兼容配置。 + /// 要保存的已上传轨迹。 public void Save(string robotName, CompatibilityRobotSettings settings, ControllerClientCompatUploadedTrajectory trajectory) { ArgumentNullException.ThrowIfNull(settings); ArgumentNullException.ThrowIfNull(trajectory); _logger?.LogInformation( - "TrajectoryStore 保存轨迹: robot={RobotName}, name={TrajectoryName}, waypoints={WaypointCount}", + "RobotConfig 保存轨迹: robot={RobotName}, name={TrajectoryName}, waypoints={WaypointCount}", robotName, trajectory.Name, trajectory.Waypoints.Count); - var path = ResolveStorePath(robotName); + var path = ResolveStorePath(); var directory = Path.GetDirectoryName(path)!; Directory.CreateDirectory(directory); @@ -103,10 +79,14 @@ public sealed class JsonFlyshotTrajectoryStore : IFlyshotTrajectoryStore }; File.WriteAllText(path, root.ToJsonString(writeOptions)); - _logger?.LogInformation("TrajectoryStore 轨迹已保存到 {Path}", path); + _logger?.LogInformation("RobotConfig 轨迹已保存到 {Path}", path); } - /// + /// + /// 从统一 RobotConfig.json 删除指定名称的轨迹。 + /// + /// 当前已初始化的机器人名称，仅用于日志诊断。 + /// 要删除的轨迹名称。 public void Delete(string robotName, string trajectoryName) { if (string.IsNullOrWhiteSpace(trajectoryName)) @@ -114,12 +94,12 @@ public sealed class JsonFlyshotTrajectoryStore : IFlyshotTrajectoryStore throw new ArgumentException("轨迹名称不能为空。", nameof(trajectoryName)); } - _logger?.LogInformation("TrajectoryStore 删除轨迹: robot={RobotName}, name={TrajectoryName}", robotName, trajectoryName); + _logger?.LogInformation("RobotConfig 删除轨迹: robot={RobotName}, name={TrajectoryName}", robotName, trajectoryName); - var path = ResolveStorePath(robotName); + var path = ResolveStorePath(); if (!File.Exists(path)) { - _logger?.LogWarning("TrajectoryStore 删除失败: 文件不存在 {Path}", path); + _logger?.LogWarning("RobotConfig 删除失败: 文件不存在 {Path}", path); return; } @@ -127,7 +107,7 @@ public sealed class JsonFlyshotTrajectoryStore : IFlyshotTrajectoryStore var root = JsonNode.Parse(existingJson)?.AsObject(); if (root is null) { - _logger?.LogWarning("TrajectoryStore 删除失败: 无法解析 JSON {Path}", path); + _logger?.LogWarning("RobotConfig 删除失败: 无法解析 JSON {Path}", path); return; } @@ -142,29 +122,34 @@ public sealed class JsonFlyshotTrajectoryStore : IFlyshotTrajectoryStore PropertyNamingPolicy = JsonNamingPolicy.SnakeCaseLower }; File.WriteAllText(path, root.ToJsonString(writeOptions)); - _logger?.LogInformation("TrajectoryStore 轨迹已删除: {TrajectoryName}", trajectoryName); + _logger?.LogInformation("RobotConfig 轨迹已删除: {TrajectoryName}", trajectoryName); } else { - _logger?.LogWarning("TrajectoryStore 删除失败: 轨迹不存在 {TrajectoryName}", trajectoryName); + _logger?.LogWarning("RobotConfig 删除失败: 轨迹不存在 {TrajectoryName}", trajectoryName); } } } - /// + /// + /// 加载统一 RobotConfig.json 中的所有轨迹，并回传机器人配置。 + /// + /// 当前已初始化的机器人名称，仅用于日志诊断。 + /// 输出 RobotConfig.json 中的机器人配置；若文件不存在或解析失败则为 null。 + /// 按轨迹名称索引的已上传轨迹集合。 public IReadOnlyDictionary LoadAll(string robotName, out CompatibilityRobotSettings? settings) { - var path = ResolveStorePath(robotName); + var path = ResolveStorePath(); if (!File.Exists(path)) { - _logger?.LogInformation("TrajectoryStore 无持久化数据: {Path}", path); + _logger?.LogInformation("RobotConfig 无持久化数据: {Path}", path); settings = null; return new Dictionary(StringComparer.Ordinal); } try { - _logger?.LogInformation("TrajectoryStore 正在加载: {Path}", path); + _logger?.LogInformation("RobotConfig 正在加载: {Path}", path); var loaded = _configLoader.Load(path, _options.ResolveConfigRoot()); settings = loaded.Robot; @@ -182,7 +167,7 @@ public sealed class JsonFlyshotTrajectoryStore : IFlyshotTrajectoryStore } _logger?.LogInformation( - "TrajectoryStore 加载完成: robot={RobotName}, 轨迹数={Count}, useDo={UseDo}, ioKeepCycles={IoKeepCycles}", + "RobotConfig 加载完成: robot={RobotName}, 轨迹数={Count}, useDo={UseDo}, ioKeepCycles={IoKeepCycles}", robotName, dict.Count, settings?.UseDo, @@ -192,7 +177,7 @@ public sealed class JsonFlyshotTrajectoryStore : IFlyshotTrajectoryStore } catch (Exception ex) { - _logger?.LogError(ex, "TrajectoryStore 加载失败: {Path}", path); + _logger?.LogError(ex, "RobotConfig 加载失败: {Path}", path); settings = null; return new Dictionary(StringComparer.Ordinal); } @@ -229,11 +214,10 @@ public sealed class JsonFlyshotTrajectoryStore : IFlyshotTrajectoryStore } /// - /// 解析当前机器人对应的持久化文件路径。 + /// 解析单程序单机器人的统一配置文件路径。 /// - private string ResolveStorePath(string robotName) + private string ResolveStorePath() { - var storeDir = Path.Combine(_options.ResolveConfigRoot(), "TrajectoryStore"); - return Path.Combine(storeDir, $"{robotName}_trajectories.json"); + return Path.Combine(_options.ResolveConfigRoot(), "RobotConfig.json"); } } diff --git a/src/Flyshot.ControllerClientCompat/MoveJointTrajectoryGenerator.cs b/src/Flyshot.ControllerClientCompat/MoveJointTrajectoryGenerator.cs index c25e062..135897c 100644 --- a/src/Flyshot.ControllerClientCompat/MoveJointTrajectoryGenerator.cs +++ b/src/Flyshot.ControllerClientCompat/MoveJointTrajectoryGenerator.cs @@ -1,4 +1,5 @@ using Flyshot.Core.Domain; +using Microsoft.Extensions.Logging; namespace Flyshot.ControllerClientCompat; @@ -59,7 +60,8 @@ internal static class MoveJointTrajectoryGenerator RobotProfile robot, IReadOnlyList startJoints, IReadOnlyList targetJoints, - double speedRatio) + double speedRatio, + ILogger? logger = null) { ArgumentNullException.ThrowIfNull(robot); ArgumentNullException.ThrowIfNull(startJoints); @@ -80,6 +82,10 @@ internal static class MoveJointTrajectoryGenerator var durationSeconds = AlignDurationToServoStep(requestedDurationSeconds, samplePeriodSeconds); var denseJointTrajectory = GenerateDenseTrajectory(startJoints, targetJoints, durationSeconds, samplePeriodSeconds); + logger?.LogDebug( + "MoveJointTrajectoryGenerator: 请求时长={RequestedDuration:F4}s, 对齐后时长={Duration:F4}s, speedRatio={SpeedRatio}, 采样周期={SamplePeriod:F6}s, 采样数={SampleCount}", + requestedDurationSeconds, durationSeconds, speedRatio, samplePeriodSeconds, denseJointTrajectory.Count); + return new TrajectoryResult( programName: "move-joint", method: PlanningMethod.Doubles, diff --git a/src/Flyshot.Core.Config/Flyshot.Core.Config.csproj b/src/Flyshot.Core.Config/Flyshot.Core.Config.csproj index 1ccc45d..41216c6 100644 --- a/src/Flyshot.Core.Config/Flyshot.Core.Config.csproj +++ b/src/Flyshot.Core.Config/Flyshot.Core.Config.csproj @@ -8,6 +8,7 @@ + diff --git a/src/Flyshot.Core.Config/RobotConfigLoader.cs b/src/Flyshot.Core.Config/RobotConfigLoader.cs index 6d04c23..99a2875 100644 --- a/src/Flyshot.Core.Config/RobotConfigLoader.cs +++ b/src/Flyshot.Core.Config/RobotConfigLoader.cs @@ -1,5 +1,6 @@ using System.Text.Json; using Flyshot.Core.Domain; +using Microsoft.Extensions.Logging; namespace Flyshot.Core.Config; @@ -17,7 +18,8 @@ public sealed class CompatibilityRobotSettings int ioKeepCycles, double accLimitScale, double jerkLimitScale, - int adaptIcspTryNum) + int adaptIcspTryNum, + double planningSpeedScale = 1.0) { ArgumentNullException.ThrowIfNull(ioAddresses); @@ -36,6 +38,11 @@ public sealed class CompatibilityRobotSettings throw new ArgumentOutOfRangeException(nameof(jerkLimitScale), "Jerk 倍率必须大于 0。"); } + if (planningSpeedScale <= 0.0 || double.IsNaN(planningSpeedScale) || double.IsInfinity(planningSpeedScale)) + { + throw new ArgumentOutOfRangeException(nameof(planningSpeedScale), "规划速度倍率必须是有限正数。"); + } + if (adaptIcspTryNum < 0) { throw new ArgumentOutOfRangeException(nameof(adaptIcspTryNum), "补点尝试次数不能为负数。"); @@ -54,6 +61,7 @@ public sealed class CompatibilityRobotSettings AccLimitScale = accLimitScale; JerkLimitScale = jerkLimitScale; AdaptIcspTryNum = adaptIcspTryNum; + PlanningSpeedScale = planningSpeedScale; } /// @@ -81,6 +89,11 @@ public sealed class CompatibilityRobotSettings /// public double JerkLimitScale { get; } + /// + /// 获取规划阶段的全局速度倍率，只影响 JointTraj 基准时间，不等同于运行时 J519 下发速度倍率。 + /// + public double PlanningSpeedScale { get; } + /// /// 获取自适应补点最大尝试次数。 /// @@ -131,6 +144,17 @@ public sealed class LoadedRobotConfig /// public sealed class RobotConfigLoader { + private readonly ILogger? _logger; + + /// + /// 初始化 RobotConfigLoader。 + /// + /// 日志记录器；允许 null。 + public RobotConfigLoader(ILogger? logger = null) + { + _logger = logger; + } + /// /// 加载一份旧版 RobotConfig.json。 /// @@ -139,6 +163,8 @@ public sealed class RobotConfigLoader /// 规范化后的配置文档。 public LoadedRobotConfig Load(string configPath, string? repoRoot = null) { + _logger?.LogInformation("RobotConfig 开始加载: configPath={ConfigPath}, repoRoot={RepoRoot}", configPath, repoRoot); + var resolvedRepoRoot = ResolveRepoRoot(repoRoot); var resolvedConfigPath = PathCompatibility.ResolveConfigPath(configPath, resolvedRepoRoot); @@ -153,7 +179,8 @@ public sealed class RobotConfigLoader ioKeepCycles: ReadInt(robotElement, "io_keep_cycles", defaultValue: 0), accLimitScale: ReadDouble(robotElement, "acc_limit", defaultValue: 1.0), jerkLimitScale: ReadDouble(robotElement, "jerk_limit", defaultValue: 1.0), - adaptIcspTryNum: ReadInt(robotElement, "adapt_icsp_try_num", defaultValue: 0)); + adaptIcspTryNum: ReadInt(robotElement, "adapt_icsp_try_num", defaultValue: 0), + planningSpeedScale: ReadDouble(robotElement, "planning_speed_scale", defaultValue: 1.0)); var programs = new Dictionary(StringComparer.Ordinal); foreach (var programElement in flyingShotsElement.EnumerateObject()) @@ -163,6 +190,10 @@ public sealed class RobotConfigLoader programs.Add(programName, program); } + _logger?.LogInformation( + "RobotConfig 加载完成: resolvedPath={ResolvedPath}, useDo={UseDo}, ioKeepCycles={IoKeepCycles}, accLimit={AccLimit}, jerkLimit={JerkLimit}, planningSpeedScale={PlanningSpeedScale}, adaptIcspTryNum={AdaptIcspTryNum}, 程序数={ProgramCount}", + resolvedConfigPath, robot.UseDo, robot.IoKeepCycles, robot.AccLimitScale, robot.JerkLimitScale, robot.PlanningSpeedScale, robot.AdaptIcspTryNum, programs.Count); + return new LoadedRobotConfig( sourcePath: resolvedConfigPath, robot: robot, diff --git a/src/Flyshot.Core.Config/RobotModelLoader.cs b/src/Flyshot.Core.Config/RobotModelLoader.cs index 06771ad..701e68b 100644 --- a/src/Flyshot.Core.Config/RobotModelLoader.cs +++ b/src/Flyshot.Core.Config/RobotModelLoader.cs @@ -1,6 +1,7 @@ using System.Text; using System.Text.Json; using Flyshot.Core.Domain; +using Microsoft.Extensions.Logging; namespace Flyshot.Core.Config; @@ -10,6 +11,16 @@ namespace Flyshot.Core.Config; public sealed class RobotModelLoader { private const uint JsonChunkType = 0x4E4F534A; + private readonly ILogger? _logger; + + /// + /// 初始化 RobotModelLoader。 + /// + /// 日志记录器；允许 null。 + public RobotModelLoader(ILogger? logger = null) + { + _logger = logger; + } /// /// 加载 .robot 文件并生成规划侧可直接消费的 RobotProfile。 @@ -35,6 +46,8 @@ public sealed class RobotModelLoader throw new ArgumentOutOfRangeException(nameof(jerkLimitScale), "Jerk 倍率必须大于 0。"); } + _logger?.LogInformation("RobotModel 开始加载: modelPath={ModelPath}, accLimitScale={AccLimitScale}, jerkLimitScale={JerkLimitScale}", modelPath, accLimitScale, jerkLimitScale); + var resolvedModelPath = Path.GetFullPath(modelPath); var jsonText = ReadJsonChunk(resolvedModelPath); using var document = JsonDocument.Parse(jsonText); @@ -76,6 +89,10 @@ public sealed class RobotModelLoader } } + _logger?.LogInformation( + "RobotModel 加载完成: profileName={ProfileName}, dof={Dof}, 关节限制数={JointLimitCount}, couple数={CouplingCount}, resolvedPath={ResolvedPath}", + profileName, jointLimits.Count, jointLimits.Count, jointCouplings.Count, resolvedModelPath); + return new RobotProfile( name: profileName, modelPath: resolvedModelPath, @@ -156,6 +173,8 @@ public sealed class RobotModelLoader throw new ArgumentException(".robot 路径不能为空。", nameof(modelPath)); } + _logger?.LogInformation("RobotKinematicsModel 开始加载: modelPath={ModelPath}", modelPath); + var resolvedModelPath = Path.GetFullPath(modelPath); var jsonText = ReadJsonChunk(resolvedModelPath); using var document = JsonDocument.Parse(jsonText); @@ -203,6 +222,8 @@ public sealed class RobotModelLoader coupleOffset: coupleOffset)); } + _logger?.LogInformation("RobotKinematicsModel 加载完成: profileName={ProfileName}, 关节数={JointCount}", profileName, joints.Count); + return new RobotKinematicsModel(name: profileName, joints: joints); } diff --git a/src/Flyshot.Core.Planning/Export/TrajectoryExporter.cs b/src/Flyshot.Core.Planning/Export/TrajectoryExporter.cs index 19f1a4f..60cb059 100644 --- a/src/Flyshot.Core.Planning/Export/TrajectoryExporter.cs +++ b/src/Flyshot.Core.Planning/Export/TrajectoryExporter.cs @@ -15,12 +15,28 @@ namespace Flyshot.Core.Planning.Export; /// public static class TrajectoryExporter { + /// + /// 导出规划关节轨迹关键点到文本文件。 + /// + public static void WriteJointTrajectory(string path, IReadOnlyList> rows) + { + WriteRows(path, rows); + } + /// /// 导出稠密关节轨迹到文本文件。 /// public static void WriteJointDenseTrajectory(string path, IReadOnlyList> rows) { - WriteDenseRows(path, rows); + WriteRows(path, rows); + } + + /// + /// 导出规划笛卡尔轨迹关键点到文本文件。 + /// + public static void WriteCartesianTrajectory(string path, IReadOnlyList> rows) + { + WriteRows(path, rows); } /// @@ -28,7 +44,7 @@ public static class TrajectoryExporter /// public static void WriteCartesianDenseTrajectory(string path, IReadOnlyList> rows) { - WriteDenseRows(path, rows); + WriteRows(path, rows); } /// @@ -53,7 +69,7 @@ public static class TrajectoryExporter File.WriteAllText(path, json, new UTF8Encoding(false)); } - private static void WriteDenseRows(string path, IReadOnlyList> rows) + private static void WriteRows(string path, IReadOnlyList> rows) { var sb = new StringBuilder(); foreach (var row in rows) diff --git a/src/Flyshot.Core.Triggering/Flyshot.Core.Triggering.csproj b/src/Flyshot.Core.Triggering/Flyshot.Core.Triggering.csproj index d3171ec..e0cc473 100644 --- a/src/Flyshot.Core.Triggering/Flyshot.Core.Triggering.csproj +++ b/src/Flyshot.Core.Triggering/Flyshot.Core.Triggering.csproj @@ -9,6 +9,7 @@ + diff --git a/src/Flyshot.Core.Triggering/ShotTimelineBuilder.cs b/src/Flyshot.Core.Triggering/ShotTimelineBuilder.cs index 7d91b67..4c84ef0 100644 --- a/src/Flyshot.Core.Triggering/ShotTimelineBuilder.cs +++ b/src/Flyshot.Core.Triggering/ShotTimelineBuilder.cs @@ -1,5 +1,6 @@ using Flyshot.Core.Domain; using Flyshot.Core.Planning; +using Microsoft.Extensions.Logging; namespace Flyshot.Core.Triggering; @@ -10,13 +11,17 @@ namespace Flyshot.Core.Triggering; public sealed class ShotTimelineBuilder { private readonly WaypointTimestampResolver _resolver; + private readonly ILogger? _logger; /// /// 初始化 ShotTimelineBuilder，依赖一个时间戳解析器来对齐补中点后的轨迹与原始示教点。 /// - public ShotTimelineBuilder(WaypointTimestampResolver resolver) + /// 时间戳解析器。 + /// 日志记录器；允许 null。 + public ShotTimelineBuilder(WaypointTimestampResolver resolver, ILogger? logger = null) { _resolver = resolver ?? throw new ArgumentNullException(nameof(resolver)); + _logger = logger; } /// @@ -82,6 +87,13 @@ public sealed class ShotTimelineBuilder } } + _logger?.LogInformation( + "ShotTimeline 构建完成: shotFlags总数={ShotFlagCount}, 触发事件数={TriggerCount}, useDo={UseDo}, holdCycles={HoldCycles}", + program.ShotFlags.Count(static f => f), + triggerTimeline.Count, + useDo, + holdCycles); + return new ShotTimeline(shotEvents, triggerTimeline); } } diff --git a/src/Flyshot.Runtime.Fanuc/Protocol/FanucCommandClient.cs b/src/Flyshot.Runtime.Fanuc/Protocol/FanucCommandClient.cs index a2a38e4..3a17cbd 100644 --- a/src/Flyshot.Runtime.Fanuc/Protocol/FanucCommandClient.cs +++ b/src/Flyshot.Runtime.Fanuc/Protocol/FanucCommandClient.cs @@ -122,9 +122,12 @@ public sealed class FanucCommandClient : IDisposable /// 程序名。 /// 取消令牌。 /// 结果响应。 - public Task StopProgramAsync(string programName, CancellationToken cancellationToken = default) + public async Task StopProgramAsync(string programName, CancellationToken cancellationToken = default) { - return SendProgramCommandAsync(FanucCommandMessageIds.StopProgram, programName, cancellationToken); + _logger?.LogInformation("CommandClient StopProgram: {ProgramName}", programName); + var result = await SendProgramCommandAsync(FanucCommandMessageIds.StopProgram, programName, cancellationToken).ConfigureAwait(false); + _logger?.LogDebug("CommandClient StopProgram 成功: {ProgramName}", programName); + return result; } /// @@ -134,8 +137,10 @@ public sealed class FanucCommandClient : IDisposable /// 结果响应。 public async Task ResetRobotAsync(CancellationToken cancellationToken = default) { + _logger?.LogInformation("CommandClient ResetRobot"); var frame = FanucCommandProtocol.PackEmptyCommand(FanucCommandMessageIds.ResetRobot); var response = await SendRawFrameAsync(frame, cancellationToken).ConfigureAwait(false); + _logger?.LogDebug("CommandClient ResetRobot 成功"); return EnsureSuccess(FanucCommandProtocol.ParseResultResponse(response)); } @@ -147,8 +152,10 @@ public sealed class FanucCommandClient : IDisposable /// 程序状态响应。 public async Task GetProgramStatusAsync(string programName, CancellationToken cancellationToken = default) { + _logger?.LogInformation("CommandClient GetProgramStatus: {ProgramName}", programName); var frame = FanucCommandProtocol.PackProgramCommand(FanucCommandMessageIds.GetProgramStatus, programName); var response = await SendRawFrameAsync(frame, cancellationToken).ConfigureAwait(false); + _logger?.LogDebug("CommandClient GetProgramStatus 成功: {ProgramName}", programName); return EnsureSuccess(FanucCommandProtocol.ParseProgramStatusResponse(response)); } @@ -158,9 +165,12 @@ public sealed class FanucCommandClient : IDisposable /// 程序名。 /// 取消令牌。 /// 结果响应。 - public Task StartProgramAsync(string programName, CancellationToken cancellationToken = default) + public async Task StartProgramAsync(string programName, CancellationToken cancellationToken = default) { - return SendProgramCommandAsync(FanucCommandMessageIds.StartProgram, programName, cancellationToken); + _logger?.LogInformation("CommandClient StartProgram: {ProgramName}", programName); + var result = await SendProgramCommandAsync(FanucCommandMessageIds.StartProgram, programName, cancellationToken).ConfigureAwait(false); + _logger?.LogDebug("CommandClient StartProgram 成功: {ProgramName}", programName); + return result; } /// @@ -170,9 +180,12 @@ public sealed class FanucCommandClient : IDisposable /// 速度倍率响应。 public async Task GetSpeedRatioAsync(CancellationToken cancellationToken = default) { + _logger?.LogDebug("CommandClient GetSpeedRatio"); var frame = FanucCommandProtocol.PackGetSpeedRatioCommand(); var response = await SendRawFrameAsync(frame, cancellationToken).ConfigureAwait(false); - return EnsureSuccess(FanucCommandProtocol.ParseSpeedRatioResponse(response)); + var result = EnsureSuccess(FanucCommandProtocol.ParseSpeedRatioResponse(response)); + _logger?.LogDebug("CommandClient GetSpeedRatio 成功: ratio={Ratio}", result.Ratio); + return result; } /// @@ -183,8 +196,10 @@ public sealed class FanucCommandClient : IDisposable /// 结果响应。 public async Task SetSpeedRatioAsync(double ratio, CancellationToken cancellationToken = default) { + _logger?.LogInformation("CommandClient SetSpeedRatio: ratio={Ratio}", ratio); var frame = FanucCommandProtocol.PackSetSpeedRatioCommand(ratio); var response = await SendRawFrameAsync(frame, cancellationToken).ConfigureAwait(false); + _logger?.LogDebug("CommandClient SetSpeedRatio 成功: ratio={Ratio}", ratio); return EnsureSuccess(FanucCommandProtocol.ParseResultResponse(response)); } @@ -196,9 +211,12 @@ public sealed class FanucCommandClient : IDisposable /// TCP 位姿响应。 public async Task GetTcpAsync(uint tcpId = 1, CancellationToken cancellationToken = default) { + _logger?.LogDebug("CommandClient GetTcp: tcpId={TcpId}", tcpId); var frame = FanucCommandProtocol.PackGetTcpCommand(tcpId); var response = await SendRawFrameAsync(frame, cancellationToken).ConfigureAwait(false); - return EnsureSuccess(FanucCommandProtocol.ParseTcpResponse(response)); + var result = EnsureSuccess(FanucCommandProtocol.ParseTcpResponse(response)); + _logger?.LogDebug("CommandClient GetTcp 成功: tcpId={TcpId}, pose=[{Pose}]", tcpId, string.Join(", ", result.Pose.Select(v => v.ToString("F2")))); + return result; } /// @@ -210,8 +228,10 @@ public sealed class FanucCommandClient : IDisposable /// 结果响应。 public async Task SetTcpAsync(uint tcpId, IReadOnlyList pose, CancellationToken cancellationToken = default) { + _logger?.LogInformation("CommandClient SetTcp: tcpId={TcpId}, pose=[{Pose}]", tcpId, string.Join(", ", pose.Take(3).Select(v => v.ToString("F2")))); var frame = FanucCommandProtocol.PackSetTcpCommand(tcpId, pose); var response = await SendRawFrameAsync(frame, cancellationToken).ConfigureAwait(false); + _logger?.LogDebug("CommandClient SetTcp 成功: tcpId={TcpId}", tcpId); return EnsureSuccess(FanucCommandProtocol.ParseResultResponse(response)); } @@ -224,9 +244,12 @@ public sealed class FanucCommandClient : IDisposable /// IO 读取响应。 public async Task GetIoAsync(int port, string ioType, CancellationToken cancellationToken = default) { + _logger?.LogDebug("CommandClient GetIo: port={Port}, ioType={IoType}", port, ioType); var frame = FanucCommandProtocol.PackGetIoCommand(FanucIoTypes.FromName(ioType), port); var response = await SendRawFrameAsync(frame, cancellationToken).ConfigureAwait(false); - return EnsureSuccess(FanucCommandProtocol.ParseIoResponse(response)); + var result = EnsureSuccess(FanucCommandProtocol.ParseIoResponse(response)); + _logger?.LogDebug("CommandClient GetIo 成功: port={Port}, value={Value}", port, result.Value); + return result; } /// @@ -239,8 +262,10 @@ public sealed class FanucCommandClient : IDisposable /// 结果响应。 public async Task SetIoAsync(int port, bool value, string ioType, CancellationToken cancellationToken = default) { + _logger?.LogInformation("CommandClient SetIo: port={Port}, value={Value}, ioType={IoType}", port, value, ioType); var frame = FanucCommandProtocol.PackSetIoCommand(FanucIoTypes.FromName(ioType), port, value); var response = await SendRawFrameAsync(frame, cancellationToken).ConfigureAwait(false); + _logger?.LogDebug("CommandClient SetIo 成功: port={Port}, value={Value}", port, value); return EnsureSuccess(FanucCommandProtocol.ParseResultResponse(response)); } diff --git a/tests/Flyshot.Core.Tests/ConfigCompatibilityTests.cs b/tests/Flyshot.Core.Tests/ConfigCompatibilityTests.cs index 47e71bd..ed2a3ba 100644 --- a/tests/Flyshot.Core.Tests/ConfigCompatibilityTests.cs +++ b/tests/Flyshot.Core.Tests/ConfigCompatibilityTests.cs @@ -24,6 +24,7 @@ public sealed class ConfigCompatibilityTests Assert.Equal(2, loaded.Robot.IoKeepCycles); Assert.Equal(1.0, loaded.Robot.AccLimitScale); Assert.Equal(1.0, loaded.Robot.JerkLimitScale); + Assert.Equal(1.0, loaded.Robot.PlanningSpeedScale); Assert.Equal(5, loaded.Robot.AdaptIcspTryNum); var program = Assert.Contains("EOL10_EAU_0", loaded.Programs); @@ -71,6 +72,7 @@ public sealed class ConfigCompatibilityTests Assert.Equal(3, loaded.Robot.IoKeepCycles); Assert.Equal(0.5, loaded.Robot.AccLimitScale); Assert.Equal(0.25, loaded.Robot.JerkLimitScale); + Assert.Equal(1.0, loaded.Robot.PlanningSpeedScale); Assert.Equal([0, 0, 0], program.OffsetValues); Assert.All(program.AddressGroups, group => Assert.Empty(group.Addresses)); } @@ -80,6 +82,46 @@ public sealed class ConfigCompatibilityTests } } + /// + /// 验证 RobotConfig.json 可以显式配置规划限速倍率，且该倍率独立于运行时 J519 速度倍率。 + /// + [Fact] + public void RobotConfigLoader_LoadsPlanningSpeedScale() + { + var tempRoot = CreateTempDirectory(); + try + { + var configPath = Path.Combine(tempRoot, "legacy.json"); + File.WriteAllText( + configPath, + """ + { + "robot": { + "use_do": true, + "io_keep_cycles": 2, + "acc_limit": 1.0, + "jerk_limit": 1.0, + "planning_speed_scale": 0.742277 + }, + "flying_shots": { + "demo": { + "traj_waypoints": [[0, 1], [2, 3], [4, 5], [6, 7]], + "shot_flags": [false, false, false, false] + } + } + } + """); + + var loaded = new RobotConfigLoader().Load(configPath); + + Assert.Equal(0.742277, loaded.Robot.PlanningSpeedScale, precision: 6); + } + finally + { + Directory.Delete(tempRoot, recursive: true); + } + } + /// /// 验证 .robot 解析会保留 Joint3 对 Joint2 的 couple 元数据，并构造规划侧可直接消费的 RobotProfile。 /// diff --git a/tests/Flyshot.Core.Tests/ControllerClientCompatConfigRootTests.cs b/tests/Flyshot.Core.Tests/ControllerClientCompatConfigRootTests.cs index e5d3af1..f4c7938 100644 --- a/tests/Flyshot.Core.Tests/ControllerClientCompatConfigRootTests.cs +++ b/tests/Flyshot.Core.Tests/ControllerClientCompatConfigRootTests.cs @@ -57,10 +57,10 @@ public sealed class ControllerClientCompatConfigRootTests } /// - /// 验证 JSON 轨迹存储保存、加载和删除都落在 ConfigRoot/TrajectoryStore 目录。 + /// 验证 JSON 轨迹存储保存、加载和删除都落在 ConfigRoot/RobotConfig.json。 /// [Fact] - public void JsonFlyshotTrajectoryStore_PersistsTrajectoriesUnderConfigRootStore() + public void JsonFlyshotTrajectoryStore_PersistsTrajectoriesInRobotConfigJson() { var configRoot = CreateTempConfigRoot(); try @@ -77,9 +77,10 @@ public sealed class ControllerClientCompatConfigRootTests var trajectory = TestRobotFactory.CreateUploadedTrajectoryWithSingleShot(); store.Save("FANUC_LR_Mate_200iD", settings, trajectory); - var expectedPath = Path.Combine(configRoot, "TrajectoryStore", "FANUC_LR_Mate_200iD_trajectories.json"); + var expectedPath = Path.Combine(configRoot, "RobotConfig.json"); - Assert.True(File.Exists(expectedPath), $"应在运行目录 Config 下创建轨迹文件: {expectedPath}"); + Assert.True(File.Exists(expectedPath), $"应在运行目录 Config 下创建统一配置文件: {expectedPath}"); + Assert.False(Directory.Exists(Path.Combine(configRoot, "TrajectoryStore")), "不应再创建独立轨迹存储目录。"); var loaded = store.LoadAll("FANUC_LR_Mate_200iD", out var loadedSettings); Assert.NotNull(loadedSettings); Assert.Contains(trajectory.Name, loaded); diff --git a/tests/Flyshot.Core.Tests/FanucControllerRuntimeDenseTests.cs b/tests/Flyshot.Core.Tests/FanucControllerRuntimeDenseTests.cs index 6d7cb7e..4dddef0 100644 --- a/tests/Flyshot.Core.Tests/FanucControllerRuntimeDenseTests.cs +++ b/tests/Flyshot.Core.Tests/FanucControllerRuntimeDenseTests.cs @@ -555,8 +555,7 @@ public sealed class FanucControllerRuntimeDenseTests new ControllerClientCompatRobotCatalog(options, new RobotModelLoader()), runtime, new ControllerClientTrajectoryOrchestrator(), - new RobotConfigLoader(), - new InMemoryFlyshotTrajectoryStore()); + new RobotConfigLoader()); } private static double ComputeLineAlpha( diff --git a/tests/Flyshot.Core.Tests/RuntimeOrchestrationTests.cs b/tests/Flyshot.Core.Tests/RuntimeOrchestrationTests.cs index 08fb423..55c8816 100644 --- a/tests/Flyshot.Core.Tests/RuntimeOrchestrationTests.cs +++ b/tests/Flyshot.Core.Tests/RuntimeOrchestrationTests.cs @@ -136,6 +136,42 @@ public sealed class RuntimeOrchestrationTests Assert.Single(bundle.Result.TriggerTimeline); } + /// + /// 验证飞拍规划会把规划限速倍率纳入速度/加速度/Jerk 限制，而不是复用运行时下发倍率。 + /// + [Fact] + public void ControllerClientTrajectoryOrchestrator_PlanUploadedFlyshot_AppliesPlanningSpeedScaleToLimits() + { + var orchestrator = new ControllerClientTrajectoryOrchestrator(); + var robot = TestRobotFactory.CreateRobotProfile(); + var uploaded = TestRobotFactory.CreateUploadedTrajectoryWithSingleShot(); + + var fullSpeed = orchestrator.PlanUploadedFlyshot(robot, uploaded, planningSpeedScale: 1.0); + var halfSpeed = orchestrator.PlanUploadedFlyshot(robot, uploaded, planningSpeedScale: 0.5); + + Assert.True( + halfSpeed.Result.Duration.TotalSeconds > fullSpeed.Result.Duration.TotalSeconds * 1.9, + $"半速规划时长应接近全速的 2 倍，实际 full={fullSpeed.Result.Duration.TotalSeconds}, half={halfSpeed.Result.Duration.TotalSeconds}"); + } + + /// + /// 验证飞拍缓存键包含规划限速倍率，避免降速验证时误用 100% 速度下的规划结果。 + /// + [Fact] + public void ControllerClientTrajectoryOrchestrator_PlanUploadedFlyshot_CacheKeyIncludesPlanningSpeedScale() + { + var orchestrator = new ControllerClientTrajectoryOrchestrator(); + var robot = TestRobotFactory.CreateRobotProfile(); + var uploaded = TestRobotFactory.CreateUploadedTrajectoryWithSingleShot(); + var options = new FlyshotExecutionOptions(useCache: true); + + var fullSpeed = orchestrator.PlanUploadedFlyshot(robot, uploaded, options, planningSpeedScale: 1.0); + var halfSpeed = orchestrator.PlanUploadedFlyshot(robot, uploaded, options, planningSpeedScale: 0.5); + + Assert.False(halfSpeed.Result.UsedCache); + Assert.True(halfSpeed.Result.Duration > fullSpeed.Result.Duration); + } + /// /// 验证飞拍编排会使用 RobotConfig.json 中的 IO 保持周期。 /// @@ -289,8 +325,7 @@ public sealed class RuntimeOrchestrationTests new ControllerClientCompatRobotCatalog(options, new RobotModelLoader()), runtime, new ControllerClientTrajectoryOrchestrator(), - new RobotConfigLoader(), - new InMemoryFlyshotTrajectoryStore()); + new RobotConfigLoader()); service.SetUpRobot("FANUC_LR_Mate_200iD"); @@ -304,6 +339,49 @@ public sealed class RuntimeOrchestrationTests } } + /// + /// 验证 IsFlyshotTrajectoryValid(saveTrajectory=true) 会把规划后的结果点位导出到 Config/Data/name。 + /// + [Fact] + public void ControllerClientCompatService_IsFlyshotTrajectoryValid_SaveTrajectoryExportsPlannedData() + { + var configRoot = CreateTempConfigRoot(); + try + { + WriteRobotConfigWithDemoTrajectory(configRoot); + var options = new ControllerClientCompatOptions { ConfigRoot = configRoot }; + var service = new ControllerClientCompatService( + options, + new ControllerClientCompatRobotCatalog(options, new RobotModelLoader()), + new RecordingControllerRuntime(), + new ControllerClientTrajectoryOrchestrator(), + new RobotConfigLoader()); + + service.SetUpRobot("FANUC_LR_Mate_200iD"); + + var valid = service.IsFlyshotTrajectoryValid( + out var duration, + "demo-flyshot", + method: "icsp", + saveTrajectory: true); + + var outputDir = Path.Combine(configRoot, "Data", "demo-flyshot"); + Assert.True(valid); + Assert.True(duration > TimeSpan.Zero); + Assert.True(File.Exists(Path.Combine(outputDir, "JointTraj.txt"))); + Assert.True(File.Exists(Path.Combine(outputDir, "JointDetialTraj.txt"))); + Assert.True(File.Exists(Path.Combine(outputDir, "CartTraj.txt"))); + Assert.True(File.Exists(Path.Combine(outputDir, "CartDetialTraj.txt"))); + Assert.True(File.Exists(Path.Combine(outputDir, "ShotEvents.json"))); + Assert.NotEmpty(File.ReadAllLines(Path.Combine(outputDir, "JointDetialTraj.txt"))); + Assert.NotEmpty(File.ReadAllLines(Path.Combine(outputDir, "CartDetialTraj.txt"))); + } + finally + { + Directory.Delete(configRoot, recursive: true); + } + } + /// /// 创建只包含当前支持机器人模型和 RobotConfig.json 的临时运行配置根。 /// @@ -322,6 +400,41 @@ public sealed class RuntimeOrchestrationTests return configRoot; } + + /// + /// 写入包含一条飞拍轨迹的最小 RobotConfig.json，供兼容服务从统一配置恢复轨迹。 + /// + /// 测试运行配置根。 + private static void WriteRobotConfigWithDemoTrajectory(string configRoot) + { + File.WriteAllText( + Path.Combine(configRoot, "RobotConfig.json"), + """ + { + "robot": { + "use_do": true, + "io_addr": [7, 8], + "io_keep_cycles": 2, + "acc_limit": 1.0, + "jerk_limit": 1.0, + "adapt_icsp_try_num": 5 + }, + "flying_shots": { + "demo-flyshot": { + "traj_waypoints": [ + [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] + ], + "shot_flags": [false, true, false, false], + "offset_values": [0, 1, 0, 0], + "addr": [[], [7, 8], [], []] + } + } + } + """); + } } /// @@ -389,8 +502,7 @@ internal static class TestRobotFactory new ControllerClientCompatRobotCatalog(options, new RobotModelLoader()), new FanucControllerRuntime(), new ControllerClientTrajectoryOrchestrator(), - new RobotConfigLoader(), - new InMemoryFlyshotTrajectoryStore()); + new RobotConfigLoader()); } /// @@ -443,33 +555,6 @@ internal static class TestRobotFactory } } -/// -/// 内存中的轨迹存储实现，用于避免单元测试污染真实文件系统。 -/// -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 入参的测试运行时，用于验证兼容服务传递的机器人配置。 /// diff --git a/tests/Flyshot.Server.IntegrationTests/StatusEndpointTests.cs b/tests/Flyshot.Server.IntegrationTests/StatusEndpointTests.cs index 1e78f3c..cfcdcdc 100644 --- a/tests/Flyshot.Server.IntegrationTests/StatusEndpointTests.cs +++ b/tests/Flyshot.Server.IntegrationTests/StatusEndpointTests.cs @@ -61,7 +61,12 @@ public sealed class StatusEndpointTests(FlyshotServerFactory factory) : IClassFi Assert.True(root.GetProperty("isSetup").GetBoolean()); Assert.Equal("FANUC_LR_Mate_200iD", root.GetProperty("robotName").GetString()); Assert.Equal(6, root.GetProperty("degreesOfFreedom").GetInt32()); - Assert.Empty(root.GetProperty("uploadedTrajectories").EnumerateArray()); + var uploadedTrajectories = root.GetProperty("uploadedTrajectories") + .EnumerateArray() + .Select(static value => value.GetString()) + .ToArray(); + Assert.Contains("20251015", uploadedTrajectories); + Assert.Contains("UTTC_MS11", uploadedTrajectories); Assert.Equal("Connected", snapshot.GetProperty("connectionState").GetString()); Assert.True(snapshot.GetProperty("isEnabled").GetBoolean()); Assert.False(snapshot.GetProperty("isInMotion").GetBoolean());