✨ feat(flyshot): 引入飞拍执行侧最终发送队列构建与校验机制
* 新增 FlyshotExecutionSendSequenceBuilder,负责在运行时前构建最终 8ms 发送队列,并进行离散限幅校验。 * 引入 FlyshotPreparedExecution 类,封装最终发送结果及相关诊断信息。 * 调整 ControllerClientCompatService 和 FanucControllerRuntime,确保运行时直接使用预生成的发送队列,避免临场重采样。 * 更新 TrajectoryResult 和 PlannedExecutionBundle,支持准备好的执行队列。 * 增加单元测试,验证非 1 倍 speedRatio 下的执行行为与预生成队列的使用。
This commit is contained in:
@@ -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;
|
||||
|
||||
Reference in New Issue
Block a user