✨ feat(*): 添加轨迹产物导出与规划速度倍率隔离
* 新增 FlyshotTrajectoryArtifactWriter,支持 saveTrajectory 将规划结果导出到 Config/Data/name(JointTraj、CartTraj、 ShotEvents 等) * RobotConfig 新增 PlanningSpeedScale,区分规划阶段限速倍率 与运行时 J519 下发倍率 * 轨迹缓存键纳入 planningSpeedScale,避免降速规划误用缓存 * 完善 FanucCommandClient 命令参数日志与状态通道重连 * 补充 RuntimeOrchestrationTests 覆盖产物导出与倍率隔离 * 更新 README 进度文档
This commit is contained in:
@@ -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>
|
||||
|
||||
Reference in New Issue
Block a user