✨ feat(runtime): 添加轨迹持久化与密集执行链路
* 新增飞拍轨迹文件存储，支持上传、加载与删除 * 接通 ControllerClientCompat 到运行时的轨迹编排 * 完善 FANUC 命令与 J519 客户端发送链路 * 补充密集轨迹执行、运行时编排和协议客户端测试 * 更新 README 与 AGENTS 中的当前实现状态
This commit is contained in:
231
src/Flyshot.ControllerClientCompat/FlyshotTrajectoryStore.cs
Normal file
231
src/Flyshot.ControllerClientCompat/FlyshotTrajectoryStore.cs
Normal file
@@ -0,0 +1,231 @@
|
||||
using System.Text.Json;
|
||||
using System.Text.Json.Nodes;
|
||||
using Flyshot.Core.Config;
|
||||
using Flyshot.Core.Domain;
|
||||
|
||||
namespace Flyshot.ControllerClientCompat;
|
||||
|
||||
/// <summary>
|
||||
/// 定义已上传飞拍轨迹的持久化存储契约。
|
||||
/// </summary>
|
||||
public interface IFlyshotTrajectoryStore
|
||||
{
|
||||
/// <summary>
|
||||
/// 将单条轨迹持久化到本地 JSON，同时更新所属机器人配置段。
|
||||
/// </summary>
|
||||
/// <param name="robotName">当前已初始化的机器人名称。</param>
|
||||
/// <param name="settings">当前机器人级兼容配置。</param>
|
||||
/// <param name="trajectory">要保存的已上传轨迹。</param>
|
||||
void Save(string robotName, CompatibilityRobotSettings settings, ControllerClientCompatUploadedTrajectory trajectory);
|
||||
|
||||
/// <summary>
|
||||
/// 从本地 JSON 删除指定名称的轨迹。
|
||||
/// </summary>
|
||||
/// <param name="robotName">当前已初始化的机器人名称。</param>
|
||||
/// <param name="trajectoryName">要删除的轨迹名称。</param>
|
||||
void Delete(string robotName, string trajectoryName);
|
||||
|
||||
/// <summary>
|
||||
/// 加载指定机器人名下所有已持久化的轨迹，并回传保存时的机器人配置。
|
||||
/// </summary>
|
||||
/// <param name="robotName">当前已初始化的机器人名称。</param>
|
||||
/// <param name="settings">输出保存时的机器人配置；若文件不存在或解析失败则为 null。</param>
|
||||
/// <returns>按轨迹名称索引的已上传轨迹集合。</returns>
|
||||
IReadOnlyDictionary<string, ControllerClientCompatUploadedTrajectory> LoadAll(string robotName, out CompatibilityRobotSettings? settings);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 使用与旧版 RobotConfig.json 一致的 JSON 格式持久化飞拍轨迹和机器人配置。
|
||||
/// </summary>
|
||||
public sealed class JsonFlyshotTrajectoryStore : IFlyshotTrajectoryStore
|
||||
{
|
||||
private readonly ControllerClientCompatOptions _options;
|
||||
private readonly RobotConfigLoader _configLoader;
|
||||
|
||||
/// <summary>
|
||||
/// 初始化基于 JSON 文件的轨迹存储。
|
||||
/// </summary>
|
||||
/// <param name="options">兼容层基础配置，用于定位工作区根目录。</param>
|
||||
/// <param name="configLoader">旧版 RobotConfig.json 加载器，用于反序列化已保存的轨迹。</param>
|
||||
public JsonFlyshotTrajectoryStore(ControllerClientCompatOptions options, RobotConfigLoader configLoader)
|
||||
{
|
||||
_options = options ?? throw new ArgumentNullException(nameof(options));
|
||||
_configLoader = configLoader ?? throw new ArgumentNullException(nameof(configLoader));
|
||||
}
|
||||
|
||||
/// <inheritdoc />
|
||||
public void Save(string robotName, CompatibilityRobotSettings settings, ControllerClientCompatUploadedTrajectory trajectory)
|
||||
{
|
||||
ArgumentNullException.ThrowIfNull(settings);
|
||||
ArgumentNullException.ThrowIfNull(trajectory);
|
||||
|
||||
var path = ResolveStorePath(robotName);
|
||||
var directory = Path.GetDirectoryName(path)!;
|
||||
Directory.CreateDirectory(directory);
|
||||
|
||||
JsonObject root;
|
||||
if (File.Exists(path))
|
||||
{
|
||||
var existingJson = File.ReadAllText(path);
|
||||
root = JsonNode.Parse(existingJson)?.AsObject() ?? new JsonObject();
|
||||
}
|
||||
else
|
||||
{
|
||||
root = new JsonObject();
|
||||
}
|
||||
|
||||
// 更新 robot 配置段，保持与旧版 RobotConfig.json 字段名一致。
|
||||
root["robot"] = SerializeRobotSettings(settings);
|
||||
|
||||
// 确保 flying_shots 节点存在。
|
||||
if (!root.TryGetPropertyValue("flying_shots", out var flyingShotsNode) || flyingShotsNode is not JsonObject flyingShotsObj)
|
||||
{
|
||||
flyingShotsObj = new JsonObject();
|
||||
root["flying_shots"] = flyingShotsObj;
|
||||
}
|
||||
|
||||
flyingShotsObj[trajectory.Name] = SerializeTrajectory(trajectory);
|
||||
|
||||
var writeOptions = new JsonSerializerOptions
|
||||
{
|
||||
WriteIndented = true,
|
||||
PropertyNamingPolicy = JsonNamingPolicy.SnakeCaseLower
|
||||
};
|
||||
File.WriteAllText(path, root.ToJsonString(writeOptions));
|
||||
}
|
||||
|
||||
/// <inheritdoc />
|
||||
public void Delete(string robotName, string trajectoryName)
|
||||
{
|
||||
if (string.IsNullOrWhiteSpace(trajectoryName))
|
||||
{
|
||||
throw new ArgumentException("轨迹名称不能为空。", nameof(trajectoryName));
|
||||
}
|
||||
|
||||
var path = ResolveStorePath(robotName);
|
||||
if (!File.Exists(path))
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
var existingJson = File.ReadAllText(path);
|
||||
var root = JsonNode.Parse(existingJson)?.AsObject();
|
||||
if (root is null)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
if (root.TryGetPropertyValue("flying_shots", out var flyingShotsNode) && flyingShotsNode is JsonObject flyingShotsObj)
|
||||
{
|
||||
flyingShotsObj.Remove(trajectoryName);
|
||||
|
||||
var writeOptions = new JsonSerializerOptions
|
||||
{
|
||||
WriteIndented = true,
|
||||
PropertyNamingPolicy = JsonNamingPolicy.SnakeCaseLower
|
||||
};
|
||||
File.WriteAllText(path, root.ToJsonString(writeOptions));
|
||||
}
|
||||
}
|
||||
|
||||
/// <inheritdoc />
|
||||
public IReadOnlyDictionary<string, ControllerClientCompatUploadedTrajectory> LoadAll(string robotName, out CompatibilityRobotSettings? settings)
|
||||
{
|
||||
var path = ResolveStorePath(robotName);
|
||||
if (!File.Exists(path))
|
||||
{
|
||||
settings = null;
|
||||
return new Dictionary<string, ControllerClientCompatUploadedTrajectory>(StringComparer.Ordinal);
|
||||
}
|
||||
|
||||
try
|
||||
{
|
||||
var workspaceRoot = ResolveWorkspaceRoot();
|
||||
var loaded = _configLoader.Load(path, workspaceRoot);
|
||||
settings = loaded.Robot;
|
||||
|
||||
var dict = new Dictionary<string, ControllerClientCompatUploadedTrajectory>(StringComparer.Ordinal);
|
||||
foreach (var program in loaded.Programs)
|
||||
{
|
||||
var traj = new ControllerClientCompatUploadedTrajectory(
|
||||
name: program.Value.Name,
|
||||
waypoints: program.Value.Waypoints.Select(static wp => wp.Positions),
|
||||
shotFlags: program.Value.ShotFlags,
|
||||
offsetValues: program.Value.OffsetValues,
|
||||
addressGroups: program.Value.AddressGroups.Select(static g => g.Addresses));
|
||||
dict[program.Key] = traj;
|
||||
}
|
||||
|
||||
return dict;
|
||||
}
|
||||
catch
|
||||
{
|
||||
settings = null;
|
||||
return new Dictionary<string, ControllerClientCompatUploadedTrajectory>(StringComparer.Ordinal);
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 把机器人兼容配置序列化为 JSON 对象，字段名与旧版 RobotConfig.json 一致。
|
||||
/// </summary>
|
||||
private static JsonObject SerializeRobotSettings(CompatibilityRobotSettings settings)
|
||||
{
|
||||
return new JsonObject
|
||||
{
|
||||
["use_do"] = JsonValue.Create(settings.UseDo),
|
||||
["io_addr"] = JsonSerializer.SerializeToNode(settings.IoAddresses),
|
||||
["io_keep_cycles"] = JsonValue.Create(settings.IoKeepCycles),
|
||||
["acc_limit"] = JsonValue.Create(settings.AccLimitScale),
|
||||
["jerk_limit"] = JsonValue.Create(settings.JerkLimitScale),
|
||||
["adapt_icsp_try_num"] = JsonValue.Create(settings.AdaptIcspTryNum)
|
||||
};
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 把已上传轨迹序列化为 JSON 对象，字段名与旧版 RobotConfig.json 的 flying_shots 节点一致。
|
||||
/// </summary>
|
||||
private static JsonObject SerializeTrajectory(ControllerClientCompatUploadedTrajectory trajectory)
|
||||
{
|
||||
return new JsonObject
|
||||
{
|
||||
["traj_waypoints"] = JsonSerializer.SerializeToNode(trajectory.Waypoints),
|
||||
["shot_flags"] = JsonSerializer.SerializeToNode(trajectory.ShotFlags),
|
||||
["offset_values"] = JsonSerializer.SerializeToNode(trajectory.OffsetValues),
|
||||
["addr"] = JsonSerializer.SerializeToNode(trajectory.AddressGroups)
|
||||
};
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 解析当前机器人对应的持久化文件路径。
|
||||
/// </summary>
|
||||
private string ResolveStorePath(string robotName)
|
||||
{
|
||||
var workspaceRoot = ResolveWorkspaceRoot();
|
||||
var storeDir = Path.Combine(workspaceRoot, "flyshot-replacement", "TrajectoryStore");
|
||||
return Path.Combine(storeDir, $"{robotName}_trajectories.json");
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 解析父工作区根目录，优先使用显式配置。
|
||||
/// </summary>
|
||||
private string ResolveWorkspaceRoot()
|
||||
{
|
||||
if (!string.IsNullOrWhiteSpace(_options.WorkspaceRoot))
|
||||
{
|
||||
return Path.GetFullPath(_options.WorkspaceRoot);
|
||||
}
|
||||
|
||||
var current = new DirectoryInfo(AppContext.BaseDirectory);
|
||||
while (current is not null)
|
||||
{
|
||||
if (File.Exists(Path.Combine(current.FullName, "FlyshotReplacement.sln")))
|
||||
{
|
||||
return Path.GetFullPath(Path.Combine(current.FullName, ".."));
|
||||
}
|
||||
|
||||
current = current.Parent;
|
||||
}
|
||||
|
||||
throw new DirectoryNotFoundException("Unable to locate the flyshot workspace root.");
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user