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

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