yunxiao.zhu
2cd42f04e5
* 新增 `MovePose` 方法，支持以直角坐标执行点到点移动。 * 引入 `LegacyCartesianPoseRequest` 类，处理直角位姿请求体的解析与验证。 * 更新 `LegacyHttpApiController`，实现 `/move_pose/` 路由以支持新功能。 * 增强状态快照元数据，提供机器人初始化状态与已上传轨迹信息。 * 更新前端状态页面，增加直角坐标点动控制面板与步长设置选项。 * 相关文档与测试用例同步更新，确保新功能的完整性与稳定性。
1150 lines
42 KiB
C#
1150 lines
42 KiB
C#
using Flyshot.Core.Config;
|
||
using Flyshot.Core.Domain;
|
||
using Flyshot.Core.Planning.Sampling;
|
||
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 object _motionLock = 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);
|
||
// RobotConfig.json 的 speed_ratio 是启动默认值；在线 set_speedRatio 仍可覆盖后续执行。
|
||
_runtime.SetSpeedRatio(robotSettings.SpeedRatio);
|
||
_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}, speedRatio={SpeedRatio}, 恢复轨迹数={TrajCount}",
|
||
robotName,
|
||
robotProfile.DegreesOfFreedom,
|
||
robotSettings.AccLimitScale,
|
||
robotSettings.JerkLimitScale,
|
||
robotSettings.SpeedRatio,
|
||
_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();
|
||
}
|
||
|
||
// DisableRobot 是中断/控制命令，不能被长时间运动串行锁挡住。
|
||
_runtime.DisableRobot();
|
||
_logger?.LogInformation("DisableRobot 完成");
|
||
}
|
||
|
||
/// <inheritdoc />
|
||
public void StopMove()
|
||
{
|
||
_logger?.LogInformation("StopMove 开始");
|
||
lock (_stateLock)
|
||
{
|
||
EnsureRobotSetup();
|
||
}
|
||
|
||
// StopMove 必须能在运动执行期间直接进入 runtime 取消发送任务。
|
||
_runtime.StopMove();
|
||
_logger?.LogInformation("StopMove 完成");
|
||
}
|
||
|
||
/// <inheritdoc />
|
||
public ControllerStateSnapshot GetControllerSnapshot()
|
||
{
|
||
return _runtime.GetSnapshot();
|
||
}
|
||
|
||
/// <inheritdoc />
|
||
public ControllerClientStatusSnapshotMetadata GetStatusSnapshotMetadata()
|
||
{
|
||
lock (_stateLock)
|
||
{
|
||
var isSetup = _activeRobotProfile is not null;
|
||
return new ControllerClientStatusSnapshotMetadata(
|
||
isSetup,
|
||
isSetup ? _configuredRobotName : null,
|
||
isSetup ? _activeRobotProfile!.DegreesOfFreedom : 0,
|
||
isSetup ? _uploadedTrajectories.Keys.ToArray() : Array.Empty<string>(),
|
||
GetServerVersion(),
|
||
GetClientVersion());
|
||
}
|
||
}
|
||
|
||
/// <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"))));
|
||
|
||
RobotProfile robot;
|
||
lock (_stateLock)
|
||
{
|
||
robot = RequireActiveRobot();
|
||
EnsureRuntimeEnabled();
|
||
}
|
||
|
||
lock (_motionLock)
|
||
{
|
||
ExecuteMoveJointAndWait(robot, jointPositions, "MoveJoint");
|
||
}
|
||
|
||
_logger?.LogInformation("MoveJoint 完成");
|
||
}
|
||
|
||
/// <inheritdoc />
|
||
public void MovePose(IReadOnlyList<double> pose)
|
||
{
|
||
ArgumentNullException.ThrowIfNull(pose);
|
||
|
||
_logger?.LogInformation("MovePose 开始: 目标位姿维度={PoseCount}", pose.Count);
|
||
_logger?.LogDebug("MovePose 目标位姿: {Pose}", string.Join(", ", pose.Select(v => v.ToString("F4"))));
|
||
|
||
RobotProfile robot;
|
||
lock (_stateLock)
|
||
{
|
||
robot = RequireActiveRobot();
|
||
EnsureRuntimeEnabled();
|
||
}
|
||
|
||
lock (_motionLock)
|
||
{
|
||
ExecuteMovePoseAndWait(robot, pose, "MovePose");
|
||
}
|
||
|
||
_logger?.LogInformation("MovePose 完成");
|
||
}
|
||
|
||
/// <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")))}]")));
|
||
|
||
RobotProfile robot;
|
||
CompatibilityRobotSettings settings;
|
||
lock (_stateLock)
|
||
{
|
||
robot = RequireActiveRobot();
|
||
EnsureRuntimeEnabled();
|
||
settings = RequireRobotSettings();
|
||
}
|
||
|
||
lock (_motionLock)
|
||
{
|
||
// 普通轨迹必须按调用方指定 method 规划，再把规划结果交给运行时执行。
|
||
var planningSpeedScale = settings.PlanningSpeedScale;
|
||
var speedRatio = _runtime.GetSnapshot().SpeedRatio;
|
||
var bundle = _trajectoryOrchestrator.PlanOrdinaryTrajectory(robot, waypoints, options, planningSpeedScale, speedRatio);
|
||
_logger?.LogInformation(
|
||
"ExecuteTrajectory 规划完成: method={Method}, 时长={Duration}s, 有效={IsValid}, 采样点数={SampleCount}, planningSpeedScale={PlanningSpeedScale}, speedRatio={SpeedRatio}",
|
||
bundle.Result.Method,
|
||
bundle.Result.Duration.TotalSeconds,
|
||
bundle.Result.IsValid,
|
||
bundle.Result.DenseJointTrajectory?.Count ?? 0,
|
||
planningSpeedScale,
|
||
speedRatio);
|
||
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));
|
||
|
||
string robotName;
|
||
CompatibilityRobotSettings settings;
|
||
lock (_stateLock)
|
||
{
|
||
EnsureRuntimeEnabled();
|
||
_uploadedTrajectories[trajectory.Name] = trajectory;
|
||
|
||
robotName = _configuredRobotName ?? throw new InvalidOperationException("Robot has not been setup.");
|
||
settings = _robotSettings ?? CreateDefaultRobotSettings();
|
||
}
|
||
|
||
// RobotConfig.json 持久化是文件 I/O，放在状态锁外，避免状态页轮询被磁盘写入拖住。
|
||
_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);
|
||
|
||
RobotProfile robot;
|
||
CompatibilityRobotSettings settings;
|
||
ControllerClientCompatUploadedTrajectory trajectory;
|
||
lock (_stateLock)
|
||
{
|
||
robot = RequireActiveRobot();
|
||
EnsureRuntimeEnabled();
|
||
|
||
if (!_uploadedTrajectories.TryGetValue(name, out var uploadedTrajectory))
|
||
{
|
||
_logger?.LogWarning("ExecuteTrajectoryByName 失败: 轨迹不存在 name={Name}", name);
|
||
throw new InvalidOperationException("FlyShot trajectory does not exist.");
|
||
}
|
||
|
||
// 飞拍执行只拿上传轨迹的瞬时副本，后续规划/导出都不再依赖字典锁。
|
||
trajectory = CloneUploadedTrajectory(uploadedTrajectory);
|
||
if (trajectory.Waypoints.Count == 0)
|
||
{
|
||
_logger?.LogWarning("ExecuteTrajectoryByName 失败: 轨迹无路点 name={Name}", name);
|
||
throw new InvalidOperationException("FlyShot trajectory contains no waypoints.");
|
||
}
|
||
|
||
settings = RequireRobotSettings();
|
||
}
|
||
|
||
lock (_motionLock)
|
||
{
|
||
// 已上传飞拍轨迹必须按调用方指定 method 生成 shot timeline 后再交给运行时。
|
||
var speedRatio = _runtime.GetSnapshot().SpeedRatio;
|
||
var bundle = _trajectoryOrchestrator.PlanUploadedFlyshot(robot, trajectory, options, settings, settings.PlanningSpeedScale, speedRatio);
|
||
bundle = PrepareFlyshotExecutionBundle(robot, bundle, speedRatio);
|
||
ExportFlyshotArtifactsIfRequested(
|
||
name,
|
||
options.SaveTrajectory,
|
||
robot,
|
||
trajectory,
|
||
options,
|
||
settings,
|
||
bundle,
|
||
settings.PlanningSpeedScale,
|
||
speedRatio);
|
||
_logger?.LogInformation(
|
||
"ExecuteTrajectoryByName 规划完成: name={Name}, method={Method}, 时长={Duration}s, 触发事件数={TriggerCount}, 使用缓存={UsedCache}, planningSpeedScale={PlanningSpeedScale}, speedRatio={SpeedRatio}",
|
||
name,
|
||
bundle.Result.Method,
|
||
bundle.Result.Duration.TotalSeconds,
|
||
bundle.Result.TriggerTimeline.Count,
|
||
bundle.Result.UsedCache,
|
||
settings.PlanningSpeedScale,
|
||
speedRatio);
|
||
|
||
if (options.MoveToStart)
|
||
{
|
||
_logger?.LogInformation("ExecuteTrajectoryByName 先移动到起点");
|
||
ExecuteMoveJointAndWait(robot, bundle.PlannedTrajectory.PlannedWaypoints[0].Positions, "ExecuteTrajectoryByName.move_to_start");
|
||
EnsureFeedbackNearFlyshotStart(bundle.PlannedTrajectory.PlannedWaypoints[0].Positions, name);
|
||
}
|
||
else
|
||
{
|
||
// 正式飞拍前必须确认机器人反馈已经在轨迹起点附近，避免 J519 目标突变。
|
||
if (!IsFeedbackNearFlyshotStart(bundle.PlannedTrajectory.PlannedWaypoints[0].Positions, name))
|
||
{
|
||
return;
|
||
}
|
||
}
|
||
|
||
var finalJointPositions = bundle.PlannedTrajectory.PlannedWaypoints[^1].Positions;
|
||
_runtime.ExecuteTrajectory(bundle.Result, finalJointPositions);
|
||
if (options.Wait)
|
||
{
|
||
var executionDuration = bundle.PreparedExecution is null
|
||
? bundle.Result.Duration
|
||
: TimeSpan.FromSeconds(bundle.PreparedExecution.FinalDurationSeconds);
|
||
WaitForRuntimeMotionComplete("ExecuteTrajectoryByName.flyshot", executionDuration);
|
||
}
|
||
}
|
||
|
||
_logger?.LogInformation("ExecuteTrajectoryByName 完成: name={Name}", name);
|
||
}
|
||
|
||
/// <summary>
|
||
/// 从当前关节位置生成临时 PTP 稠密轨迹并阻塞等待运行时完成，避免后续 J519 目标发生突变。
|
||
/// </summary>
|
||
/// <param name="robot">当前机器人模型。</param>
|
||
/// <param name="targetJointPositions">目标关节位置，单位为弧度。</param>
|
||
/// <param name="operationName">用于日志和超时异常的操作名。</param>
|
||
private void ExecuteMoveJointAndWait(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>
|
||
/// 从当前 TCP 位姿生成临时直角 PTP 稠密轨迹并阻塞等待运行时完成。
|
||
/// </summary>
|
||
/// <param name="robot">当前机器人模型，用于取得 J519 伺服周期。</param>
|
||
/// <param name="targetPose">目标位姿 [x,y,z,w,p,r]，单位为 mm/deg。</param>
|
||
/// <param name="operationName">用于日志和超时异常的操作名。</param>
|
||
private void ExecuteMovePoseAndWait(RobotProfile robot, IReadOnlyList<double> targetPose, string operationName)
|
||
{
|
||
var currentPose = NormalizeRuntimePose(_runtime.GetPose());
|
||
EnsurePoseVector(targetPose, nameof(targetPose));
|
||
|
||
var speedRatio = _runtime.GetSnapshot().SpeedRatio;
|
||
var moveResult = MovePoseTrajectoryGenerator.CreateResult(currentPose, targetPose, robot.ServoPeriod, speedRatio, _logger);
|
||
_logger?.LogInformation(
|
||
"{OperationName} 直角PTP规划完成: 当前速度倍率={SpeedRatio}, 规划时长={Duration}s, 采样点数={SampleCount}",
|
||
operationName,
|
||
speedRatio,
|
||
moveResult.Duration.TotalSeconds,
|
||
moveResult.DenseCartesianTrajectory?.Count ?? 0);
|
||
|
||
_runtime.ExecuteCartesianTrajectory(moveResult, targetPose);
|
||
WaitForRuntimeMotionComplete(operationName, moveResult.Duration);
|
||
}
|
||
|
||
/// <summary>
|
||
/// 校验当前反馈是否接近飞拍起点；不接近时直接抛出兼容错误。
|
||
/// </summary>
|
||
private void EnsureFeedbackNearFlyshotStart(IReadOnlyList<double> targetJointPositions, string name)
|
||
{
|
||
if (!IsFeedbackNearFlyshotStart(targetJointPositions, name))
|
||
{
|
||
throw new InvalidOperationException("Robot feedback is not near flyshot start.");
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 检查当前机械臂关节反馈与计划轨迹第一个点之间的差异。
|
||
/// </summary>
|
||
private bool IsFeedbackNearFlyshotStart(IReadOnlyList<double> targetJointPositions, string name)
|
||
{
|
||
var currentJointPositions = _runtime.GetJointPositions();
|
||
var diff = currentJointPositions.Zip(targetJointPositions, (current, target) => Math.Abs(current - target)).Sum();
|
||
if (diff <= 0.01)
|
||
{
|
||
return true;
|
||
}
|
||
|
||
_logger?.LogWarning("ExecuteTrajectoryByName 当前关节坐标与计划轨迹的第一个点之间的差异过大 name={Name}, diff={Diff}", name, diff);
|
||
return false;
|
||
}
|
||
|
||
/// <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);
|
||
|
||
RobotProfile robot;
|
||
CompatibilityRobotSettings planningSettings;
|
||
ControllerClientCompatUploadedTrajectory trajectory;
|
||
lock (_stateLock)
|
||
{
|
||
robot = RequireActiveRobot();
|
||
if (!_uploadedTrajectories.TryGetValue(name, out var uploadedTrajectory))
|
||
{
|
||
_logger?.LogWarning("SaveTrajectoryInfo 失败: 轨迹不存在 name={Name}", name);
|
||
throw new InvalidOperationException("FlyShot trajectory does not exist.");
|
||
}
|
||
|
||
// 先通过规划校验避免静默接受非法参数，同时把轨迹信息强制刷写到本地 JSON。
|
||
// 保存轨迹信息会执行规划和文件导出，先复制上传数据再释放状态锁。
|
||
trajectory = CloneUploadedTrajectory(uploadedTrajectory);
|
||
planningSettings = RequireRobotSettings();
|
||
}
|
||
|
||
var speedRatio = _runtime.GetSnapshot().SpeedRatio;
|
||
var bundle = _trajectoryOrchestrator.PlanUploadedFlyshot(
|
||
robot,
|
||
trajectory,
|
||
new FlyshotExecutionOptions(useCache: false, saveTrajectory: true, method: method),
|
||
planningSettings,
|
||
planningSettings.PlanningSpeedScale,
|
||
speedRatio);
|
||
bundle = PrepareFlyshotExecutionBundle(robot, bundle, speedRatio);
|
||
_logger?.LogInformation("SaveTrajectoryInfo 规划完成记录到本地");
|
||
ExportFlyshotArtifactsIfRequested(
|
||
name,
|
||
saveTrajectory: true,
|
||
robot,
|
||
trajectory,
|
||
new FlyshotExecutionOptions(useCache: false, saveTrajectory: true, method: method),
|
||
planningSettings,
|
||
bundle,
|
||
planningSettings.PlanningSpeedScale,
|
||
speedRatio);
|
||
|
||
_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);
|
||
|
||
RobotProfile robot;
|
||
CompatibilityRobotSettings planningSettings;
|
||
ControllerClientCompatUploadedTrajectory trajectory;
|
||
lock (_stateLock)
|
||
{
|
||
robot = RequireActiveRobot();
|
||
if (!_uploadedTrajectories.TryGetValue(name, out var uploadedTrajectory))
|
||
{
|
||
_logger?.LogWarning("IsFlyshotTrajectoryValid 失败: 轨迹不存在 name={Name}", name);
|
||
throw new InvalidOperationException("FlyShot trajectory does not exist.");
|
||
}
|
||
|
||
// 有效性检查只消费当前快照，不要求和后续上传/删除形成长事务。
|
||
trajectory = CloneUploadedTrajectory(uploadedTrajectory);
|
||
planningSettings = RequireRobotSettings();
|
||
}
|
||
|
||
var speedRatio = _runtime.GetSnapshot().SpeedRatio;
|
||
var bundle = _trajectoryOrchestrator.PlanUploadedFlyshot(
|
||
robot,
|
||
trajectory,
|
||
new FlyshotExecutionOptions(method: method, saveTrajectory: saveTrajectory),
|
||
planningSettings,
|
||
planningSettings.PlanningSpeedScale,
|
||
speedRatio);
|
||
bundle = PrepareFlyshotExecutionBundle(robot, bundle, speedRatio);
|
||
ExportFlyshotArtifactsIfRequested(
|
||
name,
|
||
saveTrajectory,
|
||
robot,
|
||
trajectory,
|
||
new FlyshotExecutionOptions(method: method, saveTrajectory: saveTrajectory),
|
||
planningSettings,
|
||
bundle,
|
||
planningSettings.PlanningSpeedScale,
|
||
speedRatio);
|
||
|
||
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);
|
||
|
||
string robotName;
|
||
lock (_stateLock)
|
||
{
|
||
if (!_uploadedTrajectories.Remove(name))
|
||
{
|
||
_logger?.LogWarning("DeleteTrajectory 失败: 轨迹不存在 name={Name}", name);
|
||
throw new InvalidOperationException("DeleteFlyShotTraj failed");
|
||
}
|
||
|
||
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>
|
||
/// <param name="trajectory">待复制的上传轨迹。</param>
|
||
/// <returns>上传轨迹副本。</returns>
|
||
private static ControllerClientCompatUploadedTrajectory CloneUploadedTrajectory(ControllerClientCompatUploadedTrajectory trajectory)
|
||
{
|
||
return new ControllerClientCompatUploadedTrajectory(
|
||
trajectory.Name,
|
||
trajectory.Waypoints,
|
||
trajectory.ShotFlags,
|
||
trajectory.OffsetValues,
|
||
trajectory.AddressGroups);
|
||
}
|
||
|
||
/// <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>
|
||
/// 校验直角位姿为 [x,y,z,w,p,r] 六维有限数。
|
||
/// </summary>
|
||
/// <param name="pose">待校验位姿，单位为 mm/deg。</param>
|
||
/// <param name="paramName">调用方参数名。</param>
|
||
private static void EnsurePoseVector(IReadOnlyList<double> pose, string paramName)
|
||
{
|
||
if (pose.Count != 6)
|
||
{
|
||
throw new ArgumentException("位姿必须为 [x,y,z,w,p,r] 六维数组。", paramName);
|
||
}
|
||
|
||
for (var index = 0; index < pose.Count; index++)
|
||
{
|
||
var value = pose[index];
|
||
if (double.IsNaN(value) || double.IsInfinity(value))
|
||
{
|
||
throw new ArgumentOutOfRangeException(paramName, $"第 {index} 个位姿值必须是有限数值。");
|
||
}
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 将运行时位姿快照归一化为 J519 直角命令需要的 [x,y,z,w,p,r] 六维。
|
||
/// </summary>
|
||
/// <param name="pose">运行时返回的位姿数组。</param>
|
||
/// <returns>六维直角位姿。</returns>
|
||
private static IReadOnlyList<double> NormalizeRuntimePose(IReadOnlyList<double> pose)
|
||
{
|
||
if (pose.Count < 6)
|
||
{
|
||
throw new InvalidOperationException("Runtime pose must contain at least [x,y,z,w,p,r].");
|
||
}
|
||
|
||
var normalized = pose.Take(6).ToArray();
|
||
EnsurePoseVector(normalized, nameof(pose));
|
||
return normalized;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 根据 saveTrajectory 参数把规划结果点位写入运行目录 Config/Data/name。
|
||
/// </summary>
|
||
/// <param name="name">飞拍轨迹名称。</param>
|
||
/// <param name="saveTrajectory">是否导出规划结果点位。</param>
|
||
/// <param name="robot">当前机器人模型。</param>
|
||
/// <param name="uploaded">已上传的飞拍轨迹。</param>
|
||
/// <param name="options">本次规划选项。</param>
|
||
/// <param name="settings">当前机器人兼容设置。</param>
|
||
/// <param name="executionBundle">运行时和 ActualSend 诊断使用的规划结果包。</param>
|
||
/// <param name="planningSpeedScale">规划速度倍率。</param>
|
||
/// <param name="speedRatio">本次运行时速度倍率。</param>
|
||
private void ExportFlyshotArtifactsIfRequested(
|
||
string name,
|
||
bool saveTrajectory,
|
||
RobotProfile robot,
|
||
ControllerClientCompatUploadedTrajectory uploaded,
|
||
FlyshotExecutionOptions options,
|
||
CompatibilityRobotSettings settings,
|
||
PlannedExecutionBundle executionBundle,
|
||
double planningSpeedScale,
|
||
double speedRatio)
|
||
{
|
||
if (!saveTrajectory)
|
||
{
|
||
return;
|
||
}
|
||
|
||
// 旧格式 Joint/Cart 导出必须只反映 planning_speed_scale；ActualSend* 才反映当前运行时 speedRatio。
|
||
var exportPlanningBundle = _trajectoryOrchestrator.PlanUploadedFlyshot(
|
||
robot,
|
||
uploaded,
|
||
new FlyshotExecutionOptions(
|
||
moveToStart: options.MoveToStart,
|
||
useCache: false,
|
||
saveTrajectory: options.SaveTrajectory,
|
||
method: options.Method,
|
||
wait: options.Wait),
|
||
settings,
|
||
planningSpeedScale,
|
||
speedRatio: 1.0);
|
||
_artifactWriter.WriteUploadedFlyshot(name, robot, exportPlanningBundle, executionBundle, speedRatio);
|
||
}
|
||
|
||
/// <summary>
|
||
/// 为飞拍链路预先构建最终发送队列，确保运行时只消费已经离散校验通过的 8ms 点列。
|
||
/// </summary>
|
||
private static PlannedExecutionBundle PrepareFlyshotExecutionBundle(
|
||
RobotProfile robot,
|
||
PlannedExecutionBundle bundle,
|
||
double speedRatio)
|
||
{
|
||
var preparedExecution = FlyshotExecutionSendSequenceBuilder.Build(
|
||
robot,
|
||
bundle.ExecutionTrajectory,
|
||
bundle.Result,
|
||
robot.ServoPeriod.TotalSeconds,
|
||
requestedSpeedRatio: speedRatio,
|
||
samplingSpeedRatio: 1.0);
|
||
var preparedResult = new TrajectoryResult(
|
||
programName: bundle.Result.ProgramName,
|
||
method: bundle.Result.Method,
|
||
isValid: bundle.Result.IsValid,
|
||
duration: bundle.Result.Duration,
|
||
shotEvents: bundle.Result.ShotEvents,
|
||
triggerTimeline: bundle.Result.TriggerTimeline,
|
||
artifacts: bundle.Result.Artifacts,
|
||
failureReason: bundle.Result.FailureReason,
|
||
usedCache: bundle.Result.UsedCache,
|
||
originalWaypointCount: bundle.Result.OriginalWaypointCount,
|
||
plannedWaypointCount: bundle.Result.PlannedWaypointCount,
|
||
triggerSampleIndexOffsetCycles: bundle.Result.TriggerSampleIndexOffsetCycles,
|
||
denseJointTrajectory: bundle.Result.DenseJointTrajectory,
|
||
preparedFlyshotExecution: preparedExecution);
|
||
return new PlannedExecutionBundle(
|
||
bundle.PlannedTrajectory,
|
||
bundle.ShotTimeline,
|
||
preparedResult,
|
||
preparedExecution,
|
||
bundle.ExecutionTrajectory);
|
||
}
|
||
|
||
/// <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,
|
||
triggerSampleIndexOffsetCycles: 7,
|
||
accLimitScale: 1.0,
|
||
jerkLimitScale: 1.0,
|
||
adaptIcspTryNum: 5);
|
||
}
|
||
|
||
}
|