yunxiao.zhu
c6829d214a
* 新增 J519 实发采样器，按 8ms 周期生成 timing/jerk 诊断行并完成 rad->deg 转换 * 兼容层产物导出补充 speedRatio，规划编排补齐 smoothStartStopTiming 与日志透传 * 配置与机型加载切换到运行目录 JSON 模型，并补齐 7L 展开模型与相关单元测试
839 lines
30 KiB
C#
839 lines
30 KiB
C#
using Flyshot.Core.Config;
|
||
using Flyshot.Core.Domain;
|
||
using Flyshot.Runtime.Common;
|
||
using Microsoft.Extensions.Logging;
|
||
|
||
namespace Flyshot.ControllerClientCompat;
|
||
|
||
/// <summary>
|
||
/// 在宿主进程内实现 HTTP-only ControllerClient 兼容语义，并把控制器状态委托给运行时。
|
||
/// </summary>
|
||
public sealed class ControllerClientCompatService : IControllerClientCompatService
|
||
{
|
||
private readonly object _stateLock = new();
|
||
private readonly Dictionary<string, ControllerClientCompatUploadedTrajectory> _uploadedTrajectories = new(StringComparer.Ordinal);
|
||
private readonly ControllerClientCompatOptions _options;
|
||
private readonly ControllerClientCompatRobotCatalog _robotCatalog;
|
||
private readonly IControllerRuntime _runtime;
|
||
private readonly ControllerClientTrajectoryOrchestrator _trajectoryOrchestrator;
|
||
private readonly RobotConfigLoader _configLoader;
|
||
private readonly FlyshotTrajectoryArtifactWriter _artifactWriter;
|
||
private readonly JsonFlyshotTrajectoryStore _trajectoryStore;
|
||
private readonly ILogger<ControllerClientCompatService>? _logger;
|
||
private RobotProfile? _activeRobotProfile;
|
||
private string? _configuredRobotName;
|
||
private CompatibilityRobotSettings? _robotSettings;
|
||
private string? _connectedServerIp;
|
||
private int _connectedServerPort;
|
||
private bool _showTcp = true;
|
||
private double _showTcpAxisLength = 0.1;
|
||
private int _showTcpAxisSize = 2;
|
||
|
||
/// <summary>
|
||
/// 初始化一份 HTTP-only 的 ControllerClient 兼容服务。
|
||
/// </summary>
|
||
/// <param name="options">兼容层基础配置。</param>
|
||
/// <param name="robotCatalog">机器人模型目录。</param>
|
||
/// <param name="runtime">控制器运行时。</param>
|
||
/// <param name="trajectoryOrchestrator">轨迹规划与触发编排器。</param>
|
||
/// <param name="configLoader">旧版 RobotConfig.json 加载器。</param>
|
||
/// <param name="artifactWriter">saveTrajectory 规划结果点位导出器。</param>
|
||
/// <param name="trajectoryStore">统一 RobotConfig.json 持久化存储；为空时按配置根目录创建默认实例。</param>
|
||
/// <param name="logger">日志记录器；允许测试直接构造时传入 null。</param>
|
||
public ControllerClientCompatService(
|
||
ControllerClientCompatOptions options,
|
||
ControllerClientCompatRobotCatalog robotCatalog,
|
||
IControllerRuntime runtime,
|
||
ControllerClientTrajectoryOrchestrator trajectoryOrchestrator,
|
||
RobotConfigLoader configLoader,
|
||
FlyshotTrajectoryArtifactWriter? artifactWriter = null,
|
||
JsonFlyshotTrajectoryStore? trajectoryStore = null,
|
||
ILogger<ControllerClientCompatService>? logger = null)
|
||
{
|
||
_options = options ?? throw new ArgumentNullException(nameof(options));
|
||
_robotCatalog = robotCatalog ?? throw new ArgumentNullException(nameof(robotCatalog));
|
||
_runtime = runtime ?? throw new ArgumentNullException(nameof(runtime));
|
||
_trajectoryOrchestrator = trajectoryOrchestrator ?? throw new ArgumentNullException(nameof(trajectoryOrchestrator));
|
||
_configLoader = configLoader ?? throw new ArgumentNullException(nameof(configLoader));
|
||
_artifactWriter = artifactWriter ?? new FlyshotTrajectoryArtifactWriter(_options, new RobotModelLoader());
|
||
_trajectoryStore = trajectoryStore ?? new JsonFlyshotTrajectoryStore(_options, _configLoader);
|
||
_logger = logger;
|
||
}
|
||
|
||
/// <inheritdoc />
|
||
public string ServerVersion => _options.ServerVersion;
|
||
|
||
/// <inheritdoc />
|
||
public bool IsSetUp
|
||
{
|
||
get
|
||
{
|
||
lock (_stateLock)
|
||
{
|
||
return _activeRobotProfile is not null;
|
||
}
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 获取当前运行时是否处于运动态。
|
||
/// </summary>
|
||
public bool IsInMotion => _runtime.GetSnapshot().IsInMotion;
|
||
|
||
/// <inheritdoc />
|
||
public void ConnectServer(string serverIp, int port)
|
||
{
|
||
if (string.IsNullOrWhiteSpace(serverIp))
|
||
{
|
||
throw new ArgumentException("服务端 IP 不能为空。", nameof(serverIp));
|
||
}
|
||
|
||
if (port <= 0)
|
||
{
|
||
throw new ArgumentOutOfRangeException(nameof(port), "端口必须大于 0。");
|
||
}
|
||
|
||
lock (_stateLock)
|
||
{
|
||
// HTTP-only 阶段仍记录旧客户端期望的 50001 地址，便于后续 TCP 入口恢复时复用状态。
|
||
_connectedServerIp = serverIp;
|
||
_connectedServerPort = port;
|
||
}
|
||
|
||
_logger?.LogInformation("ConnectServer 完成: {ServerIp}:{Port}", serverIp, port);
|
||
}
|
||
|
||
/// <inheritdoc />
|
||
public string GetServerVersion()
|
||
{
|
||
return ServerVersion;
|
||
}
|
||
|
||
/// <inheritdoc />
|
||
public string GetClientVersion()
|
||
{
|
||
return "flyshot-replacement-controller-client-compat/0.1.0";
|
||
}
|
||
|
||
/// <inheritdoc />
|
||
public void SetUpRobot(string robotName)
|
||
{
|
||
_logger?.LogInformation("SetUpRobot 开始: robotName={RobotName}", robotName);
|
||
|
||
var robotSettings = TryLoadRobotSettings() ?? CreateDefaultRobotSettings();
|
||
var robotProfile = _robotCatalog.LoadProfile(
|
||
robotName,
|
||
robotSettings.AccLimitScale,
|
||
robotSettings.JerkLimitScale);
|
||
|
||
lock (_stateLock)
|
||
{
|
||
// 机器人重新初始化时同步重置运行时和上传轨迹目录，保持旧服务初始化语义。
|
||
_configuredRobotName = robotName;
|
||
_activeRobotProfile = robotProfile;
|
||
_uploadedTrajectories.Clear();
|
||
_runtime.ResetRobot(robotProfile, robotName);
|
||
_robotSettings = robotSettings;
|
||
|
||
// 从持久化存储恢复该机器人名下之前已上传的轨迹。
|
||
var savedTrajectories = _trajectoryStore.LoadAll(robotName, out _);
|
||
foreach (var saved in savedTrajectories)
|
||
{
|
||
_uploadedTrajectories[saved.Key] = saved.Value;
|
||
}
|
||
}
|
||
|
||
_logger?.LogInformation(
|
||
"SetUpRobot 完成: robotName={RobotName}, dof={Dof}, accLimit={AccLimit}, jerkLimit={JerkLimit}, 恢复轨迹数={TrajCount}",
|
||
robotName,
|
||
robotProfile.DegreesOfFreedom,
|
||
robotSettings.AccLimitScale,
|
||
robotSettings.JerkLimitScale,
|
||
_uploadedTrajectories.Count);
|
||
}
|
||
|
||
/// <inheritdoc />
|
||
public void SetUpRobotFromEnv(string envFile)
|
||
{
|
||
if (string.IsNullOrWhiteSpace(envFile))
|
||
{
|
||
throw new ArgumentException("环境文件路径不能为空。", nameof(envFile));
|
||
}
|
||
|
||
throw new NotSupportedException("SetUpRobotFromEnv 尚未接入环境文件解析。");
|
||
}
|
||
|
||
/// <inheritdoc />
|
||
public void SetShowTcp(bool isShow, double axisLength, int axisSize)
|
||
{
|
||
if (axisLength <= 0.0)
|
||
{
|
||
throw new ArgumentOutOfRangeException(nameof(axisLength), "TCP 坐标轴长度必须大于 0。");
|
||
}
|
||
|
||
if (axisSize <= 0)
|
||
{
|
||
throw new ArgumentOutOfRangeException(nameof(axisSize), "TCP 坐标轴线宽必须大于 0。");
|
||
}
|
||
|
||
lock (_stateLock)
|
||
{
|
||
EnsureRobotSetup();
|
||
|
||
// 当前无 GUI 渲染层，先保存显示参数，保证旧 SDK 参数不会在 HTTP 边界丢失。
|
||
_showTcp = isShow;
|
||
_showTcpAxisLength = axisLength;
|
||
_showTcpAxisSize = axisSize;
|
||
}
|
||
}
|
||
|
||
/// <inheritdoc />
|
||
public void SetActiveController(bool sim)
|
||
{
|
||
lock (_stateLock)
|
||
{
|
||
EnsureRobotSetup();
|
||
_runtime.SetActiveController(sim);
|
||
}
|
||
}
|
||
|
||
/// <inheritdoc />
|
||
public void Connect(string robotIp)
|
||
{
|
||
if (string.IsNullOrWhiteSpace(robotIp))
|
||
{
|
||
throw new ArgumentException("控制器 IP 不能为空。", nameof(robotIp));
|
||
}
|
||
|
||
_logger?.LogInformation("Connect 开始: robotIp={RobotIp}", robotIp);
|
||
|
||
lock (_stateLock)
|
||
{
|
||
EnsureRobotSetup();
|
||
_runtime.Connect(robotIp);
|
||
}
|
||
|
||
_logger?.LogInformation("Connect 完成: robotIp={RobotIp}", robotIp);
|
||
}
|
||
|
||
/// <inheritdoc />
|
||
public void Disconnect()
|
||
{
|
||
lock (_stateLock)
|
||
{
|
||
EnsureRobotSetup();
|
||
_runtime.Disconnect();
|
||
}
|
||
}
|
||
|
||
/// <inheritdoc />
|
||
public void EnableRobot(int bufferSize)
|
||
{
|
||
_logger?.LogInformation("EnableRobot 开始: bufferSize={BufferSize}", bufferSize);
|
||
lock (_stateLock)
|
||
{
|
||
EnsureRobotSetup();
|
||
_runtime.EnableRobot(bufferSize);
|
||
}
|
||
_logger?.LogInformation("EnableRobot 完成");
|
||
}
|
||
|
||
/// <inheritdoc />
|
||
public void DisableRobot()
|
||
{
|
||
_logger?.LogInformation("DisableRobot 开始");
|
||
lock (_stateLock)
|
||
{
|
||
EnsureRobotSetup();
|
||
_runtime.DisableRobot();
|
||
}
|
||
_logger?.LogInformation("DisableRobot 完成");
|
||
}
|
||
|
||
/// <inheritdoc />
|
||
public void StopMove()
|
||
{
|
||
_logger?.LogInformation("StopMove 开始");
|
||
lock (_stateLock)
|
||
{
|
||
EnsureRobotSetup();
|
||
_runtime.StopMove();
|
||
}
|
||
_logger?.LogInformation("StopMove 完成");
|
||
}
|
||
|
||
/// <inheritdoc />
|
||
public ControllerStateSnapshot GetControllerSnapshot()
|
||
{
|
||
return _runtime.GetSnapshot();
|
||
}
|
||
|
||
/// <inheritdoc />
|
||
public double GetSpeedRatio()
|
||
{
|
||
lock (_stateLock)
|
||
{
|
||
EnsureRobotSetup();
|
||
return _runtime.GetSpeedRatio();
|
||
}
|
||
}
|
||
|
||
/// <inheritdoc />
|
||
public void SetSpeedRatio(double ratio)
|
||
{
|
||
lock (_stateLock)
|
||
{
|
||
EnsureRobotSetup();
|
||
_runtime.SetSpeedRatio(ratio);
|
||
}
|
||
}
|
||
|
||
/// <inheritdoc />
|
||
public void SetIo(int port, bool value, string ioType)
|
||
{
|
||
lock (_stateLock)
|
||
{
|
||
EnsureRobotSetup();
|
||
_runtime.SetIo(port, value, ioType);
|
||
}
|
||
}
|
||
|
||
/// <inheritdoc />
|
||
public bool GetIo(int port, string ioType)
|
||
{
|
||
lock (_stateLock)
|
||
{
|
||
EnsureRobotSetup();
|
||
return _runtime.GetIo(port, ioType);
|
||
}
|
||
}
|
||
|
||
/// <inheritdoc />
|
||
public IReadOnlyList<double> GetNearestIk(IReadOnlyList<double> pose, IReadOnlyList<double> seed)
|
||
{
|
||
ArgumentNullException.ThrowIfNull(pose);
|
||
ArgumentNullException.ThrowIfNull(seed);
|
||
|
||
lock (_stateLock)
|
||
{
|
||
EnsureRobotSetup();
|
||
if (pose.Count != 7)
|
||
{
|
||
throw new ArgumentException("位姿必须是 [x,y,z,qx,qy,qz,qw] 七元数组。", nameof(pose));
|
||
}
|
||
|
||
if (seed.Count != GetDegreesOfFreedom())
|
||
{
|
||
throw new ArgumentException("seed 关节数量必须与机器人自由度一致。", nameof(seed));
|
||
}
|
||
|
||
throw new NotSupportedException("GetNearestIK 尚未接入逆解求解器。");
|
||
}
|
||
}
|
||
|
||
/// <inheritdoc />
|
||
public void SetTcp(double x, double y, double z)
|
||
{
|
||
lock (_stateLock)
|
||
{
|
||
EnsureRobotSetup();
|
||
_runtime.SetTcp(x, y, z);
|
||
}
|
||
}
|
||
|
||
/// <inheritdoc />
|
||
public IReadOnlyList<double> GetTcp()
|
||
{
|
||
lock (_stateLock)
|
||
{
|
||
EnsureRobotSetup();
|
||
return _runtime.GetTcp();
|
||
}
|
||
}
|
||
|
||
/// <inheritdoc />
|
||
public IReadOnlyList<double> GetJointPositions()
|
||
{
|
||
lock (_stateLock)
|
||
{
|
||
EnsureRobotSetup();
|
||
return _runtime.GetJointPositions();
|
||
}
|
||
}
|
||
|
||
/// <inheritdoc />
|
||
public void MoveJoint(IReadOnlyList<double> jointPositions)
|
||
{
|
||
ArgumentNullException.ThrowIfNull(jointPositions);
|
||
|
||
_logger?.LogInformation("MoveJoint 开始: 目标关节数={JointCount}", jointPositions.Count);
|
||
_logger?.LogDebug("MoveJoint 目标关节: {Joints}", string.Join(", ", jointPositions.Select(j => j.ToString("F4"))));
|
||
|
||
lock (_stateLock)
|
||
{
|
||
var robot = RequireActiveRobot();
|
||
EnsureRuntimeEnabled();
|
||
ExecuteMoveJointAndWaitLocked(robot, jointPositions, "MoveJoint");
|
||
}
|
||
|
||
_logger?.LogInformation("MoveJoint 完成");
|
||
}
|
||
|
||
/// <inheritdoc />
|
||
public void ExecuteTrajectory(IReadOnlyList<IReadOnlyList<double>> waypoints, TrajectoryExecutionOptions? options = null)
|
||
{
|
||
ArgumentNullException.ThrowIfNull(waypoints);
|
||
options ??= new TrajectoryExecutionOptions();
|
||
if (waypoints.Count == 0)
|
||
{
|
||
throw new ArgumentException("轨迹路点不能为空。", nameof(waypoints));
|
||
}
|
||
|
||
_logger?.LogInformation("ExecuteTrajectory 开始: 路点数={WaypointCount}, method={Method}, saveTraj={SaveTraj}",
|
||
waypoints.Count, options.Method, options.SaveTrajectory);
|
||
_logger?.LogDebug("ExecuteTrajectory 路点详情: {Waypoints}",
|
||
string.Join(" | ", waypoints.Select(wp => $"[{string.Join(", ", wp.Select(j => j.ToString("F4")))}]")));
|
||
|
||
lock (_stateLock)
|
||
{
|
||
var robot = RequireActiveRobot();
|
||
EnsureRuntimeEnabled();
|
||
|
||
// 普通轨迹必须按调用方指定 method 规划，再把规划结果交给运行时执行。
|
||
var planningSpeedScale = RequireRobotSettings().PlanningSpeedScale;
|
||
var bundle = _trajectoryOrchestrator.PlanOrdinaryTrajectory(robot, waypoints, options, planningSpeedScale);
|
||
_logger?.LogInformation(
|
||
"ExecuteTrajectory 规划完成: method={Method}, 时长={Duration}s, 有效={IsValid}, 采样点数={SampleCount}, planningSpeedScale={PlanningSpeedScale}",
|
||
bundle.Result.Method,
|
||
bundle.Result.Duration.TotalSeconds,
|
||
bundle.Result.IsValid,
|
||
bundle.Result.DenseJointTrajectory?.Count ?? 0,
|
||
planningSpeedScale);
|
||
var finalJointPositions = bundle.PlannedTrajectory.PlannedWaypoints[^1].Positions;
|
||
_runtime.ExecuteTrajectory(bundle.Result, finalJointPositions);
|
||
}
|
||
|
||
_logger?.LogInformation("ExecuteTrajectory 完成");
|
||
}
|
||
|
||
/// <inheritdoc />
|
||
public IReadOnlyList<double> GetPose()
|
||
{
|
||
lock (_stateLock)
|
||
{
|
||
EnsureRobotSetup();
|
||
return _runtime.GetPose();
|
||
}
|
||
}
|
||
|
||
/// <inheritdoc />
|
||
public void UploadTrajectory(ControllerClientCompatUploadedTrajectory trajectory)
|
||
{
|
||
ArgumentNullException.ThrowIfNull(trajectory);
|
||
|
||
_logger?.LogInformation(
|
||
"UploadTrajectory 开始: name={Name}, waypoints={WaypointCount}, shotFlags={ShotCount}",
|
||
trajectory.Name,
|
||
trajectory.Waypoints.Count,
|
||
trajectory.ShotFlags.Count(static f => f));
|
||
|
||
lock (_stateLock)
|
||
{
|
||
EnsureRuntimeEnabled();
|
||
_uploadedTrajectories[trajectory.Name] = trajectory;
|
||
|
||
var robotName = _configuredRobotName ?? throw new InvalidOperationException("Robot has not been setup.");
|
||
var settings = _robotSettings ?? CreateDefaultRobotSettings();
|
||
_trajectoryStore.Save(robotName, settings, trajectory);
|
||
}
|
||
|
||
_logger?.LogInformation("UploadTrajectory 完成: name={Name}", trajectory.Name);
|
||
}
|
||
|
||
/// <inheritdoc />
|
||
public IReadOnlyList<string> ListTrajectoryNames()
|
||
{
|
||
lock (_stateLock)
|
||
{
|
||
return _uploadedTrajectories.Keys.ToArray();
|
||
}
|
||
}
|
||
|
||
/// <inheritdoc />
|
||
public void ExecuteTrajectoryByName(string name, FlyshotExecutionOptions? options = null)
|
||
{
|
||
options ??= new FlyshotExecutionOptions();
|
||
if (string.IsNullOrWhiteSpace(name))
|
||
{
|
||
throw new ArgumentException("轨迹名称不能为空。", nameof(name));
|
||
}
|
||
|
||
_logger?.LogInformation(
|
||
"ExecuteTrajectoryByName 开始: name={Name}, method={Method}, moveToStart={MoveToStart}, useCache={UseCache}, wait={Wait}",
|
||
name, options.Method, options.MoveToStart, options.UseCache, options.Wait);
|
||
|
||
lock (_stateLock)
|
||
{
|
||
var robot = RequireActiveRobot();
|
||
EnsureRuntimeEnabled();
|
||
|
||
if (!_uploadedTrajectories.TryGetValue(name, out var trajectory))
|
||
{
|
||
_logger?.LogWarning("ExecuteTrajectoryByName 失败: 轨迹不存在 name={Name}", name);
|
||
throw new InvalidOperationException("FlyShot trajectory does not exist.");
|
||
}
|
||
|
||
if (trajectory.Waypoints.Count == 0)
|
||
{
|
||
_logger?.LogWarning("ExecuteTrajectoryByName 失败: 轨迹无路点 name={Name}", name);
|
||
throw new InvalidOperationException("FlyShot trajectory contains no waypoints.");
|
||
}
|
||
|
||
// 已上传飞拍轨迹必须按调用方指定 method 生成 shot timeline 后再交给运行时。
|
||
var settings = RequireRobotSettings();
|
||
var bundle = _trajectoryOrchestrator.PlanUploadedFlyshot(robot, trajectory, options, settings, settings.PlanningSpeedScale);
|
||
ExportFlyshotArtifactsIfRequested(name, options.SaveTrajectory, robot, bundle);
|
||
_logger?.LogInformation(
|
||
"ExecuteTrajectoryByName 规划完成: name={Name}, method={Method}, 时长={Duration}s, 触发事件数={TriggerCount}, 使用缓存={UsedCache}, planningSpeedScale={PlanningSpeedScale}",
|
||
name,
|
||
bundle.Result.Method,
|
||
bundle.Result.Duration.TotalSeconds,
|
||
bundle.Result.TriggerTimeline.Count,
|
||
bundle.Result.UsedCache,
|
||
settings.PlanningSpeedScale);
|
||
|
||
if (options.MoveToStart)
|
||
{
|
||
_logger?.LogInformation("ExecuteTrajectoryByName 先移动到起点");
|
||
ExecuteMoveJointAndWaitLocked(robot, bundle.PlannedTrajectory.PlannedWaypoints[0].Positions, "ExecuteTrajectoryByName.move_to_start");
|
||
}
|
||
|
||
var finalJointPositions = bundle.PlannedTrajectory.PlannedWaypoints[^1].Positions;
|
||
_runtime.ExecuteTrajectory(bundle.Result, finalJointPositions);
|
||
if (options.Wait)
|
||
{
|
||
WaitForRuntimeMotionComplete("ExecuteTrajectoryByName.flyshot", bundle.Result.Duration);
|
||
}
|
||
}
|
||
|
||
_logger?.LogInformation("ExecuteTrajectoryByName 完成: name={Name}", name);
|
||
}
|
||
|
||
/// <summary>
|
||
/// 从当前关节位置生成临时 PTP 稠密轨迹并阻塞等待运行时完成，避免后续 J519 目标发生突变。
|
||
/// </summary>
|
||
/// <param name="robot">当前机器人模型。</param>
|
||
/// <param name="targetJointPositions">目标关节位置，单位为弧度。</param>
|
||
/// <param name="operationName">用于日志和超时异常的操作名。</param>
|
||
private void ExecuteMoveJointAndWaitLocked(RobotProfile robot, IReadOnlyList<double> targetJointPositions, string operationName)
|
||
{
|
||
var currentJointPositions = _runtime.GetJointPositions();
|
||
EnsureJointVector(currentJointPositions, robot.DegreesOfFreedom, nameof(currentJointPositions));
|
||
EnsureJointVector(targetJointPositions, robot.DegreesOfFreedom, nameof(targetJointPositions));
|
||
|
||
var speedRatio = _runtime.GetSnapshot().SpeedRatio;
|
||
var moveResult = MoveJointTrajectoryGenerator.CreateResult(robot, currentJointPositions, targetJointPositions, speedRatio, _logger);
|
||
_logger?.LogInformation(
|
||
"{OperationName} PTP规划完成: 当前速度倍率={SpeedRatio}, 规划时长={Duration}s, 采样点数={SampleCount}",
|
||
operationName,
|
||
speedRatio,
|
||
moveResult.Duration.TotalSeconds,
|
||
moveResult.DenseJointTrajectory?.Count ?? 0);
|
||
|
||
_runtime.ExecuteTrajectory(moveResult, targetJointPositions);
|
||
WaitForRuntimeMotionComplete(operationName, moveResult.Duration);
|
||
}
|
||
|
||
/// <summary>
|
||
/// 等待运行时报告当前运动结束，用于把 move_to_start 与正式飞拍轨迹串行化。
|
||
/// </summary>
|
||
/// <param name="operationName">用于日志和超时异常的操作名。</param>
|
||
/// <param name="plannedDuration">规划运动时长。</param>
|
||
private void WaitForRuntimeMotionComplete(string operationName, TimeSpan plannedDuration)
|
||
{
|
||
var timeout = ResolveMotionCompletionTimeout(plannedDuration);
|
||
var deadline = DateTimeOffset.UtcNow.Add(timeout);
|
||
|
||
while (true)
|
||
{
|
||
if (!_runtime.GetSnapshot().IsInMotion)
|
||
{
|
||
_logger?.LogInformation("{OperationName} 运动完成", operationName);
|
||
return;
|
||
}
|
||
|
||
if (DateTimeOffset.UtcNow >= deadline)
|
||
{
|
||
throw new TimeoutException($"{operationName} 等待运动完成超时，planned={plannedDuration.TotalSeconds:F3}s, timeout={timeout.TotalSeconds:F3}s。");
|
||
}
|
||
|
||
Thread.Sleep(TimeSpan.FromMilliseconds(10));
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 根据规划时长推导等待超时，给真机通信和状态更新留出余量。
|
||
/// </summary>
|
||
/// <param name="plannedDuration">规划运动时长。</param>
|
||
/// <returns>等待运行时完成的最大时长。</returns>
|
||
private static TimeSpan ResolveMotionCompletionTimeout(TimeSpan plannedDuration)
|
||
{
|
||
var timeoutSeconds = Math.Max(5.0, plannedDuration.TotalSeconds * 3.0 + 2.0);
|
||
return TimeSpan.FromSeconds(timeoutSeconds);
|
||
}
|
||
|
||
/// <inheritdoc />
|
||
public void SaveTrajectoryInfo(string name, string method = "icsp")
|
||
{
|
||
if (string.IsNullOrWhiteSpace(name))
|
||
{
|
||
throw new ArgumentException("轨迹名称不能为空。", nameof(name));
|
||
}
|
||
|
||
_logger?.LogInformation("SaveTrajectoryInfo 开始: name={Name}, method={Method}", name, method);
|
||
|
||
lock (_stateLock)
|
||
{
|
||
var robot = RequireActiveRobot();
|
||
if (!_uploadedTrajectories.TryGetValue(name, out var trajectory))
|
||
{
|
||
_logger?.LogWarning("SaveTrajectoryInfo 失败: 轨迹不存在 name={Name}", name);
|
||
throw new InvalidOperationException("FlyShot trajectory does not exist.");
|
||
}
|
||
|
||
// 先通过规划校验避免静默接受非法参数，同时把轨迹信息强制刷写到本地 JSON。
|
||
var planningSettings = RequireRobotSettings();
|
||
var bundle = _trajectoryOrchestrator.PlanUploadedFlyshot(
|
||
robot,
|
||
trajectory,
|
||
new FlyshotExecutionOptions(useCache:false,saveTrajectory: true, method: method),
|
||
planningSettings,
|
||
planningSettings.PlanningSpeedScale);
|
||
ExportFlyshotArtifactsIfRequested(name, saveTrajectory: true, robot, bundle);
|
||
|
||
var robotName = _configuredRobotName ?? throw new InvalidOperationException("Robot has not been setup.");
|
||
var settings = _robotSettings ?? CreateDefaultRobotSettings();
|
||
_trajectoryStore.Save(robotName, settings, trajectory);
|
||
}
|
||
|
||
_logger?.LogInformation("SaveTrajectoryInfo 完成: name={Name}", name);
|
||
}
|
||
|
||
/// <inheritdoc />
|
||
public bool IsFlyshotTrajectoryValid(out TimeSpan duration, string name, string method = "icsp", bool saveTrajectory = false)
|
||
{
|
||
if (string.IsNullOrWhiteSpace(name))
|
||
{
|
||
throw new ArgumentException("轨迹名称不能为空。", nameof(name));
|
||
}
|
||
|
||
_logger?.LogInformation("IsFlyshotTrajectoryValid 开始: name={Name}, method={Method}", name, method);
|
||
|
||
lock (_stateLock)
|
||
{
|
||
var robot = RequireActiveRobot();
|
||
if (!_uploadedTrajectories.TryGetValue(name, out var trajectory))
|
||
{
|
||
_logger?.LogWarning("IsFlyshotTrajectoryValid 失败: 轨迹不存在 name={Name}", name);
|
||
throw new InvalidOperationException("FlyShot trajectory does not exist.");
|
||
}
|
||
|
||
var planningSettings = RequireRobotSettings();
|
||
var bundle = _trajectoryOrchestrator.PlanUploadedFlyshot(
|
||
robot,
|
||
trajectory,
|
||
new FlyshotExecutionOptions(method: method, saveTrajectory: saveTrajectory),
|
||
planningSettings,
|
||
planningSettings.PlanningSpeedScale);
|
||
ExportFlyshotArtifactsIfRequested(name, saveTrajectory, robot, bundle);
|
||
|
||
duration = bundle.Result.Duration;
|
||
_logger?.LogInformation(
|
||
"IsFlyshotTrajectoryValid 结果: name={Name}, valid={Valid}, duration={Duration}s",
|
||
name, bundle.Result.IsValid, duration.TotalSeconds);
|
||
return bundle.Result.IsValid;
|
||
}
|
||
}
|
||
|
||
/// <inheritdoc />
|
||
public void DeleteTrajectory(string name)
|
||
{
|
||
if (string.IsNullOrWhiteSpace(name))
|
||
{
|
||
throw new ArgumentException("轨迹名称不能为空。", nameof(name));
|
||
}
|
||
|
||
_logger?.LogInformation("DeleteTrajectory 开始: name={Name}", name);
|
||
|
||
lock (_stateLock)
|
||
{
|
||
if (!_uploadedTrajectories.Remove(name))
|
||
{
|
||
_logger?.LogWarning("DeleteTrajectory 失败: 轨迹不存在 name={Name}", name);
|
||
throw new InvalidOperationException("DeleteFlyShotTraj failed");
|
||
}
|
||
|
||
var robotName = _configuredRobotName ?? throw new InvalidOperationException("Robot has not been setup.");
|
||
_trajectoryStore.Delete(robotName, name);
|
||
}
|
||
|
||
_logger?.LogInformation("DeleteTrajectory 完成: name={Name}", name);
|
||
}
|
||
|
||
/// <inheritdoc />
|
||
public string GetRobotName()
|
||
{
|
||
lock (_stateLock)
|
||
{
|
||
return _configuredRobotName ?? throw new InvalidOperationException("Robot has not been setup.");
|
||
}
|
||
}
|
||
|
||
/// <inheritdoc />
|
||
public int GetDegreesOfFreedom()
|
||
{
|
||
lock (_stateLock)
|
||
{
|
||
return _activeRobotProfile?.DegreesOfFreedom ?? throw new InvalidOperationException("Robot has not been setup.");
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 获取当前机器人配置，未初始化时抛出兼容错误。
|
||
/// </summary>
|
||
/// <returns>当前机器人配置。</returns>
|
||
private RobotProfile RequireActiveRobot()
|
||
{
|
||
return _activeRobotProfile ?? throw new InvalidOperationException("Robot has not been setup.");
|
||
}
|
||
|
||
/// <summary>
|
||
/// 获取当前机器人兼容配置；未加载旧配置时回退到现场默认值。
|
||
/// </summary>
|
||
/// <returns>当前机器人配置。</returns>
|
||
private CompatibilityRobotSettings RequireRobotSettings()
|
||
{
|
||
return _robotSettings ?? CreateDefaultRobotSettings();
|
||
}
|
||
|
||
/// <summary>
|
||
/// 校验机器人已经完成初始化。
|
||
/// </summary>
|
||
private void EnsureRobotSetup()
|
||
{
|
||
_ = RequireActiveRobot();
|
||
}
|
||
|
||
/// <summary>
|
||
/// 校验运行时已经处于可执行状态。
|
||
/// </summary>
|
||
private void EnsureRuntimeEnabled()
|
||
{
|
||
EnsureRobotSetup();
|
||
if (!_runtime.GetSnapshot().IsEnabled)
|
||
{
|
||
throw new InvalidOperationException("Robot has not been enabled.");
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 校验关节向量与当前机器人自由度一致，且所有值都是有限数值。
|
||
/// </summary>
|
||
/// <param name="joints">待校验关节向量，单位为弧度。</param>
|
||
/// <param name="expectedCount">期望自由度。</param>
|
||
/// <param name="paramName">调用方参数名。</param>
|
||
private static void EnsureJointVector(IReadOnlyList<double> joints, int expectedCount, string paramName)
|
||
{
|
||
if (joints.Count != expectedCount)
|
||
{
|
||
throw new ArgumentException($"关节数量必须为 {expectedCount}。", paramName);
|
||
}
|
||
|
||
for (var index = 0; index < joints.Count; index++)
|
||
{
|
||
var value = joints[index];
|
||
if (double.IsNaN(value) || double.IsInfinity(value))
|
||
{
|
||
throw new ArgumentOutOfRangeException(paramName, $"第 {index} 个关节值必须是有限数值。");
|
||
}
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 根据 saveTrajectory 参数把规划结果点位写入运行目录 Config/Data/name。
|
||
/// </summary>
|
||
/// <param name="name">飞拍轨迹名称。</param>
|
||
/// <param name="saveTrajectory">是否导出规划结果点位。</param>
|
||
/// <param name="robot">当前机器人模型。</param>
|
||
/// <param name="bundle">规划结果包。</param>
|
||
private void ExportFlyshotArtifactsIfRequested(
|
||
string name,
|
||
bool saveTrajectory,
|
||
RobotProfile robot,
|
||
PlannedExecutionBundle bundle)
|
||
{
|
||
if (!saveTrajectory)
|
||
{
|
||
return;
|
||
}
|
||
|
||
var speedRatio = _runtime.GetSnapshot().SpeedRatio;
|
||
_artifactWriter.WriteUploadedFlyshot(name, robot, bundle, speedRatio);
|
||
}
|
||
|
||
/// <summary>
|
||
/// 尝试从配置根目录加载 RobotConfig.json 获取机器人配置；失败时返回 null。
|
||
/// </summary>
|
||
/// <returns>加载到的机器人配置，或 null。</returns>
|
||
private CompatibilityRobotSettings? TryLoadRobotSettings()
|
||
{
|
||
foreach (var root in EnumerateRobotConfigRoots())
|
||
{
|
||
try
|
||
{
|
||
// 运行配置根本身已经是 Config 目录，这里用绝对路径避免再次追加 Config。
|
||
var configPath = Path.Combine(root, "RobotConfig.json");
|
||
var loaded = _configLoader.Load(configPath, root);
|
||
return loaded.Robot;
|
||
}
|
||
catch
|
||
{
|
||
// 单个候选根目录加载失败时继续尝试下一个兼容入口。
|
||
}
|
||
}
|
||
|
||
return null;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 枚举 RobotConfig.json 的配置根目录，运行目录 Config 优先，旧父工作区仅在显式配置时参与。
|
||
/// </summary>
|
||
/// <returns>待尝试的配置根目录列表。</returns>
|
||
private IEnumerable<string> EnumerateRobotConfigRoots()
|
||
{
|
||
yield return _options.ResolveConfigRoot();
|
||
|
||
var legacyWorkspaceRoot = _options.ResolveLegacyWorkspaceRoot();
|
||
if (legacyWorkspaceRoot is not null)
|
||
{
|
||
yield return legacyWorkspaceRoot;
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 构造与旧现场默认行为一致的机器人兼容配置。
|
||
/// </summary>
|
||
/// <returns>默认机器人配置。</returns>
|
||
private static CompatibilityRobotSettings CreateDefaultRobotSettings()
|
||
{
|
||
return new CompatibilityRobotSettings(
|
||
useDo: false,
|
||
ioAddresses: Array.Empty<int>(),
|
||
ioKeepCycles: 2,
|
||
accLimitScale: 1.0,
|
||
jerkLimitScale: 1.0,
|
||
adaptIcspTryNum: 5);
|
||
}
|
||
|
||
}
|