✨ 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

@@ -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);
}
}