✨ feat(*): 添加轨迹产物导出与规划速度倍率隔离
* 新增 FlyshotTrajectoryArtifactWriter,支持 saveTrajectory 将规划结果导出到 Config/Data/name(JointTraj、CartTraj、 ShotEvents 等) * RobotConfig 新增 PlanningSpeedScale,区分规划阶段限速倍率 与运行时 J519 下发倍率 * 轨迹缓存键纳入 planningSpeedScale,避免降速规划误用缓存 * 完善 FanucCommandClient 命令参数日志与状态通道重连 * 补充 RuntimeOrchestrationTests 覆盖产物导出与倍率隔离 * 更新 README 进度文档
This commit is contained in:
@@ -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')"
|
||||||
]
|
]
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -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 执行接口增加运行互斥、执行中拒绝重复轨迹、取消和超时控制。
|
||||||
- [ ] 增加运行日志、协议摘要日志和状态快照导出,便于现场排查。
|
- [ ] 增加运行日志、协议摘要日志和状态快照导出,便于现场排查。
|
||||||
|
|||||||
@@ -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>
|
||||||
|
|||||||
@@ -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>();
|
||||||
|
|
||||||
|
|||||||
@@ -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>
|
||||||
|
|||||||
@@ -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);
|
||||||
|
}
|
||||||
|
}
|
||||||
@@ -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");
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -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,
|
||||||
|
|||||||
@@ -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>
|
||||||
|
|||||||
@@ -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,
|
||||||
|
|||||||
@@ -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);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|||||||
@@ -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)
|
||||||
|
|||||||
@@ -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>
|
||||||
|
|||||||
@@ -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);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -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));
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|||||||
@@ -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>
|
||||||
|
|||||||
@@ -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);
|
||||||
|
|||||||
@@ -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(
|
||||||
|
|||||||
@@ -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>
|
||||||
|
|||||||
@@ -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());
|
||||||
|
|||||||
Reference in New Issue
Block a user