xsxiaosa/FlyShotHost
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

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

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

42
tests/Flyshot.Core.Tests/ConfigCompatibilityTests.cs
View File

@@ -21,6 +21,7 @@ public sealed class ConfigCompatibilityTests
Assert.True(loaded.Robot.UseDo);
Assert.Equal([7, 8], loaded.Robot.IoAddresses);
Assert.Equal(2, loaded.Robot.IoKeepCycles);
Assert.Equal(7, loaded.Robot.TriggerSampleIndexOffsetCycles);
Assert.Equal(1.0, loaded.Robot.AccLimitScale);
Assert.Equal(1.0, loaded.Robot.JerkLimitScale);
Assert.Equal(1.0, loaded.Robot.PlanningSpeedScale);
@@ -70,6 +71,7 @@ public sealed class ConfigCompatibilityTests
Assert.False(loaded.Robot.UseDo);
Assert.Empty(loaded.Robot.IoAddresses);
Assert.Equal(3, loaded.Robot.IoKeepCycles);
Assert.Equal(7, loaded.Robot.TriggerSampleIndexOffsetCycles);
Assert.Equal(0.5, loaded.Robot.AccLimitScale);
Assert.Equal(0.25, loaded.Robot.JerkLimitScale);
Assert.Equal(1.0, loaded.Robot.PlanningSpeedScale);
@@ -123,6 +125,46 @@ public sealed class ConfigCompatibilityTests
}
}
/// <summary>
/// 验证 RobotConfig.json 可以显式配置触发绑定后的命令 sample 后移周期数。
/// </summary>
[Fact]
public void RobotConfigLoader_LoadsTriggerSampleIndexOffsetCycles()
{
var tempRoot = CreateTempDirectory();
try
{
var configPath = Path.Combine(tempRoot, "legacy.json");
File.WriteAllText(
configPath,
"""
{
"robot": {
"use_do": true,
"io_keep_cycles": 2,
"trigger_sample_index_offset_cycles": 8,
"acc_limit": 1.0,
"jerk_limit": 1.0
},
"flying_shots": {
"demo": {
"traj_waypoints": [[0, 1], [2, 3], [4, 5], [6, 7]],
"shot_flags": [false, false, false, false]
}
}
}
""");
var loaded = new RobotConfigLoader().Load(configPath);
Assert.Equal(8, loaded.Robot.TriggerSampleIndexOffsetCycles);
}
finally
{
Directory.Delete(tempRoot, recursive: true);
}
}
/// <summary>
/// 验证 RobotConfig.json 可以关闭飞拍执行前的二次平滑起停时间重映射。
/// </summary>

2
tests/Flyshot.Core.Tests/ControllerClientCompatConfigRootTests.cs
View File

@@ -71,6 +71,7 @@ public sealed class ControllerClientCompatConfigRootTests
useDo: true,
ioAddresses: [7, 8],
ioKeepCycles: 2,
triggerSampleIndexOffsetCycles: 7,
accLimitScale: 1.0,
jerkLimitScale: 1.0,
adaptIcspTryNum: 5);
@@ -83,6 +84,7 @@ public sealed class ControllerClientCompatConfigRootTests
Assert.False(Directory.Exists(Path.Combine(configRoot, "TrajectoryStore")), "不应再创建独立轨迹存储目录。");
var loaded = store.LoadAll("FANUC_LR_Mate_200iD", out var loadedSettings);
Assert.NotNull(loadedSettings);
Assert.Equal(7, loadedSettings.TriggerSampleIndexOffsetCycles);
Assert.Contains(trajectory.Name, loaded);
store.Delete("FANUC_LR_Mate_200iD", trajectory.Name);

37
tests/Flyshot.Core.Tests/DomainModelTests.cs
View File

@@ -1,5 +1,6 @@
using System.Text.Json;
using Flyshot.Core.Domain;
using Flyshot.Core.Planning.Sampling;
namespace Flyshot.Core.Tests;
@@ -164,7 +165,8 @@ public sealed class DomainModelTests
triggerTime: 0.5,
offsetCycles: 0,
holdCycles: 1,
addressGroup: new IoAddressGroup(new[] { 100 }))
addressGroup: new IoAddressGroup(new[] { 100 }),
referenceJointsDegrees: new[] { 12.5, -3.0 })
},
artifacts: new[]
{
@@ -176,7 +178,8 @@ public sealed class DomainModelTests
failureReason: null,
usedCache: true,
originalWaypointCount: 4,
plannedWaypointCount: 5);
plannedWaypointCount: 5,
triggerSampleIndexOffsetCycles: 7);
var json = JsonSerializer.Serialize(result);
@@ -184,6 +187,36 @@ public sealed class DomainModelTests
Assert.Contains("\"method\":\"SelfAdaptIcsp\"", json);
Assert.Contains("\"kind\":\"JointDenseTrajectory\"", json);
Assert.Contains("\"usedCache\":true", json);
Assert.Contains("\"triggerSampleIndexOffsetCycles\":7", json);
Assert.Contains("\"referenceJointsDegrees\":[12.5,-3]", json);
}
/// <summary>
/// 验证触发绑定允许在最佳 sample 基础上继续向后偏移固定命令周期。
/// </summary>
[Fact]
public void TriggerSampleBinder_Bind_AppliesConfiguredSampleIndexOffset()
{
var trigger = new TrajectoryDoEvent(
waypointIndex: 1,
triggerTime: 0.008,
offsetCycles: 0,
holdCycles: 2,
addressGroup: new IoAddressGroup([2, 4]),
referenceJointsDegrees: [10.0, 20.0]);
var samples = new[]
{
new J519SendSample(sampleIndex: 0, sendTime: 0.0, trajectoryTime: 0.0, speedRatio: 1.0, jointsDegrees: [0.0, 0.0]),
new J519SendSample(sampleIndex: 1, sendTime: 0.008, trajectoryTime: 0.008, speedRatio: 1.0, jointsDegrees: [10.0, 20.0]),
new J519SendSample(sampleIndex: 2, sendTime: 0.016, trajectoryTime: 0.016, speedRatio: 1.0, jointsDegrees: [11.0, 21.0]),
new J519SendSample(sampleIndex: 3, sendTime: 0.024, trajectoryTime: 0.024, speedRatio: 1.0, jointsDegrees: [12.0, 22.0])
};
var binding = Assert.Single(Flyshot.Core.Planning.Sampling.TriggerSampleBinder.Bind([trigger], samples, sampleIndexOffsetCycles: 2));
Assert.True(binding.FoundInWindow);
Assert.Equal(3, binding.SampleIndex);
Assert.Equal(0.024, binding.Sample.SendTime, precision: 6);
}
/// <summary>

89
tests/Flyshot.Core.Tests/FanucControllerRuntimeDenseTests.cs
View File

@@ -146,8 +146,8 @@ public sealed class FanucControllerRuntimeDenseTests
var denseTrajectory = new[]
{
new[] { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 },
new[] { 0.008, Math.PI / 2.0, 0.0, 0.0, 0.0, 0.0, 0.0 },
new[] { 0.016, Math.PI, 0.0, 0.0, 0.0, 0.0, 0.0 }
new[] { 0.008, DegreesToRadians(1.0), 0.0, 0.0, 0.0, 0.0, 0.0 },
new[] { 0.016, DegreesToRadians(2.0), 0.0, 0.0, 0.0, 0.0, 0.0 }
};
var result = new TrajectoryResult(
@@ -164,7 +164,7 @@ public sealed class FanucControllerRuntimeDenseTests
plannedWaypointCount: 4,
denseJointTrajectory: denseTrajectory);
runtime.ExecuteTrajectory(result, [Math.PI, 0.0, 0.0, 0.0, 0.0, 0.0]);
runtime.ExecuteTrajectory(result, [DegreesToRadians(2.0), 0.0, 0.0, 0.0, 0.0, 0.0]);
WaitUntilIdle(runtime);
var commands = j519Client.GetCommandHistoryForTests();
@@ -172,14 +172,17 @@ public sealed class FanucControllerRuntimeDenseTests
var pointsPath = Path.Combine(runDir, "ActualSendJointTraj.txt");
var timingPath = Path.Combine(runDir, "ActualSendTiming.txt");
var jerkPath = Path.Combine(runDir, "ActualSendJerkStats.txt");
var shotEventsPath = Path.Combine(runDir, "ShotEvents.json");
Assert.True(File.Exists(pointsPath));
Assert.True(File.Exists(timingPath));
Assert.True(File.Exists(jerkPath));
Assert.True(File.Exists(shotEventsPath));
var pointLines = File.ReadAllLines(pointsPath);
var timingLines = File.ReadAllLines(timingPath);
var jerkLines = File.ReadAllLines(jerkPath);
var shotEventsJson = File.ReadAllText(shotEventsPath);
Assert.Equal(commands.Count, pointLines.Length);
Assert.Equal(commands.Count, timingLines.Length);
Assert.Equal(Math.Max(0, commands.Count - 1), jerkLines.Length);
@@ -191,7 +194,7 @@ public sealed class FanucControllerRuntimeDenseTests
Assert.Equal(9, lastColumns.Length);
Assert.Equal(0.0, firstColumns[0], precision: 6);
Assert.Equal(0.008, secondColumns[0], precision: 6);
Assert.Equal(180.0, lastColumns[1], precision: 6);
Assert.Equal(2.0, lastColumns[1], precision: 6);
var firstTimingColumns = ParseColumns(timingLines[0]);
var secondTimingColumns = ParseColumns(timingLines[1]);
@@ -211,6 +214,8 @@ public sealed class FanucControllerRuntimeDenseTests
Assert.Equal(10, firstJerkColumns.Length);
Assert.Equal(0.0, firstJerkColumns[0], precision: 6);
Assert.Equal(0.008, firstJerkColumns[2], precision: 6);
Assert.Equal("[]", shotEventsJson.Trim());
}
finally
{
@@ -561,8 +566,63 @@ public sealed class FanucControllerRuntimeDenseTests
var commands = j519Client.GetCommandHistoryForTests();
Assert.Equal(4, commands.Count);
Assert.Equal([(ushort)0, (ushort)10, (ushort)10, (ushort)10], commands.Select(static command => command.WriteIoMask));
Assert.Equal([(ushort)0, (ushort)10, (ushort)10, (ushort)0], commands.Select(static command => command.WriteIoValue));
Assert.Equal([(ushort)0, (ushort)5, (ushort)5, (ushort)5], commands.Select(static command => command.WriteIoMask));
Assert.Equal([(ushort)0, (ushort)5, (ushort)5, (ushort)0], commands.Select(static command => command.WriteIoValue));
}
/// <summary>
/// 验证同一时间窗口内存在多个候选 sample 时，会优先把 IO 挂到关节坐标最接近示教点的那一帧。
/// </summary>
[Fact]
public void ExecuteTrajectory_WithDenseWaypoints_RealMode_PrefersClosestJointSampleWithinTriggerWindow()
{
using var commandClient = new FanucCommandClient();
using var stateClient = new FanucStateClient();
using var j519Client = new FanucJ519Client();
using var runtime = new FanucControllerRuntime(commandClient, stateClient, j519Client);
var robot = TestRobotFactory.CreateRobotProfile();
runtime.ResetRobot(robot, "FANUC_LR_Mate_200iD");
j519Client.EnableCommandHistoryForTests();
ForceRealModeEnabled(runtime, speedRatio: 1.0);
var denseTrajectory = new[]
{
new[] { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 },
new[] { 0.008, DegreesToRadians(1.0), 0.0, 0.0, 0.0, 0.0, 0.0 },
new[] { 0.016, DegreesToRadians(2.0), 0.0, 0.0, 0.0, 0.0, 0.0 },
new[] { 0.024, DegreesToRadians(3.0), 0.0, 0.0, 0.0, 0.0, 0.0 }
};
var result = new TrajectoryResult(
programName: "demo",
method: PlanningMethod.Icsp,
isValid: true,
duration: TimeSpan.FromSeconds(0.024),
shotEvents: Array.Empty<ShotEvent>(),
triggerTimeline:
[
new TrajectoryDoEvent(
waypointIndex: 1,
triggerTime: 0.012,
offsetCycles: 0,
holdCycles: 1,
addressGroup: new IoAddressGroup([1]),
referenceJointsDegrees: [2.0, 0.0, 0.0, 0.0, 0.0, 0.0])
],
artifacts: Array.Empty<TrajectoryArtifact>(),
failureReason: null,
usedCache: false,
originalWaypointCount: 4,
plannedWaypointCount: 4,
denseJointTrajectory: denseTrajectory);
runtime.ExecuteTrajectory(result, [DegreesToRadians(3.0), 0.0, 0.0, 0.0, 0.0, 0.0]);
WaitUntilIdle(runtime);
var commands = j519Client.GetCommandHistoryForTests();
Assert.Equal(4, commands.Count);
Assert.Equal([(ushort)0, (ushort)0, (ushort)1, (ushort)1], commands.Select(static command => command.WriteIoMask));
Assert.Equal([(ushort)0, (ushort)0, (ushort)1, (ushort)0], commands.Select(static command => command.WriteIoValue));
}
/// <summary>
@@ -677,13 +737,16 @@ public sealed class FanucControllerRuntimeDenseTests
}
/// <summary>
/// 验证 IO 地址组中的地址号被正确映射为 writeIoValue 的位掩码。
/// 验证 IO 地址组中的地址号按 writeIoIndex=1 的手册语义映射为 writeIoValue 位掩码。
/// </summary>
[Theory]
[InlineData(new[] { 0 }, (ushort)1)]
[InlineData(new[] { 7 }, (ushort)128)]
[InlineData(new[] { 7, 8 }, (ushort)384)] // 128 + 256
[InlineData(new[] { 15 }, (ushort)32768)]
[InlineData(new[] { 1 }, (ushort)1)]
[InlineData(new[] { 2, 4 }, (ushort)10)]
[InlineData(new[] { 3, 4 }, (ushort)12)]
[InlineData(new[] { 2, 3, 4 }, (ushort)14)]
[InlineData(new[] { 8 }, (ushort)128)]
[InlineData(new[] { 8, 9 }, (ushort)384)] // 128 + 256
[InlineData(new[] { 16 }, (ushort)32768)]
[InlineData(new int[] { }, (ushort)0)]
public void ComputeIoValue_MapsAddressesToBitMask(int[] addresses, ushort expected)
{
@@ -693,12 +756,12 @@ public sealed class FanucControllerRuntimeDenseTests
}
/// <summary>
/// 验证超过 15 的地址号会被安全忽略，不会溢出位掩码。
/// 验证超出 1..16 范围的地址号会被安全忽略，不会污染位掩码。
/// </summary>
[Fact]
public void ComputeIoValue_IgnoresOutOfRangeAddresses()
{
var group = new IoAddressGroup([0, 16, 7]);
var group = new IoAddressGroup([0, 1, 17, 8]);
var actual = FanucControllerRuntime.ComputeIoValue(group);
Assert.Equal((ushort)(1 | 128), actual);
}

50
tests/Flyshot.Core.Tests/PlanningCompatibilityTests.cs
View File

@@ -170,6 +170,51 @@ public sealed class PlanningCompatibilityTests
Assert.Equal(1, doEvent.WaypointIndex);
Assert.Equal(1, doEvent.OffsetCycles);
Assert.Equal(2, doEvent.HoldCycles);
Assert.NotNull(doEvent.ReferenceJointsDegrees);
Assert.Equal(RadiansToDegrees(1.0), doEvent.ReferenceJointsDegrees![0], precision: 6);
Assert.Equal(0.0, doEvent.ReferenceJointsDegrees![1], precision: 6);
}
/// <summary>
/// 验证时间轴会把示教点关节角保存到 DO 事件里，供运行时在近时窗内做最小关节差绑定。
/// </summary>
[Fact]
public void ShotTimelineBuilder_PopulatesReferenceJointsDegreesForRuntimeBinding()
{
var robot = CreateRobotProfile([1, 1], [1, 1], [1, 1]);
var program = new FlyshotProgram(
name: "demo",
waypoints:
[
new JointWaypoint([0.0, 0.0]),
new JointWaypoint([Math.PI / 6.0, -Math.PI / 4.0])
],
shotFlags: [false, true],
offsetValues: [0, 0],
addressGroups:
[
new IoAddressGroup(Array.Empty<int>()),
new IoAddressGroup([1])
]);
var trajectory = new PlannedTrajectory(
robot: robot,
originalProgram: program,
plannedWaypoints: program.Waypoints,
waypointTimes: [0.0, 0.5],
segmentDurations: [0.5],
segmentScales: [1.0],
method: PlanningMethod.Icsp,
iterations: 1,
threshold: 0.0);
var timeline = new ShotTimelineBuilder(new WaypointTimestampResolver())
.Build(trajectory, holdCycles: 1, samplePeriod: TimeSpan.FromMilliseconds(8));
var trigger = Assert.Single(timeline.TriggerTimeline);
Assert.NotNull(trigger.ReferenceJointsDegrees);
Assert.Equal(30.0, trigger.ReferenceJointsDegrees![0], precision: 6);
Assert.Equal(-45.0, trigger.ReferenceJointsDegrees![1], precision: 6);
}
/// <summary>
@@ -271,4 +316,9 @@ public sealed class PlanningCompatibilityTests
throw new DirectoryNotFoundException("Unable to locate the flyshot workspace root.");
}
private static double RadiansToDegrees(double radians)
{
return radians * 180.0 / Math.PI;
}
}

8
tests/Flyshot.Core.Tests/RuntimeOrchestrationTests.cs
View File

@@ -189,6 +189,7 @@ public sealed class RuntimeOrchestrationTests
useDo: true,
ioAddresses: [7, 8],
ioKeepCycles: 4,
triggerSampleIndexOffsetCycles: 7,
accLimitScale: 1.0,
jerkLimitScale: 1.0,
adaptIcspTryNum: 5);
@@ -212,6 +213,7 @@ public sealed class RuntimeOrchestrationTests
useDo: false,
ioAddresses: [7, 8],
ioKeepCycles: 4,
triggerSampleIndexOffsetCycles: 7,
accLimitScale: 1.0,
jerkLimitScale: 1.0,
adaptIcspTryNum: 5);
@@ -940,11 +942,14 @@ public sealed class RuntimeOrchestrationTests
var outputDir = Path.Combine(configRoot, "Data", "demo-flyshot");
var pointsPath = Path.Combine(outputDir, "ActualSendJointTraj.txt");
var timingPath = Path.Combine(outputDir, "ActualSendTiming.txt");
var shotEventsPath = Path.Combine(outputDir, "ShotEvents.json");
Assert.True(File.Exists(pointsPath));
Assert.True(File.Exists(timingPath));
Assert.True(File.Exists(shotEventsPath));
var pointRows = File.ReadAllLines(pointsPath).Select(ParseSpaceSeparatedDoubles).ToArray();
var timingRows = File.ReadAllLines(timingPath).Select(ParseSpaceSeparatedDoubles).ToArray();
var shotEventsJson = File.ReadAllText(shotEventsPath);
var executionDuration = double.Parse(
File.ReadLines(Path.Combine(outputDir, "JointDetialTraj.txt")).Last().Split(' ')[0],
CultureInfo.InvariantCulture);
@@ -956,6 +961,8 @@ public sealed class RuntimeOrchestrationTests
Assert.Equal(0.008, pointRows[1][0], precision: 6);
Assert.Equal(0.004, timingRows[1][2], precision: 6);
Assert.Equal(0.5, timingRows[1][3], precision: 6);
Assert.Contains("\"trigger_window_seconds\": 0.1", shotEventsJson);
Assert.Contains("\"selected_sample_index\"", shotEventsJson);
}
finally
{
@@ -1127,6 +1134,7 @@ public sealed class RuntimeOrchestrationTests
useDo: settings.UseDo,
ioAddresses: settings.IoAddresses,
ioKeepCycles: settings.IoKeepCycles,
triggerSampleIndexOffsetCycles: settings.TriggerSampleIndexOffsetCycles,
accLimitScale: settings.AccLimitScale,
jerkLimitScale: settings.JerkLimitScale,
adaptIcspTryNum: settings.AdaptIcspTryNum,