✨ feat(*): 添加触发样本偏移与实发轨迹分析导出

* 为 RobotConfig 增加 trigger_sample_index_offset_cycles 配置
  * 让 DO 事件携带示教点关节角并按最接近 sample 绑定触发
  * 调整运行时 IO 地址位掩码映射并补充 ShotEvents 导出
  * 新增 2026042802-1 抓包分析脚本、数据产物与结论文档
  * 补齐配置兼容、规划绑定和运行时触发相关测试
This commit is contained in:
2026-05-09 11:12:31 +08:00
parent 1779067b5c
commit f7e2bb0e7b
35 changed files with 5772 additions and 55 deletions

View File

@@ -0,0 +1,134 @@
using Flyshot.Core.Domain;
namespace Flyshot.Core.Planning.Sampling;
/// <summary>
/// 负责把理论触发事件绑定到实际 J519 发送 sample。
/// 绑定规则为:先在理论触发时间前后固定时间窗内筛候选点,再优先选择关节差最小的 sample。
/// </summary>
public static class TriggerSampleBinder
{
/// <summary>
/// 当前触发绑定使用的固定近时窗半宽,单位为秒。
/// 即:以理论触发时间为中心,在前后各 100ms 的范围内寻找最优 sample。
/// </summary>
public const double TriggerBindingToleranceSeconds = 0.1;
/// <summary>
/// 把一组触发事件绑定到采样后的 J519 sample。
/// </summary>
/// <param name="triggers">待绑定的触发事件。</param>
/// <param name="samples">J519 实发 sample。</param>
/// <param name="sampleIndexOffsetCycles">在最佳绑定 sample 基础上继续向后偏移的命令周期数。</param>
/// <returns>按最终 sampleIndex 排序的绑定结果。</returns>
public static IReadOnlyList<TriggerSampleBinding> Bind(
IReadOnlyList<TrajectoryDoEvent> triggers,
IReadOnlyList<J519SendSample> samples,
int sampleIndexOffsetCycles = 0)
{
ArgumentNullException.ThrowIfNull(triggers);
ArgumentNullException.ThrowIfNull(samples);
if (sampleIndexOffsetCycles < 0)
{
throw new ArgumentOutOfRangeException(nameof(sampleIndexOffsetCycles), "触发 sample 偏移周期不能为负数。");
}
var bindings = new List<TriggerSampleBinding>(triggers.Count);
if (triggers.Count == 0 || samples.Count == 0)
{
return bindings;
}
var lastSampleIndex = samples.Count - 1;
foreach (var trigger in triggers)
{
var fallbackIndex = FindClosestTrajectoryTimeSampleIndex(samples, trigger.TriggerTime);
var candidateStart = Math.Max(0, fallbackIndex - 1);
var candidateEnd = Math.Min(lastSampleIndex, fallbackIndex + 1);
var bestIndex = fallbackIndex;
var bestScore = double.PositiveInfinity;
var foundInWindow = false;
for (var sampleIndex = candidateStart; sampleIndex <= candidateEnd; sampleIndex++)
{
var sample = samples[sampleIndex];
if (Math.Abs(sample.TrajectoryTime - trigger.TriggerTime) > TriggerBindingToleranceSeconds)
{
continue;
}
foundInWindow = true;
var score = trigger.ReferenceJointsDegrees is { Count: > 0 }
? ComputeJointDifferenceScore(trigger.ReferenceJointsDegrees, sample.JointsDegrees)
: Math.Abs(sample.TrajectoryTime - trigger.TriggerTime);
if (score < bestScore)
{
bestScore = score;
bestIndex = sampleIndex;
}
}
var finalIndex = Math.Min(lastSampleIndex, bestIndex + sampleIndexOffsetCycles);
bindings.Add(new TriggerSampleBinding(trigger, samples[finalIndex], finalIndex, foundInWindow));
}
return bindings
.OrderBy(static binding => binding.SampleIndex)
.ThenBy(static binding => binding.Trigger.WaypointIndex)
.ToArray();
}
/// <summary>
/// 按轨迹时间找到最接近理论触发时刻的 sample。
/// </summary>
public static int FindClosestTrajectoryTimeSampleIndex(IReadOnlyList<J519SendSample> samples, double triggerTime)
{
ArgumentNullException.ThrowIfNull(samples);
var bestIndex = 0;
var bestDelta = double.PositiveInfinity;
for (var sampleIndex = 0; sampleIndex < samples.Count; sampleIndex++)
{
var delta = Math.Abs(samples[sampleIndex].TrajectoryTime - triggerTime);
if (delta < bestDelta)
{
bestDelta = delta;
bestIndex = sampleIndex;
}
}
return bestIndex;
}
/// <summary>
/// 计算参考示教点与候选 sample 的关节差评分,使用平方和即可稳定排序。
/// </summary>
public static double ComputeJointDifferenceScore(
IReadOnlyList<double>? referenceJointsDegrees,
IReadOnlyList<double> sampleJointsDegrees)
{
if (referenceJointsDegrees is null || referenceJointsDegrees.Count == 0)
{
return 0.0;
}
var jointCount = Math.Min(referenceJointsDegrees.Count, sampleJointsDegrees.Count);
var score = 0.0;
for (var index = 0; index < jointCount; index++)
{
var delta = sampleJointsDegrees[index] - referenceJointsDegrees[index];
score += delta * delta;
}
return score;
}
}
/// <summary>
/// 表示一个理论触发事件最终绑定到哪个 J519 sample 的结果。
/// </summary>
public sealed record TriggerSampleBinding(
TrajectoryDoEvent Trigger,
J519SendSample Sample,
int SampleIndex,
bool FoundInWindow);