✨ feat(flyshot): 引入飞拍执行侧最终发送队列构建与校验机制

* 新增 FlyshotExecutionSendSequenceBuilder,负责在运行时前构建最终 8ms 发送队列,并进行离散限幅校验。
* 引入 FlyshotPreparedExecution 类,封装最终发送结果及相关诊断信息。
* 调整 ControllerClientCompatService 和 FanucControllerRuntime,确保运行时直接使用预生成的发送队列,避免临场重采样。
* 更新 TrajectoryResult 和 PlannedExecutionBundle,支持准备好的执行队列。
* 增加单元测试,验证非 1 倍 speedRatio 下的执行行为与预生成队列的使用。
This commit is contained in:
2026-05-09 19:06:49 +08:00
parent f7e2bb0e7b
commit 74761bb5da
11 changed files with 1185 additions and 75 deletions

View File

@@ -101,41 +101,55 @@ public sealed class FlyshotTrajectoryArtifactWriter
{
ArgumentNullException.ThrowIfNull(robot);
if (result.DenseJointTrajectory is null)
var preparedExecution = result.PreparedFlyshotExecution;
if (result.DenseJointTrajectory is null && preparedExecution is null)
{
return;
}
if (speedRatio <= 0.0 || double.IsNaN(speedRatio) || double.IsInfinity(speedRatio))
if (preparedExecution is null && (speedRatio <= 0.0 || double.IsNaN(speedRatio) || double.IsInfinity(speedRatio)))
{
throw new ArgumentOutOfRangeException(nameof(speedRatio), "speed_ratio 必须是有限正数。");
}
var samples = J519SendTrajectorySampler.SampleDenseJointTrajectory(
result.DenseJointTrajectory,
result.Duration.TotalSeconds,
ActualSendServoPeriodSeconds,
speedRatio);
try
{
TrajectoryLimitValidator.ValidateJ519SendSamples(
robot,
var samples = preparedExecution is null
? J519SendTrajectorySampler.SampleDenseJointTrajectory(
result.DenseJointTrajectory!,
result.Duration.TotalSeconds,
ActualSendServoPeriodSeconds,
speedRatio)
: preparedExecution.Samples.Select(static sample => new J519SendSample(
sample.SampleIndex,
sample.SendTime,
sample.TrajectoryTime,
sample.SpeedRatio,
sample.JointsDegrees)).ToArray();
var triggerBindings = preparedExecution is null
? TriggerSampleBinder.Bind(
result.TriggerTimeline,
samples,
trajectoryName: result.ProgramName);
}
catch (Exception e)
{
_logger?.LogError(e, "ValidateJ519SendSamples 失败,program={ProgramName}", result.ProgramName);
}
var jointRows = new List<IReadOnlyList<double>>(samples.Count);
var timingRows = new List<IReadOnlyList<double>>(samples.Count);
var jerkRows = new List<IReadOnlyList<double>>();
var triggerBindings = TriggerSampleBinder.Bind(
result.TriggerTimeline,
result.TriggerSampleIndexOffsetCycles)
: preparedExecution.TriggerBindings.Select(static binding =>
new TriggerSampleBinding(
binding.Trigger,
new J519SendSample(
binding.Sample.SampleIndex,
binding.Sample.SendTime,
binding.Sample.TrajectoryTime,
binding.Sample.SpeedRatio,
binding.Sample.JointsDegrees),
binding.SampleIndex,
binding.FoundInWindow)).ToArray();
TrajectoryLimitValidator.ValidateJ519SendSamples(
robot,
samples,
result.TriggerSampleIndexOffsetCycles);
trajectoryName: result.ProgramName);
var jointRows = new List<IReadOnlyList<double>>(samples.Count);
List<IReadOnlyList<double>> timingRows = preparedExecution is null
? new List<IReadOnlyList<double>>(samples.Count)
: preparedExecution.TimingRows.Select(static row => (IReadOnlyList<double>)row.ToArray()).ToList();
List<IReadOnlyList<double>> jerkRows = preparedExecution is null
? []
: preparedExecution.JerkRows.Select(static row => (IReadOnlyList<double>)row.ToArray()).ToList();
double? previousSendTime = null;
double[]? previousJoints = null;
double[]? previousVelocity = null;
@@ -144,17 +158,20 @@ public sealed class FlyshotTrajectoryArtifactWriter
foreach (var sample in samples)
{
jointRows.Add(BuildActualSendJointRow(sample.SendTime, sample.JointsDegrees));
timingRows.Add(J519SendTrajectorySampler.BuildTimingRow(sample));
if (previousSendTime is not null && previousJoints is not null)
if (preparedExecution is null)
{
jerkRows.Add(J519SendTrajectorySampler.BuildJerkRow(
previousSendTime.Value,
sample.SendTime,
previousJoints,
sample.JointsDegrees,
ref previousVelocity,
ref previousAcceleration));
timingRows.Add(J519SendTrajectorySampler.BuildTimingRow(sample));
if (previousSendTime is not null && previousJoints is not null)
{
jerkRows.Add(J519SendTrajectorySampler.BuildJerkRow(
previousSendTime.Value,
sample.SendTime,
previousJoints,
sample.JointsDegrees,
ref previousVelocity,
ref previousAcceleration));
}
}
previousSendTime = sample.SendTime;