feat(*): 添加轨迹产物导出与规划速度倍率隔离

* 新增 FlyshotTrajectoryArtifactWriter,支持 saveTrajectory
  将规划结果导出到 Config/Data/name(JointTraj、CartTraj、
  ShotEvents 等)
* RobotConfig 新增 PlanningSpeedScale,区分规划阶段限速倍率
  与运行时 J519 下发倍率
* 轨迹缓存键纳入 planningSpeedScale,避免降速规划误用缓存
* 完善 FanucCommandClient 命令参数日志与状态通道重连
* 补充 RuntimeOrchestrationTests 覆盖产物导出与倍率隔离
* 更新 README 进度文档
This commit is contained in:
2026-04-30 13:52:09 +08:00
parent a6579f1e5b
commit 91c1494cde
20 changed files with 593 additions and 133 deletions

View File

@@ -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(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'\\)\\); 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(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')"
] ]
} }
} }

View File

@@ -18,9 +18,10 @@
- `ExecuteTrajectory``ExecuteFlyShotTraj` 已经接入 `Planning + Triggering + Runtime` 链路Web 状态页已通过 `/status``/api/status/snapshot` 暴露当前兼容层与运行时状态。 - `ExecuteTrajectory``ExecuteFlyShotTraj` 已经接入 `Planning + Triggering + Runtime` 链路Web 状态页已通过 `/status``/api/status/snapshot` 暴露当前兼容层与运行时状态。
- `Flyshot.Core.Planning` 的 ICSP / self-adapt-icsp 轨迹已经完成旧系统导出轨迹对齐;`doubles` 仍未实现。 - `Flyshot.Core.Planning` 的 ICSP / self-adapt-icsp 轨迹已经完成旧系统导出轨迹对齐;`doubles` 仍未实现。
- `Flyshot.Runtime.Fanuc` 已固化 `10010 / 10012 / 60015` 基础协议帧编解码。`10010` 状态通道以 `j519 协议.pcap``Rvbust/uttc-20260428/20260428.pcap` 真机抓包确认为 90B 固定帧。 - `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 全流程现场联调仍需执行。 - 真机 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` 下发。 - `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] 使用本地 TCP/UDP 模拟器覆盖命令通道、状态通道和 J519 基础收发
- [x] 补齐 `Get/SetSpeedRatio``Get/SetTCP``Get/SetIO` 真机命令体与响应解析 - [x] 补齐 `Get/SetSpeedRatio``Get/SetTCP``Get/SetIO` 真机命令体与响应解析
- [x] 保留新 HTTP 接口路线,明确不再实现旧 `50001/TCP+JSON` 网关 - [x] 保留新 HTTP 接口路线,明确不再实现旧 `50001/TCP+JSON` 网关
- [x] 将飞拍轨迹持久化收敛到运行目录 `Config/RobotConfig.json`
剩余 Todo 剩余 Todo
1. 配置与测试基线 1. 配置与测试基线
- [x] 修正 `ConfigCompatibilityTests` 当前样本路径漂移:`Rvbust/EOL10_EAU_0/RobotConfig.json` 不再包含 `001`,应改用稳定样本或更新断言。 - [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]`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` - [x] 为新 HTTP API 补一份当前现场调用顺序文档,替代旧 `ControllerClient` 工作流:见 `docs/fanuc-field-runtime-workflow.md`
2. 轨迹规划 2. 轨迹规划
@@ -58,7 +61,7 @@
- [ ] 如果现场仍需要 `method="doubles"`,实现 `TrajectoryDoubleS` 等价规划;否则在 HTTP 文档中明确标为不支持。 - [ ] 如果现场仍需要 `method="doubles"`,实现 `TrajectoryDoubleS` 等价规划;否则在 HTTP 文档中明确标为不支持。
- [ ] 把已完成对齐的旧系统轨迹样本固化为 golden tests防止后续重构破坏轨迹一致性。 - [ ] 把已完成对齐的旧系统轨迹样本固化为 golden tests防止后续重构破坏轨迹一致性。
- [x]`Rvbust/uttc-20260428/Data/JointDetialTraj.txt` 固化为 J519 golden 样本:输入为 `rad`,下发为 `deg`,并按 `speed_ratio` 拉伸时间轴;覆盖 `2026042802-0.5/0.7/1.pcap` - [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/<name>`
3. FANUC TCP 10012 命令通道 3. FANUC TCP 10012 命令通道
- [x] 补齐 `GetSpeedRatio` / `SetSpeedRatio` 真机命令体与响应解析。 - [x] 补齐 `GetSpeedRatio` / `SetSpeedRatio` 真机命令体与响应解析。
@@ -82,6 +85,7 @@
6. 真机联调与运行安全 6. 真机联调与运行安全
- [ ] 在真实 R30iB + `RVBUSTSM` 程序上验证 `Connect -> EnableRobot -> ExecuteFlyShotTraj -> StopMove -> DisableRobot -> Disconnect` 全流程。 - [ ] 在真实 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 执行接口增加运行互斥、执行中拒绝重复轨迹、取消和超时控制。 - [ ] 给 HTTP 执行接口增加运行互斥、执行中拒绝重复轨迹、取消和超时控制。
- [ ] 增加运行日志、协议摘要日志和状态快照导出,便于现场排查。 - [ ] 增加运行日志、协议摘要日志和状态快照导出,便于现场排查。

View File

@@ -17,7 +17,8 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi
private readonly IControllerRuntime _runtime; private readonly IControllerRuntime _runtime;
private readonly ControllerClientTrajectoryOrchestrator _trajectoryOrchestrator; private readonly ControllerClientTrajectoryOrchestrator _trajectoryOrchestrator;
private readonly RobotConfigLoader _configLoader; private readonly RobotConfigLoader _configLoader;
private readonly IFlyshotTrajectoryStore _trajectoryStore; private readonly FlyshotTrajectoryArtifactWriter _artifactWriter;
private readonly JsonFlyshotTrajectoryStore _trajectoryStore;
private readonly ILogger<ControllerClientCompatService>? _logger; private readonly ILogger<ControllerClientCompatService>? _logger;
private RobotProfile? _activeRobotProfile; private RobotProfile? _activeRobotProfile;
private string? _configuredRobotName; private string? _configuredRobotName;
@@ -36,7 +37,8 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi
/// <param name="runtime">控制器运行时。</param> /// <param name="runtime">控制器运行时。</param>
/// <param name="trajectoryOrchestrator">轨迹规划与触发编排器。</param> /// <param name="trajectoryOrchestrator">轨迹规划与触发编排器。</param>
/// <param name="configLoader">旧版 RobotConfig.json 加载器。</param> /// <param name="configLoader">旧版 RobotConfig.json 加载器。</param>
/// <param name="trajectoryStore">已上传轨迹持久化存储。</param> /// <param name="artifactWriter">saveTrajectory 规划结果点位导出器。</param>
/// <param name="trajectoryStore">统一 RobotConfig.json 持久化存储;为空时按配置根目录创建默认实例。</param>
/// <param name="logger">日志记录器;允许测试直接构造时传入 null。</param> /// <param name="logger">日志记录器;允许测试直接构造时传入 null。</param>
public ControllerClientCompatService( public ControllerClientCompatService(
ControllerClientCompatOptions options, ControllerClientCompatOptions options,
@@ -44,7 +46,8 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi
IControllerRuntime runtime, IControllerRuntime runtime,
ControllerClientTrajectoryOrchestrator trajectoryOrchestrator, ControllerClientTrajectoryOrchestrator trajectoryOrchestrator,
RobotConfigLoader configLoader, RobotConfigLoader configLoader,
IFlyshotTrajectoryStore trajectoryStore, FlyshotTrajectoryArtifactWriter? artifactWriter = null,
JsonFlyshotTrajectoryStore? trajectoryStore = null,
ILogger<ControllerClientCompatService>? logger = null) ILogger<ControllerClientCompatService>? logger = null)
{ {
_options = options ?? throw new ArgumentNullException(nameof(options)); _options = options ?? throw new ArgumentNullException(nameof(options));
@@ -52,7 +55,8 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi
_runtime = runtime ?? throw new ArgumentNullException(nameof(runtime)); _runtime = runtime ?? throw new ArgumentNullException(nameof(runtime));
_trajectoryOrchestrator = trajectoryOrchestrator ?? throw new ArgumentNullException(nameof(trajectoryOrchestrator)); _trajectoryOrchestrator = trajectoryOrchestrator ?? throw new ArgumentNullException(nameof(trajectoryOrchestrator));
_configLoader = configLoader ?? throw new ArgumentNullException(nameof(configLoader)); _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; _logger = logger;
} }
@@ -374,7 +378,7 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi
EnsureJointVector(jointPositions, robot.DegreesOfFreedom, nameof(jointPositions)); EnsureJointVector(jointPositions, robot.DegreesOfFreedom, nameof(jointPositions));
var speedRatio = _runtime.GetSnapshot().SpeedRatio; var speedRatio = _runtime.GetSnapshot().SpeedRatio;
var moveResult = MoveJointTrajectoryGenerator.CreateResult(robot, currentJointPositions, jointPositions, speedRatio); var moveResult = MoveJointTrajectoryGenerator.CreateResult(robot, currentJointPositions, jointPositions, speedRatio, _logger);
_logger?.LogInformation( _logger?.LogInformation(
"MoveJoint 规划完成: 当前速度倍率={SpeedRatio}, 规划时长={Duration}s, 采样点数={SampleCount}", "MoveJoint 规划完成: 当前速度倍率={SpeedRatio}, 规划时长={Duration}s, 采样点数={SampleCount}",
speedRatio, speedRatio,
@@ -407,13 +411,15 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi
EnsureRuntimeEnabled(); EnsureRuntimeEnabled();
// 普通轨迹必须按调用方指定 method 规划,再把规划结果交给运行时执行。 // 普通轨迹必须按调用方指定 method 规划,再把规划结果交给运行时执行。
var bundle = _trajectoryOrchestrator.PlanOrdinaryTrajectory(robot, waypoints, options); var planningSpeedScale = RequireRobotSettings().PlanningSpeedScale;
var bundle = _trajectoryOrchestrator.PlanOrdinaryTrajectory(robot, waypoints, options, planningSpeedScale);
_logger?.LogInformation( _logger?.LogInformation(
"ExecuteTrajectory 规划完成: method={Method}, 时长={Duration}s, 有效={IsValid}, 采样点数={SampleCount}", "ExecuteTrajectory 规划完成: method={Method}, 时长={Duration}s, 有效={IsValid}, 采样点数={SampleCount}, planningSpeedScale={PlanningSpeedScale}",
bundle.Result.Method, bundle.Result.Method,
bundle.Result.Duration.TotalSeconds, bundle.Result.Duration.TotalSeconds,
bundle.Result.IsValid, bundle.Result.IsValid,
bundle.Result.DenseJointTrajectory?.Count ?? 0); bundle.Result.DenseJointTrajectory?.Count ?? 0,
planningSpeedScale);
var finalJointPositions = bundle.PlannedTrajectory.PlannedWaypoints[^1].Positions; var finalJointPositions = bundle.PlannedTrajectory.PlannedWaypoints[^1].Positions;
_runtime.ExecuteTrajectory(bundle.Result, finalJointPositions); _runtime.ExecuteTrajectory(bundle.Result, finalJointPositions);
} }
@@ -495,14 +501,17 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi
} }
// 已上传飞拍轨迹必须按调用方指定 method 生成 shot timeline 后再交给运行时。 // 已上传飞拍轨迹必须按调用方指定 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( _logger?.LogInformation(
"ExecuteTrajectoryByName 规划完成: name={Name}, method={Method}, 时长={Duration}s, 触发事件数={TriggerCount}, 使用缓存={UsedCache}", "ExecuteTrajectoryByName 规划完成: name={Name}, method={Method}, 时长={Duration}s, 触发事件数={TriggerCount}, 使用缓存={UsedCache}, planningSpeedScale={PlanningSpeedScale}",
name, name,
bundle.Result.Method, bundle.Result.Method,
bundle.Result.Duration.TotalSeconds, bundle.Result.Duration.TotalSeconds,
bundle.Result.TriggerTimeline.Count, bundle.Result.TriggerTimeline.Count,
bundle.Result.UsedCache); bundle.Result.UsedCache,
settings.PlanningSpeedScale);
if (options.MoveToStart) if (options.MoveToStart)
{ {
@@ -537,11 +546,14 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi
} }
// 先通过规划校验避免静默接受非法参数,同时把轨迹信息强制刷写到本地 JSON。 // 先通过规划校验避免静默接受非法参数,同时把轨迹信息强制刷写到本地 JSON。
_ = _trajectoryOrchestrator.PlanUploadedFlyshot( var planningSettings = RequireRobotSettings();
var bundle = _trajectoryOrchestrator.PlanUploadedFlyshot(
robot, robot,
trajectory, trajectory,
new FlyshotExecutionOptions(saveTrajectory: true, method: method), 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 robotName = _configuredRobotName ?? throw new InvalidOperationException("Robot has not been setup.");
var settings = _robotSettings ?? CreateDefaultRobotSettings(); var settings = _robotSettings ?? CreateDefaultRobotSettings();
@@ -570,11 +582,14 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi
throw new InvalidOperationException("FlyShot trajectory does not exist."); throw new InvalidOperationException("FlyShot trajectory does not exist.");
} }
var planningSettings = RequireRobotSettings();
var bundle = _trajectoryOrchestrator.PlanUploadedFlyshot( var bundle = _trajectoryOrchestrator.PlanUploadedFlyshot(
robot, robot,
trajectory, trajectory,
new FlyshotExecutionOptions(method: method, saveTrajectory: saveTrajectory), new FlyshotExecutionOptions(method: method, saveTrajectory: saveTrajectory),
RequireRobotSettings()); planningSettings,
planningSettings.PlanningSpeedScale);
ExportFlyshotArtifactsIfRequested(name, saveTrajectory, robot, bundle);
duration = bundle.Result.Duration; duration = bundle.Result.Duration;
_logger?.LogInformation( _logger?.LogInformation(
@@ -708,6 +723,27 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi
plannedWaypointCount: 1); plannedWaypointCount: 1);
} }
/// <summary>
/// 根据 saveTrajectory 参数把规划结果点位写入运行目录 Config/Data/name。
/// </summary>
/// <param name="name">飞拍轨迹名称。</param>
/// <param name="saveTrajectory">是否导出规划结果点位。</param>
/// <param name="robot">当前机器人模型。</param>
/// <param name="bundle">规划结果包。</param>
private void ExportFlyshotArtifactsIfRequested(
string name,
bool saveTrajectory,
RobotProfile robot,
PlannedExecutionBundle bundle)
{
if (!saveTrajectory)
{
return;
}
_artifactWriter.WriteUploadedFlyshot(name, robot, bundle);
}
/// <summary> /// <summary>
/// 尝试从配置根目录加载 RobotConfig.json 获取机器人配置;失败时返回 null。 /// 尝试从配置根目录加载 RobotConfig.json 获取机器人配置;失败时返回 null。
/// </summary> /// </summary>

View File

@@ -32,7 +32,8 @@ public static class ControllerClientCompatServiceCollectionExtensions
services.AddSingleton<RobotConfigLoader>(); services.AddSingleton<RobotConfigLoader>();
services.AddSingleton<ControllerClientCompatRobotCatalog>(); services.AddSingleton<ControllerClientCompatRobotCatalog>();
services.AddSingleton<ControllerClientTrajectoryOrchestrator>(); services.AddSingleton<ControllerClientTrajectoryOrchestrator>();
services.AddSingleton<IFlyshotTrajectoryStore, JsonFlyshotTrajectoryStore>(); services.AddSingleton<FlyshotTrajectoryArtifactWriter>();
services.AddSingleton<JsonFlyshotTrajectoryStore>();
services.AddSingleton<IControllerRuntime, FanucControllerRuntime>(); services.AddSingleton<IControllerRuntime, FanucControllerRuntime>();
services.AddSingleton<IControllerClientCompatService, ControllerClientCompatService>(); services.AddSingleton<IControllerClientCompatService, ControllerClientCompatService>();

View File

@@ -38,15 +38,17 @@ public sealed class ControllerClientTrajectoryOrchestrator
public PlannedExecutionBundle PlanOrdinaryTrajectory( public PlannedExecutionBundle PlanOrdinaryTrajectory(
RobotProfile robot, RobotProfile robot,
IReadOnlyList<IReadOnlyList<double>> waypoints, IReadOnlyList<IReadOnlyList<double>> waypoints,
TrajectoryExecutionOptions? options = null) TrajectoryExecutionOptions? options = null,
double planningSpeedScale = 1.0)
{ {
ArgumentNullException.ThrowIfNull(robot); ArgumentNullException.ThrowIfNull(robot);
ArgumentNullException.ThrowIfNull(waypoints); ArgumentNullException.ThrowIfNull(waypoints);
options ??= new TrajectoryExecutionOptions(); options ??= new TrajectoryExecutionOptions();
var planningRobot = ApplyPlanningSpeedScale(robot, planningSpeedScale);
_logger?.LogInformation( _logger?.LogInformation(
"PlanOrdinaryTrajectory 开始: 路点数={WaypointCount}, method={Method}", "PlanOrdinaryTrajectory 开始: 路点数={WaypointCount}, method={Method}, planningSpeedScale={PlanningSpeedScale}",
waypoints.Count, options.Method); waypoints.Count, options.Method, planningSpeedScale);
var program = CreateProgram( var program = CreateProgram(
name: "ordinary-trajectory", name: "ordinary-trajectory",
@@ -57,7 +59,7 @@ public sealed class ControllerClientTrajectoryOrchestrator
var method = ParseOrdinaryMethod(options.Method); var method = ParseOrdinaryMethod(options.Method);
var request = new TrajectoryRequest( var request = new TrajectoryRequest(
robot: robot, robot: planningRobot,
program: program, program: program,
method: method, method: method,
saveTrajectoryArtifacts: options.SaveTrajectory); saveTrajectoryArtifacts: options.SaveTrajectory);
@@ -84,16 +86,19 @@ public sealed class ControllerClientTrajectoryOrchestrator
RobotProfile robot, RobotProfile robot,
ControllerClientCompatUploadedTrajectory uploaded, ControllerClientCompatUploadedTrajectory uploaded,
FlyshotExecutionOptions? options = null, FlyshotExecutionOptions? options = null,
CompatibilityRobotSettings? settings = null) CompatibilityRobotSettings? settings = null,
double? planningSpeedScale = null)
{ {
ArgumentNullException.ThrowIfNull(robot); ArgumentNullException.ThrowIfNull(robot);
ArgumentNullException.ThrowIfNull(uploaded); ArgumentNullException.ThrowIfNull(uploaded);
options ??= new FlyshotExecutionOptions(); options ??= new FlyshotExecutionOptions();
settings ??= CreateDefaultRobotSettings(); settings ??= CreateDefaultRobotSettings();
var effectivePlanningSpeedScale = planningSpeedScale ?? settings.PlanningSpeedScale;
var planningRobot = ApplyPlanningSpeedScale(robot, effectivePlanningSpeedScale);
_logger?.LogInformation( _logger?.LogInformation(
"PlanUploadedFlyshot 开始: name={Name}, waypoints={WaypointCount}, method={Method}, useCache={UseCache}", "PlanUploadedFlyshot 开始: name={Name}, waypoints={WaypointCount}, method={Method}, useCache={UseCache}, planningSpeedScale={PlanningSpeedScale}",
uploaded.Name, uploaded.Waypoints.Count, options.Method, options.UseCache); uploaded.Name, uploaded.Waypoints.Count, options.Method, options.UseCache, effectivePlanningSpeedScale);
var program = CreateProgram( var program = CreateProgram(
name: uploaded.Name, name: uploaded.Name,
@@ -103,7 +108,7 @@ public sealed class ControllerClientTrajectoryOrchestrator
addressGroups: uploaded.AddressGroups); addressGroups: uploaded.AddressGroups);
var method = ParseFlyshotMethod(options.Method); 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)) if (options.UseCache && _flyshotCache.TryGetValue(cacheKey, out var cachedBundle))
{ {
_logger?.LogInformation("PlanUploadedFlyshot 命中缓存: name={Name}, cacheKey={CacheKey}", uploaded.Name, cacheKey); _logger?.LogInformation("PlanUploadedFlyshot 命中缓存: name={Name}, cacheKey={CacheKey}", uploaded.Name, cacheKey);
@@ -115,7 +120,7 @@ public sealed class ControllerClientTrajectoryOrchestrator
} }
var request = new TrajectoryRequest( var request = new TrajectoryRequest(
robot: robot, robot: planningRobot,
program: program, program: program,
method: method, method: method,
moveToStart: options.MoveToStart, moveToStart: options.MoveToStart,
@@ -126,7 +131,7 @@ public sealed class ControllerClientTrajectoryOrchestrator
var shotTimeline = _shotTimelineBuilder.Build( var shotTimeline = _shotTimelineBuilder.Build(
plannedTrajectory, plannedTrajectory,
holdCycles: settings.IoKeepCycles, holdCycles: settings.IoKeepCycles,
samplePeriod: robot.ServoPeriod, samplePeriod: planningRobot.ServoPeriod,
useDo: settings.UseDo); useDo: settings.UseDo);
var result = CreateResult(plannedTrajectory, shotTimeline, usedCache: false); var result = CreateResult(plannedTrajectory, shotTimeline, usedCache: false);
var bundle = new PlannedExecutionBundle(plannedTrajectory, shotTimeline, result); var bundle = new PlannedExecutionBundle(plannedTrajectory, shotTimeline, result);
@@ -219,10 +224,12 @@ public sealed class ControllerClientTrajectoryOrchestrator
RobotProfile robot, RobotProfile robot,
ControllerClientCompatUploadedTrajectory uploaded, ControllerClientCompatUploadedTrajectory uploaded,
FlyshotExecutionOptions options, FlyshotExecutionOptions options,
CompatibilityRobotSettings settings) CompatibilityRobotSettings settings,
double planningSpeedScale)
{ {
var hash = new HashCode(); var hash = new HashCode();
hash.Add(robot.Name, StringComparer.Ordinal); hash.Add(robot.Name, StringComparer.Ordinal);
hash.Add(planningSpeedScale);
hash.Add(uploaded.Name, StringComparer.Ordinal); hash.Add(uploaded.Name, StringComparer.Ordinal);
hash.Add(NormalizeMethod(options.Method), StringComparer.Ordinal); hash.Add(NormalizeMethod(options.Method), StringComparer.Ordinal);
hash.Add(options.MoveToStart); hash.Add(options.MoveToStart);
@@ -231,6 +238,14 @@ public sealed class ControllerClientTrajectoryOrchestrator
hash.Add(settings.IoKeepCycles); hash.Add(settings.IoKeepCycles);
hash.Add(settings.AdaptIcspTryNum); 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 waypoint in uploaded.Waypoints)
{ {
foreach (var value in waypoint) foreach (var value in waypoint)
@@ -275,6 +290,43 @@ public sealed class ControllerClientTrajectoryOrchestrator
adaptIcspTryNum: 5); adaptIcspTryNum: 5);
} }
/// <summary>
/// 按规划全局速度倍率生成规划专用机器人约束。
/// </summary>
/// <param name="robot">原始机器人约束。</param>
/// <param name="planningSpeedScale">规划阶段的全局速度倍率1.0 表示不额外缩放。</param>
/// <returns>已按速度倍率缩放后的规划机器人约束。</returns>
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);
}
/// <summary> /// <summary>
/// 把兼容层输入数组转换成领域层 FlyshotProgram。 /// 把兼容层输入数组转换成领域层 FlyshotProgram。
/// </summary> /// </summary>

View File

@@ -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;
/// <summary>
/// 负责把 saveTrajectory 生成的规划结果点位写入运行目录 Config/Data。
/// </summary>
public sealed class FlyshotTrajectoryArtifactWriter
{
/// <summary>
/// 旧 Data 明细点位文件使用的默认采样周期,单位为秒。
/// </summary>
private const double LegacyDetailSamplePeriodSeconds = 0.016;
private readonly ControllerClientCompatOptions _options;
private readonly RobotModelLoader _robotModelLoader;
private readonly ILogger<FlyshotTrajectoryArtifactWriter>? _logger;
/// <summary>
/// 初始化规划结果点位导出器。
/// </summary>
/// <param name="options">兼容层基础配置,用于定位运行配置根目录。</param>
/// <param name="robotModelLoader">机器人模型加载器,用于生成笛卡尔点位。</param>
/// <param name="logger">日志记录器;允许 null。</param>
public FlyshotTrajectoryArtifactWriter(
ControllerClientCompatOptions options,
RobotModelLoader robotModelLoader,
ILogger<FlyshotTrajectoryArtifactWriter>? logger = null)
{
_options = options ?? throw new ArgumentNullException(nameof(options));
_robotModelLoader = robotModelLoader ?? throw new ArgumentNullException(nameof(robotModelLoader));
_logger = logger;
}
/// <summary>
/// 将飞拍规划结果导出到 Config/Data/name。
/// </summary>
/// <param name="trajectoryName">飞拍轨迹名称。</param>
/// <param name="robot">当前机器人配置。</param>
/// <param name="bundle">规划结果包。</param>
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);
}
/// <summary>
/// 构造 JointTraj.txt 行数据,格式为 time + 关节弧度。
/// </summary>
private static IReadOnlyList<IReadOnlyList<double>> BuildJointRows(PlannedTrajectory trajectory)
{
var rows = new List<IReadOnlyList<double>>(trajectory.PlannedWaypoints.Count);
for (var index = 0; index < trajectory.PlannedWaypoints.Count; index++)
{
var row = new List<double>(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;
}
/// <summary>
/// 构造 CartTraj.txt 行数据,格式为 time + x/y/z/qx/qy/qz/qw。
/// </summary>
private static IReadOnlyList<IReadOnlyList<double>> BuildCartesianRows(
PlannedTrajectory trajectory,
RobotKinematicsModel kinematicsModel)
{
var rows = new List<IReadOnlyList<double>>(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<double>(pose.Length + 1)
{
Math.Round(trajectory.WaypointTimes[index], 6)
};
row.AddRange(pose.Select(static value => Math.Round(value, 6)));
rows.Add(row);
}
return rows;
}
/// <summary>
/// 将轨迹名转换为可用目录名,避免 HTTP 输入中的路径字符污染输出目录。
/// </summary>
private static string SanitizeDirectoryName(string name)
{
var invalidChars = Path.GetInvalidFileNameChars();
var chars = name.Select(ch => invalidChars.Contains(ch) ? '_' : ch).ToArray();
return new string(chars);
}
}

View File

@@ -7,38 +7,9 @@ using Microsoft.Extensions.Logging;
namespace Flyshot.ControllerClientCompat; namespace Flyshot.ControllerClientCompat;
/// <summary> /// <summary>
/// 定义已上传飞拍轨迹的持久化存储契约 /// 使用运行目录 Config/RobotConfig.json 持久化单机器人飞拍轨迹和机器人配置
/// </summary> /// </summary>
public interface IFlyshotTrajectoryStore public sealed class JsonFlyshotTrajectoryStore
{
/// <summary>
/// 将单条轨迹持久化到本地 JSON同时更新所属机器人配置段。
/// </summary>
/// <param name="robotName">当前已初始化的机器人名称。</param>
/// <param name="settings">当前机器人级兼容配置。</param>
/// <param name="trajectory">要保存的已上传轨迹。</param>
void Save(string robotName, CompatibilityRobotSettings settings, ControllerClientCompatUploadedTrajectory trajectory);
/// <summary>
/// 从本地 JSON 删除指定名称的轨迹。
/// </summary>
/// <param name="robotName">当前已初始化的机器人名称。</param>
/// <param name="trajectoryName">要删除的轨迹名称。</param>
void Delete(string robotName, string trajectoryName);
/// <summary>
/// 加载指定机器人名下所有已持久化的轨迹,并回传保存时的机器人配置。
/// </summary>
/// <param name="robotName">当前已初始化的机器人名称。</param>
/// <param name="settings">输出保存时的机器人配置;若文件不存在或解析失败则为 null。</param>
/// <returns>按轨迹名称索引的已上传轨迹集合。</returns>
IReadOnlyDictionary<string, ControllerClientCompatUploadedTrajectory> LoadAll(string robotName, out CompatibilityRobotSettings? settings);
}
/// <summary>
/// 使用与旧版 RobotConfig.json 一致的 JSON 格式在运行目录 Config 中持久化飞拍轨迹和机器人配置。
/// </summary>
public sealed class JsonFlyshotTrajectoryStore : IFlyshotTrajectoryStore
{ {
private readonly ControllerClientCompatOptions _options; private readonly ControllerClientCompatOptions _options;
private readonly RobotConfigLoader _configLoader; private readonly RobotConfigLoader _configLoader;
@@ -57,19 +28,24 @@ public sealed class JsonFlyshotTrajectoryStore : IFlyshotTrajectoryStore
_logger = logger; _logger = logger;
} }
/// <inheritdoc /> /// <summary>
/// 将单条轨迹持久化到统一 RobotConfig.json同时更新机器人配置段。
/// </summary>
/// <param name="robotName">当前已初始化的机器人名称,仅用于日志诊断。</param>
/// <param name="settings">当前机器人级兼容配置。</param>
/// <param name="trajectory">要保存的已上传轨迹。</param>
public void Save(string robotName, CompatibilityRobotSettings settings, ControllerClientCompatUploadedTrajectory trajectory) public void Save(string robotName, CompatibilityRobotSettings settings, ControllerClientCompatUploadedTrajectory trajectory)
{ {
ArgumentNullException.ThrowIfNull(settings); ArgumentNullException.ThrowIfNull(settings);
ArgumentNullException.ThrowIfNull(trajectory); ArgumentNullException.ThrowIfNull(trajectory);
_logger?.LogInformation( _logger?.LogInformation(
"TrajectoryStore 保存轨迹: robot={RobotName}, name={TrajectoryName}, waypoints={WaypointCount}", "RobotConfig 保存轨迹: robot={RobotName}, name={TrajectoryName}, waypoints={WaypointCount}",
robotName, robotName,
trajectory.Name, trajectory.Name,
trajectory.Waypoints.Count); trajectory.Waypoints.Count);
var path = ResolveStorePath(robotName); var path = ResolveStorePath();
var directory = Path.GetDirectoryName(path)!; var directory = Path.GetDirectoryName(path)!;
Directory.CreateDirectory(directory); Directory.CreateDirectory(directory);
@@ -103,10 +79,14 @@ public sealed class JsonFlyshotTrajectoryStore : IFlyshotTrajectoryStore
}; };
File.WriteAllText(path, root.ToJsonString(writeOptions)); File.WriteAllText(path, root.ToJsonString(writeOptions));
_logger?.LogInformation("TrajectoryStore 轨迹已保存到 {Path}", path); _logger?.LogInformation("RobotConfig 轨迹已保存到 {Path}", path);
} }
/// <inheritdoc /> /// <summary>
/// 从统一 RobotConfig.json 删除指定名称的轨迹。
/// </summary>
/// <param name="robotName">当前已初始化的机器人名称,仅用于日志诊断。</param>
/// <param name="trajectoryName">要删除的轨迹名称。</param>
public void Delete(string robotName, string trajectoryName) public void Delete(string robotName, string trajectoryName)
{ {
if (string.IsNullOrWhiteSpace(trajectoryName)) if (string.IsNullOrWhiteSpace(trajectoryName))
@@ -114,12 +94,12 @@ public sealed class JsonFlyshotTrajectoryStore : IFlyshotTrajectoryStore
throw new ArgumentException("轨迹名称不能为空。", nameof(trajectoryName)); 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)) if (!File.Exists(path))
{ {
_logger?.LogWarning("TrajectoryStore 删除失败: 文件不存在 {Path}", path); _logger?.LogWarning("RobotConfig 删除失败: 文件不存在 {Path}", path);
return; return;
} }
@@ -127,7 +107,7 @@ public sealed class JsonFlyshotTrajectoryStore : IFlyshotTrajectoryStore
var root = JsonNode.Parse(existingJson)?.AsObject(); var root = JsonNode.Parse(existingJson)?.AsObject();
if (root is null) if (root is null)
{ {
_logger?.LogWarning("TrajectoryStore 删除失败: 无法解析 JSON {Path}", path); _logger?.LogWarning("RobotConfig 删除失败: 无法解析 JSON {Path}", path);
return; return;
} }
@@ -142,29 +122,34 @@ public sealed class JsonFlyshotTrajectoryStore : IFlyshotTrajectoryStore
PropertyNamingPolicy = JsonNamingPolicy.SnakeCaseLower PropertyNamingPolicy = JsonNamingPolicy.SnakeCaseLower
}; };
File.WriteAllText(path, root.ToJsonString(writeOptions)); File.WriteAllText(path, root.ToJsonString(writeOptions));
_logger?.LogInformation("TrajectoryStore 轨迹已删除: {TrajectoryName}", trajectoryName); _logger?.LogInformation("RobotConfig 轨迹已删除: {TrajectoryName}", trajectoryName);
} }
else else
{ {
_logger?.LogWarning("TrajectoryStore 删除失败: 轨迹不存在 {TrajectoryName}", trajectoryName); _logger?.LogWarning("RobotConfig 删除失败: 轨迹不存在 {TrajectoryName}", trajectoryName);
} }
} }
} }
/// <inheritdoc /> /// <summary>
/// 加载统一 RobotConfig.json 中的所有轨迹,并回传机器人配置。
/// </summary>
/// <param name="robotName">当前已初始化的机器人名称,仅用于日志诊断。</param>
/// <param name="settings">输出 RobotConfig.json 中的机器人配置;若文件不存在或解析失败则为 null。</param>
/// <returns>按轨迹名称索引的已上传轨迹集合。</returns>
public IReadOnlyDictionary<string, ControllerClientCompatUploadedTrajectory> LoadAll(string robotName, out CompatibilityRobotSettings? settings) public IReadOnlyDictionary<string, ControllerClientCompatUploadedTrajectory> LoadAll(string robotName, out CompatibilityRobotSettings? settings)
{ {
var path = ResolveStorePath(robotName); var path = ResolveStorePath();
if (!File.Exists(path)) if (!File.Exists(path))
{ {
_logger?.LogInformation("TrajectoryStore 无持久化数据: {Path}", path); _logger?.LogInformation("RobotConfig 无持久化数据: {Path}", path);
settings = null; settings = null;
return new Dictionary<string, ControllerClientCompatUploadedTrajectory>(StringComparer.Ordinal); return new Dictionary<string, ControllerClientCompatUploadedTrajectory>(StringComparer.Ordinal);
} }
try try
{ {
_logger?.LogInformation("TrajectoryStore 正在加载: {Path}", path); _logger?.LogInformation("RobotConfig 正在加载: {Path}", path);
var loaded = _configLoader.Load(path, _options.ResolveConfigRoot()); var loaded = _configLoader.Load(path, _options.ResolveConfigRoot());
settings = loaded.Robot; settings = loaded.Robot;
@@ -182,7 +167,7 @@ public sealed class JsonFlyshotTrajectoryStore : IFlyshotTrajectoryStore
} }
_logger?.LogInformation( _logger?.LogInformation(
"TrajectoryStore 加载完成: robot={RobotName}, 轨迹数={Count}, useDo={UseDo}, ioKeepCycles={IoKeepCycles}", "RobotConfig 加载完成: robot={RobotName}, 轨迹数={Count}, useDo={UseDo}, ioKeepCycles={IoKeepCycles}",
robotName, robotName,
dict.Count, dict.Count,
settings?.UseDo, settings?.UseDo,
@@ -192,7 +177,7 @@ public sealed class JsonFlyshotTrajectoryStore : IFlyshotTrajectoryStore
} }
catch (Exception ex) catch (Exception ex)
{ {
_logger?.LogError(ex, "TrajectoryStore 加载失败: {Path}", path); _logger?.LogError(ex, "RobotConfig 加载失败: {Path}", path);
settings = null; settings = null;
return new Dictionary<string, ControllerClientCompatUploadedTrajectory>(StringComparer.Ordinal); return new Dictionary<string, ControllerClientCompatUploadedTrajectory>(StringComparer.Ordinal);
} }
@@ -229,11 +214,10 @@ public sealed class JsonFlyshotTrajectoryStore : IFlyshotTrajectoryStore
} }
/// <summary> /// <summary>
/// 解析当前机器人对应的持久化文件路径。 /// 解析单程序单机器人的统一配置文件路径。
/// </summary> /// </summary>
private string ResolveStorePath(string robotName) private string ResolveStorePath()
{ {
var storeDir = Path.Combine(_options.ResolveConfigRoot(), "TrajectoryStore"); return Path.Combine(_options.ResolveConfigRoot(), "RobotConfig.json");
return Path.Combine(storeDir, $"{robotName}_trajectories.json");
} }
} }

View File

@@ -1,4 +1,5 @@
using Flyshot.Core.Domain; using Flyshot.Core.Domain;
using Microsoft.Extensions.Logging;
namespace Flyshot.ControllerClientCompat; namespace Flyshot.ControllerClientCompat;
@@ -59,7 +60,8 @@ internal static class MoveJointTrajectoryGenerator
RobotProfile robot, RobotProfile robot,
IReadOnlyList<double> startJoints, IReadOnlyList<double> startJoints,
IReadOnlyList<double> targetJoints, IReadOnlyList<double> targetJoints,
double speedRatio) double speedRatio,
ILogger? logger = null)
{ {
ArgumentNullException.ThrowIfNull(robot); ArgumentNullException.ThrowIfNull(robot);
ArgumentNullException.ThrowIfNull(startJoints); ArgumentNullException.ThrowIfNull(startJoints);
@@ -80,6 +82,10 @@ internal static class MoveJointTrajectoryGenerator
var durationSeconds = AlignDurationToServoStep(requestedDurationSeconds, samplePeriodSeconds); var durationSeconds = AlignDurationToServoStep(requestedDurationSeconds, samplePeriodSeconds);
var denseJointTrajectory = GenerateDenseTrajectory(startJoints, targetJoints, durationSeconds, 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( return new TrajectoryResult(
programName: "move-joint", programName: "move-joint",
method: PlanningMethod.Doubles, method: PlanningMethod.Doubles,

View File

@@ -8,6 +8,7 @@
<ItemGroup> <ItemGroup>
<ProjectReference Include="..\Flyshot.Core.Domain\Flyshot.Core.Domain.csproj" /> <ProjectReference Include="..\Flyshot.Core.Domain\Flyshot.Core.Domain.csproj" />
<PackageReference Include="Microsoft.Extensions.Logging.Abstractions" Version="8.0.1" />
</ItemGroup> </ItemGroup>
</Project> </Project>

View File

@@ -1,5 +1,6 @@
using System.Text.Json; using System.Text.Json;
using Flyshot.Core.Domain; using Flyshot.Core.Domain;
using Microsoft.Extensions.Logging;
namespace Flyshot.Core.Config; namespace Flyshot.Core.Config;
@@ -17,7 +18,8 @@ public sealed class CompatibilityRobotSettings
int ioKeepCycles, int ioKeepCycles,
double accLimitScale, double accLimitScale,
double jerkLimitScale, double jerkLimitScale,
int adaptIcspTryNum) int adaptIcspTryNum,
double planningSpeedScale = 1.0)
{ {
ArgumentNullException.ThrowIfNull(ioAddresses); ArgumentNullException.ThrowIfNull(ioAddresses);
@@ -36,6 +38,11 @@ public sealed class CompatibilityRobotSettings
throw new ArgumentOutOfRangeException(nameof(jerkLimitScale), "Jerk 倍率必须大于 0。"); 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) if (adaptIcspTryNum < 0)
{ {
throw new ArgumentOutOfRangeException(nameof(adaptIcspTryNum), "补点尝试次数不能为负数。"); throw new ArgumentOutOfRangeException(nameof(adaptIcspTryNum), "补点尝试次数不能为负数。");
@@ -54,6 +61,7 @@ public sealed class CompatibilityRobotSettings
AccLimitScale = accLimitScale; AccLimitScale = accLimitScale;
JerkLimitScale = jerkLimitScale; JerkLimitScale = jerkLimitScale;
AdaptIcspTryNum = adaptIcspTryNum; AdaptIcspTryNum = adaptIcspTryNum;
PlanningSpeedScale = planningSpeedScale;
} }
/// <summary> /// <summary>
@@ -81,6 +89,11 @@ public sealed class CompatibilityRobotSettings
/// </summary> /// </summary>
public double JerkLimitScale { get; } public double JerkLimitScale { get; }
/// <summary>
/// 获取规划阶段的全局速度倍率,只影响 JointTraj 基准时间,不等同于运行时 J519 下发速度倍率。
/// </summary>
public double PlanningSpeedScale { get; }
/// <summary> /// <summary>
/// 获取自适应补点最大尝试次数。 /// 获取自适应补点最大尝试次数。
/// </summary> /// </summary>
@@ -131,6 +144,17 @@ public sealed class LoadedRobotConfig
/// </summary> /// </summary>
public sealed class RobotConfigLoader public sealed class RobotConfigLoader
{ {
private readonly ILogger<RobotConfigLoader>? _logger;
/// <summary>
/// 初始化 RobotConfigLoader。
/// </summary>
/// <param name="logger">日志记录器;允许 null。</param>
public RobotConfigLoader(ILogger<RobotConfigLoader>? logger = null)
{
_logger = logger;
}
/// <summary> /// <summary>
/// 加载一份旧版 RobotConfig.json。 /// 加载一份旧版 RobotConfig.json。
/// </summary> /// </summary>
@@ -139,6 +163,8 @@ public sealed class RobotConfigLoader
/// <returns>规范化后的配置文档。</returns> /// <returns>规范化后的配置文档。</returns>
public LoadedRobotConfig Load(string configPath, string? repoRoot = null) public LoadedRobotConfig Load(string configPath, string? repoRoot = null)
{ {
_logger?.LogInformation("RobotConfig 开始加载: configPath={ConfigPath}, repoRoot={RepoRoot}", configPath, repoRoot);
var resolvedRepoRoot = ResolveRepoRoot(repoRoot); var resolvedRepoRoot = ResolveRepoRoot(repoRoot);
var resolvedConfigPath = PathCompatibility.ResolveConfigPath(configPath, resolvedRepoRoot); var resolvedConfigPath = PathCompatibility.ResolveConfigPath(configPath, resolvedRepoRoot);
@@ -153,7 +179,8 @@ public sealed class RobotConfigLoader
ioKeepCycles: ReadInt(robotElement, "io_keep_cycles", defaultValue: 0), ioKeepCycles: ReadInt(robotElement, "io_keep_cycles", defaultValue: 0),
accLimitScale: ReadDouble(robotElement, "acc_limit", defaultValue: 1.0), accLimitScale: ReadDouble(robotElement, "acc_limit", defaultValue: 1.0),
jerkLimitScale: ReadDouble(robotElement, "jerk_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<string, FlyshotProgram>(StringComparer.Ordinal); var programs = new Dictionary<string, FlyshotProgram>(StringComparer.Ordinal);
foreach (var programElement in flyingShotsElement.EnumerateObject()) foreach (var programElement in flyingShotsElement.EnumerateObject())
@@ -163,6 +190,10 @@ public sealed class RobotConfigLoader
programs.Add(programName, program); 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( return new LoadedRobotConfig(
sourcePath: resolvedConfigPath, sourcePath: resolvedConfigPath,
robot: robot, robot: robot,

View File

@@ -1,6 +1,7 @@
using System.Text; using System.Text;
using System.Text.Json; using System.Text.Json;
using Flyshot.Core.Domain; using Flyshot.Core.Domain;
using Microsoft.Extensions.Logging;
namespace Flyshot.Core.Config; namespace Flyshot.Core.Config;
@@ -10,6 +11,16 @@ namespace Flyshot.Core.Config;
public sealed class RobotModelLoader public sealed class RobotModelLoader
{ {
private const uint JsonChunkType = 0x4E4F534A; private const uint JsonChunkType = 0x4E4F534A;
private readonly ILogger<RobotModelLoader>? _logger;
/// <summary>
/// 初始化 RobotModelLoader。
/// </summary>
/// <param name="logger">日志记录器;允许 null。</param>
public RobotModelLoader(ILogger<RobotModelLoader>? logger = null)
{
_logger = logger;
}
/// <summary> /// <summary>
/// 加载 .robot 文件并生成规划侧可直接消费的 RobotProfile。 /// 加载 .robot 文件并生成规划侧可直接消费的 RobotProfile。
@@ -35,6 +46,8 @@ public sealed class RobotModelLoader
throw new ArgumentOutOfRangeException(nameof(jerkLimitScale), "Jerk 倍率必须大于 0。"); 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 resolvedModelPath = Path.GetFullPath(modelPath);
var jsonText = ReadJsonChunk(resolvedModelPath); var jsonText = ReadJsonChunk(resolvedModelPath);
using var document = JsonDocument.Parse(jsonText); 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( return new RobotProfile(
name: profileName, name: profileName,
modelPath: resolvedModelPath, modelPath: resolvedModelPath,
@@ -156,6 +173,8 @@ public sealed class RobotModelLoader
throw new ArgumentException(".robot 路径不能为空。", nameof(modelPath)); throw new ArgumentException(".robot 路径不能为空。", nameof(modelPath));
} }
_logger?.LogInformation("RobotKinematicsModel 开始加载: modelPath={ModelPath}", modelPath);
var resolvedModelPath = Path.GetFullPath(modelPath); var resolvedModelPath = Path.GetFullPath(modelPath);
var jsonText = ReadJsonChunk(resolvedModelPath); var jsonText = ReadJsonChunk(resolvedModelPath);
using var document = JsonDocument.Parse(jsonText); using var document = JsonDocument.Parse(jsonText);
@@ -203,6 +222,8 @@ public sealed class RobotModelLoader
coupleOffset: coupleOffset)); coupleOffset: coupleOffset));
} }
_logger?.LogInformation("RobotKinematicsModel 加载完成: profileName={ProfileName}, 关节数={JointCount}", profileName, joints.Count);
return new RobotKinematicsModel(name: profileName, joints: joints); return new RobotKinematicsModel(name: profileName, joints: joints);
} }

View File

@@ -15,12 +15,28 @@ namespace Flyshot.Core.Planning.Export;
/// </summary> /// </summary>
public static class TrajectoryExporter public static class TrajectoryExporter
{ {
/// <summary>
/// 导出规划关节轨迹关键点到文本文件。
/// </summary>
public static void WriteJointTrajectory(string path, IReadOnlyList<IReadOnlyList<double>> rows)
{
WriteRows(path, rows);
}
/// <summary> /// <summary>
/// 导出稠密关节轨迹到文本文件。 /// 导出稠密关节轨迹到文本文件。
/// </summary> /// </summary>
public static void WriteJointDenseTrajectory(string path, IReadOnlyList<IReadOnlyList<double>> rows) public static void WriteJointDenseTrajectory(string path, IReadOnlyList<IReadOnlyList<double>> rows)
{ {
WriteDenseRows(path, rows); WriteRows(path, rows);
}
/// <summary>
/// 导出规划笛卡尔轨迹关键点到文本文件。
/// </summary>
public static void WriteCartesianTrajectory(string path, IReadOnlyList<IReadOnlyList<double>> rows)
{
WriteRows(path, rows);
} }
/// <summary> /// <summary>
@@ -28,7 +44,7 @@ public static class TrajectoryExporter
/// </summary> /// </summary>
public static void WriteCartesianDenseTrajectory(string path, IReadOnlyList<IReadOnlyList<double>> rows) public static void WriteCartesianDenseTrajectory(string path, IReadOnlyList<IReadOnlyList<double>> rows)
{ {
WriteDenseRows(path, rows); WriteRows(path, rows);
} }
/// <summary> /// <summary>
@@ -53,7 +69,7 @@ public static class TrajectoryExporter
File.WriteAllText(path, json, new UTF8Encoding(false)); File.WriteAllText(path, json, new UTF8Encoding(false));
} }
private static void WriteDenseRows(string path, IReadOnlyList<IReadOnlyList<double>> rows) private static void WriteRows(string path, IReadOnlyList<IReadOnlyList<double>> rows)
{ {
var sb = new StringBuilder(); var sb = new StringBuilder();
foreach (var row in rows) foreach (var row in rows)

View File

@@ -9,6 +9,7 @@
<ItemGroup> <ItemGroup>
<ProjectReference Include="..\Flyshot.Core.Domain\Flyshot.Core.Domain.csproj" /> <ProjectReference Include="..\Flyshot.Core.Domain\Flyshot.Core.Domain.csproj" />
<ProjectReference Include="..\Flyshot.Core.Planning\Flyshot.Core.Planning.csproj" /> <ProjectReference Include="..\Flyshot.Core.Planning\Flyshot.Core.Planning.csproj" />
<PackageReference Include="Microsoft.Extensions.Logging.Abstractions" Version="8.0.1" />
</ItemGroup> </ItemGroup>
</Project> </Project>

View File

@@ -1,5 +1,6 @@
using Flyshot.Core.Domain; using Flyshot.Core.Domain;
using Flyshot.Core.Planning; using Flyshot.Core.Planning;
using Microsoft.Extensions.Logging;
namespace Flyshot.Core.Triggering; namespace Flyshot.Core.Triggering;
@@ -10,13 +11,17 @@ namespace Flyshot.Core.Triggering;
public sealed class ShotTimelineBuilder public sealed class ShotTimelineBuilder
{ {
private readonly WaypointTimestampResolver _resolver; private readonly WaypointTimestampResolver _resolver;
private readonly ILogger<ShotTimelineBuilder>? _logger;
/// <summary> /// <summary>
/// 初始化 ShotTimelineBuilder依赖一个时间戳解析器来对齐补中点后的轨迹与原始示教点。 /// 初始化 ShotTimelineBuilder依赖一个时间戳解析器来对齐补中点后的轨迹与原始示教点。
/// </summary> /// </summary>
public ShotTimelineBuilder(WaypointTimestampResolver resolver) /// <param name="resolver">时间戳解析器。</param>
/// <param name="logger">日志记录器;允许 null。</param>
public ShotTimelineBuilder(WaypointTimestampResolver resolver, ILogger<ShotTimelineBuilder>? logger = null)
{ {
_resolver = resolver ?? throw new ArgumentNullException(nameof(resolver)); _resolver = resolver ?? throw new ArgumentNullException(nameof(resolver));
_logger = logger;
} }
/// <summary> /// <summary>
@@ -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); return new ShotTimeline(shotEvents, triggerTimeline);
} }
} }

View File

@@ -122,9 +122,12 @@ public sealed class FanucCommandClient : IDisposable
/// <param name="programName">程序名。</param> /// <param name="programName">程序名。</param>
/// <param name="cancellationToken">取消令牌。</param> /// <param name="cancellationToken">取消令牌。</param>
/// <returns>结果响应。</returns> /// <returns>结果响应。</returns>
public Task<FanucCommandResultResponse> StopProgramAsync(string programName, CancellationToken cancellationToken = default) public async Task<FanucCommandResultResponse> 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;
} }
/// <summary> /// <summary>
@@ -134,8 +137,10 @@ public sealed class FanucCommandClient : IDisposable
/// <returns>结果响应。</returns> /// <returns>结果响应。</returns>
public async Task<FanucCommandResultResponse> ResetRobotAsync(CancellationToken cancellationToken = default) public async Task<FanucCommandResultResponse> ResetRobotAsync(CancellationToken cancellationToken = default)
{ {
_logger?.LogInformation("CommandClient ResetRobot");
var frame = FanucCommandProtocol.PackEmptyCommand(FanucCommandMessageIds.ResetRobot); var frame = FanucCommandProtocol.PackEmptyCommand(FanucCommandMessageIds.ResetRobot);
var response = await SendRawFrameAsync(frame, cancellationToken).ConfigureAwait(false); var response = await SendRawFrameAsync(frame, cancellationToken).ConfigureAwait(false);
_logger?.LogDebug("CommandClient ResetRobot 成功");
return EnsureSuccess(FanucCommandProtocol.ParseResultResponse(response)); return EnsureSuccess(FanucCommandProtocol.ParseResultResponse(response));
} }
@@ -147,8 +152,10 @@ public sealed class FanucCommandClient : IDisposable
/// <returns>程序状态响应。</returns> /// <returns>程序状态响应。</returns>
public async Task<FanucProgramStatusResponse> GetProgramStatusAsync(string programName, CancellationToken cancellationToken = default) public async Task<FanucProgramStatusResponse> GetProgramStatusAsync(string programName, CancellationToken cancellationToken = default)
{ {
_logger?.LogInformation("CommandClient GetProgramStatus: {ProgramName}", programName);
var frame = FanucCommandProtocol.PackProgramCommand(FanucCommandMessageIds.GetProgramStatus, programName); var frame = FanucCommandProtocol.PackProgramCommand(FanucCommandMessageIds.GetProgramStatus, programName);
var response = await SendRawFrameAsync(frame, cancellationToken).ConfigureAwait(false); var response = await SendRawFrameAsync(frame, cancellationToken).ConfigureAwait(false);
_logger?.LogDebug("CommandClient GetProgramStatus 成功: {ProgramName}", programName);
return EnsureSuccess(FanucCommandProtocol.ParseProgramStatusResponse(response)); return EnsureSuccess(FanucCommandProtocol.ParseProgramStatusResponse(response));
} }
@@ -158,9 +165,12 @@ public sealed class FanucCommandClient : IDisposable
/// <param name="programName">程序名。</param> /// <param name="programName">程序名。</param>
/// <param name="cancellationToken">取消令牌。</param> /// <param name="cancellationToken">取消令牌。</param>
/// <returns>结果响应。</returns> /// <returns>结果响应。</returns>
public Task<FanucCommandResultResponse> StartProgramAsync(string programName, CancellationToken cancellationToken = default) public async Task<FanucCommandResultResponse> 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;
} }
/// <summary> /// <summary>
@@ -170,9 +180,12 @@ public sealed class FanucCommandClient : IDisposable
/// <returns>速度倍率响应。</returns> /// <returns>速度倍率响应。</returns>
public async Task<FanucSpeedRatioResponse> GetSpeedRatioAsync(CancellationToken cancellationToken = default) public async Task<FanucSpeedRatioResponse> GetSpeedRatioAsync(CancellationToken cancellationToken = default)
{ {
_logger?.LogDebug("CommandClient GetSpeedRatio");
var frame = FanucCommandProtocol.PackGetSpeedRatioCommand(); var frame = FanucCommandProtocol.PackGetSpeedRatioCommand();
var response = await SendRawFrameAsync(frame, cancellationToken).ConfigureAwait(false); 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;
} }
/// <summary> /// <summary>
@@ -183,8 +196,10 @@ public sealed class FanucCommandClient : IDisposable
/// <returns>结果响应。</returns> /// <returns>结果响应。</returns>
public async Task<FanucCommandResultResponse> SetSpeedRatioAsync(double ratio, CancellationToken cancellationToken = default) public async Task<FanucCommandResultResponse> SetSpeedRatioAsync(double ratio, CancellationToken cancellationToken = default)
{ {
_logger?.LogInformation("CommandClient SetSpeedRatio: ratio={Ratio}", ratio);
var frame = FanucCommandProtocol.PackSetSpeedRatioCommand(ratio); var frame = FanucCommandProtocol.PackSetSpeedRatioCommand(ratio);
var response = await SendRawFrameAsync(frame, cancellationToken).ConfigureAwait(false); var response = await SendRawFrameAsync(frame, cancellationToken).ConfigureAwait(false);
_logger?.LogDebug("CommandClient SetSpeedRatio 成功: ratio={Ratio}", ratio);
return EnsureSuccess(FanucCommandProtocol.ParseResultResponse(response)); return EnsureSuccess(FanucCommandProtocol.ParseResultResponse(response));
} }
@@ -196,9 +211,12 @@ public sealed class FanucCommandClient : IDisposable
/// <returns>TCP 位姿响应。</returns> /// <returns>TCP 位姿响应。</returns>
public async Task<FanucTcpResponse> GetTcpAsync(uint tcpId = 1, CancellationToken cancellationToken = default) public async Task<FanucTcpResponse> GetTcpAsync(uint tcpId = 1, CancellationToken cancellationToken = default)
{ {
_logger?.LogDebug("CommandClient GetTcp: tcpId={TcpId}", tcpId);
var frame = FanucCommandProtocol.PackGetTcpCommand(tcpId); var frame = FanucCommandProtocol.PackGetTcpCommand(tcpId);
var response = await SendRawFrameAsync(frame, cancellationToken).ConfigureAwait(false); 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;
} }
/// <summary> /// <summary>
@@ -210,8 +228,10 @@ public sealed class FanucCommandClient : IDisposable
/// <returns>结果响应。</returns> /// <returns>结果响应。</returns>
public async Task<FanucCommandResultResponse> SetTcpAsync(uint tcpId, IReadOnlyList<double> pose, CancellationToken cancellationToken = default) public async Task<FanucCommandResultResponse> SetTcpAsync(uint tcpId, IReadOnlyList<double> 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 frame = FanucCommandProtocol.PackSetTcpCommand(tcpId, pose);
var response = await SendRawFrameAsync(frame, cancellationToken).ConfigureAwait(false); var response = await SendRawFrameAsync(frame, cancellationToken).ConfigureAwait(false);
_logger?.LogDebug("CommandClient SetTcp 成功: tcpId={TcpId}", tcpId);
return EnsureSuccess(FanucCommandProtocol.ParseResultResponse(response)); return EnsureSuccess(FanucCommandProtocol.ParseResultResponse(response));
} }
@@ -224,9 +244,12 @@ public sealed class FanucCommandClient : IDisposable
/// <returns>IO 读取响应。</returns> /// <returns>IO 读取响应。</returns>
public async Task<FanucIoResponse> GetIoAsync(int port, string ioType, CancellationToken cancellationToken = default) public async Task<FanucIoResponse> 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 frame = FanucCommandProtocol.PackGetIoCommand(FanucIoTypes.FromName(ioType), port);
var response = await SendRawFrameAsync(frame, cancellationToken).ConfigureAwait(false); 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;
} }
/// <summary> /// <summary>
@@ -239,8 +262,10 @@ public sealed class FanucCommandClient : IDisposable
/// <returns>结果响应。</returns> /// <returns>结果响应。</returns>
public async Task<FanucCommandResultResponse> SetIoAsync(int port, bool value, string ioType, CancellationToken cancellationToken = default) public async Task<FanucCommandResultResponse> 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 frame = FanucCommandProtocol.PackSetIoCommand(FanucIoTypes.FromName(ioType), port, value);
var response = await SendRawFrameAsync(frame, cancellationToken).ConfigureAwait(false); var response = await SendRawFrameAsync(frame, cancellationToken).ConfigureAwait(false);
_logger?.LogDebug("CommandClient SetIo 成功: port={Port}, value={Value}", port, value);
return EnsureSuccess(FanucCommandProtocol.ParseResultResponse(response)); return EnsureSuccess(FanucCommandProtocol.ParseResultResponse(response));
} }

View File

@@ -24,6 +24,7 @@ public sealed class ConfigCompatibilityTests
Assert.Equal(2, loaded.Robot.IoKeepCycles); Assert.Equal(2, loaded.Robot.IoKeepCycles);
Assert.Equal(1.0, loaded.Robot.AccLimitScale); Assert.Equal(1.0, loaded.Robot.AccLimitScale);
Assert.Equal(1.0, loaded.Robot.JerkLimitScale); Assert.Equal(1.0, loaded.Robot.JerkLimitScale);
Assert.Equal(1.0, loaded.Robot.PlanningSpeedScale);
Assert.Equal(5, loaded.Robot.AdaptIcspTryNum); Assert.Equal(5, loaded.Robot.AdaptIcspTryNum);
var program = Assert.Contains("EOL10_EAU_0", loaded.Programs); 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(3, loaded.Robot.IoKeepCycles);
Assert.Equal(0.5, loaded.Robot.AccLimitScale); Assert.Equal(0.5, loaded.Robot.AccLimitScale);
Assert.Equal(0.25, loaded.Robot.JerkLimitScale); Assert.Equal(0.25, loaded.Robot.JerkLimitScale);
Assert.Equal(1.0, loaded.Robot.PlanningSpeedScale);
Assert.Equal([0, 0, 0], program.OffsetValues); Assert.Equal([0, 0, 0], program.OffsetValues);
Assert.All(program.AddressGroups, group => Assert.Empty(group.Addresses)); Assert.All(program.AddressGroups, group => Assert.Empty(group.Addresses));
} }
@@ -80,6 +82,46 @@ public sealed class ConfigCompatibilityTests
} }
} }
/// <summary>
/// 验证 RobotConfig.json 可以显式配置规划限速倍率,且该倍率独立于运行时 J519 速度倍率。
/// </summary>
[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);
}
}
/// <summary> /// <summary>
/// 验证 .robot 解析会保留 Joint3 对 Joint2 的 couple 元数据,并构造规划侧可直接消费的 RobotProfile。 /// 验证 .robot 解析会保留 Joint3 对 Joint2 的 couple 元数据,并构造规划侧可直接消费的 RobotProfile。
/// </summary> /// </summary>

View File

@@ -57,10 +57,10 @@ public sealed class ControllerClientCompatConfigRootTests
} }
/// <summary> /// <summary>
/// 验证 JSON 轨迹存储保存、加载和删除都落在 ConfigRoot/TrajectoryStore 目录 /// 验证 JSON 轨迹存储保存、加载和删除都落在 ConfigRoot/RobotConfig.json
/// </summary> /// </summary>
[Fact] [Fact]
public void JsonFlyshotTrajectoryStore_PersistsTrajectoriesUnderConfigRootStore() public void JsonFlyshotTrajectoryStore_PersistsTrajectoriesInRobotConfigJson()
{ {
var configRoot = CreateTempConfigRoot(); var configRoot = CreateTempConfigRoot();
try try
@@ -77,9 +77,10 @@ public sealed class ControllerClientCompatConfigRootTests
var trajectory = TestRobotFactory.CreateUploadedTrajectoryWithSingleShot(); var trajectory = TestRobotFactory.CreateUploadedTrajectoryWithSingleShot();
store.Save("FANUC_LR_Mate_200iD", settings, trajectory); 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); var loaded = store.LoadAll("FANUC_LR_Mate_200iD", out var loadedSettings);
Assert.NotNull(loadedSettings); Assert.NotNull(loadedSettings);
Assert.Contains(trajectory.Name, loaded); Assert.Contains(trajectory.Name, loaded);

View File

@@ -555,8 +555,7 @@ public sealed class FanucControllerRuntimeDenseTests
new ControllerClientCompatRobotCatalog(options, new RobotModelLoader()), new ControllerClientCompatRobotCatalog(options, new RobotModelLoader()),
runtime, runtime,
new ControllerClientTrajectoryOrchestrator(), new ControllerClientTrajectoryOrchestrator(),
new RobotConfigLoader(), new RobotConfigLoader());
new InMemoryFlyshotTrajectoryStore());
} }
private static double ComputeLineAlpha( private static double ComputeLineAlpha(

View File

@@ -136,6 +136,42 @@ public sealed class RuntimeOrchestrationTests
Assert.Single(bundle.Result.TriggerTimeline); Assert.Single(bundle.Result.TriggerTimeline);
} }
/// <summary>
/// 验证飞拍规划会把规划限速倍率纳入速度/加速度/Jerk 限制,而不是复用运行时下发倍率。
/// </summary>
[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}");
}
/// <summary>
/// 验证飞拍缓存键包含规划限速倍率,避免降速验证时误用 100% 速度下的规划结果。
/// </summary>
[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);
}
/// <summary> /// <summary>
/// 验证飞拍编排会使用 RobotConfig.json 中的 IO 保持周期。 /// 验证飞拍编排会使用 RobotConfig.json 中的 IO 保持周期。
/// </summary> /// </summary>
@@ -289,8 +325,7 @@ public sealed class RuntimeOrchestrationTests
new ControllerClientCompatRobotCatalog(options, new RobotModelLoader()), new ControllerClientCompatRobotCatalog(options, new RobotModelLoader()),
runtime, runtime,
new ControllerClientTrajectoryOrchestrator(), new ControllerClientTrajectoryOrchestrator(),
new RobotConfigLoader(), new RobotConfigLoader());
new InMemoryFlyshotTrajectoryStore());
service.SetUpRobot("FANUC_LR_Mate_200iD"); service.SetUpRobot("FANUC_LR_Mate_200iD");
@@ -304,6 +339,49 @@ public sealed class RuntimeOrchestrationTests
} }
} }
/// <summary>
/// 验证 IsFlyshotTrajectoryValid(saveTrajectory=true) 会把规划后的结果点位导出到 Config/Data/name。
/// </summary>
[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);
}
}
/// <summary> /// <summary>
/// 创建只包含当前支持机器人模型和 RobotConfig.json 的临时运行配置根。 /// 创建只包含当前支持机器人模型和 RobotConfig.json 的临时运行配置根。
/// </summary> /// </summary>
@@ -322,6 +400,41 @@ public sealed class RuntimeOrchestrationTests
return configRoot; return configRoot;
} }
/// <summary>
/// 写入包含一条飞拍轨迹的最小 RobotConfig.json供兼容服务从统一配置恢复轨迹。
/// </summary>
/// <param name="configRoot">测试运行配置根。</param>
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], [], []]
}
}
}
""");
}
} }
/// <summary> /// <summary>
@@ -389,8 +502,7 @@ internal static class TestRobotFactory
new ControllerClientCompatRobotCatalog(options, new RobotModelLoader()), new ControllerClientCompatRobotCatalog(options, new RobotModelLoader()),
new FanucControllerRuntime(), new FanucControllerRuntime(),
new ControllerClientTrajectoryOrchestrator(), new ControllerClientTrajectoryOrchestrator(),
new RobotConfigLoader(), new RobotConfigLoader());
new InMemoryFlyshotTrajectoryStore());
} }
/// <summary> /// <summary>
@@ -443,33 +555,6 @@ internal static class TestRobotFactory
} }
} }
/// <summary>
/// 内存中的轨迹存储实现,用于避免单元测试污染真实文件系统。
/// </summary>
internal sealed class InMemoryFlyshotTrajectoryStore : IFlyshotTrajectoryStore
{
private readonly Dictionary<string, ControllerClientCompatUploadedTrajectory> _store = new(StringComparer.Ordinal);
/// <inheritdoc />
public void Save(string robotName, CompatibilityRobotSettings settings, ControllerClientCompatUploadedTrajectory trajectory)
{
_store[trajectory.Name] = trajectory;
}
/// <inheritdoc />
public void Delete(string robotName, string trajectoryName)
{
_store.Remove(trajectoryName);
}
/// <inheritdoc />
public IReadOnlyDictionary<string, ControllerClientCompatUploadedTrajectory> LoadAll(string robotName, out CompatibilityRobotSettings? settings)
{
settings = null;
return _store;
}
}
/// <summary> /// <summary>
/// 记录 ResetRobot 入参的测试运行时,用于验证兼容服务传递的机器人配置。 /// 记录 ResetRobot 入参的测试运行时,用于验证兼容服务传递的机器人配置。
/// </summary> /// </summary>

View File

@@ -61,7 +61,12 @@ public sealed class StatusEndpointTests(FlyshotServerFactory factory) : IClassFi
Assert.True(root.GetProperty("isSetup").GetBoolean()); Assert.True(root.GetProperty("isSetup").GetBoolean());
Assert.Equal("FANUC_LR_Mate_200iD", root.GetProperty("robotName").GetString()); Assert.Equal("FANUC_LR_Mate_200iD", root.GetProperty("robotName").GetString());
Assert.Equal(6, root.GetProperty("degreesOfFreedom").GetInt32()); 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.Equal("Connected", snapshot.GetProperty("connectionState").GetString());
Assert.True(snapshot.GetProperty("isEnabled").GetBoolean()); Assert.True(snapshot.GetProperty("isEnabled").GetBoolean());
Assert.False(snapshot.GetProperty("isInMotion").GetBoolean()); Assert.False(snapshot.GetProperty("isInMotion").GetBoolean());