✨ feat(fanuc): 添加协议编解码与状态页" -m "* 固化 10010 状态帧、10012 命令帧和 60015 J519 包编解码
* 扩展 ControllerClient 兼容层的执行参数和运行时编排 * 新增 /status 页面与 /api/status/snapshot 状态快照接口 * 补充 FANUC 协议、客户端和状态接口的最小验证测试 * 更新 README、兼容要求和真机 Socket 通信实现计划
This commit is contained in:
@@ -12,6 +12,7 @@ public sealed class ControllerClientTrajectoryOrchestrator
|
||||
private readonly ICspPlanner _icspPlanner = new();
|
||||
private readonly SelfAdaptIcspPlanner _selfAdaptIcspPlanner = new();
|
||||
private readonly ShotTimelineBuilder _shotTimelineBuilder = new(new WaypointTimestampResolver());
|
||||
private readonly Dictionary<string, PlannedExecutionBundle> _flyshotCache = new(StringComparer.Ordinal);
|
||||
|
||||
/// <summary>
|
||||
/// 对普通轨迹执行 ICSP 规划。
|
||||
@@ -21,10 +22,12 @@ public sealed class ControllerClientTrajectoryOrchestrator
|
||||
/// <returns>包含规划轨迹、空触发时间轴和执行结果的结果包。</returns>
|
||||
public PlannedExecutionBundle PlanOrdinaryTrajectory(
|
||||
RobotProfile robot,
|
||||
IReadOnlyList<IReadOnlyList<double>> waypoints)
|
||||
IReadOnlyList<IReadOnlyList<double>> waypoints,
|
||||
TrajectoryExecutionOptions? options = null)
|
||||
{
|
||||
ArgumentNullException.ThrowIfNull(robot);
|
||||
ArgumentNullException.ThrowIfNull(waypoints);
|
||||
options ??= new TrajectoryExecutionOptions();
|
||||
|
||||
var program = CreateProgram(
|
||||
name: "ordinary-trajectory",
|
||||
@@ -33,14 +36,16 @@ public sealed class ControllerClientTrajectoryOrchestrator
|
||||
offsetValues: Enumerable.Repeat(0, waypoints.Count),
|
||||
addressGroups: Enumerable.Range(0, waypoints.Count).Select(static _ => Array.Empty<int>()));
|
||||
|
||||
var method = ParseOrdinaryMethod(options.Method);
|
||||
var request = new TrajectoryRequest(
|
||||
robot: robot,
|
||||
program: program,
|
||||
method: PlanningMethod.Icsp);
|
||||
method: method,
|
||||
saveTrajectoryArtifacts: options.SaveTrajectory);
|
||||
|
||||
var plannedTrajectory = _icspPlanner.Plan(request);
|
||||
var plannedTrajectory = PlanByMethod(request, method);
|
||||
var shotTimeline = new ShotTimeline(Array.Empty<ShotEvent>(), Array.Empty<TrajectoryDoEvent>());
|
||||
var result = CreateResult(plannedTrajectory, shotTimeline);
|
||||
var result = CreateResult(plannedTrajectory, shotTimeline, usedCache: false);
|
||||
|
||||
return new PlannedExecutionBundle(plannedTrajectory, shotTimeline, result);
|
||||
}
|
||||
@@ -51,10 +56,14 @@ public sealed class ControllerClientTrajectoryOrchestrator
|
||||
/// <param name="robot">当前机器人配置。</param>
|
||||
/// <param name="uploaded">兼容层保存的上传轨迹。</param>
|
||||
/// <returns>包含规划轨迹、触发时间轴和执行结果的结果包。</returns>
|
||||
public PlannedExecutionBundle PlanUploadedFlyshot(RobotProfile robot, ControllerClientCompatUploadedTrajectory uploaded)
|
||||
public PlannedExecutionBundle PlanUploadedFlyshot(
|
||||
RobotProfile robot,
|
||||
ControllerClientCompatUploadedTrajectory uploaded,
|
||||
FlyshotExecutionOptions? options = null)
|
||||
{
|
||||
ArgumentNullException.ThrowIfNull(robot);
|
||||
ArgumentNullException.ThrowIfNull(uploaded);
|
||||
options ??= new FlyshotExecutionOptions();
|
||||
|
||||
var program = CreateProgram(
|
||||
name: uploaded.Name,
|
||||
@@ -63,19 +72,152 @@ public sealed class ControllerClientTrajectoryOrchestrator
|
||||
offsetValues: uploaded.OffsetValues,
|
||||
addressGroups: uploaded.AddressGroups);
|
||||
|
||||
var method = ParseFlyshotMethod(options.Method);
|
||||
var cacheKey = CreateFlyshotCacheKey(robot, uploaded, options);
|
||||
if (options.UseCache && _flyshotCache.TryGetValue(cacheKey, out var cachedBundle))
|
||||
{
|
||||
// 命中缓存时只替换 TrajectoryResult 的 usedCache 标志，规划轨迹和触发时间轴保持不可变复用。
|
||||
return new PlannedExecutionBundle(
|
||||
cachedBundle.PlannedTrajectory,
|
||||
cachedBundle.ShotTimeline,
|
||||
CreateResult(cachedBundle.PlannedTrajectory, cachedBundle.ShotTimeline, usedCache: true));
|
||||
}
|
||||
|
||||
var request = new TrajectoryRequest(
|
||||
robot: robot,
|
||||
program: program,
|
||||
method: PlanningMethod.SelfAdaptIcsp);
|
||||
method: method,
|
||||
moveToStart: options.MoveToStart,
|
||||
saveTrajectoryArtifacts: options.SaveTrajectory,
|
||||
useCache: options.UseCache);
|
||||
|
||||
var plannedTrajectory = _selfAdaptIcspPlanner.Plan(request);
|
||||
var plannedTrajectory = PlanByMethod(request, method);
|
||||
var shotTimeline = _shotTimelineBuilder.Build(
|
||||
plannedTrajectory,
|
||||
holdCycles: 0,
|
||||
samplePeriod: robot.ServoPeriod);
|
||||
var result = CreateResult(plannedTrajectory, shotTimeline);
|
||||
var result = CreateResult(plannedTrajectory, shotTimeline, usedCache: false);
|
||||
var bundle = new PlannedExecutionBundle(plannedTrajectory, shotTimeline, result);
|
||||
|
||||
return new PlannedExecutionBundle(plannedTrajectory, shotTimeline, result);
|
||||
if (options.UseCache)
|
||||
{
|
||||
_flyshotCache[cacheKey] = bundle;
|
||||
}
|
||||
|
||||
return bundle;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 按普通轨迹执行接口约束解析 method 参数。
|
||||
/// </summary>
|
||||
/// <param name="method">旧 SDK 传入的方法名。</param>
|
||||
/// <returns>领域层规划方法。</returns>
|
||||
private static PlanningMethod ParseOrdinaryMethod(string method)
|
||||
{
|
||||
var normalized = NormalizeMethod(method);
|
||||
return normalized switch
|
||||
{
|
||||
"icsp" => PlanningMethod.Icsp,
|
||||
"doubles" => PlanningMethod.Doubles,
|
||||
_ => throw new ArgumentException($"Unsupported ExecuteTrajectory method: {method}", nameof(method))
|
||||
};
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 按飞拍轨迹执行接口约束解析 method 参数。
|
||||
/// </summary>
|
||||
/// <param name="method">旧 SDK 传入的方法名。</param>
|
||||
/// <returns>领域层规划方法。</returns>
|
||||
private static PlanningMethod ParseFlyshotMethod(string method)
|
||||
{
|
||||
var normalized = NormalizeMethod(method);
|
||||
return normalized switch
|
||||
{
|
||||
"icsp" => PlanningMethod.Icsp,
|
||||
"self-adapt-icsp" => PlanningMethod.SelfAdaptIcsp,
|
||||
"doubles" => PlanningMethod.Doubles,
|
||||
_ => throw new ArgumentException($"Unsupported ExecuteFlyShotTraj method: {method}", nameof(method))
|
||||
};
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 按领域枚举分派到当前已经落地的规划器。
|
||||
/// </summary>
|
||||
/// <param name="request">规划请求。</param>
|
||||
/// <param name="method">规划方法。</param>
|
||||
/// <returns>规划轨迹。</returns>
|
||||
private PlannedTrajectory PlanByMethod(TrajectoryRequest request, PlanningMethod method)
|
||||
{
|
||||
return method switch
|
||||
{
|
||||
PlanningMethod.Icsp => _icspPlanner.Plan(request),
|
||||
PlanningMethod.SelfAdaptIcsp => _selfAdaptIcspPlanner.Plan(request),
|
||||
PlanningMethod.Doubles => throw new NotSupportedException("doubles 轨迹规划尚未落地。"),
|
||||
_ => throw new ArgumentOutOfRangeException(nameof(method), method, "未知轨迹规划方法。")
|
||||
};
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 归一化旧 SDK 的 method 字符串。
|
||||
/// </summary>
|
||||
/// <param name="method">原始方法名。</param>
|
||||
/// <returns>小写短横线方法名。</returns>
|
||||
private static string NormalizeMethod(string method)
|
||||
{
|
||||
if (string.IsNullOrWhiteSpace(method))
|
||||
{
|
||||
return "icsp";
|
||||
}
|
||||
|
||||
return method.Trim().ToLowerInvariant();
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 为已上传飞拍轨迹构造包含参数和轨迹内容的缓存键，避免同名覆盖后误用旧规划结果。
|
||||
/// </summary>
|
||||
/// <param name="robot">机器人配置。</param>
|
||||
/// <param name="uploaded">上传轨迹。</param>
|
||||
/// <param name="options">执行参数。</param>
|
||||
/// <returns>缓存键。</returns>
|
||||
private static string CreateFlyshotCacheKey(
|
||||
RobotProfile robot,
|
||||
ControllerClientCompatUploadedTrajectory uploaded,
|
||||
FlyshotExecutionOptions options)
|
||||
{
|
||||
var hash = new HashCode();
|
||||
hash.Add(robot.Name, StringComparer.Ordinal);
|
||||
hash.Add(uploaded.Name, StringComparer.Ordinal);
|
||||
hash.Add(NormalizeMethod(options.Method), StringComparer.Ordinal);
|
||||
hash.Add(options.MoveToStart);
|
||||
hash.Add(options.SaveTrajectory);
|
||||
|
||||
foreach (var waypoint in uploaded.Waypoints)
|
||||
{
|
||||
foreach (var value in waypoint)
|
||||
{
|
||||
hash.Add(value);
|
||||
}
|
||||
}
|
||||
|
||||
foreach (var flag in uploaded.ShotFlags)
|
||||
{
|
||||
hash.Add(flag);
|
||||
}
|
||||
|
||||
foreach (var offset in uploaded.OffsetValues)
|
||||
{
|
||||
hash.Add(offset);
|
||||
}
|
||||
|
||||
foreach (var group in uploaded.AddressGroups)
|
||||
{
|
||||
foreach (var address in group)
|
||||
{
|
||||
hash.Add(address);
|
||||
}
|
||||
}
|
||||
|
||||
return hash.ToHashCode().ToString("X8");
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
@@ -108,7 +250,7 @@ public sealed class ControllerClientTrajectoryOrchestrator
|
||||
/// <param name="plannedTrajectory">规划后的轨迹。</param>
|
||||
/// <param name="shotTimeline">触发时间轴。</param>
|
||||
/// <returns>运行时执行结果描述。</returns>
|
||||
private static TrajectoryResult CreateResult(PlannedTrajectory plannedTrajectory, ShotTimeline shotTimeline)
|
||||
private static TrajectoryResult CreateResult(PlannedTrajectory plannedTrajectory, ShotTimeline shotTimeline, bool usedCache)
|
||||
{
|
||||
return new TrajectoryResult(
|
||||
programName: plannedTrajectory.OriginalProgram.Name,
|
||||
@@ -119,7 +261,7 @@ public sealed class ControllerClientTrajectoryOrchestrator
|
||||
triggerTimeline: shotTimeline.TriggerTimeline,
|
||||
artifacts: Array.Empty<TrajectoryArtifact>(),
|
||||
failureReason: null,
|
||||
usedCache: false,
|
||||
usedCache: usedCache,
|
||||
originalWaypointCount: plannedTrajectory.OriginalWaypointCount,
|
||||
plannedWaypointCount: plannedTrajectory.PlannedWaypointCount);
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user