✨ feat(flyshot): 引入飞拍执行侧最终发送队列构建与校验机制
* 新增 FlyshotExecutionSendSequenceBuilder，负责在运行时前构建最终 8ms 发送队列，并进行离散限幅校验。 * 引入 FlyshotPreparedExecution 类，封装最终发送结果及相关诊断信息。 * 调整 ControllerClientCompatService 和 FanucControllerRuntime，确保运行时直接使用预生成的发送队列，避免临场重采样。 * 更新 TrajectoryResult 和 PlannedExecutionBundle，支持准备好的执行队列。 * 增加单元测试，验证非 1 倍 speedRatio 下的执行行为与预生成队列的使用。
This commit is contained in:
@@ -1,6 +1,7 @@
|
||||
using Flyshot.Core.Domain;
|
||||
using Flyshot.ControllerClientCompat;
|
||||
using Flyshot.Core.Config;
|
||||
using Flyshot.Core.Planning.Sampling;
|
||||
using Flyshot.Runtime.Fanuc;
|
||||
using Flyshot.Runtime.Fanuc.Protocol;
|
||||
using System.Buffers.Binary;
|
||||
@@ -274,8 +275,8 @@ public sealed class FanucControllerRuntimeDenseTests
|
||||
Assert.NotEmpty(pointsLines);
|
||||
Assert.NotEmpty(timingLines);
|
||||
Assert.NotEmpty(jerkLines);
|
||||
Assert.Equal(927, pointsLines.Length);
|
||||
Assert.Equal(927, timingLines.Length);
|
||||
Assert.Equal(result.DenseJointTrajectory!.Count, pointsLines.Length);
|
||||
Assert.Equal(result.DenseJointTrajectory.Count, timingLines.Length);
|
||||
|
||||
var firstPoint = ParseColumns(pointsLines[0]);
|
||||
var secondPoint = ParseColumns(pointsLines[1]);
|
||||
@@ -323,6 +324,144 @@ public sealed class FanucControllerRuntimeDenseTests
|
||||
Assert.True(File.Exists(summaryPath));
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 验证真实 UTTC_MS11 轨迹在非 1 倍 speedRatio 下仍能生成并装载 J519 实发队列。
|
||||
/// </summary>
|
||||
[Theory]
|
||||
[InlineData(0.7)]
|
||||
[InlineData(0.5)]
|
||||
public void ExecuteTrajectory_UttcMs11FromHostRuntimeConfig_RealMode_AllowsNonOneSpeedRatio(double speedRatio)
|
||||
{
|
||||
using var commandClient = new FanucCommandClient();
|
||||
using var stateClient = new FanucStateClient();
|
||||
using var j519Client = new FanucJ519Client();
|
||||
using var runtime = new FanucControllerRuntime(commandClient, stateClient, j519Client);
|
||||
var fixture = LoadUttcMs11RuntimeFixture();
|
||||
var fullSpeedSettings = new CompatibilityRobotSettings(
|
||||
useDo: fixture.Settings.UseDo,
|
||||
ioAddresses: fixture.Settings.IoAddresses,
|
||||
ioKeepCycles: fixture.Settings.IoKeepCycles,
|
||||
triggerSampleIndexOffsetCycles: fixture.Settings.TriggerSampleIndexOffsetCycles,
|
||||
accLimitScale: fixture.Settings.AccLimitScale,
|
||||
jerkLimitScale: fixture.Settings.JerkLimitScale,
|
||||
adaptIcspTryNum: fixture.Settings.AdaptIcspTryNum,
|
||||
planningSpeedScale: 1.0,
|
||||
smoothStartStopTiming: true);
|
||||
var orchestrator = new ControllerClientTrajectoryOrchestrator();
|
||||
var bundle = orchestrator.PlanUploadedFlyshot(
|
||||
fixture.Robot,
|
||||
fixture.Uploaded,
|
||||
settings: fullSpeedSettings,
|
||||
planningSpeedScale: fullSpeedSettings.PlanningSpeedScale);
|
||||
var preparedExecution = FlyshotExecutionSendSequenceBuilder.Build(
|
||||
fixture.Robot,
|
||||
bundle.Result,
|
||||
fixture.Robot.ServoPeriod.TotalSeconds,
|
||||
speedRatio);
|
||||
var result = WithUniqueProgramName(new TrajectoryResult(
|
||||
programName: bundle.Result.ProgramName,
|
||||
method: bundle.Result.Method,
|
||||
isValid: bundle.Result.IsValid,
|
||||
duration: bundle.Result.Duration,
|
||||
shotEvents: bundle.Result.ShotEvents,
|
||||
triggerTimeline: bundle.Result.TriggerTimeline,
|
||||
artifacts: bundle.Result.Artifacts,
|
||||
failureReason: bundle.Result.FailureReason,
|
||||
usedCache: bundle.Result.UsedCache,
|
||||
originalWaypointCount: bundle.Result.OriginalWaypointCount,
|
||||
plannedWaypointCount: bundle.Result.PlannedWaypointCount,
|
||||
triggerSampleIndexOffsetCycles: bundle.Result.TriggerSampleIndexOffsetCycles,
|
||||
denseJointTrajectory: bundle.Result.DenseJointTrajectory,
|
||||
preparedFlyshotExecution: preparedExecution), $"UTTC_MS11_speedratio_{speedRatio:F1}_{Guid.NewGuid():N}");
|
||||
var outputRoot = Path.Combine(AppContext.BaseDirectory, "Config", "Data", result.ProgramName);
|
||||
|
||||
try
|
||||
{
|
||||
runtime.ResetRobot(fixture.Robot, fixture.Robot.Name);
|
||||
j519Client.EnableCommandHistoryForTests();
|
||||
ForceRealModeEnabled(runtime, speedRatio);
|
||||
|
||||
runtime.ExecuteTrajectory(result, result.DenseJointTrajectory![^1].Skip(1).ToArray());
|
||||
WaitUntilIdle(runtime);
|
||||
|
||||
var commands = j519Client.GetCommandHistoryForTests();
|
||||
Assert.NotEmpty(commands);
|
||||
Assert.Equal(preparedExecution.Samples.Count, commands.Count);
|
||||
AssertJointDegreesEqual(result.DenseJointTrajectory[0].Skip(1).ToArray(), commands[0].TargetJoints);
|
||||
}
|
||||
finally
|
||||
{
|
||||
if (Directory.Exists(outputRoot))
|
||||
{
|
||||
Directory.Delete(outputRoot, recursive: true);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 验证运行时拿到飞拍预生成发送队列后，会直接消费该队列，而不是再按当前 speedRatio 临场重采样。
|
||||
/// </summary>
|
||||
[Fact]
|
||||
public void ExecuteTrajectory_WithPreparedFlyshotExecution_RealMode_UsesPreparedSamplesDirectly()
|
||||
{
|
||||
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: 0.5);
|
||||
|
||||
var preparedSamples = new[]
|
||||
{
|
||||
new FlyshotPreparedSample(0, 0.0, 0.0, 1.0, [1.0, 0.0, 0.0, 0.0, 0.0, 0.0]),
|
||||
new FlyshotPreparedSample(1, 0.008, 0.008, 1.0, [2.0, 0.0, 0.0, 0.0, 0.0, 0.0]),
|
||||
new FlyshotPreparedSample(2, 0.016, 0.016, 1.0, [3.0, 0.0, 0.0, 0.0, 0.0, 0.0])
|
||||
};
|
||||
var preparedExecution = new FlyshotPreparedExecution(
|
||||
samples: preparedSamples,
|
||||
triggerBindings: Array.Empty<FlyshotPreparedTriggerBinding>(),
|
||||
timingRows: preparedSamples.Select(static sample => (IReadOnlyList<double>)
|
||||
[
|
||||
sample.SampleIndex,
|
||||
Math.Round(sample.SendTime, 6),
|
||||
Math.Round(sample.TrajectoryTime, 6),
|
||||
Math.Round(sample.SpeedRatio, 6)
|
||||
]).ToArray(),
|
||||
jerkRows: Array.Empty<IReadOnlyList<double>>(),
|
||||
requestSpeedRatio: 1.0,
|
||||
finalSpeedRatio: 1.0,
|
||||
finalDurationSeconds: 0.016,
|
||||
stretchIterationCount: 0);
|
||||
var result = new TrajectoryResult(
|
||||
programName: "prepared-demo",
|
||||
method: PlanningMethod.Icsp,
|
||||
isValid: true,
|
||||
duration: TimeSpan.FromSeconds(0.016),
|
||||
shotEvents: Array.Empty<ShotEvent>(),
|
||||
triggerTimeline: Array.Empty<TrajectoryDoEvent>(),
|
||||
artifacts: Array.Empty<TrajectoryArtifact>(),
|
||||
failureReason: null,
|
||||
usedCache: false,
|
||||
originalWaypointCount: 4,
|
||||
plannedWaypointCount: 4,
|
||||
denseJointTrajectory:
|
||||
[
|
||||
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 }
|
||||
],
|
||||
preparedFlyshotExecution: preparedExecution);
|
||||
|
||||
runtime.ExecuteTrajectory(result, [DegreesToRadians(3.0), 0.0, 0.0, 0.0, 0.0, 0.0]);
|
||||
WaitUntilIdle(runtime);
|
||||
|
||||
var commands = j519Client.GetCommandHistoryForTests();
|
||||
Assert.Equal(preparedSamples.Length, commands.Count);
|
||||
Assert.Equal([1.0, 2.0, 3.0], commands.Select(static command => command.TargetJoints[0]));
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 验证 MoveJoint 会按抓包确认的点到点临时轨迹生成稠密 J519 目标，并继续叠加 speed_ratio 重采样。
|
||||
/// </summary>
|
||||
@@ -1223,7 +1362,8 @@ public sealed class FanucControllerRuntimeDenseTests
|
||||
usedCache: result.UsedCache,
|
||||
originalWaypointCount: result.OriginalWaypointCount,
|
||||
plannedWaypointCount: result.PlannedWaypointCount,
|
||||
denseJointTrajectory: result.DenseJointTrajectory);
|
||||
denseJointTrajectory: result.DenseJointTrajectory,
|
||||
preparedFlyshotExecution: result.PreparedFlyshotExecution);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
|
||||
Reference in New Issue
Block a user