✨ feat(planning): 添加离线飞拍规划能力

* 固化离线飞拍轨迹加载、保存和规划结果。

* 真机初始化失败时保留模型加载后的离线规划能力。

* 更新状态接口、调试页和回归测试覆盖。
This commit is contained in:
2026-06-24 11:40:07 +08:00
parent 070d2ba5c0
commit 5193f537b0
24 changed files with 1398 additions and 738 deletions

View File

@@ -61,7 +61,7 @@ public sealed class AutoInitOptions
public string RobotName { get; set; } = string.Empty;
/// <summary>
/// 获取或设置机器人 IP 地址,用于连接真机或仿真控制器。
/// 获取或设置机器人 IP 地址,用于连接真实 FANUC 控制器。
/// </summary>
public string RobotIp { get; set; } = string.Empty;
}

View File

@@ -25,11 +25,12 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi
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;
private string _operatingMode = "ModelLoadFailed";
private string _initializationStage = "NotStarted";
private string? _robotIp;
private string? _lastConnectionError;
private DateTimeOffset? _lastConnectionErrorAt;
private DateTimeOffset? _lastInitializationAttemptAt;
/// <summary>
/// 初始化一份 HTTP-only 的 ControllerClient 兼容服务。
@@ -60,6 +61,15 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi
_artifactWriter = artifactWriter ?? new FlyshotTrajectoryArtifactWriter(_options, new RobotModelLoader());
_trajectoryStore = trajectoryStore ?? new JsonFlyshotTrajectoryStore(_options, _configLoader);
_logger = logger;
// 飞拍轨迹是固定 RobotConfig.json 中的独立资产,服务构造后立即加载,不依赖机器人模型。
var savedTrajectories = _trajectoryStore.LoadAll(out var savedSettings);
foreach (var saved in savedTrajectories)
{
_uploadedTrajectories[saved.Key] = saved.Value;
}
_robotSettings = savedSettings;
}
/// <inheritdoc />
@@ -82,125 +92,111 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi
/// </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;
_initializationStage = "LoadingModel";
}
// 从持久化存储恢复该机器人名下之前已上传的轨迹。
var savedTrajectories = _trajectoryStore.LoadAll(robotName, out _);
foreach (var saved in savedTrajectories)
try
{
var robotSettings = TryLoadRobotSettings() ?? _robotSettings ?? CreateDefaultRobotSettings();
var robotProfile = _robotCatalog.LoadProfile(
robotName,
robotSettings.AccLimitScale,
robotSettings.JerkLimitScale);
lock (_stateLock)
{
_uploadedTrajectories[saved.Key] = saved.Value;
// 重新加载模型只重置 Runtime;飞拍资产继续保留在固定 RobotConfig.json 中。
_configuredRobotName = robotName;
_activeRobotProfile = robotProfile;
_runtime.ResetRobot(robotProfile, robotName);
_runtime.SetSpeedRatio(robotSettings.SpeedRatio);
_robotSettings = robotSettings;
_operatingMode = "OfflineReady";
_initializationStage = "ModelLoaded";
}
_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);
}
catch
{
try
{
_runtime.Disconnect();
}
catch
{
// 模型切换失败时优先清理上层状态;底层若尚未初始化,Disconnect 失败可忽略。
}
lock (_stateLock)
{
_activeRobotProfile = null;
_configuredRobotName = null;
_operatingMode = "ModelLoadFailed";
_initializationStage = "ModelLoadFailed";
}
throw;
}
}
/// <inheritdoc />
public void InitializeRobot(string robotName, string robotIp, int bufferSize = 8)
{
lock (_stateLock)
{
_lastInitializationAttemptAt = DateTimeOffset.UtcNow;
_robotIp = robotIp;
_lastConnectionError = null;
_lastConnectionErrorAt = null;
}
SetUpRobot(robotName);
try
{
lock (_stateLock)
{
_initializationStage = "Connecting";
}
// 真机优先初始化固定执行连接和使能,不再选择生产仿真控制器。
Connect(robotIp);
EnableRobot(bufferSize);
lock (_stateLock)
{
_operatingMode = "RealReady";
_initializationStage = "RealReady";
}
}
_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))
catch (Exception exception)
{
throw new ArgumentException("环境文件路径不能为空。", nameof(envFile));
lock (_stateLock)
{
// 模型已加载时,真机失败必须保留离线规划能力。
_operatingMode = _activeRobotProfile is null ? "ModelLoadFailed" : "OfflineReady";
_initializationStage = "OfflineReady";
_lastConnectionError = exception.Message;
_lastConnectionErrorAt = DateTimeOffset.UtcNow;
}
throw;
}
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 />
@@ -216,6 +212,8 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi
lock (_stateLock)
{
EnsureRobotSetup();
_robotIp = robotIp;
_initializationStage = "Connecting";
}
_runtime.Connect(robotIp);
@@ -232,6 +230,11 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi
}
_runtime.Disconnect();
lock (_stateLock)
{
_operatingMode = _activeRobotProfile is null ? "ModelLoadFailed" : "OfflineReady";
_initializationStage = _activeRobotProfile is null ? "ModelLoadFailed" : "OfflineReady";
}
}
/// <inheritdoc />
@@ -243,6 +246,13 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi
EnsureRobotSetup();
}
_runtime.EnableRobot(bufferSize);
lock (_stateLock)
{
_operatingMode = "RealReady";
_initializationStage = "RealReady";
_lastConnectionError = null;
_lastConnectionErrorAt = null;
}
_logger?.LogInformation("EnableRobot 完成");
}
@@ -257,6 +267,11 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi
// DisableRobot 是中断/控制命令,不能被长时间运动串行锁挡住。
_runtime.DisableRobot();
lock (_stateLock)
{
_operatingMode = _activeRobotProfile is null ? "ModelLoadFailed" : "OfflineReady";
_initializationStage = _activeRobotProfile is null ? "ModelLoadFailed" : "OfflineReady";
}
_logger?.LogInformation("DisableRobot 完成");
}
@@ -290,9 +305,18 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi
isSetup,
isSetup ? _configuredRobotName : null,
isSetup ? _activeRobotProfile!.DegreesOfFreedom : 0,
isSetup ? _uploadedTrajectories.Keys.ToArray() : Array.Empty<string>(),
GetServerVersion(),
GetClientVersion());
_uploadedTrajectories.Keys.ToArray(),
ServerVersion,
"flyshot-replacement-controller-client-compat/0.1.0",
_operatingMode,
_initializationStage,
_robotIp,
_lastConnectionError,
_lastConnectionErrorAt,
_lastInitializationAttemptAt,
canPlanOffline: isSetup,
canExecuteRealMotion: string.Equals(_operatingMode, "RealReady", StringComparison.Ordinal)
&& _runtime.GetSnapshot().IsEnabled);
}
}
@@ -509,19 +533,16 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi
trajectory.Waypoints.Count,
trajectory.ShotFlags.Count(static f => f));
string robotName;
CompatibilityRobotSettings settings;
lock (_stateLock)
{
EnsureRuntimeEnabled();
ValidateUploadedTrajectoryAsset(trajectory);
_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);
_trajectoryStore.Save(settings, trajectory);
_logger?.LogInformation("UploadTrajectory 完成: name={Name}", trajectory.Name);
}
@@ -743,7 +764,7 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi
}
/// <inheritdoc />
public void SaveTrajectoryInfo(string name, string method = "icsp")
public OfflineFlyshotPlanningResult SaveTrajectoryInfo(string name, string method = "icsp")
{
if (string.IsNullOrWhiteSpace(name))
{
@@ -792,6 +813,7 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi
speedRatio);
_logger?.LogInformation("SaveTrajectoryInfo 完成: name={Name}", name);
return CreateOfflinePlanningResult(name, method, robot, bundle, speedRatio);
}
/// <inheritdoc />
@@ -858,7 +880,6 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi
_logger?.LogInformation("DeleteTrajectory 开始: name={Name}", name);
string robotName;
lock (_stateLock)
{
if (!_uploadedTrajectories.Remove(name))
@@ -867,11 +888,10 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi
throw new InvalidOperationException("DeleteFlyShotTraj failed");
}
robotName = _configuredRobotName ?? throw new InvalidOperationException("Robot has not been setup.");
}
// 删除持久化文件不占用状态锁,状态页只需要看到内存字典的即时快照。
_trajectoryStore.Delete(robotName, name);
_trajectoryStore.Delete(name);
_logger?.LogInformation("DeleteTrajectory 完成: name={Name}", name);
}
@@ -1066,8 +1086,7 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi
bundle.ExecutionTrajectory,
bundle.Result,
robot.ServoPeriod.TotalSeconds,
requestedSpeedRatio: speedRatio,
samplingSpeedRatio: 1.0);
requestedSpeedRatio: speedRatio);
var preparedResult = new TrajectoryResult(
programName: bundle.Result.ProgramName,
method: bundle.Result.Method,
@@ -1146,4 +1165,248 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi
adaptIcspTryNum: 5);
}
/// <summary>
/// 校验上传资产自身结构,不在上传阶段绑定机器人自由度或关节限制。
/// </summary>
/// <param name="trajectory">待保存的飞拍资产。</param>
private static void ValidateUploadedTrajectoryAsset(ControllerClientCompatUploadedTrajectory trajectory)
{
if (trajectory.Waypoints.Count == 0)
{
throw new ArgumentException("飞拍轨迹至少需要一个路点。", nameof(trajectory));
}
if (trajectory.ShotFlags.Count != trajectory.Waypoints.Count
|| trajectory.OffsetValues.Count != trajectory.Waypoints.Count
|| trajectory.AddressGroups.Count != trajectory.Waypoints.Count)
{
throw new ArgumentException("飞拍轨迹的辅助数组长度必须与路点数量一致。", nameof(trajectory));
}
for (var waypointIndex = 0; waypointIndex < trajectory.Waypoints.Count; waypointIndex++)
{
if (trajectory.Waypoints[waypointIndex].Length == 0)
{
throw new ArgumentException($"第 {waypointIndex} 个路点不能为空。", nameof(trajectory));
}
for (var jointIndex = 0; jointIndex < trajectory.Waypoints[waypointIndex].Length; jointIndex++)
{
var value = trajectory.Waypoints[waypointIndex][jointIndex];
if (double.IsNaN(value) || double.IsInfinity(value))
{
throw new ArgumentOutOfRangeException(
nameof(trajectory),
$"第 {waypointIndex} 个路点的第 {jointIndex} 个值必须是有限数值。");
}
}
}
}
/// <summary>
/// 把内部规划结果转换成供 HTTP 和 Debug 页面直接消费的完整离线结果。
/// </summary>
/// <param name="name">轨迹名称。</param>
/// <param name="method">规划方法。</param>
/// <param name="robot">当前机器人模型。</param>
/// <param name="bundle">已经完成最终发送队列构建的结果包。</param>
/// <param name="speedRatio">本次速度倍率。</param>
/// <returns>完整离线规划结果。</returns>
private OfflineFlyshotPlanningResult CreateOfflinePlanningResult(
string name,
string method,
RobotProfile robot,
PlannedExecutionBundle bundle,
double speedRatio)
{
var prepared = bundle.PreparedExecution
?? throw new InvalidOperationException("离线规划结果缺少最终发送队列。");
var ioBySampleIndex = BuildIoTimeline(prepared);
var finalSendPoints = prepared.Samples.Select((sample, index) =>
{
ioBySampleIndex.TryGetValue(index, out var io);
return new OfflineFlyshotSendPoint(
sample.SampleIndex,
sample.SendTime,
sample.TrajectoryTime,
sample.JointsDegrees,
io.Mask,
io.Value);
}).ToArray();
var triggerBindings = prepared.TriggerBindings.Select(binding =>
{
var ioValue = ComputeIoValue(binding.Trigger.AddressGroup);
return new OfflineFlyshotTriggerBinding(
binding.Trigger.WaypointIndex,
binding.Trigger.TriggerTime,
binding.SampleIndex,
binding.Sample.SampleIndex,
binding.FoundInWindow,
ioValue,
ioValue);
}).ToArray();
var plannedRows = bundle.ExecutionTrajectory.PlannedWaypoints
.Select((waypoint, index) =>
(IReadOnlyList<double>)[bundle.ExecutionTrajectory.WaypointTimes[index], .. waypoint.Positions])
.ToArray();
var artifactDirectory = Path.Combine(_options.ResolveConfigRoot(), "Data", SanitizeDirectoryName(name));
var artifactPaths = new[]
{
"JointTraj.txt",
"JointDetialTraj.txt",
"CartTraj.txt",
"CartDetialTraj.txt",
"ActualSendJointTraj.txt",
"ActualSendTiming.txt",
"ActualSendJerkStats.txt",
"ShotEvents.json"
}.ToDictionary(
static fileName => fileName,
fileName => Path.GetFullPath(Path.Combine(artifactDirectory, fileName)),
StringComparer.Ordinal);
var cartesianTrajectory = ReadNumericRows(artifactPaths["CartDetialTraj.txt"]);
var diagnostics = CalculateDiagnostics(prepared.Samples);
return new OfflineFlyshotPlanningResult(
name,
method,
bundle.Result.IsValid,
bundle.Result.Duration.TotalSeconds,
prepared.FinalDurationSeconds,
bundle.Result.OriginalWaypointCount,
bundle.Result.PlannedWaypointCount,
bundle.Result.DenseJointTrajectory?.Count ?? 0,
speedRatio,
robot.ServoPeriod.TotalSeconds,
plannedRows,
bundle.Result.DenseJointTrajectory ?? Array.Empty<IReadOnlyList<double>>(),
cartesianTrajectory,
finalSendPoints,
bundle.Result.ShotEvents,
triggerBindings,
artifactPaths,
diagnostics);
}
/// <summary>
/// 根据触发绑定和保持周期构造每个最终发送点的 IO 掩码和值。
/// </summary>
/// <param name="prepared">最终发送结果。</param>
/// <returns>按发送点索引保存的 IO 数据。</returns>
private static Dictionary<int, (ushort Mask, ushort Value)> BuildIoTimeline(FlyshotPreparedExecution prepared)
{
var result = new Dictionary<int, (ushort Mask, ushort Value)>();
foreach (var binding in prepared.TriggerBindings)
{
var ioValue = ComputeIoValue(binding.Trigger.AddressGroup);
var holdCount = Math.Max(1, binding.Trigger.HoldCycles);
for (var offset = 0; offset < holdCount && binding.SampleIndex + offset < prepared.Samples.Count; offset++)
{
result[binding.SampleIndex + offset] = (ioValue, ioValue);
}
}
return result;
}
/// <summary>
/// 把 IO 地址组转换为 16 位写出掩码。
/// </summary>
/// <param name="group">IO 地址组。</param>
/// <returns>写出掩码和值。</returns>
private static ushort ComputeIoValue(IoAddressGroup group)
{
ushort value = 0;
foreach (var address in group.Addresses)
{
if (address is >= 1 and <= 16)
{
value |= (ushort)(1 << (address - 1));
}
}
return value;
}
/// <summary>
/// 清理轨迹名称中的非法路径字符。
/// </summary>
/// <param name="name">原始轨迹名称。</param>
/// <returns>可安全用于目录名的字符串。</returns>
private static string SanitizeDirectoryName(string name)
{
var invalidChars = Path.GetInvalidFileNameChars();
return new string(name.Select(character => invalidChars.Contains(character) ? '_' : character).ToArray());
}
/// <summary>
/// 读取空格分隔的数值轨迹文件。
/// </summary>
/// <param name="path">待读取文件绝对路径。</param>
/// <returns>数值行集合;文件不存在时返回空集合。</returns>
private static IReadOnlyList<IReadOnlyList<double>> ReadNumericRows(string path)
{
if (!File.Exists(path))
{
return Array.Empty<IReadOnlyList<double>>();
}
return File.ReadLines(path)
.Where(static line => !string.IsNullOrWhiteSpace(line))
.Select(line => (IReadOnlyList<double>)line
.Split(' ', StringSplitOptions.RemoveEmptyEntries)
.Select(value => double.Parse(value, System.Globalization.CultureInfo.InvariantCulture))
.ToArray())
.ToArray();
}
/// <summary>
/// 从最终发送点列计算最大绝对速度、加速度和跃度。
/// </summary>
/// <param name="samples">最终发送采样点。</param>
/// <returns>离散运动学诊断统计。</returns>
private static OfflineFlyshotDiagnostics CalculateDiagnostics(IReadOnlyList<FlyshotPreparedSample> samples)
{
var maxVelocity = 0.0;
var maxAcceleration = 0.0;
var maxJerk = 0.0;
double[]? previousVelocity = null;
double[]? previousAcceleration = null;
for (var sampleIndex = 1; sampleIndex < samples.Count; sampleIndex++)
{
var dt = samples[sampleIndex].SendTime - samples[sampleIndex - 1].SendTime;
if (dt <= 0.0)
{
continue;
}
var velocity = new double[samples[sampleIndex].JointsDegrees.Count];
var acceleration = new double[velocity.Length];
for (var jointIndex = 0; jointIndex < velocity.Length; jointIndex++)
{
velocity[jointIndex] =
(samples[sampleIndex].JointsDegrees[jointIndex] - samples[sampleIndex - 1].JointsDegrees[jointIndex]) / dt;
maxVelocity = Math.Max(maxVelocity, Math.Abs(velocity[jointIndex]));
if (previousVelocity is not null)
{
acceleration[jointIndex] = (velocity[jointIndex] - previousVelocity[jointIndex]) / dt;
maxAcceleration = Math.Max(maxAcceleration, Math.Abs(acceleration[jointIndex]));
}
if (previousAcceleration is not null)
{
var jerk = (acceleration[jointIndex] - previousAcceleration[jointIndex]) / dt;
maxJerk = Math.Max(maxJerk, Math.Abs(jerk));
}
}
previousVelocity = velocity;
previousAcceleration = acceleration;
}
return new OfflineFlyshotDiagnostics(maxVelocity, maxAcceleration, maxJerk);
}
}

View File

@@ -20,7 +20,15 @@ public sealed record ControllerClientStatusSnapshotMetadata
int degreesOfFreedom,
IReadOnlyList<string> uploadedTrajectories,
string serverVersion,
string clientVersion)
string clientVersion,
string operatingMode = "ModelLoadFailed",
string initializationStage = "NotStarted",
string? robotIp = null,
string? lastConnectionError = null,
DateTimeOffset? lastConnectionErrorAt = null,
DateTimeOffset? lastInitializationAttemptAt = null,
bool canPlanOffline = false,
bool canExecuteRealMotion = false)
{
IsSetup = isSetup;
RobotName = robotName;
@@ -28,6 +36,14 @@ public sealed record ControllerClientStatusSnapshotMetadata
UploadedTrajectories = uploadedTrajectories.ToArray();
ServerVersion = serverVersion;
ClientVersion = clientVersion;
OperatingMode = operatingMode;
InitializationStage = initializationStage;
RobotIp = robotIp;
LastConnectionError = lastConnectionError;
LastConnectionErrorAt = lastConnectionErrorAt;
LastInitializationAttemptAt = lastInitializationAttemptAt;
CanPlanOffline = canPlanOffline;
CanExecuteRealMotion = canExecuteRealMotion;
}
/// <summary>
@@ -59,4 +75,28 @@ public sealed record ControllerClientStatusSnapshotMetadata
/// 获取兼容客户端版本。
/// </summary>
public string ClientVersion { get; }
/// <summary>获取当前运行模式。</summary>
public string OperatingMode { get; }
/// <summary>获取当前初始化阶段。</summary>
public string InitializationStage { get; }
/// <summary>获取最近尝试连接的机器人 IP。</summary>
public string? RobotIp { get; }
/// <summary>获取最后一次真机连接或使能错误。</summary>
public string? LastConnectionError { get; }
/// <summary>获取最后一次真机连接或使能错误发生时间。</summary>
public DateTimeOffset? LastConnectionErrorAt { get; }
/// <summary>获取最后一次初始化尝试时间。</summary>
public DateTimeOffset? LastInitializationAttemptAt { get; }
/// <summary>获取当前是否允许离线规划。</summary>
public bool CanPlanOffline { get; }
/// <summary>获取当前是否允许真机执行。</summary>
public bool CanExecuteRealMotion { get; }
}

View File

@@ -34,14 +34,14 @@ public sealed class JsonFlyshotTrajectoryStore
/// <param name="robotName">当前已初始化的机器人名称,仅用于日志诊断。</param>
/// <param name="settings">当前机器人级兼容配置。</param>
/// <param name="trajectory">要保存的已上传轨迹。</param>
public void Save(string robotName, CompatibilityRobotSettings settings, ControllerClientCompatUploadedTrajectory trajectory)
public void Save(CompatibilityRobotSettings settings, ControllerClientCompatUploadedTrajectory trajectory)
{
ArgumentNullException.ThrowIfNull(settings);
ArgumentNullException.ThrowIfNull(trajectory);
_logger?.LogInformation(
"RobotConfig 保存轨迹: robot={RobotName}, name={TrajectoryName}, waypoints={WaypointCount}",
robotName,
"fixed-config",
trajectory.Name,
trajectory.Waypoints.Count);
@@ -82,19 +82,30 @@ public sealed class JsonFlyshotTrajectoryStore
_logger?.LogInformation("RobotConfig 轨迹已保存到 {Path}", path);
}
/// <summary>
/// 兼容旧调用签名;机器人名称不再参与固定配置文件分区。
/// </summary>
/// <param name="robotName">仅用于兼容的机器人名称。</param>
/// <param name="settings">机器人级兼容配置。</param>
/// <param name="trajectory">待保存轨迹。</param>
public void Save(string robotName, CompatibilityRobotSettings settings, ControllerClientCompatUploadedTrajectory trajectory)
{
Save(settings, trajectory);
}
/// <summary>
/// 从统一 RobotConfig.json 删除指定名称的轨迹。
/// </summary>
/// <param name="robotName">当前已初始化的机器人名称,仅用于日志诊断。</param>
/// <param name="trajectoryName">要删除的轨迹名称。</param>
public void Delete(string robotName, string trajectoryName)
public void Delete(string trajectoryName)
{
if (string.IsNullOrWhiteSpace(trajectoryName))
{
throw new ArgumentException("轨迹名称不能为空。", nameof(trajectoryName));
}
_logger?.LogInformation("RobotConfig 删除轨迹: robot={RobotName}, name={TrajectoryName}", robotName, trajectoryName);
_logger?.LogInformation("RobotConfig 删除轨迹: name={TrajectoryName}", trajectoryName);
var path = ResolveStorePath();
if (!File.Exists(path))
@@ -131,13 +142,23 @@ public sealed class JsonFlyshotTrajectoryStore
}
}
/// <summary>
/// 兼容旧调用签名;机器人名称不再参与固定配置文件分区。
/// </summary>
/// <param name="robotName">仅用于兼容的机器人名称。</param>
/// <param name="trajectoryName">待删除轨迹名称。</param>
public void Delete(string robotName, string trajectoryName)
{
Delete(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)
public IReadOnlyDictionary<string, ControllerClientCompatUploadedTrajectory> LoadAll(out CompatibilityRobotSettings? settings)
{
var path = ResolveStorePath();
if (!File.Exists(path))
@@ -168,7 +189,7 @@ public sealed class JsonFlyshotTrajectoryStore
_logger?.LogInformation(
"RobotConfig 加载完成: robot={RobotName}, 轨迹数={Count}, useDo={UseDo}, ioKeepCycles={IoKeepCycles}",
robotName,
"fixed-config",
dict.Count,
settings?.UseDo,
settings?.IoKeepCycles);
@@ -183,6 +204,19 @@ public sealed class JsonFlyshotTrajectoryStore
}
}
/// <summary>
/// 兼容旧调用签名;机器人名称不再参与固定配置文件分区。
/// </summary>
/// <param name="robotName">仅用于兼容的机器人名称。</param>
/// <param name="settings">输出机器人配置。</param>
/// <returns>固定 RobotConfig.json 中的全部飞拍资产。</returns>
public IReadOnlyDictionary<string, ControllerClientCompatUploadedTrajectory> LoadAll(
string robotName,
out CompatibilityRobotSettings? settings)
{
return LoadAll(out settings);
}
/// <summary>
/// 把机器人兼容配置序列化为 JSON 对象,字段名与旧版 RobotConfig.json 一致。
/// </summary>

View File

@@ -17,25 +17,6 @@ public interface IControllerClientCompatService
/// </summary>
bool IsSetUp { get; }
/// <summary>
/// 保存当前调用方期望连接的 replacement 服务端地址。
/// </summary>
/// <param name="serverIp">客户端传入的服务端 IP。</param>
/// <param name="port">客户端传入的服务端端口。</param>
void ConnectServer(string serverIp, int port);
/// <summary>
/// 获取兼容服务端版本号。
/// </summary>
/// <returns>服务端版本号。</returns>
string GetServerVersion();
/// <summary>
/// 获取兼容客户端版本号。
/// </summary>
/// <returns>客户端版本号。</returns>
string GetClientVersion();
/// <summary>
/// 根据旧客户端使用的机器人名称完成机器人初始化。
/// </summary>
@@ -43,24 +24,12 @@ public interface IControllerClientCompatService
void SetUpRobot(string robotName);
/// <summary>
/// 根据旧客户端传入的环境文件完成机器人初始化。
/// 按“加载模型、连接真机、使能机器人”的固定顺序执行初始化。
/// </summary>
/// <param name="envFile">环境文件路径。</param>
void SetUpRobotFromEnv(string envFile);
/// <summary>
/// 设置是否显示 TCP 坐标轴。
/// </summary>
/// <param name="isShow">是否显示 TCP。</param>
/// <param name="axisLength">坐标轴长度。</param>
/// <param name="axisSize">坐标轴线宽。</param>
void SetShowTcp(bool isShow, double axisLength, int axisSize);
/// <summary>
/// 记录当前激活的控制器类型。
/// </summary>
/// <param name="sim">是否为仿真控制器。</param>
void SetActiveController(bool sim);
/// <param name="robotName">机器人名称。</param>
/// <param name="robotIp">机器人控制器 IP。</param>
/// <param name="bufferSize">J519 发送缓冲区大小。</param>
void InitializeRobot(string robotName, string robotIp, int bufferSize = 8);
/// <summary>
/// 记录当前控制器已经建立连接。
@@ -206,7 +175,7 @@ public interface IControllerClientCompatService
/// </summary>
/// <param name="name">轨迹名称。</param>
/// <param name="method">轨迹生成方法。</param>
void SaveTrajectoryInfo(string name, string method = "icsp");
OfflineFlyshotPlanningResult SaveTrajectoryInfo(string name, string method = "icsp");
/// <summary>
/// 检查指定飞拍轨迹是否可执行。

View File

@@ -0,0 +1,174 @@
using Flyshot.Core.Domain;
namespace Flyshot.ControllerClientCompat;
/// <summary>
/// 表示离线飞拍规划返回给 HTTP、Debug 页面和导出链路的完整结果。
/// </summary>
public sealed class OfflineFlyshotPlanningResult
{
/// <summary>
/// 初始化一份离线飞拍规划结果。
/// </summary>
/// <param name="trajectoryName">轨迹名称。</param>
/// <param name="method">规划方法。</param>
/// <param name="isValid">规划结果是否有效。</param>
/// <param name="planningDurationSeconds">连续规划轨迹时长,单位为秒。</param>
/// <param name="finalSendDurationSeconds">最终发送时长,单位为秒。</param>
/// <param name="originalWaypointCount">原始示教路点数量。</param>
/// <param name="plannedWaypointCount">规划关键路点数量。</param>
/// <param name="densePointCount">稠密轨迹点数量。</param>
/// <param name="speedRatio">本次最终发送使用的速度倍率。</param>
/// <param name="servoPeriodSeconds">最终发送采样周期,单位为秒。</param>
/// <param name="plannedJointTrajectory">关键关节轨迹,每行为时间加弧度制关节值。</param>
/// <param name="denseJointTrajectory">稠密关节轨迹,每行为时间加弧度制关节值。</param>
/// <param name="cartesianTrajectory">稠密笛卡尔轨迹。</param>
/// <param name="finalSendPoints">最终 8ms 发送点列。</param>
/// <param name="shotEvents">理论拍照事件。</param>
/// <param name="triggerBindings">触发事件与最终发送点的绑定结果。</param>
/// <param name="artifactPaths">导出文件名与绝对路径。</param>
/// <param name="diagnostics">最终发送点列的速度、加速度和跃度统计。</param>
public OfflineFlyshotPlanningResult(
string trajectoryName,
string method,
bool isValid,
double planningDurationSeconds,
double finalSendDurationSeconds,
int originalWaypointCount,
int plannedWaypointCount,
int densePointCount,
double speedRatio,
double servoPeriodSeconds,
IEnumerable<IReadOnlyList<double>> plannedJointTrajectory,
IEnumerable<IReadOnlyList<double>> denseJointTrajectory,
IEnumerable<IReadOnlyList<double>> cartesianTrajectory,
IEnumerable<OfflineFlyshotSendPoint> finalSendPoints,
IEnumerable<ShotEvent> shotEvents,
IEnumerable<OfflineFlyshotTriggerBinding> triggerBindings,
IReadOnlyDictionary<string, string> artifactPaths,
OfflineFlyshotDiagnostics diagnostics)
{
TrajectoryName = trajectoryName;
Method = method;
IsValid = isValid;
PlanningDurationSeconds = planningDurationSeconds;
FinalSendDurationSeconds = finalSendDurationSeconds;
OriginalWaypointCount = originalWaypointCount;
PlannedWaypointCount = plannedWaypointCount;
DensePointCount = densePointCount;
SpeedRatio = speedRatio;
ServoPeriodSeconds = servoPeriodSeconds;
PlannedJointTrajectory = plannedJointTrajectory.Select(static row => row.ToArray()).ToArray();
DenseJointTrajectory = denseJointTrajectory.Select(static row => row.ToArray()).ToArray();
CartesianTrajectory = cartesianTrajectory.Select(static row => row.ToArray()).ToArray();
FinalSendPoints = finalSendPoints.ToArray();
ShotEvents = shotEvents.ToArray();
TriggerBindings = triggerBindings.ToArray();
ArtifactPaths = new Dictionary<string, string>(artifactPaths, StringComparer.Ordinal);
Diagnostics = diagnostics;
}
/// <summary>获取轨迹名称。</summary>
public string TrajectoryName { get; }
/// <summary>获取规划方法。</summary>
public string Method { get; }
/// <summary>获取规划结果是否有效。</summary>
public bool IsValid { get; }
/// <summary>获取连续规划轨迹时长,单位为秒。</summary>
public double PlanningDurationSeconds { get; }
/// <summary>获取最终发送时长,单位为秒。</summary>
public double FinalSendDurationSeconds { get; }
/// <summary>获取原始示教路点数量。</summary>
public int OriginalWaypointCount { get; }
/// <summary>获取规划关键路点数量。</summary>
public int PlannedWaypointCount { get; }
/// <summary>获取稠密轨迹点数量。</summary>
public int DensePointCount { get; }
/// <summary>获取最终发送点数量。</summary>
public int FinalSendPointCount => FinalSendPoints.Count;
/// <summary>获取本次最终发送使用的速度倍率。</summary>
public double SpeedRatio { get; }
/// <summary>获取最终发送采样周期,单位为秒。</summary>
public double ServoPeriodSeconds { get; }
/// <summary>获取关键关节轨迹。</summary>
public IReadOnlyList<IReadOnlyList<double>> PlannedJointTrajectory { get; }
/// <summary>获取稠密关节轨迹。</summary>
public IReadOnlyList<IReadOnlyList<double>> DenseJointTrajectory { get; }
/// <summary>获取稠密笛卡尔轨迹。</summary>
public IReadOnlyList<IReadOnlyList<double>> CartesianTrajectory { get; }
/// <summary>获取最终 8ms 发送点列。</summary>
public IReadOnlyList<OfflineFlyshotSendPoint> FinalSendPoints { get; }
/// <summary>获取理论拍照事件。</summary>
public IReadOnlyList<ShotEvent> ShotEvents { get; }
/// <summary>获取触发事件与最终发送点的绑定结果。</summary>
public IReadOnlyList<OfflineFlyshotTriggerBinding> TriggerBindings { get; }
/// <summary>获取导出文件名与绝对路径。</summary>
public IReadOnlyDictionary<string, string> ArtifactPaths { get; }
/// <summary>获取最终发送点列的速度、加速度和跃度统计。</summary>
public OfflineFlyshotDiagnostics Diagnostics { get; }
}
/// <summary>
/// 表示离线规划结果中的单个最终 J519 发送点。
/// </summary>
/// <param name="SampleIndex">发送周期序号。</param>
/// <param name="SendTime">物理发送时间,单位为秒。</param>
/// <param name="TrajectoryTime">映射到连续轨迹的采样时间,单位为秒。</param>
/// <param name="JointsDegrees">角度制关节目标。</param>
/// <param name="IoMask">该点写出的 IO 掩码。</param>
/// <param name="IoValue">该点写出的 IO 值。</param>
public sealed record OfflineFlyshotSendPoint(
long SampleIndex,
double SendTime,
double TrajectoryTime,
IReadOnlyList<double> JointsDegrees,
ushort IoMask,
ushort IoValue);
/// <summary>
/// 表示离线规划结果中的触发采样点绑定。
/// </summary>
/// <param name="WaypointIndex">原始示教路点索引。</param>
/// <param name="TriggerTime">理论触发时间,单位为秒。</param>
/// <param name="SampleIndex">最终发送点数组索引。</param>
/// <param name="SampleSequence">最终发送序号。</param>
/// <param name="FoundInWindow">是否在理论搜索窗口内完成绑定。</param>
/// <param name="IoMask">触发 IO 掩码。</param>
/// <param name="IoValue">触发 IO 值。</param>
public sealed record OfflineFlyshotTriggerBinding(
int WaypointIndex,
double TriggerTime,
int SampleIndex,
long SampleSequence,
bool FoundInWindow,
ushort IoMask,
ushort IoValue);
/// <summary>
/// 表示最终发送点列的离散运动学诊断统计。
/// </summary>
/// <param name="MaxAbsVelocityDegreesPerSecond">最大绝对速度,单位 deg/s。</param>
/// <param name="MaxAbsAccelerationDegreesPerSecondSquared">最大绝对加速度,单位 deg/s²。</param>
/// <param name="MaxAbsJerkDegreesPerSecondCubed">最大绝对跃度,单位 deg/s³。</param>
public sealed record OfflineFlyshotDiagnostics(
double MaxAbsVelocityDegreesPerSecond,
double MaxAbsAccelerationDegreesPerSecondSquared,
double MaxAbsJerkDegreesPerSecondCubed);