yunxiao.zhu
d82128da4a
* 在 RobotConfig.json 中新增 speed_ratio 配置，允许在运行时设置默认速度倍率。 * 调整 ControllerClientCompatService 以使用 speed_ratio 初始化机器人设置。 * 更新 TrajectoryLimitValidator 和 FlyshotExecutionSendSequenceBuilder，支持在飞拍链路中临时关闭 jerk 校验，仅保留速度和加速度约束。 * 新增文档记录对 UTTC_MS11 的 jerk 阻断策略调整，确保飞拍链路的执行效率。 * 增加单元测试以验证 speed_ratio 的加载和 jerk 校验的行为。
228 lines
9.5 KiB
C#
228 lines
9.5 KiB
C#
using System.Text.Json;
|
||
using System.Text.Json.Nodes;
|
||
using Flyshot.Core.Config;
|
||
using Flyshot.Core.Domain;
|
||
using Microsoft.Extensions.Logging;
|
||
|
||
namespace Flyshot.ControllerClientCompat;
|
||
|
||
/// <summary>
|
||
/// 使用运行目录 Config/RobotConfig.json 持久化单机器人飞拍轨迹和机器人配置。
|
||
/// </summary>
|
||
public sealed class JsonFlyshotTrajectoryStore
|
||
{
|
||
private readonly ControllerClientCompatOptions _options;
|
||
private readonly RobotConfigLoader _configLoader;
|
||
private readonly ILogger<JsonFlyshotTrajectoryStore>? _logger;
|
||
|
||
/// <summary>
|
||
/// 初始化基于 JSON 文件的轨迹存储。
|
||
/// </summary>
|
||
/// <param name="options">兼容层基础配置，用于定位运行配置根目录。</param>
|
||
/// <param name="configLoader">旧版 RobotConfig.json 加载器，用于反序列化已保存的轨迹。</param>
|
||
/// <param name="logger">日志记录器；允许 null。</param>
|
||
public JsonFlyshotTrajectoryStore(ControllerClientCompatOptions options, RobotConfigLoader configLoader, ILogger<JsonFlyshotTrajectoryStore>? logger = null)
|
||
{
|
||
_options = options ?? throw new ArgumentNullException(nameof(options));
|
||
_configLoader = configLoader ?? throw new ArgumentNullException(nameof(configLoader));
|
||
_logger = logger;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 将单条轨迹持久化到统一 RobotConfig.json，同时更新机器人配置段。
|
||
/// </summary>
|
||
/// <param name="robotName">当前已初始化的机器人名称，仅用于日志诊断。</param>
|
||
/// <param name="settings">当前机器人级兼容配置。</param>
|
||
/// <param name="trajectory">要保存的已上传轨迹。</param>
|
||
public void Save(string robotName, CompatibilityRobotSettings settings, ControllerClientCompatUploadedTrajectory trajectory)
|
||
{
|
||
ArgumentNullException.ThrowIfNull(settings);
|
||
ArgumentNullException.ThrowIfNull(trajectory);
|
||
|
||
_logger?.LogInformation(
|
||
"RobotConfig 保存轨迹: robot={RobotName}, name={TrajectoryName}, waypoints={WaypointCount}",
|
||
robotName,
|
||
trajectory.Name,
|
||
trajectory.Waypoints.Count);
|
||
|
||
var path = ResolveStorePath();
|
||
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));
|
||
|
||
_logger?.LogInformation("RobotConfig 轨迹已保存到 {Path}", path);
|
||
}
|
||
|
||
/// <summary>
|
||
/// 从统一 RobotConfig.json 删除指定名称的轨迹。
|
||
/// </summary>
|
||
/// <param name="robotName">当前已初始化的机器人名称，仅用于日志诊断。</param>
|
||
/// <param name="trajectoryName">要删除的轨迹名称。</param>
|
||
public void Delete(string robotName, string trajectoryName)
|
||
{
|
||
if (string.IsNullOrWhiteSpace(trajectoryName))
|
||
{
|
||
throw new ArgumentException("轨迹名称不能为空。", nameof(trajectoryName));
|
||
}
|
||
|
||
_logger?.LogInformation("RobotConfig 删除轨迹: robot={RobotName}, name={TrajectoryName}", robotName, trajectoryName);
|
||
|
||
var path = ResolveStorePath();
|
||
if (!File.Exists(path))
|
||
{
|
||
_logger?.LogWarning("RobotConfig 删除失败: 文件不存在 {Path}", path);
|
||
return;
|
||
}
|
||
|
||
var existingJson = File.ReadAllText(path);
|
||
var root = JsonNode.Parse(existingJson)?.AsObject();
|
||
if (root is null)
|
||
{
|
||
_logger?.LogWarning("RobotConfig 删除失败: 无法解析 JSON {Path}", path);
|
||
return;
|
||
}
|
||
|
||
if (root.TryGetPropertyValue("flying_shots", out var flyingShotsNode) && flyingShotsNode is JsonObject flyingShotsObj)
|
||
{
|
||
var removed = flyingShotsObj.Remove(trajectoryName);
|
||
if (removed)
|
||
{
|
||
var writeOptions = new JsonSerializerOptions
|
||
{
|
||
WriteIndented = true,
|
||
PropertyNamingPolicy = JsonNamingPolicy.SnakeCaseLower
|
||
};
|
||
File.WriteAllText(path, root.ToJsonString(writeOptions));
|
||
_logger?.LogInformation("RobotConfig 轨迹已删除: {TrajectoryName}", trajectoryName);
|
||
}
|
||
else
|
||
{
|
||
_logger?.LogWarning("RobotConfig 删除失败: 轨迹不存在 {TrajectoryName}", trajectoryName);
|
||
}
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 加载统一 RobotConfig.json 中的所有轨迹，并回传机器人配置。
|
||
/// </summary>
|
||
/// <param name="robotName">当前已初始化的机器人名称，仅用于日志诊断。</param>
|
||
/// <param name="settings">输出 RobotConfig.json 中的机器人配置；若文件不存在或解析失败则为 null。</param>
|
||
/// <returns>按轨迹名称索引的已上传轨迹集合。</returns>
|
||
public IReadOnlyDictionary<string, ControllerClientCompatUploadedTrajectory> LoadAll(string robotName, out CompatibilityRobotSettings? settings)
|
||
{
|
||
var path = ResolveStorePath();
|
||
if (!File.Exists(path))
|
||
{
|
||
_logger?.LogInformation("RobotConfig 无持久化数据: {Path}", path);
|
||
settings = null;
|
||
return new Dictionary<string, ControllerClientCompatUploadedTrajectory>(StringComparer.Ordinal);
|
||
}
|
||
|
||
try
|
||
{
|
||
_logger?.LogInformation("RobotConfig 正在加载: {Path}", path);
|
||
|
||
var loaded = _configLoader.Load(path, _options.ResolveConfigRoot());
|
||
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;
|
||
}
|
||
|
||
_logger?.LogInformation(
|
||
"RobotConfig 加载完成: robot={RobotName}, 轨迹数={Count}, useDo={UseDo}, ioKeepCycles={IoKeepCycles}",
|
||
robotName,
|
||
dict.Count,
|
||
settings?.UseDo,
|
||
settings?.IoKeepCycles);
|
||
|
||
return dict;
|
||
}
|
||
catch (Exception ex)
|
||
{
|
||
_logger?.LogError(ex, "RobotConfig 加载失败: {Path}", path);
|
||
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),
|
||
["trigger_sample_index_offset_cycles"] = JsonValue.Create(settings.TriggerSampleIndexOffsetCycles),
|
||
["acc_limit"] = JsonValue.Create(settings.AccLimitScale),
|
||
["jerk_limit"] = JsonValue.Create(settings.JerkLimitScale),
|
||
["adapt_icsp_try_num"] = JsonValue.Create(settings.AdaptIcspTryNum),
|
||
["planning_speed_scale"] = JsonValue.Create(settings.PlanningSpeedScale),
|
||
["speed_ratio"] = JsonValue.Create(settings.SpeedRatio),
|
||
["smooth_start_stop_timing"] = JsonValue.Create(settings.SmoothStartStopTiming)
|
||
};
|
||
}
|
||
|
||
/// <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()
|
||
{
|
||
return Path.Combine(_options.ResolveConfigRoot(), "RobotConfig.json");
|
||
}
|
||
}
|