✨ 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>
|
||||
|
||||
@@ -826,6 +826,48 @@ public sealed class RuntimeOrchestrationTests
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 验证飞拍链路在进入运行时前就会准备最终发送队列，而不是把 speedRatio 重采样留给运行时临场处理。
|
||||
/// </summary>
|
||||
[Fact]
|
||||
public void ControllerClientCompatService_ExecuteTrajectoryByName_PreparesFinalSendQueueBeforeRuntime()
|
||||
{
|
||||
var configRoot = CreateTempConfigRoot();
|
||||
try
|
||||
{
|
||||
WriteRobotConfigWithDemoTrajectory(configRoot);
|
||||
var options = new ControllerClientCompatOptions { ConfigRoot = configRoot };
|
||||
var runtime = new RecordingControllerRuntime();
|
||||
var service = new ControllerClientCompatService(
|
||||
options,
|
||||
new ControllerClientCompatRobotCatalog(options, new RobotModelLoader()),
|
||||
runtime,
|
||||
new ControllerClientTrajectoryOrchestrator(),
|
||||
new RobotConfigLoader());
|
||||
|
||||
service.SetUpRobot("FANUC_LR_Mate_200iD");
|
||||
service.SetActiveController(sim: false);
|
||||
service.Connect("192.168.10.101");
|
||||
service.EnableRobot(2);
|
||||
service.UploadTrajectory(TestRobotFactory.CreateUploadedTrajectoryWithSingleShot());
|
||||
runtime.SetSpeedRatio(0.5);
|
||||
|
||||
service.ExecuteTrajectoryByName(
|
||||
"demo-flyshot",
|
||||
new FlyshotExecutionOptions(moveToStart: false, method: "icsp", saveTrajectory: false, useCache: false));
|
||||
|
||||
var result = Assert.IsType<TrajectoryResult>(runtime.LastExecutedResult);
|
||||
var preparedExecution = Assert.IsType<FlyshotPreparedExecution>(result.PreparedFlyshotExecution);
|
||||
Assert.NotEmpty(preparedExecution.Samples);
|
||||
Assert.Equal(preparedExecution.Samples.Count, preparedExecution.TimingRows.Count);
|
||||
Assert.Equal(result.TriggerTimeline.Count, preparedExecution.TriggerBindings.Count);
|
||||
}
|
||||
finally
|
||||
{
|
||||
Directory.Delete(configRoot, recursive: true);
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 验证兼容服务初始化机器人时会把 RobotConfig.json 中的 acc_limit / jerk_limit 传给模型加载器。
|
||||
/// </summary>
|
||||
@@ -953,14 +995,14 @@ public sealed class RuntimeOrchestrationTests
|
||||
var executionDuration = double.Parse(
|
||||
File.ReadLines(Path.Combine(outputDir, "JointDetialTraj.txt")).Last().Split(' ')[0],
|
||||
CultureInfo.InvariantCulture);
|
||||
var expectedRows = (int)Math.Ceiling(Math.Max(0.0, (executionDuration / (0.008 * 0.5)) - 1e-9)) + 1;
|
||||
var minimumExpectedRows = (int)Math.Ceiling(Math.Max(0.0, (executionDuration / (0.008 * 0.5)) - 1e-9)) + 1;
|
||||
|
||||
Assert.Equal(expectedRows, pointRows.Length);
|
||||
Assert.Equal(expectedRows, timingRows.Length);
|
||||
Assert.Equal(pointRows.Length, timingRows.Length);
|
||||
Assert.True(pointRows.Length >= minimumExpectedRows, $"最终发送点数应不少于请求倍率的首轮候选值，actual={pointRows.Length}, min={minimumExpectedRows}");
|
||||
Assert.Equal(0.0, pointRows[0][0], precision: 6);
|
||||
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.True(timingRows[1][2] <= 0.004 + 1e-6, $"自动拉长后 trajectory_time 推进不应快于请求倍率，actual={timingRows[1][2]:F6}");
|
||||
Assert.True(timingRows[1][3] <= 0.5 + 1e-6, $"最终采用倍率不应快于请求倍率，actual={timingRows[1][3]:F6}");
|
||||
Assert.Contains("\"trigger_window_seconds\": 0.1", shotEventsJson);
|
||||
Assert.Contains("\"selected_sample_index\"", shotEventsJson);
|
||||
}
|
||||
@@ -1607,6 +1649,11 @@ internal sealed class RecordingControllerRuntime : IControllerRuntime
|
||||
/// </summary>
|
||||
public RobotProfile? LastRobotProfile { get; private set; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取最近一次 ExecuteTrajectory 收到的结果对象。
|
||||
/// </summary>
|
||||
public TrajectoryResult? LastExecutedResult { get; private set; }
|
||||
|
||||
/// <inheritdoc />
|
||||
public void ResetRobot(RobotProfile robot, string robotName)
|
||||
{
|
||||
@@ -1691,6 +1738,7 @@ internal sealed class RecordingControllerRuntime : IControllerRuntime
|
||||
/// <inheritdoc />
|
||||
public void ExecuteTrajectory(TrajectoryResult result, IReadOnlyList<double> finalJointPositions)
|
||||
{
|
||||
LastExecutedResult = result;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
Reference in New Issue
Block a user