✨ feat(runtime): 添加轨迹持久化与密集执行链路
* 新增飞拍轨迹文件存储,支持上传、加载与删除 * 接通 ControllerClientCompat 到运行时的轨迹编排 * 完善 FANUC 命令与 J519 客户端发送链路 * 补充密集轨迹执行、运行时编排和协议客户端测试 * 更新 README 与 AGENTS 中的当前实现状态
This commit is contained in:
@@ -1,5 +1,7 @@
|
||||
using Flyshot.Core.Config;
|
||||
using Flyshot.Core.Domain;
|
||||
using Flyshot.Core.Planning;
|
||||
using Flyshot.Core.Planning.Sampling;
|
||||
using Flyshot.Core.Triggering;
|
||||
|
||||
namespace Flyshot.ControllerClientCompat;
|
||||
@@ -59,11 +61,13 @@ public sealed class ControllerClientTrajectoryOrchestrator
|
||||
public PlannedExecutionBundle PlanUploadedFlyshot(
|
||||
RobotProfile robot,
|
||||
ControllerClientCompatUploadedTrajectory uploaded,
|
||||
FlyshotExecutionOptions? options = null)
|
||||
FlyshotExecutionOptions? options = null,
|
||||
CompatibilityRobotSettings? settings = null)
|
||||
{
|
||||
ArgumentNullException.ThrowIfNull(robot);
|
||||
ArgumentNullException.ThrowIfNull(uploaded);
|
||||
options ??= new FlyshotExecutionOptions();
|
||||
settings ??= CreateDefaultRobotSettings();
|
||||
|
||||
var program = CreateProgram(
|
||||
name: uploaded.Name,
|
||||
@@ -73,7 +77,7 @@ public sealed class ControllerClientTrajectoryOrchestrator
|
||||
addressGroups: uploaded.AddressGroups);
|
||||
|
||||
var method = ParseFlyshotMethod(options.Method);
|
||||
var cacheKey = CreateFlyshotCacheKey(robot, uploaded, options);
|
||||
var cacheKey = CreateFlyshotCacheKey(robot, uploaded, options, settings);
|
||||
if (options.UseCache && _flyshotCache.TryGetValue(cacheKey, out var cachedBundle))
|
||||
{
|
||||
// 命中缓存时只替换 TrajectoryResult 的 usedCache 标志,规划轨迹和触发时间轴保持不可变复用。
|
||||
@@ -91,11 +95,12 @@ public sealed class ControllerClientTrajectoryOrchestrator
|
||||
saveTrajectoryArtifacts: options.SaveTrajectory,
|
||||
useCache: options.UseCache);
|
||||
|
||||
var plannedTrajectory = PlanByMethod(request, method);
|
||||
var plannedTrajectory = PlanByMethod(request, method, settings);
|
||||
var shotTimeline = _shotTimelineBuilder.Build(
|
||||
plannedTrajectory,
|
||||
holdCycles: 0,
|
||||
samplePeriod: robot.ServoPeriod);
|
||||
holdCycles: settings.IoKeepCycles,
|
||||
samplePeriod: robot.ServoPeriod,
|
||||
useDo: settings.UseDo);
|
||||
var result = CreateResult(plannedTrajectory, shotTimeline, usedCache: false);
|
||||
var bundle = new PlannedExecutionBundle(plannedTrajectory, shotTimeline, result);
|
||||
|
||||
@@ -146,12 +151,12 @@ public sealed class ControllerClientTrajectoryOrchestrator
|
||||
/// <param name="request">规划请求。</param>
|
||||
/// <param name="method">规划方法。</param>
|
||||
/// <returns>规划轨迹。</returns>
|
||||
private PlannedTrajectory PlanByMethod(TrajectoryRequest request, PlanningMethod method)
|
||||
private PlannedTrajectory PlanByMethod(TrajectoryRequest request, PlanningMethod method, CompatibilityRobotSettings? settings = null)
|
||||
{
|
||||
return method switch
|
||||
{
|
||||
PlanningMethod.Icsp => _icspPlanner.Plan(request),
|
||||
PlanningMethod.SelfAdaptIcsp => _selfAdaptIcspPlanner.Plan(request),
|
||||
PlanningMethod.SelfAdaptIcsp => _selfAdaptIcspPlanner.Plan(request, settings?.AdaptIcspTryNum ?? 5),
|
||||
PlanningMethod.Doubles => throw new NotSupportedException("doubles 轨迹规划尚未落地。"),
|
||||
_ => throw new ArgumentOutOfRangeException(nameof(method), method, "未知轨迹规划方法。")
|
||||
};
|
||||
@@ -182,7 +187,8 @@ public sealed class ControllerClientTrajectoryOrchestrator
|
||||
private static string CreateFlyshotCacheKey(
|
||||
RobotProfile robot,
|
||||
ControllerClientCompatUploadedTrajectory uploaded,
|
||||
FlyshotExecutionOptions options)
|
||||
FlyshotExecutionOptions options,
|
||||
CompatibilityRobotSettings settings)
|
||||
{
|
||||
var hash = new HashCode();
|
||||
hash.Add(robot.Name, StringComparer.Ordinal);
|
||||
@@ -190,6 +196,9 @@ public sealed class ControllerClientTrajectoryOrchestrator
|
||||
hash.Add(NormalizeMethod(options.Method), StringComparer.Ordinal);
|
||||
hash.Add(options.MoveToStart);
|
||||
hash.Add(options.SaveTrajectory);
|
||||
hash.Add(settings.UseDo);
|
||||
hash.Add(settings.IoKeepCycles);
|
||||
hash.Add(settings.AdaptIcspTryNum);
|
||||
|
||||
foreach (var waypoint in uploaded.Waypoints)
|
||||
{
|
||||
@@ -220,6 +229,21 @@ public sealed class ControllerClientTrajectoryOrchestrator
|
||||
return hash.ToHashCode().ToString("X8");
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 构造编排器直接调用时的默认兼容配置,保持既有单元测试中的 DO 生成行为。
|
||||
/// </summary>
|
||||
/// <returns>默认机器人兼容配置。</returns>
|
||||
private static CompatibilityRobotSettings CreateDefaultRobotSettings()
|
||||
{
|
||||
return new CompatibilityRobotSettings(
|
||||
useDo: true,
|
||||
ioAddresses: Array.Empty<int>(),
|
||||
ioKeepCycles: 0,
|
||||
accLimitScale: 1.0,
|
||||
jerkLimitScale: 1.0,
|
||||
adaptIcspTryNum: 5);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 把兼容层输入数组转换成领域层 FlyshotProgram。
|
||||
/// </summary>
|
||||
@@ -252,6 +276,10 @@ public sealed class ControllerClientTrajectoryOrchestrator
|
||||
/// <returns>运行时执行结果描述。</returns>
|
||||
private static TrajectoryResult CreateResult(PlannedTrajectory plannedTrajectory, ShotTimeline shotTimeline, bool usedCache)
|
||||
{
|
||||
var denseJointTrajectory = TrajectorySampler.SampleJointTrajectory(
|
||||
plannedTrajectory,
|
||||
samplePeriod: plannedTrajectory.Robot.ServoPeriod.TotalSeconds);
|
||||
|
||||
return new TrajectoryResult(
|
||||
programName: plannedTrajectory.OriginalProgram.Name,
|
||||
method: plannedTrajectory.Method,
|
||||
@@ -263,6 +291,7 @@ public sealed class ControllerClientTrajectoryOrchestrator
|
||||
failureReason: null,
|
||||
usedCache: usedCache,
|
||||
originalWaypointCount: plannedTrajectory.OriginalWaypointCount,
|
||||
plannedWaypointCount: plannedTrajectory.PlannedWaypointCount);
|
||||
plannedWaypointCount: plannedTrajectory.PlannedWaypointCount,
|
||||
denseJointTrajectory: denseJointTrajectory);
|
||||
}
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user