Merge commit '0952958af7685646cdbf2a4907c6e2ae59d4d352'
This commit is contained in:
@@ -0,0 +1,46 @@
|
||||
namespace Flyshot.ControllerClientCompat;
|
||||
|
||||
/// <summary>
|
||||
/// 表示 HTTP-only ControllerClient 兼容层的基础配置。
|
||||
/// </summary>
|
||||
public sealed class ControllerClientCompatOptions
|
||||
{
|
||||
/// <summary>
|
||||
/// 获取或设置对外暴露的服务端版本号。
|
||||
/// </summary>
|
||||
public string ServerVersion { get; set; } = "flyshot-replacement-controller-client-compat/0.1.0";
|
||||
|
||||
/// <summary>
|
||||
/// 获取或设置运行配置根目录；为空时默认使用程序基目录下的 Config。
|
||||
/// </summary>
|
||||
public string? ConfigRoot { get; set; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取或设置旧父工作区根目录；仅用于测试或旧样本显式兼容。
|
||||
/// </summary>
|
||||
public string? WorkspaceRoot { get; set; }
|
||||
|
||||
/// <summary>
|
||||
/// 解析运行配置根目录，确保运行时默认不再依赖源码仓库位置。
|
||||
/// </summary>
|
||||
/// <returns>运行配置根目录的绝对路径。</returns>
|
||||
public string ResolveConfigRoot()
|
||||
{
|
||||
var root = string.IsNullOrWhiteSpace(ConfigRoot)
|
||||
? Path.Combine(AppContext.BaseDirectory, "Config")
|
||||
: ConfigRoot;
|
||||
|
||||
return Path.GetFullPath(root);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 解析显式配置的旧父工作区根目录；未配置时返回 null。
|
||||
/// </summary>
|
||||
/// <returns>旧父工作区根目录的绝对路径，或 null。</returns>
|
||||
public string? ResolveLegacyWorkspaceRoot()
|
||||
{
|
||||
return string.IsNullOrWhiteSpace(WorkspaceRoot)
|
||||
? null
|
||||
: Path.GetFullPath(WorkspaceRoot);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,74 @@
|
||||
using Flyshot.Core.Config;
|
||||
using Flyshot.Core.Domain;
|
||||
|
||||
namespace Flyshot.ControllerClientCompat;
|
||||
|
||||
/// <summary>
|
||||
/// 根据旧版 ControllerClient 的机器人名称，解析当前 replacement 仓库支持的真实模型文件。
|
||||
/// </summary>
|
||||
public sealed class ControllerClientCompatRobotCatalog
|
||||
{
|
||||
/// <summary>
|
||||
/// 保存当前现场支持的机器人名称到运行目录 JSON 模型文件名映射。
|
||||
/// </summary>
|
||||
private static readonly IReadOnlyDictionary<string, string> SupportedRobotModelFileMap = new Dictionary<string, string>(StringComparer.Ordinal)
|
||||
{
|
||||
["FANUC_LR_Mate_200iD"] = "LR_Mate_200iD_7L.json",
|
||||
["FANUC_LR_Mate_200iD_7L"] = "LR_Mate_200iD_7L.json"
|
||||
};
|
||||
|
||||
private readonly ControllerClientCompatOptions _options;
|
||||
private readonly RobotModelLoader _robotModelLoader;
|
||||
|
||||
/// <summary>
|
||||
/// 初始化机器人兼容目录解析器。
|
||||
/// </summary>
|
||||
/// <param name="options">兼容层基础配置。</param>
|
||||
/// <param name="robotModelLoader">机器人 JSON 模型加载器。</param>
|
||||
public ControllerClientCompatRobotCatalog(
|
||||
ControllerClientCompatOptions options,
|
||||
RobotModelLoader robotModelLoader)
|
||||
{
|
||||
_options = options ?? throw new ArgumentNullException(nameof(options));
|
||||
_robotModelLoader = robotModelLoader ?? throw new ArgumentNullException(nameof(robotModelLoader));
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 根据旧客户端的机器人名称加载对应模型。
|
||||
/// </summary>
|
||||
/// <param name="robotName">旧客户端传入的机器人名称。</param>
|
||||
/// <param name="accLimitScale">RobotConfig.json 中的加速度倍率。</param>
|
||||
/// <param name="jerkLimitScale">RobotConfig.json 中的 jerk 倍率。</param>
|
||||
/// <returns>兼容层加载出的机器人模型。</returns>
|
||||
public RobotProfile LoadProfile(string robotName, double accLimitScale = 1.0, double jerkLimitScale = 1.0)
|
||||
{
|
||||
if (string.IsNullOrWhiteSpace(robotName))
|
||||
{
|
||||
throw new ArgumentException("机器人名称不能为空。", nameof(robotName));
|
||||
}
|
||||
|
||||
if (!SupportedRobotModelFileMap.TryGetValue(robotName, out var modelFileName))
|
||||
{
|
||||
throw new InvalidOperationException($"Unsupported robot name: {robotName}");
|
||||
}
|
||||
|
||||
var modelPath = ResolveModelPath(modelFileName);
|
||||
return _robotModelLoader.LoadProfile(modelPath, accLimitScale, jerkLimitScale);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 解析机器人模型路径，只从运行目录 Config/Models 读取当前现场固化的 JSON 模型。
|
||||
/// </summary>
|
||||
/// <param name="modelFileName">运行目录 Config/Models 下的机器人 JSON 模型文件名。</param>
|
||||
/// <returns>可传给 JSON 模型加载器的模型文件绝对路径。</returns>
|
||||
private string ResolveModelPath(string modelFileName)
|
||||
{
|
||||
var configModelPath = Path.Combine(_options.ResolveConfigRoot(), "Models", modelFileName);
|
||||
if (File.Exists(configModelPath))
|
||||
{
|
||||
return configModelPath;
|
||||
}
|
||||
|
||||
return "Not found";
|
||||
}
|
||||
}
|
||||
1149
src/Flyshot.ControllerClientCompat/ControllerClientCompatService.cs
Normal file
1149
src/Flyshot.ControllerClientCompat/ControllerClientCompatService.cs
Normal file
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,42 @@
|
||||
using Flyshot.Core.Config;
|
||||
using Flyshot.Runtime.Common;
|
||||
using Flyshot.Runtime.Fanuc;
|
||||
using Microsoft.Extensions.Configuration;
|
||||
using Microsoft.Extensions.DependencyInjection;
|
||||
using Microsoft.Extensions.Options;
|
||||
|
||||
namespace Flyshot.ControllerClientCompat;
|
||||
|
||||
/// <summary>
|
||||
/// 提供 ControllerClient HTTP 兼容层的依赖注入注册入口。
|
||||
/// </summary>
|
||||
public static class ControllerClientCompatServiceCollectionExtensions
|
||||
{
|
||||
/// <summary>
|
||||
/// 将 HTTP-only 的 ControllerClient 兼容服务注册到当前宿主。
|
||||
/// </summary>
|
||||
/// <param name="services">当前宿主服务集合。</param>
|
||||
/// <param name="configuration">宿主配置根。</param>
|
||||
/// <returns>同一服务集合，便于链式调用。</returns>
|
||||
public static IServiceCollection AddControllerClientCompat(this IServiceCollection services, IConfiguration configuration)
|
||||
{
|
||||
ArgumentNullException.ThrowIfNull(services);
|
||||
ArgumentNullException.ThrowIfNull(configuration);
|
||||
|
||||
services
|
||||
.AddOptions<ControllerClientCompatOptions>()
|
||||
.Bind(configuration.GetSection("ControllerClientCompat"));
|
||||
|
||||
services.AddSingleton(static serviceProvider => serviceProvider.GetRequiredService<IOptions<ControllerClientCompatOptions>>().Value);
|
||||
services.AddSingleton<RobotModelLoader>();
|
||||
services.AddSingleton<RobotConfigLoader>();
|
||||
services.AddSingleton<ControllerClientCompatRobotCatalog>();
|
||||
services.AddSingleton<ControllerClientTrajectoryOrchestrator>();
|
||||
services.AddSingleton<FlyshotTrajectoryArtifactWriter>();
|
||||
services.AddSingleton<JsonFlyshotTrajectoryStore>();
|
||||
services.AddSingleton<IControllerRuntime, FanucControllerRuntime>();
|
||||
services.AddSingleton<IControllerClientCompatService, ControllerClientCompatService>();
|
||||
|
||||
return services;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,64 @@
|
||||
namespace Flyshot.ControllerClientCompat;
|
||||
|
||||
/// <summary>
|
||||
/// 保存一条已上传到兼容层内存目录中的飞拍轨迹，供 HTTP API 层列出、执行和删除。
|
||||
/// </summary>
|
||||
public sealed class ControllerClientCompatUploadedTrajectory
|
||||
{
|
||||
/// <summary>
|
||||
/// 初始化一条飞拍轨迹快照，并立即复制所有数组，避免调用方后续原地修改。
|
||||
/// </summary>
|
||||
/// <param name="name">轨迹名称。</param>
|
||||
/// <param name="waypoints">关节路点集合。</param>
|
||||
/// <param name="shotFlags">拍摄标志集合。</param>
|
||||
/// <param name="offsetValues">偏移周期集合。</param>
|
||||
/// <param name="addressGroups">地址组集合。</param>
|
||||
public ControllerClientCompatUploadedTrajectory(
|
||||
string name,
|
||||
IEnumerable<IReadOnlyList<double>> waypoints,
|
||||
IEnumerable<bool> shotFlags,
|
||||
IEnumerable<int> offsetValues,
|
||||
IEnumerable<IReadOnlyList<int>> addressGroups)
|
||||
{
|
||||
if (string.IsNullOrWhiteSpace(name))
|
||||
{
|
||||
throw new ArgumentException("轨迹名称不能为空。", nameof(name));
|
||||
}
|
||||
|
||||
ArgumentNullException.ThrowIfNull(waypoints);
|
||||
ArgumentNullException.ThrowIfNull(shotFlags);
|
||||
ArgumentNullException.ThrowIfNull(offsetValues);
|
||||
ArgumentNullException.ThrowIfNull(addressGroups);
|
||||
|
||||
Name = name;
|
||||
Waypoints = waypoints.Select(static waypoint => waypoint.ToArray()).ToArray();
|
||||
ShotFlags = shotFlags.ToArray();
|
||||
OffsetValues = offsetValues.ToArray();
|
||||
AddressGroups = addressGroups.Select(static group => group.ToArray()).ToArray();
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 获取轨迹名称。
|
||||
/// </summary>
|
||||
public string Name { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取已复制的关节路点集合。
|
||||
/// </summary>
|
||||
public IReadOnlyList<double[]> Waypoints { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取拍摄标志集合。
|
||||
/// </summary>
|
||||
public IReadOnlyList<bool> ShotFlags { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取偏移周期集合。
|
||||
/// </summary>
|
||||
public IReadOnlyList<int> OffsetValues { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取地址组集合。
|
||||
/// </summary>
|
||||
public IReadOnlyList<int[]> AddressGroups { get; }
|
||||
}
|
||||
@@ -0,0 +1,62 @@
|
||||
namespace Flyshot.ControllerClientCompat;
|
||||
|
||||
/// <summary>
|
||||
/// 保存状态页需要的 ControllerClient 兼容层元数据快照，避免状态页连续读取多个受保护字段。
|
||||
/// </summary>
|
||||
public sealed record ControllerClientStatusSnapshotMetadata
|
||||
{
|
||||
/// <summary>
|
||||
/// 初始化一份兼容层状态元数据快照。
|
||||
/// </summary>
|
||||
/// <param name="isSetup">当前是否已经完成机器人初始化。</param>
|
||||
/// <param name="robotName">当前机器人名称；未初始化时为空。</param>
|
||||
/// <param name="degreesOfFreedom">当前机器人自由度；未初始化时为 0。</param>
|
||||
/// <param name="uploadedTrajectories">当前已上传轨迹名称快照。</param>
|
||||
/// <param name="serverVersion">兼容服务端版本。</param>
|
||||
/// <param name="clientVersion">兼容客户端版本。</param>
|
||||
public ControllerClientStatusSnapshotMetadata(
|
||||
bool isSetup,
|
||||
string? robotName,
|
||||
int degreesOfFreedom,
|
||||
IReadOnlyList<string> uploadedTrajectories,
|
||||
string serverVersion,
|
||||
string clientVersion)
|
||||
{
|
||||
IsSetup = isSetup;
|
||||
RobotName = robotName;
|
||||
DegreesOfFreedom = degreesOfFreedom;
|
||||
UploadedTrajectories = uploadedTrajectories.ToArray();
|
||||
ServerVersion = serverVersion;
|
||||
ClientVersion = clientVersion;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 获取当前是否已经完成机器人初始化。
|
||||
/// </summary>
|
||||
public bool IsSetup { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取当前机器人名称；未初始化时为空。
|
||||
/// </summary>
|
||||
public string? RobotName { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取当前机器人自由度；未初始化时为 0。
|
||||
/// </summary>
|
||||
public int DegreesOfFreedom { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取已上传轨迹名称数组快照。
|
||||
/// </summary>
|
||||
public IReadOnlyList<string> UploadedTrajectories { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取兼容服务端版本。
|
||||
/// </summary>
|
||||
public string ServerVersion { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取兼容客户端版本。
|
||||
/// </summary>
|
||||
public string ClientVersion { get; }
|
||||
}
|
||||
@@ -0,0 +1,639 @@
|
||||
using Flyshot.Core.Config;
|
||||
using Flyshot.Core.Domain;
|
||||
using Flyshot.Core.Planning;
|
||||
using Flyshot.Core.Planning.Sampling;
|
||||
using Flyshot.Core.Triggering;
|
||||
using Microsoft.Extensions.Logging;
|
||||
|
||||
namespace Flyshot.ControllerClientCompat;
|
||||
|
||||
/// <summary>
|
||||
/// 负责把 ControllerClient 兼容层的轨迹输入转换为规划结果和触发时间轴。
|
||||
/// </summary>
|
||||
public sealed class ControllerClientTrajectoryOrchestrator
|
||||
{
|
||||
/// <summary>
|
||||
/// 稠密轨迹离散限幅失败后允许统一拉长时间轴的最大次数。
|
||||
/// </summary>
|
||||
private const int MaxDenseLimitStretchIterations = 100;
|
||||
|
||||
/// <summary>
|
||||
/// 每次离散限幅失败后统一放大的时间倍率。
|
||||
/// </summary>
|
||||
private const double DenseLimitStretchFactor = 1.01;
|
||||
|
||||
/// <summary>
|
||||
/// 平滑起停重定时混合系数：0 表示不平滑，1 表示完全使用平滑时间律。
|
||||
/// 当前取值用于“弱平滑”，仅轻微拉伸首尾时间段，避免起停过慢。
|
||||
/// </summary>
|
||||
private const double SmoothStartStopBlend = 0.60;
|
||||
|
||||
private readonly ICspPlanner _icspPlanner;
|
||||
private readonly SelfAdaptIcspPlanner _selfAdaptIcspPlanner;
|
||||
private readonly ShotTimelineBuilder _shotTimelineBuilder = new(new WaypointTimestampResolver());
|
||||
private readonly Dictionary<string, PlannedExecutionBundle> _flyshotCache = new(StringComparer.Ordinal);
|
||||
private readonly ILogger<ControllerClientTrajectoryOrchestrator>? _logger;
|
||||
|
||||
/// <summary>
|
||||
/// 初始化轨迹编排器。
|
||||
/// </summary>
|
||||
/// <param name="logger">日志记录器；允许 null。</param>
|
||||
/// <param name="loggerFactory">日志工厂；允许 null。</param>
|
||||
public ControllerClientTrajectoryOrchestrator(
|
||||
ILogger<ControllerClientTrajectoryOrchestrator>? logger = null,
|
||||
ILoggerFactory? loggerFactory = null)
|
||||
{
|
||||
_logger = logger;
|
||||
_icspPlanner = new(logger: loggerFactory?.CreateLogger<ICspPlanner>());
|
||||
_selfAdaptIcspPlanner = new(logger: loggerFactory?.CreateLogger<SelfAdaptIcspPlanner>());
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 对普通轨迹执行 ICSP 规划。
|
||||
/// </summary>
|
||||
/// <param name="robot">当前机器人配置。</param>
|
||||
/// <param name="waypoints">普通轨迹关节路点。</param>
|
||||
/// <param name="options">执行参数。</param>
|
||||
/// <param name="planningSpeedScale">规划速度倍率。</param>
|
||||
/// <param name="speedRatio">本次执行速度倍率；普通轨迹在规划阶段折算进有效限速倍率。</param>
|
||||
/// <returns>包含规划轨迹、空触发时间轴和执行结果的结果包。</returns>
|
||||
public PlannedExecutionBundle PlanOrdinaryTrajectory(
|
||||
RobotProfile robot,
|
||||
IReadOnlyList<IReadOnlyList<double>> waypoints,
|
||||
TrajectoryExecutionOptions? options = null,
|
||||
double planningSpeedScale = 1.0,
|
||||
double speedRatio = 1.0)
|
||||
{
|
||||
ArgumentNullException.ThrowIfNull(robot);
|
||||
ArgumentNullException.ThrowIfNull(waypoints);
|
||||
options ??= new TrajectoryExecutionOptions();
|
||||
var effectivePlanningSpeedScale = ResolveEffectivePlanningSpeedScale(planningSpeedScale, speedRatio, "普通轨迹");
|
||||
var planningRobot = ApplyPlanningSpeedScale(robot, effectivePlanningSpeedScale);
|
||||
|
||||
_logger?.LogInformation(
|
||||
"PlanOrdinaryTrajectory 开始: 路点数={WaypointCount}, method={Method}, planningSpeedScale={PlanningSpeedScale}, speedRatio={SpeedRatio}, effectivePlanningSpeedScale={EffectivePlanningSpeedScale}",
|
||||
waypoints.Count, options.Method, planningSpeedScale, speedRatio, effectivePlanningSpeedScale);
|
||||
|
||||
var program = CreateProgram(
|
||||
name: "ordinary-trajectory",
|
||||
waypoints: waypoints,
|
||||
shotFlags: Enumerable.Repeat(false, waypoints.Count),
|
||||
offsetValues: Enumerable.Repeat(0, waypoints.Count),
|
||||
addressGroups: Enumerable.Range(0, waypoints.Count).Select(static _ => Array.Empty<int>()));
|
||||
|
||||
var method = ParseOrdinaryMethod(options.Method);
|
||||
var request = new TrajectoryRequest(
|
||||
robot: planningRobot,
|
||||
program: program,
|
||||
method: method,
|
||||
saveTrajectoryArtifacts: options.SaveTrajectory);
|
||||
|
||||
var plannedTrajectory = PlanByMethod(request, method);
|
||||
var executionTrajectory = plannedTrajectory;
|
||||
var denseJointTrajectory = CreateLimitCompliantDenseTrajectory(ref executionTrajectory, shapeTrajectoryEdges: false);
|
||||
var shotTimeline = new ShotTimeline(Array.Empty<ShotEvent>(), Array.Empty<TrajectoryDoEvent>());
|
||||
var result = CreateResult(
|
||||
executionTrajectory,
|
||||
shotTimeline,
|
||||
denseJointTrajectory,
|
||||
usedCache: false,
|
||||
triggerSampleIndexOffsetCycles: 0);
|
||||
|
||||
_logger?.LogInformation(
|
||||
"PlanOrdinaryTrajectory 完成: 时长={Duration}s, 采样点数={SampleCount}",
|
||||
result.Duration.TotalSeconds,
|
||||
result.DenseJointTrajectory?.Count ?? 0);
|
||||
|
||||
return new PlannedExecutionBundle(plannedTrajectory, shotTimeline, result, executionTrajectory: executionTrajectory);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 对已经上传的飞拍轨迹执行自适应 ICSP 规划并生成触发时间轴。
|
||||
/// </summary>
|
||||
/// <param name="robot">当前机器人配置。</param>
|
||||
/// <param name="uploaded">兼容层保存的上传轨迹。</param>
|
||||
/// <param name="options">执行参数。</param>
|
||||
/// <param name="settings">兼容层机器人设置。</param>
|
||||
/// <param name="planningSpeedScale">规划速度倍率。</param>
|
||||
/// <param name="speedRatio">本次飞拍使用的速度倍率；为空时使用配置默认值。</param>
|
||||
/// <returns>包含规划轨迹、触发时间轴和执行结果的结果包。</returns>
|
||||
public PlannedExecutionBundle PlanUploadedFlyshot(
|
||||
RobotProfile robot,
|
||||
ControllerClientCompatUploadedTrajectory uploaded,
|
||||
FlyshotExecutionOptions? options = null,
|
||||
CompatibilityRobotSettings? settings = null,
|
||||
double? planningSpeedScale = null,
|
||||
double? speedRatio = null)
|
||||
{
|
||||
ArgumentNullException.ThrowIfNull(robot);
|
||||
ArgumentNullException.ThrowIfNull(uploaded);
|
||||
options ??= new FlyshotExecutionOptions();
|
||||
settings ??= CreateDefaultRobotSettings();
|
||||
var requestedSpeedRatio = speedRatio ?? settings.SpeedRatio;
|
||||
var configuredPlanningSpeedScale = planningSpeedScale ?? settings.PlanningSpeedScale;
|
||||
var effectivePlanningSpeedScale = ResolveEffectivePlanningSpeedScale(configuredPlanningSpeedScale, requestedSpeedRatio, "飞拍");
|
||||
var planningRobot = ApplyPlanningSpeedScale(robot, effectivePlanningSpeedScale);
|
||||
|
||||
_logger?.LogInformation(
|
||||
"PlanUploadedFlyshot 开始: name={Name}, waypoints={WaypointCount}, method={Method}, useCache={UseCache}, planningSpeedScale={PlanningSpeedScale}, speedRatio={SpeedRatio}, effectivePlanningSpeedScale={EffectivePlanningSpeedScale}, smoothStartStopTiming={SmoothStartStopTiming}",
|
||||
uploaded.Name,
|
||||
uploaded.Waypoints.Count,
|
||||
options.Method,
|
||||
options.UseCache,
|
||||
configuredPlanningSpeedScale,
|
||||
requestedSpeedRatio,
|
||||
effectivePlanningSpeedScale,
|
||||
settings.SmoothStartStopTiming);
|
||||
|
||||
var program = CreateProgram(
|
||||
name: uploaded.Name,
|
||||
waypoints: uploaded.Waypoints,
|
||||
shotFlags: uploaded.ShotFlags,
|
||||
offsetValues: uploaded.OffsetValues,
|
||||
addressGroups: uploaded.AddressGroups);
|
||||
|
||||
var method = ParseFlyshotMethod(options.Method);
|
||||
var cacheKey = CreateFlyshotCacheKey(planningRobot, uploaded, options, settings, effectivePlanningSpeedScale);
|
||||
if (options.UseCache && _flyshotCache.TryGetValue(cacheKey, out var cachedBundle))
|
||||
{
|
||||
_logger?.LogInformation("PlanUploadedFlyshot 命中缓存: name={Name}, cacheKey={CacheKey}", uploaded.Name, cacheKey);
|
||||
|
||||
// 命中缓存时只替换 TrajectoryResult 的 usedCache 标志，规划轨迹和触发时间轴保持不可变复用。
|
||||
return cachedBundle;
|
||||
}
|
||||
|
||||
var request = new TrajectoryRequest(
|
||||
robot: planningRobot,
|
||||
program: program,
|
||||
method: method,
|
||||
moveToStart: options.MoveToStart,
|
||||
saveTrajectoryArtifacts: options.SaveTrajectory,
|
||||
useCache: options.UseCache);
|
||||
|
||||
var plannedTrajectory = PlanByMethod(request, method, settings);
|
||||
var smoothedExecutionTrajectory = ApplyExecutionTiming(plannedTrajectory, settings);
|
||||
var denseJointTrajectory = CreateLimitCompliantDenseTrajectory(ref smoothedExecutionTrajectory, shapeTrajectoryEdges: false);
|
||||
var shotTimeline = _shotTimelineBuilder.Build(
|
||||
smoothedExecutionTrajectory,
|
||||
holdCycles: settings.IoKeepCycles,
|
||||
samplePeriod: planningRobot.ServoPeriod,
|
||||
useDo: settings.UseDo);
|
||||
var result = CreateResult(
|
||||
smoothedExecutionTrajectory,
|
||||
shotTimeline,
|
||||
denseJointTrajectory,
|
||||
usedCache: false,
|
||||
triggerSampleIndexOffsetCycles: settings.TriggerSampleIndexOffsetCycles);
|
||||
var bundle = new PlannedExecutionBundle(plannedTrajectory, shotTimeline, result, executionTrajectory: smoothedExecutionTrajectory);
|
||||
|
||||
_logger?.LogInformation(
|
||||
"PlanUploadedFlyshot 完成: name={Name}, 时长={Duration}s, 触发事件数={TriggerCount}, 采样点数={SampleCount}",
|
||||
uploaded.Name, result.Duration.TotalSeconds, result.TriggerTimeline.Count, result.DenseJointTrajectory?.Count ?? 0);
|
||||
|
||||
if (options.UseCache)
|
||||
{
|
||||
_flyshotCache[cacheKey] = bundle;
|
||||
}
|
||||
|
||||
return bundle;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 按普通轨迹执行接口约束解析 method 参数。
|
||||
/// </summary>
|
||||
/// <param name="method">旧 SDK 传入的方法名。</param>
|
||||
/// <returns>领域层规划方法。</returns>
|
||||
private static PlanningMethod ParseOrdinaryMethod(string method)
|
||||
{
|
||||
var normalized = NormalizeMethod(method);
|
||||
return normalized switch
|
||||
{
|
||||
"icsp" => PlanningMethod.Icsp,
|
||||
"doubles" => PlanningMethod.Doubles,
|
||||
_ => throw new ArgumentException($"Unsupported ExecuteTrajectory method: {method}", nameof(method))
|
||||
};
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 按飞拍轨迹执行接口约束解析 method 参数。
|
||||
/// </summary>
|
||||
/// <param name="method">旧 SDK 传入的方法名。</param>
|
||||
/// <returns>领域层规划方法。</returns>
|
||||
private static PlanningMethod ParseFlyshotMethod(string method)
|
||||
{
|
||||
var normalized = NormalizeMethod(method);
|
||||
return normalized switch
|
||||
{
|
||||
"icsp" => PlanningMethod.Icsp,
|
||||
"self-adapt-icsp" => PlanningMethod.SelfAdaptIcsp,
|
||||
"doubles" => PlanningMethod.Doubles,
|
||||
_ => throw new ArgumentException($"Unsupported ExecuteFlyShotTraj method: {method}", nameof(method))
|
||||
};
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 按领域枚举分派到当前已经落地的规划器。
|
||||
/// </summary>
|
||||
/// <param name="request">规划请求。</param>
|
||||
/// <param name="method">规划方法。</param>
|
||||
/// <returns>规划轨迹。</returns>
|
||||
private PlannedTrajectory PlanByMethod(TrajectoryRequest request, PlanningMethod method, CompatibilityRobotSettings? settings = null)
|
||||
{
|
||||
return method switch
|
||||
{
|
||||
PlanningMethod.Icsp => _icspPlanner.Plan(request),
|
||||
PlanningMethod.SelfAdaptIcsp => _selfAdaptIcspPlanner.Plan(request, settings?.AdaptIcspTryNum ?? 5),
|
||||
PlanningMethod.Doubles => throw new NotSupportedException("doubles 轨迹规划尚未落地。"),
|
||||
_ => throw new ArgumentOutOfRangeException(nameof(method), method, "未知轨迹规划方法。")
|
||||
};
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 归一化旧 SDK 的 method 字符串。
|
||||
/// </summary>
|
||||
/// <param name="method">原始方法名。</param>
|
||||
/// <returns>小写短横线方法名。</returns>
|
||||
private static string NormalizeMethod(string method)
|
||||
{
|
||||
if (string.IsNullOrWhiteSpace(method))
|
||||
{
|
||||
return "icsp";
|
||||
}
|
||||
|
||||
return method.Trim().ToLowerInvariant();
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 为已上传飞拍轨迹构造包含参数和轨迹内容的缓存键，避免同名覆盖后误用旧规划结果。
|
||||
/// </summary>
|
||||
/// <param name="robot">机器人配置。</param>
|
||||
/// <param name="uploaded">上传轨迹。</param>
|
||||
/// <param name="options">执行参数。</param>
|
||||
/// <returns>缓存键。</returns>
|
||||
private static string CreateFlyshotCacheKey(
|
||||
RobotProfile robot,
|
||||
ControllerClientCompatUploadedTrajectory uploaded,
|
||||
FlyshotExecutionOptions options,
|
||||
CompatibilityRobotSettings settings,
|
||||
double planningSpeedScale)
|
||||
{
|
||||
var hash = new HashCode();
|
||||
hash.Add(robot.Name, StringComparer.Ordinal);
|
||||
hash.Add(planningSpeedScale);
|
||||
hash.Add(uploaded.Name, StringComparer.Ordinal);
|
||||
hash.Add(NormalizeMethod(options.Method), StringComparer.Ordinal);
|
||||
hash.Add(options.MoveToStart);
|
||||
hash.Add(options.SaveTrajectory);
|
||||
hash.Add(settings.UseDo);
|
||||
hash.Add(settings.IoKeepCycles);
|
||||
hash.Add(settings.TriggerSampleIndexOffsetCycles);
|
||||
hash.Add(settings.AdaptIcspTryNum);
|
||||
hash.Add(settings.SmoothStartStopTiming);
|
||||
|
||||
foreach (var limit in robot.JointLimits)
|
||||
{
|
||||
hash.Add(limit.JointName, StringComparer.Ordinal);
|
||||
hash.Add(limit.VelocityLimit);
|
||||
hash.Add(limit.AccelerationLimit);
|
||||
hash.Add(limit.JerkLimit);
|
||||
}
|
||||
|
||||
foreach (var waypoint in uploaded.Waypoints)
|
||||
{
|
||||
foreach (var value in waypoint)
|
||||
{
|
||||
hash.Add(value);
|
||||
}
|
||||
}
|
||||
|
||||
foreach (var flag in uploaded.ShotFlags)
|
||||
{
|
||||
hash.Add(flag);
|
||||
}
|
||||
|
||||
foreach (var offset in uploaded.OffsetValues)
|
||||
{
|
||||
hash.Add(offset);
|
||||
}
|
||||
|
||||
foreach (var group in uploaded.AddressGroups)
|
||||
{
|
||||
foreach (var address in group)
|
||||
{
|
||||
hash.Add(address);
|
||||
}
|
||||
}
|
||||
|
||||
return hash.ToHashCode().ToString("X8");
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 构造编排器直接调用时的默认兼容配置，保持既有单元测试中的 DO 生成行为。
|
||||
/// </summary>
|
||||
/// <returns>默认机器人兼容配置。</returns>
|
||||
private static CompatibilityRobotSettings CreateDefaultRobotSettings()
|
||||
{
|
||||
return new CompatibilityRobotSettings(
|
||||
useDo: true,
|
||||
ioAddresses: Array.Empty<int>(),
|
||||
ioKeepCycles: 0,
|
||||
triggerSampleIndexOffsetCycles: 7,
|
||||
accLimitScale: 1.0,
|
||||
jerkLimitScale: 1.0,
|
||||
adaptIcspTryNum: 5);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 解析飞拍生成轨迹时真正使用的规划速度倍率。
|
||||
/// </summary>
|
||||
/// <param name="planningSpeedScale">配置或调用方传入的规划倍率。</param>
|
||||
/// <param name="speedRatio">本次执行速度倍率。</param>
|
||||
/// <returns>用于缩放规划关节限制的有效倍率。</returns>
|
||||
private static double ResolveEffectivePlanningSpeedScale(double planningSpeedScale, double speedRatio, string trajectoryKind)
|
||||
{
|
||||
if (speedRatio <= 0.0 || double.IsNaN(speedRatio) || double.IsInfinity(speedRatio))
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(nameof(speedRatio), $"{trajectoryKind}速度倍率必须是有限正数。");
|
||||
}
|
||||
|
||||
// 生成轨迹时把执行倍率折算进规划限制，保证 planning_speed_scale * speed_ratio 相同的配置产出同一条轨迹。
|
||||
return planningSpeedScale * speedRatio;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 按运行配置决定是否对规划结果做执行前时间轴重映射。
|
||||
/// </summary>
|
||||
/// <param name="plannedTrajectory">规划阶段得到的轨迹。</param>
|
||||
/// <param name="settings">当前 RobotConfig.json 解析出的兼容设置。</param>
|
||||
/// <returns>运行时真正用于采样和触发的轨迹。</returns>
|
||||
private static PlannedTrajectory ApplyExecutionTiming(PlannedTrajectory plannedTrajectory, CompatibilityRobotSettings settings)
|
||||
{
|
||||
// legacy-fit 模式需要严格保留 waypoint.txt 反推出的节点时间，不能再二次改写时间轴。
|
||||
return settings.SmoothStartStopTiming
|
||||
? ApplySmoothStartStopTiming(plannedTrajectory)
|
||||
: plannedTrajectory;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 按规划全局速度倍率生成规划专用机器人约束。
|
||||
/// </summary>
|
||||
/// <param name="robot">原始机器人约束。</param>
|
||||
/// <param name="planningSpeedScale">规划阶段的全局速度倍率，1.0 表示不额外缩放。</param>
|
||||
/// <returns>已按速度倍率缩放后的规划机器人约束。</returns>
|
||||
private static RobotProfile ApplyPlanningSpeedScale(RobotProfile robot, double planningSpeedScale)
|
||||
{
|
||||
if (double.IsNaN(planningSpeedScale) || double.IsInfinity(planningSpeedScale) || planningSpeedScale <= 0.0)
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(nameof(planningSpeedScale), "规划速度倍率必须是有限正数。");
|
||||
}
|
||||
|
||||
if (Math.Abs(planningSpeedScale - 1.0) < 1e-12)
|
||||
{
|
||||
return robot;
|
||||
}
|
||||
|
||||
// speed_ratio 已在调用方折算到规划倍率中；运行时只消费已经生成好的 8ms 稠密轨迹。
|
||||
var scaledLimits = robot.JointLimits
|
||||
.Select(limit => new JointLimit(
|
||||
limit.JointName,
|
||||
limit.VelocityLimit * planningSpeedScale,
|
||||
limit.AccelerationLimit * planningSpeedScale * planningSpeedScale,
|
||||
limit.JerkLimit * planningSpeedScale * planningSpeedScale * planningSpeedScale))
|
||||
.ToArray();
|
||||
|
||||
return new RobotProfile(
|
||||
name: robot.Name,
|
||||
modelPath: robot.ModelPath,
|
||||
degreesOfFreedom: robot.DegreesOfFreedom,
|
||||
jointLimits: scaledLimits,
|
||||
jointCouplings: robot.JointCouplings,
|
||||
servoPeriod: robot.ServoPeriod,
|
||||
triggerPeriod: robot.TriggerPeriod);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 把兼容层输入数组转换成领域层 FlyshotProgram。
|
||||
/// </summary>
|
||||
/// <param name="name">轨迹名称。</param>
|
||||
/// <param name="waypoints">关节路点。</param>
|
||||
/// <param name="shotFlags">拍照标志。</param>
|
||||
/// <param name="offsetValues">偏移周期。</param>
|
||||
/// <param name="addressGroups">IO 地址组。</param>
|
||||
/// <returns>领域层飞拍程序。</returns>
|
||||
private static FlyshotProgram CreateProgram(
|
||||
string name,
|
||||
IEnumerable<IReadOnlyList<double>> waypoints,
|
||||
IEnumerable<bool> shotFlags,
|
||||
IEnumerable<int> offsetValues,
|
||||
IEnumerable<IReadOnlyList<int>> addressGroups)
|
||||
{
|
||||
return new FlyshotProgram(
|
||||
name: name,
|
||||
waypoints: waypoints.Select(static waypoint => new JointWaypoint(waypoint)).ToArray(),
|
||||
shotFlags: shotFlags.ToArray(),
|
||||
offsetValues: offsetValues.ToArray(),
|
||||
addressGroups: addressGroups.Select(static group => new IoAddressGroup(group)).ToArray());
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 从规划轨迹和触发时间轴构造运行时可消费的稳定结果对象。
|
||||
/// </summary>
|
||||
/// <param name="plannedTrajectory">规划后的轨迹。</param>
|
||||
/// <param name="shotTimeline">触发时间轴。</param>
|
||||
/// <returns>运行时执行结果描述。</returns>
|
||||
private static TrajectoryResult CreateResult(
|
||||
PlannedTrajectory plannedTrajectory,
|
||||
ShotTimeline shotTimeline,
|
||||
IReadOnlyList<IReadOnlyList<double>> denseJointTrajectory,
|
||||
bool usedCache,
|
||||
int triggerSampleIndexOffsetCycles)
|
||||
{
|
||||
return new TrajectoryResult(
|
||||
programName: plannedTrajectory.OriginalProgram.Name,
|
||||
method: plannedTrajectory.Method,
|
||||
isValid: true,
|
||||
duration: TimeSpan.FromSeconds(plannedTrajectory.WaypointTimes[^1]),
|
||||
shotEvents: shotTimeline.ShotEvents,
|
||||
triggerTimeline: shotTimeline.TriggerTimeline,
|
||||
artifacts: Array.Empty<TrajectoryArtifact>(),
|
||||
failureReason: null,
|
||||
usedCache: usedCache,
|
||||
originalWaypointCount: plannedTrajectory.OriginalWaypointCount,
|
||||
plannedWaypointCount: plannedTrajectory.PlannedWaypointCount,
|
||||
triggerSampleIndexOffsetCycles: triggerSampleIndexOffsetCycles,
|
||||
denseJointTrajectory: denseJointTrajectory);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 生成满足离散速度、加速度和 Jerk 限制的稠密执行轨迹。
|
||||
/// </summary>
|
||||
private IReadOnlyList<IReadOnlyList<double>> CreateLimitCompliantDenseTrajectory(
|
||||
ref PlannedTrajectory executionTrajectory,
|
||||
bool shapeTrajectoryEdges)
|
||||
{
|
||||
for (var iteration = 0; iteration <= MaxDenseLimitStretchIterations; iteration++)
|
||||
{
|
||||
var denseJointTrajectory = TrajectorySampler.SampleJointTrajectory(
|
||||
executionTrajectory,
|
||||
samplePeriod: executionTrajectory.Robot.ServoPeriod.TotalSeconds,
|
||||
smoothStartStop: shapeTrajectoryEdges);
|
||||
|
||||
try
|
||||
{
|
||||
TrajectoryLimitValidator.ValidateDenseJointTrajectory(
|
||||
executionTrajectory.Robot,
|
||||
denseJointTrajectory,
|
||||
trajectoryName: executionTrajectory.OriginalProgram.Name,
|
||||
validateJerk: false);
|
||||
return denseJointTrajectory;
|
||||
}
|
||||
catch (InvalidOperationException ex) when (iteration < MaxDenseLimitStretchIterations)
|
||||
{
|
||||
_logger?.LogWarning(ex, "稠密轨迹离散限幅校验失败，准备拉长时间轴重试");
|
||||
// 离散差分超限时统一拉长时间轴，保持路点几何不变并降低速度、加速度和 Jerk。
|
||||
executionTrajectory = StretchTrajectoryTiming(executionTrajectory, DenseLimitStretchFactor);
|
||||
_logger?.LogInformation(
|
||||
"离散差分超限，拉长时间轴，iteration={Iteration}, factor={StretchFactor}",
|
||||
iteration,
|
||||
DenseLimitStretchFactor);
|
||||
_logger?.LogInformation("拉长之后的总时间={TotalTime}", executionTrajectory.WaypointTimes[^1]);
|
||||
}
|
||||
}
|
||||
|
||||
throw new InvalidOperationException("稠密轨迹离散限幅校验未能产生有效结果。");
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 按统一倍率拉长轨迹时间轴，保留原始路点和触发元数据。
|
||||
/// </summary>
|
||||
private PlannedTrajectory StretchTrajectoryTiming(PlannedTrajectory trajectory, double stretchFactor)
|
||||
{
|
||||
var waypointTimes = trajectory.WaypointTimes.Select(time => time * stretchFactor).ToArray();
|
||||
var segmentDurations = trajectory.SegmentDurations.Select(duration => duration * stretchFactor).ToArray();
|
||||
var segmentScales = trajectory.SegmentScales.Select(scale => scale / stretchFactor).ToArray();
|
||||
|
||||
return new PlannedTrajectory(
|
||||
robot: trajectory.Robot,
|
||||
originalProgram: trajectory.OriginalProgram,
|
||||
plannedWaypoints: trajectory.PlannedWaypoints,
|
||||
waypointTimes: waypointTimes,
|
||||
segmentDurations: segmentDurations,
|
||||
segmentScales: segmentScales,
|
||||
method: trajectory.Method,
|
||||
iterations: trajectory.Iterations,
|
||||
threshold: trajectory.Threshold);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 为飞拍执行生成平滑起停时间轴（仅重定时，不改几何路点）。
|
||||
/// 该方法保持 <see cref="PlannedTrajectory.PlannedWaypoints"/> 不变，只重映射 <see cref="PlannedTrajectory.WaypointTimes"/>，
|
||||
/// 让轨迹在起点和终点附近具有更柔和的速度变化，从而降低“突然起步/突然收尾”带来的离散差分尖峰。
|
||||
/// </summary>
|
||||
/// <param name="plannedTrajectory">规划器输出的原始轨迹（通常是线性时间轴）。</param>
|
||||
/// <returns>
|
||||
/// 若满足平滑条件，则返回新的重定时轨迹；若路点数量不足或总时长无效，则直接返回输入轨迹。
|
||||
/// </returns>
|
||||
private static PlannedTrajectory ApplySmoothStartStopTiming(PlannedTrajectory plannedTrajectory)
|
||||
{
|
||||
var originalTimes = plannedTrajectory.WaypointTimes;
|
||||
// 至少需要“起点-中间点-终点”三类点，才有可平滑的中间区间。
|
||||
if (originalTimes.Count < 3)
|
||||
{
|
||||
return plannedTrajectory;
|
||||
}
|
||||
|
||||
var totalDuration = originalTimes[^1];
|
||||
// 轨迹总时长必须为正，避免后续归一化进度出现除零或无意义映射。
|
||||
if (totalDuration <= 0.0)
|
||||
{
|
||||
return plannedTrajectory;
|
||||
}
|
||||
|
||||
var smoothedTimes = new double[originalTimes.Count];
|
||||
// 强制固定边界：起点仍在 0，终点仍在总时长，保证任务总耗时不变。
|
||||
smoothedTimes[0] = 0.0;
|
||||
smoothedTimes[^1] = totalDuration;
|
||||
|
||||
for (var index = 1; index < originalTimes.Count - 1; index++)
|
||||
{
|
||||
// 把原始时刻归一化到 [0, 1]，用于统一时间律变换。
|
||||
var normalizedProgress = originalTimes[index] / totalDuration;
|
||||
// 先求完整平滑时间律对应的时刻，再与原线性时刻按比例混合。
|
||||
// 这里采用弱平滑（blend=0.35）：保留大部分原节奏，仅轻微降低首尾突变。
|
||||
var smoothedProgress = InvertSmoothStartStopProgress(normalizedProgress);
|
||||
var blendedProgress = ((1.0 - SmoothStartStopBlend) * normalizedProgress)
|
||||
+ (SmoothStartStopBlend * smoothedProgress);
|
||||
smoothedTimes[index] = totalDuration * blendedProgress;
|
||||
}
|
||||
|
||||
var segmentDurations = new double[smoothedTimes.Length - 1];
|
||||
for (var index = 0; index < segmentDurations.Length; index++)
|
||||
{
|
||||
// 重建每段持续时间，供后续稠密采样和限幅检查使用。
|
||||
segmentDurations[index] = smoothedTimes[index + 1] - smoothedTimes[index];
|
||||
}
|
||||
|
||||
return new PlannedTrajectory(
|
||||
robot: plannedTrajectory.Robot,
|
||||
originalProgram: plannedTrajectory.OriginalProgram,
|
||||
plannedWaypoints: plannedTrajectory.PlannedWaypoints,
|
||||
waypointTimes: smoothedTimes,
|
||||
segmentDurations: segmentDurations,
|
||||
segmentScales: plannedTrajectory.SegmentScales,
|
||||
method: plannedTrajectory.Method,
|
||||
iterations: plannedTrajectory.Iterations,
|
||||
threshold: plannedTrajectory.Threshold);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 反解平滑时间律进度：给定目标进度 p，求满足 f(t)=p 的 t。
|
||||
/// 其中 f(t) 由 <see cref="EvaluateSmoothStartStopProgress"/> 给出，是单调递增的 7 次 smootherstep 曲线，
|
||||
/// 通过二分法可稳定得到对应的归一化时间，避免显式求高次方程根带来的复杂性和数值不稳定。
|
||||
/// </summary>
|
||||
/// <param name="normalizedProgress">目标归一化进度，理论范围 [0, 1]。</param>
|
||||
/// <returns>与目标进度对应的归一化时间，范围 [0, 1]。</returns>
|
||||
private static double InvertSmoothStartStopProgress(double normalizedProgress)
|
||||
{
|
||||
// 防御式裁剪，避免上游浮点误差将进度推到区间外。
|
||||
var target = Math.Clamp(normalizedProgress, 0.0, 1.0);
|
||||
var low = 0.0;
|
||||
var high = 1.0;
|
||||
// 固定迭代次数确保耗时可预测；40 次足以把区间收敛到工程可用精度。
|
||||
for (var iteration = 0; iteration < 40; iteration++)
|
||||
{
|
||||
var middle = (low + high) / 2.0;
|
||||
var progress = EvaluateSmoothStartStopProgress(middle);
|
||||
// f(middle) 小于目标，说明根在右半区间；否则在左半区间。
|
||||
if (progress < target)
|
||||
{
|
||||
low = middle;
|
||||
}
|
||||
else
|
||||
{
|
||||
high = middle;
|
||||
}
|
||||
}
|
||||
|
||||
return (low + high) / 2.0;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 计算 7 次 smootherstep 的进度函数值 f(u)。
|
||||
/// 该函数在 u=0 和 u=1 处具有更高阶导数连续性，可让起停段速度变化更平滑，
|
||||
/// 适合用于飞拍轨迹的时间重参数化，减少离散导数在边界处的突变。
|
||||
/// </summary>
|
||||
/// <param name="normalizedTime">归一化时间 u，理论范围 [0, 1]。</param>
|
||||
/// <returns>归一化进度 f(u)，范围 [0, 1]。</returns>
|
||||
private static double EvaluateSmoothStartStopProgress(double normalizedTime)
|
||||
{
|
||||
// 先裁剪再计算多项式，确保数值稳定且不受外部越界输入影响。
|
||||
var u = Math.Clamp(normalizedTime, 0.0, 1.0);
|
||||
var u2 = u * u;
|
||||
var u3 = u2 * u;
|
||||
var u4 = u3 * u;
|
||||
var u5 = u4 * u;
|
||||
var u6 = u5 * u;
|
||||
var u7 = u6 * u;
|
||||
return (35.0 * u4) - (84.0 * u5) + (70.0 * u6) - (20.0 * u7);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,22 @@
|
||||
<Project Sdk="Microsoft.NET.Sdk">
|
||||
|
||||
<ItemGroup>
|
||||
<ProjectReference Include="..\Flyshot.Core.Config\Flyshot.Core.Config.csproj" />
|
||||
<ProjectReference Include="..\Flyshot.Core.Domain\Flyshot.Core.Domain.csproj" />
|
||||
<ProjectReference Include="..\Flyshot.Core.Planning\Flyshot.Core.Planning.csproj" />
|
||||
<ProjectReference Include="..\Flyshot.Core.Triggering\Flyshot.Core.Triggering.csproj" />
|
||||
<ProjectReference Include="..\Flyshot.Runtime.Common\Flyshot.Runtime.Common.csproj" />
|
||||
<ProjectReference Include="..\Flyshot.Runtime.Fanuc\Flyshot.Runtime.Fanuc.csproj" />
|
||||
</ItemGroup>
|
||||
|
||||
<ItemGroup>
|
||||
<FrameworkReference Include="Microsoft.AspNetCore.App" />
|
||||
</ItemGroup>
|
||||
|
||||
<ItemGroup>
|
||||
<AssemblyAttribute Include="System.Runtime.CompilerServices.InternalsVisibleTo">
|
||||
<_Parameter1>Flyshot.Core.Tests</_Parameter1>
|
||||
</AssemblyAttribute>
|
||||
</ItemGroup>
|
||||
|
||||
</Project>
|
||||
@@ -0,0 +1,54 @@
|
||||
namespace Flyshot.ControllerClientCompat;
|
||||
|
||||
/// <summary>
|
||||
/// 表示飞拍轨迹执行接口的可选参数，字段名对齐旧 `ControllerClient::ExecuteFlyShotTraj`。
|
||||
/// </summary>
|
||||
public sealed class FlyshotExecutionOptions
|
||||
{
|
||||
/// <summary>
|
||||
/// 初始化飞拍轨迹执行参数。
|
||||
/// </summary>
|
||||
/// <param name="moveToStart">执行前是否自动移动到轨迹起点。</param>
|
||||
/// <param name="method">轨迹生成方法，支持 `icsp`、`doubles` 或 `self-adapt-icsp`。</param>
|
||||
/// <param name="saveTrajectory">是否保存轨迹信息。</param>
|
||||
/// <param name="useCache">是否优先复用已规划轨迹缓存。</param>
|
||||
/// <param name="wait">是否等待机器人执行完整条飞拍轨迹后再返回。</param>
|
||||
public FlyshotExecutionOptions(
|
||||
bool moveToStart = true,
|
||||
string method = "icsp",
|
||||
bool saveTrajectory = true,
|
||||
bool useCache = true,
|
||||
bool wait = true)
|
||||
{
|
||||
MoveToStart = moveToStart;
|
||||
Method = string.IsNullOrWhiteSpace(method) ? "icsp" : method;
|
||||
SaveTrajectory = saveTrajectory;
|
||||
UseCache = useCache;
|
||||
Wait = wait;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 获取执行前是否自动移动到轨迹起点。
|
||||
/// </summary>
|
||||
public bool MoveToStart { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取轨迹生成方法。
|
||||
/// </summary>
|
||||
public string Method { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取是否保存轨迹信息。
|
||||
/// </summary>
|
||||
public bool SaveTrajectory { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取是否优先复用已规划轨迹缓存。
|
||||
/// </summary>
|
||||
public bool UseCache { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取是否等待机器人执行完整条飞拍轨迹后再返回。
|
||||
/// </summary>
|
||||
public bool Wait { get; }
|
||||
}
|
||||
@@ -0,0 +1,341 @@
|
||||
using Flyshot.Core.Config;
|
||||
using Flyshot.Core.Domain;
|
||||
using Flyshot.Core.Planning;
|
||||
using Flyshot.Core.Planning.Export;
|
||||
using Flyshot.Core.Planning.Kinematics;
|
||||
using Flyshot.Core.Planning.Sampling;
|
||||
using Microsoft.Extensions.Logging;
|
||||
using System.Text;
|
||||
|
||||
namespace Flyshot.ControllerClientCompat;
|
||||
|
||||
/// <summary>
|
||||
/// 负责把 saveTrajectory 生成的规划结果点位写入运行目录 Config/Data。
|
||||
/// </summary>
|
||||
public sealed class FlyshotTrajectoryArtifactWriter
|
||||
{
|
||||
/// <summary>
|
||||
/// 旧 Data 明细点位文件使用的默认采样周期，单位为秒。
|
||||
/// </summary>
|
||||
private const double LegacyDetailSamplePeriodSeconds = 0.016;
|
||||
|
||||
/// <summary>
|
||||
/// FANUC J519 实际下发的固定伺服周期，单位为秒。
|
||||
/// </summary>
|
||||
private const double ActualSendServoPeriodSeconds = 0.008;
|
||||
|
||||
private readonly ControllerClientCompatOptions _options;
|
||||
private readonly RobotModelLoader _robotModelLoader;
|
||||
private readonly ILogger<FlyshotTrajectoryArtifactWriter>? _logger;
|
||||
|
||||
/// <summary>
|
||||
/// 初始化规划结果点位导出器。
|
||||
/// </summary>
|
||||
/// <param name="options">兼容层基础配置，用于定位运行配置根目录。</param>
|
||||
/// <param name="robotModelLoader">机器人模型加载器，用于生成笛卡尔点位。</param>
|
||||
/// <param name="logger">日志记录器；允许 null。</param>
|
||||
public FlyshotTrajectoryArtifactWriter(
|
||||
ControllerClientCompatOptions options,
|
||||
RobotModelLoader robotModelLoader,
|
||||
ILogger<FlyshotTrajectoryArtifactWriter>? logger = null)
|
||||
{
|
||||
_options = options ?? throw new ArgumentNullException(nameof(options));
|
||||
_robotModelLoader = robotModelLoader ?? throw new ArgumentNullException(nameof(robotModelLoader));
|
||||
_logger = logger;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 将飞拍规划结果导出到 Config/Data/name。
|
||||
/// </summary>
|
||||
/// <param name="trajectoryName">飞拍轨迹名称。</param>
|
||||
/// <param name="robot">当前机器人配置。</param>
|
||||
/// <param name="bundle">规划结果包；旧规划导出和实发诊断使用同一份视图。</param>
|
||||
/// <param name="speedRatio">请求速度倍率，仅作为诊断列保留；最终采样倍率已在规划阶段处理。</param>
|
||||
public void WriteUploadedFlyshot(string trajectoryName, RobotProfile robot, PlannedExecutionBundle bundle, double speedRatio = 1.0)
|
||||
{
|
||||
WriteUploadedFlyshot(trajectoryName, robot, bundle, bundle, speedRatio);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 将飞拍规划结果导出到 Config/Data/name，并显式区分旧规划导出视图和实发诊断视图。
|
||||
/// </summary>
|
||||
/// <param name="trajectoryName">飞拍轨迹名称。</param>
|
||||
/// <param name="robot">当前机器人配置。</param>
|
||||
/// <param name="exportPlanningBundle">旧格式规划导出使用的结果包，不应包含运行时 speedRatio 折算。</param>
|
||||
/// <param name="executionBundle">实发诊断使用的结果包，应与运行时最终发送队列保持一致。</param>
|
||||
/// <param name="speedRatio">请求速度倍率，仅作为 ActualSend 诊断列保留。</param>
|
||||
public void WriteUploadedFlyshot(
|
||||
string trajectoryName,
|
||||
RobotProfile robot,
|
||||
PlannedExecutionBundle exportPlanningBundle,
|
||||
PlannedExecutionBundle executionBundle,
|
||||
double speedRatio = 1.0)
|
||||
{
|
||||
if (string.IsNullOrWhiteSpace(trajectoryName))
|
||||
{
|
||||
throw new ArgumentException("轨迹名称不能为空。", nameof(trajectoryName));
|
||||
}
|
||||
|
||||
ArgumentNullException.ThrowIfNull(robot);
|
||||
ArgumentNullException.ThrowIfNull(exportPlanningBundle);
|
||||
ArgumentNullException.ThrowIfNull(executionBundle);
|
||||
|
||||
var outputDir = Path.Combine(_options.ResolveConfigRoot(), "Data", SanitizeDirectoryName(trajectoryName));
|
||||
Directory.CreateDirectory(outputDir);
|
||||
if (exportPlanningBundle.Result.DenseJointTrajectory is null)
|
||||
{
|
||||
throw new InvalidOperationException("导出飞拍旧格式规划工件前必须先生成规划明细轨迹。");
|
||||
}
|
||||
|
||||
// 旧格式规划文件固定使用不含运行时 speedRatio 的规划视图，避免和 ActualSend 实发诊断混在一起。
|
||||
var kinematicsModel = _robotModelLoader.LoadKinematicsModel(robot.ModelPath);
|
||||
var jointTrajectory = BuildJointRows(exportPlanningBundle.PlannedTrajectory);
|
||||
_logger?.LogInformation("规划之后的轨迹点位数量为:{}", jointTrajectory.Count);
|
||||
var executionDenseTrajectory = exportPlanningBundle.Result.DenseJointTrajectory;
|
||||
var jointDetailTrajectory = DownsampleDenseRows(
|
||||
executionDenseTrajectory,
|
||||
samplePeriodSeconds: LegacyDetailSamplePeriodSeconds);
|
||||
var cartTrajectory = BuildCartesianRows(exportPlanningBundle.PlannedTrajectory, kinematicsModel);
|
||||
var cartDetailTrajectory = BuildCartesianRowsFromJointDense(jointDetailTrajectory, kinematicsModel);
|
||||
|
||||
TrajectoryExporter.WriteJointTrajectory(Path.Combine(outputDir, "JointTraj.txt"), jointTrajectory);
|
||||
TrajectoryExporter.WriteJointDenseTrajectory(Path.Combine(outputDir, "JointDetialTraj.txt"), jointDetailTrajectory);
|
||||
TrajectoryExporter.WriteCartesianTrajectory(Path.Combine(outputDir, "CartTraj.txt"), cartTrajectory);
|
||||
TrajectoryExporter.WriteCartesianDenseTrajectory(Path.Combine(outputDir, "CartDetialTraj.txt"), cartDetailTrajectory);
|
||||
WriteActualSendArtifacts(outputDir, robot, executionBundle.Result, speedRatio);
|
||||
|
||||
_logger?.LogInformation(
|
||||
"saveTrajectory 已导出规划点位: name={TrajectoryName}, outputDir={OutputDir}, jointRows={JointRows}, detailRows={DetailRows}, speedRatio={SpeedRatio}",
|
||||
trajectoryName,
|
||||
outputDir,
|
||||
jointTrajectory.Count,
|
||||
jointDetailTrajectory.Count,
|
||||
speedRatio);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 生成按 J519 8ms 实际发送周期重采样的轨迹点，供 saveTrajectory 离线对比真实下发序列。
|
||||
/// </summary>
|
||||
private void WriteActualSendArtifacts(string outputDir, RobotProfile robot, TrajectoryResult result, double speedRatio)
|
||||
{
|
||||
ArgumentNullException.ThrowIfNull(robot);
|
||||
|
||||
var preparedExecution = result.PreparedFlyshotExecution;
|
||||
if (result.DenseJointTrajectory is null && preparedExecution is null)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
var samples = preparedExecution is null
|
||||
? J519SendTrajectorySampler.SampleDenseJointTrajectory(
|
||||
result.DenseJointTrajectory!,
|
||||
result.Duration.TotalSeconds,
|
||||
ActualSendServoPeriodSeconds,
|
||||
speedRatio: 1.0)
|
||||
: preparedExecution.Samples.Select(static sample => new J519SendSample(
|
||||
sample.SampleIndex,
|
||||
sample.SendTime,
|
||||
sample.TrajectoryTime,
|
||||
sample.SpeedRatio,
|
||||
sample.JointsDegrees)).ToArray();
|
||||
var triggerBindings = preparedExecution is null
|
||||
? TriggerSampleBinder.Bind(
|
||||
result.TriggerTimeline,
|
||||
samples,
|
||||
result.TriggerSampleIndexOffsetCycles)
|
||||
: preparedExecution.TriggerBindings.Select(static binding =>
|
||||
new TriggerSampleBinding(
|
||||
binding.Trigger,
|
||||
new J519SendSample(
|
||||
binding.Sample.SampleIndex,
|
||||
binding.Sample.SendTime,
|
||||
binding.Sample.TrajectoryTime,
|
||||
binding.Sample.SpeedRatio,
|
||||
binding.Sample.JointsDegrees),
|
||||
binding.SampleIndex,
|
||||
binding.FoundInWindow)).ToArray();
|
||||
TrajectoryLimitValidator.ValidateJ519SendSamples(
|
||||
robot,
|
||||
samples,
|
||||
trajectoryName: result.ProgramName,
|
||||
validateJerk: false);
|
||||
var jointRows = new List<IReadOnlyList<double>>(samples.Count);
|
||||
List<IReadOnlyList<double>> timingRows = preparedExecution is null
|
||||
? new List<IReadOnlyList<double>>(samples.Count)
|
||||
: preparedExecution.TimingRows.Select(static row => (IReadOnlyList<double>)row.ToArray()).ToList();
|
||||
List<IReadOnlyList<double>> jerkRows = preparedExecution is null
|
||||
? []
|
||||
: preparedExecution.JerkRows.Select(static row => (IReadOnlyList<double>)row.ToArray()).ToList();
|
||||
double? previousSendTime = null;
|
||||
double[]? previousJoints = null;
|
||||
double[]? previousVelocity = null;
|
||||
double[]? previousAcceleration = null;
|
||||
|
||||
foreach (var sample in samples)
|
||||
{
|
||||
jointRows.Add(BuildActualSendJointRow(sample.SendTime, sample.JointsDegrees));
|
||||
if (preparedExecution is null)
|
||||
{
|
||||
timingRows.Add(J519SendTrajectorySampler.BuildTimingRow(sample, speedRatio, stretchIterationCount: 0));
|
||||
|
||||
if (previousSendTime is not null && previousJoints is not null)
|
||||
{
|
||||
jerkRows.Add(J519SendTrajectorySampler.BuildJerkRow(
|
||||
previousSendTime.Value,
|
||||
sample.SendTime,
|
||||
previousJoints,
|
||||
sample.JointsDegrees,
|
||||
ref previousVelocity,
|
||||
ref previousAcceleration));
|
||||
}
|
||||
}
|
||||
|
||||
previousSendTime = sample.SendTime;
|
||||
previousJoints = sample.JointsDegrees.ToArray();
|
||||
}
|
||||
|
||||
WriteDenseRows(Path.Combine(outputDir, "ActualSendJointTraj.txt"), jointRows);
|
||||
WriteDenseRows(Path.Combine(outputDir, "ActualSendTiming.txt"), timingRows);
|
||||
WriteDenseRows(Path.Combine(outputDir, "ActualSendJerkStats.txt"), jerkRows);
|
||||
TrajectoryExporter.WriteShotEvents(Path.Combine(outputDir, "ShotEvents.json"), result.ShotEvents, triggerBindings);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 构造实际发送点位文本行，格式为 send_time + 关节角度 + io_mask + io_value。
|
||||
/// </summary>
|
||||
private static IReadOnlyList<double> BuildActualSendJointRow(double sendTime, IReadOnlyList<double> joints)
|
||||
{
|
||||
var row = new double[joints.Count + 3];
|
||||
row[0] = Math.Round(sendTime, 6);
|
||||
for (var index = 0; index < joints.Count; index++)
|
||||
{
|
||||
row[index + 1] = Math.Round(joints[index], 6);
|
||||
}
|
||||
|
||||
row[^2] = 0.0;
|
||||
row[^1] = 0.0;
|
||||
return row;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 以空格分隔的旧轨迹文本格式写出数值行。
|
||||
/// </summary>
|
||||
private static void WriteDenseRows(string path, IReadOnlyList<IReadOnlyList<double>> rows)
|
||||
{
|
||||
var sb = new StringBuilder();
|
||||
foreach (var row in rows)
|
||||
{
|
||||
sb.AppendLine(string.Join(" ", row.Select(static value => $"{value:F6}")));
|
||||
}
|
||||
|
||||
File.WriteAllText(path, sb.ToString(), new UTF8Encoding(false));
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 构造 JointTraj.txt 行数据，格式为 time + 关节弧度。
|
||||
/// </summary>
|
||||
private static IReadOnlyList<IReadOnlyList<double>> BuildJointRows(PlannedTrajectory trajectory)
|
||||
{
|
||||
var rows = new List<IReadOnlyList<double>>(trajectory.PlannedWaypoints.Count);
|
||||
for (var index = 0; index < trajectory.PlannedWaypoints.Count; index++)
|
||||
{
|
||||
var row = new List<double>(trajectory.PlannedWaypoints[index].Positions.Count + 1)
|
||||
{
|
||||
Math.Round(trajectory.WaypointTimes[index], 6)
|
||||
};
|
||||
row.AddRange(trajectory.PlannedWaypoints[index].Positions.Select(static value => Math.Round(value, 6)));
|
||||
rows.Add(row);
|
||||
}
|
||||
|
||||
return rows;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 构造 CartTraj.txt 行数据，格式为 time + x/y/z/qx/qy/qz/qw。
|
||||
/// </summary>
|
||||
private static IReadOnlyList<IReadOnlyList<double>> BuildCartesianRows(
|
||||
PlannedTrajectory trajectory,
|
||||
RobotKinematicsModel kinematicsModel)
|
||||
{
|
||||
var rows = new List<IReadOnlyList<double>>(trajectory.PlannedWaypoints.Count);
|
||||
for (var index = 0; index < trajectory.PlannedWaypoints.Count; index++)
|
||||
{
|
||||
var pose = RobotKinematics.ForwardKinematics(kinematicsModel, trajectory.PlannedWaypoints[index].Positions.ToArray());
|
||||
var row = new List<double>(pose.Length + 1)
|
||||
{
|
||||
Math.Round(trajectory.WaypointTimes[index], 6)
|
||||
};
|
||||
row.AddRange(pose.Select(static value => Math.Round(value, 6)));
|
||||
rows.Add(row);
|
||||
}
|
||||
|
||||
return rows;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 基于执行侧稠密关节轨迹生成笛卡尔导出行，保持与 JointDetialTraj.txt 同一来源。
|
||||
/// </summary>
|
||||
private static IReadOnlyList<IReadOnlyList<double>> BuildCartesianRowsFromJointDense(
|
||||
IReadOnlyList<IReadOnlyList<double>> jointDenseRows,
|
||||
RobotKinematicsModel kinematicsModel)
|
||||
{
|
||||
var rows = new List<IReadOnlyList<double>>(jointDenseRows.Count);
|
||||
foreach (var jointRow in jointDenseRows)
|
||||
{
|
||||
var jointPositions = jointRow.Skip(1).ToArray();
|
||||
var pose = RobotKinematics.ForwardKinematics(kinematicsModel, jointPositions);
|
||||
var row = new List<double>(pose.Length + 1)
|
||||
{
|
||||
Math.Round(jointRow[0], 6)
|
||||
};
|
||||
row.AddRange(pose.Select(static value => Math.Round(value, 6)));
|
||||
rows.Add(row);
|
||||
}
|
||||
|
||||
return rows;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 将 8ms 执行稠密轨迹按指定周期抽稀为低频兼容视图，并始终保留终点。
|
||||
/// </summary>
|
||||
private static IReadOnlyList<IReadOnlyList<double>> DownsampleDenseRows(
|
||||
IReadOnlyList<IReadOnlyList<double>> denseRows,
|
||||
double samplePeriodSeconds)
|
||||
{
|
||||
var result = new List<IReadOnlyList<double>>();
|
||||
var epsilon = 1e-6;
|
||||
var nextSampleTime = 0.0;
|
||||
|
||||
foreach (var row in denseRows)
|
||||
{
|
||||
var sampleTime = row[0];
|
||||
if (sampleTime + epsilon < nextSampleTime)
|
||||
{
|
||||
continue;
|
||||
}
|
||||
|
||||
if (Math.Abs(sampleTime - nextSampleTime) <= epsilon || sampleTime.Equals(0.0))
|
||||
{
|
||||
result.Add(row);
|
||||
nextSampleTime += samplePeriodSeconds;
|
||||
}
|
||||
}
|
||||
|
||||
if (result.Count == 0 || !ReferenceEquals(result[^1], denseRows[^1]))
|
||||
{
|
||||
result.Add(denseRows[^1]);
|
||||
}
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 将轨迹名转换为可用目录名，避免 HTTP 输入中的路径字符污染输出目录。
|
||||
/// </summary>
|
||||
private static string SanitizeDirectoryName(string name)
|
||||
{
|
||||
var invalidChars = Path.GetInvalidFileNameChars();
|
||||
var chars = name.Select(ch => invalidChars.Contains(ch) ? '_' : ch).ToArray();
|
||||
return new string(chars);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,217 @@
|
||||
namespace Flyshot.ControllerClientCompat;
|
||||
|
||||
/// <summary>
|
||||
/// 对飞拍稠密关节轨迹的首尾采样点做速度整形，降低启动和结束时的单步角度变化。
|
||||
/// </summary>
|
||||
internal static class FlyshotTrajectoryEdgeShaper
|
||||
{
|
||||
/// <summary>
|
||||
/// 首尾整形默认覆盖的采样点数（含锚点）。
|
||||
/// </summary>
|
||||
internal const int DefaultEdgePointCount = 10;
|
||||
|
||||
/// <summary>
|
||||
/// 对稠密关节轨迹做首尾整形，时间列保持不变，首段采用 ease-in，尾段采用 ease-out。
|
||||
/// </summary>
|
||||
/// <param name="denseJointTrajectory">输入稠密关节轨迹，每行格式为 [time, j1..jN]，关节单位为弧度。</param>
|
||||
/// <param name="maxEdgeStepDegrees">保留旧签名兼容调用方；当前实现不再按角度阈值扩窗。</param>
|
||||
/// <param name="maxWindowPoints">单侧整形覆盖的采样点数（含锚点），默认首尾各 10 点。</param>
|
||||
/// <returns>经过首尾整形后的新轨迹；若不满足整形条件则返回原轨迹副本。</returns>
|
||||
internal static IReadOnlyList<IReadOnlyList<double>> ShapeDenseJointTrajectory(
|
||||
IReadOnlyList<IReadOnlyList<double>> denseJointTrajectory,
|
||||
double maxEdgeStepDegrees = 0.0,
|
||||
int maxWindowPoints = DefaultEdgePointCount)
|
||||
{
|
||||
ArgumentNullException.ThrowIfNull(denseJointTrajectory);
|
||||
if (denseJointTrajectory.Count == 0)
|
||||
{
|
||||
return Array.Empty<IReadOnlyList<double>>();
|
||||
}
|
||||
|
||||
var copiedRows = denseJointTrajectory
|
||||
.Select(static row => row.ToArray())
|
||||
.ToArray();
|
||||
if (copiedRows.Length < 5 || maxWindowPoints < 2)
|
||||
{
|
||||
return copiedRows;
|
||||
}
|
||||
|
||||
var lastIndex = copiedRows.Length - 1;
|
||||
var window = Math.Min(maxWindowPoints, lastIndex / 2);
|
||||
if (window < 2)
|
||||
{
|
||||
return copiedRows;
|
||||
}
|
||||
|
||||
// 以原始轨迹为参考估计窗口边界的速度，并在位移累计量上做单段单调整形，
|
||||
// 目标是让首尾 10 点表现为更平滑的加减速，而不是硬匹配高阶导数导致振荡。
|
||||
var originalRows = copiedRows
|
||||
.Select(static row => row.ToArray())
|
||||
.ToArray();
|
||||
|
||||
ApplyLeadingHermiteBlend(copiedRows, originalRows, window);
|
||||
ApplyTrailingHermiteBlend(copiedRows, originalRows, window);
|
||||
|
||||
return copiedRows;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 对首段做单段 Hermite 累计位移整形：起点速度为 0，窗口末端按原轨迹边界速度接回中段。
|
||||
/// </summary>
|
||||
private static void ApplyLeadingHermiteBlend(double[][] rows, double[][] originalRows, int window)
|
||||
{
|
||||
var startRow = originalRows[0];
|
||||
var endRow = originalRows[window];
|
||||
var totalDuration = endRow[0] - startRow[0];
|
||||
if (totalDuration <= 0.0)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
for (var jointIndex = 1; jointIndex < startRow.Length; jointIndex++)
|
||||
{
|
||||
var delta = endRow[jointIndex] - startRow[jointIndex];
|
||||
if (Math.Abs(delta) <= 1e-12)
|
||||
{
|
||||
continue;
|
||||
}
|
||||
|
||||
var endVelocity = EstimateVelocity(originalRows, window, jointIndex);
|
||||
var normalizedEndSlope = ClampNormalizedSlope((endVelocity * totalDuration) / delta);
|
||||
|
||||
for (var index = 1; index < window; index++)
|
||||
{
|
||||
var normalizedTime = (rows[index][0] - startRow[0]) / totalDuration;
|
||||
var shapedValue = startRow[jointIndex]
|
||||
+ (delta * EvaluateHermiteProgress(normalizedTime, startSlope: 0.0, endSlope: normalizedEndSlope));
|
||||
var blendWeight = Math.Pow(1.0 - normalizedTime, 2.0);
|
||||
rows[index][jointIndex] = Lerp(originalRows[index][jointIndex], shapedValue, blendWeight);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 对尾段做单段 Hermite 累计位移整形：窗口起点按原轨迹边界速度接入，终点速度减到 0。
|
||||
/// </summary>
|
||||
private static void ApplyTrailingHermiteBlend(double[][] rows, double[][] originalRows, int window)
|
||||
{
|
||||
var startIndex = rows.Length - 1 - window;
|
||||
var startRow = originalRows[startIndex];
|
||||
var endRow = originalRows[^1];
|
||||
var totalDuration = endRow[0] - startRow[0];
|
||||
if (totalDuration <= 0.0)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
for (var jointIndex = 1; jointIndex < startRow.Length; jointIndex++)
|
||||
{
|
||||
var delta = endRow[jointIndex] - startRow[jointIndex];
|
||||
if (Math.Abs(delta) <= 1e-12)
|
||||
{
|
||||
continue;
|
||||
}
|
||||
|
||||
var startVelocity = EstimateVelocity(originalRows, startIndex, jointIndex);
|
||||
var normalizedStartSlope = ClampNormalizedSlope((startVelocity * totalDuration) / delta);
|
||||
|
||||
for (var index = 1; index < window; index++)
|
||||
{
|
||||
var normalizedTime = (rows[startIndex + index][0] - startRow[0]) / totalDuration;
|
||||
var shapedValue = startRow[jointIndex]
|
||||
+ (delta * EvaluateHermiteProgress(normalizedTime, startSlope: normalizedStartSlope, endSlope: 0.0));
|
||||
var blendWeight = Math.Pow(normalizedTime, 2.0);
|
||||
rows[startIndex + index][jointIndex] = Lerp(originalRows[startIndex + index][jointIndex], shapedValue, blendWeight);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 估算给定行在原始轨迹上的一阶导，首尾退化为单边差分。
|
||||
/// </summary>
|
||||
private static double EstimateVelocity(double[][] rows, int index, int jointIndex)
|
||||
{
|
||||
if (index <= 0)
|
||||
{
|
||||
var dt = rows[1][0] - rows[0][0];
|
||||
return dt <= 0.0 ? 0.0 : (rows[1][jointIndex] - rows[0][jointIndex]) / dt;
|
||||
}
|
||||
|
||||
if (index >= rows.Length - 1)
|
||||
{
|
||||
var dt = rows[^1][0] - rows[^2][0];
|
||||
return dt <= 0.0 ? 0.0 : (rows[^1][jointIndex] - rows[^2][jointIndex]) / dt;
|
||||
}
|
||||
|
||||
var previousDt = rows[index][0] - rows[index - 1][0];
|
||||
var nextDt = rows[index + 1][0] - rows[index][0];
|
||||
if (previousDt <= 0.0 || nextDt <= 0.0)
|
||||
{
|
||||
return 0.0;
|
||||
}
|
||||
|
||||
var backward = (rows[index][jointIndex] - rows[index - 1][jointIndex]) / previousDt;
|
||||
var forward = (rows[index + 1][jointIndex] - rows[index][jointIndex]) / nextDt;
|
||||
return (backward + forward) / 2.0;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 估算给定行在原始轨迹上的二阶导，端点退化为 0 以避免放大边界噪声。
|
||||
/// </summary>
|
||||
private static double EstimateAcceleration(double[][] rows, int index, int jointIndex)
|
||||
{
|
||||
if (index <= 0 || index >= rows.Length - 1)
|
||||
{
|
||||
return 0.0;
|
||||
}
|
||||
|
||||
var previousDt = rows[index][0] - rows[index - 1][0];
|
||||
var nextDt = rows[index + 1][0] - rows[index][0];
|
||||
if (previousDt <= 0.0 || nextDt <= 0.0)
|
||||
{
|
||||
return 0.0;
|
||||
}
|
||||
|
||||
var backward = (rows[index][jointIndex] - rows[index - 1][jointIndex]) / previousDt;
|
||||
var forward = (rows[index + 1][jointIndex] - rows[index][jointIndex]) / nextDt;
|
||||
var averageDt = (previousDt + nextDt) / 2.0;
|
||||
return averageDt <= 0.0 ? 0.0 : (forward - backward) / averageDt;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 计算 Hermite 累计位移曲线在 0..1 归一化时间上的进度值。
|
||||
/// </summary>
|
||||
private static double EvaluateHermiteProgress(double normalizedTime, double startSlope, double endSlope)
|
||||
{
|
||||
var u = Math.Clamp(normalizedTime, 0.0, 1.0);
|
||||
var u2 = u * u;
|
||||
var u3 = u2 * u;
|
||||
var h00 = (2.0 * u3) - (3.0 * u2) + 1.0;
|
||||
var h10 = u3 - (2.0 * u2) + u;
|
||||
var h01 = (-2.0 * u3) + (3.0 * u2);
|
||||
var h11 = u3 - u2;
|
||||
return (h00 * 0.0) + (h10 * startSlope) + (h01 * 1.0) + (h11 * endSlope);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 把归一化边界斜率限制在单调 Hermite 常见的稳定区间内，避免过冲和窗口内振荡。
|
||||
/// </summary>
|
||||
private static double ClampNormalizedSlope(double normalizedSlope)
|
||||
{
|
||||
if (double.IsNaN(normalizedSlope) || double.IsInfinity(normalizedSlope))
|
||||
{
|
||||
return 0.0;
|
||||
}
|
||||
|
||||
return Math.Clamp(normalizedSlope, 0.0, 3.0);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 在线性插值基础上做温和混合，避免首尾窗口为了追赶锚点而产生过大的局部跃度。
|
||||
/// </summary>
|
||||
private static double Lerp(double originalValue, double shapedValue, double weight)
|
||||
{
|
||||
var clampedWeight = Math.Clamp(weight, 0.0, 1.0);
|
||||
return originalValue + ((shapedValue - originalValue) * clampedWeight);
|
||||
}
|
||||
}
|
||||
227
src/Flyshot.ControllerClientCompat/FlyshotTrajectoryStore.cs
Normal file
227
src/Flyshot.ControllerClientCompat/FlyshotTrajectoryStore.cs
Normal file
@@ -0,0 +1,227 @@
|
||||
using System.Text.Json;
|
||||
using System.Text.Json.Nodes;
|
||||
using Flyshot.Core.Config;
|
||||
using Flyshot.Core.Domain;
|
||||
using Microsoft.Extensions.Logging;
|
||||
|
||||
namespace Flyshot.ControllerClientCompat;
|
||||
|
||||
/// <summary>
|
||||
/// 使用运行目录 Config/RobotConfig.json 持久化单机器人飞拍轨迹和机器人配置。
|
||||
/// </summary>
|
||||
public sealed class JsonFlyshotTrajectoryStore
|
||||
{
|
||||
private readonly ControllerClientCompatOptions _options;
|
||||
private readonly RobotConfigLoader _configLoader;
|
||||
private readonly ILogger<JsonFlyshotTrajectoryStore>? _logger;
|
||||
|
||||
/// <summary>
|
||||
/// 初始化基于 JSON 文件的轨迹存储。
|
||||
/// </summary>
|
||||
/// <param name="options">兼容层基础配置，用于定位运行配置根目录。</param>
|
||||
/// <param name="configLoader">旧版 RobotConfig.json 加载器，用于反序列化已保存的轨迹。</param>
|
||||
/// <param name="logger">日志记录器；允许 null。</param>
|
||||
public JsonFlyshotTrajectoryStore(ControllerClientCompatOptions options, RobotConfigLoader configLoader, ILogger<JsonFlyshotTrajectoryStore>? logger = null)
|
||||
{
|
||||
_options = options ?? throw new ArgumentNullException(nameof(options));
|
||||
_configLoader = configLoader ?? throw new ArgumentNullException(nameof(configLoader));
|
||||
_logger = logger;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 将单条轨迹持久化到统一 RobotConfig.json，同时更新机器人配置段。
|
||||
/// </summary>
|
||||
/// <param name="robotName">当前已初始化的机器人名称，仅用于日志诊断。</param>
|
||||
/// <param name="settings">当前机器人级兼容配置。</param>
|
||||
/// <param name="trajectory">要保存的已上传轨迹。</param>
|
||||
public void Save(string robotName, CompatibilityRobotSettings settings, ControllerClientCompatUploadedTrajectory trajectory)
|
||||
{
|
||||
ArgumentNullException.ThrowIfNull(settings);
|
||||
ArgumentNullException.ThrowIfNull(trajectory);
|
||||
|
||||
_logger?.LogInformation(
|
||||
"RobotConfig 保存轨迹: robot={RobotName}, name={TrajectoryName}, waypoints={WaypointCount}",
|
||||
robotName,
|
||||
trajectory.Name,
|
||||
trajectory.Waypoints.Count);
|
||||
|
||||
var path = ResolveStorePath();
|
||||
var directory = Path.GetDirectoryName(path)!;
|
||||
Directory.CreateDirectory(directory);
|
||||
|
||||
JsonObject root;
|
||||
if (File.Exists(path))
|
||||
{
|
||||
var existingJson = File.ReadAllText(path);
|
||||
root = JsonNode.Parse(existingJson)?.AsObject() ?? new JsonObject();
|
||||
}
|
||||
else
|
||||
{
|
||||
root = new JsonObject();
|
||||
}
|
||||
|
||||
// 更新 robot 配置段，保持与旧版 RobotConfig.json 字段名一致。
|
||||
root["robot"] = SerializeRobotSettings(settings);
|
||||
|
||||
// 确保 flying_shots 节点存在。
|
||||
if (!root.TryGetPropertyValue("flying_shots", out var flyingShotsNode) || flyingShotsNode is not JsonObject flyingShotsObj)
|
||||
{
|
||||
flyingShotsObj = new JsonObject();
|
||||
root["flying_shots"] = flyingShotsObj;
|
||||
}
|
||||
|
||||
flyingShotsObj[trajectory.Name] = SerializeTrajectory(trajectory);
|
||||
|
||||
var writeOptions = new JsonSerializerOptions
|
||||
{
|
||||
WriteIndented = true,
|
||||
PropertyNamingPolicy = JsonNamingPolicy.SnakeCaseLower
|
||||
};
|
||||
File.WriteAllText(path, root.ToJsonString(writeOptions));
|
||||
|
||||
_logger?.LogInformation("RobotConfig 轨迹已保存到 {Path}", path);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 从统一 RobotConfig.json 删除指定名称的轨迹。
|
||||
/// </summary>
|
||||
/// <param name="robotName">当前已初始化的机器人名称，仅用于日志诊断。</param>
|
||||
/// <param name="trajectoryName">要删除的轨迹名称。</param>
|
||||
public void Delete(string robotName, string trajectoryName)
|
||||
{
|
||||
if (string.IsNullOrWhiteSpace(trajectoryName))
|
||||
{
|
||||
throw new ArgumentException("轨迹名称不能为空。", nameof(trajectoryName));
|
||||
}
|
||||
|
||||
_logger?.LogInformation("RobotConfig 删除轨迹: robot={RobotName}, name={TrajectoryName}", robotName, trajectoryName);
|
||||
|
||||
var path = ResolveStorePath();
|
||||
if (!File.Exists(path))
|
||||
{
|
||||
_logger?.LogWarning("RobotConfig 删除失败: 文件不存在 {Path}", path);
|
||||
return;
|
||||
}
|
||||
|
||||
var existingJson = File.ReadAllText(path);
|
||||
var root = JsonNode.Parse(existingJson)?.AsObject();
|
||||
if (root is null)
|
||||
{
|
||||
_logger?.LogWarning("RobotConfig 删除失败: 无法解析 JSON {Path}", path);
|
||||
return;
|
||||
}
|
||||
|
||||
if (root.TryGetPropertyValue("flying_shots", out var flyingShotsNode) && flyingShotsNode is JsonObject flyingShotsObj)
|
||||
{
|
||||
var removed = flyingShotsObj.Remove(trajectoryName);
|
||||
if (removed)
|
||||
{
|
||||
var writeOptions = new JsonSerializerOptions
|
||||
{
|
||||
WriteIndented = true,
|
||||
PropertyNamingPolicy = JsonNamingPolicy.SnakeCaseLower
|
||||
};
|
||||
File.WriteAllText(path, root.ToJsonString(writeOptions));
|
||||
_logger?.LogInformation("RobotConfig 轨迹已删除: {TrajectoryName}", trajectoryName);
|
||||
}
|
||||
else
|
||||
{
|
||||
_logger?.LogWarning("RobotConfig 删除失败: 轨迹不存在 {TrajectoryName}", trajectoryName);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 加载统一 RobotConfig.json 中的所有轨迹，并回传机器人配置。
|
||||
/// </summary>
|
||||
/// <param name="robotName">当前已初始化的机器人名称，仅用于日志诊断。</param>
|
||||
/// <param name="settings">输出 RobotConfig.json 中的机器人配置；若文件不存在或解析失败则为 null。</param>
|
||||
/// <returns>按轨迹名称索引的已上传轨迹集合。</returns>
|
||||
public IReadOnlyDictionary<string, ControllerClientCompatUploadedTrajectory> LoadAll(string robotName, out CompatibilityRobotSettings? settings)
|
||||
{
|
||||
var path = ResolveStorePath();
|
||||
if (!File.Exists(path))
|
||||
{
|
||||
_logger?.LogInformation("RobotConfig 无持久化数据: {Path}", path);
|
||||
settings = null;
|
||||
return new Dictionary<string, ControllerClientCompatUploadedTrajectory>(StringComparer.Ordinal);
|
||||
}
|
||||
|
||||
try
|
||||
{
|
||||
_logger?.LogInformation("RobotConfig 正在加载: {Path}", path);
|
||||
|
||||
var loaded = _configLoader.Load(path, _options.ResolveConfigRoot());
|
||||
settings = loaded.Robot;
|
||||
|
||||
var dict = new Dictionary<string, ControllerClientCompatUploadedTrajectory>(StringComparer.Ordinal);
|
||||
foreach (var program in loaded.Programs)
|
||||
{
|
||||
var traj = new ControllerClientCompatUploadedTrajectory(
|
||||
name: program.Value.Name,
|
||||
waypoints: program.Value.Waypoints.Select(static wp => wp.Positions),
|
||||
shotFlags: program.Value.ShotFlags,
|
||||
offsetValues: program.Value.OffsetValues,
|
||||
addressGroups: program.Value.AddressGroups.Select(static g => g.Addresses));
|
||||
dict[program.Key] = traj;
|
||||
}
|
||||
|
||||
_logger?.LogInformation(
|
||||
"RobotConfig 加载完成: robot={RobotName}, 轨迹数={Count}, useDo={UseDo}, ioKeepCycles={IoKeepCycles}",
|
||||
robotName,
|
||||
dict.Count,
|
||||
settings?.UseDo,
|
||||
settings?.IoKeepCycles);
|
||||
|
||||
return dict;
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
_logger?.LogError(ex, "RobotConfig 加载失败: {Path}", path);
|
||||
settings = null;
|
||||
return new Dictionary<string, ControllerClientCompatUploadedTrajectory>(StringComparer.Ordinal);
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 把机器人兼容配置序列化为 JSON 对象，字段名与旧版 RobotConfig.json 一致。
|
||||
/// </summary>
|
||||
private static JsonObject SerializeRobotSettings(CompatibilityRobotSettings settings)
|
||||
{
|
||||
return new JsonObject
|
||||
{
|
||||
["use_do"] = JsonValue.Create(settings.UseDo),
|
||||
["io_addr"] = JsonSerializer.SerializeToNode(settings.IoAddresses),
|
||||
["io_keep_cycles"] = JsonValue.Create(settings.IoKeepCycles),
|
||||
["trigger_sample_index_offset_cycles"] = JsonValue.Create(settings.TriggerSampleIndexOffsetCycles),
|
||||
["acc_limit"] = JsonValue.Create(settings.AccLimitScale),
|
||||
["jerk_limit"] = JsonValue.Create(settings.JerkLimitScale),
|
||||
["adapt_icsp_try_num"] = JsonValue.Create(settings.AdaptIcspTryNum),
|
||||
["planning_speed_scale"] = JsonValue.Create(settings.PlanningSpeedScale),
|
||||
["speed_ratio"] = JsonValue.Create(settings.SpeedRatio),
|
||||
["smooth_start_stop_timing"] = JsonValue.Create(settings.SmoothStartStopTiming)
|
||||
};
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 把已上传轨迹序列化为 JSON 对象，字段名与旧版 RobotConfig.json 的 flying_shots 节点一致。
|
||||
/// </summary>
|
||||
private static JsonObject SerializeTrajectory(ControllerClientCompatUploadedTrajectory trajectory)
|
||||
{
|
||||
return new JsonObject
|
||||
{
|
||||
["traj_waypoints"] = JsonSerializer.SerializeToNode(trajectory.Waypoints),
|
||||
["shot_flags"] = JsonSerializer.SerializeToNode(trajectory.ShotFlags),
|
||||
["offset_values"] = JsonSerializer.SerializeToNode(trajectory.OffsetValues),
|
||||
["addr"] = JsonSerializer.SerializeToNode(trajectory.AddressGroups)
|
||||
};
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 解析单程序单机器人的统一配置文件路径。
|
||||
/// </summary>
|
||||
private string ResolveStorePath()
|
||||
{
|
||||
return Path.Combine(_options.ResolveConfigRoot(), "RobotConfig.json");
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,238 @@
|
||||
using Flyshot.Core.Domain;
|
||||
|
||||
namespace Flyshot.ControllerClientCompat;
|
||||
|
||||
/// <summary>
|
||||
/// 定义 HTTP-only 兼容层对外暴露的 ControllerClient 语义服务接口。
|
||||
/// </summary>
|
||||
public interface IControllerClientCompatService
|
||||
{
|
||||
/// <summary>
|
||||
/// 获取当前兼容层对外报告的服务端版本号。
|
||||
/// </summary>
|
||||
string ServerVersion { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取当前是否已经完成机器人初始化。
|
||||
/// </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>
|
||||
/// <param name="robotName">机器人名称。</param>
|
||||
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);
|
||||
|
||||
/// <summary>
|
||||
/// 记录当前控制器已经建立连接。
|
||||
/// </summary>
|
||||
/// <param name="robotIp">控制器 IP。</param>
|
||||
void Connect(string robotIp);
|
||||
|
||||
/// <summary>
|
||||
/// 记录当前控制器已经断开。
|
||||
/// </summary>
|
||||
void Disconnect();
|
||||
|
||||
/// <summary>
|
||||
/// 记录当前机器人进入使能态。
|
||||
/// </summary>
|
||||
/// <param name="bufferSize">缓冲区大小。</param>
|
||||
void EnableRobot(int bufferSize);
|
||||
|
||||
/// <summary>
|
||||
/// 记录当前机器人退出使能态。
|
||||
/// </summary>
|
||||
void DisableRobot();
|
||||
|
||||
/// <summary>
|
||||
/// 停止当前运动状态。
|
||||
/// </summary>
|
||||
void StopMove();
|
||||
|
||||
/// <summary>
|
||||
/// 读取当前控制器运行时状态快照。
|
||||
/// </summary>
|
||||
/// <returns>控制器运行时状态快照。</returns>
|
||||
ControllerStateSnapshot GetControllerSnapshot();
|
||||
|
||||
/// <summary>
|
||||
/// 读取状态页所需的兼容层元数据快照。
|
||||
/// </summary>
|
||||
/// <returns>兼容层元数据快照。</returns>
|
||||
ControllerClientStatusSnapshotMetadata GetStatusSnapshotMetadata();
|
||||
|
||||
/// <summary>
|
||||
/// 获取当前速度倍率。
|
||||
/// </summary>
|
||||
/// <returns>当前速度倍率。</returns>
|
||||
double GetSpeedRatio();
|
||||
|
||||
/// <summary>
|
||||
/// 更新当前速度倍率。
|
||||
/// </summary>
|
||||
/// <param name="ratio">目标速度倍率。</param>
|
||||
void SetSpeedRatio(double ratio);
|
||||
|
||||
/// <summary>
|
||||
/// 写入兼容层缓存的 IO 数值。
|
||||
/// </summary>
|
||||
/// <param name="port">IO 端口号。</param>
|
||||
/// <param name="value">IO 值。</param>
|
||||
/// <param name="ioType">IO 类型。</param>
|
||||
void SetIo(int port, bool value, string ioType);
|
||||
|
||||
/// <summary>
|
||||
/// 读取兼容层缓存的 IO 数值。
|
||||
/// </summary>
|
||||
/// <param name="port">IO 端口号。</param>
|
||||
/// <param name="ioType">IO 类型。</param>
|
||||
/// <returns>缓存中的 IO 值。</returns>
|
||||
bool GetIo(int port, string ioType);
|
||||
|
||||
/// <summary>
|
||||
/// 按给定位姿和 seed 计算最近 IK。
|
||||
/// </summary>
|
||||
/// <param name="pose">目标位姿数组。</param>
|
||||
/// <param name="seed">IK seed 关节数组。</param>
|
||||
/// <returns>IK 结果关节数组。</returns>
|
||||
IReadOnlyList<double> GetNearestIk(IReadOnlyList<double> pose, IReadOnlyList<double> seed);
|
||||
|
||||
/// <summary>
|
||||
/// 设置当前 TCP 三维坐标。
|
||||
/// </summary>
|
||||
/// <param name="x">TCP X。</param>
|
||||
/// <param name="y">TCP Y。</param>
|
||||
/// <param name="z">TCP Z。</param>
|
||||
void SetTcp(double x, double y, double z);
|
||||
|
||||
/// <summary>
|
||||
/// 读取当前 TCP 三维坐标。
|
||||
/// </summary>
|
||||
/// <returns>TCP 数组。</returns>
|
||||
IReadOnlyList<double> GetTcp();
|
||||
|
||||
/// <summary>
|
||||
/// 读取当前关节位置。
|
||||
/// </summary>
|
||||
/// <returns>关节位置数组。</returns>
|
||||
IReadOnlyList<double> GetJointPositions();
|
||||
|
||||
/// <summary>
|
||||
/// 更新当前关节位置。
|
||||
/// </summary>
|
||||
/// <param name="jointPositions">目标关节位置。</param>
|
||||
void MoveJoint(IReadOnlyList<double> jointPositions);
|
||||
|
||||
/// <summary>
|
||||
/// 按直角坐标移动到目标 TCP 位姿。
|
||||
/// </summary>
|
||||
/// <param name="pose">目标位姿 [x,y,z,w,p,r]，单位为 mm/deg。</param>
|
||||
void MovePose(IReadOnlyList<double> pose);
|
||||
|
||||
/// <summary>
|
||||
/// 执行普通轨迹。
|
||||
/// </summary>
|
||||
/// <param name="waypoints">轨迹路点集合。</param>
|
||||
/// <param name="options">执行参数。</param>
|
||||
void ExecuteTrajectory(IReadOnlyList<IReadOnlyList<double>> waypoints, TrajectoryExecutionOptions? options = null);
|
||||
|
||||
/// <summary>
|
||||
/// 读取当前末端位姿快照。
|
||||
/// </summary>
|
||||
/// <returns>位姿数组。</returns>
|
||||
IReadOnlyList<double> GetPose();
|
||||
|
||||
/// <summary>
|
||||
/// 上传一条飞拍轨迹。
|
||||
/// </summary>
|
||||
/// <param name="trajectory">飞拍轨迹。</param>
|
||||
void UploadTrajectory(ControllerClientCompatUploadedTrajectory trajectory);
|
||||
|
||||
/// <summary>
|
||||
/// 列出当前已上传的飞拍轨迹名称。
|
||||
/// </summary>
|
||||
/// <returns>轨迹名称列表。</returns>
|
||||
IReadOnlyList<string> ListTrajectoryNames();
|
||||
|
||||
/// <summary>
|
||||
/// 执行指定名称的飞拍轨迹。
|
||||
/// </summary>
|
||||
/// <param name="name">轨迹名称。</param>
|
||||
/// <param name="options">飞拍执行参数。</param>
|
||||
void ExecuteTrajectoryByName(string name, FlyshotExecutionOptions? options = null);
|
||||
|
||||
/// <summary>
|
||||
/// 保存指定飞拍轨迹的轨迹信息。
|
||||
/// </summary>
|
||||
/// <param name="name">轨迹名称。</param>
|
||||
/// <param name="method">轨迹生成方法。</param>
|
||||
void SaveTrajectoryInfo(string name, string method = "icsp");
|
||||
|
||||
/// <summary>
|
||||
/// 检查指定飞拍轨迹是否可执行。
|
||||
/// </summary>
|
||||
/// <param name="duration">输出规划轨迹总时长。</param>
|
||||
/// <param name="name">轨迹名称。</param>
|
||||
/// <param name="method">轨迹生成方法。</param>
|
||||
/// <param name="saveTrajectory">是否保存轨迹信息。</param>
|
||||
/// <returns>轨迹是否有效。</returns>
|
||||
bool IsFlyshotTrajectoryValid(out TimeSpan duration, string name, string method = "icsp", bool saveTrajectory = false);
|
||||
|
||||
/// <summary>
|
||||
/// 删除指定名称的飞拍轨迹。
|
||||
/// </summary>
|
||||
/// <param name="name">轨迹名称。</param>
|
||||
void DeleteTrajectory(string name);
|
||||
|
||||
/// <summary>
|
||||
/// 读取当前配置过的机器人名称。
|
||||
/// </summary>
|
||||
/// <returns>机器人名称。</returns>
|
||||
string GetRobotName();
|
||||
|
||||
/// <summary>
|
||||
/// 读取当前机器人自由度。
|
||||
/// </summary>
|
||||
/// <returns>机器人自由度。</returns>
|
||||
int GetDegreesOfFreedom();
|
||||
}
|
||||
@@ -0,0 +1,356 @@
|
||||
using Flyshot.Core.Domain;
|
||||
using Microsoft.Extensions.Logging;
|
||||
|
||||
namespace Flyshot.ControllerClientCompat;
|
||||
|
||||
/// <summary>
|
||||
/// MoveJoint 轨迹生成器。
|
||||
/// 将起始关节角到目标关节角的单段运动，按速度、加速度、jerk 约束生成稠密点到点轨迹，
|
||||
/// 供 FANUC J519 伺服流逐周期下发。
|
||||
///
|
||||
/// 核心思路：路径只取关节空间直线 q=q0+(q1-q0)*s(t)，时间律使用 7 阶平滑函数；
|
||||
/// 生成后再按离散采样点反算速度、加速度和 jerk，确保真实下发点也满足限制。
|
||||
/// </summary>
|
||||
internal static class MoveJointTrajectoryGenerator
|
||||
{
|
||||
/// <summary>
|
||||
/// 最小段基础时长（秒）。零位移时仍由采样对齐逻辑生成起点和终点两帧。
|
||||
/// </summary>
|
||||
private const double MinimumMoveJointDurationSeconds = 0.0;
|
||||
|
||||
/// <summary>
|
||||
/// 7 阶平滑点到点时间律的一阶导数最大值。
|
||||
/// </summary>
|
||||
private const double SmoothPtpVelocityShapeCoefficient = 2.1875;
|
||||
|
||||
/// <summary>
|
||||
/// 7 阶平滑点到点时间律的二阶导数最大值。
|
||||
/// </summary>
|
||||
private const double SmoothPtpAccelerationShapeCoefficient = 7.513188404399293;
|
||||
|
||||
/// <summary>
|
||||
/// 7 阶平滑点到点时间律的三阶导数最大值。
|
||||
/// </summary>
|
||||
private const double SmoothPtpJerkShapeCoefficient = 52.5;
|
||||
|
||||
/// <summary>
|
||||
/// 单次 MoveJoint 最大采样点数上限，防止极端配置下生成过大的轨迹数组。
|
||||
/// </summary>
|
||||
private const int MaxMoveJointSampleCount = 1_000_000;
|
||||
|
||||
/// <summary>
|
||||
/// 离散限位校验允许的浮点容差。
|
||||
/// </summary>
|
||||
private const double DiscreteLimitTolerance = 1.000001;
|
||||
|
||||
/// <summary>
|
||||
/// 离散限位校验失败时最多拉长的采样周期次数。
|
||||
/// </summary>
|
||||
private const int MaxDiscreteLimitStretchIterations = 10_000;
|
||||
|
||||
/// <summary>
|
||||
/// 计算 MoveJoint 轨迹的完整结果。
|
||||
///
|
||||
/// 处理流程：
|
||||
/// 1. 根据关节限位计算连续时间律理论最短时长
|
||||
/// 2. 按 speedRatio 缩放规划限位，并将时长对齐到固定伺服采样间隔
|
||||
/// 3. 用 7 阶平滑点到点时间律生成稠密轨迹点
|
||||
/// 4. 按离散点反查速度、加速度和 jerk，必要时拉长时长重算
|
||||
/// 5. 封装为 TrajectoryResult 返回
|
||||
/// </summary>
|
||||
/// <param name="robot">机器人配置，含自由度数和关节限位。</param>
|
||||
/// <param name="startJoints">起始关节角（弧度）。</param>
|
||||
/// <param name="targetJoints">目标关节角（弧度）。</param>
|
||||
/// <param name="speedRatio">速度倍率，必须大于 0；在生成阶段折算进有效关节限位。</param>
|
||||
/// <param name="logger">可选的诊断日志。</param>
|
||||
public static TrajectoryResult CreateResult(
|
||||
RobotProfile robot,
|
||||
IReadOnlyList<double> startJoints,
|
||||
IReadOnlyList<double> targetJoints,
|
||||
double speedRatio,
|
||||
ILogger? logger = null)
|
||||
{
|
||||
ArgumentNullException.ThrowIfNull(robot);
|
||||
ArgumentNullException.ThrowIfNull(startJoints);
|
||||
ArgumentNullException.ThrowIfNull(targetJoints);
|
||||
|
||||
if (speedRatio <= 0.0 || double.IsNaN(speedRatio) || double.IsInfinity(speedRatio))
|
||||
{
|
||||
throw new InvalidOperationException("Speed ratio must be greater than zero for MoveJoint execution.");
|
||||
}
|
||||
|
||||
if (startJoints.Count != robot.DegreesOfFreedom || targetJoints.Count != robot.DegreesOfFreedom)
|
||||
{
|
||||
throw new InvalidOperationException($"MoveJoint expects {robot.DegreesOfFreedom} joints.");
|
||||
}
|
||||
|
||||
var planningRobot = ApplySpeedRatioToJointLimits(robot, speedRatio);
|
||||
var requestedDurationSeconds = ResolveDurationSeconds(planningRobot, startJoints, targetJoints);
|
||||
var samplePeriodSeconds = robot.ServoPeriod.TotalSeconds;
|
||||
var durationSeconds = AlignDurationToServoStep(requestedDurationSeconds, samplePeriodSeconds);
|
||||
var denseJointTrajectory = GenerateDenseTrajectory(startJoints, targetJoints, durationSeconds, samplePeriodSeconds);
|
||||
|
||||
var stretchCount = 0;
|
||||
while (!SatisfiesDiscreteJointLimits(planningRobot, denseJointTrajectory))
|
||||
{
|
||||
stretchCount++;
|
||||
if (stretchCount > MaxDiscreteLimitStretchIterations)
|
||||
{
|
||||
throw new InvalidOperationException("MoveJoint duration cannot be stretched enough to satisfy joint limits.");
|
||||
}
|
||||
|
||||
// 连续时间律满足限位后，仍以实际离散点为准；不满足时逐周期拉长后重采样。
|
||||
durationSeconds = AlignDurationToServoStep(durationSeconds + samplePeriodSeconds, samplePeriodSeconds);
|
||||
denseJointTrajectory = GenerateDenseTrajectory(startJoints, targetJoints, durationSeconds, samplePeriodSeconds);
|
||||
}
|
||||
|
||||
logger?.LogDebug(
|
||||
"MoveJointTrajectoryGenerator: requestedDuration={RequestedDuration:F4}s, duration={Duration:F4}s, speedRatio={SpeedRatio}, samplePeriod={SamplePeriod:F6}s, sampleCount={SampleCount}, stretchCount={StretchCount}",
|
||||
requestedDurationSeconds,
|
||||
durationSeconds,
|
||||
speedRatio,
|
||||
samplePeriodSeconds,
|
||||
denseJointTrajectory.Count,
|
||||
stretchCount);
|
||||
|
||||
return new TrajectoryResult(
|
||||
programName: "move-joint",
|
||||
method: PlanningMethod.Doubles,
|
||||
isValid: true,
|
||||
duration: TimeSpan.FromSeconds(durationSeconds),
|
||||
shotEvents: Array.Empty<ShotEvent>(),
|
||||
triggerTimeline: Array.Empty<TrajectoryDoEvent>(),
|
||||
artifacts: Array.Empty<TrajectoryArtifact>(),
|
||||
failureReason: null,
|
||||
usedCache: false,
|
||||
originalWaypointCount: 2,
|
||||
plannedWaypointCount: denseJointTrajectory.Count,
|
||||
denseJointTrajectory: denseJointTrajectory);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 将执行速度倍率折算为 MoveJoint 规划限位，保持最终稠密轨迹仍按物理 8ms 周期生成。
|
||||
/// </summary>
|
||||
private static RobotProfile ApplySpeedRatioToJointLimits(RobotProfile robot, double speedRatio)
|
||||
{
|
||||
if (Math.Abs(speedRatio - 1.0) < 1e-12)
|
||||
{
|
||||
return robot;
|
||||
}
|
||||
|
||||
var scaledLimits = robot.JointLimits
|
||||
.Select(limit => new JointLimit(
|
||||
limit.JointName,
|
||||
limit.VelocityLimit * speedRatio,
|
||||
limit.AccelerationLimit * speedRatio * speedRatio,
|
||||
limit.JerkLimit * speedRatio * speedRatio * speedRatio))
|
||||
.ToArray();
|
||||
|
||||
return new RobotProfile(
|
||||
name: robot.Name,
|
||||
modelPath: robot.ModelPath,
|
||||
degreesOfFreedom: robot.DegreesOfFreedom,
|
||||
jointLimits: scaledLimits,
|
||||
jointCouplings: robot.JointCouplings,
|
||||
servoPeriod: robot.ServoPeriod,
|
||||
triggerPeriod: robot.TriggerPeriod);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 根据 7 阶平滑点到点时间律和每轴限位，计算 MoveJoint 理论最短时长。
|
||||
///
|
||||
/// 时间律为 s(u)=35u^4-84u^5+70u^6-20u^7，其中 u=t/T。
|
||||
/// 各轴位移 d_i 共用同一个 s(t)，所以每轴分别按 d_i 放大速度、加速度和 jerk，再取全局最大时长。
|
||||
/// </summary>
|
||||
internal static double ResolveDurationSeconds(
|
||||
RobotProfile robot,
|
||||
IReadOnlyList<double> startJoints,
|
||||
IReadOnlyList<double> targetJoints)
|
||||
{
|
||||
var duration = MinimumMoveJointDurationSeconds;
|
||||
|
||||
for (var index = 0; index < robot.DegreesOfFreedom; index++)
|
||||
{
|
||||
var distance = Math.Abs(targetJoints[index] - startJoints[index]);
|
||||
if (distance <= 0.0)
|
||||
{
|
||||
continue;
|
||||
}
|
||||
|
||||
var limit = robot.JointLimits[index];
|
||||
var velocityDuration = distance * SmoothPtpVelocityShapeCoefficient / limit.VelocityLimit;
|
||||
var accelerationDuration = Math.Sqrt(distance * SmoothPtpAccelerationShapeCoefficient / limit.AccelerationLimit);
|
||||
var jerkDuration = Math.Cbrt(distance * SmoothPtpJerkShapeCoefficient / limit.JerkLimit);
|
||||
|
||||
duration = Math.Max(duration, Math.Max(velocityDuration, Math.Max(accelerationDuration, jerkDuration)));
|
||||
}
|
||||
|
||||
return duration;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 将请求时长向上对齐到整数个采样周期，确保轨迹末帧正好落在 duration 处。
|
||||
/// </summary>
|
||||
/// <param name="durationSeconds">请求的理论最短时长（秒）。</param>
|
||||
/// <param name="samplePeriodSeconds">采样周期（秒）。</param>
|
||||
/// <returns>对齐后的时长，为 samplePeriodSeconds 的整数倍。</returns>
|
||||
internal static double AlignDurationToServoStep(double durationSeconds, double samplePeriodSeconds)
|
||||
{
|
||||
if (samplePeriodSeconds <= 0.0 || double.IsNaN(samplePeriodSeconds) || double.IsInfinity(samplePeriodSeconds))
|
||||
{
|
||||
throw new InvalidOperationException("Speed ratio must be greater than zero for MoveJoint execution.");
|
||||
}
|
||||
|
||||
var intervals = ResolveSampleIntervalCount(durationSeconds, samplePeriodSeconds);
|
||||
|
||||
return intervals * samplePeriodSeconds;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 生成从起始关节角到目标关节角的稠密等时间隔轨迹点序列。
|
||||
///
|
||||
/// 每行格式：[time_seconds, joint_0, joint_1, ..., joint_n-1]。
|
||||
/// </summary>
|
||||
internal static IReadOnlyList<IReadOnlyList<double>> GenerateDenseTrajectory(
|
||||
IReadOnlyList<double> startJoints,
|
||||
IReadOnlyList<double> targetJoints,
|
||||
double durationSeconds,
|
||||
double samplePeriodSeconds)
|
||||
{
|
||||
var sampleCount = ResolveSampleIntervalCount(durationSeconds, samplePeriodSeconds) + 1;
|
||||
var rows = new List<IReadOnlyList<double>>(checked((int)sampleCount));
|
||||
|
||||
for (var index = 0L; index < sampleCount; index++)
|
||||
{
|
||||
var time = Math.Min(index * samplePeriodSeconds, durationSeconds);
|
||||
rows.Add(CreateRow(time, durationSeconds, startJoints, targetJoints));
|
||||
}
|
||||
|
||||
return rows;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 计算 7 阶平滑点到点时间律的位置归一化值。
|
||||
/// </summary>
|
||||
/// <param name="normalizedTime">归一化时间 u，取值会被限制在 [0, 1]。</param>
|
||||
/// <returns>归一化位置 s(u)，范围 [0, 1]。</returns>
|
||||
internal static double EvaluateSmoothPtpPositionScale(double normalizedTime)
|
||||
{
|
||||
var clamped = Math.Clamp(normalizedTime, 0.0, 1.0);
|
||||
var u2 = clamped * clamped;
|
||||
var u4 = u2 * u2;
|
||||
|
||||
return u4 * (35.0 + (clamped * (-84.0 + (clamped * (70.0 - (20.0 * clamped))))));
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 计算时长对应的采样间隔数（向上取整）。
|
||||
/// 采样间隔数 + 1 = 采样点数，因为轨迹包含起点和终点。
|
||||
/// </summary>
|
||||
private static long ResolveSampleIntervalCount(double durationSeconds, double samplePeriodSeconds)
|
||||
{
|
||||
var rawIntervals = durationSeconds / samplePeriodSeconds;
|
||||
if (double.IsNaN(rawIntervals) || double.IsInfinity(rawIntervals))
|
||||
{
|
||||
throw new InvalidOperationException("MoveJoint sample count is not representable.");
|
||||
}
|
||||
|
||||
var intervals = (long)Math.Ceiling(Math.Max(0.0, rawIntervals) - 1e-9);
|
||||
intervals = Math.Max(1, intervals);
|
||||
if (intervals + 1 > MaxMoveJointSampleCount)
|
||||
{
|
||||
throw new InvalidOperationException($"MoveJoint sample count must be between 2 and {MaxMoveJointSampleCount}.");
|
||||
}
|
||||
|
||||
return intervals;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 构造单个轨迹行：[time_seconds, joint_0, ..., joint_N-1]。
|
||||
/// </summary>
|
||||
private static IReadOnlyList<double> CreateRow(
|
||||
double timeSeconds,
|
||||
double durationSeconds,
|
||||
IReadOnlyList<double> startJoints,
|
||||
IReadOnlyList<double> targetJoints)
|
||||
{
|
||||
var u = durationSeconds <= 0.0 ? 1.0 : Math.Clamp(timeSeconds / durationSeconds, 0.0, 1.0);
|
||||
var scale = EvaluateSmoothPtpPositionScale(u);
|
||||
var row = new double[startJoints.Count + 1];
|
||||
row[0] = Math.Round(timeSeconds, 9);
|
||||
|
||||
for (var index = 0; index < startJoints.Count; index++)
|
||||
{
|
||||
row[index + 1] = startJoints[index] + ((targetJoints[index] - startJoints[index]) * scale);
|
||||
}
|
||||
|
||||
return row;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 用生成后的离散采样点复核每轴速度、加速度和 jerk，避免连续时间律在采样后仍出现差分越限。
|
||||
/// </summary>
|
||||
private static bool SatisfiesDiscreteJointLimits(RobotProfile robot, IReadOnlyList<IReadOnlyList<double>> rows)
|
||||
{
|
||||
double? previousTime = null;
|
||||
double[]? previousPositions = null;
|
||||
double[]? previousVelocities = null;
|
||||
double[]? previousAccelerations = null;
|
||||
|
||||
foreach (var row in rows)
|
||||
{
|
||||
var currentTime = row[0];
|
||||
var currentPositions = new double[robot.DegreesOfFreedom];
|
||||
for (var index = 0; index < robot.DegreesOfFreedom; index++)
|
||||
{
|
||||
currentPositions[index] = row[index + 1];
|
||||
}
|
||||
|
||||
if (previousTime is not null && previousPositions is not null)
|
||||
{
|
||||
var dt = currentTime - previousTime.Value;
|
||||
if (dt <= 0.0)
|
||||
{
|
||||
throw new InvalidOperationException("MoveJoint dense trajectory timestamps must be strictly increasing.");
|
||||
}
|
||||
|
||||
var velocities = new double[robot.DegreesOfFreedom];
|
||||
var accelerations = new double[robot.DegreesOfFreedom];
|
||||
for (var index = 0; index < robot.DegreesOfFreedom; index++)
|
||||
{
|
||||
var limit = robot.JointLimits[index];
|
||||
velocities[index] = (currentPositions[index] - previousPositions[index]) / dt;
|
||||
if (Math.Abs(velocities[index]) > limit.VelocityLimit * DiscreteLimitTolerance)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
accelerations[index] = previousVelocities is null
|
||||
? 0.0
|
||||
: (velocities[index] - previousVelocities[index]) / dt;
|
||||
if (Math.Abs(accelerations[index]) > limit.AccelerationLimit * DiscreteLimitTolerance)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
if (previousAccelerations is not null)
|
||||
{
|
||||
var jerk = (accelerations[index] - previousAccelerations[index]) / dt;
|
||||
if (Math.Abs(jerk) > limit.JerkLimit * DiscreteLimitTolerance)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
previousVelocities = velocities;
|
||||
previousAccelerations = accelerations;
|
||||
}
|
||||
|
||||
previousTime = currentTime;
|
||||
previousPositions = currentPositions;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,353 @@
|
||||
using Flyshot.Core.Domain;
|
||||
using Microsoft.Extensions.Logging;
|
||||
|
||||
namespace Flyshot.ControllerClientCompat;
|
||||
|
||||
/// <summary>
|
||||
/// MovePose 直角坐标轨迹生成器。
|
||||
/// 将起始 TCP 位姿到目标 TCP 位姿的单段运动，按保守直角速度、加速度和 jerk 限制生成 8ms 稠密点。
|
||||
/// </summary>
|
||||
internal static class MovePoseTrajectoryGenerator
|
||||
{
|
||||
/// <summary>
|
||||
/// 直角位置轴最大速度，单位为 mm/s。
|
||||
/// </summary>
|
||||
private const double LinearVelocityLimit = 50.0;
|
||||
|
||||
/// <summary>
|
||||
/// 直角位置轴最大加速度，单位为 mm/s^2。
|
||||
/// </summary>
|
||||
private const double LinearAccelerationLimit = 250.0;
|
||||
|
||||
/// <summary>
|
||||
/// 直角位置轴最大 jerk，单位为 mm/s^3。
|
||||
/// </summary>
|
||||
private const double LinearJerkLimit = 1250.0;
|
||||
|
||||
/// <summary>
|
||||
/// 姿态轴最大速度，单位为 deg/s。
|
||||
/// </summary>
|
||||
private const double AngularVelocityLimit = 10.0;
|
||||
|
||||
/// <summary>
|
||||
/// 姿态轴最大加速度，单位为 deg/s^2。
|
||||
/// </summary>
|
||||
private const double AngularAccelerationLimit = 50.0;
|
||||
|
||||
/// <summary>
|
||||
/// 姿态轴最大 jerk，单位为 deg/s^3。
|
||||
/// </summary>
|
||||
private const double AngularJerkLimit = 250.0;
|
||||
|
||||
/// <summary>
|
||||
/// 7 阶平滑点到点时间律的一阶导数最大值。
|
||||
/// </summary>
|
||||
private const double SmoothPtpVelocityShapeCoefficient = 2.1875;
|
||||
|
||||
/// <summary>
|
||||
/// 7 阶平滑点到点时间律的二阶导数最大值。
|
||||
/// </summary>
|
||||
private const double SmoothPtpAccelerationShapeCoefficient = 7.513188404399293;
|
||||
|
||||
/// <summary>
|
||||
/// 7 阶平滑点到点时间律的三阶导数最大值。
|
||||
/// </summary>
|
||||
private const double SmoothPtpJerkShapeCoefficient = 52.5;
|
||||
|
||||
/// <summary>
|
||||
/// 单次 MovePose 最大采样点数上限，避免极低速度倍率生成过大的队列。
|
||||
/// </summary>
|
||||
private const int MaxMovePoseSampleCount = 1_000_000;
|
||||
|
||||
/// <summary>
|
||||
/// 离散限位校验允许的浮点容差。
|
||||
/// </summary>
|
||||
private const double DiscreteLimitTolerance = 1.000001;
|
||||
|
||||
/// <summary>
|
||||
/// 离散限位校验失败时最多拉长的采样周期次数。
|
||||
/// </summary>
|
||||
private const int MaxDiscreteLimitStretchIterations = 10_000;
|
||||
|
||||
/// <summary>
|
||||
/// 计算 MovePose 轨迹的完整结果。
|
||||
/// </summary>
|
||||
/// <param name="startPose">起始位姿 [x,y,z,w,p,r]，单位为 mm/deg。</param>
|
||||
/// <param name="targetPose">目标位姿 [x,y,z,w,p,r]，单位为 mm/deg。</param>
|
||||
/// <param name="servoPeriod">J519 伺服发送周期。</param>
|
||||
/// <param name="speedRatio">速度倍率，必须大于 0。</param>
|
||||
/// <param name="logger">可选诊断日志。</param>
|
||||
/// <returns>包含稠密直角轨迹的规划结果。</returns>
|
||||
public static TrajectoryResult CreateResult(
|
||||
IReadOnlyList<double> startPose,
|
||||
IReadOnlyList<double> targetPose,
|
||||
TimeSpan servoPeriod,
|
||||
double speedRatio,
|
||||
ILogger? logger = null)
|
||||
{
|
||||
ArgumentNullException.ThrowIfNull(startPose);
|
||||
ArgumentNullException.ThrowIfNull(targetPose);
|
||||
EnsurePoseVector(startPose, nameof(startPose));
|
||||
EnsurePoseVector(targetPose, nameof(targetPose));
|
||||
|
||||
if (speedRatio <= 0.0 || double.IsNaN(speedRatio) || double.IsInfinity(speedRatio))
|
||||
{
|
||||
throw new InvalidOperationException("Speed ratio must be greater than zero for MovePose execution.");
|
||||
}
|
||||
|
||||
var samplePeriodSeconds = servoPeriod.TotalSeconds;
|
||||
if (samplePeriodSeconds <= 0.0 || double.IsNaN(samplePeriodSeconds) || double.IsInfinity(samplePeriodSeconds))
|
||||
{
|
||||
throw new InvalidOperationException("MovePose servo period must be a finite positive duration.");
|
||||
}
|
||||
|
||||
var requestedDurationSeconds = ResolveDurationSeconds(startPose, targetPose, speedRatio);
|
||||
var durationSeconds = AlignDurationToServoStep(requestedDurationSeconds, samplePeriodSeconds);
|
||||
var denseCartesianTrajectory = GenerateDenseTrajectory(startPose, targetPose, durationSeconds, samplePeriodSeconds);
|
||||
|
||||
var stretchCount = 0;
|
||||
while (!SatisfiesDefaultCartesianLimits(denseCartesianTrajectory, speedRatio))
|
||||
{
|
||||
stretchCount++;
|
||||
if (stretchCount > MaxDiscreteLimitStretchIterations)
|
||||
{
|
||||
throw new InvalidOperationException("MovePose duration cannot be stretched enough to satisfy Cartesian limits.");
|
||||
}
|
||||
|
||||
// 离散差分以真实下发点为准；不满足时逐周期拉长后重采样。
|
||||
durationSeconds = AlignDurationToServoStep(durationSeconds + samplePeriodSeconds, samplePeriodSeconds);
|
||||
denseCartesianTrajectory = GenerateDenseTrajectory(startPose, targetPose, durationSeconds, samplePeriodSeconds);
|
||||
}
|
||||
|
||||
logger?.LogDebug(
|
||||
"MovePoseTrajectoryGenerator: requestedDuration={RequestedDuration:F4}s, duration={Duration:F4}s, speedRatio={SpeedRatio}, samplePeriod={SamplePeriod:F6}s, sampleCount={SampleCount}, stretchCount={StretchCount}",
|
||||
requestedDurationSeconds,
|
||||
durationSeconds,
|
||||
speedRatio,
|
||||
samplePeriodSeconds,
|
||||
denseCartesianTrajectory.Count,
|
||||
stretchCount);
|
||||
|
||||
return new TrajectoryResult(
|
||||
programName: "move-pose",
|
||||
method: PlanningMethod.Doubles,
|
||||
isValid: true,
|
||||
duration: TimeSpan.FromSeconds(durationSeconds),
|
||||
shotEvents: Array.Empty<ShotEvent>(),
|
||||
triggerTimeline: Array.Empty<TrajectoryDoEvent>(),
|
||||
artifacts: Array.Empty<TrajectoryArtifact>(),
|
||||
failureReason: null,
|
||||
usedCache: false,
|
||||
originalWaypointCount: 2,
|
||||
plannedWaypointCount: denseCartesianTrajectory.Count,
|
||||
denseCartesianTrajectory: denseCartesianTrajectory);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 检查稠密直角轨迹是否满足默认六维速度、加速度和 jerk 限制。
|
||||
/// </summary>
|
||||
/// <param name="rows">稠密轨迹行，每行为 [time,x,y,z,w,p,r]。</param>
|
||||
/// <param name="speedRatio">生成该轨迹时使用的速度倍率。</param>
|
||||
/// <returns>满足限制时返回 true，否则返回 false。</returns>
|
||||
public static bool SatisfiesDefaultCartesianLimits(IReadOnlyList<IReadOnlyList<double>> rows, double speedRatio)
|
||||
{
|
||||
if (speedRatio <= 0.0 || double.IsNaN(speedRatio) || double.IsInfinity(speedRatio))
|
||||
{
|
||||
throw new InvalidOperationException("Speed ratio must be greater than zero for MovePose limit validation.");
|
||||
}
|
||||
|
||||
double? previousTime = null;
|
||||
double[]? previousPositions = null;
|
||||
double[]? previousVelocities = null;
|
||||
double[]? previousAccelerations = null;
|
||||
|
||||
foreach (var row in rows)
|
||||
{
|
||||
if (row.Count != 7)
|
||||
{
|
||||
throw new InvalidOperationException("MovePose dense trajectory rows must contain time plus six pose values.");
|
||||
}
|
||||
|
||||
var currentTime = row[0];
|
||||
var currentPositions = row.Skip(1).Take(6).ToArray();
|
||||
|
||||
if (previousTime is not null && previousPositions is not null)
|
||||
{
|
||||
var dt = currentTime - previousTime.Value;
|
||||
if (dt <= 0.0)
|
||||
{
|
||||
throw new InvalidOperationException("MovePose dense trajectory timestamps must be strictly increasing.");
|
||||
}
|
||||
|
||||
var velocities = new double[6];
|
||||
var accelerations = new double[6];
|
||||
for (var index = 0; index < 6; index++)
|
||||
{
|
||||
var limits = GetAxisLimits(index, speedRatio);
|
||||
velocities[index] = (currentPositions[index] - previousPositions[index]) / dt;
|
||||
if (Math.Abs(velocities[index]) > limits.Velocity * DiscreteLimitTolerance)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
accelerations[index] = previousVelocities is null
|
||||
? 0.0
|
||||
: (velocities[index] - previousVelocities[index]) / dt;
|
||||
if (Math.Abs(accelerations[index]) > limits.Acceleration * DiscreteLimitTolerance)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
if (previousAccelerations is not null)
|
||||
{
|
||||
var jerk = (accelerations[index] - previousAccelerations[index]) / dt;
|
||||
if (Math.Abs(jerk) > limits.Jerk * DiscreteLimitTolerance)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
previousVelocities = velocities;
|
||||
previousAccelerations = accelerations;
|
||||
}
|
||||
|
||||
previousTime = currentTime;
|
||||
previousPositions = currentPositions;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 根据 7 阶平滑点到点时间律和直角六维限制计算理论最短时长。
|
||||
/// </summary>
|
||||
private static double ResolveDurationSeconds(IReadOnlyList<double> startPose, IReadOnlyList<double> targetPose, double speedRatio)
|
||||
{
|
||||
var duration = 0.0;
|
||||
for (var index = 0; index < 6; index++)
|
||||
{
|
||||
var distance = Math.Abs(targetPose[index] - startPose[index]);
|
||||
if (distance <= 0.0)
|
||||
{
|
||||
continue;
|
||||
}
|
||||
|
||||
var limits = GetAxisLimits(index, speedRatio);
|
||||
var velocityDuration = distance * SmoothPtpVelocityShapeCoefficient / limits.Velocity;
|
||||
var accelerationDuration = Math.Sqrt(distance * SmoothPtpAccelerationShapeCoefficient / limits.Acceleration);
|
||||
var jerkDuration = Math.Cbrt(distance * SmoothPtpJerkShapeCoefficient / limits.Jerk);
|
||||
duration = Math.Max(duration, Math.Max(velocityDuration, Math.Max(accelerationDuration, jerkDuration)));
|
||||
}
|
||||
|
||||
return duration;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 生成从起始位姿到目标位姿的稠密等时间隔直角轨迹点序列。
|
||||
/// </summary>
|
||||
private static IReadOnlyList<IReadOnlyList<double>> GenerateDenseTrajectory(
|
||||
IReadOnlyList<double> startPose,
|
||||
IReadOnlyList<double> targetPose,
|
||||
double durationSeconds,
|
||||
double samplePeriodSeconds)
|
||||
{
|
||||
var sampleCount = ResolveSampleIntervalCount(durationSeconds, samplePeriodSeconds) + 1;
|
||||
var rows = new List<IReadOnlyList<double>>(checked((int)sampleCount));
|
||||
|
||||
for (var index = 0L; index < sampleCount; index++)
|
||||
{
|
||||
var time = Math.Min(index * samplePeriodSeconds, durationSeconds);
|
||||
rows.Add(CreateRow(time, durationSeconds, startPose, targetPose));
|
||||
}
|
||||
|
||||
return rows;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 将请求时长向上对齐到整数个伺服周期。
|
||||
/// </summary>
|
||||
private static double AlignDurationToServoStep(double durationSeconds, double samplePeriodSeconds)
|
||||
{
|
||||
return ResolveSampleIntervalCount(durationSeconds, samplePeriodSeconds) * samplePeriodSeconds;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 计算时长对应的采样间隔数，轨迹至少包含起点和终点两帧。
|
||||
/// </summary>
|
||||
private static long ResolveSampleIntervalCount(double durationSeconds, double samplePeriodSeconds)
|
||||
{
|
||||
var rawIntervals = durationSeconds / samplePeriodSeconds;
|
||||
if (double.IsNaN(rawIntervals) || double.IsInfinity(rawIntervals))
|
||||
{
|
||||
throw new InvalidOperationException("MovePose sample count is not representable.");
|
||||
}
|
||||
|
||||
var intervals = (long)Math.Ceiling(Math.Max(0.0, rawIntervals) - 1e-9);
|
||||
intervals = Math.Max(1, intervals);
|
||||
if (intervals + 1 > MaxMovePoseSampleCount)
|
||||
{
|
||||
throw new InvalidOperationException($"MovePose sample count must be between 2 and {MaxMovePoseSampleCount}.");
|
||||
}
|
||||
|
||||
return intervals;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 构造单个轨迹行：[time_seconds,x,y,z,w,p,r]。
|
||||
/// </summary>
|
||||
private static IReadOnlyList<double> CreateRow(
|
||||
double timeSeconds,
|
||||
double durationSeconds,
|
||||
IReadOnlyList<double> startPose,
|
||||
IReadOnlyList<double> targetPose)
|
||||
{
|
||||
var u = durationSeconds <= 0.0 ? 1.0 : Math.Clamp(timeSeconds / durationSeconds, 0.0, 1.0);
|
||||
var scale = MoveJointTrajectoryGenerator.EvaluateSmoothPtpPositionScale(u);
|
||||
var row = new double[7];
|
||||
row[0] = Math.Round(timeSeconds, 9);
|
||||
|
||||
for (var index = 0; index < 6; index++)
|
||||
{
|
||||
row[index + 1] = startPose[index] + ((targetPose[index] - startPose[index]) * scale);
|
||||
}
|
||||
|
||||
return row;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 获取指定直角轴在当前速度倍率下的有效限制。
|
||||
/// </summary>
|
||||
private static CartesianAxisLimit GetAxisLimits(int index, double speedRatio)
|
||||
{
|
||||
var linearAxis = index < 3;
|
||||
var velocity = linearAxis ? LinearVelocityLimit : AngularVelocityLimit;
|
||||
var acceleration = linearAxis ? LinearAccelerationLimit : AngularAccelerationLimit;
|
||||
var jerk = linearAxis ? LinearJerkLimit : AngularJerkLimit;
|
||||
return new CartesianAxisLimit(
|
||||
velocity * speedRatio,
|
||||
acceleration * speedRatio * speedRatio,
|
||||
jerk * speedRatio * speedRatio * speedRatio);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 校验位姿向量必须为六维有限数。
|
||||
/// </summary>
|
||||
private static void EnsurePoseVector(IReadOnlyList<double> pose, string parameterName)
|
||||
{
|
||||
if (pose.Count != 6)
|
||||
{
|
||||
throw new ArgumentException("MovePose expects pose [x,y,z,w,p,r].", parameterName);
|
||||
}
|
||||
|
||||
if (pose.Any(static value => double.IsNaN(value) || double.IsInfinity(value)))
|
||||
{
|
||||
throw new ArgumentException("MovePose pose values must be finite.", parameterName);
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 表示单个直角轴的有效速度、加速度和 jerk 限制。
|
||||
/// </summary>
|
||||
private readonly record struct CartesianAxisLimit(double Velocity, double Acceleration, double Jerk);
|
||||
}
|
||||
58
src/Flyshot.ControllerClientCompat/PlannedExecutionBundle.cs
Normal file
58
src/Flyshot.ControllerClientCompat/PlannedExecutionBundle.cs
Normal file
@@ -0,0 +1,58 @@
|
||||
using Flyshot.Core.Domain;
|
||||
using Flyshot.Core.Planning;
|
||||
using Flyshot.Core.Triggering;
|
||||
|
||||
namespace Flyshot.ControllerClientCompat;
|
||||
|
||||
/// <summary>
|
||||
/// 表示兼容层执行轨迹前生成的完整规划结果包。
|
||||
/// </summary>
|
||||
public sealed class PlannedExecutionBundle
|
||||
{
|
||||
/// <summary>
|
||||
/// 初始化一份执行规划结果包。
|
||||
/// </summary>
|
||||
/// <param name="plannedTrajectory">规划后的原始连续轨迹。</param>
|
||||
/// <param name="shotTimeline">飞拍触发时间轴。</param>
|
||||
/// <param name="result">对运行时和监控层暴露的规划结果。</param>
|
||||
/// <param name="preparedExecution">飞拍链路预先准备好的最终发送队列。</param>
|
||||
/// <param name="executionTrajectory">与运行时结果同口径的连续执行轨迹；为空时复用 plannedTrajectory。</param>
|
||||
public PlannedExecutionBundle(
|
||||
PlannedTrajectory plannedTrajectory,
|
||||
ShotTimeline shotTimeline,
|
||||
TrajectoryResult result,
|
||||
FlyshotPreparedExecution? preparedExecution = null,
|
||||
PlannedTrajectory? executionTrajectory = null)
|
||||
{
|
||||
PlannedTrajectory = plannedTrajectory ?? throw new ArgumentNullException(nameof(plannedTrajectory));
|
||||
ShotTimeline = shotTimeline ?? throw new ArgumentNullException(nameof(shotTimeline));
|
||||
Result = result ?? throw new ArgumentNullException(nameof(result));
|
||||
PreparedExecution = preparedExecution;
|
||||
ExecutionTrajectory = executionTrajectory ?? plannedTrajectory;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 获取规划后的原始连续轨迹。
|
||||
/// </summary>
|
||||
public PlannedTrajectory PlannedTrajectory { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取与运行时结果同口径的连续执行轨迹，用于构建最终 8ms 发送队列。
|
||||
/// </summary>
|
||||
public PlannedTrajectory ExecutionTrajectory { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取飞拍触发时间轴。
|
||||
/// </summary>
|
||||
public ShotTimeline ShotTimeline { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取运行时可消费的规划结果。
|
||||
/// </summary>
|
||||
public TrajectoryResult Result { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取飞拍链路预先准备好的最终发送队列；普通轨迹与 move_joint 为 null。
|
||||
/// </summary>
|
||||
public FlyshotPreparedExecution? PreparedExecution { get; }
|
||||
}
|
||||
@@ -0,0 +1,28 @@
|
||||
namespace Flyshot.ControllerClientCompat;
|
||||
|
||||
/// <summary>
|
||||
/// 表示普通轨迹执行接口的可选参数，字段名对齐旧 `ControllerClient::ExecuteTrajectory`。
|
||||
/// </summary>
|
||||
public sealed class TrajectoryExecutionOptions
|
||||
{
|
||||
/// <summary>
|
||||
/// 初始化普通轨迹执行参数。
|
||||
/// </summary>
|
||||
/// <param name="method">轨迹生成方法，支持 `icsp` 或 `doubles`。</param>
|
||||
/// <param name="saveTrajectory">是否保存轨迹信息。</param>
|
||||
public TrajectoryExecutionOptions(string method = "icsp", bool saveTrajectory = false)
|
||||
{
|
||||
Method = string.IsNullOrWhiteSpace(method) ? "icsp" : method;
|
||||
SaveTrajectory = saveTrajectory;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 获取轨迹生成方法。
|
||||
/// </summary>
|
||||
public string Method { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取是否保存轨迹信息。
|
||||
/// </summary>
|
||||
public bool SaveTrajectory { get; }
|
||||
}
|
||||
@@ -8,6 +8,7 @@
|
||||
|
||||
<ItemGroup>
|
||||
<ProjectReference Include="..\Flyshot.Core.Domain\Flyshot.Core.Domain.csproj" />
|
||||
<PackageReference Include="Microsoft.Extensions.Logging.Abstractions" Version="8.0.1" />
|
||||
</ItemGroup>
|
||||
|
||||
</Project>
|
||||
|
||||
30
src/Flyshot.Core.Config/LoadedRobotModel.cs
Normal file
30
src/Flyshot.Core.Config/LoadedRobotModel.cs
Normal file
@@ -0,0 +1,30 @@
|
||||
using Flyshot.Core.Domain;
|
||||
|
||||
namespace Flyshot.Core.Config;
|
||||
|
||||
/// <summary>
|
||||
/// 表示一次 JSON 模型解析后生成的完整机器人模型视图集合。
|
||||
/// </summary>
|
||||
public sealed class LoadedRobotModel
|
||||
{
|
||||
/// <summary>
|
||||
/// 初始化完整机器人模型视图集合。
|
||||
/// </summary>
|
||||
/// <param name="profile">规划和运行时使用的关节约束视图。</param>
|
||||
/// <param name="kinematicsModel">正运动学导出使用的几何链视图。</param>
|
||||
public LoadedRobotModel(RobotProfile profile, RobotKinematicsModel kinematicsModel)
|
||||
{
|
||||
Profile = profile ?? throw new ArgumentNullException(nameof(profile));
|
||||
KinematicsModel = kinematicsModel ?? throw new ArgumentNullException(nameof(kinematicsModel));
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 获取规划和运行时使用的关节约束视图。
|
||||
/// </summary>
|
||||
public RobotProfile Profile { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取正运动学导出使用的几何链视图。
|
||||
/// </summary>
|
||||
public RobotKinematicsModel KinematicsModel { get; }
|
||||
}
|
||||
@@ -22,7 +22,7 @@ public enum CompatibilityPathStyle
|
||||
public static class PathCompatibility
|
||||
{
|
||||
/// <summary>
|
||||
/// 按旧系统常见目录约定解析配置文件路径。
|
||||
/// 按当前服务配置目录约定解析配置文件路径。
|
||||
/// </summary>
|
||||
/// <param name="configPath">调用方传入的原始配置路径。</param>
|
||||
/// <param name="repoRoot">当前兼容搜索的仓库根目录。</param>
|
||||
@@ -48,11 +48,10 @@ public static class PathCompatibility
|
||||
}
|
||||
|
||||
var normalizedRepoRoot = Path.GetFullPath(repoRoot);
|
||||
var fileName = Path.GetFileName(rawPath);
|
||||
var checkedPaths = new List<string>();
|
||||
|
||||
// 先按最常见的候选路径顺序尝试，保持与旧工具链相近的定位逻辑。
|
||||
foreach (var candidate in BuildConfigCandidates(normalizedRepoRoot, rawPath, fileName))
|
||||
// 相对路径只允许落在当前服务根目录的 Config 下，避免隐式回退到父工作区旧文件。
|
||||
foreach (var candidate in BuildConfigCandidates(normalizedRepoRoot, rawPath))
|
||||
{
|
||||
var fullCandidate = Path.GetFullPath(candidate);
|
||||
if (checkedPaths.Contains(fullCandidate, StringComparer.OrdinalIgnoreCase))
|
||||
@@ -67,18 +66,6 @@ public static class PathCompatibility
|
||||
}
|
||||
}
|
||||
|
||||
// 最后一层兜底按文件名全仓库搜索，但只接受唯一命中，避免同名配置误判。
|
||||
var matches = Directory
|
||||
.EnumerateFiles(normalizedRepoRoot, fileName, SearchOption.AllDirectories)
|
||||
.Select(Path.GetFullPath)
|
||||
.Distinct(StringComparer.OrdinalIgnoreCase)
|
||||
.ToArray();
|
||||
|
||||
if (matches.Length == 1)
|
||||
{
|
||||
return matches[0];
|
||||
}
|
||||
|
||||
throw new FileNotFoundException(
|
||||
$"未找到配置文件 '{configPath}'。已检查: {string.Join(", ", checkedPaths)}",
|
||||
configPath);
|
||||
@@ -106,15 +93,11 @@ public static class PathCompatibility
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 枚举旧系统中最常见的配置候选路径。
|
||||
/// 枚举当前服务配置目录下允许的配置候选路径。
|
||||
/// </summary>
|
||||
private static IEnumerable<string> BuildConfigCandidates(string repoRoot, string rawPath, string fileName)
|
||||
private static IEnumerable<string> BuildConfigCandidates(string repoRoot, string rawPath)
|
||||
{
|
||||
yield return Path.Combine(repoRoot, rawPath);
|
||||
yield return Path.Combine(repoRoot, "Rvbust", "Data", fileName);
|
||||
yield return Path.Combine(repoRoot, "Rvbust", "Install", "FlyingShot", "Config", fileName);
|
||||
yield return Path.Combine(repoRoot, "Rvbust", fileName);
|
||||
yield return Path.Combine(repoRoot, fileName);
|
||||
yield return Path.Combine(repoRoot, "Config", rawPath);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
|
||||
@@ -1,5 +1,6 @@
|
||||
using System.Text.Json;
|
||||
using Flyshot.Core.Domain;
|
||||
using Microsoft.Extensions.Logging;
|
||||
|
||||
namespace Flyshot.Core.Config;
|
||||
|
||||
@@ -15,9 +16,13 @@ public sealed class CompatibilityRobotSettings
|
||||
bool useDo,
|
||||
IEnumerable<int> ioAddresses,
|
||||
int ioKeepCycles,
|
||||
int triggerSampleIndexOffsetCycles,
|
||||
double accLimitScale,
|
||||
double jerkLimitScale,
|
||||
int adaptIcspTryNum)
|
||||
int adaptIcspTryNum,
|
||||
double planningSpeedScale = 1.0,
|
||||
double speedRatio = 1.0,
|
||||
bool smoothStartStopTiming = true)
|
||||
{
|
||||
ArgumentNullException.ThrowIfNull(ioAddresses);
|
||||
|
||||
@@ -26,6 +31,11 @@ public sealed class CompatibilityRobotSettings
|
||||
throw new ArgumentOutOfRangeException(nameof(ioKeepCycles), "IO 保持周期不能为负数。");
|
||||
}
|
||||
|
||||
if (triggerSampleIndexOffsetCycles < 0)
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(nameof(triggerSampleIndexOffsetCycles), "触发 sample 偏移周期不能为负数。");
|
||||
}
|
||||
|
||||
if (accLimitScale <= 0.0)
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(nameof(accLimitScale), "加速度倍率必须大于 0。");
|
||||
@@ -36,6 +46,16 @@ public sealed class CompatibilityRobotSettings
|
||||
throw new ArgumentOutOfRangeException(nameof(jerkLimitScale), "Jerk 倍率必须大于 0。");
|
||||
}
|
||||
|
||||
if (planningSpeedScale <= 0.0 || double.IsNaN(planningSpeedScale) || double.IsInfinity(planningSpeedScale))
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(nameof(planningSpeedScale), "规划速度倍率必须是有限正数。");
|
||||
}
|
||||
|
||||
if (speedRatio <= 0.0 || speedRatio > 1.0 || double.IsNaN(speedRatio) || double.IsInfinity(speedRatio))
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(nameof(speedRatio), "运行时速度倍率必须在 (0, 1] 范围内。");
|
||||
}
|
||||
|
||||
if (adaptIcspTryNum < 0)
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(nameof(adaptIcspTryNum), "补点尝试次数不能为负数。");
|
||||
@@ -51,9 +71,13 @@ public sealed class CompatibilityRobotSettings
|
||||
UseDo = useDo;
|
||||
IoAddresses = copiedIoAddresses;
|
||||
IoKeepCycles = ioKeepCycles;
|
||||
TriggerSampleIndexOffsetCycles = triggerSampleIndexOffsetCycles;
|
||||
AccLimitScale = accLimitScale;
|
||||
JerkLimitScale = jerkLimitScale;
|
||||
AdaptIcspTryNum = adaptIcspTryNum;
|
||||
PlanningSpeedScale = planningSpeedScale;
|
||||
SpeedRatio = speedRatio;
|
||||
SmoothStartStopTiming = smoothStartStopTiming;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
@@ -71,6 +95,12 @@ public sealed class CompatibilityRobotSettings
|
||||
/// </summary>
|
||||
public int IoKeepCycles { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取触发绑定到最佳 sample 后，还要再向后偏移的命令周期数。
|
||||
/// 该值作用在 J519 命令 sample 时间轴，不是 60015 状态反馈周期。
|
||||
/// </summary>
|
||||
public int TriggerSampleIndexOffsetCycles { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取加速度全局倍率。
|
||||
/// </summary>
|
||||
@@ -81,6 +111,21 @@ public sealed class CompatibilityRobotSettings
|
||||
/// </summary>
|
||||
public double JerkLimitScale { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取规划阶段的全局速度倍率，只影响 JointTraj 基准时间，不等同于运行时 J519 下发速度倍率。
|
||||
/// </summary>
|
||||
public double PlanningSpeedScale { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取运行时默认速度倍率，会在 SetUpRobot 时同步到控制器运行时，供 MoveJoint 和后续执行链路使用。
|
||||
/// </summary>
|
||||
public double SpeedRatio { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取是否在飞拍执行前对整段时间轴做二次平滑起停重映射。
|
||||
/// </summary>
|
||||
public bool SmoothStartStopTiming { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取自适应补点最大尝试次数。
|
||||
/// </summary>
|
||||
@@ -131,6 +176,17 @@ public sealed class LoadedRobotConfig
|
||||
/// </summary>
|
||||
public sealed class RobotConfigLoader
|
||||
{
|
||||
private readonly ILogger<RobotConfigLoader>? _logger;
|
||||
|
||||
/// <summary>
|
||||
/// 初始化 RobotConfigLoader。
|
||||
/// </summary>
|
||||
/// <param name="logger">日志记录器；允许 null。</param>
|
||||
public RobotConfigLoader(ILogger<RobotConfigLoader>? logger = null)
|
||||
{
|
||||
_logger = logger;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 加载一份旧版 RobotConfig.json。
|
||||
/// </summary>
|
||||
@@ -139,6 +195,8 @@ public sealed class RobotConfigLoader
|
||||
/// <returns>规范化后的配置文档。</returns>
|
||||
public LoadedRobotConfig Load(string configPath, string? repoRoot = null)
|
||||
{
|
||||
_logger?.LogInformation("RobotConfig 开始加载: configPath={ConfigPath}, repoRoot={RepoRoot}", configPath, repoRoot);
|
||||
|
||||
var resolvedRepoRoot = ResolveRepoRoot(repoRoot);
|
||||
var resolvedConfigPath = PathCompatibility.ResolveConfigPath(configPath, resolvedRepoRoot);
|
||||
|
||||
@@ -151,9 +209,13 @@ public sealed class RobotConfigLoader
|
||||
useDo: ReadBoolean(robotElement, "use_do", defaultValue: false),
|
||||
ioAddresses: ReadIntArray(robotElement, "io_addr"),
|
||||
ioKeepCycles: ReadInt(robotElement, "io_keep_cycles", defaultValue: 0),
|
||||
triggerSampleIndexOffsetCycles: ReadInt(robotElement, "trigger_sample_index_offset_cycles", defaultValue: 7),
|
||||
accLimitScale: ReadDouble(robotElement, "acc_limit", defaultValue: 1.0),
|
||||
jerkLimitScale: ReadDouble(robotElement, "jerk_limit", defaultValue: 1.0),
|
||||
adaptIcspTryNum: ReadInt(robotElement, "adapt_icsp_try_num", defaultValue: 0));
|
||||
adaptIcspTryNum: ReadInt(robotElement, "adapt_icsp_try_num", defaultValue: 0),
|
||||
planningSpeedScale: ReadDouble(robotElement, "planning_speed_scale", defaultValue: 1.0),
|
||||
speedRatio: ReadDouble(robotElement, "speed_ratio", defaultValue: 1.0),
|
||||
smoothStartStopTiming: ReadBoolean(robotElement, "smooth_start_stop_timing", defaultValue: true));
|
||||
|
||||
var programs = new Dictionary<string, FlyshotProgram>(StringComparer.Ordinal);
|
||||
foreach (var programElement in flyingShotsElement.EnumerateObject())
|
||||
@@ -163,6 +225,10 @@ public sealed class RobotConfigLoader
|
||||
programs.Add(programName, program);
|
||||
}
|
||||
|
||||
_logger?.LogInformation(
|
||||
"RobotConfig 加载完成: resolvedPath={ResolvedPath}, useDo={UseDo}, ioKeepCycles={IoKeepCycles}, accLimit={AccLimit}, jerkLimit={JerkLimit}, planningSpeedScale={PlanningSpeedScale}, speedRatio={SpeedRatio}, smoothStartStopTiming={SmoothStartStopTiming}, adaptIcspTryNum={AdaptIcspTryNum}, 程序数={ProgramCount}",
|
||||
resolvedConfigPath, robot.UseDo, robot.IoKeepCycles, robot.AccLimitScale, robot.JerkLimitScale, robot.PlanningSpeedScale, robot.SpeedRatio, robot.SmoothStartStopTiming, robot.AdaptIcspTryNum, programs.Count);
|
||||
|
||||
return new LoadedRobotConfig(
|
||||
sourcePath: resolvedConfigPath,
|
||||
robot: robot,
|
||||
@@ -253,7 +319,7 @@ public sealed class RobotConfigLoader
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 推断仓库根目录，优先使用调用方显式传入的值。
|
||||
/// 推断当前 replacement 仓库根目录，优先使用调用方显式传入的值。
|
||||
/// </summary>
|
||||
private static string ResolveRepoRoot(string? repoRoot)
|
||||
{
|
||||
@@ -267,7 +333,7 @@ public sealed class RobotConfigLoader
|
||||
{
|
||||
if (File.Exists(Path.Combine(current.FullName, "FlyshotReplacement.sln")))
|
||||
{
|
||||
return Path.GetFullPath(Path.Combine(current.FullName, ".."));
|
||||
return current.FullName;
|
||||
}
|
||||
|
||||
current = current.Parent;
|
||||
|
||||
@@ -1,28 +1,49 @@
|
||||
using System.Text;
|
||||
using System.Text.Json;
|
||||
using Flyshot.Core.Domain;
|
||||
using Microsoft.Extensions.Logging;
|
||||
|
||||
namespace Flyshot.Core.Config;
|
||||
|
||||
/// <summary>
|
||||
/// 从旧版 .robot(GLB) 文件中提取关节限制、模型名和 couple 元数据。
|
||||
/// 从现场固化的机器人 JSON 模型中提取关节限制、几何链和 couple 元数据。
|
||||
/// </summary>
|
||||
public sealed class RobotModelLoader
|
||||
{
|
||||
private const uint JsonChunkType = 0x4E4F534A;
|
||||
private readonly ILogger<RobotModelLoader>? _logger;
|
||||
|
||||
/// <summary>
|
||||
/// 加载 .robot 文件并生成规划侧可直接消费的 RobotProfile。
|
||||
/// 初始化 RobotModelLoader。
|
||||
/// </summary>
|
||||
/// <param name="modelPath">.robot 文件路径。</param>
|
||||
/// <param name="logger">日志记录器；允许 null。</param>
|
||||
public RobotModelLoader(ILogger<RobotModelLoader>? logger = null)
|
||||
{
|
||||
_logger = logger;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 加载机器人 JSON 文件并生成规划侧可直接消费的 RobotProfile。
|
||||
/// </summary>
|
||||
/// <param name="modelPath">机器人 JSON 文件路径。</param>
|
||||
/// <param name="accLimitScale">加速度全局倍率。</param>
|
||||
/// <param name="jerkLimitScale">Jerk 全局倍率。</param>
|
||||
/// <returns>包含关节限制和 couple 信息的 RobotProfile。</returns>
|
||||
public RobotProfile LoadProfile(string modelPath, double accLimitScale = 1.0, double jerkLimitScale = 1.0)
|
||||
{
|
||||
return LoadProfileAndKinematics(modelPath, accLimitScale, jerkLimitScale).Profile;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 加载机器人 JSON 文件并一次性生成规划约束视图与运动学几何视图。
|
||||
/// </summary>
|
||||
/// <param name="modelPath">机器人 JSON 文件路径。</param>
|
||||
/// <param name="accLimitScale">加速度全局倍率。</param>
|
||||
/// <param name="jerkLimitScale">Jerk 全局倍率。</param>
|
||||
/// <returns>包含规划约束视图和运动学几何视图的加载结果。</returns>
|
||||
public LoadedRobotModel LoadProfileAndKinematics(string modelPath, double accLimitScale = 1.0, double jerkLimitScale = 1.0)
|
||||
{
|
||||
if (string.IsNullOrWhiteSpace(modelPath))
|
||||
{
|
||||
throw new ArgumentException(".robot 路径不能为空。", nameof(modelPath));
|
||||
throw new ArgumentException("机器人 JSON 路径不能为空。", nameof(modelPath));
|
||||
}
|
||||
|
||||
if (accLimitScale <= 0.0)
|
||||
@@ -35,15 +56,72 @@ public sealed class RobotModelLoader
|
||||
throw new ArgumentOutOfRangeException(nameof(jerkLimitScale), "Jerk 倍率必须大于 0。");
|
||||
}
|
||||
|
||||
_logger?.LogInformation("RobotModel JSON 开始加载: modelPath={ModelPath}, accLimitScale={AccLimitScale}, jerkLimitScale={JerkLimitScale}", modelPath, accLimitScale, jerkLimitScale);
|
||||
|
||||
var resolvedModelPath = Path.GetFullPath(modelPath);
|
||||
var jsonText = ReadJsonChunk(resolvedModelPath);
|
||||
using var document = JsonDocument.Parse(jsonText);
|
||||
using var document = JsonDocument.Parse(File.ReadAllText(resolvedModelPath));
|
||||
|
||||
var robotBody = FindRobotBody(document.RootElement);
|
||||
var profileName = robotBody.TryGetProperty("name", out var nameElement)
|
||||
? nameElement.GetString() ?? Path.GetFileNameWithoutExtension(resolvedModelPath)
|
||||
: Path.GetFileNameWithoutExtension(resolvedModelPath);
|
||||
|
||||
var profile = BuildProfile(robotBody, profileName, resolvedModelPath, accLimitScale, jerkLimitScale);
|
||||
var kinematicsModel = BuildKinematicsModel(robotBody, profileName);
|
||||
|
||||
_logger?.LogInformation(
|
||||
"RobotModel JSON 加载完成: profileName={ProfileName}, dof={Dof}, 几何关节数={JointCount}, resolvedPath={ResolvedPath}",
|
||||
profile.Name, profile.DegreesOfFreedom, kinematicsModel.Joints.Count, resolvedModelPath);
|
||||
|
||||
return new LoadedRobotModel(profile, kinematicsModel);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 加载机器人 JSON 文件并生成运动学侧需要的完整几何模型。
|
||||
/// </summary>
|
||||
/// <param name="modelPath">机器人 JSON 文件路径。</param>
|
||||
/// <returns>包含完整关节几何链的运动学模型。</returns>
|
||||
public RobotKinematicsModel LoadKinematicsModel(string modelPath)
|
||||
{
|
||||
return LoadProfileAndKinematics(modelPath).KinematicsModel;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 在 robotics.bodies 中找到当前现场机器人主体。
|
||||
/// </summary>
|
||||
private static JsonElement FindRobotBody(JsonElement root)
|
||||
{
|
||||
var bodies = root
|
||||
.GetProperty("scenes")[0]
|
||||
.GetProperty("extras")
|
||||
.GetProperty("rvbust")
|
||||
.GetProperty("robotics")
|
||||
.GetProperty("bodies");
|
||||
|
||||
foreach (var body in bodies.EnumerateArray())
|
||||
{
|
||||
if (body.TryGetProperty("type", out var typeElement) && typeElement.GetInt32() == 2)
|
||||
{
|
||||
return body;
|
||||
}
|
||||
}
|
||||
|
||||
foreach (var body in bodies.EnumerateArray())
|
||||
{
|
||||
if (body.TryGetProperty("joints", out _) && body.TryGetProperty("name", out _))
|
||||
{
|
||||
return body;
|
||||
}
|
||||
}
|
||||
|
||||
throw new InvalidDataException("未在机器人 JSON 中找到包含 joints 的机器人主体。");
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 从机器人主体构造规划约束视图。
|
||||
/// </summary>
|
||||
private RobotProfile BuildProfile(JsonElement robotBody, string profileName, string resolvedModelPath, double accLimitScale, double jerkLimitScale)
|
||||
{
|
||||
var jointLimits = new List<JointLimit>();
|
||||
var jointCouplings = new List<JointCoupling>();
|
||||
|
||||
@@ -67,15 +145,22 @@ public sealed class RobotModelLoader
|
||||
{
|
||||
var masterJointName = coupleElement.GetProperty("master_joint").GetString()
|
||||
?? throw new InvalidDataException($"关节 {jointName} 的 couple 缺少 master_joint。");
|
||||
|
||||
var multiplier = coupleElement.TryGetProperty("multiplier", out var multiplierElement) ? multiplierElement.GetDouble() : 0.0;
|
||||
var offset = coupleElement.TryGetProperty("offset", out var offsetElement) ? offsetElement.GetDouble() : 0.0;
|
||||
jointCouplings.Add(new JointCoupling(
|
||||
slaveJointName: jointName,
|
||||
masterJointName: masterJointName,
|
||||
multiplier: coupleElement.TryGetProperty("multiplier", out var multiplierElement) ? multiplierElement.GetDouble() : 0.0,
|
||||
offset: coupleElement.TryGetProperty("offset", out var offsetElement) ? offsetElement.GetDouble() : 0.0));
|
||||
multiplier: multiplier,
|
||||
offset: offset));
|
||||
_logger?.LogInformation("关节 {JointName} 的耦合关系: 主关节={MasterJointName}, 比例={Multiplier}, 偏移={Offset}", jointName, masterJointName, multiplier, offset);
|
||||
}
|
||||
}
|
||||
|
||||
foreach (var jointLimit in jointLimits)
|
||||
{
|
||||
_logger?.LogInformation("关节 {JointName} 的限制值: 速度={VelocityLimit}, 加速度={AccelerationLimit}, Jerk={JerkLimit}", jointLimit.JointName, jointLimit.VelocityLimit, jointLimit.AccelerationLimit, jointLimit.JerkLimit);
|
||||
}
|
||||
|
||||
return new RobotProfile(
|
||||
name: profileName,
|
||||
modelPath: resolvedModelPath,
|
||||
@@ -87,99 +172,32 @@ public sealed class RobotModelLoader
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 从 GLB 文件中提取 JSON chunk 文本。
|
||||
/// 从机器人主体构造正运动学几何视图。
|
||||
/// </summary>
|
||||
private static string ReadJsonChunk(string modelPath)
|
||||
private RobotKinematicsModel BuildKinematicsModel(JsonElement robotBody, string profileName)
|
||||
{
|
||||
using var stream = File.OpenRead(modelPath);
|
||||
using var reader = new BinaryReader(stream, Encoding.UTF8, leaveOpen: false);
|
||||
|
||||
var magic = Encoding.ASCII.GetString(reader.ReadBytes(4));
|
||||
if (!string.Equals(magic, "glTF", StringComparison.Ordinal))
|
||||
{
|
||||
throw new InvalidDataException($"{modelPath} 不是合法的 GLB 文件。");
|
||||
}
|
||||
|
||||
var version = reader.ReadUInt32();
|
||||
if (version != 2)
|
||||
{
|
||||
throw new NotSupportedException($"当前仅支持 GLB 2.0，实际版本为 {version}。");
|
||||
}
|
||||
|
||||
var totalLength = reader.ReadUInt32();
|
||||
while (stream.Position < totalLength)
|
||||
{
|
||||
var chunkLength = reader.ReadUInt32();
|
||||
var chunkType = reader.ReadUInt32();
|
||||
var chunkBytes = reader.ReadBytes((int)chunkLength);
|
||||
if (chunkType == JsonChunkType)
|
||||
{
|
||||
return Encoding.UTF8.GetString(chunkBytes);
|
||||
}
|
||||
}
|
||||
|
||||
throw new InvalidDataException($"{modelPath} 不包含 JSON chunk。");
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 在 robotics.bodies 中找到 type=2 的机器人主体。
|
||||
/// </summary>
|
||||
private static JsonElement FindRobotBody(JsonElement root)
|
||||
{
|
||||
var bodies = root
|
||||
.GetProperty("scenes")[0]
|
||||
.GetProperty("extras")
|
||||
.GetProperty("rvbust")
|
||||
.GetProperty("robotics")
|
||||
.GetProperty("bodies");
|
||||
|
||||
foreach (var body in bodies.EnumerateArray())
|
||||
{
|
||||
if (body.TryGetProperty("type", out var typeElement) && typeElement.GetInt32() == 2)
|
||||
{
|
||||
return body;
|
||||
}
|
||||
}
|
||||
|
||||
throw new InvalidDataException("未在 .robot 文件中找到 type=2 的机器人主体。");
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 加载 .robot 文件并生成运动学侧需要的完整几何模型。
|
||||
/// </summary>
|
||||
/// <param name="modelPath">.robot 文件路径。</param>
|
||||
/// <returns>包含完整关节几何链的运动学模型。</returns>
|
||||
public RobotKinematicsModel LoadKinematicsModel(string modelPath)
|
||||
{
|
||||
if (string.IsNullOrWhiteSpace(modelPath))
|
||||
{
|
||||
throw new ArgumentException(".robot 路径不能为空。", nameof(modelPath));
|
||||
}
|
||||
|
||||
var resolvedModelPath = Path.GetFullPath(modelPath);
|
||||
var jsonText = ReadJsonChunk(resolvedModelPath);
|
||||
using var document = JsonDocument.Parse(jsonText);
|
||||
|
||||
var robotBody = FindRobotBody(document.RootElement);
|
||||
var profileName = robotBody.TryGetProperty("name", out var nameElement)
|
||||
? nameElement.GetString() ?? Path.GetFileNameWithoutExtension(resolvedModelPath)
|
||||
: Path.GetFileNameWithoutExtension(resolvedModelPath);
|
||||
|
||||
var joints = new List<RobotJointGeometry>();
|
||||
foreach (var jointElement in robotBody.GetProperty("joints").EnumerateArray())
|
||||
{
|
||||
var jointName = jointElement.GetProperty("name").GetString()
|
||||
?? throw new InvalidDataException("关节缺少 name。");
|
||||
// jointType: 关节类型编码；用于区分旋转关节/其他结构关节，后续几何链路可据此决定求解策略。
|
||||
var jointType = jointElement.TryGetProperty("type", out var typeElement)
|
||||
? typeElement.GetInt32()
|
||||
: 0;
|
||||
// origin: 关节局部原点配置，格式通常为 [x, y, z, qx, qy, qz, qw]，定义父坐标到关节坐标的位姿。
|
||||
var origin = jointElement.GetProperty("origin").EnumerateArray().Select(static e => e.GetDouble()).ToArray();
|
||||
// axis: 关节运动轴；部分模型为 4 元组 [x, y, z, scale]，其中方向向量用于正运动学雅可比计算。
|
||||
var axis = jointElement.GetProperty("axis").EnumerateArray().Select(static e => e.GetDouble()).ToArray();
|
||||
// axis 字段有时存的是 4 元组 [x, y, z, scale]，取最后 3 个作为方向向量。
|
||||
var axisVector = axis.Length >= 3 ? axis[^3..] : axis;
|
||||
// originXyz: 平移分量 (x,y,z)，用于构建关节在父链路下的位置偏移。
|
||||
var originXyz = origin.Length >= 3 ? origin[..3] : origin;
|
||||
var originQuat = origin.Length >= 7 ? origin[3..7] : new double[] { 0.0, 0.0, 0.0, 1.0 };
|
||||
// originQuat: 旋转分量 (qx,qy,qz,qw)，用于构建关节在父链路下的姿态；缺省时回退单位四元数。
|
||||
var originQuat = origin.Length >= 7 ? origin[3..7] : [0.0, 0.0, 0.0, 1.0];
|
||||
|
||||
// coupleMaster/coupleMultiplier/coupleOffset: 关节耦合参数，描述 slave 关节如何由 master 关节线性映射得到。
|
||||
// 典型关系: slave = master * multiplier + offset。
|
||||
string? coupleMaster = null;
|
||||
double coupleMultiplier = 0.0;
|
||||
double coupleOffset = 0.0;
|
||||
@@ -190,19 +208,45 @@ public sealed class RobotModelLoader
|
||||
coupleOffset = coupleElement.TryGetProperty("offset", out var o) ? o.GetDouble() : 0.0;
|
||||
}
|
||||
|
||||
var parentLink = jointElement.GetProperty("parent").GetString() ?? string.Empty;
|
||||
var childLink = jointElement.GetProperty("child").GetString() ?? string.Empty;
|
||||
_logger?.LogInformation(
|
||||
"几何关节解析: name={JointName}, parent={Parent}, child={Child}, type={JointType}, axis={Axis}, originXyz={OriginXyz}, originQuat={OriginQuat}, coupleMaster={CoupleMaster}, coupleMultiplier={CoupleMultiplier}, coupleOffset={CoupleOffset}",
|
||||
jointName,
|
||||
parentLink,
|
||||
childLink,
|
||||
jointType,
|
||||
string.Join(", ", axisVector.Select(static v => v.ToString("G17"))),
|
||||
string.Join(", ", originXyz.Select(static v => v.ToString("G17"))),
|
||||
string.Join(", ", originQuat.Select(static v => v.ToString("G17"))),
|
||||
coupleMaster ?? "<none>",
|
||||
coupleMultiplier,
|
||||
coupleOffset);
|
||||
|
||||
joints.Add(new RobotJointGeometry(
|
||||
// name: 当前关节名，作为几何链和耦合关系的主键。
|
||||
name: jointName,
|
||||
parent: jointElement.GetProperty("parent").GetString() ?? string.Empty,
|
||||
child: jointElement.GetProperty("child").GetString() ?? string.Empty,
|
||||
// parent: 父 link 名称，用于串起机器人树结构。
|
||||
parent: parentLink,
|
||||
// child: 子 link 名称，标识该关节输出到哪个连杆。
|
||||
child: childLink,
|
||||
// jointType: 关节类型编码，供运动学模型区分计算路径。
|
||||
jointType: jointType,
|
||||
// axis: 关节轴方向向量，决定旋转/平移沿哪个局部方向发生。
|
||||
axis: axisVector,
|
||||
// originXyz: 关节原点平移分量。
|
||||
originXyz: originXyz,
|
||||
// originQuatXyzw: 关节原点旋转四元数分量。
|
||||
originQuatXyzw: originQuat,
|
||||
// coupleMaster: 耦合主关节名（无耦合时为 null）。
|
||||
coupleMaster: coupleMaster,
|
||||
// coupleMultiplier: 耦合线性比例系数。
|
||||
coupleMultiplier: coupleMultiplier,
|
||||
// coupleOffset: 耦合常量偏移量。
|
||||
coupleOffset: coupleOffset));
|
||||
}
|
||||
|
||||
_logger?.LogInformation("几何模型构建完成: profileName={ProfileName}, jointCount={JointCount}", profileName, joints.Count);
|
||||
return new RobotKinematicsModel(name: profileName, joints: joints);
|
||||
}
|
||||
|
||||
|
||||
@@ -18,7 +18,14 @@ public sealed class ControllerStateSnapshot
|
||||
double speedRatio,
|
||||
IEnumerable<double>? jointPositions = null,
|
||||
IEnumerable<double>? cartesianPose = null,
|
||||
IEnumerable<RuntimeAlarm>? activeAlarms = null)
|
||||
IEnumerable<RuntimeAlarm>? activeAlarms = null,
|
||||
IEnumerable<uint>? stateTailWords = null,
|
||||
byte? j519Status = null,
|
||||
uint? j519Sequence = null,
|
||||
bool? j519AcceptsCommand = null,
|
||||
bool? j519ReceivedCommand = null,
|
||||
bool? j519SystemReady = null,
|
||||
bool? j519RobotInMotion = null)
|
||||
{
|
||||
if (string.IsNullOrWhiteSpace(connectionState))
|
||||
{
|
||||
@@ -34,6 +41,7 @@ public sealed class ControllerStateSnapshot
|
||||
var copiedJointPositions = jointPositions?.ToArray() ?? Array.Empty<double>();
|
||||
var copiedCartesianPose = cartesianPose?.ToArray() ?? Array.Empty<double>();
|
||||
var copiedActiveAlarms = activeAlarms?.ToArray() ?? Array.Empty<RuntimeAlarm>();
|
||||
var copiedStateTailWords = stateTailWords?.ToArray() ?? Array.Empty<uint>();
|
||||
|
||||
CapturedAt = capturedAt;
|
||||
ConnectionState = connectionState;
|
||||
@@ -43,6 +51,13 @@ public sealed class ControllerStateSnapshot
|
||||
JointPositions = copiedJointPositions;
|
||||
CartesianPose = copiedCartesianPose;
|
||||
ActiveAlarms = copiedActiveAlarms;
|
||||
StateTailWords = copiedStateTailWords;
|
||||
J519Status = j519Status;
|
||||
J519Sequence = j519Sequence;
|
||||
J519AcceptsCommand = j519AcceptsCommand;
|
||||
J519ReceivedCommand = j519ReceivedCommand;
|
||||
J519SystemReady = j519SystemReady;
|
||||
J519RobotInMotion = j519RobotInMotion;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
@@ -92,4 +107,46 @@ public sealed class ControllerStateSnapshot
|
||||
/// </summary>
|
||||
[JsonPropertyName("activeAlarms")]
|
||||
public IReadOnlyList<RuntimeAlarm> ActiveAlarms { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取 TCP 10010 状态帧尾部原始状态字，仅用于诊断，不直接推断运行语义。
|
||||
/// </summary>
|
||||
[JsonPropertyName("stateTailWords")]
|
||||
public IReadOnlyList<uint> StateTailWords { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取最近一次 UDP 60015 J519 响应的原始状态字节；没有响应时为 null。
|
||||
/// </summary>
|
||||
[JsonPropertyName("j519Status")]
|
||||
public byte? J519Status { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取最近一次 UDP 60015 J519 响应序号；没有响应时为 null。
|
||||
/// </summary>
|
||||
[JsonPropertyName("j519Sequence")]
|
||||
public uint? J519Sequence { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取 J519 accept_cmd 状态位；没有响应时为 null。
|
||||
/// </summary>
|
||||
[JsonPropertyName("j519AcceptsCommand")]
|
||||
public bool? J519AcceptsCommand { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取 J519 received_cmd 状态位；没有响应时为 null。
|
||||
/// </summary>
|
||||
[JsonPropertyName("j519ReceivedCommand")]
|
||||
public bool? J519ReceivedCommand { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取 J519 sysrdy 状态位；没有响应时为 null。
|
||||
/// </summary>
|
||||
[JsonPropertyName("j519SystemReady")]
|
||||
public bool? J519SystemReady { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取 J519 rbt_inmotion 状态位；没有响应时为 null。
|
||||
/// </summary>
|
||||
[JsonPropertyName("j519RobotInMotion")]
|
||||
public bool? J519RobotInMotion { get; }
|
||||
}
|
||||
|
||||
202
src/Flyshot.Core.Domain/FlyshotPreparedExecution.cs
Normal file
202
src/Flyshot.Core.Domain/FlyshotPreparedExecution.cs
Normal file
@@ -0,0 +1,202 @@
|
||||
namespace Flyshot.Core.Domain;
|
||||
|
||||
/// <summary>
|
||||
/// 表示飞拍链路在进入运行时之前就已经准备完成的最终发送结果。
|
||||
/// </summary>
|
||||
public sealed class FlyshotPreparedExecution
|
||||
{
|
||||
/// <summary>
|
||||
/// 初始化一份飞拍最终发送结果。
|
||||
/// </summary>
|
||||
/// <param name="samples">最终 8ms 发送点列。</param>
|
||||
/// <param name="triggerBindings">与最终发送点列对齐的触发绑定结果。</param>
|
||||
/// <param name="timingRows">与最终发送点列一致的时间映射诊断行。</param>
|
||||
/// <param name="jerkRows">与最终发送点列一致的跃度诊断行。</param>
|
||||
/// <param name="requestSpeedRatio">请求的执行倍率。</param>
|
||||
/// <param name="finalSpeedRatio">最终发送队列采用的采样倍率；请求倍率已折算进规划时为 1。</param>
|
||||
/// <param name="finalDurationSeconds">最终发送总时长，单位为秒。</param>
|
||||
/// <param name="stretchIterationCount">历史诊断字段；当前执行侧不自动改写 speed_ratio，因此正常为 0。</param>
|
||||
public FlyshotPreparedExecution(
|
||||
IEnumerable<FlyshotPreparedSample> samples,
|
||||
IEnumerable<FlyshotPreparedTriggerBinding> triggerBindings,
|
||||
IEnumerable<IReadOnlyList<double>> timingRows,
|
||||
IEnumerable<IReadOnlyList<double>> jerkRows,
|
||||
double requestSpeedRatio,
|
||||
double finalSpeedRatio,
|
||||
double finalDurationSeconds,
|
||||
int stretchIterationCount)
|
||||
{
|
||||
ArgumentNullException.ThrowIfNull(samples);
|
||||
ArgumentNullException.ThrowIfNull(triggerBindings);
|
||||
ArgumentNullException.ThrowIfNull(timingRows);
|
||||
ArgumentNullException.ThrowIfNull(jerkRows);
|
||||
|
||||
if (requestSpeedRatio <= 0.0 || double.IsNaN(requestSpeedRatio) || double.IsInfinity(requestSpeedRatio))
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(nameof(requestSpeedRatio), "请求速度倍率必须是有限正数。");
|
||||
}
|
||||
|
||||
if (finalSpeedRatio <= 0.0 || double.IsNaN(finalSpeedRatio) || double.IsInfinity(finalSpeedRatio))
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(nameof(finalSpeedRatio), "最终速度倍率必须是有限正数。");
|
||||
}
|
||||
|
||||
if (finalDurationSeconds < 0.0 || double.IsNaN(finalDurationSeconds) || double.IsInfinity(finalDurationSeconds))
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(nameof(finalDurationSeconds), "最终发送总时长必须是有限非负数。");
|
||||
}
|
||||
|
||||
if (stretchIterationCount < 0)
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(nameof(stretchIterationCount), "倍率改写迭代次数必须是非负整数。");
|
||||
}
|
||||
|
||||
Samples = samples.Select(static sample => sample).ToArray();
|
||||
TriggerBindings = triggerBindings.Select(static binding => binding).ToArray();
|
||||
TimingRows = timingRows.Select(static row => row.ToArray()).ToArray();
|
||||
JerkRows = jerkRows.Select(static row => row.ToArray()).ToArray();
|
||||
RequestSpeedRatio = requestSpeedRatio;
|
||||
FinalSpeedRatio = finalSpeedRatio;
|
||||
FinalDurationSeconds = finalDurationSeconds;
|
||||
StretchIterationCount = stretchIterationCount;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 获取最终 8ms 发送点列。
|
||||
/// </summary>
|
||||
public IReadOnlyList<FlyshotPreparedSample> Samples { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取与最终发送点列对齐的触发绑定结果。
|
||||
/// </summary>
|
||||
public IReadOnlyList<FlyshotPreparedTriggerBinding> TriggerBindings { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取与最终发送点列一致的时间映射诊断行。
|
||||
/// </summary>
|
||||
public IReadOnlyList<IReadOnlyList<double>> TimingRows { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取与最终发送点列一致的跃度诊断行。
|
||||
/// </summary>
|
||||
public IReadOnlyList<IReadOnlyList<double>> JerkRows { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取请求的执行倍率。
|
||||
/// </summary>
|
||||
public double RequestSpeedRatio { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取最终发送队列采用的采样倍率；请求倍率保留在 <see cref="RequestSpeedRatio"/> 中用于诊断。
|
||||
/// </summary>
|
||||
public double FinalSpeedRatio { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取最终发送总时长，单位为秒。
|
||||
/// </summary>
|
||||
public double FinalDurationSeconds { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取历史诊断字段；当前执行侧不自动改写 speed_ratio，因此正常为 0。
|
||||
/// </summary>
|
||||
public int StretchIterationCount { get; }
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 表示飞拍最终发送队列中的一个 8ms 发送点。
|
||||
/// </summary>
|
||||
public sealed class FlyshotPreparedSample
|
||||
{
|
||||
/// <summary>
|
||||
/// 初始化一条最终发送点。
|
||||
/// </summary>
|
||||
/// <param name="sampleIndex">发送周期序号。</param>
|
||||
/// <param name="sendTime">物理发送时间，单位为秒。</param>
|
||||
/// <param name="trajectoryTime">回映射到规划轨迹的采样时间，单位为秒。</param>
|
||||
/// <param name="speedRatio">生成该发送点时采用的队列采样倍率，已规划调速路径通常为 1。</param>
|
||||
/// <param name="jointsDegrees">J519 下发使用的角度制关节目标。</param>
|
||||
public FlyshotPreparedSample(
|
||||
long sampleIndex,
|
||||
double sendTime,
|
||||
double trajectoryTime,
|
||||
double speedRatio,
|
||||
IReadOnlyList<double> jointsDegrees)
|
||||
{
|
||||
ArgumentNullException.ThrowIfNull(jointsDegrees);
|
||||
SampleIndex = sampleIndex;
|
||||
SendTime = sendTime;
|
||||
TrajectoryTime = trajectoryTime;
|
||||
SpeedRatio = speedRatio;
|
||||
JointsDegrees = jointsDegrees.ToArray();
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 获取发送周期序号。
|
||||
/// </summary>
|
||||
public long SampleIndex { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取物理发送时间，单位为秒。
|
||||
/// </summary>
|
||||
public double SendTime { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取回映射到规划轨迹的采样时间，单位为秒。
|
||||
/// </summary>
|
||||
public double TrajectoryTime { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取生成该发送点时采用的队列采样倍率。
|
||||
/// </summary>
|
||||
public double SpeedRatio { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取 J519 下发使用的角度制关节目标。
|
||||
/// </summary>
|
||||
public IReadOnlyList<double> JointsDegrees { get; }
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 表示理论触发事件最终绑定到哪个发送点的结果。
|
||||
/// </summary>
|
||||
public sealed class FlyshotPreparedTriggerBinding
|
||||
{
|
||||
/// <summary>
|
||||
/// 初始化一条最终触发绑定结果。
|
||||
/// </summary>
|
||||
/// <param name="trigger">理论触发事件。</param>
|
||||
/// <param name="sample">最终绑定的发送点。</param>
|
||||
/// <param name="sampleIndex">最终绑定到的发送点索引。</param>
|
||||
/// <param name="foundInWindow">是否在理论搜索窗口内找到该发送点。</param>
|
||||
public FlyshotPreparedTriggerBinding(
|
||||
TrajectoryDoEvent trigger,
|
||||
FlyshotPreparedSample sample,
|
||||
int sampleIndex,
|
||||
bool foundInWindow)
|
||||
{
|
||||
Trigger = trigger ?? throw new ArgumentNullException(nameof(trigger));
|
||||
Sample = sample ?? throw new ArgumentNullException(nameof(sample));
|
||||
SampleIndex = sampleIndex;
|
||||
FoundInWindow = foundInWindow;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 获取理论触发事件。
|
||||
/// </summary>
|
||||
public TrajectoryDoEvent Trigger { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取最终绑定的发送点。
|
||||
/// </summary>
|
||||
public FlyshotPreparedSample Sample { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取最终绑定到的发送点索引。
|
||||
/// </summary>
|
||||
public int SampleIndex { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取是否在理论搜索窗口内完成绑定。
|
||||
/// </summary>
|
||||
public bool FoundInWindow { get; }
|
||||
}
|
||||
@@ -3,7 +3,7 @@ using System.Text.Json.Serialization;
|
||||
namespace Flyshot.Core.Domain;
|
||||
|
||||
/// <summary>
|
||||
/// 描述机器人运动学链所需的完整关节几何信息，从 .robot GLB 中提取。
|
||||
/// 描述机器人运动学链所需的完整关节几何信息，从现场固化的机器人 JSON 中提取。
|
||||
///
|
||||
/// 为什么与 RobotProfile 分开？
|
||||
/// ---
|
||||
|
||||
@@ -3,12 +3,12 @@ using System.Text.Json.Serialization;
|
||||
namespace Flyshot.Core.Domain;
|
||||
|
||||
/// <summary>
|
||||
/// Describes the robot model contract consumed by planning and runtime orchestration.
|
||||
/// 描述规划与运行时编排共同使用的机器人模型契约。
|
||||
/// </summary>
|
||||
public sealed class RobotProfile
|
||||
{
|
||||
/// <summary>
|
||||
/// Initializes a new robot profile with validated joint limits and coupling metadata.
|
||||
/// 使用已校验的关节约束与耦合元数据初始化机器人画像。
|
||||
/// </summary>
|
||||
public RobotProfile(
|
||||
string name,
|
||||
@@ -47,7 +47,7 @@ public sealed class RobotProfile
|
||||
ArgumentNullException.ThrowIfNull(jointLimits);
|
||||
ArgumentNullException.ThrowIfNull(jointCouplings);
|
||||
|
||||
// Snapshot the collections once so downstream layers cannot mutate domain state in place.
|
||||
// 先对集合做一次快照，避免下游直接原地修改领域状态。
|
||||
var copiedJointLimits = jointLimits.ToArray();
|
||||
var copiedJointCouplings = jointCouplings.ToArray();
|
||||
|
||||
@@ -66,55 +66,55 @@ public sealed class RobotProfile
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Gets the robot profile name exposed to the rest of the runtime.
|
||||
/// 获取对运行时其余模块暴露的机器人画像名称。
|
||||
/// </summary>
|
||||
[JsonPropertyName("name")]
|
||||
public string Name { get; }
|
||||
|
||||
/// <summary>
|
||||
/// Gets the source path of the robot model file.
|
||||
/// 获取机器人模型文件的来源路径。
|
||||
/// </summary>
|
||||
[JsonPropertyName("modelPath")]
|
||||
public string ModelPath { get; }
|
||||
|
||||
/// <summary>
|
||||
/// Gets the active revolute degree-of-freedom count.
|
||||
/// 获取当前生效的旋转关节自由度数量。
|
||||
/// </summary>
|
||||
[JsonPropertyName("degreesOfFreedom")]
|
||||
public int DegreesOfFreedom { get; }
|
||||
|
||||
/// <summary>
|
||||
/// Gets the validated per-joint kinematic limits.
|
||||
/// 获取按关节校验后的运动学约束。
|
||||
/// </summary>
|
||||
[JsonPropertyName("jointLimits")]
|
||||
public IReadOnlyList<JointLimit> JointLimits { get; }
|
||||
|
||||
/// <summary>
|
||||
/// Gets optional joint coupling metadata parsed from the robot model.
|
||||
/// 获取从机器人模型解析出的可选关节耦合元数据。
|
||||
/// </summary>
|
||||
[JsonPropertyName("jointCouplings")]
|
||||
public IReadOnlyList<JointCoupling> JointCouplings { get; }
|
||||
|
||||
/// <summary>
|
||||
/// Gets the servo scheduling period used by the runtime.
|
||||
/// 获取运行时使用的伺服调度周期。
|
||||
/// </summary>
|
||||
[JsonPropertyName("servoPeriod")]
|
||||
public TimeSpan ServoPeriod { get; }
|
||||
|
||||
/// <summary>
|
||||
/// Gets the trigger scheduling period used by shot-event alignment.
|
||||
/// 获取飞拍事件对齐使用的触发调度周期。
|
||||
/// </summary>
|
||||
[JsonPropertyName("triggerPeriod")]
|
||||
public TimeSpan TriggerPeriod { get; }
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Describes a single revolute joint limit set required by the planners.
|
||||
/// 描述规划器所需的单个旋转关节约束集合。
|
||||
/// </summary>
|
||||
public sealed class JointLimit
|
||||
{
|
||||
/// <summary>
|
||||
/// Initializes a validated joint limit record.
|
||||
/// 初始化一个已校验的关节约束记录。
|
||||
/// </summary>
|
||||
public JointLimit(string jointName, double velocityLimit, double accelerationLimit, double jerkLimit)
|
||||
{
|
||||
@@ -145,37 +145,37 @@ public sealed class JointLimit
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Gets the joint name associated with the limits.
|
||||
/// 获取该约束对应的关节名称。
|
||||
/// </summary>
|
||||
[JsonPropertyName("jointName")]
|
||||
public string JointName { get; }
|
||||
|
||||
/// <summary>
|
||||
/// Gets the velocity limit in joint space units.
|
||||
/// 获取关节空间单位下的速度上限。
|
||||
/// </summary>
|
||||
[JsonPropertyName("velocityLimit")]
|
||||
public double VelocityLimit { get; }
|
||||
|
||||
/// <summary>
|
||||
/// Gets the acceleration limit in joint space units.
|
||||
/// 获取关节空间单位下的加速度上限。
|
||||
/// </summary>
|
||||
[JsonPropertyName("accelerationLimit")]
|
||||
public double AccelerationLimit { get; }
|
||||
|
||||
/// <summary>
|
||||
/// Gets the jerk limit in joint space units.
|
||||
/// 获取关节空间单位下的跃度上限。
|
||||
/// </summary>
|
||||
[JsonPropertyName("jerkLimit")]
|
||||
public double JerkLimit { get; }
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Describes a joint-coupling rule that must be applied before kinematics or planning.
|
||||
/// 描述在运动学计算或轨迹规划前必须应用的关节耦合规则。
|
||||
/// </summary>
|
||||
public sealed class JointCoupling
|
||||
{
|
||||
/// <summary>
|
||||
/// Initializes a validated joint-coupling description.
|
||||
/// 初始化一个已校验的关节耦合描述。
|
||||
/// </summary>
|
||||
public JointCoupling(string slaveJointName, string masterJointName, double multiplier, double offset)
|
||||
{
|
||||
@@ -201,25 +201,25 @@ public sealed class JointCoupling
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Gets the dependent joint name.
|
||||
/// 获取从属（被驱动）关节名称。
|
||||
/// </summary>
|
||||
[JsonPropertyName("slaveJointName")]
|
||||
public string SlaveJointName { get; }
|
||||
|
||||
/// <summary>
|
||||
/// Gets the source joint name.
|
||||
/// 获取主导（驱动）关节名称。
|
||||
/// </summary>
|
||||
[JsonPropertyName("masterJointName")]
|
||||
public string MasterJointName { get; }
|
||||
|
||||
/// <summary>
|
||||
/// Gets the coupling multiplier applied to the master joint angle.
|
||||
/// 获取作用在主导关节角度上的耦合倍率。
|
||||
/// </summary>
|
||||
[JsonPropertyName("multiplier")]
|
||||
public double Multiplier { get; }
|
||||
|
||||
/// <summary>
|
||||
/// Gets the additive offset applied after the multiplier.
|
||||
/// 获取在耦合倍率之后叠加的偏移量。
|
||||
/// </summary>
|
||||
[JsonPropertyName("offset")]
|
||||
public double Offset { get; }
|
||||
|
||||
@@ -21,7 +21,11 @@ public sealed class TrajectoryResult
|
||||
string? failureReason,
|
||||
bool usedCache,
|
||||
int originalWaypointCount,
|
||||
int plannedWaypointCount)
|
||||
int plannedWaypointCount,
|
||||
int triggerSampleIndexOffsetCycles = 0,
|
||||
IEnumerable<IReadOnlyList<double>>? denseJointTrajectory = null,
|
||||
FlyshotPreparedExecution? preparedFlyshotExecution = null,
|
||||
IEnumerable<IReadOnlyList<double>>? denseCartesianTrajectory = null)
|
||||
{
|
||||
if (string.IsNullOrWhiteSpace(programName))
|
||||
{
|
||||
@@ -43,6 +47,11 @@ public sealed class TrajectoryResult
|
||||
throw new ArgumentOutOfRangeException(nameof(plannedWaypointCount), "Planned waypoint count must be greater than or equal to the original waypoint count.");
|
||||
}
|
||||
|
||||
if (triggerSampleIndexOffsetCycles < 0)
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(nameof(triggerSampleIndexOffsetCycles), "Trigger sample index offset cycles must be zero or positive.");
|
||||
}
|
||||
|
||||
ArgumentNullException.ThrowIfNull(shotEvents);
|
||||
ArgumentNullException.ThrowIfNull(triggerTimeline);
|
||||
ArgumentNullException.ThrowIfNull(artifacts);
|
||||
@@ -51,6 +60,8 @@ public sealed class TrajectoryResult
|
||||
var copiedShotEvents = shotEvents.ToArray();
|
||||
var copiedTriggerTimeline = triggerTimeline.ToArray();
|
||||
var copiedArtifacts = artifacts.ToArray();
|
||||
var copiedDenseJointTrajectory = denseJointTrajectory?.Select(static row => row.ToArray()).ToArray();
|
||||
var copiedDenseCartesianTrajectory = denseCartesianTrajectory?.Select(static row => row.ToArray()).ToArray();
|
||||
|
||||
ProgramName = programName;
|
||||
Method = method;
|
||||
@@ -63,6 +74,10 @@ public sealed class TrajectoryResult
|
||||
UsedCache = usedCache;
|
||||
OriginalWaypointCount = originalWaypointCount;
|
||||
PlannedWaypointCount = plannedWaypointCount;
|
||||
TriggerSampleIndexOffsetCycles = triggerSampleIndexOffsetCycles;
|
||||
DenseJointTrajectory = copiedDenseJointTrajectory;
|
||||
DenseCartesianTrajectory = copiedDenseCartesianTrajectory;
|
||||
PreparedFlyshotExecution = preparedFlyshotExecution;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
@@ -130,6 +145,32 @@ public sealed class TrajectoryResult
|
||||
/// </summary>
|
||||
[JsonPropertyName("plannedWaypointCount")]
|
||||
public int PlannedWaypointCount { get; }
|
||||
|
||||
/// <summary>
|
||||
/// Gets the configured command-sample offset applied after trigger binding picks the best sample.
|
||||
/// </summary>
|
||||
[JsonPropertyName("triggerSampleIndexOffsetCycles")]
|
||||
public int TriggerSampleIndexOffsetCycles { get; }
|
||||
|
||||
/// <summary>
|
||||
/// Gets the dense joint trajectory samples where each row is [time, j1, j2, ...].
|
||||
/// Null when dense sampling was not performed (e.g. simulation fallback).
|
||||
/// </summary>
|
||||
[JsonPropertyName("denseJointTrajectory")]
|
||||
public IReadOnlyList<IReadOnlyList<double>>? DenseJointTrajectory { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取稠密直角坐标轨迹采样点，每行格式为 [time, x, y, z, w, p, r]。
|
||||
/// Null 表示本结果不是直角流式运动。
|
||||
/// </summary>
|
||||
[JsonPropertyName("denseCartesianTrajectory")]
|
||||
public IReadOnlyList<IReadOnlyList<double>>? DenseCartesianTrajectory { get; }
|
||||
|
||||
/// <summary>
|
||||
/// Gets the prepared flyshot execution queue when the flyshot chain has already built the final 8ms send sequence.
|
||||
/// </summary>
|
||||
[JsonIgnore]
|
||||
public FlyshotPreparedExecution? PreparedFlyshotExecution { get; }
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
@@ -140,7 +181,13 @@ public sealed class TrajectoryDoEvent
|
||||
/// <summary>
|
||||
/// Initializes a validated runtime trigger event.
|
||||
/// </summary>
|
||||
public TrajectoryDoEvent(int waypointIndex, double triggerTime, int offsetCycles, int holdCycles, IoAddressGroup addressGroup)
|
||||
public TrajectoryDoEvent(
|
||||
int waypointIndex,
|
||||
double triggerTime,
|
||||
int offsetCycles,
|
||||
int holdCycles,
|
||||
IoAddressGroup addressGroup,
|
||||
IEnumerable<double>? referenceJointsDegrees = null)
|
||||
{
|
||||
if (waypointIndex < 0)
|
||||
{
|
||||
@@ -162,6 +209,7 @@ public sealed class TrajectoryDoEvent
|
||||
OffsetCycles = offsetCycles;
|
||||
HoldCycles = holdCycles;
|
||||
AddressGroup = addressGroup ?? throw new ArgumentNullException(nameof(addressGroup));
|
||||
ReferenceJointsDegrees = referenceJointsDegrees?.ToArray();
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
@@ -193,6 +241,13 @@ public sealed class TrajectoryDoEvent
|
||||
/// </summary>
|
||||
[JsonPropertyName("addressGroup")]
|
||||
public IoAddressGroup AddressGroup { get; }
|
||||
|
||||
/// <summary>
|
||||
/// Gets the teach waypoint joints converted to degrees.
|
||||
/// This is used to select the closest real send sample inside the trigger time window.
|
||||
/// </summary>
|
||||
[JsonPropertyName("referenceJointsDegrees")]
|
||||
public IReadOnlyList<double>? ReferenceJointsDegrees { get; }
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
|
||||
@@ -1,6 +1,7 @@
|
||||
using System.Text;
|
||||
using System.Text.Json;
|
||||
using Flyshot.Core.Domain;
|
||||
using Flyshot.Core.Planning.Sampling;
|
||||
|
||||
namespace Flyshot.Core.Planning.Export;
|
||||
|
||||
@@ -15,12 +16,28 @@ namespace Flyshot.Core.Planning.Export;
|
||||
/// </summary>
|
||||
public static class TrajectoryExporter
|
||||
{
|
||||
/// <summary>
|
||||
/// 导出规划关节轨迹关键点到文本文件。
|
||||
/// </summary>
|
||||
public static void WriteJointTrajectory(string path, IReadOnlyList<IReadOnlyList<double>> rows)
|
||||
{
|
||||
WriteRows(path, rows);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 导出稠密关节轨迹到文本文件。
|
||||
/// </summary>
|
||||
public static void WriteJointDenseTrajectory(string path, IReadOnlyList<IReadOnlyList<double>> rows)
|
||||
{
|
||||
WriteDenseRows(path, rows);
|
||||
WriteRows(path, rows);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 导出规划笛卡尔轨迹关键点到文本文件。
|
||||
/// </summary>
|
||||
public static void WriteCartesianTrajectory(string path, IReadOnlyList<IReadOnlyList<double>> rows)
|
||||
{
|
||||
WriteRows(path, rows);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
@@ -28,7 +45,7 @@ public static class TrajectoryExporter
|
||||
/// </summary>
|
||||
public static void WriteCartesianDenseTrajectory(string path, IReadOnlyList<IReadOnlyList<double>> rows)
|
||||
{
|
||||
WriteDenseRows(path, rows);
|
||||
WriteRows(path, rows);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
@@ -36,13 +53,51 @@ public static class TrajectoryExporter
|
||||
/// </summary>
|
||||
public static void WriteShotEvents(string path, IReadOnlyList<ShotEvent> events)
|
||||
{
|
||||
WriteShotEvents(path, events, bindings: null);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 导出触发事件到 JSON 文件，并可选附带最终绑定到的实际 sample 信息。
|
||||
/// </summary>
|
||||
public static void WriteShotEvents(
|
||||
string path,
|
||||
IReadOnlyList<ShotEvent> events,
|
||||
IReadOnlyList<TriggerSampleBinding>? bindings)
|
||||
{
|
||||
var bindingByWaypointIndex = bindings?
|
||||
.GroupBy(static binding => binding.Trigger.WaypointIndex)
|
||||
.ToDictionary(static group => group.Key, static group => group.First());
|
||||
|
||||
var payload = events.Select(e => new
|
||||
{
|
||||
waypoint_index = e.WaypointIndex,
|
||||
trigger_time = Math.Round(e.TriggerTime, 6),
|
||||
sample_index = e.SampleIndex,
|
||||
sample_time = Math.Round(e.SampleTime, 6),
|
||||
addrs = e.AddressGroup.Addresses.ToList()
|
||||
addrs = e.AddressGroup.Addresses.ToList(),
|
||||
trigger_window_seconds = bindingByWaypointIndex is not null && bindingByWaypointIndex.TryGetValue(e.WaypointIndex, out var binding)
|
||||
? Math.Round(TriggerSampleBinder.TriggerBindingToleranceSeconds, 6)
|
||||
: (double?)null,
|
||||
selected_sample_index = bindingByWaypointIndex is not null && bindingByWaypointIndex.TryGetValue(e.WaypointIndex, out binding)
|
||||
? binding.SampleIndex
|
||||
: (int?)null,
|
||||
selected_send_time = bindingByWaypointIndex is not null && bindingByWaypointIndex.TryGetValue(e.WaypointIndex, out binding)
|
||||
? Math.Round(binding.Sample.SendTime, 6)
|
||||
: (double?)null,
|
||||
selected_trajectory_time = bindingByWaypointIndex is not null && bindingByWaypointIndex.TryGetValue(e.WaypointIndex, out binding)
|
||||
? Math.Round(binding.Sample.TrajectoryTime, 6)
|
||||
: (double?)null,
|
||||
io_mask = bindingByWaypointIndex is not null && bindingByWaypointIndex.TryGetValue(e.WaypointIndex, out binding)
|
||||
? ComputeIoValue(binding.Trigger.AddressGroup)
|
||||
: (ushort?)null,
|
||||
reference_joints_deg = bindingByWaypointIndex is not null
|
||||
&& bindingByWaypointIndex.TryGetValue(e.WaypointIndex, out binding)
|
||||
&& binding.Trigger.ReferenceJointsDegrees is { Count: > 0 }
|
||||
? binding.Trigger.ReferenceJointsDegrees.Select(static value => Math.Round(value, 6)).ToArray()
|
||||
: null,
|
||||
trigger_joints_deg = bindingByWaypointIndex is not null && bindingByWaypointIndex.TryGetValue(e.WaypointIndex, out binding)
|
||||
? binding.Sample.JointsDegrees.Select(static value => Math.Round(value, 6)).ToArray()
|
||||
: null
|
||||
}).ToList();
|
||||
|
||||
var json = JsonSerializer.Serialize(payload, new JsonSerializerOptions
|
||||
@@ -53,7 +108,7 @@ public static class TrajectoryExporter
|
||||
File.WriteAllText(path, json, new UTF8Encoding(false));
|
||||
}
|
||||
|
||||
private static void WriteDenseRows(string path, IReadOnlyList<IReadOnlyList<double>> rows)
|
||||
private static void WriteRows(string path, IReadOnlyList<IReadOnlyList<double>> rows)
|
||||
{
|
||||
var sb = new StringBuilder();
|
||||
foreach (var row in rows)
|
||||
@@ -63,4 +118,21 @@ public static class TrajectoryExporter
|
||||
|
||||
File.WriteAllText(path, sb.ToString(), new UTF8Encoding(false));
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 把 IO 地址组中的地址号映射为 writeIoValue 的位掩码，和运行时 / saveTrajectory 导出保持一致。
|
||||
/// </summary>
|
||||
private static ushort ComputeIoValue(IoAddressGroup group)
|
||||
{
|
||||
ushort value = 0;
|
||||
foreach (var addr in group.Addresses)
|
||||
{
|
||||
if (addr is >= 1 and <= 16)
|
||||
{
|
||||
value |= (ushort)(1 << (addr - 1));
|
||||
}
|
||||
}
|
||||
|
||||
return value;
|
||||
}
|
||||
}
|
||||
|
||||
@@ -6,6 +6,10 @@
|
||||
<Nullable>enable</Nullable>
|
||||
</PropertyGroup>
|
||||
|
||||
<ItemGroup>
|
||||
<PackageReference Include="Microsoft.Extensions.Logging.Abstractions" Version="8.0.1" />
|
||||
</ItemGroup>
|
||||
|
||||
<ItemGroup>
|
||||
<ProjectReference Include="..\Flyshot.Core.Domain\Flyshot.Core.Domain.csproj" />
|
||||
</ItemGroup>
|
||||
|
||||
194
src/Flyshot.Core.Planning/ICSP-算法说明.md
Normal file
194
src/Flyshot.Core.Planning/ICSP-算法说明.md
Normal file
@@ -0,0 +1,194 @@
|
||||
# ICSP 算法说明（`ICspPlanner`）
|
||||
|
||||
本文档用于解释 `Flyshot.Core.Planning.ICspPlanner` 当前实现的 **ICSP 规划算法**在本仓库中的真实含义与计算逻辑，便于与逆向结论对照、以及指导后续改造（例如“按约束生成中间点位”）。
|
||||
|
||||
> 适用范围：本文描述的是当前 C# 实现的 **“CubicSpline + 逐段时间缩放迭代（retiming）”** 版本。
|
||||
> 重要澄清：`ICspPlanner` 的主要输出是 **时间轴**（每个示教点的时间戳），而不是直接输出固定周期的稠密点序列；稠密点在后续采样层生成。
|
||||
|
||||
---
|
||||
|
||||
## 1. 名词与数据形态
|
||||
|
||||
### 1.1 输入
|
||||
|
||||
- **示教点（路点）**：`request.Program.Waypoints`
|
||||
每个路点是关节空间向量 \(q_i \in \mathbb{R}^{dof}\)。
|
||||
- **关节约束**：`request.Robot.JointLimits[d]` 提供每轴上限：
|
||||
- 速度上限 \(v_{lim}[d]\)
|
||||
- 加速度上限 \(a_{lim}[d]\)
|
||||
- 跃度（jerk）上限 \(j_{lim}[d]\)
|
||||
|
||||
### 1.2 输出（`PlannedTrajectory`）
|
||||
|
||||
`ICspPlanner` 的输出 **不是稠密轨迹点序列**，而是：
|
||||
|
||||
- `PlannedWaypoints`：规划后路点（对于普通 `icsp`，与输入示教点相同；补点发生在 `SelfAdaptIcspPlanner`）
|
||||
- `WaypointTimes`：每个路点的绝对时间 \(t_i\)（秒）
|
||||
- `SegmentDurations`：每段时长 \(T_i = t_{i+1}-t_i\)（秒）
|
||||
- `SegmentScales`：每段缩放因子 `scale_i`
|
||||
- `Iterations` / `Threshold`：收敛信息
|
||||
|
||||
后续模块会基于 `PlannedWaypoints + WaypointTimes` 重建样条并采样，生成稠密点：
|
||||
|
||||
- 规划层稠密采样：`TrajectorySampler.SampleJointTrajectory(...)`
|
||||
- 运行时 J519 重采样（速度倍率映射 + rad->deg）：`J519SendTrajectorySampler.SampleDenseJointTrajectory(...)`
|
||||
|
||||
---
|
||||
|
||||
## 2. 算法总体目标（retiming）
|
||||
|
||||
给定一组关节示教点 \(\{q_i\}_{i=0}^{N-1}\)，在不改变路点位置的前提下，为每段分配时长 \(\{T_i\}_{i=0}^{N-2}\)，使得用 **clamped-zero 三次样条**连接后的轨迹在每段上满足：
|
||||
|
||||
- \(\max|\dot q_d(t)| \le v_{lim}[d]\)
|
||||
- \(\max|\ddot q_d(t)| \le a_{lim}[d]\)
|
||||
- \(\max|\dddot q_d(t)| \le j_{lim}[d]\)
|
||||
|
||||
实现策略是“逐段缩放时长”的迭代法：每轮用当前 \(\{T_i\}\) 构造样条并解析求导峰值，再根据超限程度把相应段时长乘以缩放因子，使峰值回落。
|
||||
|
||||
---
|
||||
|
||||
## 3. 计算步骤（与代码一致）
|
||||
|
||||
### 3.1 前置条件
|
||||
|
||||
- 路点数 \(N \ge 4\)（否则抛异常）
|
||||
|
||||
### 3.2 初始段时长
|
||||
|
||||
段数 \(nseg = N-1\)。
|
||||
|
||||
初始段时长取相邻路点关节空间欧氏距离：
|
||||
|
||||
\[
|
||||
T_i^{(0)} = \|q_{i+1}-q_i\|_2
|
||||
\]
|
||||
|
||||
### 3.3 由段时长构造绝对时间轴
|
||||
|
||||
\[
|
||||
t_0 = 0,\quad t_{i+1} = t_i + T_i
|
||||
\]
|
||||
|
||||
### 3.4 用 clamped-zero 边界构造三次样条
|
||||
|
||||
以 \((t_i, q_i)\) 为节点构造分段三次多项式：
|
||||
|
||||
\[
|
||||
S_i(t) = a_i t^3 + b_i t^2 + c_i t + d_i,\quad t \in [t_i, t_{i+1}]
|
||||
\]
|
||||
|
||||
边界条件为 **clamped-zero**（起点/终点一阶导为 0），用于与逆向锁定的参考行为对齐。
|
||||
|
||||
### 3.5 解析计算每段导数峰值
|
||||
|
||||
对每段、每轴，解析求最大绝对值：
|
||||
|
||||
- 一阶导（速度）是二次函数：端点与顶点候选取最大
|
||||
- 二阶导（加速度）是一次函数：端点取最大
|
||||
- 三阶导（跃度）是常数：直接取绝对值
|
||||
|
||||
得到三张矩阵：
|
||||
|
||||
- `maxDq[seg,d] = max_t |dq/dt|`
|
||||
- `maxDdq[seg,d] = max_t |d²q/dt²|`
|
||||
- `maxDddq[seg,d] = max_t |d³q/dt³|`
|
||||
|
||||
### 3.6 计算每段缩放因子（核心公式）
|
||||
|
||||
对段 `seg`，对每个关节 `d` 计算三类“超限比”：
|
||||
|
||||
\[
|
||||
s_v = \left|\frac{maxDq[seg,d]}{v_{lim}[d]}\right|
|
||||
\]
|
||||
|
||||
\[
|
||||
s_a = \sqrt{\left|\frac{maxDdq[seg,d]}{a_{lim}[d]}\right|}
|
||||
\]
|
||||
|
||||
\[
|
||||
s_j = \sqrt[3]{\left|\frac{maxDddq[seg,d]}{j_{lim}[d]}\right|}
|
||||
\]
|
||||
|
||||
段缩放因子取所有轴、三类约束的最大值：
|
||||
|
||||
\[
|
||||
scale_{seg}=\max_d \max(s_v, s_a, s_j)
|
||||
\]
|
||||
|
||||
> 指数来源：时间拉长 \(k\) 倍时，速度按 \(1/k\) 缩小、加速度按 \(1/k^2\) 缩小、跃度按 \(1/k^3\) 缩小，因此超限比需要分别取一次方/平方根/立方根来求“应当拉长多少倍”。
|
||||
|
||||
### 3.7 收敛指标与最优解保存
|
||||
|
||||
每轮用如下指标衡量“离约束满足还差多少”：
|
||||
|
||||
\[
|
||||
threshold = \sum_{seg} |scale_{seg} - 1|
|
||||
\]
|
||||
|
||||
若本轮 `threshold` 小于历史最佳，则保存当前解作为 `best`（包含 `bestDurations / bestScales / bestWaypointTimes` 等）。
|
||||
|
||||
### 3.8 收敛判定与段时长更新
|
||||
|
||||
- 若 `threshold < _threshold`（默认 `1e-3`），认为收敛并提前结束迭代
|
||||
- 否则更新每段时长：
|
||||
|
||||
\[
|
||||
T_{seg} \leftarrow T_{seg} \cdot scale_{seg}
|
||||
\]
|
||||
|
||||
并进入下一轮。
|
||||
|
||||
---
|
||||
|
||||
## 4. 最终判定（global_scale）
|
||||
|
||||
迭代结束后取历史最优缩放因子的最大值：
|
||||
|
||||
\[
|
||||
globalScale=\max_{seg}(scale_{seg})
|
||||
\]
|
||||
|
||||
若启用强制判定（`enforceFinalScale=true`）且：
|
||||
|
||||
\[
|
||||
globalScale > 1 + \text{finalScaleTolerance}
|
||||
\]
|
||||
|
||||
则判定“未收敛/不可执行”并抛异常。默认容差 `finalScaleTolerance=1e-2`，用于容忍 C# spline 与参考实现间的小量数值差异。
|
||||
|
||||
---
|
||||
|
||||
## 5. 与“补点/中间点位”的关系（常见误解澄清）
|
||||
|
||||
### 5.1 `ICspPlanner` 不负责生成固定周期的中间点位
|
||||
|
||||
`ICspPlanner` 的核心工作是 **时间轴规划（retiming）**：在不改变示教点位置的情况下，通过缩放每段时长让样条导数峰值满足约束。
|
||||
|
||||
固定周期（例如 8ms/16ms）的“中间点位序列”属于 **采样层**：
|
||||
|
||||
- `TrajectorySampler`：按 `samplePeriod` 在样条上取样，得到 `[time, j1..jN]`（关节单位仍为 rad）
|
||||
- `J519SendTrajectorySampler`：按 `servoPeriod` 生成真实发送序列，用 `speedRatio` 把 `sendTime` 映射到 `trajectoryTime` 并线性插值，再做 `rad -> deg`
|
||||
|
||||
### 5.2 `SelfAdaptIcspPlanner` 才包含“补点”逻辑，但它很粗
|
||||
|
||||
`self-adapt-icsp` 的补点策略在 `SelfAdaptIcspPlanner` 中：当某些段 `scale > 1 + tolerance` 时，对这些段插入关节空间**中点**再重规划。该策略的目的主要是“救收敛”，不是生成最终稠密序列。
|
||||
|
||||
---
|
||||
|
||||
## 6. 后续改造建议（定位落点）
|
||||
|
||||
如果需求是“根据示教点 + v/a/j 限制，直接生成可下发的稠密点位序列”，通常有两条路径：
|
||||
|
||||
1. **保留 ICSP retiming**：继续用 `ICspPlanner` 求时间轴，再在采样层按固定周期生成中间点位（当前架构就是这条路）。此时需要讨论的是采样周期、速度倍率映射、以及是否要对采样序列再做约束校验或二次整形。
|
||||
2. **做真正的自适应插点/细分**：把“插点策略”升级为基于约束的细分（而不只是插中点），这更自然的落点是 `SelfAdaptIcspPlanner` 或新增一个“约束驱动细分器”，而不是把稠密点生成塞进 `ICspPlanner`。
|
||||
|
||||
---
|
||||
|
||||
## 7. 关联实现位置（便于跳转）
|
||||
|
||||
- 算法入口：`src/Flyshot.Core.Planning/ICspPlanner.cs`
|
||||
- 自适应补点：`src/Flyshot.Core.Planning/SelfAdaptIcspPlanner.cs`
|
||||
- 三次样条实现（clamped-zero + 解析导峰值）：`src/Flyshot.Core.Planning/CubicSplineInterpolator.cs`
|
||||
- 规划层稠密采样：`src/Flyshot.Core.Planning/Sampling/TrajectorySampler.cs`
|
||||
- J519 实发重采样：`src/Flyshot.Core.Planning/Sampling/J519SendTrajectorySampler.cs`
|
||||
|
||||
@@ -1,4 +1,5 @@
|
||||
using Flyshot.Core.Domain;
|
||||
using Microsoft.Extensions.Logging;
|
||||
|
||||
namespace Flyshot.Core.Planning;
|
||||
|
||||
@@ -24,6 +25,54 @@ public sealed class ICspPlanner
|
||||
/// </summary>
|
||||
public const int DefaultMaxIterations = 1000;
|
||||
|
||||
/// <summary>
|
||||
/// 默认最终 scale 容差。当前 C# spline 与旧系统对齐样本存在约 1% 内的数值余量。
|
||||
/// </summary>
|
||||
public const double DefaultFinalScaleTolerance = 1e-2;
|
||||
|
||||
private readonly double _threshold;
|
||||
private readonly int _maxIterations;
|
||||
private readonly bool _enforceFinalScale;
|
||||
private readonly double _finalScaleTolerance;
|
||||
private readonly ILogger<ICspPlanner>? _logger;
|
||||
|
||||
/// <summary>
|
||||
/// 初始化 ICSP 规划器。
|
||||
/// </summary>
|
||||
/// <param name="threshold">收敛阈值。</param>
|
||||
/// <param name="maxIterations">最大迭代轮数。</param>
|
||||
/// <param name="enforceFinalScale">是否在最终最优 scale 仍大于 1.0 时抛出失败。</param>
|
||||
/// <param name="finalScaleTolerance">最终 scale 判定容差。</param>
|
||||
/// <param name="logger">日志记录器；允许 null，供无日志场景使用。</param>
|
||||
public ICspPlanner(
|
||||
double threshold = DefaultThreshold,
|
||||
int maxIterations = DefaultMaxIterations,
|
||||
bool enforceFinalScale = true,
|
||||
double finalScaleTolerance = DefaultFinalScaleTolerance,
|
||||
ILogger<ICspPlanner>? logger = null)
|
||||
{
|
||||
if (threshold <= 0.0 || double.IsNaN(threshold) || double.IsInfinity(threshold))
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(nameof(threshold), "收敛阈值必须为有限正数。");
|
||||
}
|
||||
|
||||
if (maxIterations < 0)
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(nameof(maxIterations), "最大迭代轮数不能为负数。");
|
||||
}
|
||||
|
||||
if (finalScaleTolerance < 0.0 || double.IsNaN(finalScaleTolerance) || double.IsInfinity(finalScaleTolerance))
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(nameof(finalScaleTolerance), "最终 scale 容差必须为有限非负数。");
|
||||
}
|
||||
|
||||
_threshold = threshold;
|
||||
_maxIterations = maxIterations;
|
||||
_enforceFinalScale = enforceFinalScale;
|
||||
_finalScaleTolerance = finalScaleTolerance;
|
||||
_logger = logger;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 执行 ICSP 规划，返回包含完整时间轴和收敛信息的轨迹。
|
||||
/// </summary>
|
||||
@@ -37,9 +86,22 @@ public sealed class ICspPlanner
|
||||
throw new ArgumentException("ICSP 至少需要 4 个示教点。", nameof(request));
|
||||
}
|
||||
|
||||
_logger?.LogInformation(
|
||||
"ICSP 规划开始: 名称={Name}, 路点数={WaypointCount}, 自由度={Dof}, threshold={Threshold}, maxIterations={MaxIterations}",
|
||||
request.Program.Name, waypoints.Count, request.Robot.DegreesOfFreedom, _threshold, _maxIterations);
|
||||
_logger?.LogDebug(
|
||||
"ICSP 输入路点: {Waypoints}",
|
||||
string.Join(" | ", waypoints.Select(wp => $"[{string.Join(", ", wp.Positions.Select(j => j.ToString("F4")))}]")));
|
||||
|
||||
var qs = WaypointsToArray(waypoints);
|
||||
var (velLimits, accLimits, jerkLimits) = ExtractLimits(request.Robot);
|
||||
|
||||
_logger?.LogDebug(
|
||||
"ICSP 约束限值: vel=[{Vel}], acc=[{Acc}], jerk=[{Jerk}]",
|
||||
string.Join(", ", velLimits.Select(v => v.ToString("F2"))),
|
||||
string.Join(", ", accLimits.Select(a => a.ToString("F2"))),
|
||||
string.Join(", ", jerkLimits.Select(j => j.ToString("F2"))));
|
||||
|
||||
// 初始段时长直接取相邻示教点的关节空间欧氏距离。
|
||||
var segmentDurations = ComputeInitialDurations(qs);
|
||||
int nseg = segmentDurations.Length;
|
||||
@@ -52,7 +114,7 @@ public sealed class ICspPlanner
|
||||
int bestIterations = 0;
|
||||
double[]? bestWaypointTimes = null;
|
||||
|
||||
for (int iteration = 0; iteration <= DefaultMaxIterations; iteration++)
|
||||
for (int iteration = 0; iteration <= _maxIterations; iteration++)
|
||||
{
|
||||
var waypointTimes = CumulativeTimes(segmentDurations);
|
||||
var spline = new CubicSplineInterpolator(waypointTimes, qs);
|
||||
@@ -89,8 +151,11 @@ public sealed class ICspPlanner
|
||||
bestWaypointTimes = (double[])waypointTimes.Clone();
|
||||
}
|
||||
|
||||
if (currentThreshold < DefaultThreshold)
|
||||
if (currentThreshold < _threshold)
|
||||
{
|
||||
_logger?.LogDebug(
|
||||
"ICSP 第 {Iteration} 轮收敛: threshold={CurrentThreshold:E6}",
|
||||
iteration + 1, currentThreshold);
|
||||
break;
|
||||
}
|
||||
|
||||
@@ -105,6 +170,25 @@ public sealed class ICspPlanner
|
||||
throw new InvalidOperationException("ICSP 规划未能产生有效结果。");
|
||||
}
|
||||
|
||||
var globalScale = bestScales.Max();
|
||||
if (_enforceFinalScale && globalScale > 1.0 + _finalScaleTolerance)
|
||||
{
|
||||
_logger?.LogError(
|
||||
"ICSP 规划未收敛: global_scale={GlobalScale:F6} > {Tolerance:F6}, 段缩放=[{Scales}]",
|
||||
globalScale, 1.0 + _finalScaleTolerance,
|
||||
string.Join(", ", bestScales.Select(s => s.ToString("F4"))));
|
||||
throw new InvalidOperationException(
|
||||
$"ICSP 规划未收敛，global_scale={globalScale:F6} > {1.0 + _finalScaleTolerance:F6}，轨迹不可执行。");
|
||||
}
|
||||
|
||||
_logger?.LogInformation(
|
||||
"ICSP 规划完成: 名称={Name}, 迭代轮数={Iterations}, 收敛阈值={Threshold:E6}, 总时长={Duration:F4}s, global_scale={GlobalScale:F6}",
|
||||
request.Program.Name, bestIterations, bestThreshold, bestWaypointTimes[^1], globalScale);
|
||||
_logger?.LogDebug(
|
||||
"ICSP 段时长: [{Durations}], 段缩放: [{Scales}]",
|
||||
string.Join(", ", bestDurations.Select(d => d.ToString("F4"))),
|
||||
string.Join(", ", bestScales.Select(s => s.ToString("F4"))));
|
||||
|
||||
return new PlannedTrajectory(
|
||||
robot: request.Robot,
|
||||
originalProgram: request.Program,
|
||||
|
||||
@@ -0,0 +1,146 @@
|
||||
using Flyshot.Core.Domain;
|
||||
|
||||
namespace Flyshot.Core.Planning.Sampling;
|
||||
|
||||
/// <summary>
|
||||
/// 负责在飞拍进入运行时前构建最终 8ms 发送队列，并执行发送口径的离散限幅校验。
|
||||
/// </summary>
|
||||
public static class FlyshotExecutionSendSequenceBuilder
|
||||
{
|
||||
/// <summary>
|
||||
/// 根据连续规划轨迹构建最终发送队列；请求 speedRatio 只作为诊断信息保留。
|
||||
/// </summary>
|
||||
/// <param name="robot">机器人关节限值配置。</param>
|
||||
/// <param name="plannedTrajectory">规划阶段产出的连续轨迹。</param>
|
||||
/// <param name="result">规划结果。</param>
|
||||
/// <param name="servoPeriodSeconds">J519 物理发送周期，单位为秒。</param>
|
||||
/// <param name="requestedSpeedRatio">请求的执行倍率，必须已经在规划阶段折算进轨迹。</param>
|
||||
/// <param name="samplingSpeedRatio">最终队列采样倍率；为空时使用 1.0，表示直接采样已调速后的规划轨迹。</param>
|
||||
/// <returns>通过离散校验后的飞拍最终发送结果。</returns>
|
||||
public static FlyshotPreparedExecution Build(
|
||||
RobotProfile robot,
|
||||
PlannedTrajectory plannedTrajectory,
|
||||
TrajectoryResult result,
|
||||
double servoPeriodSeconds,
|
||||
double requestedSpeedRatio,
|
||||
double? samplingSpeedRatio = null)
|
||||
{
|
||||
ArgumentNullException.ThrowIfNull(robot);
|
||||
ArgumentNullException.ThrowIfNull(plannedTrajectory);
|
||||
ArgumentNullException.ThrowIfNull(result);
|
||||
|
||||
if (requestedSpeedRatio <= 0.0 || double.IsNaN(requestedSpeedRatio) || double.IsInfinity(requestedSpeedRatio))
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(nameof(requestedSpeedRatio), "speed_ratio 必须是有限正数。");
|
||||
}
|
||||
|
||||
if (servoPeriodSeconds <= 0.0 || double.IsNaN(servoPeriodSeconds) || double.IsInfinity(servoPeriodSeconds))
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(nameof(servoPeriodSeconds), "伺服周期必须是有限正数。");
|
||||
}
|
||||
|
||||
var finalSpeedRatio = samplingSpeedRatio ?? 1.0;
|
||||
if (finalSpeedRatio <= 0.0 || double.IsNaN(finalSpeedRatio) || double.IsInfinity(finalSpeedRatio))
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(nameof(samplingSpeedRatio), "最终队列采样倍率必须是有限正数。");
|
||||
}
|
||||
|
||||
var samples = J519SendTrajectorySampler.SamplePlannedTrajectory(
|
||||
plannedTrajectory,
|
||||
servoPeriodSeconds,
|
||||
finalSpeedRatio);
|
||||
|
||||
try
|
||||
{
|
||||
TrajectoryLimitValidator.ValidateJ519SendSamples(
|
||||
robot,
|
||||
samples,
|
||||
trajectoryName: result.ProgramName,
|
||||
validateJerk: false,
|
||||
strictJerkTolerance: true);
|
||||
}
|
||||
catch (InvalidOperationException exception)
|
||||
{
|
||||
throw new InvalidOperationException(
|
||||
$"飞拍最终发送队列在请求 speed_ratio={requestedSpeedRatio:F6} 下离散限幅校验失败。" +
|
||||
"请降低 planning_speed_scale 或 speed_ratio 后重新规划；执行侧不会在发送阶段临场重采样。",
|
||||
exception);
|
||||
}
|
||||
|
||||
return BuildPreparedExecution(
|
||||
result,
|
||||
samples,
|
||||
requestedSpeedRatio,
|
||||
finalSpeedRatio,
|
||||
stretchIterationCount: 0);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 将通过校验的 J519 采样点和触发绑定结果封装为稳定的飞拍执行结果。
|
||||
/// </summary>
|
||||
private static FlyshotPreparedExecution BuildPreparedExecution(
|
||||
TrajectoryResult result,
|
||||
IReadOnlyList<J519SendSample> samples,
|
||||
double requestedSpeedRatio,
|
||||
double finalSpeedRatio,
|
||||
int stretchIterationCount)
|
||||
{
|
||||
var preparedSamples = samples.Select(static sample =>
|
||||
new FlyshotPreparedSample(
|
||||
sample.SampleIndex,
|
||||
sample.SendTime,
|
||||
sample.TrajectoryTime,
|
||||
sample.SpeedRatio,
|
||||
sample.JointsDegrees)).ToArray();
|
||||
var sampleLookup = preparedSamples.ToDictionary(static sample => sample.SampleIndex);
|
||||
var triggerBindings = TriggerSampleBinder.Bind(
|
||||
result.TriggerTimeline,
|
||||
samples,
|
||||
result.TriggerSampleIndexOffsetCycles)
|
||||
.Select(binding =>
|
||||
new FlyshotPreparedTriggerBinding(
|
||||
binding.Trigger,
|
||||
sampleLookup[binding.Sample.SampleIndex],
|
||||
binding.SampleIndex,
|
||||
binding.FoundInWindow))
|
||||
.ToArray();
|
||||
var timingRows = samples
|
||||
.Select(sample => J519SendTrajectorySampler.BuildTimingRow(sample, requestedSpeedRatio, stretchIterationCount))
|
||||
.ToArray();
|
||||
var jerkRows = BuildJerkRows(samples);
|
||||
var finalDurationSeconds = preparedSamples.Length == 0 ? 0.0 : preparedSamples[^1].SendTime;
|
||||
|
||||
return new FlyshotPreparedExecution(
|
||||
preparedSamples,
|
||||
triggerBindings,
|
||||
timingRows,
|
||||
jerkRows,
|
||||
requestedSpeedRatio,
|
||||
finalSpeedRatio,
|
||||
finalDurationSeconds,
|
||||
stretchIterationCount);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 为最终发送点列构建跃度诊断行，保证导出工件和运行时发送复用同一份数据。
|
||||
/// </summary>
|
||||
private static IReadOnlyList<IReadOnlyList<double>> BuildJerkRows(IReadOnlyList<J519SendSample> samples)
|
||||
{
|
||||
var jerkRows = new List<IReadOnlyList<double>>(Math.Max(0, samples.Count - 1));
|
||||
double[]? previousVelocity = null;
|
||||
double[]? previousAcceleration = null;
|
||||
|
||||
for (var index = 1; index < samples.Count; index++)
|
||||
{
|
||||
jerkRows.Add(J519SendTrajectorySampler.BuildJerkRow(
|
||||
samples[index - 1].SendTime,
|
||||
samples[index].SendTime,
|
||||
samples[index - 1].JointsDegrees,
|
||||
samples[index].JointsDegrees,
|
||||
ref previousVelocity,
|
||||
ref previousAcceleration));
|
||||
}
|
||||
|
||||
return jerkRows;
|
||||
}
|
||||
}
|
||||
42
src/Flyshot.Core.Planning/Sampling/J519SendSample.cs
Normal file
42
src/Flyshot.Core.Planning/Sampling/J519SendSample.cs
Normal file
@@ -0,0 +1,42 @@
|
||||
namespace Flyshot.Core.Planning.Sampling;
|
||||
|
||||
/// <summary>
|
||||
/// 表示 J519 伺服链路在某一个物理发送周期上的轨迹采样结果。
|
||||
/// </summary>
|
||||
/// <param name="sampleIndex">从 0 开始的发送周期序号。</param>
|
||||
/// <param name="sendTime">J519 物理发送时间，单位为秒。</param>
|
||||
/// <param name="trajectoryTime">映射回规划轨迹的采样时间，单位为秒。</param>
|
||||
/// <param name="speedRatio">生成该采样点时使用的速度倍率。</param>
|
||||
/// <param name="jointsDegrees">J519 下发使用的角度制关节目标。</param>
|
||||
public sealed class J519SendSample(
|
||||
long sampleIndex,
|
||||
double sendTime,
|
||||
double trajectoryTime,
|
||||
double speedRatio,
|
||||
IReadOnlyList<double> jointsDegrees)
|
||||
{
|
||||
/// <summary>
|
||||
/// 获取从 0 开始的发送周期序号。
|
||||
/// </summary>
|
||||
public long SampleIndex { get; } = sampleIndex;
|
||||
|
||||
/// <summary>
|
||||
/// 获取 J519 物理发送时间，单位为秒。
|
||||
/// </summary>
|
||||
public double SendTime { get; } = sendTime;
|
||||
|
||||
/// <summary>
|
||||
/// 获取映射回规划轨迹的采样时间，单位为秒。
|
||||
/// </summary>
|
||||
public double TrajectoryTime { get; } = trajectoryTime;
|
||||
|
||||
/// <summary>
|
||||
/// 获取生成该采样点时使用的速度倍率。
|
||||
/// </summary>
|
||||
public double SpeedRatio { get; } = speedRatio;
|
||||
|
||||
/// <summary>
|
||||
/// 获取 J519 下发使用的角度制关节目标。
|
||||
/// </summary>
|
||||
public IReadOnlyList<double> JointsDegrees { get; } = jointsDegrees.ToArray();
|
||||
}
|
||||
352
src/Flyshot.Core.Planning/Sampling/J519SendTrajectorySampler.cs
Normal file
352
src/Flyshot.Core.Planning/Sampling/J519SendTrajectorySampler.cs
Normal file
@@ -0,0 +1,352 @@
|
||||
using Flyshot.Core.Planning;
|
||||
|
||||
namespace Flyshot.Core.Planning.Sampling;
|
||||
|
||||
/// <summary>
|
||||
/// 负责把规划层关节轨迹重采样为 J519 物理发送周期上的角度制目标。
|
||||
/// <para>
|
||||
/// 算法约定：
|
||||
/// 输入的稠密关节轨迹行格式固定为 [time, j1..jN]，time 为规划轨迹时间，关节单位为弧度；
|
||||
/// 输出的 J519 采样点按物理伺服周期排列，关节单位转换为角度制，供 UDP 60015 实时下发和离线 ActualSend 文件共用。
|
||||
/// </para>
|
||||
/// <para>
|
||||
/// 采样点数先按轨迹时间步长 trajectoryStep = servoPeriod * speedRatio 计算：
|
||||
/// sampleCount = ceil(max(0, duration / trajectoryStep - 1e-9)) + 1。
|
||||
/// 末尾额外保留一个终点钳制周期，确保轨迹时长不是周期整数倍时仍会输出最终点。
|
||||
/// </para>
|
||||
/// <para>
|
||||
/// 飞拍最终发送队列优先从 <see cref="PlannedTrajectory"/> 重建连续三次样条并直接取点；
|
||||
/// 旧稠密点入口仅保留给运行时 fallback 和兼容导出。
|
||||
/// 第 k 个采样点的物理发送时间为 sendTime = k * servoPeriod；
|
||||
/// speedRatio 不改变物理发送周期，只用于把发送时间映射回规划轨迹时间：
|
||||
/// trajectoryTime = min(sendTime * speedRatio, duration)。
|
||||
/// 之后在连续样条或旧稠密轨迹上求取关节，并把每个关节从 rad 转为 deg。
|
||||
/// </para>
|
||||
/// <para>
|
||||
/// 诊断行也在这里统一生成：Timing 行格式为 sample_index + send_time + trajectory_time + speed_ratio；
|
||||
/// Jerk 行使用相邻发送点上的角度制关节目标做后向差分，依次近似速度、加速度和跃度，格式为
|
||||
/// start_time + end_time + dt + max_abs_jerk + jerk[j1..jN]。
|
||||
/// </para>
|
||||
/// </summary>
|
||||
public static class J519SendTrajectorySampler
|
||||
{
|
||||
/// <summary>
|
||||
/// 根据连续规划样条和 J519 伺服周期生成完整实发采样序列。
|
||||
/// </summary>
|
||||
/// <param name="trajectory">规划阶段产出的连续轨迹信息。</param>
|
||||
/// <param name="servoPeriodSeconds">J519 物理发送周期，单位为秒。</param>
|
||||
/// <param name="speedRatio">速度倍率；只缩放轨迹采样时间，不改变物理发送周期。</param>
|
||||
/// <returns>按 J519 发送周期排列的角度制采样序列。</returns>
|
||||
public static IReadOnlyList<J519SendSample> SamplePlannedTrajectory(
|
||||
PlannedTrajectory trajectory,
|
||||
double servoPeriodSeconds,
|
||||
double speedRatio)
|
||||
{
|
||||
ArgumentNullException.ThrowIfNull(trajectory);
|
||||
ValidatePlannedTrajectoryInputs(trajectory, servoPeriodSeconds, speedRatio);
|
||||
|
||||
var durationSeconds = trajectory.WaypointTimes[^1];
|
||||
var trajectoryStepSeconds = servoPeriodSeconds * speedRatio;
|
||||
var sampleCount = CalculateSampleCount(durationSeconds, trajectoryStepSeconds);
|
||||
var spline = RebuildSpline(trajectory);
|
||||
var samples = new List<J519SendSample>((int)Math.Min(sampleCount, int.MaxValue));
|
||||
|
||||
for (long sampleIndex = 0; sampleIndex < sampleCount; sampleIndex++)
|
||||
{
|
||||
// 直接在连续样条上按 trajectoryTime 取点，避免先离散再线性回采样引入局部跃度尖峰。
|
||||
var sendTime = sampleIndex * servoPeriodSeconds;
|
||||
var trajectoryTime = Math.Min(sendTime * speedRatio, durationSeconds);
|
||||
var joints = spline.Evaluate(trajectoryTime).Select(RadiansToDegrees).ToArray();
|
||||
samples.Add(new J519SendSample(sampleIndex, sendTime, trajectoryTime, speedRatio, joints));
|
||||
}
|
||||
|
||||
return samples;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 根据 J519 伺服周期和 speed_ratio 生成完整实发采样序列。
|
||||
/// </summary>
|
||||
/// <param name="denseJointTrajectory">规划层稠密关节轨迹，每行格式为 [time, j1..jN]，关节单位为弧度。</param>
|
||||
/// <param name="durationSeconds">规划轨迹总时长，单位为秒。</param>
|
||||
/// <param name="servoPeriodSeconds">J519 物理发送周期，单位为秒。</param>
|
||||
/// <param name="speedRatio">速度倍率；只缩放轨迹采样时间，不改变物理发送周期。</param>
|
||||
/// <returns>按 J519 发送周期排列的角度制采样序列。</returns>
|
||||
public static IReadOnlyList<J519SendSample> SampleDenseJointTrajectory(
|
||||
IReadOnlyList<IReadOnlyList<double>> denseJointTrajectory,
|
||||
double durationSeconds,
|
||||
double servoPeriodSeconds,
|
||||
double speedRatio)
|
||||
{
|
||||
ArgumentNullException.ThrowIfNull(denseJointTrajectory);
|
||||
ValidateInputs(denseJointTrajectory, durationSeconds, servoPeriodSeconds, speedRatio);
|
||||
|
||||
var trajectoryStepSeconds = servoPeriodSeconds * speedRatio;
|
||||
var sampleCount = CalculateSampleCount(durationSeconds, trajectoryStepSeconds);
|
||||
var samples = new List<J519SendSample>((int)Math.Min(sampleCount, int.MaxValue));
|
||||
var segmentIndex = 0;
|
||||
|
||||
for (long sampleIndex = 0; sampleIndex < sampleCount; sampleIndex++)
|
||||
{
|
||||
// J519 物理周期固定，speed_ratio 只用于把发送时间映射回原始轨迹时间。
|
||||
var sendTime = sampleIndex * servoPeriodSeconds;
|
||||
var trajectoryTime = Math.Min(sendTime * speedRatio, durationSeconds);
|
||||
var joints = SampleDenseJointTrajectoryDegrees(denseJointTrajectory, trajectoryTime, ref segmentIndex);
|
||||
samples.Add(new J519SendSample(sampleIndex, sendTime, trajectoryTime, speedRatio, joints));
|
||||
}
|
||||
|
||||
return samples;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 按原始轨迹时长和 speed_ratio 后的轨迹时间步长计算 J519 实发采样数。
|
||||
/// </summary>
|
||||
/// <param name="durationSeconds">规划轨迹总时长，单位为秒。</param>
|
||||
/// <param name="trajectoryStepSeconds">每个物理发送周期对应的轨迹时间步长，单位为秒。</param>
|
||||
/// <returns>包含终点钳制周期的采样点数量。</returns>
|
||||
public static long CalculateSampleCount(double durationSeconds, double trajectoryStepSeconds)
|
||||
{
|
||||
if (durationSeconds < 0.0)
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(nameof(durationSeconds), "轨迹时长不能为负数。");
|
||||
}
|
||||
|
||||
if (trajectoryStepSeconds <= 0.0 || double.IsNaN(trajectoryStepSeconds) || double.IsInfinity(trajectoryStepSeconds))
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(nameof(trajectoryStepSeconds), "轨迹采样步长必须是有限正数。");
|
||||
}
|
||||
|
||||
// 非周期整数倍时多保留一个终点钳制周期，和真实 J519 下发序列保持一致。
|
||||
return (long)Math.Ceiling(Math.Max(0.0, (durationSeconds / trajectoryStepSeconds) - 1e-9)) + 1;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 构造实发时间映射文本行，格式为 sample_index + send_time + trajectory_time + final_speed_ratio。
|
||||
/// </summary>
|
||||
/// <param name="sample">待写出的 J519 实发采样点。</param>
|
||||
/// <returns>与 ActualSendTiming.txt 兼容的数值行。</returns>
|
||||
public static IReadOnlyList<double> BuildTimingRow(J519SendSample sample)
|
||||
{
|
||||
ArgumentNullException.ThrowIfNull(sample);
|
||||
return
|
||||
[
|
||||
sample.SampleIndex,
|
||||
Math.Round(sample.SendTime, 6),
|
||||
Math.Round(sample.TrajectoryTime, 6),
|
||||
Math.Round(sample.SpeedRatio, 6)
|
||||
];
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 构造带倍率诊断信息的实发时间映射文本行。
|
||||
/// </summary>
|
||||
/// <param name="sample">待写出的 J519 实发采样点。</param>
|
||||
/// <param name="requestedSpeedRatio">用户请求的执行倍率。</param>
|
||||
/// <param name="stretchIterationCount">历史诊断字段；当前执行侧不自动改写 speed_ratio，因此正常为 0。</param>
|
||||
/// <returns>前 4 列兼容 ActualSendTiming.txt，后续列记录请求倍率和倍率改写次数。</returns>
|
||||
public static IReadOnlyList<double> BuildTimingRow(
|
||||
J519SendSample sample,
|
||||
double requestedSpeedRatio,
|
||||
int stretchIterationCount)
|
||||
{
|
||||
if (requestedSpeedRatio <= 0.0 || double.IsNaN(requestedSpeedRatio) || double.IsInfinity(requestedSpeedRatio))
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(nameof(requestedSpeedRatio), "请求 speed_ratio 必须是有限正数。");
|
||||
}
|
||||
|
||||
if (stretchIterationCount < 0)
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(nameof(stretchIterationCount), "倍率改写次数不能为负数。");
|
||||
}
|
||||
|
||||
var legacyRow = BuildTimingRow(sample);
|
||||
return
|
||||
[
|
||||
legacyRow[0],
|
||||
legacyRow[1],
|
||||
legacyRow[2],
|
||||
legacyRow[3],
|
||||
Math.Round(requestedSpeedRatio, 6),
|
||||
stretchIterationCount
|
||||
];
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 构造相邻发送点之间的角度制跃度统计行。
|
||||
/// </summary>
|
||||
/// <param name="previousTime">上一帧发送时间，单位为秒。</param>
|
||||
/// <param name="currentTime">当前帧发送时间，单位为秒。</param>
|
||||
/// <param name="previousJoints">上一帧角度制关节目标。</param>
|
||||
/// <param name="currentJoints">当前帧角度制关节目标。</param>
|
||||
/// <param name="previousVelocity">上一帧关节速度，调用后更新为当前帧速度。</param>
|
||||
/// <param name="previousAcceleration">上一帧关节加速度，调用后更新为当前帧加速度。</param>
|
||||
/// <returns>与 ActualSendJerkStats.txt 兼容的数值行。</returns>
|
||||
public static IReadOnlyList<double> BuildJerkRow(
|
||||
double previousTime,
|
||||
double currentTime,
|
||||
IReadOnlyList<double> previousJoints,
|
||||
IReadOnlyList<double> currentJoints,
|
||||
ref double[]? previousVelocity,
|
||||
ref double[]? previousAcceleration)
|
||||
{
|
||||
ArgumentNullException.ThrowIfNull(previousJoints);
|
||||
ArgumentNullException.ThrowIfNull(currentJoints);
|
||||
|
||||
var dt = currentTime - previousTime;
|
||||
if (dt <= 0.0)
|
||||
{
|
||||
dt = 1e-9;
|
||||
}
|
||||
|
||||
var jointCount = currentJoints.Count;
|
||||
var currentVelocity = new double[jointCount];
|
||||
var currentAcceleration = new double[jointCount];
|
||||
var currentJerk = new double[jointCount];
|
||||
var maxAbsJerk = 0.0;
|
||||
|
||||
for (var index = 0; index < jointCount; index++)
|
||||
{
|
||||
currentVelocity[index] = (currentJoints[index] - previousJoints[index]) / dt;
|
||||
if (previousVelocity is not null)
|
||||
{
|
||||
currentAcceleration[index] = (currentVelocity[index] - previousVelocity[index]) / dt;
|
||||
}
|
||||
|
||||
if (previousAcceleration is not null)
|
||||
{
|
||||
currentJerk[index] = (currentAcceleration[index] - previousAcceleration[index]) / dt;
|
||||
maxAbsJerk = Math.Max(maxAbsJerk, Math.Abs(currentJerk[index]));
|
||||
}
|
||||
}
|
||||
|
||||
previousVelocity = currentVelocity;
|
||||
previousAcceleration = currentAcceleration;
|
||||
|
||||
var row = new double[jointCount + 4];
|
||||
row[0] = Math.Round(previousTime, 6);
|
||||
row[1] = Math.Round(currentTime, 6);
|
||||
row[2] = Math.Round(dt, 6);
|
||||
row[3] = Math.Round(maxAbsJerk, 6);
|
||||
for (var index = 0; index < jointCount; index++)
|
||||
{
|
||||
row[index + 4] = Math.Round(currentJerk[index], 6);
|
||||
}
|
||||
|
||||
return row;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 在稠密关节轨迹上按时间线性插值，并转换成 J519 下发使用的角度制目标。
|
||||
/// </summary>
|
||||
private static double[] SampleDenseJointTrajectoryDegrees(
|
||||
IReadOnlyList<IReadOnlyList<double>> denseJointTrajectory,
|
||||
double trajectoryTime,
|
||||
ref int segmentIndex)
|
||||
{
|
||||
if (denseJointTrajectory.Count == 1 || trajectoryTime <= denseJointTrajectory[0][0])
|
||||
{
|
||||
return denseJointTrajectory[0].Skip(1).Select(RadiansToDegrees).ToArray();
|
||||
}
|
||||
|
||||
var lastIndex = denseJointTrajectory.Count - 1;
|
||||
if (trajectoryTime >= denseJointTrajectory[lastIndex][0])
|
||||
{
|
||||
return denseJointTrajectory[lastIndex].Skip(1).Select(RadiansToDegrees).ToArray();
|
||||
}
|
||||
|
||||
while (segmentIndex < lastIndex - 1 && denseJointTrajectory[segmentIndex + 1][0] < trajectoryTime)
|
||||
{
|
||||
segmentIndex++;
|
||||
}
|
||||
|
||||
var start = denseJointTrajectory[segmentIndex];
|
||||
var end = denseJointTrajectory[segmentIndex + 1];
|
||||
var startTime = start[0];
|
||||
var endTime = end[0];
|
||||
var segmentDuration = endTime - startTime;
|
||||
var alpha = segmentDuration <= 0.0 ? 0.0 : (trajectoryTime - startTime) / segmentDuration;
|
||||
var joints = new double[start.Count - 1];
|
||||
for (var index = 0; index < joints.Length; index++)
|
||||
{
|
||||
joints[index] = RadiansToDegrees(start[index + 1] + ((end[index + 1] - start[index + 1]) * alpha));
|
||||
}
|
||||
|
||||
return joints;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 校验 J519 实发采样的基础输入，避免错误时间轴进入运行时链路。
|
||||
/// </summary>
|
||||
private static void ValidateInputs(
|
||||
IReadOnlyList<IReadOnlyList<double>> denseJointTrajectory,
|
||||
double durationSeconds,
|
||||
double servoPeriodSeconds,
|
||||
double speedRatio)
|
||||
{
|
||||
if (denseJointTrajectory.Count == 0)
|
||||
{
|
||||
throw new InvalidOperationException("稠密关节轨迹为空。");
|
||||
}
|
||||
|
||||
if (durationSeconds < 0.0)
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(nameof(durationSeconds), "轨迹时长不能为负数。");
|
||||
}
|
||||
|
||||
if (servoPeriodSeconds <= 0.0 || double.IsNaN(servoPeriodSeconds) || double.IsInfinity(servoPeriodSeconds))
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(nameof(servoPeriodSeconds), "J519 伺服周期必须是有限正数。");
|
||||
}
|
||||
|
||||
if (speedRatio <= 0.0 || double.IsNaN(speedRatio) || double.IsInfinity(speedRatio))
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(nameof(speedRatio), "speed_ratio 必须是有限正数。");
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 校验连续规划轨迹采样入口的基础输入，避免异常时间轴进入最终发送队列。
|
||||
/// </summary>
|
||||
private static void ValidatePlannedTrajectoryInputs(
|
||||
PlannedTrajectory trajectory,
|
||||
double servoPeriodSeconds,
|
||||
double speedRatio)
|
||||
{
|
||||
if (trajectory.WaypointTimes.Count < 2)
|
||||
{
|
||||
throw new InvalidOperationException("规划轨迹至少需要两个路点才能重建连续样条。");
|
||||
}
|
||||
|
||||
if (trajectory.WaypointTimes[^1] < 0.0)
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(nameof(trajectory), "规划轨迹总时长不能为负数。");
|
||||
}
|
||||
|
||||
if (servoPeriodSeconds <= 0.0 || double.IsNaN(servoPeriodSeconds) || double.IsInfinity(servoPeriodSeconds))
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(nameof(servoPeriodSeconds), "J519 伺服周期必须是有限正数。");
|
||||
}
|
||||
|
||||
if (speedRatio <= 0.0 || double.IsNaN(speedRatio) || double.IsInfinity(speedRatio))
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(nameof(speedRatio), "speed_ratio 必须是有限正数。");
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 从规划轨迹重建连续三次样条，供飞拍最终发送队列直接采样。
|
||||
/// </summary>
|
||||
private static CubicSplineInterpolator RebuildSpline(PlannedTrajectory trajectory)
|
||||
{
|
||||
var times = trajectory.WaypointTimes.ToArray();
|
||||
var values = trajectory.PlannedWaypoints.Select(static waypoint => waypoint.Positions.ToArray()).ToArray();
|
||||
return new CubicSplineInterpolator(times, values);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 角度单位转换：rad -> deg。
|
||||
/// </summary>
|
||||
private static double RadiansToDegrees(double radians)
|
||||
{
|
||||
return radians * 180.0 / Math.PI;
|
||||
}
|
||||
}
|
||||
220
src/Flyshot.Core.Planning/Sampling/TrajectoryLimitValidator.cs
Normal file
220
src/Flyshot.Core.Planning/Sampling/TrajectoryLimitValidator.cs
Normal file
@@ -0,0 +1,220 @@
|
||||
using Flyshot.Core.Domain;
|
||||
|
||||
namespace Flyshot.Core.Planning.Sampling;
|
||||
|
||||
/// <summary>
|
||||
/// 对最终生成的关节轨迹点做速度、加速度和 Jerk 离散复核。
|
||||
/// </summary>
|
||||
public static class TrajectoryLimitValidator
|
||||
{
|
||||
/// <summary>
|
||||
/// 离散差分校验允许的默认浮点容差倍率。
|
||||
/// </summary>
|
||||
public const double DefaultLimitTolerance = 1.000001;
|
||||
|
||||
/// <summary>
|
||||
/// 校验弧度制稠密关节轨迹是否满足机器人关节限制。
|
||||
/// </summary>
|
||||
/// <param name="robot">机器人约束配置。</param>
|
||||
/// <param name="rows">稠密轨迹行，格式为 time + 关节弧度。</param>
|
||||
/// <param name="toleranceMultiplier">限值容差倍率，用于过滤浮点舍入误差。</param>
|
||||
/// <param name="trajectoryName">诊断用轨迹名称。</param>
|
||||
/// <param name="validateJerk">是否校验离散 jerk；飞拍链路可临时关闭，仅保留速度/加速度约束。</param>
|
||||
public static void ValidateDenseJointTrajectory(
|
||||
RobotProfile robot,
|
||||
IReadOnlyList<IReadOnlyList<double>> rows,
|
||||
double toleranceMultiplier = DefaultLimitTolerance,
|
||||
string? trajectoryName = null,
|
||||
bool validateJerk = true)
|
||||
{
|
||||
ArgumentNullException.ThrowIfNull(robot);
|
||||
ArgumentNullException.ThrowIfNull(rows);
|
||||
ValidateTolerance(toleranceMultiplier);
|
||||
ValidateRows(robot, rows, toleranceMultiplier, trajectoryName ?? "dense-joint-trajectory", validateJerk);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 校验 J519 实际发送采样点是否满足机器人关节限制。
|
||||
/// </summary>
|
||||
/// <param name="robot">机器人约束配置。</param>
|
||||
/// <param name="samples">J519 发送采样点，关节单位为角度。</param>
|
||||
/// <param name="toleranceMultiplier">限值容差倍率，用于过滤浮点舍入误差。</param>
|
||||
/// <param name="trajectoryName">诊断用轨迹名称。</param>
|
||||
/// <param name="validateJerk">是否校验离散 jerk；飞拍链路可临时关闭，仅保留速度/加速度约束。</param>
|
||||
/// <param name="strictJerkTolerance">是否按原始 jerk 限值做硬约束；false 时保留历史宽容口径。</param>
|
||||
public static void ValidateJ519SendSamples(
|
||||
RobotProfile robot,
|
||||
IReadOnlyList<J519SendSample> samples,
|
||||
double toleranceMultiplier = DefaultLimitTolerance,
|
||||
string? trajectoryName = null,
|
||||
bool validateJerk = false,
|
||||
bool strictJerkTolerance = true)
|
||||
{
|
||||
ArgumentNullException.ThrowIfNull(robot);
|
||||
ArgumentNullException.ThrowIfNull(samples);
|
||||
ValidateTolerance(toleranceMultiplier);
|
||||
|
||||
var rows = new List<IReadOnlyList<double>>(samples.Count);
|
||||
foreach (var sample in samples)
|
||||
{
|
||||
var row = new double[robot.DegreesOfFreedom + 1];
|
||||
row[0] = sample.SendTime;
|
||||
for (var index = 0; index < robot.DegreesOfFreedom; index++)
|
||||
{
|
||||
row[index + 1] = DegreesToRadians(sample.JointsDegrees[index]);
|
||||
}
|
||||
|
||||
rows.Add(row);
|
||||
}
|
||||
|
||||
ValidateRows(
|
||||
robot,
|
||||
rows,
|
||||
toleranceMultiplier,
|
||||
trajectoryName ?? "j519-send-trajectory",
|
||||
validateJerk,
|
||||
strictJerkTolerance);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 校验容差倍率必须为有限正数。
|
||||
/// </summary>
|
||||
private static void ValidateTolerance(double toleranceMultiplier)
|
||||
{
|
||||
if (toleranceMultiplier <= 0.0 || double.IsNaN(toleranceMultiplier) || double.IsInfinity(toleranceMultiplier))
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(nameof(toleranceMultiplier), "限值容差倍率必须是有限正数。");
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 对弧度制轨迹行执行统一的离散差分限幅校验。
|
||||
/// </summary>
|
||||
private static void ValidateRows(
|
||||
RobotProfile robot,
|
||||
IReadOnlyList<IReadOnlyList<double>> rows,
|
||||
double toleranceMultiplier,
|
||||
string trajectoryName,
|
||||
bool validateJerk,
|
||||
bool strictJerkTolerance = false)
|
||||
{
|
||||
double? previousTime = null;
|
||||
double[]? previousPositions = null;
|
||||
double[]? previousVelocities = null;
|
||||
double[]? previousAccelerations = null;
|
||||
|
||||
for (var rowIndex = 0; rowIndex < rows.Count; rowIndex++)
|
||||
{
|
||||
var row = rows[rowIndex];
|
||||
if (row.Count < robot.DegreesOfFreedom + 1)
|
||||
{
|
||||
throw new InvalidOperationException(
|
||||
$"轨迹 {trajectoryName} 第 {rowIndex + 1} 行关节列数量不足，期望至少 {robot.DegreesOfFreedom + 1} 列，实际 {row.Count} 列。");
|
||||
}
|
||||
|
||||
var currentTime = row[0];
|
||||
var currentPositions = new double[robot.DegreesOfFreedom];
|
||||
for (var index = 0; index < robot.DegreesOfFreedom; index++)
|
||||
{
|
||||
currentPositions[index] = row[index + 1];
|
||||
}
|
||||
|
||||
if (previousTime is not null && previousPositions is not null)
|
||||
{
|
||||
var dt = currentTime - previousTime.Value;
|
||||
if (dt <= 0.0)
|
||||
{
|
||||
throw new InvalidOperationException(
|
||||
$"轨迹 {trajectoryName} 时间戳必须严格递增，第 {rowIndex + 1} 行 dt={dt:F9}s。");
|
||||
}
|
||||
|
||||
var currentVelocities = new double[robot.DegreesOfFreedom];
|
||||
var currentAccelerations = new double[robot.DegreesOfFreedom];
|
||||
for (var index = 0; index < robot.DegreesOfFreedom; index++)
|
||||
{
|
||||
var jointLimit = robot.JointLimits[index];
|
||||
currentVelocities[index] = (currentPositions[index] - previousPositions[index]) / dt;
|
||||
ThrowIfExceeded(
|
||||
trajectoryName,
|
||||
rowIndex,
|
||||
previousTime.Value,
|
||||
currentTime,
|
||||
jointLimit.JointName,
|
||||
"速度",
|
||||
currentVelocities[index],
|
||||
jointLimit.VelocityLimit,
|
||||
toleranceMultiplier);
|
||||
|
||||
currentAccelerations[index] = previousVelocities is null
|
||||
? 0.0
|
||||
: (currentVelocities[index] - previousVelocities[index]) / dt;
|
||||
ThrowIfExceeded(
|
||||
trajectoryName,
|
||||
rowIndex,
|
||||
previousTime.Value,
|
||||
currentTime,
|
||||
jointLimit.JointName,
|
||||
"加速度",
|
||||
currentAccelerations[index],
|
||||
jointLimit.AccelerationLimit,
|
||||
toleranceMultiplier);
|
||||
|
||||
if (validateJerk && previousAccelerations is not null)
|
||||
{
|
||||
var jerk = (currentAccelerations[index] - previousAccelerations[index]) / dt;
|
||||
ThrowIfExceeded(
|
||||
trajectoryName,
|
||||
rowIndex,
|
||||
previousTime.Value,
|
||||
currentTime,
|
||||
jointLimit.JointName,
|
||||
"Jerk",
|
||||
jerk,
|
||||
jointLimit.JerkLimit,
|
||||
strictJerkTolerance ? toleranceMultiplier : toleranceMultiplier * 4.0);
|
||||
}
|
||||
}
|
||||
|
||||
previousVelocities = currentVelocities;
|
||||
previousAccelerations = currentAccelerations;
|
||||
}
|
||||
|
||||
previousTime = currentTime;
|
||||
previousPositions = currentPositions;
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 当某个差分指标超过限制时抛出包含关节和时间窗的诊断异常。
|
||||
/// </summary>
|
||||
private static void ThrowIfExceeded(
|
||||
string trajectoryName,
|
||||
int rowIndex,
|
||||
double previousTime,
|
||||
double currentTime,
|
||||
string jointName,
|
||||
string metricName,
|
||||
double actual,
|
||||
double limit,
|
||||
double toleranceMultiplier)
|
||||
{
|
||||
var absActual = Math.Abs(actual);
|
||||
var effectiveLimit = limit * toleranceMultiplier;
|
||||
if (absActual <= effectiveLimit)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
throw new InvalidOperationException(
|
||||
$"轨迹 {trajectoryName} 第 {rowIndex + 1} 行 {jointName} {metricName}超限: " +
|
||||
$"time={previousTime:F6}->{currentTime:F6}s, actual={actual:F6}, limit={limit:F6}, ratio={absActual / limit:F4}。");
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 角度单位转换：deg -> rad。
|
||||
/// </summary>
|
||||
private static double DegreesToRadians(double degrees)
|
||||
{
|
||||
return degrees * Math.PI / 180.0;
|
||||
}
|
||||
}
|
||||
@@ -19,7 +19,8 @@ public static class TrajectorySampler
|
||||
public static IReadOnlyList<IReadOnlyList<double>> SampleJointTrajectory(
|
||||
PlannedTrajectory trajectory,
|
||||
double samplePeriod = 0.016,
|
||||
int decimals = 6)
|
||||
int decimals = 6,
|
||||
bool smoothStartStop = false)
|
||||
{
|
||||
var spline = RebuildSpline(trajectory);
|
||||
double duration = trajectory.WaypointTimes[^1];
|
||||
@@ -28,7 +29,10 @@ public static class TrajectorySampler
|
||||
|
||||
foreach (var t in times)
|
||||
{
|
||||
var pos = spline.Evaluate(t);
|
||||
var evaluationTime = smoothStartStop
|
||||
? MapSmoothStartStopEvaluationTime(t, duration)
|
||||
: t;
|
||||
var pos = spline.Evaluate(evaluationTime);
|
||||
var row = new List<double>(pos.Length + 1);
|
||||
row.Add(Math.Round(t, decimals));
|
||||
foreach (var value in pos)
|
||||
@@ -49,7 +53,8 @@ public static class TrajectorySampler
|
||||
PlannedTrajectory trajectory,
|
||||
RobotKinematicsModel kinematicsModel,
|
||||
double samplePeriod = 0.016,
|
||||
int decimals = 6)
|
||||
int decimals = 6,
|
||||
bool smoothStartStop = false)
|
||||
{
|
||||
var spline = RebuildSpline(trajectory);
|
||||
double duration = trajectory.WaypointTimes[^1];
|
||||
@@ -58,7 +63,10 @@ public static class TrajectorySampler
|
||||
|
||||
foreach (var t in times)
|
||||
{
|
||||
var jointPos = spline.Evaluate(t);
|
||||
var evaluationTime = smoothStartStop
|
||||
? MapSmoothStartStopEvaluationTime(t, duration)
|
||||
: t;
|
||||
var jointPos = spline.Evaluate(evaluationTime);
|
||||
var pose = RobotKinematics.ForwardKinematics(kinematicsModel, jointPos);
|
||||
var row = new List<double>(pose.Length + 1);
|
||||
row.Add(Math.Round(t, decimals));
|
||||
@@ -103,4 +111,26 @@ public static class TrajectorySampler
|
||||
|
||||
return times;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 把线性采样时间映射为整段平滑起停的评估时间。
|
||||
/// 使用 7 次 smootherstep 时间律，让起点和终点的一到三阶导都自然收敛到 0。
|
||||
/// </summary>
|
||||
private static double MapSmoothStartStopEvaluationTime(double sampleTime, double duration)
|
||||
{
|
||||
if (duration <= 0.0)
|
||||
{
|
||||
return 0.0;
|
||||
}
|
||||
|
||||
var normalizedTime = Math.Clamp(sampleTime / duration, 0.0, 1.0);
|
||||
var u2 = normalizedTime * normalizedTime;
|
||||
var u3 = u2 * normalizedTime;
|
||||
var u4 = u3 * normalizedTime;
|
||||
var u5 = u4 * normalizedTime;
|
||||
var u6 = u5 * normalizedTime;
|
||||
var u7 = u6 * normalizedTime;
|
||||
var progress = (35.0 * u4) - (84.0 * u5) + (70.0 * u6) - (20.0 * u7);
|
||||
return duration * progress;
|
||||
}
|
||||
}
|
||||
|
||||
134
src/Flyshot.Core.Planning/Sampling/TriggerSampleBinder.cs
Normal file
134
src/Flyshot.Core.Planning/Sampling/TriggerSampleBinder.cs
Normal file
@@ -0,0 +1,134 @@
|
||||
using Flyshot.Core.Domain;
|
||||
|
||||
namespace Flyshot.Core.Planning.Sampling;
|
||||
|
||||
/// <summary>
|
||||
/// 负责把理论触发事件绑定到实际 J519 发送 sample。
|
||||
/// 绑定规则为：先在理论触发时间前后固定时间窗内筛候选点，再优先选择关节差最小的 sample。
|
||||
/// </summary>
|
||||
public static class TriggerSampleBinder
|
||||
{
|
||||
/// <summary>
|
||||
/// 当前触发绑定使用的固定近时窗半宽，单位为秒。
|
||||
/// 即：以理论触发时间为中心，在前后各 100ms 的范围内寻找最优 sample。
|
||||
/// </summary>
|
||||
public const double TriggerBindingToleranceSeconds = 0.1;
|
||||
|
||||
/// <summary>
|
||||
/// 把一组触发事件绑定到采样后的 J519 sample。
|
||||
/// </summary>
|
||||
/// <param name="triggers">待绑定的触发事件。</param>
|
||||
/// <param name="samples">J519 实发 sample。</param>
|
||||
/// <param name="sampleIndexOffsetCycles">在最佳绑定 sample 基础上继续向后偏移的命令周期数。</param>
|
||||
/// <returns>按最终 sampleIndex 排序的绑定结果。</returns>
|
||||
public static IReadOnlyList<TriggerSampleBinding> Bind(
|
||||
IReadOnlyList<TrajectoryDoEvent> triggers,
|
||||
IReadOnlyList<J519SendSample> samples,
|
||||
int sampleIndexOffsetCycles = 0)
|
||||
{
|
||||
ArgumentNullException.ThrowIfNull(triggers);
|
||||
ArgumentNullException.ThrowIfNull(samples);
|
||||
if (sampleIndexOffsetCycles < 0)
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(nameof(sampleIndexOffsetCycles), "触发 sample 偏移周期不能为负数。");
|
||||
}
|
||||
|
||||
var bindings = new List<TriggerSampleBinding>(triggers.Count);
|
||||
if (triggers.Count == 0 || samples.Count == 0)
|
||||
{
|
||||
return bindings;
|
||||
}
|
||||
|
||||
var lastSampleIndex = samples.Count - 1;
|
||||
foreach (var trigger in triggers)
|
||||
{
|
||||
var fallbackIndex = FindClosestTrajectoryTimeSampleIndex(samples, trigger.TriggerTime);
|
||||
var candidateStart = Math.Max(0, fallbackIndex - 1);
|
||||
var candidateEnd = Math.Min(lastSampleIndex, fallbackIndex + 1);
|
||||
var bestIndex = fallbackIndex;
|
||||
var bestScore = double.PositiveInfinity;
|
||||
var foundInWindow = false;
|
||||
|
||||
for (var sampleIndex = candidateStart; sampleIndex <= candidateEnd; sampleIndex++)
|
||||
{
|
||||
var sample = samples[sampleIndex];
|
||||
if (Math.Abs(sample.TrajectoryTime - trigger.TriggerTime) > TriggerBindingToleranceSeconds)
|
||||
{
|
||||
continue;
|
||||
}
|
||||
|
||||
foundInWindow = true;
|
||||
var score = trigger.ReferenceJointsDegrees is { Count: > 0 }
|
||||
? ComputeJointDifferenceScore(trigger.ReferenceJointsDegrees, sample.JointsDegrees)
|
||||
: Math.Abs(sample.TrajectoryTime - trigger.TriggerTime);
|
||||
if (score < bestScore)
|
||||
{
|
||||
bestScore = score;
|
||||
bestIndex = sampleIndex;
|
||||
}
|
||||
}
|
||||
|
||||
var finalIndex = Math.Min(lastSampleIndex, bestIndex + sampleIndexOffsetCycles);
|
||||
bindings.Add(new TriggerSampleBinding(trigger, samples[finalIndex], finalIndex, foundInWindow));
|
||||
}
|
||||
|
||||
return bindings
|
||||
.OrderBy(static binding => binding.SampleIndex)
|
||||
.ThenBy(static binding => binding.Trigger.WaypointIndex)
|
||||
.ToArray();
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 按轨迹时间找到最接近理论触发时刻的 sample。
|
||||
/// </summary>
|
||||
public static int FindClosestTrajectoryTimeSampleIndex(IReadOnlyList<J519SendSample> samples, double triggerTime)
|
||||
{
|
||||
ArgumentNullException.ThrowIfNull(samples);
|
||||
|
||||
var bestIndex = 0;
|
||||
var bestDelta = double.PositiveInfinity;
|
||||
for (var sampleIndex = 0; sampleIndex < samples.Count; sampleIndex++)
|
||||
{
|
||||
var delta = Math.Abs(samples[sampleIndex].TrajectoryTime - triggerTime);
|
||||
if (delta < bestDelta)
|
||||
{
|
||||
bestDelta = delta;
|
||||
bestIndex = sampleIndex;
|
||||
}
|
||||
}
|
||||
|
||||
return bestIndex;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 计算参考示教点与候选 sample 的关节差评分，使用平方和即可稳定排序。
|
||||
/// </summary>
|
||||
public static double ComputeJointDifferenceScore(
|
||||
IReadOnlyList<double>? referenceJointsDegrees,
|
||||
IReadOnlyList<double> sampleJointsDegrees)
|
||||
{
|
||||
if (referenceJointsDegrees is null || referenceJointsDegrees.Count == 0)
|
||||
{
|
||||
return 0.0;
|
||||
}
|
||||
|
||||
var jointCount = Math.Min(referenceJointsDegrees.Count, sampleJointsDegrees.Count);
|
||||
var score = 0.0;
|
||||
for (var index = 0; index < jointCount; index++)
|
||||
{
|
||||
var delta = sampleJointsDegrees[index] - referenceJointsDegrees[index];
|
||||
score += delta * delta;
|
||||
}
|
||||
|
||||
return score;
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 表示一个理论触发事件最终绑定到哪个 J519 sample 的结果。
|
||||
/// </summary>
|
||||
public sealed record TriggerSampleBinding(
|
||||
TrajectoryDoEvent Trigger,
|
||||
J519SendSample Sample,
|
||||
int SampleIndex,
|
||||
bool FoundInWindow);
|
||||
@@ -1,182 +1,213 @@
|
||||
using Flyshot.Core.Domain;
|
||||
|
||||
namespace Flyshot.Core.Planning;
|
||||
|
||||
/// <summary>
|
||||
/// 在 ICSP 外层包裹补中点策略，实现 self-adapt-icsp 行为。
|
||||
///
|
||||
/// 为什么需要这层？
|
||||
/// ---
|
||||
/// 逆向分析已经指出：原系统里普通 icsp 若仍有段 scale > 1，不会直接返回未收敛结果；
|
||||
/// 配置中还明确存在 adapt_icsp_try_num。本层把“超限段统一插入中点后再重规划”的逻辑显式落地，
|
||||
/// 补上 demo 缺失的失败恢复路径。
|
||||
///
|
||||
/// 补点策略：
|
||||
/// ---
|
||||
/// 对当前所有 scale > 1 + tolerance 的段统一插入关节空间中点，然后把新路点集交给 ICSPPlanner
|
||||
/// 重新规划。这种"先把明显病灶都降一档，再整体重规划"的策略比逐段拆分更稳定，
|
||||
/// 也更符合服务端 adapt_icsp_try_num 的意图。
|
||||
/// </summary>
|
||||
public sealed class SelfAdaptIcspPlanner
|
||||
{
|
||||
/// <summary>
|
||||
/// 判定段是否超限的数值容差，过滤浮点噪声。
|
||||
/// </summary>
|
||||
public const double ScaleTolerance = 5e-4;
|
||||
|
||||
private readonly ICspPlanner _innerPlanner = new();
|
||||
|
||||
/// <summary>
|
||||
/// 执行自适应 ICSP 规划，允许在超限段插入中点后重试。
|
||||
/// </summary>
|
||||
/// <param name="request">轨迹规划请求。</param>
|
||||
/// <param name="adaptIcspTryNum">最大补点重试次数（默认 5）。</param>
|
||||
/// <returns>规划后的轨迹结果。</returns>
|
||||
/// <exception cref="InvalidOperationException">超过最大重试次数仍未收敛时抛出。</exception>
|
||||
public PlannedTrajectory Plan(TrajectoryRequest request, int adaptIcspTryNum = 5)
|
||||
{
|
||||
ArgumentNullException.ThrowIfNull(request);
|
||||
|
||||
var currentWaypoints = request.Program.Waypoints.ToArray();
|
||||
var currentShotFlags = request.Program.ShotFlags.ToArray();
|
||||
var currentOffsets = request.Program.OffsetValues.ToArray();
|
||||
var currentAddrs = request.Program.AddressGroups.ToArray();
|
||||
int originalWaypointCount = currentWaypoints.Length;
|
||||
|
||||
if (originalWaypointCount < 4)
|
||||
{
|
||||
throw new ArgumentException("ICSP 至少需要 4 个示教点。", nameof(request));
|
||||
}
|
||||
|
||||
var currentProgram = BuildProgram(request.Program.Name, currentWaypoints, currentShotFlags, currentOffsets, currentAddrs);
|
||||
var currentRequest = new TrajectoryRequest(
|
||||
robot: request.Robot,
|
||||
program: currentProgram,
|
||||
method: PlanningMethod.SelfAdaptIcsp,
|
||||
moveToStart: request.MoveToStart,
|
||||
saveTrajectoryArtifacts: request.SaveTrajectoryArtifacts,
|
||||
useCache: request.UseCache);
|
||||
|
||||
PlannedTrajectory? lastTrajectory = null;
|
||||
int maxAttempts = Math.Max(0, adaptIcspTryNum);
|
||||
|
||||
for (int attempt = 0; attempt <= maxAttempts; attempt++)
|
||||
{
|
||||
var trajectory = _innerPlanner.Plan(currentRequest);
|
||||
lastTrajectory = trajectory;
|
||||
|
||||
var badSegments = new List<int>();
|
||||
for (int seg = 0; seg < trajectory.SegmentScales.Count; seg++)
|
||||
{
|
||||
if (trajectory.SegmentScales[seg] > 1.0 + ScaleTolerance)
|
||||
{
|
||||
badSegments.Add(seg);
|
||||
}
|
||||
}
|
||||
|
||||
if (badSegments.Count == 0)
|
||||
{
|
||||
// 所有段都满足约束，收敛成功。返回包含补中点后路点的轨迹。
|
||||
return new PlannedTrajectory(
|
||||
robot: trajectory.Robot,
|
||||
originalProgram: request.Program,
|
||||
plannedWaypoints: currentWaypoints,
|
||||
waypointTimes: trajectory.WaypointTimes,
|
||||
segmentDurations: trajectory.SegmentDurations,
|
||||
segmentScales: trajectory.SegmentScales,
|
||||
method: PlanningMethod.SelfAdaptIcsp,
|
||||
iterations: trajectory.Iterations,
|
||||
threshold: trajectory.Threshold);
|
||||
}
|
||||
|
||||
if (attempt >= maxAttempts)
|
||||
{
|
||||
break;
|
||||
}
|
||||
|
||||
// 对超限段插入中点，并同步扩展 shot 元数据。
|
||||
(currentWaypoints, currentShotFlags, currentOffsets, currentAddrs) =
|
||||
InsertSegmentMidpoints(currentWaypoints, currentShotFlags, currentOffsets, currentAddrs, badSegments);
|
||||
|
||||
currentProgram = BuildProgram(request.Program.Name, currentWaypoints, currentShotFlags, currentOffsets, currentAddrs);
|
||||
currentRequest = new TrajectoryRequest(
|
||||
robot: request.Robot,
|
||||
program: currentProgram,
|
||||
method: PlanningMethod.SelfAdaptIcsp,
|
||||
moveToStart: request.MoveToStart,
|
||||
saveTrajectoryArtifacts: request.SaveTrajectoryArtifacts,
|
||||
useCache: request.UseCache);
|
||||
}
|
||||
|
||||
double maxScale = lastTrajectory?.SegmentScales.Max() ?? double.NaN;
|
||||
throw new InvalidOperationException(
|
||||
$"self-adapt ICSP 在 {adaptIcspTryNum} 轮补点后仍未收敛，最大段缩放因子={maxScale:F6}。");
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 用当前路点集和元数据构造 FlyshotProgram。
|
||||
/// </summary>
|
||||
private static FlyshotProgram BuildProgram(
|
||||
string name,
|
||||
JointWaypoint[] waypoints,
|
||||
bool[] shotFlags,
|
||||
int[] offsets,
|
||||
IoAddressGroup[] addrs)
|
||||
{
|
||||
return new FlyshotProgram(
|
||||
name: name,
|
||||
waypoints: waypoints,
|
||||
shotFlags: shotFlags,
|
||||
offsetValues: offsets,
|
||||
addressGroups: addrs);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 对超限段统一插入关节空间中点，并同步扩展 shot 元数据。
|
||||
/// 新插入的路点默认 shotFlag=false、offset=0、addr=空。
|
||||
/// </summary>
|
||||
private static (JointWaypoint[] waypoints, bool[] shotFlags, int[] offsets, IoAddressGroup[] addrs)
|
||||
InsertSegmentMidpoints(
|
||||
JointWaypoint[] waypoints,
|
||||
bool[] shotFlags,
|
||||
int[] offsets,
|
||||
IoAddressGroup[] addrs,
|
||||
List<int> badSegments)
|
||||
{
|
||||
if (badSegments.Count == 0)
|
||||
{
|
||||
return (waypoints, shotFlags, offsets, addrs);
|
||||
}
|
||||
|
||||
var badSet = new HashSet<int>(badSegments);
|
||||
var newWaypoints = new List<JointWaypoint>(waypoints.Length + badSegments.Count);
|
||||
var newShotFlags = new List<bool>(waypoints.Length + badSegments.Count);
|
||||
var newOffsets = new List<int>(waypoints.Length + badSegments.Count);
|
||||
var newAddrs = new List<IoAddressGroup>(waypoints.Length + badSegments.Count);
|
||||
|
||||
newWaypoints.Add(waypoints[0]);
|
||||
newShotFlags.Add(shotFlags[0]);
|
||||
newOffsets.Add(offsets[0]);
|
||||
newAddrs.Add(addrs[0]);
|
||||
|
||||
for (int seg = 0; seg < waypoints.Length - 1; seg++)
|
||||
{
|
||||
if (badSet.Contains(seg))
|
||||
{
|
||||
var mid = new JointWaypoint(
|
||||
waypoints[seg].Positions.Zip(waypoints[seg + 1].Positions, (a, b) => (a + b) / 2.0));
|
||||
newWaypoints.Add(mid);
|
||||
newShotFlags.Add(false);
|
||||
newOffsets.Add(0);
|
||||
newAddrs.Add(new IoAddressGroup(Array.Empty<int>()));
|
||||
}
|
||||
|
||||
newWaypoints.Add(waypoints[seg + 1]);
|
||||
newShotFlags.Add(shotFlags[seg + 1]);
|
||||
newOffsets.Add(offsets[seg + 1]);
|
||||
newAddrs.Add(addrs[seg + 1]);
|
||||
}
|
||||
|
||||
return (newWaypoints.ToArray(), newShotFlags.ToArray(), newOffsets.ToArray(), newAddrs.ToArray());
|
||||
}
|
||||
}
|
||||
using Flyshot.Core.Domain;
|
||||
using Microsoft.Extensions.Logging;
|
||||
|
||||
namespace Flyshot.Core.Planning;
|
||||
|
||||
/// <summary>
|
||||
/// 在 ICSP 外层包裹补中点策略，实现 self-adapt-icsp 行为。
|
||||
///
|
||||
/// 为什么需要这层？
|
||||
/// ---
|
||||
/// 逆向分析已经指出：原系统里普通 icsp 若仍有段 scale > 1，不会直接返回未收敛结果；
|
||||
/// 配置中还明确存在 adapt_icsp_try_num。本层把"超限段统一插入中点后再重规划"的逻辑显式落地，
|
||||
/// 补上 demo 缺失的失败恢复路径。
|
||||
///
|
||||
/// 补点策略：
|
||||
/// ---
|
||||
/// 对当前所有 scale > 1 + tolerance 的段统一插入关节空间中点，然后把新路点集交给 ICSPPlanner
|
||||
/// 重新规划。这种"先把明显病灶都降一档，再整体重规划"的策略比逐段拆分更稳定，
|
||||
/// 也更符合服务端 adapt_icsp_try_num 的意图。
|
||||
/// </summary>
|
||||
public sealed class SelfAdaptIcspPlanner
|
||||
{
|
||||
/// <summary>
|
||||
/// 判定段是否超限的数值容差，过滤浮点噪声。
|
||||
/// </summary>
|
||||
public const double ScaleTolerance = 5e-4;
|
||||
|
||||
private readonly ICspPlanner _innerPlanner;
|
||||
private readonly ILogger<SelfAdaptIcspPlanner>? _logger;
|
||||
|
||||
/// <summary>
|
||||
/// 初始化 SelfAdaptIcspPlanner。
|
||||
/// </summary>
|
||||
/// <param name="logger">日志记录器；允许 null。</param>
|
||||
public SelfAdaptIcspPlanner(ILogger<SelfAdaptIcspPlanner>? logger = null)
|
||||
{
|
||||
_innerPlanner = new ICspPlanner(enforceFinalScale: false, logger: null);
|
||||
_logger = logger;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 执行自适应 ICSP 规划，允许在超限段插入中点后重试。
|
||||
/// </summary>
|
||||
/// <param name="request">轨迹规划请求。</param>
|
||||
/// <param name="adaptIcspTryNum">最大补点重试次数（默认 5）。</param>
|
||||
/// <returns>规划后的轨迹结果。</returns>
|
||||
/// <exception cref="InvalidOperationException">超过最大重试次数仍未收敛时抛出。</exception>
|
||||
public PlannedTrajectory Plan(TrajectoryRequest request, int adaptIcspTryNum = 5)
|
||||
{
|
||||
ArgumentNullException.ThrowIfNull(request);
|
||||
|
||||
var currentWaypoints = request.Program.Waypoints.ToArray();
|
||||
var currentShotFlags = request.Program.ShotFlags.ToArray();
|
||||
var currentOffsets = request.Program.OffsetValues.ToArray();
|
||||
var currentAddrs = request.Program.AddressGroups.ToArray();
|
||||
int originalWaypointCount = currentWaypoints.Length;
|
||||
|
||||
if (originalWaypointCount < 4)
|
||||
{
|
||||
throw new ArgumentException("ICSP 至少需要 4 个示教点。", nameof(request));
|
||||
}
|
||||
|
||||
_logger?.LogInformation(
|
||||
"SelfAdaptICSP 规划开始: 名称={Name}, 原始路点数={WaypointCount}, 最大补点次数={MaxAttempts}",
|
||||
request.Program.Name, originalWaypointCount, adaptIcspTryNum);
|
||||
|
||||
var currentProgram = BuildProgram(request.Program.Name, currentWaypoints, currentShotFlags, currentOffsets, currentAddrs);
|
||||
var currentRequest = new TrajectoryRequest(
|
||||
robot: request.Robot,
|
||||
program: currentProgram,
|
||||
method: PlanningMethod.SelfAdaptIcsp,
|
||||
moveToStart: request.MoveToStart,
|
||||
saveTrajectoryArtifacts: request.SaveTrajectoryArtifacts,
|
||||
useCache: request.UseCache);
|
||||
|
||||
PlannedTrajectory? lastTrajectory = null;
|
||||
int maxAttempts = Math.Max(0, adaptIcspTryNum);
|
||||
|
||||
for (int attempt = 0; attempt <= maxAttempts; attempt++)
|
||||
{
|
||||
var trajectory = _innerPlanner.Plan(currentRequest);
|
||||
lastTrajectory = trajectory;
|
||||
|
||||
var badSegments = new List<int>();
|
||||
for (int seg = 0; seg < trajectory.SegmentScales.Count; seg++)
|
||||
{
|
||||
if (trajectory.SegmentScales[seg] > 1.0 + ScaleTolerance)
|
||||
{
|
||||
badSegments.Add(seg);
|
||||
}
|
||||
}
|
||||
|
||||
if (badSegments.Count == 0)
|
||||
{
|
||||
_logger?.LogInformation(
|
||||
"SelfAdaptICSP 规划完成: 名称={Name}, 补点轮数={Attempts}, 最终路点数={WaypointCount}, 迭代次数={Iterations}, 总时长={Duration:F4}s",
|
||||
request.Program.Name, attempt, currentWaypoints.Length, trajectory.Iterations, trajectory.WaypointTimes[^1]);
|
||||
// 所有段都满足约束，收敛成功。返回包含补中点后路点的轨迹。
|
||||
return new PlannedTrajectory(
|
||||
robot: trajectory.Robot,
|
||||
originalProgram: request.Program,
|
||||
plannedWaypoints: currentWaypoints,
|
||||
waypointTimes: trajectory.WaypointTimes,
|
||||
segmentDurations: trajectory.SegmentDurations,
|
||||
segmentScales: trajectory.SegmentScales,
|
||||
method: PlanningMethod.SelfAdaptIcsp,
|
||||
iterations: trajectory.Iterations,
|
||||
threshold: trajectory.Threshold);
|
||||
}
|
||||
|
||||
_logger?.LogWarning(
|
||||
"SelfAdaptICSP 第 {Attempt} 轮存在超限段: 超限段数={BadCount}, 段索引=[{Segments}], 最大缩放={MaxScale:F4}",
|
||||
attempt, badSegments.Count, string.Join(", ", badSegments), trajectory.SegmentScales.Max());
|
||||
|
||||
if (attempt >= maxAttempts)
|
||||
{
|
||||
break;
|
||||
}
|
||||
|
||||
// 对超限段插入中点，并同步扩展 shot 元数据。
|
||||
int waypointCountBefore = currentWaypoints.Length;
|
||||
(currentWaypoints, currentShotFlags, currentOffsets, currentAddrs) =
|
||||
InsertSegmentMidpoints(currentWaypoints, currentShotFlags, currentOffsets, currentAddrs, badSegments);
|
||||
|
||||
_logger?.LogDebug(
|
||||
"SelfAdaptICSP 补中点: 路点数 {Before} -> {After}, 插入段=[{Segments}]",
|
||||
waypointCountBefore, currentWaypoints.Length, string.Join(", ", badSegments));
|
||||
|
||||
currentProgram = BuildProgram(request.Program.Name, currentWaypoints, currentShotFlags, currentOffsets, currentAddrs);
|
||||
currentRequest = new TrajectoryRequest(
|
||||
robot: request.Robot,
|
||||
program: currentProgram,
|
||||
method: PlanningMethod.SelfAdaptIcsp,
|
||||
moveToStart: request.MoveToStart,
|
||||
saveTrajectoryArtifacts: request.SaveTrajectoryArtifacts,
|
||||
useCache: request.UseCache);
|
||||
}
|
||||
|
||||
double maxScale = lastTrajectory?.SegmentScales.Max() ?? double.NaN;
|
||||
_logger?.LogError(
|
||||
"SelfAdaptICSP 规划失败: 名称={Name}, 在 {Attempts} 轮补点后仍未收敛, 最大段缩放因子={MaxScale:F6}",
|
||||
request.Program.Name, adaptIcspTryNum, maxScale);
|
||||
throw new InvalidOperationException(
|
||||
$"self-adapt ICSP 在 {adaptIcspTryNum} 轮补点后仍未收敛，最大段缩放因子={maxScale:F6}。");
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 用当前路点集和元数据构造 FlyshotProgram。
|
||||
/// </summary>
|
||||
private static FlyshotProgram BuildProgram(
|
||||
string name,
|
||||
JointWaypoint[] waypoints,
|
||||
bool[] shotFlags,
|
||||
int[] offsets,
|
||||
IoAddressGroup[] addrs)
|
||||
{
|
||||
return new FlyshotProgram(
|
||||
name: name,
|
||||
waypoints: waypoints,
|
||||
shotFlags: shotFlags,
|
||||
offsetValues: offsets,
|
||||
addressGroups: addrs);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 对超限段统一插入关节空间中点，并同步扩展 shot 元数据。
|
||||
/// 新插入的路点默认 shotFlag=false、offset=0、addr=空。
|
||||
/// </summary>
|
||||
private static (JointWaypoint[] waypoints, bool[] shotFlags, int[] offsets, IoAddressGroup[] addrs)
|
||||
InsertSegmentMidpoints(
|
||||
JointWaypoint[] waypoints,
|
||||
bool[] shotFlags,
|
||||
int[] offsets,
|
||||
IoAddressGroup[] addrs,
|
||||
List<int> badSegments)
|
||||
{
|
||||
if (badSegments.Count == 0)
|
||||
{
|
||||
return (waypoints, shotFlags, offsets, addrs);
|
||||
}
|
||||
|
||||
var badSet = new HashSet<int>(badSegments);
|
||||
var newWaypoints = new List<JointWaypoint>(waypoints.Length + badSegments.Count);
|
||||
var newShotFlags = new List<bool>(waypoints.Length + badSegments.Count);
|
||||
var newOffsets = new List<int>(waypoints.Length + badSegments.Count);
|
||||
var newAddrs = new List<IoAddressGroup>(waypoints.Length + badSegments.Count);
|
||||
|
||||
newWaypoints.Add(waypoints[0]);
|
||||
newShotFlags.Add(shotFlags[0]);
|
||||
newOffsets.Add(offsets[0]);
|
||||
newAddrs.Add(addrs[0]);
|
||||
|
||||
for (int seg = 0; seg < waypoints.Length - 1; seg++)
|
||||
{
|
||||
if (badSet.Contains(seg))
|
||||
{
|
||||
var mid = new JointWaypoint(
|
||||
waypoints[seg].Positions.Zip(waypoints[seg + 1].Positions, (a, b) => (a + b) / 2.0));
|
||||
newWaypoints.Add(mid);
|
||||
newShotFlags.Add(false);
|
||||
newOffsets.Add(0);
|
||||
newAddrs.Add(new IoAddressGroup(Array.Empty<int>()));
|
||||
}
|
||||
|
||||
newWaypoints.Add(waypoints[seg + 1]);
|
||||
newShotFlags.Add(shotFlags[seg + 1]);
|
||||
newOffsets.Add(offsets[seg + 1]);
|
||||
newAddrs.Add(addrs[seg + 1]);
|
||||
}
|
||||
|
||||
return (newWaypoints.ToArray(), newShotFlags.ToArray(), newOffsets.ToArray(), newAddrs.ToArray());
|
||||
}
|
||||
}
|
||||
|
||||
@@ -9,6 +9,7 @@
|
||||
<ItemGroup>
|
||||
<ProjectReference Include="..\Flyshot.Core.Domain\Flyshot.Core.Domain.csproj" />
|
||||
<ProjectReference Include="..\Flyshot.Core.Planning\Flyshot.Core.Planning.csproj" />
|
||||
<PackageReference Include="Microsoft.Extensions.Logging.Abstractions" Version="8.0.1" />
|
||||
</ItemGroup>
|
||||
|
||||
</Project>
|
||||
|
||||
@@ -1,5 +1,6 @@
|
||||
using Flyshot.Core.Domain;
|
||||
using Flyshot.Core.Planning;
|
||||
using Microsoft.Extensions.Logging;
|
||||
|
||||
namespace Flyshot.Core.Triggering;
|
||||
|
||||
@@ -7,16 +8,33 @@ namespace Flyshot.Core.Triggering;
|
||||
/// 根据规划轨迹和飞拍配置生成触发时间轴，把示教点上的 shot_flags / offset_values / addr
|
||||
/// 映射成带理论时间和离散化时间的 ShotEvent，以及可直接注入伺服流的 TrajectoryDoEvent。
|
||||
/// </summary>
|
||||
/// <remarks>
|
||||
/// 本构建器只负责“时间轴映射”和“事件打包”，不负责轨迹规划与运行时发送。
|
||||
/// 其中：
|
||||
/// - ShotEvent 用于诊断、导出和日志观察；
|
||||
/// - TrajectoryDoEvent 用于运行时伺服链路注入 DO 脉冲。
|
||||
/// </remarks>
|
||||
public sealed class ShotTimelineBuilder
|
||||
{
|
||||
/// <summary>
|
||||
/// 将原始示教点索引映射到规划后时间轴上的时间戳解析器。
|
||||
/// </summary>
|
||||
private readonly WaypointTimestampResolver _resolver;
|
||||
|
||||
/// <summary>
|
||||
/// 可选日志记录器，用于输出时间轴构建统计信息。
|
||||
/// </summary>
|
||||
private readonly ILogger<ShotTimelineBuilder>? _logger;
|
||||
|
||||
/// <summary>
|
||||
/// 初始化 ShotTimelineBuilder，依赖一个时间戳解析器来对齐补中点后的轨迹与原始示教点。
|
||||
/// </summary>
|
||||
public ShotTimelineBuilder(WaypointTimestampResolver resolver)
|
||||
/// <param name="resolver">时间戳解析器。</param>
|
||||
/// <param name="logger">日志记录器；允许 null。</param>
|
||||
public ShotTimelineBuilder(WaypointTimestampResolver resolver, ILogger<ShotTimelineBuilder>? logger = null)
|
||||
{
|
||||
_resolver = resolver ?? throw new ArgumentNullException(nameof(resolver));
|
||||
_logger = logger;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
@@ -25,8 +43,20 @@ public sealed class ShotTimelineBuilder
|
||||
/// <param name="trajectory">规划后的轨迹（含补中点信息和机器人配置）。</param>
|
||||
/// <param name="holdCycles">IO 保持周期数（对应原系统的 io_keep_cycles）。</param>
|
||||
/// <param name="samplePeriod">稠密采样周期，用于离散化 sample_index 和 sample_time。</param>
|
||||
/// <param name="useDo">是否生成可注入伺服流的 DO 事件。</param>
|
||||
/// <returns>包含 ShotEvent 和 TrajectoryDoEvent 的触发时间轴。</returns>
|
||||
public ShotTimeline Build(PlannedTrajectory trajectory, int holdCycles, TimeSpan samplePeriod)
|
||||
/// <exception cref="ArgumentNullException">当 <paramref name="trajectory"/> 为 null 时抛出。</exception>
|
||||
/// <exception cref="ArgumentOutOfRangeException">
|
||||
/// 当 <paramref name="holdCycles"/> 小于 0，或 <paramref name="samplePeriod"/> 小于等于 0 时抛出。
|
||||
/// </exception>
|
||||
/// <remarks>
|
||||
/// 时间计算规则：
|
||||
/// - 理论触发时间：timestamp[i] + offset_values[i] * trigger_period；
|
||||
/// - 离散采样索引：Round(trigger_time / sample_period)；
|
||||
/// - 离散采样时间：sample_index * sample_period。
|
||||
/// 该离散化与运行时 8ms 周期下发模型一致，可用于对齐导出与诊断。
|
||||
/// </remarks>
|
||||
public ShotTimeline Build(PlannedTrajectory trajectory, int holdCycles, TimeSpan samplePeriod, bool useDo = true)
|
||||
{
|
||||
ArgumentNullException.ThrowIfNull(trajectory);
|
||||
|
||||
@@ -49,6 +79,7 @@ public sealed class ShotTimelineBuilder
|
||||
var shotEvents = new List<ShotEvent>();
|
||||
var triggerTimeline = new List<TrajectoryDoEvent>();
|
||||
|
||||
// 遍历原始示教点，仅对 shot_flag=true 的点生成触发事件。
|
||||
for (int i = 0; i < program.Waypoints.Count; i++)
|
||||
{
|
||||
if (!program.ShotFlags[i])
|
||||
@@ -56,6 +87,7 @@ public sealed class ShotTimelineBuilder
|
||||
continue;
|
||||
}
|
||||
|
||||
// 先得到连续时间上的理论触发时刻，再映射到离散采样周期。
|
||||
double triggerTime = timestamps[i] + program.OffsetValues[i] * triggerPeriodSeconds;
|
||||
int sampleIndex = (int)Math.Round(triggerTime / samplePeriodSeconds);
|
||||
double sampleTime = sampleIndex * samplePeriodSeconds;
|
||||
@@ -69,14 +101,26 @@ public sealed class ShotTimelineBuilder
|
||||
sampleTime: sampleTime,
|
||||
addressGroup: addressGroup));
|
||||
|
||||
triggerTimeline.Add(new TrajectoryDoEvent(
|
||||
waypointIndex: i,
|
||||
triggerTime: triggerTime,
|
||||
offsetCycles: program.OffsetValues[i],
|
||||
holdCycles: holdCycles,
|
||||
addressGroup: addressGroup));
|
||||
if (useDo)
|
||||
{
|
||||
// use_do=false 时保留 ShotEvent 诊断信息，但不向运行时下发 IO 脉冲。
|
||||
triggerTimeline.Add(new TrajectoryDoEvent(
|
||||
waypointIndex: i,
|
||||
triggerTime: triggerTime,
|
||||
offsetCycles: program.OffsetValues[i],
|
||||
holdCycles: holdCycles,
|
||||
addressGroup: addressGroup,
|
||||
referenceJointsDegrees: program.Waypoints[i].Positions.Select(static radians => radians * 180.0 / Math.PI)));
|
||||
}
|
||||
}
|
||||
|
||||
_logger?.LogInformation(
|
||||
"ShotTimeline 构建完成: shotFlags总数={ShotFlagCount}, 触发事件数={TriggerCount}, useDo={UseDo}, holdCycles={HoldCycles}",
|
||||
program.ShotFlags.Count(static f => f),
|
||||
triggerTimeline.Count,
|
||||
useDo,
|
||||
holdCycles);
|
||||
|
||||
return new ShotTimeline(shotEvents, triggerTimeline);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -6,4 +6,8 @@
|
||||
<Nullable>enable</Nullable>
|
||||
</PropertyGroup>
|
||||
|
||||
<ItemGroup>
|
||||
<ProjectReference Include="..\Flyshot.Core.Domain\Flyshot.Core.Domain.csproj" />
|
||||
</ItemGroup>
|
||||
|
||||
</Project>
|
||||
|
||||
123
src/Flyshot.Runtime.Common/IControllerRuntime.cs
Normal file
123
src/Flyshot.Runtime.Common/IControllerRuntime.cs
Normal file
@@ -0,0 +1,123 @@
|
||||
using Flyshot.Core.Domain;
|
||||
|
||||
namespace Flyshot.Runtime.Common;
|
||||
|
||||
/// <summary>
|
||||
/// 定义控制器运行时的最小状态与执行契约，供兼容层在不关心底层 Socket 细节的情况下调度轨迹。
|
||||
/// </summary>
|
||||
public interface IControllerRuntime
|
||||
{
|
||||
/// <summary>
|
||||
/// 重置当前机器人模型并清空控制器运行时状态。
|
||||
/// </summary>
|
||||
/// <param name="robot">当前机器人配置。</param>
|
||||
/// <param name="robotName">兼容层传入的机器人名称。</param>
|
||||
void ResetRobot(RobotProfile robot, string robotName);
|
||||
|
||||
/// <summary>
|
||||
/// 选择当前活动控制器类型。
|
||||
/// </summary>
|
||||
/// <param name="sim">是否使用仿真控制器。</param>
|
||||
void SetActiveController(bool sim);
|
||||
|
||||
/// <summary>
|
||||
/// 建立到控制器 IP 的连接。
|
||||
/// </summary>
|
||||
/// <param name="robotIp">控制器 IP。</param>
|
||||
void Connect(string robotIp);
|
||||
|
||||
/// <summary>
|
||||
/// 断开当前控制器连接。
|
||||
/// </summary>
|
||||
void Disconnect();
|
||||
|
||||
/// <summary>
|
||||
/// 使能机器人并记录底层缓冲区大小。
|
||||
/// </summary>
|
||||
/// <param name="bufferSize">运行时缓冲区大小。</param>
|
||||
void EnableRobot(int bufferSize);
|
||||
|
||||
/// <summary>
|
||||
/// 关闭机器人使能。
|
||||
/// </summary>
|
||||
void DisableRobot();
|
||||
|
||||
/// <summary>
|
||||
/// 停止当前运动。
|
||||
/// </summary>
|
||||
void StopMove();
|
||||
|
||||
/// <summary>
|
||||
/// 获取当前速度倍率。
|
||||
/// </summary>
|
||||
/// <returns>速度倍率。</returns>
|
||||
double GetSpeedRatio();
|
||||
|
||||
/// <summary>
|
||||
/// 设置当前速度倍率。
|
||||
/// </summary>
|
||||
/// <param name="ratio">目标速度倍率。</param>
|
||||
void SetSpeedRatio(double ratio);
|
||||
|
||||
/// <summary>
|
||||
/// 获取当前 TCP 坐标。
|
||||
/// </summary>
|
||||
/// <returns>TCP 三维坐标。</returns>
|
||||
IReadOnlyList<double> GetTcp();
|
||||
|
||||
/// <summary>
|
||||
/// 设置当前 TCP 坐标。
|
||||
/// </summary>
|
||||
/// <param name="x">TCP X。</param>
|
||||
/// <param name="y">TCP Y。</param>
|
||||
/// <param name="z">TCP Z。</param>
|
||||
void SetTcp(double x, double y, double z);
|
||||
|
||||
/// <summary>
|
||||
/// 读取指定 IO 端口。
|
||||
/// </summary>
|
||||
/// <param name="port">IO 端口。</param>
|
||||
/// <param name="ioType">IO 类型。</param>
|
||||
/// <returns>IO 当前值。</returns>
|
||||
bool GetIo(int port, string ioType);
|
||||
|
||||
/// <summary>
|
||||
/// 写入指定 IO 端口。
|
||||
/// </summary>
|
||||
/// <param name="port">IO 端口。</param>
|
||||
/// <param name="value">目标 IO 值。</param>
|
||||
/// <param name="ioType">IO 类型。</param>
|
||||
void SetIo(int port, bool value, string ioType);
|
||||
|
||||
/// <summary>
|
||||
/// 获取当前关节位置。
|
||||
/// </summary>
|
||||
/// <returns>当前关节位置。</returns>
|
||||
IReadOnlyList<double> GetJointPositions();
|
||||
|
||||
/// <summary>
|
||||
/// 获取当前末端位姿。
|
||||
/// </summary>
|
||||
/// <returns>当前末端位姿。</returns>
|
||||
IReadOnlyList<double> GetPose();
|
||||
|
||||
/// <summary>
|
||||
/// 获取当前运行时状态快照。
|
||||
/// </summary>
|
||||
/// <returns>控制器状态快照。</returns>
|
||||
ControllerStateSnapshot GetSnapshot();
|
||||
|
||||
/// <summary>
|
||||
/// 执行一条已经完成规划的轨迹，并更新最终关节位置。
|
||||
/// </summary>
|
||||
/// <param name="result">规划结果。</param>
|
||||
/// <param name="finalJointPositions">轨迹执行结束后的关节位置。</param>
|
||||
void ExecuteTrajectory(TrajectoryResult result, IReadOnlyList<double> finalJointPositions);
|
||||
|
||||
/// <summary>
|
||||
/// 执行一条已经完成规划的直角坐标轨迹，并更新最终 TCP 位姿。
|
||||
/// </summary>
|
||||
/// <param name="result">规划结果，必须包含稠密直角坐标轨迹。</param>
|
||||
/// <param name="finalPose">轨迹执行结束后的 [x,y,z,w,p,r] 位姿。</param>
|
||||
void ExecuteCartesianTrajectory(TrajectoryResult result, IReadOnlyList<double> finalPose);
|
||||
}
|
||||
1340
src/Flyshot.Runtime.Fanuc/FanucControllerRuntime.cs
Normal file
1340
src/Flyshot.Runtime.Fanuc/FanucControllerRuntime.cs
Normal file
File diff suppressed because it is too large
Load Diff
25
src/Flyshot.Runtime.Fanuc/Flyshot.Runtime.Fanuc.csproj
Normal file
25
src/Flyshot.Runtime.Fanuc/Flyshot.Runtime.Fanuc.csproj
Normal file
@@ -0,0 +1,25 @@
|
||||
<Project Sdk="Microsoft.NET.Sdk">
|
||||
|
||||
<PropertyGroup>
|
||||
<TargetFramework>net8.0</TargetFramework>
|
||||
<ImplicitUsings>enable</ImplicitUsings>
|
||||
<Nullable>enable</Nullable>
|
||||
</PropertyGroup>
|
||||
|
||||
<ItemGroup>
|
||||
<PackageReference Include="Microsoft.Extensions.Logging.Abstractions" Version="8.0.1" />
|
||||
</ItemGroup>
|
||||
|
||||
<ItemGroup>
|
||||
<AssemblyAttribute Include="System.Runtime.CompilerServices.InternalsVisibleTo">
|
||||
<_Parameter1>Flyshot.Core.Tests</_Parameter1>
|
||||
</AssemblyAttribute>
|
||||
</ItemGroup>
|
||||
|
||||
<ItemGroup>
|
||||
<ProjectReference Include="..\Flyshot.Core.Domain\Flyshot.Core.Domain.csproj" />
|
||||
<ProjectReference Include="..\Flyshot.Core.Planning\Flyshot.Core.Planning.csproj" />
|
||||
<ProjectReference Include="..\Flyshot.Runtime.Common\Flyshot.Runtime.Common.csproj" />
|
||||
</ItemGroup>
|
||||
|
||||
</Project>
|
||||
454
src/Flyshot.Runtime.Fanuc/Protocol/FanucCommandClient.cs
Normal file
454
src/Flyshot.Runtime.Fanuc/Protocol/FanucCommandClient.cs
Normal file
@@ -0,0 +1,454 @@
|
||||
using System.Net.Sockets;
|
||||
using Microsoft.Extensions.Logging;
|
||||
|
||||
namespace Flyshot.Runtime.Fanuc.Protocol;
|
||||
|
||||
/// <summary>
|
||||
/// FANUC TCP 10012 命令通道客户端，提供 Req/Res 同步命令下发能力。
|
||||
/// </summary>
|
||||
public sealed class FanucCommandClient : IDisposable
|
||||
{
|
||||
private readonly SemaphoreSlim _sendLock = new(1, 1);
|
||||
private readonly ILogger<FanucCommandClient>? _logger;
|
||||
private TcpClient? _tcpClient;
|
||||
private NetworkStream? _stream;
|
||||
private bool _disposed;
|
||||
|
||||
/// <summary>
|
||||
/// 获取当前是否已建立连接。
|
||||
/// </summary>
|
||||
public bool IsConnected => _tcpClient?.Connected ?? false;
|
||||
|
||||
/// <summary>
|
||||
/// 初始化 FANUC 命令通道客户端。
|
||||
/// </summary>
|
||||
/// <param name="logger">日志记录器；允许 null。</param>
|
||||
public FanucCommandClient(ILogger<FanucCommandClient>? logger = null)
|
||||
{
|
||||
_logger = logger;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 建立到 FANUC 控制柜 TCP 10012 命令通道的连接。
|
||||
/// </summary>
|
||||
/// <param name="ip">控制柜 IP 地址。</param>
|
||||
/// <param name="port">命令通道端口，默认 10012。</param>
|
||||
/// <param name="cancellationToken">取消令牌。</param>
|
||||
public async Task ConnectAsync(string ip, int port = 10012, CancellationToken cancellationToken = default)
|
||||
{
|
||||
ObjectDisposedException.ThrowIf(_disposed, this);
|
||||
|
||||
if (string.IsNullOrWhiteSpace(ip))
|
||||
{
|
||||
throw new ArgumentException("IP 不能为空。", nameof(ip));
|
||||
}
|
||||
|
||||
if (_tcpClient is not null)
|
||||
{
|
||||
throw new InvalidOperationException("命令通道已经连接，请先 Disconnect。");
|
||||
}
|
||||
|
||||
_logger?.LogInformation("CommandClient ConnectAsync: {Ip}:{Port}", ip, port);
|
||||
|
||||
_tcpClient = new TcpClient { NoDelay = true };
|
||||
await _tcpClient.ConnectAsync(ip, port, cancellationToken).ConfigureAwait(false);
|
||||
_stream = _tcpClient.GetStream();
|
||||
|
||||
_logger?.LogInformation("CommandClient 已连接: {Ip}:{Port}", ip, port);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 断开命令通道并释放资源。
|
||||
/// </summary>
|
||||
public void Disconnect()
|
||||
{
|
||||
ObjectDisposedException.ThrowIf(_disposed, this);
|
||||
|
||||
_logger?.LogInformation("CommandClient Disconnect");
|
||||
|
||||
_stream?.Dispose();
|
||||
_stream = null;
|
||||
_tcpClient?.Dispose();
|
||||
_tcpClient = null;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 发送通用命令并等待响应。
|
||||
/// </summary>
|
||||
/// <param name="messageId">命令消息号。</param>
|
||||
/// <param name="body">命令业务体。</param>
|
||||
/// <param name="cancellationToken">取消令牌。</param>
|
||||
/// <returns>原始响应帧。</returns>
|
||||
public async Task<byte[]> SendCommandAsync(uint messageId, ReadOnlyMemory<byte> body, CancellationToken cancellationToken = default)
|
||||
{
|
||||
ObjectDisposedException.ThrowIf(_disposed, this);
|
||||
|
||||
if (_stream is null)
|
||||
{
|
||||
throw new InvalidOperationException("命令通道未连接。");
|
||||
}
|
||||
|
||||
await _sendLock.WaitAsync(cancellationToken).ConfigureAwait(false);
|
||||
try
|
||||
{
|
||||
var frame = FanucCommandProtocol.PackFrame(messageId, body.Span);
|
||||
await _stream.WriteAsync(frame, cancellationToken).ConfigureAwait(false);
|
||||
|
||||
return await ReadResponseFrameAsync(cancellationToken).ConfigureAwait(false);
|
||||
}
|
||||
finally
|
||||
{
|
||||
_sendLock.Release();
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 发送携带程序名的命令并等待响应。
|
||||
/// </summary>
|
||||
/// <param name="messageId">命令消息号。</param>
|
||||
/// <param name="programName">程序名。</param>
|
||||
/// <param name="cancellationToken">取消令牌。</param>
|
||||
/// <returns>结果响应。</returns>
|
||||
public async Task<FanucCommandResultResponse> SendProgramCommandAsync(uint messageId, string programName, CancellationToken cancellationToken = default)
|
||||
{
|
||||
var frame = FanucCommandProtocol.PackProgramCommand(messageId, programName);
|
||||
var response = await SendRawFrameAsync(frame, cancellationToken).ConfigureAwait(false);
|
||||
return EnsureSuccess(FanucCommandProtocol.ParseResultResponse(response));
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 停止指定程序。
|
||||
/// </summary>
|
||||
/// <param name="programName">程序名。</param>
|
||||
/// <param name="cancellationToken">取消令牌。</param>
|
||||
/// <returns>结果响应。</returns>
|
||||
public async Task<FanucCommandResultResponse> StopProgramAsync(string programName, CancellationToken cancellationToken = default)
|
||||
{
|
||||
_logger?.LogInformation("CommandClient StopProgram: {ProgramName}", programName);
|
||||
var result = await SendProgramCommandAsync(FanucCommandMessageIds.StopProgram, programName, cancellationToken).ConfigureAwait(false);
|
||||
_logger?.LogDebug("CommandClient StopProgram 成功: {ProgramName}", programName);
|
||||
return result;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 复位控制器。
|
||||
/// </summary>
|
||||
/// <param name="cancellationToken">取消令牌。</param>
|
||||
/// <returns>结果响应。</returns>
|
||||
public async Task<FanucCommandResultResponse> ResetRobotAsync(CancellationToken cancellationToken = default)
|
||||
{
|
||||
_logger?.LogInformation("CommandClient ResetRobot");
|
||||
var frame = FanucCommandProtocol.PackEmptyCommand(FanucCommandMessageIds.ResetRobot);
|
||||
var response = await SendRawFrameAsync(frame, cancellationToken).ConfigureAwait(false);
|
||||
_logger?.LogDebug("CommandClient ResetRobot 成功");
|
||||
return EnsureSuccess(FanucCommandProtocol.ParseResultResponse(response));
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 查询指定程序状态。
|
||||
/// </summary>
|
||||
/// <param name="programName">程序名。</param>
|
||||
/// <param name="cancellationToken">取消令牌。</param>
|
||||
/// <returns>程序状态响应。</returns>
|
||||
public async Task<FanucProgramStatusResponse> GetProgramStatusAsync(string programName, CancellationToken cancellationToken = default)
|
||||
{
|
||||
_logger?.LogInformation("CommandClient GetProgramStatus: {ProgramName}", programName);
|
||||
var frame = FanucCommandProtocol.PackProgramCommand(FanucCommandMessageIds.GetProgramStatus, programName);
|
||||
var response = await SendRawFrameAsync(frame, cancellationToken).ConfigureAwait(false);
|
||||
_logger?.LogDebug("CommandClient GetProgramStatus 成功: {ProgramName}", programName);
|
||||
return EnsureSuccess(FanucCommandProtocol.ParseProgramStatusResponse(response));
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 启动指定程序。
|
||||
/// </summary>
|
||||
/// <param name="programName">程序名。</param>
|
||||
/// <param name="cancellationToken">取消令牌。</param>
|
||||
/// <returns>结果响应。</returns>
|
||||
public async Task<FanucCommandResultResponse> StartProgramAsync(string programName, CancellationToken cancellationToken = default)
|
||||
{
|
||||
_logger?.LogInformation("CommandClient StartProgram: {ProgramName}", programName);
|
||||
var result = await SendProgramCommandAsync(FanucCommandMessageIds.StartProgram, programName, cancellationToken).ConfigureAwait(false);
|
||||
_logger?.LogDebug("CommandClient StartProgram 成功: {ProgramName}", programName);
|
||||
return result;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 读取控制器速度倍率。
|
||||
/// </summary>
|
||||
/// <param name="cancellationToken">取消令牌。</param>
|
||||
/// <returns>速度倍率响应。</returns>
|
||||
public async Task<FanucSpeedRatioResponse> GetSpeedRatioAsync(CancellationToken cancellationToken = default)
|
||||
{
|
||||
_logger?.LogDebug("CommandClient GetSpeedRatio");
|
||||
var frame = FanucCommandProtocol.PackGetSpeedRatioCommand();
|
||||
var response = await SendRawFrameAsync(frame, cancellationToken).ConfigureAwait(false);
|
||||
var result = EnsureSuccess(FanucCommandProtocol.ParseSpeedRatioResponse(response));
|
||||
_logger?.LogDebug("CommandClient GetSpeedRatio 成功: ratio={Ratio}", result.Ratio);
|
||||
return result;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 设置控制器速度倍率。
|
||||
/// </summary>
|
||||
/// <param name="ratio">目标速度倍率。</param>
|
||||
/// <param name="cancellationToken">取消令牌。</param>
|
||||
/// <returns>结果响应。</returns>
|
||||
public async Task<FanucCommandResultResponse> SetSpeedRatioAsync(double ratio, CancellationToken cancellationToken = default)
|
||||
{
|
||||
_logger?.LogInformation("CommandClient SetSpeedRatio: ratio={Ratio}", ratio);
|
||||
var frame = FanucCommandProtocol.PackSetSpeedRatioCommand(ratio);
|
||||
var response = await SendRawFrameAsync(frame, cancellationToken).ConfigureAwait(false);
|
||||
_logger?.LogDebug("CommandClient SetSpeedRatio 成功: ratio={Ratio}", ratio);
|
||||
return EnsureSuccess(FanucCommandProtocol.ParseResultResponse(response));
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 读取控制器 TCP 位姿。
|
||||
/// </summary>
|
||||
/// <param name="tcpId">TCP ID。</param>
|
||||
/// <param name="cancellationToken">取消令牌。</param>
|
||||
/// <returns>TCP 位姿响应。</returns>
|
||||
public async Task<FanucTcpResponse> GetTcpAsync(uint tcpId = 1, CancellationToken cancellationToken = default)
|
||||
{
|
||||
_logger?.LogDebug("CommandClient GetTcp: tcpId={TcpId}", tcpId);
|
||||
var frame = FanucCommandProtocol.PackGetTcpCommand(tcpId);
|
||||
var response = await SendRawFrameAsync(frame, cancellationToken).ConfigureAwait(false);
|
||||
var result = EnsureSuccess(FanucCommandProtocol.ParseTcpResponse(response));
|
||||
_logger?.LogDebug("CommandClient GetTcp 成功: tcpId={TcpId}, pose=[{Pose}]", tcpId, string.Join(", ", result.Pose.Select(v => v.ToString("F2"))));
|
||||
return result;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 设置控制器 TCP 位姿。
|
||||
/// </summary>
|
||||
/// <param name="tcpId">TCP ID。</param>
|
||||
/// <param name="pose">7 维 TCP 位姿。</param>
|
||||
/// <param name="cancellationToken">取消令牌。</param>
|
||||
/// <returns>结果响应。</returns>
|
||||
public async Task<FanucCommandResultResponse> SetTcpAsync(uint tcpId, IReadOnlyList<double> pose, CancellationToken cancellationToken = default)
|
||||
{
|
||||
_logger?.LogInformation("CommandClient SetTcp: tcpId={TcpId}, pose=[{Pose}]", tcpId, string.Join(", ", pose.Take(3).Select(v => v.ToString("F2"))));
|
||||
var frame = FanucCommandProtocol.PackSetTcpCommand(tcpId, pose);
|
||||
var response = await SendRawFrameAsync(frame, cancellationToken).ConfigureAwait(false);
|
||||
_logger?.LogDebug("CommandClient SetTcp 成功: tcpId={TcpId}", tcpId);
|
||||
return EnsureSuccess(FanucCommandProtocol.ParseResultResponse(response));
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 读取控制器 IO。
|
||||
/// </summary>
|
||||
/// <param name="port">IO 索引。</param>
|
||||
/// <param name="ioType">IO 类型字符串。</param>
|
||||
/// <param name="cancellationToken">取消令牌。</param>
|
||||
/// <returns>IO 读取响应。</returns>
|
||||
public async Task<FanucIoResponse> GetIoAsync(int port, string ioType, CancellationToken cancellationToken = default)
|
||||
{
|
||||
_logger?.LogDebug("CommandClient GetIo: port={Port}, ioType={IoType}", port, ioType);
|
||||
var frame = FanucCommandProtocol.PackGetIoCommand(FanucIoTypes.FromName(ioType), port);
|
||||
var response = await SendRawFrameAsync(frame, cancellationToken).ConfigureAwait(false);
|
||||
var result = EnsureSuccess(FanucCommandProtocol.ParseIoResponse(response));
|
||||
_logger?.LogDebug("CommandClient GetIo 成功: port={Port}, value={Value}", port, result.Value);
|
||||
return result;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 设置控制器 IO。
|
||||
/// </summary>
|
||||
/// <param name="port">IO 索引。</param>
|
||||
/// <param name="value">目标 IO 值。</param>
|
||||
/// <param name="ioType">IO 类型字符串。</param>
|
||||
/// <param name="cancellationToken">取消令牌。</param>
|
||||
/// <returns>结果响应。</returns>
|
||||
public async Task<FanucCommandResultResponse> SetIoAsync(int port, bool value, string ioType, CancellationToken cancellationToken = default)
|
||||
{
|
||||
_logger?.LogInformation("CommandClient SetIo: port={Port}, value={Value}, ioType={IoType}", port, value, ioType);
|
||||
var frame = FanucCommandProtocol.PackSetIoCommand(FanucIoTypes.FromName(ioType), port, value);
|
||||
var response = await SendRawFrameAsync(frame, cancellationToken).ConfigureAwait(false);
|
||||
_logger?.LogDebug("CommandClient SetIo 成功: port={Port}, value={Value}", port, value);
|
||||
return EnsureSuccess(FanucCommandProtocol.ParseResultResponse(response));
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 释放客户端资源。
|
||||
/// </summary>
|
||||
public void Dispose()
|
||||
{
|
||||
if (_disposed)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
_disposed = true;
|
||||
_stream?.Dispose();
|
||||
_stream = null;
|
||||
_tcpClient?.Dispose();
|
||||
_tcpClient = null;
|
||||
_sendLock.Dispose();
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 直接发送已封装的帧并读取响应。
|
||||
/// </summary>
|
||||
private async Task<byte[]> SendRawFrameAsync(byte[] frame, CancellationToken cancellationToken)
|
||||
{
|
||||
if (_stream is null)
|
||||
{
|
||||
throw new InvalidOperationException("命令通道未连接。");
|
||||
}
|
||||
|
||||
await _sendLock.WaitAsync(cancellationToken).ConfigureAwait(false);
|
||||
try
|
||||
{
|
||||
await _stream.WriteAsync(frame, cancellationToken).ConfigureAwait(false);
|
||||
var response = await ReadResponseFrameAsync(cancellationToken).ConfigureAwait(false);
|
||||
_logger?.LogDebug("CommandClient 发送帧成功: 帧长度={FrameLength}, 响应长度={ResponseLength}", frame.Length, response.Length);
|
||||
return response;
|
||||
}
|
||||
finally
|
||||
{
|
||||
_sendLock.Release();
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 校验普通命令响应结果码，失败时抛出包含消息号和结果码的诊断异常。
|
||||
/// </summary>
|
||||
private FanucCommandResultResponse EnsureSuccess(FanucCommandResultResponse response)
|
||||
{
|
||||
if (!response.IsSuccess)
|
||||
{
|
||||
_logger?.LogError("FANUC 命令失败: msgId=0x{MessageId:X4}, resultCode={ResultCode}", response.MessageId, response.ResultCode);
|
||||
throw CreateCommandFailureException(response.MessageId, response.ResultCode);
|
||||
}
|
||||
|
||||
return response;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 校验程序状态响应结果码，失败时抛出包含消息号和结果码的诊断异常。
|
||||
/// </summary>
|
||||
private FanucProgramStatusResponse EnsureSuccess(FanucProgramStatusResponse response)
|
||||
{
|
||||
if (!response.IsSuccess)
|
||||
{
|
||||
_logger?.LogError("FANUC 命令失败: msgId=0x{MessageId:X4}, resultCode={ResultCode}", response.MessageId, response.ResultCode);
|
||||
throw CreateCommandFailureException(response.MessageId, response.ResultCode);
|
||||
}
|
||||
|
||||
return response;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 校验速度倍率响应结果码，失败时抛出包含消息号和结果码的诊断异常。
|
||||
/// </summary>
|
||||
private FanucSpeedRatioResponse EnsureSuccess(FanucSpeedRatioResponse response)
|
||||
{
|
||||
if (!response.IsSuccess)
|
||||
{
|
||||
_logger?.LogError("FANUC 命令失败: msgId=0x{MessageId:X4}, resultCode={ResultCode}", response.MessageId, response.ResultCode);
|
||||
throw CreateCommandFailureException(response.MessageId, response.ResultCode);
|
||||
}
|
||||
|
||||
return response;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 校验 TCP 位姿响应结果码，失败时抛出包含消息号和结果码的诊断异常。
|
||||
/// </summary>
|
||||
private FanucTcpResponse EnsureSuccess(FanucTcpResponse response)
|
||||
{
|
||||
if (!response.IsSuccess)
|
||||
{
|
||||
_logger?.LogError("FANUC 命令失败: msgId=0x{MessageId:X4}, resultCode={ResultCode}", response.MessageId, response.ResultCode);
|
||||
throw CreateCommandFailureException(response.MessageId, response.ResultCode);
|
||||
}
|
||||
|
||||
return response;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 校验 IO 读取响应结果码，失败时抛出包含消息号和结果码的诊断异常。
|
||||
/// </summary>
|
||||
private FanucIoResponse EnsureSuccess(FanucIoResponse response)
|
||||
{
|
||||
if (!response.IsSuccess)
|
||||
{
|
||||
_logger?.LogError("FANUC 命令失败: msgId=0x{MessageId:X4}, resultCode={ResultCode}", response.MessageId, response.ResultCode);
|
||||
throw CreateCommandFailureException(response.MessageId, response.ResultCode);
|
||||
}
|
||||
|
||||
return response;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 构造包含 FANUC 命令上下文的失败异常。
|
||||
/// </summary>
|
||||
private static InvalidOperationException CreateCommandFailureException(uint messageId, uint resultCode)
|
||||
{
|
||||
return new InvalidOperationException(
|
||||
$"FANUC command 0x{messageId:X4} failed with result_code {resultCode}.");
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 从流中读取一条完整的 doz/zod 响应帧。
|
||||
/// </summary>
|
||||
private async Task<byte[]> ReadResponseFrameAsync(CancellationToken cancellationToken)
|
||||
{
|
||||
if (_stream is null)
|
||||
{
|
||||
throw new InvalidOperationException("命令通道未连接。");
|
||||
}
|
||||
|
||||
// 先读取 11 字节头：doz(3) + length(4) + msg_id(4)
|
||||
var header = new byte[11];
|
||||
await ReadExactAsync(header, cancellationToken).ConfigureAwait(false);
|
||||
|
||||
if (header[0] != (byte)'d' || header[1] != (byte)'o' || header[2] != (byte)'z')
|
||||
{
|
||||
throw new InvalidDataException("响应帧头 magic 不正确。");
|
||||
}
|
||||
|
||||
var declaredLength = System.Buffers.Binary.BinaryPrimitives.ReadUInt32BigEndian(header.AsSpan(3, 4));
|
||||
if (declaredLength < 14)
|
||||
{
|
||||
throw new InvalidDataException("响应帧声明长度过短。");
|
||||
}
|
||||
|
||||
var remaining = (int)declaredLength - 11;
|
||||
var frame = new byte[declaredLength];
|
||||
header.CopyTo(frame, 0);
|
||||
await ReadExactAsync(frame.AsMemory(11, remaining), cancellationToken).ConfigureAwait(false);
|
||||
|
||||
// 校验帧尾
|
||||
if (frame[^3] != (byte)'z' || frame[^2] != (byte)'o' || frame[^1] != (byte)'d')
|
||||
{
|
||||
throw new InvalidDataException("响应帧尾 magic 不正确。");
|
||||
}
|
||||
|
||||
return frame;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 从流中精确读取指定长度的字节。
|
||||
/// </summary>
|
||||
private async Task ReadExactAsync(Memory<byte> buffer, CancellationToken cancellationToken)
|
||||
{
|
||||
if (_stream is null)
|
||||
{
|
||||
throw new InvalidOperationException("命令通道未连接。");
|
||||
}
|
||||
|
||||
var totalRead = 0;
|
||||
while (totalRead < buffer.Length)
|
||||
{
|
||||
var read = await _stream.ReadAsync(buffer.Slice(totalRead), cancellationToken).ConfigureAwait(false);
|
||||
if (read == 0)
|
||||
{
|
||||
throw new IOException("命令通道已断开，读取到 EOF。");
|
||||
}
|
||||
|
||||
totalRead += read;
|
||||
}
|
||||
}
|
||||
}
|
||||
621
src/Flyshot.Runtime.Fanuc/Protocol/FanucCommandProtocol.cs
Normal file
621
src/Flyshot.Runtime.Fanuc/Protocol/FanucCommandProtocol.cs
Normal file
@@ -0,0 +1,621 @@
|
||||
using System.Buffers.Binary;
|
||||
using System.Text;
|
||||
|
||||
namespace Flyshot.Runtime.Fanuc.Protocol;
|
||||
|
||||
/// <summary>
|
||||
/// 定义 FANUC TCP 10012 命令通道已经由抓包和逆向资料确认的消息号。
|
||||
/// </summary>
|
||||
public static class FanucCommandMessageIds
|
||||
{
|
||||
/// <summary>
|
||||
/// 获取控制器程序状态的消息号。
|
||||
/// </summary>
|
||||
public const uint GetProgramStatus = 0x2003;
|
||||
|
||||
/// <summary>
|
||||
/// 复位控制器的消息号。
|
||||
/// </summary>
|
||||
public const uint ResetRobot = 0x2100;
|
||||
|
||||
/// <summary>
|
||||
/// 启动控制器程序的消息号。
|
||||
/// </summary>
|
||||
public const uint StartProgram = 0x2102;
|
||||
|
||||
/// <summary>
|
||||
/// 停止控制器程序的消息号。
|
||||
/// </summary>
|
||||
public const uint StopProgram = 0x2103;
|
||||
|
||||
/// <summary>
|
||||
/// 读取控制器 TCP 的消息号。
|
||||
/// </summary>
|
||||
public const uint GetTcp = 0x2200;
|
||||
|
||||
/// <summary>
|
||||
/// 设置控制器 TCP 的消息号。
|
||||
/// </summary>
|
||||
public const uint SetTcp = 0x2201;
|
||||
|
||||
/// <summary>
|
||||
/// 读取控制器速度倍率的消息号。
|
||||
/// </summary>
|
||||
public const uint GetSpeedRatio = 0x2206;
|
||||
|
||||
/// <summary>
|
||||
/// 设置控制器速度倍率的消息号。
|
||||
/// </summary>
|
||||
public const uint SetSpeedRatio = 0x2207;
|
||||
|
||||
/// <summary>
|
||||
/// 读取控制器 IO 的消息号。
|
||||
/// </summary>
|
||||
public const uint GetIo = 0x2208;
|
||||
|
||||
/// <summary>
|
||||
/// 设置控制器 IO 的消息号。
|
||||
/// </summary>
|
||||
public const uint SetIo = 0x2209;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 定义旧 ControllerClient 公开的 FANUC IO 类型枚举值。
|
||||
/// </summary>
|
||||
public static class FanucIoTypes
|
||||
{
|
||||
/// <summary>
|
||||
/// FANUC 数字输入 DI。
|
||||
/// </summary>
|
||||
public const uint DigitalInput = 1;
|
||||
|
||||
/// <summary>
|
||||
/// FANUC 数字输出 DO。
|
||||
/// </summary>
|
||||
public const uint DigitalOutput = 2;
|
||||
|
||||
/// <summary>
|
||||
/// FANUC 机器人输入 RI。
|
||||
/// </summary>
|
||||
public const uint RobotInput = 8;
|
||||
|
||||
/// <summary>
|
||||
/// FANUC 机器人输出 RO。
|
||||
/// </summary>
|
||||
public const uint RobotOutput = 9;
|
||||
|
||||
/// <summary>
|
||||
/// 将 HTTP/兼容层传入的 IO 类型字符串转换为 FANUC 命令通道枚举值。
|
||||
/// </summary>
|
||||
/// <param name="ioType">IO 类型字符串，例如 DI、DO、RI、RO。</param>
|
||||
/// <returns>命令通道使用的 IO 类型数值。</returns>
|
||||
public static uint FromName(string ioType)
|
||||
{
|
||||
if (string.IsNullOrWhiteSpace(ioType))
|
||||
{
|
||||
throw new ArgumentException("IO 类型不能为空。", nameof(ioType));
|
||||
}
|
||||
|
||||
return ioType.Trim().ToUpperInvariant() switch
|
||||
{
|
||||
"DI" or "KIOTYPEDI" => DigitalInput,
|
||||
"DO" or "KIOTYPEDO" => DigitalOutput,
|
||||
"RI" or "KIOTYPERI" => RobotInput,
|
||||
"RO" or "KIOTYPERO" => RobotOutput,
|
||||
_ => throw new ArgumentOutOfRangeException(nameof(ioType), ioType, "未知 IO 类型。")
|
||||
};
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 表示 FANUC TCP 10012 命令通道中只携带结果码的响应。
|
||||
/// </summary>
|
||||
public sealed class FanucCommandResultResponse
|
||||
{
|
||||
/// <summary>
|
||||
/// 初始化命令结果响应。
|
||||
/// </summary>
|
||||
/// <param name="messageId">响应对应的消息号。</param>
|
||||
/// <param name="resultCode">控制器返回的结果码。</param>
|
||||
public FanucCommandResultResponse(uint messageId, uint resultCode)
|
||||
{
|
||||
MessageId = messageId;
|
||||
ResultCode = resultCode;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 获取响应对应的消息号。
|
||||
/// </summary>
|
||||
public uint MessageId { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取控制器返回的结果码。
|
||||
/// </summary>
|
||||
public uint ResultCode { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取当前响应是否表示成功。
|
||||
/// </summary>
|
||||
public bool IsSuccess => ResultCode == 0;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 表示 FANUC TCP 10012 速度倍率响应。
|
||||
/// </summary>
|
||||
public sealed class FanucSpeedRatioResponse
|
||||
{
|
||||
/// <summary>
|
||||
/// 初始化速度倍率响应。
|
||||
/// </summary>
|
||||
/// <param name="messageId">响应对应的消息号。</param>
|
||||
/// <param name="ratioInt">控制器返回的整数百分比。</param>
|
||||
/// <param name="resultCode">控制器返回的结果码。</param>
|
||||
public FanucSpeedRatioResponse(uint messageId, uint ratioInt, uint resultCode)
|
||||
{
|
||||
MessageId = messageId;
|
||||
RatioInt = ratioInt;
|
||||
ResultCode = resultCode;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 获取响应对应的消息号。
|
||||
/// </summary>
|
||||
public uint MessageId { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取控制器返回的整数百分比。
|
||||
/// </summary>
|
||||
public uint RatioInt { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取控制器返回的结果码。
|
||||
/// </summary>
|
||||
public uint ResultCode { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取转换后的 0.0 到 1.0 速度倍率。
|
||||
/// </summary>
|
||||
public double Ratio => RatioInt / 100.0;
|
||||
|
||||
/// <summary>
|
||||
/// 获取当前响应是否表示成功。
|
||||
/// </summary>
|
||||
public bool IsSuccess => ResultCode == 0;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 表示 FANUC TCP 10012 TCP 位姿响应。
|
||||
/// </summary>
|
||||
public sealed class FanucTcpResponse
|
||||
{
|
||||
/// <summary>
|
||||
/// 初始化 TCP 位姿响应。
|
||||
/// </summary>
|
||||
/// <param name="messageId">响应对应的消息号。</param>
|
||||
/// <param name="resultCode">控制器返回的结果码。</param>
|
||||
/// <param name="tcpId">控制器返回的 TCP ID。</param>
|
||||
/// <param name="pose">7 维 TCP 位姿。</param>
|
||||
public FanucTcpResponse(uint messageId, uint resultCode, uint tcpId, IReadOnlyList<double> pose)
|
||||
{
|
||||
MessageId = messageId;
|
||||
ResultCode = resultCode;
|
||||
TcpId = tcpId;
|
||||
Pose = pose.ToArray();
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 获取响应对应的消息号。
|
||||
/// </summary>
|
||||
public uint MessageId { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取控制器返回的结果码。
|
||||
/// </summary>
|
||||
public uint ResultCode { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取控制器返回的 TCP ID。
|
||||
/// </summary>
|
||||
public uint TcpId { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取 7 维 TCP 位姿。
|
||||
/// </summary>
|
||||
public IReadOnlyList<double> Pose { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取当前响应是否表示成功。
|
||||
/// </summary>
|
||||
public bool IsSuccess => ResultCode == 0;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 表示 FANUC TCP 10012 IO 读取响应。
|
||||
/// </summary>
|
||||
public sealed class FanucIoResponse
|
||||
{
|
||||
/// <summary>
|
||||
/// 初始化 IO 读取响应。
|
||||
/// </summary>
|
||||
/// <param name="messageId">响应对应的消息号。</param>
|
||||
/// <param name="resultCode">控制器返回的结果码。</param>
|
||||
/// <param name="numericValue">控制器返回的 float IO 数值。</param>
|
||||
public FanucIoResponse(uint messageId, uint resultCode, double numericValue)
|
||||
{
|
||||
MessageId = messageId;
|
||||
ResultCode = resultCode;
|
||||
NumericValue = numericValue;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 获取响应对应的消息号。
|
||||
/// </summary>
|
||||
public uint MessageId { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取控制器返回的结果码。
|
||||
/// </summary>
|
||||
public uint ResultCode { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取控制器返回的原始数值。
|
||||
/// </summary>
|
||||
public double NumericValue { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取按布尔 IO 解释后的值。
|
||||
/// </summary>
|
||||
public bool Value => Math.Abs(NumericValue) > double.Epsilon;
|
||||
|
||||
/// <summary>
|
||||
/// 获取当前响应是否表示成功。
|
||||
/// </summary>
|
||||
public bool IsSuccess => ResultCode == 0;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 表示 FANUC TCP 10012 程序状态响应。
|
||||
/// </summary>
|
||||
public sealed class FanucProgramStatusResponse
|
||||
{
|
||||
/// <summary>
|
||||
/// 初始化程序状态响应。
|
||||
/// </summary>
|
||||
/// <param name="messageId">响应对应的消息号。</param>
|
||||
/// <param name="resultCode">控制器返回的结果码。</param>
|
||||
/// <param name="programStatus">控制器程序状态。</param>
|
||||
public FanucProgramStatusResponse(uint messageId, uint resultCode, uint programStatus)
|
||||
{
|
||||
MessageId = messageId;
|
||||
ResultCode = resultCode;
|
||||
ProgramStatus = programStatus;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 获取响应对应的消息号。
|
||||
/// </summary>
|
||||
public uint MessageId { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取控制器返回的结果码。
|
||||
/// </summary>
|
||||
public uint ResultCode { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取控制器程序状态值。
|
||||
/// </summary>
|
||||
public uint ProgramStatus { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取当前响应是否表示成功。
|
||||
/// </summary>
|
||||
public bool IsSuccess => ResultCode == 0;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 提供 FANUC TCP 10012 命令通道的基础封包与响应解析能力。
|
||||
/// </summary>
|
||||
public static class FanucCommandProtocol
|
||||
{
|
||||
/// <summary>
|
||||
/// 将无业务体命令封装为 TCP 10012 二进制帧。
|
||||
/// </summary>
|
||||
/// <param name="messageId">命令消息号。</param>
|
||||
/// <returns>可直接写入命令通道 Socket 的完整帧。</returns>
|
||||
public static byte[] PackEmptyCommand(uint messageId)
|
||||
{
|
||||
return PackFrame(messageId, ReadOnlySpan<byte>.Empty);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 将程序名命令封装为 TCP 10012 二进制帧。
|
||||
/// </summary>
|
||||
/// <param name="messageId">命令消息号。</param>
|
||||
/// <param name="programName">控制器程序名。</param>
|
||||
/// <returns>可直接写入命令通道 Socket 的完整帧。</returns>
|
||||
public static byte[] PackProgramCommand(uint messageId, string programName)
|
||||
{
|
||||
if (string.IsNullOrWhiteSpace(programName))
|
||||
{
|
||||
throw new ArgumentException("程序名不能为空。", nameof(programName));
|
||||
}
|
||||
|
||||
var programNameBytes = Encoding.ASCII.GetBytes(programName);
|
||||
var body = new byte[sizeof(uint) + programNameBytes.Length];
|
||||
BinaryPrimitives.WriteUInt32BigEndian(body.AsSpan(0, sizeof(uint)), (uint)programNameBytes.Length);
|
||||
programNameBytes.CopyTo(body.AsSpan(sizeof(uint)));
|
||||
|
||||
return PackFrame(messageId, body);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 封装读取速度倍率命令。
|
||||
/// </summary>
|
||||
/// <returns>可直接写入命令通道 Socket 的完整帧。</returns>
|
||||
public static byte[] PackGetSpeedRatioCommand()
|
||||
{
|
||||
return PackEmptyCommand(FanucCommandMessageIds.GetSpeedRatio);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 封装设置速度倍率命令，按旧系统逻辑转换为 0..100 的整数百分比。
|
||||
/// </summary>
|
||||
/// <param name="ratio">目标速度倍率。</param>
|
||||
/// <returns>可直接写入命令通道 Socket 的完整帧。</returns>
|
||||
public static byte[] PackSetSpeedRatioCommand(double ratio)
|
||||
{
|
||||
if (double.IsNaN(ratio) || double.IsInfinity(ratio))
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(nameof(ratio), "ratio 必须是有限数值。");
|
||||
}
|
||||
|
||||
var ratioInt = (uint)Math.Clamp((int)(ratio * 100.0), 0, 100);
|
||||
var body = new byte[sizeof(uint)];
|
||||
BinaryPrimitives.WriteUInt32BigEndian(body, ratioInt);
|
||||
return PackFrame(FanucCommandMessageIds.SetSpeedRatio, body);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 封装读取 TCP 位姿命令。
|
||||
/// </summary>
|
||||
/// <param name="tcpId">目标 TCP ID。</param>
|
||||
/// <returns>可直接写入命令通道 Socket 的完整帧。</returns>
|
||||
public static byte[] PackGetTcpCommand(uint tcpId)
|
||||
{
|
||||
var body = new byte[sizeof(uint)];
|
||||
BinaryPrimitives.WriteUInt32BigEndian(body, tcpId);
|
||||
return PackFrame(FanucCommandMessageIds.GetTcp, body);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 封装设置 TCP 位姿命令。
|
||||
/// </summary>
|
||||
/// <param name="tcpId">目标 TCP ID。</param>
|
||||
/// <param name="pose">7 维 TCP 位姿。</param>
|
||||
/// <returns>可直接写入命令通道 Socket 的完整帧。</returns>
|
||||
public static byte[] PackSetTcpCommand(uint tcpId, IReadOnlyList<double> pose)
|
||||
{
|
||||
ArgumentNullException.ThrowIfNull(pose);
|
||||
if (pose.Count != 7)
|
||||
{
|
||||
throw new ArgumentException("TCP 位姿必须包含 7 个数值。", nameof(pose));
|
||||
}
|
||||
|
||||
var body = new byte[sizeof(uint) + sizeof(float) * 7];
|
||||
BinaryPrimitives.WriteUInt32BigEndian(body.AsSpan(0, sizeof(uint)), tcpId);
|
||||
for (int i = 0; i < 7; i++)
|
||||
{
|
||||
BinaryPrimitives.WriteSingleBigEndian(body.AsSpan(sizeof(uint) + i * sizeof(float), sizeof(float)), (float)pose[i]);
|
||||
}
|
||||
|
||||
return PackFrame(FanucCommandMessageIds.SetTcp, body);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 封装读取 IO 命令，字段顺序为 io_type 后接 io_index。
|
||||
/// </summary>
|
||||
/// <param name="ioType">IO 类型数值。</param>
|
||||
/// <param name="ioIndex">IO 索引。</param>
|
||||
/// <returns>可直接写入命令通道 Socket 的完整帧。</returns>
|
||||
public static byte[] PackGetIoCommand(uint ioType, int ioIndex)
|
||||
{
|
||||
if (ioIndex < 0)
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(nameof(ioIndex), "IO 索引不能为负数。");
|
||||
}
|
||||
|
||||
var body = new byte[sizeof(uint) * 2];
|
||||
BinaryPrimitives.WriteUInt32BigEndian(body.AsSpan(0, sizeof(uint)), ioType);
|
||||
BinaryPrimitives.WriteUInt32BigEndian(body.AsSpan(sizeof(uint), sizeof(uint)), (uint)ioIndex);
|
||||
return PackFrame(FanucCommandMessageIds.GetIo, body);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 封装设置 IO 命令，字段顺序为 io_type、io_index、float io_value。
|
||||
/// </summary>
|
||||
/// <param name="ioType">IO 类型数值。</param>
|
||||
/// <param name="ioIndex">IO 索引。</param>
|
||||
/// <param name="value">目标 IO 布尔值。</param>
|
||||
/// <returns>可直接写入命令通道 Socket 的完整帧。</returns>
|
||||
public static byte[] PackSetIoCommand(uint ioType, int ioIndex, bool value)
|
||||
{
|
||||
if (ioIndex < 0)
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(nameof(ioIndex), "IO 索引不能为负数。");
|
||||
}
|
||||
|
||||
var body = new byte[sizeof(uint) * 2 + sizeof(float)];
|
||||
BinaryPrimitives.WriteUInt32BigEndian(body.AsSpan(0, sizeof(uint)), ioType);
|
||||
BinaryPrimitives.WriteUInt32BigEndian(body.AsSpan(sizeof(uint), sizeof(uint)), (uint)ioIndex);
|
||||
BinaryPrimitives.WriteSingleBigEndian(body.AsSpan(sizeof(uint) * 2, sizeof(float)), value ? 1.0f : 0.0f);
|
||||
return PackFrame(FanucCommandMessageIds.SetIo, body);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 解析只携带结果码的 TCP 10012 响应帧。
|
||||
/// </summary>
|
||||
/// <param name="frame">完整响应帧。</param>
|
||||
/// <returns>命令结果响应。</returns>
|
||||
public static FanucCommandResultResponse ParseResultResponse(ReadOnlySpan<byte> frame)
|
||||
{
|
||||
var messageId = ValidateAndReadMessageId(frame);
|
||||
var body = GetBody(frame);
|
||||
if (body.Length < sizeof(uint))
|
||||
{
|
||||
throw new InvalidDataException("FANUC 命令响应体长度不足。");
|
||||
}
|
||||
|
||||
return new FanucCommandResultResponse(
|
||||
messageId,
|
||||
BinaryPrimitives.ReadUInt32BigEndian(body[..sizeof(uint)]));
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 解析 GetSpeedRatio 的 TCP 10012 响应帧。
|
||||
/// </summary>
|
||||
/// <param name="frame">完整响应帧。</param>
|
||||
/// <returns>速度倍率响应。</returns>
|
||||
public static FanucSpeedRatioResponse ParseSpeedRatioResponse(ReadOnlySpan<byte> frame)
|
||||
{
|
||||
var messageId = ValidateAndReadMessageId(frame);
|
||||
var body = GetBody(frame);
|
||||
if (body.Length < sizeof(uint) * 2)
|
||||
{
|
||||
throw new InvalidDataException("FANUC 速度倍率响应体长度不足。");
|
||||
}
|
||||
|
||||
// GetSpeedRatio 的字段顺序特殊：ratio_int 在前，result_code 在后。
|
||||
var ratioInt = BinaryPrimitives.ReadUInt32BigEndian(body[..sizeof(uint)]);
|
||||
var resultCode = BinaryPrimitives.ReadUInt32BigEndian(body.Slice(sizeof(uint), sizeof(uint)));
|
||||
return new FanucSpeedRatioResponse(messageId, ratioInt, resultCode);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 解析 GetTCP 的 TCP 10012 响应帧。
|
||||
/// </summary>
|
||||
/// <param name="frame">完整响应帧。</param>
|
||||
/// <returns>TCP 位姿响应。</returns>
|
||||
public static FanucTcpResponse ParseTcpResponse(ReadOnlySpan<byte> frame)
|
||||
{
|
||||
var messageId = ValidateAndReadMessageId(frame);
|
||||
var body = GetBody(frame);
|
||||
if (body.Length < sizeof(uint) * 2 + sizeof(float) * 7)
|
||||
{
|
||||
throw new InvalidDataException("FANUC TCP 响应体长度不足。");
|
||||
}
|
||||
|
||||
var resultCode = BinaryPrimitives.ReadUInt32BigEndian(body[..sizeof(uint)]);
|
||||
var tcpId = BinaryPrimitives.ReadUInt32BigEndian(body.Slice(sizeof(uint), sizeof(uint)));
|
||||
var pose = new double[7];
|
||||
for (int i = 0; i < pose.Length; i++)
|
||||
{
|
||||
pose[i] = BinaryPrimitives.ReadSingleBigEndian(body.Slice(sizeof(uint) * 2 + i * sizeof(float), sizeof(float)));
|
||||
}
|
||||
|
||||
return new FanucTcpResponse(messageId, resultCode, tcpId, pose);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 解析 GetIO 的 TCP 10012 响应帧。
|
||||
/// </summary>
|
||||
/// <param name="frame">完整响应帧。</param>
|
||||
/// <returns>IO 读取响应。</returns>
|
||||
public static FanucIoResponse ParseIoResponse(ReadOnlySpan<byte> frame)
|
||||
{
|
||||
var messageId = ValidateAndReadMessageId(frame);
|
||||
var body = GetBody(frame);
|
||||
if (body.Length < sizeof(uint) + sizeof(float))
|
||||
{
|
||||
throw new InvalidDataException("FANUC IO 响应体长度不足。");
|
||||
}
|
||||
|
||||
var resultCode = BinaryPrimitives.ReadUInt32BigEndian(body[..sizeof(uint)]);
|
||||
var ioValue = BinaryPrimitives.ReadSingleBigEndian(body.Slice(sizeof(uint), sizeof(float)));
|
||||
return new FanucIoResponse(messageId, resultCode, ioValue);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 解析 GetProgStatus 的 TCP 10012 响应帧。
|
||||
/// </summary>
|
||||
/// <param name="frame">完整响应帧。</param>
|
||||
/// <returns>程序状态响应。</returns>
|
||||
public static FanucProgramStatusResponse ParseProgramStatusResponse(ReadOnlySpan<byte> frame)
|
||||
{
|
||||
var messageId = ValidateAndReadMessageId(frame);
|
||||
var body = GetBody(frame);
|
||||
if (body.Length < sizeof(uint) * 2)
|
||||
{
|
||||
throw new InvalidDataException("FANUC 程序状态响应体长度不足。");
|
||||
}
|
||||
|
||||
// all-reconnect.pcap 中字段顺序为 prog_status 后接 result_code。
|
||||
var programStatus = BinaryPrimitives.ReadUInt32BigEndian(body[..sizeof(uint)]);
|
||||
var resultCode = BinaryPrimitives.ReadUInt32BigEndian(body.Slice(sizeof(uint), sizeof(uint)));
|
||||
return new FanucProgramStatusResponse(messageId, resultCode, programStatus);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 按 FANUC 命令通道 framing 规则封装完整帧。
|
||||
/// </summary>
|
||||
/// <param name="messageId">命令消息号。</param>
|
||||
/// <param name="body">业务体。</param>
|
||||
/// <returns>完整命令帧。</returns>
|
||||
internal static byte[] PackFrame(uint messageId, ReadOnlySpan<byte> body)
|
||||
{
|
||||
var frameLength = 3 + sizeof(uint) + sizeof(uint) + body.Length + 3;
|
||||
var frame = new byte[frameLength];
|
||||
|
||||
frame[0] = (byte)'d';
|
||||
frame[1] = (byte)'o';
|
||||
frame[2] = (byte)'z';
|
||||
BinaryPrimitives.WriteUInt32BigEndian(frame.AsSpan(3, sizeof(uint)), (uint)frameLength);
|
||||
BinaryPrimitives.WriteUInt32BigEndian(frame.AsSpan(7, sizeof(uint)), messageId);
|
||||
body.CopyTo(frame.AsSpan(11));
|
||||
frame[^3] = (byte)'z';
|
||||
frame[^2] = (byte)'o';
|
||||
frame[^1] = (byte)'d';
|
||||
|
||||
return frame;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 校验完整帧并读取消息号。
|
||||
/// </summary>
|
||||
/// <param name="frame">完整响应帧。</param>
|
||||
/// <returns>响应消息号。</returns>
|
||||
private static uint ValidateAndReadMessageId(ReadOnlySpan<byte> frame)
|
||||
{
|
||||
if (frame.Length < 14)
|
||||
{
|
||||
throw new InvalidDataException("FANUC 命令帧长度不足。");
|
||||
}
|
||||
|
||||
if (frame[0] != (byte)'d' || frame[1] != (byte)'o' || frame[2] != (byte)'z')
|
||||
{
|
||||
throw new InvalidDataException("FANUC 命令帧头 magic 不正确。");
|
||||
}
|
||||
|
||||
if (frame[^3] != (byte)'z' || frame[^2] != (byte)'o' || frame[^1] != (byte)'d')
|
||||
{
|
||||
throw new InvalidDataException("FANUC 命令帧尾 magic 不正确。");
|
||||
}
|
||||
|
||||
var declaredLength = BinaryPrimitives.ReadUInt32BigEndian(frame.Slice(3, sizeof(uint)));
|
||||
if (declaredLength != frame.Length)
|
||||
{
|
||||
throw new InvalidDataException("FANUC 命令帧长度字段与实际长度不一致。");
|
||||
}
|
||||
|
||||
return BinaryPrimitives.ReadUInt32BigEndian(frame.Slice(7, sizeof(uint)));
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 获取完整帧中的业务体切片。
|
||||
/// </summary>
|
||||
/// <param name="frame">完整响应帧。</param>
|
||||
/// <returns>业务体切片。</returns>
|
||||
private static ReadOnlySpan<byte> GetBody(ReadOnlySpan<byte> frame)
|
||||
{
|
||||
return frame.Slice(11, frame.Length - 14);
|
||||
}
|
||||
}
|
||||
654
src/Flyshot.Runtime.Fanuc/Protocol/FanucJ519Client.cs
Normal file
654
src/Flyshot.Runtime.Fanuc/Protocol/FanucJ519Client.cs
Normal file
@@ -0,0 +1,654 @@
|
||||
using System.Diagnostics;
|
||||
using System.Net.Sockets;
|
||||
using Microsoft.Extensions.Logging;
|
||||
|
||||
namespace Flyshot.Runtime.Fanuc.Protocol;
|
||||
|
||||
/// <summary>
|
||||
/// FANUC UDP 60015 J519/ICSP 伺服运动客户端，提供状态包驱动的命令发送与响应接收能力。
|
||||
/// </summary>
|
||||
public sealed class FanucJ519Client : IDisposable
|
||||
{
|
||||
private readonly object _commandLock = new();
|
||||
private readonly object _responseLock = new();
|
||||
private readonly ILogger<FanucJ519Client>? _logger;
|
||||
private UdpClient? _udpClient;
|
||||
private CancellationTokenSource? _cts;
|
||||
private Thread? _receiveThread;
|
||||
private FanucJ519Command? _currentCommand;
|
||||
private FanucJ519Command? _lastSentCommand;
|
||||
// 稠密轨迹执行时预装的命令队列，由机器人状态包节拍逐帧出队。
|
||||
private Queue<FanucJ519Command>? _commandQueue;
|
||||
private TaskCompletionSource? _commandQueueDrainedCompletion;
|
||||
private List<FanucJ519Command>? _commandHistoryForTests;
|
||||
private FanucJ519Response? _latestResponse;
|
||||
private long _slowSendCount;
|
||||
private long _maxReceiveToSendTicks;
|
||||
private uint _sequenceBufferSize;
|
||||
// 标记 StartMotion 前是否刚装载过新目标，用于区分新命令和上次运动残留目标。
|
||||
private bool _hasPendingCommandForStart;
|
||||
private bool _motionStarted;
|
||||
private bool _disposed;
|
||||
|
||||
/// <summary>
|
||||
/// 获取当前是否已创建 UDP 套接字。
|
||||
/// </summary>
|
||||
public bool IsConnected => _udpClient is not null;
|
||||
|
||||
/// <summary>
|
||||
/// 初始化 FANUC J519 客户端。
|
||||
/// </summary>
|
||||
/// <param name="logger">日志记录器；允许 null，供无日志场景使用。</param>
|
||||
public FanucJ519Client(ILogger<FanucJ519Client>? logger = null)
|
||||
{
|
||||
_logger = logger;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 建立到 FANUC 控制柜 UDP 60015 运动通道的连接并启动接收循环。
|
||||
/// </summary>
|
||||
/// <param name="ip">控制柜 IP 地址。</param>
|
||||
/// <param name="port">运动通道端口，默认 60015。</param>
|
||||
/// <param name="cancellationToken">取消令牌。</param>
|
||||
public async Task ConnectAsync(string ip, int port = 60015, CancellationToken cancellationToken = default)
|
||||
{
|
||||
ObjectDisposedException.ThrowIf(_disposed, this);
|
||||
|
||||
if (string.IsNullOrWhiteSpace(ip))
|
||||
{
|
||||
throw new ArgumentException("IP 不能为空。", nameof(ip));
|
||||
}
|
||||
|
||||
if (_udpClient is not null)
|
||||
{
|
||||
throw new InvalidOperationException("J519 通道已经连接，请先 Disconnect。");
|
||||
}
|
||||
|
||||
_logger?.LogInformation("J519 ConnectAsync: {Ip}:{Port}", ip, port);
|
||||
|
||||
_udpClient = new UdpClient();
|
||||
_udpClient.Connect(ip, port);
|
||||
ConfigureRealtimeSocket(_udpClient.Client);
|
||||
ConfigureProcessPriority();
|
||||
|
||||
// 发送初始化包。
|
||||
await _udpClient.SendAsync(FanucJ519Protocol.PackInitPacket(), cancellationToken).ConfigureAwait(false);
|
||||
_logger?.LogInformation("J519 初始化包已发送");
|
||||
|
||||
_cts = new CancellationTokenSource();
|
||||
_receiveThread = new Thread(ReceiveLoop)
|
||||
{
|
||||
IsBackground = true,
|
||||
Name = "J519 UDP realtime loop",
|
||||
Priority = ThreadPriority.Highest
|
||||
};
|
||||
_receiveThread.Start();
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 启动 J519 命令发送许可；实际发包由机器人状态包节拍驱动。
|
||||
/// </summary>
|
||||
public void StartMotion()
|
||||
{
|
||||
ObjectDisposedException.ThrowIf(_disposed, this);
|
||||
|
||||
if (_udpClient is null)
|
||||
{
|
||||
throw new InvalidOperationException("J519 通道未连接。");
|
||||
}
|
||||
|
||||
lock (_commandLock)
|
||||
{
|
||||
_lastSentCommand = null;
|
||||
if (_motionStarted)
|
||||
{
|
||||
_logger?.LogDebug("J519 StartMotion: 状态包驱动发送已启用");
|
||||
return;
|
||||
}
|
||||
|
||||
if (!_hasPendingCommandForStart)
|
||||
{
|
||||
_currentCommand = null;
|
||||
CompleteCommandQueueLocked();
|
||||
}
|
||||
|
||||
_hasPendingCommandForStart = false;
|
||||
_motionStarted = true;
|
||||
|
||||
}
|
||||
|
||||
_logger?.LogInformation("J519 StartMotion: 已启用状态包驱动发送");
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 配置状态包驱动回发时附加到机器人 sequence 的前视缓冲深度。
|
||||
/// </summary>
|
||||
/// <param name="bufferSize">要附加到状态序号上的缓冲深度。</param>
|
||||
public void SetSequenceBufferSize(int bufferSize)
|
||||
{
|
||||
ObjectDisposedException.ThrowIf(_disposed, this);
|
||||
|
||||
if (bufferSize < 0)
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(nameof(bufferSize), "J519 sequence buffer size 不能为负数。");
|
||||
}
|
||||
|
||||
lock (_commandLock)
|
||||
{
|
||||
_sequenceBufferSize = (uint)bufferSize;
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 发送状态输出停止包并停止 J519 命令发送。
|
||||
/// </summary>
|
||||
public async Task StopMotionAsync(CancellationToken cancellationToken = default)
|
||||
{
|
||||
ObjectDisposedException.ThrowIf(_disposed, this);
|
||||
|
||||
if (_udpClient is null)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
_logger?.LogInformation("J519 StopMotionAsync: 停止状态包驱动发送");
|
||||
lock (_commandLock)
|
||||
{
|
||||
_motionStarted = false;
|
||||
_hasPendingCommandForStart = false;
|
||||
CompleteCommandQueueLocked();
|
||||
}
|
||||
|
||||
// FANUC 手册中 packet type=2 表示停止状态包输出；当前保留现场抓包兼容行为。
|
||||
await _udpClient.SendAsync(FanucJ519Protocol.PackEndPacket(), cancellationToken).ConfigureAwait(false);
|
||||
_logger?.LogInformation("J519 StopMotionAsync: 状态输出停止包已发送");
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 原子更新下一周期要发送的 J519 命令。
|
||||
/// </summary>
|
||||
/// <param name="command">新的 J519 命令。</param>
|
||||
public void UpdateCommand(FanucJ519Command command)
|
||||
{
|
||||
ArgumentNullException.ThrowIfNull(command);
|
||||
ObjectDisposedException.ThrowIf(_disposed, this);
|
||||
|
||||
lock (_commandLock)
|
||||
{
|
||||
CompleteCommandQueueLocked();
|
||||
_currentCommand = command;
|
||||
_hasPendingCommandForStart = true;
|
||||
_commandHistoryForTests?.Add(command);
|
||||
}
|
||||
|
||||
if (_logger?.IsEnabled(LogLevel.Debug) == true)
|
||||
{
|
||||
//_logger.LogDebug(
|
||||
// "J519 UpdateCommand: joints={Joints}, ioMask={IoMask}, ioValue={IoValue}",
|
||||
// command.TargetJoints,
|
||||
// command.WriteIoMask,
|
||||
// command.WriteIoValue);
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 装载一整段 J519 命令队列；后续每个可接收命令的机器人状态包会自动取出下一帧。
|
||||
/// </summary>
|
||||
/// <param name="commands">按执行顺序排列的 J519 命令列表，至少包含一帧。</param>
|
||||
public void LoadCommandQueue(IReadOnlyList<FanucJ519Command> commands)
|
||||
{
|
||||
ArgumentNullException.ThrowIfNull(commands);
|
||||
ObjectDisposedException.ThrowIf(_disposed, this);
|
||||
if (commands.Count == 0)
|
||||
{
|
||||
throw new ArgumentException("J519 命令队列至少需要包含一帧。", nameof(commands));
|
||||
}
|
||||
|
||||
lock (_commandLock)
|
||||
{
|
||||
CompleteCommandQueueLocked();
|
||||
_commandQueue = new Queue<FanucJ519Command>(commands);
|
||||
// 队列耗尽后继续保持最后一帧目标，避免运动结束后回落到旧目标或空目标。
|
||||
_currentCommand = commands[^1];
|
||||
_hasPendingCommandForStart = true;
|
||||
_commandQueueDrainedCompletion = new TaskCompletionSource(TaskCreationOptions.RunContinuationsAsynchronously);
|
||||
_commandHistoryForTests?.AddRange(commands);
|
||||
}
|
||||
_logger?.LogInformation("J519 命令队列已装载: count={Count}", commands.Count);
|
||||
_logger?.LogInformation("开始运动前向机器人发送的sequence={Sequence}",_lastSentCommand?.Sequence ?? 0);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 等待当前预装命令队列被状态包全部取出；无队列时立即完成。
|
||||
/// </summary>
|
||||
/// <param name="cancellationToken">取消令牌。</param>
|
||||
/// <returns>表示等待过程的任务。</returns>
|
||||
internal Task WaitForCommandQueueDrainedAsync(CancellationToken cancellationToken = default)
|
||||
{
|
||||
ObjectDisposedException.ThrowIf(_disposed, this);
|
||||
|
||||
Task waitTask;
|
||||
lock (_commandLock)
|
||||
{
|
||||
waitTask = _commandQueueDrainedCompletion?.Task ?? Task.CompletedTask;
|
||||
}
|
||||
|
||||
return waitTask.WaitAsync(cancellationToken);
|
||||
}
|
||||
|
||||
/// 判断当前是否没有等待出队的命令；仅供单元测试断言。
|
||||
/// </summary>
|
||||
/// <returns>如果队列为空或尚未装载队列，则返回 true。</returns>
|
||||
internal bool IsCommandQueueDrainedForTests()
|
||||
{
|
||||
ObjectDisposedException.ThrowIf(_disposed, this);
|
||||
|
||||
lock (_commandLock)
|
||||
{
|
||||
return _commandQueue is null || _commandQueue.Count == 0;
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 打开命令历史记录，仅供单元测试验证运行时生成的命令序列。
|
||||
/// </summary>
|
||||
internal void EnableCommandHistoryForTests()
|
||||
{
|
||||
ObjectDisposedException.ThrowIf(_disposed, this);
|
||||
|
||||
lock (_commandLock)
|
||||
{
|
||||
_commandHistoryForTests = [];
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 获取测试记录的命令历史。
|
||||
/// </summary>
|
||||
/// <returns>命令历史快照。</returns>
|
||||
internal IReadOnlyList<FanucJ519Command> GetCommandHistoryForTests()
|
||||
{
|
||||
ObjectDisposedException.ThrowIf(_disposed, this);
|
||||
|
||||
lock (_commandLock)
|
||||
{
|
||||
return _commandHistoryForTests?.ToArray() ?? Array.Empty<FanucJ519Command>();
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 获取最近一次通过 UpdateCommand 设置的 J519 命令；供测试断言使用。
|
||||
/// </summary>
|
||||
/// <returns>当前 J519 命令或 null。</returns>
|
||||
internal FanucJ519Command? GetCurrentCommand()
|
||||
{
|
||||
ObjectDisposedException.ThrowIf(_disposed, this);
|
||||
|
||||
lock (_commandLock)
|
||||
{
|
||||
return _currentCommand;
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 获取最近一次解析的 J519 响应；若尚未收到任何响应则返回 null。
|
||||
/// </summary>
|
||||
/// <returns>最新 J519 响应或 null。</returns>
|
||||
public FanucJ519Response? GetLatestResponse()
|
||||
{
|
||||
ObjectDisposedException.ThrowIf(_disposed, this);
|
||||
|
||||
lock (_responseLock)
|
||||
{
|
||||
return _latestResponse;
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 断开 J519 通道并释放资源。
|
||||
/// </summary>
|
||||
public void Disconnect()
|
||||
{
|
||||
ObjectDisposedException.ThrowIf(_disposed, this);
|
||||
|
||||
_cts?.Cancel();
|
||||
|
||||
try
|
||||
{
|
||||
_udpClient?.Dispose();
|
||||
_receiveThread?.Join(TimeSpan.FromSeconds(1));
|
||||
}
|
||||
catch (ObjectDisposedException)
|
||||
{
|
||||
// 忽略释放期间的套接字关闭异常。
|
||||
}
|
||||
|
||||
_receiveThread = null;
|
||||
|
||||
_cts?.Dispose();
|
||||
_cts = null;
|
||||
|
||||
_udpClient = null;
|
||||
|
||||
lock (_commandLock)
|
||||
{
|
||||
_currentCommand = null;
|
||||
_lastSentCommand = null;
|
||||
CompleteCommandQueueLocked();
|
||||
_commandHistoryForTests = null;
|
||||
_hasPendingCommandForStart = false;
|
||||
_motionStarted = false;
|
||||
}
|
||||
|
||||
lock (_responseLock)
|
||||
{
|
||||
_latestResponse = null;
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 释放客户端资源。
|
||||
/// </summary>
|
||||
public void Dispose()
|
||||
{
|
||||
if (_disposed)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
_disposed = true;
|
||||
_cts?.Cancel();
|
||||
|
||||
try
|
||||
{
|
||||
_udpClient?.Dispose();
|
||||
_receiveThread?.Join(TimeSpan.FromSeconds(1));
|
||||
}
|
||||
catch (ObjectDisposedException)
|
||||
{
|
||||
// 忽略释放期间的套接字关闭异常。
|
||||
}
|
||||
|
||||
_cts?.Dispose();
|
||||
|
||||
lock (_commandLock)
|
||||
{
|
||||
CompleteCommandQueueLocked();
|
||||
_hasPendingCommandForStart = false;
|
||||
}
|
||||
}
|
||||
|
||||
private static void ConfigureProcessPriority()
|
||||
{
|
||||
try
|
||||
{
|
||||
var process = Process.GetCurrentProcess();
|
||||
if (process.PriorityClass < ProcessPriorityClass.High)
|
||||
{
|
||||
process.PriorityClass = ProcessPriorityClass.High;
|
||||
}
|
||||
}
|
||||
catch (Exception)
|
||||
{
|
||||
// 某些部署环境不允许提升进程优先级；实时链路仍按普通优先级运行。
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 配置 J519 UDP 套接字的低延迟参数。
|
||||
/// </summary>
|
||||
/// <param name="socket">已连接 FANUC 60015 的 UDP 套接字。</param>
|
||||
private void ConfigureRealtimeSocket(Socket socket)
|
||||
{
|
||||
socket.ReceiveBufferSize = 1024 * 1024;
|
||||
socket.SendBufferSize = 1024 * 1024;
|
||||
|
||||
try
|
||||
{
|
||||
// DSCP EF(46) 标记低延迟流量，是否生效取决于现场网卡、交换机和控制柜网络策略。
|
||||
socket.SetSocketOption(SocketOptionLevel.IP, SocketOptionName.TypeOfService, 0xB8);
|
||||
}
|
||||
catch (SocketException ex)
|
||||
{
|
||||
_logger?.LogWarning(ex, "J519 UDP 套接字无法设置 DSCP EF 优先级标记");
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 后台接收循环：在专用高优先级线程中同步接收 132B 响应并立即回发命令。
|
||||
/// </summary>
|
||||
private void ReceiveLoop()
|
||||
{
|
||||
var udpClient = _udpClient;
|
||||
var cancellationToken = _cts?.Token ?? CancellationToken.None;
|
||||
if (udpClient is null)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
_logger?.LogInformation("J519 ReceiveLoop 启动");
|
||||
long receiveCount = 0;
|
||||
FanucJ519Response? lastLoggedResponse = null;
|
||||
var receiveBuffer = new byte[FanucJ519Protocol.ResponsePacketLength];
|
||||
var commandBuffer = new byte[FanucJ519Protocol.CommandPacketLength];
|
||||
|
||||
try
|
||||
{
|
||||
while (!cancellationToken.IsCancellationRequested)
|
||||
{
|
||||
var received = udpClient.Client.Receive(receiveBuffer);
|
||||
if (received == FanucJ519Protocol.ResponsePacketLength)
|
||||
{
|
||||
var receiveTicks = Stopwatch.GetTimestamp();
|
||||
var response = FanucJ519Protocol.ParseResponse(receiveBuffer);
|
||||
|
||||
// 先按状态包节拍回发命令，再做低频日志处理，减少受信周期内的非必要工作。
|
||||
if (response.AcceptsCommand)
|
||||
{
|
||||
SendCommandForStatus(udpClient.Client, response, commandBuffer, receiveTicks);
|
||||
}
|
||||
|
||||
lock (_responseLock)
|
||||
{
|
||||
_latestResponse = response;
|
||||
}
|
||||
|
||||
receiveCount++;
|
||||
|
||||
// 仅在状态变化时记录 Info，避免高频日志。
|
||||
if (lastLoggedResponse is null
|
||||
|| lastLoggedResponse.Status != response.Status
|
||||
|| lastLoggedResponse.RobotInMotion != response.RobotInMotion
|
||||
|| lastLoggedResponse.SystemReady != response.SystemReady
|
||||
|| lastLoggedResponse.AcceptsCommand != response.AcceptsCommand)
|
||||
{
|
||||
_logger?.LogInformation(
|
||||
"J519 响应: status=0x{Status:X2}, seq={Seq}, accept={Accept}, received={received}, SystemReady={SystemReady}, RobotInMotion={RobotInMotion}, pose=[{Pose}], joints=[{Joints}]",
|
||||
response.Status,
|
||||
response.Sequence,
|
||||
response.AcceptsCommand,
|
||||
response.ReceivedCommand,
|
||||
response.SystemReady,
|
||||
response.RobotInMotion,
|
||||
string.Join(", ", response.Pose.Select(v => v.ToString("F3"))),
|
||||
string.Join(", ", response.JointDegrees.Take(6).Select(v => v.ToString("F3"))));
|
||||
|
||||
var lastSentTargetJoints = GetLastSentTargetJointsLogText();
|
||||
_logger?.LogInformation("J519 最后一条发送目标关节轴: joints=[{Joints}]", lastSentTargetJoints);
|
||||
lastLoggedResponse = response;
|
||||
|
||||
// 如果状态从AcceptsCommand true 变为false,说明机器人报错,清空队列
|
||||
if (!response.AcceptsCommand)
|
||||
{
|
||||
lock (_commandLock)
|
||||
{
|
||||
_currentCommand = null;
|
||||
CompleteCommandQueueLocked();
|
||||
}
|
||||
_logger?.LogWarning("J519 接收状态包显示机器人不可接受命令，已清空命令队列");
|
||||
}
|
||||
|
||||
}
|
||||
else if (receiveCount % 1000 == 0)
|
||||
{
|
||||
var maxReceiveToSendMs = Stopwatch.GetElapsedTime(0, Interlocked.Read(ref _maxReceiveToSendTicks)).TotalMilliseconds;
|
||||
_logger?.LogDebug(
|
||||
"J519 已接收 {Count} 个响应包，receive-to-send 最大耗时约 {MaxMs:F3}ms，超过 0.5ms 次数 {SlowCount}",
|
||||
receiveCount,
|
||||
maxReceiveToSendMs,
|
||||
Interlocked.Read(ref _slowSendCount));
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
catch (OperationCanceledException)
|
||||
{
|
||||
_logger?.LogInformation("J519 ReceiveLoop 正常取消，共接收 {Count} 个包", receiveCount);
|
||||
}
|
||||
catch (ObjectDisposedException)
|
||||
{
|
||||
_logger?.LogInformation("J519 ReceiveLoop 因 UDP 释放退出，共接收 {Count} 个包", receiveCount);
|
||||
}
|
||||
catch (SocketException ex) when (cancellationToken.IsCancellationRequested)
|
||||
{
|
||||
_logger?.LogInformation(ex, "J519 ReceiveLoop 因取消关闭套接字退出，共接收 {Count} 个包", receiveCount);
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 按机器人状态包的 sequence 立即回发当前 J519 命令。
|
||||
/// </summary>
|
||||
/// <param name="socket">已连接的 UDP 套接字。</param>
|
||||
/// <param name="response">刚收到的机器人状态包。</param>
|
||||
/// <param name="commandBuffer">可复用的 64B 命令包缓冲区。</param>
|
||||
/// <param name="receiveTicks">收到状态包后的时间戳。</param>
|
||||
private void SendCommandForStatus(Socket socket, FanucJ519Response response, byte[] commandBuffer, long receiveTicks)
|
||||
{
|
||||
FanucJ519Command? command;
|
||||
var willDrainQueue = false;
|
||||
lock (_commandLock)
|
||||
{
|
||||
if (!_motionStarted)
|
||||
{
|
||||
command = null;
|
||||
}
|
||||
else if (_commandQueue is { Count: > 0 } queue)
|
||||
{
|
||||
// 状态包是唯一节拍源：每收到一帧可接收状态，才取出下一条目标。
|
||||
var queuedCommand = queue.Dequeue();
|
||||
_currentCommand = queuedCommand;
|
||||
willDrainQueue = queue.Count == 0;
|
||||
command = queuedCommand;
|
||||
}
|
||||
else
|
||||
{
|
||||
command = _currentCommand;
|
||||
}
|
||||
}
|
||||
|
||||
if (command is null)
|
||||
{
|
||||
command = TryBuildHoldCommandFromLatestResponse(response);
|
||||
}
|
||||
|
||||
if (command is null)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
uint sequenceToSend;
|
||||
lock (_commandLock)
|
||||
{
|
||||
sequenceToSend = response.Sequence + _sequenceBufferSize;
|
||||
}
|
||||
|
||||
FanucJ519Protocol.PackCommandPacket(command, sequenceToSend, commandBuffer);
|
||||
socket.Send(commandBuffer);
|
||||
TrackReceiveToSendLatency(receiveTicks);
|
||||
// _logger?.LogDebug("J519 已回发命令包，seq={Seq}", sequence);
|
||||
// _logger?.LogDebug(
|
||||
// "J519 回发命令详情: joints={Joints}, ioMask={IoMask}, ioValue={IoValue}",
|
||||
// command.TargetJoints,
|
||||
// command.WriteIoMask,
|
||||
// command.WriteIoValue);
|
||||
lock (_commandLock)
|
||||
{
|
||||
_lastSentCommand = command;
|
||||
if (willDrainQueue && _commandQueue is { Count: 0 })
|
||||
{
|
||||
CompleteCommandQueueLocked();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 当当前没有显式目标时，使用最近一帧状态反馈关节角构造保姿命令，维持机器人当前位置。
|
||||
/// </summary>
|
||||
/// <param name="response">当前收到的机器人状态包。</param>
|
||||
/// <returns>可用于保姿的临时 J519 命令；若反馈关节不足则返回 null。</returns>
|
||||
private static FanucJ519Command? TryBuildHoldCommandFromLatestResponse(FanucJ519Response response)
|
||||
{
|
||||
if (response.JointDegrees.Count < 6)
|
||||
{
|
||||
return null;
|
||||
}
|
||||
|
||||
// 无运动目标时，持续回发机器人当前反馈关节，保持伺服流与机器人当前位置一致。
|
||||
return new FanucJ519Command(
|
||||
sequence: response.Sequence,
|
||||
targetJoints:
|
||||
[
|
||||
response.JointDegrees[0],
|
||||
response.JointDegrees[1],
|
||||
response.JointDegrees[2],
|
||||
response.JointDegrees[3],
|
||||
response.JointDegrees[4],
|
||||
response.JointDegrees[5]
|
||||
]);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 记录状态包到命令包发出的最大耗时和慢发送次数，供低频诊断日志观察调度抖动。
|
||||
/// </summary>
|
||||
/// <param name="receiveTicks">收到状态包后的时间戳。</param>
|
||||
private void TrackReceiveToSendLatency(long receiveTicks)
|
||||
{
|
||||
var elapsedTicks = Stopwatch.GetTimestamp() - receiveTicks;
|
||||
var currentMax = Interlocked.Read(ref _maxReceiveToSendTicks);
|
||||
while (elapsedTicks > currentMax
|
||||
&& Interlocked.CompareExchange(ref _maxReceiveToSendTicks, elapsedTicks, currentMax) != currentMax)
|
||||
{
|
||||
currentMax = Interlocked.Read(ref _maxReceiveToSendTicks);
|
||||
}
|
||||
|
||||
if (Stopwatch.GetElapsedTime(0, elapsedTicks) > TimeSpan.FromMilliseconds(0.5))
|
||||
{
|
||||
Interlocked.Increment(ref _slowSendCount);
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 清空当前命令队列，并唤醒等待队列结束的运行时任务。
|
||||
/// </summary>
|
||||
private void CompleteCommandQueueLocked()
|
||||
{
|
||||
_commandQueue?.Clear();
|
||||
_commandQueue = null;
|
||||
_commandQueueDrainedCompletion?.TrySetResult();
|
||||
_commandQueueDrainedCompletion = null;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 读取最近一次已成功发送命令的目标关节轴文本，便于状态日志直接对照控制目标。
|
||||
/// </summary>
|
||||
/// <returns>格式化后的目标关节轴文本；如果尚未发送命令则返回占位符。</returns>
|
||||
private string GetLastSentTargetJointsLogText()
|
||||
{
|
||||
lock (_commandLock)
|
||||
{
|
||||
return _lastSentCommand is null
|
||||
? "n/a"
|
||||
: string.Join(", ", _lastSentCommand.TargetJoints.Take(6).Select(v => v.ToString("F5")));
|
||||
}
|
||||
}
|
||||
}
|
||||
450
src/Flyshot.Runtime.Fanuc/Protocol/FanucJ519Protocol.cs
Normal file
450
src/Flyshot.Runtime.Fanuc/Protocol/FanucJ519Protocol.cs
Normal file
@@ -0,0 +1,450 @@
|
||||
using System.Buffers.Binary;
|
||||
|
||||
namespace Flyshot.Runtime.Fanuc.Protocol;
|
||||
|
||||
/// <summary>
|
||||
/// 表示 FANUC UDP 60015 J519/ICSP 伺服流中的一帧命令数据。
|
||||
/// </summary>
|
||||
public sealed class FanucJ519Command
|
||||
{
|
||||
private readonly double[] _targetValues;
|
||||
|
||||
/// <summary>
|
||||
/// 初始化 J519 命令数据。
|
||||
/// </summary>
|
||||
/// <param name="sequence">命令序号。</param>
|
||||
/// <param name="targetJoints">目标关节或扩展轴数据，最多 9 个槽位。</param>
|
||||
/// <param name="lastData">是否为最后一帧数据。</param>
|
||||
/// <param name="readIoType">读取 IO 类型。</param>
|
||||
/// <param name="readIoIndex">读取 IO 起始索引。</param>
|
||||
/// <param name="readIoMask">读取 IO 掩码。</param>
|
||||
/// <param name="dataStyle">目标数据类型。</param>
|
||||
/// <param name="writeIoType">写入 IO 类型。</param>
|
||||
/// <param name="writeIoIndex">写入 IO 起始索引。</param>
|
||||
/// <param name="writeIoMask">写入 IO 掩码。</param>
|
||||
/// <param name="writeIoValue">写入 IO 数值。</param>
|
||||
public FanucJ519Command(
|
||||
uint sequence,
|
||||
IReadOnlyList<double> targetJoints,
|
||||
byte lastData = 0,
|
||||
byte readIoType = 2,
|
||||
ushort readIoIndex = 1,
|
||||
ushort readIoMask = 255,
|
||||
byte dataStyle = 1,
|
||||
byte writeIoType = 2,
|
||||
ushort writeIoIndex = 1,
|
||||
ushort writeIoMask = 0,
|
||||
ushort writeIoValue = 0)
|
||||
: this(
|
||||
sequence,
|
||||
targetValues: targetJoints,
|
||||
lastData,
|
||||
readIoType,
|
||||
readIoIndex,
|
||||
readIoMask,
|
||||
dataStyle,
|
||||
writeIoType,
|
||||
writeIoIndex,
|
||||
writeIoMask,
|
||||
writeIoValue)
|
||||
{
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 初始化 J519 命令数据。
|
||||
/// </summary>
|
||||
/// <param name="sequence">命令序号。</param>
|
||||
/// <param name="targetValues">J519 目标槽位，直角模式为 X/Y/Z/W/P/R/E1/E2/E3，关节模式为 J1..J9。</param>
|
||||
/// <param name="lastData">是否为最后一帧数据。</param>
|
||||
/// <param name="readIoType">读取 IO 类型。</param>
|
||||
/// <param name="readIoIndex">读取 IO 起始索引。</param>
|
||||
/// <param name="readIoMask">读取 IO 掩码。</param>
|
||||
/// <param name="dataStyle">目标数据类型，0 为直角坐标，1 为关节坐标。</param>
|
||||
/// <param name="writeIoType">写入 IO 类型。</param>
|
||||
/// <param name="writeIoIndex">写入 IO 起始索引。</param>
|
||||
/// <param name="writeIoMask">写入 IO 掩码。</param>
|
||||
/// <param name="writeIoValue">写入 IO 数值。</param>
|
||||
public FanucJ519Command(
|
||||
uint sequence,
|
||||
IEnumerable<double> targetValues,
|
||||
byte lastData = 0,
|
||||
byte readIoType = 2,
|
||||
ushort readIoIndex = 1,
|
||||
ushort readIoMask = 255,
|
||||
byte dataStyle = 1,
|
||||
byte writeIoType = 2,
|
||||
ushort writeIoIndex = 1,
|
||||
ushort writeIoMask = 0,
|
||||
ushort writeIoValue = 0)
|
||||
{
|
||||
ArgumentNullException.ThrowIfNull(targetValues);
|
||||
var copiedTargetValues = targetValues.ToArray();
|
||||
if (copiedTargetValues.Length is <= 0 or > 9)
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(nameof(targetValues), "J519 目标数据必须包含 1 到 9 个槽位。");
|
||||
}
|
||||
|
||||
Sequence = sequence;
|
||||
LastData = lastData;
|
||||
ReadIoType = readIoType;
|
||||
ReadIoIndex = readIoIndex;
|
||||
ReadIoMask = readIoMask;
|
||||
DataStyle = dataStyle;
|
||||
WriteIoType = writeIoType;
|
||||
WriteIoIndex = writeIoIndex;
|
||||
WriteIoMask = writeIoMask;
|
||||
WriteIoValue = writeIoValue;
|
||||
_targetValues = copiedTargetValues;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 获取命令序号。
|
||||
/// </summary>
|
||||
public uint Sequence { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取是否为最后一帧数据。
|
||||
/// </summary>
|
||||
public byte LastData { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取读取 IO 类型。
|
||||
/// </summary>
|
||||
public byte ReadIoType { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取读取 IO 起始索引。
|
||||
/// </summary>
|
||||
public ushort ReadIoIndex { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取读取 IO 掩码。
|
||||
/// </summary>
|
||||
public ushort ReadIoMask { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取目标数据类型。
|
||||
/// </summary>
|
||||
public byte DataStyle { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取写入 IO 类型。
|
||||
/// </summary>
|
||||
public byte WriteIoType { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取写入 IO 起始索引。
|
||||
/// </summary>
|
||||
public ushort WriteIoIndex { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取写入 IO 掩码。
|
||||
/// </summary>
|
||||
public ushort WriteIoMask { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取写入 IO 数值。
|
||||
/// </summary>
|
||||
public ushort WriteIoValue { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取目标关节或扩展轴数据。
|
||||
/// </summary>
|
||||
public IReadOnlyList<double> TargetJoints => _targetValues;
|
||||
|
||||
/// <summary>
|
||||
/// 获取 J519 目标槽位，直角模式为 X/Y/Z/W/P/R/E1/E2/E3，关节模式为 J1..J9。
|
||||
/// </summary>
|
||||
public IReadOnlyList<double> TargetValues => _targetValues;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 表示 FANUC UDP 60015 J519/ICSP 伺服流中的一帧响应数据。
|
||||
/// </summary>
|
||||
public sealed class FanucJ519Response
|
||||
{
|
||||
private readonly double[] _pose;
|
||||
private readonly double[] _externalAxes;
|
||||
private readonly double[] _jointDegrees;
|
||||
private readonly double[] _motorCurrents;
|
||||
|
||||
/// <summary>
|
||||
/// 初始化 J519 响应数据。
|
||||
/// </summary>
|
||||
/// <param name="messageType">响应类型。</param>
|
||||
/// <param name="version">协议版本。</param>
|
||||
/// <param name="sequence">响应序号。</param>
|
||||
/// <param name="status">状态位集合。</param>
|
||||
/// <param name="readIoType">读取 IO 类型。</param>
|
||||
/// <param name="readIoIndex">读取 IO 起始索引。</param>
|
||||
/// <param name="readIoMask">读取 IO 掩码。</param>
|
||||
/// <param name="readIoValue">读取 IO 数值。</param>
|
||||
/// <param name="timestamp">控制器时间戳。</param>
|
||||
/// <param name="pose">TCP 笛卡尔位姿。</param>
|
||||
/// <param name="externalAxes">扩展轴反馈。</param>
|
||||
/// <param name="jointDegrees">关节角度反馈。</param>
|
||||
/// <param name="motorCurrents">电机电流反馈。</param>
|
||||
public FanucJ519Response(
|
||||
uint messageType,
|
||||
uint version,
|
||||
uint sequence,
|
||||
byte status,
|
||||
byte readIoType,
|
||||
ushort readIoIndex,
|
||||
ushort readIoMask,
|
||||
ushort readIoValue,
|
||||
uint timestamp,
|
||||
IEnumerable<double> pose,
|
||||
IEnumerable<double> externalAxes,
|
||||
IEnumerable<double> jointDegrees,
|
||||
IEnumerable<double> motorCurrents)
|
||||
{
|
||||
MessageType = messageType;
|
||||
Version = version;
|
||||
Sequence = sequence;
|
||||
Status = status;
|
||||
ReadIoType = readIoType;
|
||||
ReadIoIndex = readIoIndex;
|
||||
ReadIoMask = readIoMask;
|
||||
ReadIoValue = readIoValue;
|
||||
Timestamp = timestamp;
|
||||
_pose = pose?.ToArray() ?? throw new ArgumentNullException(nameof(pose));
|
||||
_externalAxes = externalAxes?.ToArray() ?? throw new ArgumentNullException(nameof(externalAxes));
|
||||
_jointDegrees = jointDegrees?.ToArray() ?? throw new ArgumentNullException(nameof(jointDegrees));
|
||||
_motorCurrents = motorCurrents?.ToArray() ?? throw new ArgumentNullException(nameof(motorCurrents));
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 获取响应类型。
|
||||
/// </summary>
|
||||
public uint MessageType { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取协议版本。
|
||||
/// </summary>
|
||||
public uint Version { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取响应序号。
|
||||
/// </summary>
|
||||
public uint Sequence { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取状态位集合。
|
||||
/// </summary>
|
||||
public byte Status { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取读取 IO 类型。
|
||||
/// </summary>
|
||||
public byte ReadIoType { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取读取 IO 起始索引。
|
||||
/// </summary>
|
||||
public ushort ReadIoIndex { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取读取 IO 掩码。
|
||||
/// </summary>
|
||||
public ushort ReadIoMask { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取读取 IO 数值。
|
||||
/// </summary>
|
||||
public ushort ReadIoValue { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取控制器时间戳。
|
||||
/// </summary>
|
||||
public uint Timestamp { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取 TCP 笛卡尔位姿。
|
||||
/// </summary>
|
||||
public IReadOnlyList<double> Pose => _pose;
|
||||
|
||||
/// <summary>
|
||||
/// 获取扩展轴反馈。
|
||||
/// </summary>
|
||||
public IReadOnlyList<double> ExternalAxes => _externalAxes;
|
||||
|
||||
/// <summary>
|
||||
/// 获取关节角度反馈。
|
||||
/// </summary>
|
||||
public IReadOnlyList<double> JointDegrees => _jointDegrees;
|
||||
|
||||
/// <summary>
|
||||
/// 获取电机电流反馈。
|
||||
/// </summary>
|
||||
public IReadOnlyList<double> MotorCurrents => _motorCurrents;
|
||||
|
||||
/// <summary>
|
||||
/// 获取控制器是否接受命令。
|
||||
/// </summary>
|
||||
public bool AcceptsCommand => (Status & 0b0001) != 0;
|
||||
|
||||
/// <summary>
|
||||
/// 获取控制器是否已收到命令。
|
||||
/// </summary>
|
||||
public bool ReceivedCommand => (Status & 0b0010) != 0;
|
||||
|
||||
/// <summary>
|
||||
/// 获取控制器系统是否就绪。
|
||||
/// </summary>
|
||||
public bool SystemReady => (Status & 0b0100) != 0;
|
||||
|
||||
/// <summary>
|
||||
/// 获取机器人是否处于运动中。
|
||||
/// </summary>
|
||||
public bool RobotInMotion => (Status & 0b1000) != 0;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 提供 FANUC UDP 60015 J519/ICSP 伺服流的基础封包与响应解析能力。
|
||||
/// </summary>
|
||||
public static class FanucJ519Protocol
|
||||
{
|
||||
/// <summary>
|
||||
/// J519 初始化和结束控制包长度。
|
||||
/// </summary>
|
||||
public const int ControlPacketLength = 8;
|
||||
|
||||
/// <summary>
|
||||
/// J519 命令包长度。
|
||||
/// </summary>
|
||||
public const int CommandPacketLength = 64;
|
||||
|
||||
/// <summary>
|
||||
/// J519 响应包长度。
|
||||
/// </summary>
|
||||
public const int ResponsePacketLength = 132;
|
||||
|
||||
/// <summary>
|
||||
/// 封装 J519 初始化包。
|
||||
/// </summary>
|
||||
/// <returns>初始化包。</returns>
|
||||
public static byte[] PackInitPacket()
|
||||
{
|
||||
return PackControlPacket(0);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 封装 J519 结束包。
|
||||
/// </summary>
|
||||
/// <returns>结束包。</returns>
|
||||
public static byte[] PackEndPacket()
|
||||
{
|
||||
return PackControlPacket(2);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 封装 J519 64 字节命令包。
|
||||
/// </summary>
|
||||
/// <param name="command">命令数据。</param>
|
||||
/// <returns>命令包。</returns>
|
||||
public static byte[] PackCommandPacket(FanucJ519Command command)
|
||||
{
|
||||
ArgumentNullException.ThrowIfNull(command);
|
||||
|
||||
var packet = new byte[CommandPacketLength];
|
||||
PackCommandPacket(command, command.Sequence, packet);
|
||||
return packet;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 将 J519 64 字节命令包写入调用方提供的缓冲区。
|
||||
/// </summary>
|
||||
/// <param name="command">命令数据。</param>
|
||||
/// <param name="sequence">本次出包使用的机器人状态包序号。</param>
|
||||
/// <param name="packet">长度至少为 64 字节的命令包缓冲区。</param>
|
||||
public static void PackCommandPacket(FanucJ519Command command, uint sequence, Span<byte> packet)
|
||||
{
|
||||
ArgumentNullException.ThrowIfNull(command);
|
||||
if (packet.Length < CommandPacketLength)
|
||||
{
|
||||
throw new ArgumentException("J519 命令包缓冲区长度不足。", nameof(packet));
|
||||
}
|
||||
|
||||
packet.Slice(0, CommandPacketLength).Clear();
|
||||
BinaryPrimitives.WriteUInt32BigEndian(packet.Slice(0x00, sizeof(uint)), 1);
|
||||
BinaryPrimitives.WriteUInt32BigEndian(packet.Slice(0x04, sizeof(uint)), 1);
|
||||
BinaryPrimitives.WriteUInt32BigEndian(packet.Slice(0x08, sizeof(uint)), sequence);
|
||||
packet[0x0c] = command.LastData;
|
||||
packet[0x0d] = command.ReadIoType;
|
||||
BinaryPrimitives.WriteUInt16BigEndian(packet.Slice(0x0e, sizeof(ushort)), command.ReadIoIndex);
|
||||
BinaryPrimitives.WriteUInt16BigEndian(packet.Slice(0x10, sizeof(ushort)), command.ReadIoMask);
|
||||
packet[0x12] = command.DataStyle;
|
||||
packet[0x13] = command.WriteIoType;
|
||||
BinaryPrimitives.WriteUInt16BigEndian(packet.Slice(0x14, sizeof(ushort)), command.WriteIoIndex);
|
||||
BinaryPrimitives.WriteUInt16BigEndian(packet.Slice(0x16, sizeof(ushort)), command.WriteIoMask);
|
||||
BinaryPrimitives.WriteUInt16BigEndian(packet.Slice(0x18, sizeof(ushort)), command.WriteIoValue);
|
||||
BinaryPrimitives.WriteUInt16BigEndian(packet.Slice(0x1a, sizeof(ushort)), 0);
|
||||
|
||||
// J519 命令包固定保留 9 个 f32 目标槽位，少于 9 个时剩余槽位补零。
|
||||
for (var index = 0; index < 9; index++)
|
||||
{
|
||||
var value = index < command.TargetJoints.Count ? command.TargetJoints[index] : 0.0;
|
||||
BinaryPrimitives.WriteSingleBigEndian(packet.Slice(0x1c + (index * sizeof(float)), sizeof(float)), (float)value);
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 解析 J519 132 字节响应包。
|
||||
/// </summary>
|
||||
/// <param name="packet">响应包。</param>
|
||||
/// <returns>响应解析结果。</returns>
|
||||
public static FanucJ519Response ParseResponse(ReadOnlySpan<byte> packet)
|
||||
{
|
||||
if (packet.Length != ResponsePacketLength)
|
||||
{
|
||||
throw new InvalidDataException("FANUC J519 响应包长度不正确。");
|
||||
}
|
||||
|
||||
return new FanucJ519Response(
|
||||
BinaryPrimitives.ReadUInt32BigEndian(packet.Slice(0x00, sizeof(uint))),
|
||||
BinaryPrimitives.ReadUInt32BigEndian(packet.Slice(0x04, sizeof(uint))),
|
||||
BinaryPrimitives.ReadUInt32BigEndian(packet.Slice(0x08, sizeof(uint))),
|
||||
packet[0x0c],
|
||||
packet[0x0d],
|
||||
BinaryPrimitives.ReadUInt16BigEndian(packet.Slice(0x0e, sizeof(ushort))),
|
||||
BinaryPrimitives.ReadUInt16BigEndian(packet.Slice(0x10, sizeof(ushort))),
|
||||
BinaryPrimitives.ReadUInt16BigEndian(packet.Slice(0x12, sizeof(ushort))),
|
||||
BinaryPrimitives.ReadUInt32BigEndian(packet.Slice(0x14, sizeof(uint))),
|
||||
ReadFloatArray(packet, 0x18, 6),
|
||||
ReadFloatArray(packet, 0x30, 3),
|
||||
ReadFloatArray(packet, 0x3c, 9),
|
||||
ReadFloatArray(packet, 0x60, 9));
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 封装 J519 控制包。
|
||||
/// </summary>
|
||||
/// <param name="packetType">控制包类型。</param>
|
||||
/// <returns>控制包。</returns>
|
||||
private static byte[] PackControlPacket(uint packetType)
|
||||
{
|
||||
var packet = new byte[ControlPacketLength];
|
||||
BinaryPrimitives.WriteUInt32BigEndian(packet.AsSpan(0, sizeof(uint)), packetType);
|
||||
BinaryPrimitives.WriteUInt32BigEndian(packet.AsSpan(sizeof(uint), sizeof(uint)), 1);
|
||||
return packet;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 从响应包中读取固定长度 f32 数组。
|
||||
/// </summary>
|
||||
/// <param name="packet">响应包。</param>
|
||||
/// <param name="offset">数组起始偏移。</param>
|
||||
/// <param name="count">数组元素数量。</param>
|
||||
/// <returns>转换成 double 的数值数组。</returns>
|
||||
private static double[] ReadFloatArray(ReadOnlySpan<byte> packet, int offset, int count)
|
||||
{
|
||||
var values = new double[count];
|
||||
for (var index = 0; index < count; index++)
|
||||
{
|
||||
values[index] = BinaryPrimitives.ReadSingleBigEndian(packet.Slice(offset + (index * sizeof(float)), sizeof(float)));
|
||||
}
|
||||
|
||||
return values;
|
||||
}
|
||||
}
|
||||
632
src/Flyshot.Runtime.Fanuc/Protocol/FanucStateClient.cs
Normal file
632
src/Flyshot.Runtime.Fanuc/Protocol/FanucStateClient.cs
Normal file
@@ -0,0 +1,632 @@
|
||||
using System.Net.Sockets;
|
||||
using Microsoft.Extensions.Logging;
|
||||
|
||||
namespace Flyshot.Runtime.Fanuc.Protocol;
|
||||
|
||||
/// <summary>
|
||||
/// 表示 FANUC TCP 10010 状态通道客户端的连接阶段。
|
||||
/// </summary>
|
||||
public enum FanucStateConnectionState
|
||||
{
|
||||
/// <summary>
|
||||
/// 状态通道未连接。
|
||||
/// </summary>
|
||||
Disconnected,
|
||||
|
||||
/// <summary>
|
||||
/// 状态通道正在建立连接。
|
||||
/// </summary>
|
||||
Connecting,
|
||||
|
||||
/// <summary>
|
||||
/// 状态通道已连接并由后台循环接收状态帧。
|
||||
/// </summary>
|
||||
Connected,
|
||||
|
||||
/// <summary>
|
||||
/// 状态通道在限定时间内没有收到完整状态帧。
|
||||
/// </summary>
|
||||
TimedOut,
|
||||
|
||||
/// <summary>
|
||||
/// 状态通道正在按退避策略重新连接。
|
||||
/// </summary>
|
||||
Reconnecting,
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 定义 FANUC TCP 10010 状态通道的超时和重连参数。
|
||||
/// </summary>
|
||||
public sealed class FanucStateClientOptions
|
||||
{
|
||||
/// <summary>
|
||||
/// 获取或设置接收一帧完整 90B 状态帧允许的最长时间。
|
||||
/// </summary>
|
||||
public TimeSpan FrameTimeout { get; init; } = TimeSpan.FromMilliseconds(250);
|
||||
|
||||
/// <summary>
|
||||
/// 获取或设置初始重连等待时间。
|
||||
/// </summary>
|
||||
public TimeSpan ReconnectInitialDelay { get; init; } = TimeSpan.FromMilliseconds(100);
|
||||
|
||||
/// <summary>
|
||||
/// 获取或设置重连等待时间的上限。
|
||||
/// </summary>
|
||||
public TimeSpan ReconnectMaxDelay { get; init; } = TimeSpan.FromSeconds(5);
|
||||
|
||||
/// <summary>
|
||||
/// 获取或设置单次 TCP 建连允许的最长时间。
|
||||
/// </summary>
|
||||
public TimeSpan ConnectTimeout { get; init; } = TimeSpan.FromSeconds(5);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 表示 FANUC TCP 10010 状态通道客户端的当前诊断状态。
|
||||
/// </summary>
|
||||
public sealed class FanucStateClientStatus
|
||||
{
|
||||
/// <summary>
|
||||
/// 初始化状态通道诊断状态。
|
||||
/// </summary>
|
||||
public FanucStateClientStatus(
|
||||
FanucStateConnectionState state,
|
||||
bool isFrameStale,
|
||||
DateTimeOffset? lastFrameAt,
|
||||
long reconnectAttemptCount,
|
||||
string? lastErrorMessage)
|
||||
{
|
||||
State = state;
|
||||
IsFrameStale = isFrameStale;
|
||||
LastFrameAt = lastFrameAt;
|
||||
ReconnectAttemptCount = reconnectAttemptCount;
|
||||
LastErrorMessage = lastErrorMessage;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 获取状态通道当前连接阶段。
|
||||
/// </summary>
|
||||
public FanucStateConnectionState State { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取最近缓存状态帧是否已经超过状态帧超时窗口。
|
||||
/// </summary>
|
||||
public bool IsFrameStale { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取最近一次成功解析状态帧的 UTC 时间。
|
||||
/// </summary>
|
||||
public DateTimeOffset? LastFrameAt { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取后台循环发起重连的累计次数。
|
||||
/// </summary>
|
||||
public long ReconnectAttemptCount { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取最近一次状态通道异常的诊断文本。
|
||||
/// </summary>
|
||||
public string? LastErrorMessage { get; }
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// FANUC TCP 10010 状态通道客户端，持续接收状态帧并缓存最新快照。
|
||||
/// </summary>
|
||||
public sealed class FanucStateClient : IDisposable
|
||||
{
|
||||
private readonly object _stateLock = new();
|
||||
private readonly FanucStateClientOptions _options;
|
||||
private readonly ILogger<FanucStateClient>? _logger;
|
||||
private TcpClient? _tcpClient;
|
||||
private NetworkStream? _stream;
|
||||
private CancellationTokenSource? _receiveCts;
|
||||
private Task? _receiveTask;
|
||||
private FanucStateFrame? _latestFrame;
|
||||
private FanucStateConnectionState _connectionState = FanucStateConnectionState.Disconnected;
|
||||
private DateTimeOffset? _lastConnectedAt;
|
||||
private DateTimeOffset? _lastFrameAt;
|
||||
private long _reconnectAttemptCount;
|
||||
private string? _lastErrorMessage;
|
||||
private bool _disposed;
|
||||
|
||||
/// <summary>
|
||||
/// 使用默认状态通道参数初始化客户端。
|
||||
/// </summary>
|
||||
public FanucStateClient()
|
||||
: this(new FanucStateClientOptions(), null)
|
||||
{
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 使用指定状态通道参数初始化客户端。
|
||||
/// </summary>
|
||||
/// <param name="options">超时和重连参数。</param>
|
||||
public FanucStateClient(FanucStateClientOptions options)
|
||||
: this(options, null)
|
||||
{
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 使用指定状态通道参数和日志记录器初始化客户端。
|
||||
/// </summary>
|
||||
/// <param name="options">超时和重连参数。</param>
|
||||
/// <param name="logger">日志记录器；允许 null。</param>
|
||||
public FanucStateClient(FanucStateClientOptions options, ILogger<FanucStateClient>? logger)
|
||||
{
|
||||
ArgumentNullException.ThrowIfNull(options);
|
||||
ValidateOptions(options);
|
||||
_options = options;
|
||||
_logger = logger;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 获取当前是否已建立连接。
|
||||
/// </summary>
|
||||
public bool IsConnected => GetStatus().State == FanucStateConnectionState.Connected;
|
||||
|
||||
/// <summary>
|
||||
/// 建立到 FANUC 控制柜 TCP 10010 状态通道的连接并启动后台接收循环。
|
||||
/// </summary>
|
||||
/// <param name="ip">控制柜 IP 地址。</param>
|
||||
/// <param name="port">状态通道端口，默认 10010。</param>
|
||||
/// <param name="cancellationToken">取消令牌。</param>
|
||||
public async Task ConnectAsync(string ip, int port = 10010, CancellationToken cancellationToken = default)
|
||||
{
|
||||
ObjectDisposedException.ThrowIf(_disposed, this);
|
||||
|
||||
if (string.IsNullOrWhiteSpace(ip))
|
||||
{
|
||||
throw new ArgumentException("IP 不能为空。", nameof(ip));
|
||||
}
|
||||
|
||||
if (_receiveTask is not null)
|
||||
{
|
||||
throw new InvalidOperationException("状态通道已经连接，请先 Disconnect。");
|
||||
}
|
||||
|
||||
_logger?.LogInformation("StateClient ConnectAsync: {Ip}:{Port}", ip, port);
|
||||
|
||||
_receiveCts = new CancellationTokenSource();
|
||||
using var linkedCts = CancellationTokenSource.CreateLinkedTokenSource(cancellationToken, _receiveCts.Token);
|
||||
|
||||
lock (_stateLock)
|
||||
{
|
||||
_connectionState = FanucStateConnectionState.Connecting;
|
||||
_latestFrame = null;
|
||||
_lastConnectedAt = null;
|
||||
_lastFrameAt = null;
|
||||
_reconnectAttemptCount = 0;
|
||||
_lastErrorMessage = null;
|
||||
}
|
||||
|
||||
try
|
||||
{
|
||||
await OpenConnectionAsync(ip, port, linkedCts.Token).ConfigureAwait(false);
|
||||
}
|
||||
catch (Exception exception)
|
||||
{
|
||||
_logger?.LogError(exception, "StateClient 连接失败: {Ip}:{Port}", ip, port);
|
||||
CloseCurrentConnection();
|
||||
lock (_stateLock)
|
||||
{
|
||||
_connectionState = FanucStateConnectionState.Disconnected;
|
||||
}
|
||||
|
||||
_receiveCts.Dispose();
|
||||
_receiveCts = null;
|
||||
throw;
|
||||
}
|
||||
|
||||
_receiveTask = Task.Run(
|
||||
() => ReceiveAndReconnectLoopAsync(ip, port, _receiveCts.Token),
|
||||
_receiveCts.Token);
|
||||
|
||||
_logger?.LogInformation("StateClient 已连接并启动接收循环: {Ip}:{Port}", ip, port);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 断开状态通道并停止后台接收循环。
|
||||
/// </summary>
|
||||
public void Disconnect()
|
||||
{
|
||||
ObjectDisposedException.ThrowIf(_disposed, this);
|
||||
|
||||
_logger?.LogInformation("StateClient Disconnect");
|
||||
Shutdown(clearLatestFrame: true);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 获取最近一次解析的状态帧；若尚未收到任何帧则返回 null。
|
||||
/// </summary>
|
||||
/// <returns>最新状态帧或 null。</returns>
|
||||
public FanucStateFrame? GetLatestFrame()
|
||||
{
|
||||
ObjectDisposedException.ThrowIf(_disposed, this);
|
||||
|
||||
lock (_stateLock)
|
||||
{
|
||||
return _latestFrame;
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 获取状态通道当前诊断状态。
|
||||
/// </summary>
|
||||
/// <returns>状态通道诊断快照。</returns>
|
||||
public FanucStateClientStatus GetStatus()
|
||||
{
|
||||
ObjectDisposedException.ThrowIf(_disposed, this);
|
||||
|
||||
lock (_stateLock)
|
||||
{
|
||||
return new FanucStateClientStatus(
|
||||
_connectionState,
|
||||
IsFrameStaleLocked(DateTimeOffset.UtcNow),
|
||||
_lastFrameAt,
|
||||
_reconnectAttemptCount,
|
||||
_lastErrorMessage);
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 释放客户端资源。
|
||||
/// </summary>
|
||||
public void Dispose()
|
||||
{
|
||||
if (_disposed)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
_disposed = true;
|
||||
Shutdown(clearLatestFrame: true);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 后台循环：持续接收状态帧；断线、超时或坏帧后进入退避重连。
|
||||
/// </summary>
|
||||
private async Task ReceiveAndReconnectLoopAsync(string ip, int port, CancellationToken cancellationToken)
|
||||
{
|
||||
var reconnectDelay = _options.ReconnectInitialDelay;
|
||||
_logger?.LogInformation("StateClient 接收循环启动: {Ip}:{Port}", ip, port);
|
||||
|
||||
while (!cancellationToken.IsCancellationRequested)
|
||||
{
|
||||
try
|
||||
{
|
||||
await ReceiveCurrentConnectionAsync(cancellationToken).ConfigureAwait(false);
|
||||
reconnectDelay = _options.ReconnectInitialDelay;
|
||||
}
|
||||
catch (OperationCanceledException) when (cancellationToken.IsCancellationRequested)
|
||||
{
|
||||
_logger?.LogInformation("StateClient 接收循环正常取消");
|
||||
return;
|
||||
}
|
||||
catch (TimeoutException ex)
|
||||
{
|
||||
_logger?.LogWarning(ex, "StateClient 接收超时");
|
||||
MarkReceiveFailure(FanucStateConnectionState.TimedOut, ex.Message);
|
||||
}
|
||||
catch (Exception ex) when (ex is IOException or InvalidDataException or SocketException or ObjectDisposedException)
|
||||
{
|
||||
_logger?.LogWarning(ex, "StateClient 连接异常，准备重连");
|
||||
MarkReceiveFailure(FanucStateConnectionState.Reconnecting, ex.Message);
|
||||
}
|
||||
|
||||
CloseCurrentConnection();
|
||||
|
||||
if (cancellationToken.IsCancellationRequested)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
reconnectDelay = await ReconnectWithBackoffAsync(ip, port, reconnectDelay, cancellationToken).ConfigureAwait(false);
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 从当前连接中持续读取状态帧，直到连接异常或被取消。
|
||||
/// </summary>
|
||||
private async Task ReceiveCurrentConnectionAsync(CancellationToken cancellationToken)
|
||||
{
|
||||
NetworkStream stream;
|
||||
lock (_stateLock)
|
||||
{
|
||||
stream = _stream ?? throw new IOException("状态通道未连接。");
|
||||
}
|
||||
|
||||
var buffer = new byte[FanucStateProtocol.StateFrameLength];
|
||||
long frameCount = 0;
|
||||
FanucStateFrame? lastLoggedFrame = null;
|
||||
|
||||
while (!cancellationToken.IsCancellationRequested)
|
||||
{
|
||||
await ReadExactAsync(stream, buffer, cancellationToken).ConfigureAwait(false);
|
||||
|
||||
var frame = FanucStateProtocol.ParseFrame(buffer);
|
||||
lock (_stateLock)
|
||||
{
|
||||
_latestFrame = frame;
|
||||
_lastFrameAt = DateTimeOffset.UtcNow;
|
||||
_connectionState = FanucStateConnectionState.Connected;
|
||||
_lastErrorMessage = null;
|
||||
}
|
||||
|
||||
frameCount++;
|
||||
|
||||
// 仅在状态变化或首次接收时记录 Info，避免高频日志。
|
||||
if (lastLoggedFrame is null
|
||||
|| lastLoggedFrame.CartesianPose[0] != frame.CartesianPose[0]
|
||||
|| !lastLoggedFrame.RawTailWords.SequenceEqual(frame.RawTailWords))
|
||||
{
|
||||
_logger?.LogInformation(
|
||||
"StateClient 收到状态帧: pose=[{X:F1}, {Y:F1}, {Z:F1}], tail=[{Tail}]",
|
||||
frame.CartesianPose[0],
|
||||
frame.CartesianPose[1],
|
||||
frame.CartesianPose[2],
|
||||
string.Join(", ", frame.RawTailWords));
|
||||
lastLoggedFrame = frame;
|
||||
}
|
||||
else if (frameCount % 1000 == 0)
|
||||
{
|
||||
_logger?.LogDebug("StateClient 已接收 {Count} 个状态帧", frameCount);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 从流中精确读取固定长度字节，超过帧超时窗口则抛出超时异常。
|
||||
/// </summary>
|
||||
private async Task ReadExactAsync(NetworkStream stream, byte[] buffer, CancellationToken cancellationToken)
|
||||
{
|
||||
using var timeoutCts = CancellationTokenSource.CreateLinkedTokenSource(cancellationToken);
|
||||
timeoutCts.CancelAfter(_options.FrameTimeout);
|
||||
|
||||
var totalRead = 0;
|
||||
try
|
||||
{
|
||||
while (totalRead < buffer.Length)
|
||||
{
|
||||
var read = await stream.ReadAsync(
|
||||
buffer.AsMemory(totalRead, buffer.Length - totalRead),
|
||||
timeoutCts.Token).ConfigureAwait(false);
|
||||
|
||||
if (read == 0)
|
||||
{
|
||||
throw new IOException("状态通道已断开，读取到 EOF。");
|
||||
}
|
||||
|
||||
totalRead += read;
|
||||
}
|
||||
}
|
||||
catch (OperationCanceledException) when (!cancellationToken.IsCancellationRequested)
|
||||
{
|
||||
throw new TimeoutException("状态通道接收超时，未在限定时间内收到完整 90B 状态帧。");
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 打开 TCP 状态通道并更新连接状态。
|
||||
/// </summary>
|
||||
private async Task OpenConnectionAsync(string ip, int port, CancellationToken cancellationToken)
|
||||
{
|
||||
var tcpClient = new TcpClient { NoDelay = true };
|
||||
try
|
||||
{
|
||||
_logger?.LogInformation("StateClient 正在连接 {Ip}:{Port}...", ip, port);
|
||||
|
||||
using var timeoutCts = CancellationTokenSource.CreateLinkedTokenSource(cancellationToken);
|
||||
timeoutCts.CancelAfter(_options.ConnectTimeout);
|
||||
await tcpClient.ConnectAsync(ip, port, timeoutCts.Token).ConfigureAwait(false);
|
||||
|
||||
lock (_stateLock)
|
||||
{
|
||||
_tcpClient = tcpClient;
|
||||
_stream = tcpClient.GetStream();
|
||||
_lastConnectedAt = DateTimeOffset.UtcNow;
|
||||
_connectionState = FanucStateConnectionState.Connected;
|
||||
}
|
||||
|
||||
_logger?.LogInformation("StateClient 已连接到 {Ip}:{Port}", ip, port);
|
||||
}
|
||||
catch (OperationCanceledException) when (!cancellationToken.IsCancellationRequested)
|
||||
{
|
||||
_logger?.LogWarning("StateClient 连接 {Ip}:{Port} 超时", ip, port);
|
||||
tcpClient.Dispose();
|
||||
throw new TimeoutException("状态通道建连超时。");
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
_logger?.LogWarning(ex, "StateClient 连接 {Ip}:{Port} 失败", ip, port);
|
||||
tcpClient.Dispose();
|
||||
throw;
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 按退避策略循环尝试重新连接，并返回下一次异常后的退避时间。
|
||||
/// </summary>
|
||||
private async Task<TimeSpan> ReconnectWithBackoffAsync(
|
||||
string ip,
|
||||
int port,
|
||||
TimeSpan reconnectDelay,
|
||||
CancellationToken cancellationToken)
|
||||
{
|
||||
var nextDelay = reconnectDelay;
|
||||
|
||||
while (!cancellationToken.IsCancellationRequested)
|
||||
{
|
||||
lock (_stateLock)
|
||||
{
|
||||
_connectionState = FanucStateConnectionState.Reconnecting;
|
||||
}
|
||||
|
||||
_logger?.LogInformation(
|
||||
"StateClient 将在 {Delay}ms 后尝试重连 {Ip}:{Port}...",
|
||||
nextDelay.TotalMilliseconds,
|
||||
ip,
|
||||
port);
|
||||
|
||||
await Task.Delay(nextDelay, cancellationToken).ConfigureAwait(false);
|
||||
|
||||
lock (_stateLock)
|
||||
{
|
||||
_reconnectAttemptCount++;
|
||||
}
|
||||
|
||||
try
|
||||
{
|
||||
await OpenConnectionAsync(ip, port, cancellationToken).ConfigureAwait(false);
|
||||
_logger?.LogInformation(
|
||||
"StateClient 重连成功: {Ip}:{Port}, 累计重连次数={Count}",
|
||||
ip,
|
||||
port,
|
||||
_reconnectAttemptCount);
|
||||
return _options.ReconnectInitialDelay;
|
||||
}
|
||||
catch (OperationCanceledException) when (cancellationToken.IsCancellationRequested)
|
||||
{
|
||||
throw;
|
||||
}
|
||||
catch (Exception ex) when (ex is SocketException or IOException or TimeoutException)
|
||||
{
|
||||
CloseCurrentConnection();
|
||||
lock (_stateLock)
|
||||
{
|
||||
_connectionState = FanucStateConnectionState.Reconnecting;
|
||||
_lastErrorMessage = ex.Message;
|
||||
}
|
||||
|
||||
_logger?.LogWarning(
|
||||
ex,
|
||||
"StateClient 重连失败: {Ip}:{Port}, 下次等待={NextDelay}ms",
|
||||
ip,
|
||||
port,
|
||||
nextDelay.TotalMilliseconds * 2);
|
||||
|
||||
nextDelay = IncreaseReconnectDelay(nextDelay);
|
||||
}
|
||||
}
|
||||
|
||||
return nextDelay;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 关闭当前 TCP 连接，不清除最新状态帧，供重连路径保留诊断数据。
|
||||
/// </summary>
|
||||
private void CloseCurrentConnection()
|
||||
{
|
||||
NetworkStream? stream;
|
||||
TcpClient? tcpClient;
|
||||
lock (_stateLock)
|
||||
{
|
||||
stream = _stream;
|
||||
tcpClient = _tcpClient;
|
||||
_stream = null;
|
||||
_tcpClient = null;
|
||||
}
|
||||
|
||||
stream?.Dispose();
|
||||
tcpClient?.Dispose();
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 记录接收异常并更新状态通道连接阶段。
|
||||
/// </summary>
|
||||
private void MarkReceiveFailure(FanucStateConnectionState state, string message)
|
||||
{
|
||||
lock (_stateLock)
|
||||
{
|
||||
_connectionState = state;
|
||||
_lastErrorMessage = message;
|
||||
}
|
||||
|
||||
_logger?.LogWarning("StateClient 接收失败: state={State}, message={Message}", state, message);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 关闭后台循环和 socket 资源。
|
||||
/// </summary>
|
||||
private void Shutdown(bool clearLatestFrame)
|
||||
{
|
||||
_receiveCts?.Cancel();
|
||||
CloseCurrentConnection();
|
||||
|
||||
try
|
||||
{
|
||||
_receiveTask?.Wait(TimeSpan.FromSeconds(2));
|
||||
}
|
||||
catch (AggregateException)
|
||||
{
|
||||
// 后台循环可能因取消而抛出 OperationCanceledException，忽略即可。
|
||||
}
|
||||
|
||||
_receiveTask = null;
|
||||
_receiveCts?.Dispose();
|
||||
_receiveCts = null;
|
||||
|
||||
lock (_stateLock)
|
||||
{
|
||||
_connectionState = FanucStateConnectionState.Disconnected;
|
||||
_lastConnectedAt = null;
|
||||
_lastErrorMessage = null;
|
||||
_reconnectAttemptCount = 0;
|
||||
if (clearLatestFrame)
|
||||
{
|
||||
_latestFrame = null;
|
||||
_lastFrameAt = null;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 判断缓存帧是否已经不能代表当前控制柜状态。
|
||||
/// </summary>
|
||||
private bool IsFrameStaleLocked(DateTimeOffset now)
|
||||
{
|
||||
if (_latestFrame is null)
|
||||
{
|
||||
return _connectionState is FanucStateConnectionState.TimedOut or FanucStateConnectionState.Reconnecting
|
||||
|| _reconnectAttemptCount > 0
|
||||
|| (_lastConnectedAt.HasValue && now - _lastConnectedAt.Value > _options.FrameTimeout);
|
||||
}
|
||||
|
||||
return _lastFrameAt.HasValue && now - _lastFrameAt.Value > _options.FrameTimeout;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 计算下一轮重连等待时间。
|
||||
/// </summary>
|
||||
private TimeSpan IncreaseReconnectDelay(TimeSpan currentDelay)
|
||||
{
|
||||
var doubledMilliseconds = Math.Max(currentDelay.TotalMilliseconds * 2.0, _options.ReconnectInitialDelay.TotalMilliseconds);
|
||||
var cappedMilliseconds = Math.Min(doubledMilliseconds, _options.ReconnectMaxDelay.TotalMilliseconds);
|
||||
return TimeSpan.FromMilliseconds(cappedMilliseconds);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 校验状态通道参数，避免后台循环使用无效时间窗口。
|
||||
/// </summary>
|
||||
private static void ValidateOptions(FanucStateClientOptions options)
|
||||
{
|
||||
ValidatePositive(options.FrameTimeout, nameof(options.FrameTimeout));
|
||||
ValidatePositive(options.ReconnectInitialDelay, nameof(options.ReconnectInitialDelay));
|
||||
ValidatePositive(options.ReconnectMaxDelay, nameof(options.ReconnectMaxDelay));
|
||||
ValidatePositive(options.ConnectTimeout, nameof(options.ConnectTimeout));
|
||||
|
||||
if (options.ReconnectMaxDelay < options.ReconnectInitialDelay)
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(nameof(options), "最大重连等待时间不能小于初始重连等待时间。");
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 校验时间参数必须为正值。
|
||||
/// </summary>
|
||||
private static void ValidatePositive(TimeSpan value, string parameterName)
|
||||
{
|
||||
if (value <= TimeSpan.Zero)
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(parameterName, "时间参数必须大于 0。");
|
||||
}
|
||||
}
|
||||
}
|
||||
187
src/Flyshot.Runtime.Fanuc/Protocol/FanucStateProtocol.cs
Normal file
187
src/Flyshot.Runtime.Fanuc/Protocol/FanucStateProtocol.cs
Normal file
@@ -0,0 +1,187 @@
|
||||
using System.Buffers.Binary;
|
||||
|
||||
namespace Flyshot.Runtime.Fanuc.Protocol;
|
||||
|
||||
/// <summary>
|
||||
/// 表示 FANUC TCP 10010 状态通道中的单个状态帧。
|
||||
/// </summary>
|
||||
public sealed class FanucStateFrame
|
||||
{
|
||||
private readonly double[] _pose;
|
||||
private readonly double[] _jointOrExtensionValues;
|
||||
private readonly double[] _jointRadians;
|
||||
private readonly double[] _externalAxes;
|
||||
private readonly uint[] _tailWords;
|
||||
|
||||
/// <summary>
|
||||
/// 初始化状态帧解析结果。
|
||||
/// </summary>
|
||||
/// <param name="messageId">状态帧消息号或序号。</param>
|
||||
/// <param name="pose">控制器回传的笛卡尔位姿。</param>
|
||||
/// <param name="jointOrExtensionValues">控制器回传的关节或扩展轴状态。</param>
|
||||
/// <param name="tailWords">状态帧尾部状态槽位。</param>
|
||||
public FanucStateFrame(
|
||||
uint messageId,
|
||||
IEnumerable<double> pose,
|
||||
IEnumerable<double> jointOrExtensionValues,
|
||||
IEnumerable<uint> tailWords)
|
||||
{
|
||||
MessageId = messageId;
|
||||
_pose = pose?.ToArray() ?? throw new ArgumentNullException(nameof(pose));
|
||||
_jointOrExtensionValues = jointOrExtensionValues?.ToArray() ?? throw new ArgumentNullException(nameof(jointOrExtensionValues));
|
||||
_tailWords = tailWords?.ToArray() ?? throw new ArgumentNullException(nameof(tailWords));
|
||||
|
||||
if (_pose.Length != 6)
|
||||
{
|
||||
throw new ArgumentException("状态帧位姿必须包含 6 个 float。", nameof(pose));
|
||||
}
|
||||
|
||||
if (_jointOrExtensionValues.Length != 9)
|
||||
{
|
||||
throw new ArgumentException("状态帧关节/扩展轴必须包含 9 个 float。", nameof(jointOrExtensionValues));
|
||||
}
|
||||
|
||||
if (_tailWords.Length != 4)
|
||||
{
|
||||
throw new ArgumentException("状态帧尾部状态字必须包含 4 个 u32。", nameof(tailWords));
|
||||
}
|
||||
|
||||
_jointRadians = _jointOrExtensionValues.Take(6).ToArray();
|
||||
_externalAxes = _jointOrExtensionValues.Skip(6).ToArray();
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 获取状态帧消息号或序号。
|
||||
/// </summary>
|
||||
public uint MessageId { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取控制器回传的笛卡尔位姿。
|
||||
/// </summary>
|
||||
public IReadOnlyList<double> Pose => _pose;
|
||||
|
||||
/// <summary>
|
||||
/// 获取控制器回传的笛卡尔位姿 X/Y/Z/W/P/R，单位来自 FANUC 状态服务器。
|
||||
/// </summary>
|
||||
public IReadOnlyList<double> CartesianPose => _pose;
|
||||
|
||||
/// <summary>
|
||||
/// 获取控制器回传的关节或扩展轴状态。
|
||||
/// </summary>
|
||||
public IReadOnlyList<double> JointOrExtensionValues => _jointOrExtensionValues;
|
||||
|
||||
/// <summary>
|
||||
/// 获取前 6 个机器人关节角度，当前现场抓包更支持按弧度制理解。
|
||||
/// </summary>
|
||||
public IReadOnlyList<double> JointRadians => _jointRadians;
|
||||
|
||||
/// <summary>
|
||||
/// 获取后 3 个扩展轴槽位。当前现场样本中这些值通常为 0。
|
||||
/// </summary>
|
||||
public IReadOnlyList<double> ExternalAxes => _externalAxes;
|
||||
|
||||
/// <summary>
|
||||
/// 获取状态帧尾部状态槽位。
|
||||
/// </summary>
|
||||
public IReadOnlyList<uint> TailWords => _tailWords;
|
||||
|
||||
/// <summary>
|
||||
/// 获取原始尾部状态字。当前抓包中恒为 [2,0,0,1]，语义暂不强行推断。
|
||||
/// </summary>
|
||||
public IReadOnlyList<uint> RawTailWords => _tailWords;
|
||||
|
||||
/// <summary>
|
||||
/// 获取第 0 个原始尾部状态字。
|
||||
/// </summary>
|
||||
public uint StatusWord0 => _tailWords[0];
|
||||
|
||||
/// <summary>
|
||||
/// 获取第 1 个原始尾部状态字。
|
||||
/// </summary>
|
||||
public uint StatusWord1 => _tailWords[1];
|
||||
|
||||
/// <summary>
|
||||
/// 获取第 2 个原始尾部状态字。
|
||||
/// </summary>
|
||||
public uint StatusWord2 => _tailWords[2];
|
||||
|
||||
/// <summary>
|
||||
/// 获取第 3 个原始尾部状态字。
|
||||
/// </summary>
|
||||
public uint StatusWord3 => _tailWords[3];
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 提供 FANUC TCP 10010 状态通道固定帧解析能力。
|
||||
/// </summary>
|
||||
public static class FanucStateProtocol
|
||||
{
|
||||
/// <summary>
|
||||
/// FANUC 状态通道抓包确认的完整帧长度。
|
||||
/// </summary>
|
||||
public const int StateFrameLength = 90;
|
||||
|
||||
/// <summary>
|
||||
/// 解析 TCP 10010 状态通道中的单个完整状态帧。
|
||||
/// </summary>
|
||||
/// <param name="frame">完整状态帧。</param>
|
||||
/// <returns>状态帧解析结果。</returns>
|
||||
public static FanucStateFrame ParseFrame(ReadOnlySpan<byte> frame)
|
||||
{
|
||||
ValidateFrame(frame);
|
||||
|
||||
var pose = new double[6];
|
||||
var jointOrExtensionValues = new double[9];
|
||||
var tailWords = new uint[4];
|
||||
|
||||
// 状态帧采用固定布局，偏移来自抓包与 StateServer 逆向结论。
|
||||
for (var index = 0; index < pose.Length; index++)
|
||||
{
|
||||
pose[index] = BinaryPrimitives.ReadSingleBigEndian(frame.Slice(11 + (index * sizeof(float)), sizeof(float)));
|
||||
}
|
||||
|
||||
for (var index = 0; index < jointOrExtensionValues.Length; index++)
|
||||
{
|
||||
jointOrExtensionValues[index] = BinaryPrimitives.ReadSingleBigEndian(frame.Slice(35 + (index * sizeof(float)), sizeof(float)));
|
||||
}
|
||||
|
||||
for (var index = 0; index < tailWords.Length; index++)
|
||||
{
|
||||
tailWords[index] = BinaryPrimitives.ReadUInt32BigEndian(frame.Slice(71 + (index * sizeof(uint)), sizeof(uint)));
|
||||
}
|
||||
|
||||
return new FanucStateFrame(
|
||||
BinaryPrimitives.ReadUInt32BigEndian(frame.Slice(7, sizeof(uint))),
|
||||
pose,
|
||||
jointOrExtensionValues,
|
||||
tailWords);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 校验状态帧的长度、magic 和长度字段。
|
||||
/// </summary>
|
||||
/// <param name="frame">完整状态帧。</param>
|
||||
private static void ValidateFrame(ReadOnlySpan<byte> frame)
|
||||
{
|
||||
if (frame.Length != StateFrameLength)
|
||||
{
|
||||
throw new InvalidDataException("FANUC 状态帧长度不符合 TCP 10010 固定帧布局。");
|
||||
}
|
||||
|
||||
if (frame[0] != (byte)'d' || frame[1] != (byte)'o' || frame[2] != (byte)'z')
|
||||
{
|
||||
throw new InvalidDataException("FANUC 状态帧头 magic 不正确。");
|
||||
}
|
||||
|
||||
if (frame[^3] != (byte)'z' || frame[^2] != (byte)'o' || frame[^1] != (byte)'d')
|
||||
{
|
||||
throw new InvalidDataException("FANUC 状态帧尾 magic 不正确。");
|
||||
}
|
||||
|
||||
var declaredLength = BinaryPrimitives.ReadUInt32BigEndian(frame.Slice(3, sizeof(uint)));
|
||||
if (declaredLength != frame.Length)
|
||||
{
|
||||
throw new InvalidDataException("FANUC 状态帧长度字段与实际长度不一致。");
|
||||
}
|
||||
}
|
||||
}
|
||||
13
src/Flyshot.Server.Host/.config/dotnet-tools.json
Normal file
13
src/Flyshot.Server.Host/.config/dotnet-tools.json
Normal file
@@ -0,0 +1,13 @@
|
||||
{
|
||||
"version": 1,
|
||||
"isRoot": true,
|
||||
"tools": {
|
||||
"dotnet-ef": {
|
||||
"version": "10.0.7",
|
||||
"commands": [
|
||||
"dotnet-ef"
|
||||
],
|
||||
"rollForward": false
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,64 @@
|
||||
using Flyshot.Server.Host;
|
||||
using Microsoft.AspNetCore.Mvc;
|
||||
using Microsoft.Extensions.Options;
|
||||
|
||||
namespace Flyshot.Server.Host.Controllers;
|
||||
|
||||
/// <summary>
|
||||
/// 提供浏览器调试页所需的运行时配置 API。
|
||||
/// </summary>
|
||||
/// <remarks>
|
||||
/// 本控制器自身不进入 Swagger 文档（<see cref="ApiExplorerSettingsAttribute.IgnoreApi"/>）。
|
||||
/// 调试页静态资源位于 wwwroot，Swagger 地址由配置 API 下发。
|
||||
/// </remarks>
|
||||
[ApiController]
|
||||
[ApiExplorerSettings(IgnoreApi = true)]
|
||||
[Tags("基础与状态")]
|
||||
public sealed class DebugConsoleController : ControllerBase
|
||||
{
|
||||
/// <summary>
|
||||
/// Swagger 配置项，用于决定调试页是否对外暴露以及拼接 OpenAPI JSON 地址。
|
||||
/// </summary>
|
||||
private readonly HostSwaggerOptions _swaggerOptions;
|
||||
|
||||
/// <summary>
|
||||
/// 初始化在线调试页控制器。
|
||||
/// </summary>
|
||||
/// <param name="swaggerOptions">来自 <c>Swagger</c> 配置节的标准选项。</param>
|
||||
public DebugConsoleController(IOptions<HostSwaggerOptions> swaggerOptions)
|
||||
{
|
||||
ArgumentNullException.ThrowIfNull(swaggerOptions);
|
||||
_swaggerOptions = swaggerOptions.Value ?? new HostSwaggerOptions();
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 返回静态调试页启动时所需的 Swagger 文档地址。
|
||||
/// </summary>
|
||||
/// <returns>当 Swagger 启用时返回配置；否则返回 404，与 Swagger UI 保持一致的可见性策略。</returns>
|
||||
[HttpGet("/api/debug/config")]
|
||||
public IActionResult GetDebugConfig()
|
||||
{
|
||||
if (!_swaggerOptions.Enabled)
|
||||
{
|
||||
return NotFound();
|
||||
}
|
||||
|
||||
return Ok(new
|
||||
{
|
||||
SwaggerJsonUrl = ResolveSwaggerJsonUrl(_swaggerOptions)
|
||||
});
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 根据 <see cref="HostSwaggerOptions.JsonRouteTemplate"/> 与 <see cref="HostSwaggerOptions.DocumentName"/> 解析出 Swagger JSON 实际地址。
|
||||
/// </summary>
|
||||
/// <param name="options">Swagger 配置选项。</param>
|
||||
/// <returns>形如 <c>/swagger/v1/swagger.json</c> 的绝对路径。</returns>
|
||||
private static string ResolveSwaggerJsonUrl(HostSwaggerOptions options)
|
||||
{
|
||||
// Swashbuckle 的 RouteTemplate 不带前导斜杠，这里统一加上保证前端 fetch 走绝对路径。
|
||||
var template = options.JsonRouteTemplate ?? "swagger/{documentName}/swagger.json";
|
||||
var path = template.Replace("{documentName}", options.DocumentName ?? "v1", StringComparison.Ordinal);
|
||||
return path.StartsWith('/') ? path : "/" + path;
|
||||
}
|
||||
}
|
||||
25
src/Flyshot.Server.Host/Controllers/HealthController.cs
Normal file
25
src/Flyshot.Server.Host/Controllers/HealthController.cs
Normal file
@@ -0,0 +1,25 @@
|
||||
using Microsoft.AspNetCore.Mvc;
|
||||
|
||||
namespace Flyshot.Server.Host.Controllers;
|
||||
|
||||
/// <summary>
|
||||
/// 提供宿主基础探活与诊断接口。
|
||||
/// </summary>
|
||||
[ApiController]
|
||||
[Tags("基础与状态")]
|
||||
public sealed class HealthController : ControllerBase
|
||||
{
|
||||
/// <summary>
|
||||
/// 返回宿主健康状态。
|
||||
/// </summary>
|
||||
/// <returns>固定的健康检查 JSON。</returns>
|
||||
[HttpGet("/healthz")]
|
||||
public IActionResult GetHealth()
|
||||
{
|
||||
return Ok(new
|
||||
{
|
||||
status = "ok",
|
||||
service = "flyshot-server-host"
|
||||
});
|
||||
}
|
||||
}
|
||||
1204
src/Flyshot.Server.Host/Controllers/LegacyHttpApiController.cs
Normal file
1204
src/Flyshot.Server.Host/Controllers/LegacyHttpApiController.cs
Normal file
File diff suppressed because it is too large
Load Diff
67
src/Flyshot.Server.Host/Controllers/StatusController.cs
Normal file
67
src/Flyshot.Server.Host/Controllers/StatusController.cs
Normal file
@@ -0,0 +1,67 @@
|
||||
using Flyshot.ControllerClientCompat;
|
||||
using Microsoft.AspNetCore.Mvc;
|
||||
|
||||
namespace Flyshot.Server.Host.Controllers;
|
||||
|
||||
/// <summary>
|
||||
/// 提供控制器状态快照 API，状态监控页面由 wwwroot 静态资源承载。
|
||||
/// </summary>
|
||||
[ApiController]
|
||||
[Tags("基础与状态")]
|
||||
public sealed class StatusController : ControllerBase
|
||||
{
|
||||
private readonly IControllerClientCompatService _compatService;
|
||||
|
||||
/// <summary>
|
||||
/// 初始化状态监控控制器。
|
||||
/// </summary>
|
||||
/// <param name="compatService">ControllerClient 兼容层服务。</param>
|
||||
public StatusController(IControllerClientCompatService compatService)
|
||||
{
|
||||
_compatService = compatService ?? throw new ArgumentNullException(nameof(compatService));
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 提供短路由 `/status`，跳转到静态状态页。
|
||||
/// </summary>
|
||||
/// <returns>重定向到 <c>/status.html</c>。</returns>
|
||||
[HttpGet("/status")]
|
||||
public IActionResult StatusPage()
|
||||
{
|
||||
return Redirect("/status.html");
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 提供短路由 `/debug`，跳转到静态调试页。
|
||||
/// </summary>
|
||||
/// <returns>重定向到 <c>/debug.html</c>。</returns>
|
||||
[HttpGet("/debug")]
|
||||
public IActionResult DebugPage()
|
||||
{
|
||||
return Redirect("/debug.html");
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 返回当前 ControllerClient 兼容层与控制器运行时状态快照。
|
||||
/// </summary>
|
||||
/// <returns>面向状态页和外部诊断的 JSON 快照。</returns>
|
||||
[HttpGet("/api/status/snapshot")]
|
||||
public IActionResult GetSnapshot()
|
||||
{
|
||||
var snapshot = _compatService.GetControllerSnapshot();
|
||||
var metadata = _compatService.GetStatusSnapshotMetadata();
|
||||
|
||||
return Ok(new
|
||||
{
|
||||
Status = "ok",
|
||||
Service = "flyshot-server-host",
|
||||
ServerVersion = metadata.ServerVersion,
|
||||
ClientVersion = metadata.ClientVersion,
|
||||
IsSetup = metadata.IsSetup,
|
||||
RobotName = metadata.RobotName,
|
||||
DegreesOfFreedom = metadata.DegreesOfFreedom,
|
||||
UploadedTrajectories = metadata.UploadedTrajectories,
|
||||
Snapshot = snapshot
|
||||
});
|
||||
}
|
||||
}
|
||||
@@ -1,6 +1,31 @@
|
||||
<Project Sdk="Microsoft.NET.Sdk.Web">
|
||||
<PropertyGroup>
|
||||
<!-- 生成 XML 文档以便 Swashbuckle 把控制器/DTO 上的 /// summary 注释注入 OpenAPI 文档，
|
||||
调试页和 Swagger UI 的端点标题都依赖这一份文档。1591 抑制掉 “缺失 XML 注释” 的噪音。 -->
|
||||
<GenerateDocumentationFile>true</GenerateDocumentationFile>
|
||||
<NoWarn>$(NoWarn);1591</NoWarn>
|
||||
</PropertyGroup>
|
||||
|
||||
<ItemGroup>
|
||||
<PackageReference Include="NLog.Web.AspNetCore" Version="5.3.11" />
|
||||
<PackageReference Include="Swashbuckle.AspNetCore" Version="6.6.2" />
|
||||
</ItemGroup>
|
||||
|
||||
<ItemGroup>
|
||||
<!-- 运行时需要把仓库根目录的 NLog.config 带到 Host 输出目录，确保控制台和文件日志目标生效。 -->
|
||||
<Content Include="..\..\NLog.config" Link="NLog.config" CopyToOutputDirectory="PreserveNewest" />
|
||||
<!-- 运行时配置根目录固定为输出目录 Config，调试和发布都复制仓库内固化配置。 -->
|
||||
<Content Include="..\..\Config\**\*" Link="Config\%(RecursiveDir)%(Filename)%(Extension)" CopyToOutputDirectory="PreserveNewest" CopyToPublishDirectory="PreserveNewest" />
|
||||
</ItemGroup>
|
||||
|
||||
<ItemGroup>
|
||||
<!-- 让静态调试页在构建和发布时都物理复制到输出目录，避免运行时只依赖源码树中的 wwwroot。 -->
|
||||
<Content Update="wwwroot\**\*" CopyToOutputDirectory="PreserveNewest" CopyToPublishDirectory="PreserveNewest" />
|
||||
</ItemGroup>
|
||||
|
||||
<ItemGroup>
|
||||
<ProjectReference Include="..\Flyshot.Core.Domain\Flyshot.Core.Domain.csproj" />
|
||||
<ProjectReference Include="..\Flyshot.ControllerClientCompat\Flyshot.ControllerClientCompat.csproj" />
|
||||
<ProjectReference Include="..\Flyshot.Runtime.Common\Flyshot.Runtime.Common.csproj" />
|
||||
</ItemGroup>
|
||||
|
||||
|
||||
27
src/Flyshot.Server.Host/HostCorsOptions.cs
Normal file
27
src/Flyshot.Server.Host/HostCorsOptions.cs
Normal file
@@ -0,0 +1,27 @@
|
||||
namespace Flyshot.Server.Host;
|
||||
|
||||
/// <summary>
|
||||
/// 表示宿主 CORS 策略的标准配置项。
|
||||
/// </summary>
|
||||
public sealed class HostCorsOptions
|
||||
{
|
||||
/// <summary>
|
||||
/// 获取或设置 CORS 策略名称。
|
||||
/// </summary>
|
||||
public string PolicyName { get; set; } = "LegacyHttpApi";
|
||||
|
||||
/// <summary>
|
||||
/// 获取或设置允许的源列表。
|
||||
/// </summary>
|
||||
public string[] AllowedOrigins { get; set; } = ["*"];
|
||||
|
||||
/// <summary>
|
||||
/// 获取或设置允许的 HTTP 方法列表。
|
||||
/// </summary>
|
||||
public string[] AllowedMethods { get; set; } = ["GET", "POST", "OPTIONS"];
|
||||
|
||||
/// <summary>
|
||||
/// 获取或设置允许的请求头列表。
|
||||
/// </summary>
|
||||
public string[] AllowedHeaders { get; set; } = ["*"];
|
||||
}
|
||||
37
src/Flyshot.Server.Host/HostSwaggerOptions.cs
Normal file
37
src/Flyshot.Server.Host/HostSwaggerOptions.cs
Normal file
@@ -0,0 +1,37 @@
|
||||
namespace Flyshot.Server.Host;
|
||||
|
||||
/// <summary>
|
||||
/// 表示宿主 Swagger/OpenAPI 文档的标准配置项。
|
||||
/// </summary>
|
||||
public sealed class HostSwaggerOptions
|
||||
{
|
||||
/// <summary>
|
||||
/// 获取或设置是否启用 Swagger。
|
||||
/// </summary>
|
||||
public bool Enabled { get; set; } = true;
|
||||
|
||||
/// <summary>
|
||||
/// 获取或设置文档名称。
|
||||
/// </summary>
|
||||
public string DocumentName { get; set; } = "v1";
|
||||
|
||||
/// <summary>
|
||||
/// 获取或设置 Swagger 文档标题。
|
||||
/// </summary>
|
||||
public string Title { get; set; } = "Flyshot Replacement HTTP API";
|
||||
|
||||
/// <summary>
|
||||
/// 获取或设置 Swagger 文档版本文本。
|
||||
/// </summary>
|
||||
public string Version { get; set; } = "v1";
|
||||
|
||||
/// <summary>
|
||||
/// 获取或设置 Swagger JSON 路由模板。
|
||||
/// </summary>
|
||||
public string JsonRouteTemplate { get; set; } = "swagger/{documentName}/swagger.json";
|
||||
|
||||
/// <summary>
|
||||
/// 获取或设置 Swagger UI 路由前缀。
|
||||
/// </summary>
|
||||
public string RoutePrefix { get; set; } = "swagger";
|
||||
}
|
||||
@@ -0,0 +1,172 @@
|
||||
using System.Diagnostics;
|
||||
using System.Text;
|
||||
|
||||
namespace Flyshot.Server.Host.Middleware;
|
||||
|
||||
/// <summary>
|
||||
/// HTTP 请求与响应日志中间件。
|
||||
/// 记录每个 HTTP 请求的进入时间、方法、路径、查询串、请求体，
|
||||
/// 以及响应的状态码、耗时和响应体（调试级别）。
|
||||
/// </summary>
|
||||
public sealed class RequestResponseLoggingMiddleware
|
||||
{
|
||||
private readonly RequestDelegate _next;
|
||||
private readonly ILogger<RequestResponseLoggingMiddleware> _logger;
|
||||
|
||||
/// <summary>
|
||||
/// 请求体最大日志长度，超出则截断并附加省略标记。
|
||||
/// </summary>
|
||||
private const int MaxBodyLogLength = 4096;
|
||||
|
||||
/// <summary>
|
||||
/// 请求/响应日志忽略路径前缀列表，用于跳过高频轮询接口的常规日志。
|
||||
/// </summary>
|
||||
private static readonly string[] IgnoredLogPathPrefixes =
|
||||
[
|
||||
"/api/status/snapshot"
|
||||
];
|
||||
|
||||
/// <summary>
|
||||
/// 初始化请求响应日志中间件。
|
||||
/// </summary>
|
||||
/// <param name="next">下一个中间件委托。</param>
|
||||
/// <param name="logger">日志记录器。</param>
|
||||
public RequestResponseLoggingMiddleware(RequestDelegate next, ILogger<RequestResponseLoggingMiddleware> logger)
|
||||
{
|
||||
_next = next ?? throw new ArgumentNullException(nameof(next));
|
||||
_logger = logger ?? throw new ArgumentNullException(nameof(logger));
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 处理 HTTP 请求并记录输入输出。
|
||||
/// </summary>
|
||||
/// <param name="context">HTTP 上下文。</param>
|
||||
public async Task InvokeAsync(HttpContext context)
|
||||
{
|
||||
// 高频状态轮询接口只转发请求，不记录请求/响应日志，避免控制台和文件日志被刷屏。
|
||||
if (ShouldSkipRequestResponseLog(context.Request.Path))
|
||||
{
|
||||
await _next(context).ConfigureAwait(false);
|
||||
return;
|
||||
}
|
||||
|
||||
var stopwatch = Stopwatch.StartNew();
|
||||
var request = context.Request;
|
||||
var requestId = Activity.Current?.Id ?? context.TraceIdentifier;
|
||||
|
||||
// 记录请求进入信息（Info 级别：方法、路径、查询参数）。
|
||||
_logger.LogInformation(
|
||||
"[HTTP-REQ] [{RequestId}] {Method} {Path}{QueryString} — 客户端 {RemoteIp}",
|
||||
requestId,
|
||||
request.Method,
|
||||
request.Path,
|
||||
request.QueryString.HasValue ? request.QueryString.Value : string.Empty,
|
||||
context.Connection.RemoteIpAddress);
|
||||
|
||||
// 读取并记录请求体（Debug 级别）。
|
||||
string? requestBody = null;
|
||||
if (request.ContentLength > 0 && request.Body.CanRead)
|
||||
{
|
||||
request.EnableBuffering();
|
||||
requestBody = await ReadBodyAsync(request.Body, context.RequestAborted).ConfigureAwait(false);
|
||||
request.Body.Position = 0;
|
||||
|
||||
if (!string.IsNullOrEmpty(requestBody))
|
||||
{
|
||||
_logger.LogDebug(
|
||||
"[HTTP-REQ-BODY] [{RequestId}] {Body}",
|
||||
requestId,
|
||||
TruncateBody(requestBody));
|
||||
}
|
||||
}
|
||||
|
||||
// 拦截响应流以便读取响应体。
|
||||
var originalResponseBody = context.Response.Body;
|
||||
using var responseBodyStream = new MemoryStream();
|
||||
context.Response.Body = responseBodyStream;
|
||||
|
||||
try
|
||||
{
|
||||
await _next(context).ConfigureAwait(false);
|
||||
}
|
||||
catch (Exception exception)
|
||||
{
|
||||
_logger.LogError(
|
||||
exception,
|
||||
"[HTTP-ERR] [{RequestId}] {Method} {Path} 处理过程中发生未捕获异常",
|
||||
requestId,
|
||||
request.Method,
|
||||
request.Path);
|
||||
throw;
|
||||
}
|
||||
finally
|
||||
{
|
||||
stopwatch.Stop();
|
||||
responseBodyStream.Position = 0;
|
||||
var responseBody = await ReadBodyAsync(responseBodyStream, context.RequestAborted).ConfigureAwait(false);
|
||||
responseBodyStream.Position = 0;
|
||||
await responseBodyStream.CopyToAsync(originalResponseBody, context.RequestAborted).ConfigureAwait(false);
|
||||
context.Response.Body = originalResponseBody;
|
||||
|
||||
var statusCode = context.Response.StatusCode;
|
||||
var level = statusCode >= 500 ? LogLevel.Error : statusCode >= 400 ? LogLevel.Warning : LogLevel.Information;
|
||||
|
||||
// 记录响应概要（Info/Warning/Error 级别）。
|
||||
_logger.Log(
|
||||
level,
|
||||
"[HTTP-RES] [{RequestId}] {Method} {Path} => {StatusCode} ({ElapsedMs}ms)",
|
||||
requestId,
|
||||
request.Method,
|
||||
request.Path,
|
||||
statusCode,
|
||||
stopwatch.ElapsedMilliseconds);
|
||||
|
||||
// 记录响应体（Debug 级别）。
|
||||
if (!string.IsNullOrEmpty(responseBody))
|
||||
{
|
||||
_logger.LogDebug(
|
||||
"[HTTP-RES-BODY] [{RequestId}] {Body}",
|
||||
requestId,
|
||||
TruncateBody(responseBody));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 从流中读取文本内容。
|
||||
/// </summary>
|
||||
private static async Task<string> ReadBodyAsync(Stream stream, CancellationToken cancellationToken)
|
||||
{
|
||||
if (!stream.CanRead)
|
||||
{
|
||||
return string.Empty;
|
||||
}
|
||||
|
||||
using var reader = new StreamReader(stream, Encoding.UTF8, leaveOpen: true);
|
||||
var body = await reader.ReadToEndAsync(cancellationToken).ConfigureAwait(false);
|
||||
return body;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 截断过长内容，避免日志膨胀。
|
||||
/// </summary>
|
||||
private static string TruncateBody(string body)
|
||||
{
|
||||
if (body.Length <= MaxBodyLogLength)
|
||||
{
|
||||
return body;
|
||||
}
|
||||
|
||||
return body[..MaxBodyLogLength] + " ... [截断，总长度=" + body.Length + "]";
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 判断当前请求路径是否命中请求/响应日志忽略前缀。
|
||||
/// </summary>
|
||||
private static bool ShouldSkipRequestResponseLog(PathString path)
|
||||
{
|
||||
var pathValue = path.Value;
|
||||
return !string.IsNullOrEmpty(pathValue)
|
||||
&& IgnoredLogPathPrefixes.Any(prefix => pathValue.StartsWith(prefix, StringComparison.OrdinalIgnoreCase));
|
||||
}
|
||||
}
|
||||
@@ -1,13 +1,124 @@
|
||||
var builder = WebApplication.CreateBuilder(args);
|
||||
var app = builder.Build();
|
||||
using Flyshot.ControllerClientCompat;
|
||||
using Flyshot.Server.Host;
|
||||
using Flyshot.Server.Host.Middleware;
|
||||
using Microsoft.Extensions.Options;
|
||||
using Microsoft.OpenApi.Models;
|
||||
using NLog.Web;
|
||||
using Swashbuckle.AspNetCore.SwaggerGen;
|
||||
|
||||
app.MapGet("/", () => Results.Redirect("/healthz"));
|
||||
app.MapGet("/healthz", () => Results.Ok(new
|
||||
// NLog: 在 ASP.NET Core 启动前完成配置加载，确保最早期的日志也能被捕获。
|
||||
NLog.LogManager.Setup().LoadConfigurationFromAppSettings();
|
||||
var logger = NLog.LogManager.GetCurrentClassLogger();
|
||||
|
||||
try
|
||||
{
|
||||
status = "ok",
|
||||
service = "flyshot-server-host"
|
||||
}));
|
||||
logger.Info("Flyshot Server Host 启动中...");
|
||||
|
||||
app.Run();
|
||||
var builder = WebApplication.CreateBuilder(args);
|
||||
|
||||
// NLog: 替换默认日志提供者为 NLog，清除其他 Provider 避免重复输出。
|
||||
builder.Logging.ClearProviders();
|
||||
builder.Host.UseNLog();
|
||||
|
||||
builder.Services.Configure<HostSwaggerOptions>(builder.Configuration.GetSection("Swagger"));
|
||||
builder.Services.Configure<HostCorsOptions>(builder.Configuration.GetSection("Cors"));
|
||||
builder.Services.AddControllerClientCompat(builder.Configuration);
|
||||
builder.Services.AddControllers();
|
||||
builder.Services.AddEndpointsApiExplorer();
|
||||
|
||||
var swaggerOptions = builder.Configuration.GetSection("Swagger").Get<HostSwaggerOptions>() ?? new HostSwaggerOptions();
|
||||
builder.Services.AddSwaggerGen(options =>
|
||||
{
|
||||
options.SwaggerDoc(swaggerOptions.DocumentName, new OpenApiInfo
|
||||
{
|
||||
Title = swaggerOptions.Title,
|
||||
Version = swaggerOptions.Version
|
||||
});
|
||||
|
||||
// 把控制器与 DTO 上的 /// summary 注释纳入 OpenAPI 文档；调试页据此渲染端点标题。
|
||||
var xmlDocumentationPath = Path.Combine(AppContext.BaseDirectory, $"{typeof(Program).Assembly.GetName().Name}.xml");
|
||||
if (File.Exists(xmlDocumentationPath))
|
||||
{
|
||||
options.IncludeXmlComments(xmlDocumentationPath, includeControllerXmlComments: true);
|
||||
}
|
||||
});
|
||||
|
||||
var corsOptions = builder.Configuration.GetSection("Cors").Get<HostCorsOptions>() ?? new HostCorsOptions();
|
||||
builder.Services.AddCors(options =>
|
||||
{
|
||||
options.AddPolicy(corsOptions.PolicyName, policyBuilder =>
|
||||
{
|
||||
// 兼容本地调试时最常见的任意源配置，同时保留显式白名单模式。
|
||||
if (corsOptions.AllowedOrigins.Length == 1 && string.Equals(corsOptions.AllowedOrigins[0], "*", StringComparison.Ordinal))
|
||||
{
|
||||
policyBuilder.AllowAnyOrigin();
|
||||
}
|
||||
else
|
||||
{
|
||||
policyBuilder.WithOrigins(corsOptions.AllowedOrigins);
|
||||
}
|
||||
|
||||
if (corsOptions.AllowedMethods.Length == 1 && string.Equals(corsOptions.AllowedMethods[0], "*", StringComparison.Ordinal))
|
||||
{
|
||||
policyBuilder.AllowAnyMethod();
|
||||
}
|
||||
else
|
||||
{
|
||||
policyBuilder.WithMethods(corsOptions.AllowedMethods);
|
||||
}
|
||||
|
||||
if (corsOptions.AllowedHeaders.Length == 1 && string.Equals(corsOptions.AllowedHeaders[0], "*", StringComparison.Ordinal))
|
||||
{
|
||||
policyBuilder.AllowAnyHeader();
|
||||
}
|
||||
else
|
||||
{
|
||||
policyBuilder.WithHeaders(corsOptions.AllowedHeaders);
|
||||
}
|
||||
});
|
||||
});
|
||||
|
||||
var app = builder.Build();
|
||||
|
||||
var resolvedSwaggerOptions = app.Services.GetRequiredService<IOptions<HostSwaggerOptions>>().Value;
|
||||
var resolvedCorsOptions = app.Services.GetRequiredService<IOptions<HostCorsOptions>>().Value;
|
||||
|
||||
if (resolvedSwaggerOptions.Enabled)
|
||||
{
|
||||
app.UseSwagger(options =>
|
||||
{
|
||||
options.RouteTemplate = resolvedSwaggerOptions.JsonRouteTemplate;
|
||||
});
|
||||
|
||||
app.UseSwaggerUI(options =>
|
||||
{
|
||||
options.RoutePrefix = resolvedSwaggerOptions.RoutePrefix;
|
||||
options.SwaggerEndpoint(
|
||||
$"/swagger/{resolvedSwaggerOptions.DocumentName}/swagger.json",
|
||||
$"{resolvedSwaggerOptions.Title} {resolvedSwaggerOptions.Version}");
|
||||
options.DocumentTitle = resolvedSwaggerOptions.Title;
|
||||
});
|
||||
}
|
||||
|
||||
app.UseCors(resolvedCorsOptions.PolicyName);
|
||||
app.UseStaticFiles();
|
||||
|
||||
// 注册 HTTP 请求/响应日志中间件，记录所有 API 调用的输入输出。
|
||||
app.UseMiddleware<RequestResponseLoggingMiddleware>();
|
||||
|
||||
app.MapControllers();
|
||||
|
||||
logger.Info("Flyshot Server Host 已就绪，开始监听请求。");
|
||||
app.Run();
|
||||
}
|
||||
catch (Exception exception)
|
||||
{
|
||||
logger.Error(exception, "Flyshot Server Host 启动失败。");
|
||||
throw;
|
||||
}
|
||||
finally
|
||||
{
|
||||
NLog.LogManager.Shutdown();
|
||||
}
|
||||
|
||||
public partial class Program;
|
||||
|
||||
@@ -4,5 +4,33 @@
|
||||
"Default": "Information",
|
||||
"Microsoft.AspNetCore": "Warning"
|
||||
}
|
||||
},
|
||||
"ControllerClientCompat": {
|
||||
"ServerVersion": "flyshot-replacement-controller-client-compat/0.1.0"
|
||||
},
|
||||
"Swagger": {
|
||||
"Enabled": true
|
||||
},
|
||||
"Cors": {
|
||||
"PolicyName": "LegacyHttpApi",
|
||||
"AllowedOrigins": [
|
||||
"http://localhost:3000",
|
||||
"http://127.0.0.1:3000"
|
||||
],
|
||||
"AllowedMethods": [
|
||||
"GET",
|
||||
"POST",
|
||||
"OPTIONS"
|
||||
],
|
||||
"AllowedHeaders": [
|
||||
"*"
|
||||
]
|
||||
},
|
||||
"Kestrel": {
|
||||
"Endpoints": {
|
||||
"Http": {
|
||||
"Url": "http://0.0.0.0:5190"
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@@ -5,5 +5,37 @@
|
||||
"Microsoft.AspNetCore": "Warning"
|
||||
}
|
||||
},
|
||||
"ControllerClientCompat": {
|
||||
"ServerVersion": "flyshot-replacement-controller-client-compat/0.1.0"
|
||||
},
|
||||
"Swagger": {
|
||||
"Enabled": true,
|
||||
"DocumentName": "v1",
|
||||
"Title": "Flyshot Replacement HTTP API",
|
||||
"Version": "v1",
|
||||
"JsonRouteTemplate": "swagger/{documentName}/swagger.json",
|
||||
"RoutePrefix": "swagger"
|
||||
},
|
||||
"Cors": {
|
||||
"PolicyName": "LegacyHttpApi",
|
||||
"AllowedOrigins": [
|
||||
"*"
|
||||
],
|
||||
"AllowedMethods": [
|
||||
"GET",
|
||||
"POST",
|
||||
"OPTIONS"
|
||||
],
|
||||
"AllowedHeaders": [
|
||||
"*"
|
||||
]
|
||||
},
|
||||
"Kestrel": {
|
||||
"Endpoints": {
|
||||
"Http": {
|
||||
"Url": "http://0.0.0.0:5000"
|
||||
}
|
||||
}
|
||||
},
|
||||
"AllowedHosts": "*"
|
||||
}
|
||||
|
||||
424
src/Flyshot.Server.Host/wwwroot/assets/debug.css
Normal file
424
src/Flyshot.Server.Host/wwwroot/assets/debug.css
Normal file
@@ -0,0 +1,424 @@
|
||||
:root {
|
||||
color-scheme: light;
|
||||
--bg: #f5f7fb;
|
||||
--surface: #ffffff;
|
||||
--line: #d8dee9;
|
||||
--text: #172033;
|
||||
--muted: #5b667a;
|
||||
--accent: #007c89;
|
||||
--good: #12805c;
|
||||
--warn: #b7791f;
|
||||
--bad: #b42318;
|
||||
--get: #1f6feb;
|
||||
--post: #2da44e;
|
||||
--put: #9a6700;
|
||||
--delete: #cf222e;
|
||||
--code-bg: #f4f6fa;
|
||||
}
|
||||
|
||||
* { box-sizing: border-box; }
|
||||
|
||||
body {
|
||||
margin: 0;
|
||||
min-height: 100vh;
|
||||
background: var(--bg);
|
||||
color: var(--text);
|
||||
font-family: "Segoe UI", "Microsoft YaHei", Arial, sans-serif;
|
||||
font-size: 14px;
|
||||
}
|
||||
|
||||
header {
|
||||
border-bottom: 1px solid var(--line);
|
||||
background: var(--surface);
|
||||
position: sticky;
|
||||
top: 0;
|
||||
z-index: 10;
|
||||
}
|
||||
|
||||
.topbar {
|
||||
display: flex;
|
||||
align-items: center;
|
||||
justify-content: space-between;
|
||||
gap: 16px;
|
||||
width: min(1280px, calc(100% - 32px));
|
||||
margin: 0 auto;
|
||||
padding: 18px 0;
|
||||
}
|
||||
|
||||
h1 {
|
||||
margin: 0;
|
||||
font-size: 22px;
|
||||
font-weight: 650;
|
||||
}
|
||||
|
||||
.actions {
|
||||
display: flex;
|
||||
align-items: center;
|
||||
gap: 10px;
|
||||
}
|
||||
|
||||
button {
|
||||
min-height: 34px;
|
||||
padding: 0 14px;
|
||||
border: 1px solid var(--accent);
|
||||
border-radius: 6px;
|
||||
background: var(--accent);
|
||||
color: #ffffff;
|
||||
font: inherit;
|
||||
cursor: pointer;
|
||||
}
|
||||
|
||||
button.secondary {
|
||||
background: transparent;
|
||||
color: var(--accent);
|
||||
}
|
||||
|
||||
button:disabled {
|
||||
opacity: 0.55;
|
||||
cursor: default;
|
||||
}
|
||||
|
||||
.link-button {
|
||||
display: inline-flex;
|
||||
align-items: center;
|
||||
min-height: 34px;
|
||||
padding: 0 14px;
|
||||
border: 1px solid var(--accent);
|
||||
border-radius: 6px;
|
||||
background: transparent;
|
||||
color: var(--accent);
|
||||
font: inherit;
|
||||
text-decoration: none;
|
||||
}
|
||||
|
||||
.link-button:hover {
|
||||
background: rgba(0, 124, 137, 0.08);
|
||||
}
|
||||
|
||||
main {
|
||||
width: min(1280px, calc(100% - 32px));
|
||||
margin: 22px auto 60px;
|
||||
}
|
||||
|
||||
.meta {
|
||||
border: 1px solid var(--line);
|
||||
border-radius: 8px;
|
||||
background: var(--surface);
|
||||
margin-bottom: 18px;
|
||||
}
|
||||
|
||||
.meta dl {
|
||||
display: grid;
|
||||
grid-template-columns: repeat(3, minmax(0, 1fr));
|
||||
gap: 0;
|
||||
margin: 0;
|
||||
padding: 12px 16px;
|
||||
}
|
||||
|
||||
.meta dt {
|
||||
color: var(--muted);
|
||||
font-size: 12px;
|
||||
}
|
||||
|
||||
.meta dd {
|
||||
margin: 4px 0 0;
|
||||
font-family: Consolas, "Cascadia Mono", monospace;
|
||||
overflow-wrap: anywhere;
|
||||
}
|
||||
|
||||
.meta dd.bad { color: var(--bad); }
|
||||
.meta dd.good { color: var(--good); }
|
||||
|
||||
.group {
|
||||
margin-top: 22px;
|
||||
}
|
||||
|
||||
.group h2 {
|
||||
margin: 0 0 10px 4px;
|
||||
font-size: 16px;
|
||||
font-weight: 650;
|
||||
color: var(--muted);
|
||||
}
|
||||
|
||||
.card {
|
||||
border: 1px solid var(--line);
|
||||
border-radius: 8px;
|
||||
background: var(--surface);
|
||||
margin-bottom: 12px;
|
||||
overflow: hidden;
|
||||
}
|
||||
|
||||
.card-head {
|
||||
display: flex;
|
||||
align-items: center;
|
||||
gap: 12px;
|
||||
padding: 12px 16px;
|
||||
cursor: pointer;
|
||||
user-select: none;
|
||||
}
|
||||
|
||||
.card-head:hover {
|
||||
background: #fafbfd;
|
||||
}
|
||||
|
||||
.badge {
|
||||
flex: 0 0 auto;
|
||||
min-width: 60px;
|
||||
padding: 3px 10px;
|
||||
border-radius: 999px;
|
||||
text-align: center;
|
||||
color: #ffffff;
|
||||
font-weight: 650;
|
||||
font-size: 12px;
|
||||
letter-spacing: 0.5px;
|
||||
}
|
||||
|
||||
.badge.GET { background: var(--get); }
|
||||
.badge.POST { background: var(--post); }
|
||||
.badge.PUT { background: var(--put); }
|
||||
.badge.DELETE { background: var(--delete); }
|
||||
.badge.OTHER { background: var(--muted); }
|
||||
|
||||
.card-path {
|
||||
flex: 1 1 auto;
|
||||
font-family: Consolas, "Cascadia Mono", monospace;
|
||||
font-size: 14px;
|
||||
overflow-wrap: anywhere;
|
||||
}
|
||||
|
||||
.card-summary {
|
||||
flex: 0 1 auto;
|
||||
max-width: 50%;
|
||||
color: var(--muted);
|
||||
font-size: 13px;
|
||||
overflow: hidden;
|
||||
text-overflow: ellipsis;
|
||||
white-space: nowrap;
|
||||
}
|
||||
|
||||
.card-toggle {
|
||||
flex: 0 0 auto;
|
||||
color: var(--muted);
|
||||
font-size: 12px;
|
||||
}
|
||||
|
||||
.card-body {
|
||||
padding: 12px 16px 16px;
|
||||
border-top: 1px solid var(--line);
|
||||
}
|
||||
|
||||
.card.collapsed .card-body {
|
||||
display: none;
|
||||
}
|
||||
|
||||
.form-row {
|
||||
display: grid;
|
||||
grid-template-columns: 180px minmax(0, 1fr) 90px;
|
||||
gap: 8px 12px;
|
||||
align-items: center;
|
||||
margin-bottom: 8px;
|
||||
}
|
||||
|
||||
.form-row .name {
|
||||
font-family: Consolas, "Cascadia Mono", monospace;
|
||||
color: var(--text);
|
||||
overflow-wrap: anywhere;
|
||||
}
|
||||
|
||||
.form-row .name .required {
|
||||
color: var(--bad);
|
||||
margin-left: 4px;
|
||||
}
|
||||
|
||||
.form-row input[type="text"],
|
||||
.form-row input[type="number"] {
|
||||
width: 100%;
|
||||
min-height: 32px;
|
||||
padding: 4px 10px;
|
||||
border: 1px solid var(--line);
|
||||
border-radius: 4px;
|
||||
font: inherit;
|
||||
}
|
||||
|
||||
.form-row .type {
|
||||
color: var(--muted);
|
||||
font-size: 12px;
|
||||
font-family: Consolas, "Cascadia Mono", monospace;
|
||||
}
|
||||
|
||||
.body-block {
|
||||
margin-top: 6px;
|
||||
}
|
||||
|
||||
.body-label {
|
||||
display: flex;
|
||||
align-items: center;
|
||||
justify-content: space-between;
|
||||
gap: 12px;
|
||||
margin-bottom: 6px;
|
||||
}
|
||||
|
||||
.body-label .left {
|
||||
color: var(--muted);
|
||||
font-size: 12px;
|
||||
}
|
||||
|
||||
textarea.body-editor {
|
||||
width: 100%;
|
||||
min-height: 140px;
|
||||
padding: 10px 12px;
|
||||
border: 1px solid var(--line);
|
||||
border-radius: 4px;
|
||||
background: var(--code-bg);
|
||||
font-family: Consolas, "Cascadia Mono", monospace;
|
||||
font-size: 13px;
|
||||
resize: vertical;
|
||||
}
|
||||
|
||||
.button-row {
|
||||
display: flex;
|
||||
flex-wrap: wrap;
|
||||
gap: 8px;
|
||||
margin-top: 12px;
|
||||
}
|
||||
|
||||
.response-block {
|
||||
margin-top: 14px;
|
||||
padding-top: 12px;
|
||||
border-top: 1px dashed var(--line);
|
||||
}
|
||||
|
||||
.response-summary {
|
||||
display: flex;
|
||||
flex-wrap: wrap;
|
||||
align-items: center;
|
||||
gap: 10px;
|
||||
margin-bottom: 8px;
|
||||
font-size: 13px;
|
||||
}
|
||||
|
||||
.status-badge {
|
||||
padding: 2px 8px;
|
||||
border-radius: 4px;
|
||||
color: #ffffff;
|
||||
font-weight: 650;
|
||||
}
|
||||
|
||||
.status-badge.s2xx { background: var(--good); }
|
||||
.status-badge.s3xx { background: var(--get); }
|
||||
.status-badge.s4xx { background: var(--warn); }
|
||||
.status-badge.s5xx { background: var(--bad); }
|
||||
.status-badge.error { background: var(--bad); }
|
||||
|
||||
pre.response-body,
|
||||
pre.response-headers {
|
||||
margin: 6px 0 0;
|
||||
padding: 10px 12px;
|
||||
background: var(--code-bg);
|
||||
border: 1px solid var(--line);
|
||||
border-radius: 4px;
|
||||
max-height: 360px;
|
||||
overflow: auto;
|
||||
white-space: pre-wrap;
|
||||
overflow-wrap: anywhere;
|
||||
font-family: Consolas, "Cascadia Mono", monospace;
|
||||
font-size: 12.5px;
|
||||
}
|
||||
|
||||
pre.response-headers {
|
||||
max-height: 160px;
|
||||
}
|
||||
|
||||
details > summary {
|
||||
cursor: pointer;
|
||||
color: var(--muted);
|
||||
font-size: 12px;
|
||||
margin: 6px 0 0;
|
||||
}
|
||||
|
||||
.empty-hint {
|
||||
padding: 12px 0;
|
||||
color: var(--muted);
|
||||
font-style: italic;
|
||||
}
|
||||
|
||||
.history {
|
||||
position: fixed;
|
||||
right: 16px;
|
||||
bottom: 16px;
|
||||
width: 360px;
|
||||
max-width: calc(100vw - 32px);
|
||||
max-height: 50vh;
|
||||
border: 1px solid var(--line);
|
||||
border-radius: 8px;
|
||||
background: var(--surface);
|
||||
box-shadow: 0 8px 24px rgba(23, 32, 51, 0.12);
|
||||
display: flex;
|
||||
flex-direction: column;
|
||||
z-index: 20;
|
||||
}
|
||||
|
||||
.history h3 {
|
||||
margin: 0;
|
||||
padding: 10px 14px;
|
||||
border-bottom: 1px solid var(--line);
|
||||
font-size: 13px;
|
||||
font-weight: 650;
|
||||
display: flex;
|
||||
justify-content: space-between;
|
||||
align-items: center;
|
||||
}
|
||||
|
||||
.history h3 button {
|
||||
min-height: 24px;
|
||||
padding: 0 8px;
|
||||
font-size: 12px;
|
||||
}
|
||||
|
||||
.history ul {
|
||||
list-style: none;
|
||||
margin: 0;
|
||||
padding: 6px 0;
|
||||
overflow: auto;
|
||||
}
|
||||
|
||||
.history li {
|
||||
padding: 6px 14px;
|
||||
font-size: 12px;
|
||||
border-bottom: 1px solid #edf1f7;
|
||||
display: grid;
|
||||
grid-template-columns: 50px 1fr auto;
|
||||
gap: 8px;
|
||||
align-items: center;
|
||||
}
|
||||
|
||||
.history li:last-child { border-bottom: 0; }
|
||||
|
||||
.history li .h-method {
|
||||
font-weight: 650;
|
||||
font-family: Consolas, "Cascadia Mono", monospace;
|
||||
}
|
||||
|
||||
.history li .h-path {
|
||||
font-family: Consolas, "Cascadia Mono", monospace;
|
||||
overflow: hidden;
|
||||
text-overflow: ellipsis;
|
||||
white-space: nowrap;
|
||||
}
|
||||
|
||||
@media (max-width: 920px) {
|
||||
.form-row {
|
||||
grid-template-columns: 1fr;
|
||||
}
|
||||
|
||||
.meta dl {
|
||||
grid-template-columns: 1fr;
|
||||
}
|
||||
|
||||
.history {
|
||||
position: static;
|
||||
width: auto;
|
||||
margin-top: 18px;
|
||||
max-height: none;
|
||||
}
|
||||
}
|
||||
686
src/Flyshot.Server.Host/wwwroot/assets/debug.js
Normal file
686
src/Flyshot.Server.Host/wwwroot/assets/debug.js
Normal file
@@ -0,0 +1,686 @@
|
||||
// 静态调试页通过配置 API 获取实际 Swagger JSON 地址，避免硬编码路由前缀。
|
||||
const DEBUG_CONFIG_URL = "/api/debug/config";
|
||||
const STORAGE_PREFIX = "flyshot.debug.";
|
||||
const HISTORY_LIMIT = 10;
|
||||
|
||||
const groupTitleByPrefix = [
|
||||
// 基础与状态分组：探活和状态快照两个固定 API 路径
|
||||
{ match: function (op) { return op.path === "/healthz" || op.path === "/api/status/snapshot"; }, title: "基础与状态" },
|
||||
// 默认兜底：剩余全部走 ControllerClient 兼容分组
|
||||
{ match: function () { return true; }, title: "ControllerClient 兼容" }
|
||||
];
|
||||
|
||||
const state = {
|
||||
spec: null,
|
||||
operations: [],
|
||||
history: []
|
||||
};
|
||||
|
||||
const requestBodySamples = {
|
||||
"POST /move_pose/": {
|
||||
x: 100.0,
|
||||
y: 200.0,
|
||||
z: 300.0,
|
||||
w: 0.0,
|
||||
p: 45.0,
|
||||
r: 0.0
|
||||
}
|
||||
};
|
||||
|
||||
/** 简单的 escape：把任意字符串安全嵌入 textContent 之外的位置时使用。 */
|
||||
function escapeHtml(value) {
|
||||
return String(value).replace(/[&<>"']/g, function (ch) {
|
||||
return { "&": "&", "<": "<", ">": ">", "\"": """, "'": "'" }[ch];
|
||||
});
|
||||
}
|
||||
|
||||
/** 解析 OpenAPI 中的 $ref 引用，仅支持本地 components.schemas 形式。 */
|
||||
function resolveRef(ref) {
|
||||
if (!ref || !state.spec) return null;
|
||||
const parts = ref.replace(/^#\//, "").split("/");
|
||||
let cursor = state.spec;
|
||||
for (const part of parts) {
|
||||
if (cursor && Object.prototype.hasOwnProperty.call(cursor, part)) {
|
||||
cursor = cursor[part];
|
||||
} else {
|
||||
return null;
|
||||
}
|
||||
}
|
||||
return cursor;
|
||||
}
|
||||
|
||||
/** 根据 schema 生成默认 JSON 模板，用于自动填充请求体编辑器。 */
|
||||
function buildSampleFromSchema(schema, depth) {
|
||||
depth = depth || 0;
|
||||
// 防御递归：复杂自引用 schema 在 4 层后停下，避免栈爆。
|
||||
if (!schema || depth > 4) return null;
|
||||
|
||||
if (schema.$ref) {
|
||||
const resolved = resolveRef(schema.$ref);
|
||||
return resolved ? buildSampleFromSchema(resolved, depth + 1) : null;
|
||||
}
|
||||
|
||||
// 部分 schema 只标 oneOf/anyOf/allOf，挑第一个分支即可，调试场景够用。
|
||||
if (Array.isArray(schema.oneOf) && schema.oneOf.length > 0) return buildSampleFromSchema(schema.oneOf[0], depth + 1);
|
||||
if (Array.isArray(schema.anyOf) && schema.anyOf.length > 0) return buildSampleFromSchema(schema.anyOf[0], depth + 1);
|
||||
if (Array.isArray(schema.allOf) && schema.allOf.length > 0) {
|
||||
const merged = {};
|
||||
schema.allOf.forEach(function (sub) {
|
||||
const value = buildSampleFromSchema(sub, depth + 1);
|
||||
if (value && typeof value === "object" && !Array.isArray(value)) Object.assign(merged, value);
|
||||
});
|
||||
return merged;
|
||||
}
|
||||
|
||||
const type = schema.type || (schema.properties ? "object" : "string");
|
||||
switch (type) {
|
||||
case "object": {
|
||||
const result = {};
|
||||
const props = schema.properties || {};
|
||||
Object.keys(props).forEach(function (key) {
|
||||
result[key] = buildSampleFromSchema(props[key], depth + 1);
|
||||
});
|
||||
return result;
|
||||
}
|
||||
case "array":
|
||||
return [];
|
||||
case "integer":
|
||||
case "number":
|
||||
return 0;
|
||||
case "boolean":
|
||||
return false;
|
||||
case "string":
|
||||
default:
|
||||
if (schema.enum && schema.enum.length > 0) return schema.enum[0];
|
||||
return "";
|
||||
}
|
||||
}
|
||||
|
||||
/** 把 schema.type 翻译成 input type 与展示文本。 */
|
||||
function inputKindForType(schema) {
|
||||
if (!schema) return { kind: "text", label: "string" };
|
||||
const type = schema.type || "string";
|
||||
if (type === "boolean") return { kind: "checkbox", label: "boolean" };
|
||||
if (type === "integer" || type === "number") return { kind: "number", label: type };
|
||||
return { kind: "text", label: type };
|
||||
}
|
||||
|
||||
/** 把 OpenAPI 的 paths 节点展开成扁平的 operation 列表。 */
|
||||
function extractOperations(spec) {
|
||||
const operations = [];
|
||||
const paths = spec.paths || {};
|
||||
Object.keys(paths).forEach(function (path) {
|
||||
const pathItem = paths[path] || {};
|
||||
["get", "post", "put", "delete", "patch", "options", "head"].forEach(function (method) {
|
||||
const op = pathItem[method];
|
||||
if (!op) return;
|
||||
const parameters = (op.parameters || []).filter(function (p) { return p.in === "query" || p.in === "path"; });
|
||||
let bodySchema = null;
|
||||
if (op.requestBody && op.requestBody.content) {
|
||||
const json = op.requestBody.content["application/json"];
|
||||
if (json && json.schema) bodySchema = json.schema;
|
||||
}
|
||||
operations.push({
|
||||
method: method.toUpperCase(),
|
||||
path: path,
|
||||
summary: op.summary || "",
|
||||
description: op.description || "",
|
||||
tags: op.tags || [],
|
||||
parameters: parameters,
|
||||
bodySchema: bodySchema
|
||||
});
|
||||
});
|
||||
});
|
||||
return operations;
|
||||
}
|
||||
|
||||
/** 选择分组：优先用第一条匹配的 groupTitleByPrefix 规则，OpenAPI tag 留作兜底。 */
|
||||
function pickGroup(op) {
|
||||
for (const rule of groupTitleByPrefix) {
|
||||
if (rule.match(op)) return rule.title;
|
||||
}
|
||||
if (op.tags && op.tags.length > 0) return op.tags[0];
|
||||
return "其它";
|
||||
}
|
||||
|
||||
/** localStorage key 必须避免冲突，使用 method:path 复合键。 */
|
||||
function storageKey(op) {
|
||||
return STORAGE_PREFIX + op.method + ":" + op.path;
|
||||
}
|
||||
|
||||
/** 返回指定端点的手工调试样例，处理 JsonElement 等 OpenAPI 无法自动推断字段的接口。 */
|
||||
function getRequestBodySample(op) {
|
||||
return requestBodySamples[op.method + " " + op.path] || null;
|
||||
}
|
||||
|
||||
/** 读取本端点最近一次输入；解析失败则当作空。 */
|
||||
function loadInputs(op) {
|
||||
try {
|
||||
const raw = window.localStorage.getItem(storageKey(op));
|
||||
return raw ? JSON.parse(raw) : null;
|
||||
} catch (e) {
|
||||
return null;
|
||||
}
|
||||
}
|
||||
|
||||
/** 保存本端点最近一次输入；写入失败时静默忽略，避免影响调试体验。 */
|
||||
function saveInputs(op, payload) {
|
||||
try {
|
||||
window.localStorage.setItem(storageKey(op), JSON.stringify(payload));
|
||||
} catch (e) {
|
||||
// localStorage 可能被禁用或满载，忽略写入失败。
|
||||
}
|
||||
}
|
||||
|
||||
/** 拼接最终请求 URL（含 query 串与 path 参数替换）。 */
|
||||
function buildRequestUrl(op, paramValues) {
|
||||
let path = op.path;
|
||||
const queryPairs = [];
|
||||
op.parameters.forEach(function (param) {
|
||||
const raw = paramValues[param.name];
|
||||
if (raw === undefined || raw === null || raw === "") return;
|
||||
if (param.in === "path") {
|
||||
path = path.replace("{" + param.name + "}", encodeURIComponent(raw));
|
||||
} else if (param.in === "query") {
|
||||
queryPairs.push(encodeURIComponent(param.name) + "=" + encodeURIComponent(raw));
|
||||
}
|
||||
});
|
||||
return path + (queryPairs.length > 0 ? "?" + queryPairs.join("&") : "");
|
||||
}
|
||||
|
||||
/** 生成与浏览器请求等价的 curl 命令，便于复制到终端复现。 */
|
||||
function buildCurlCommand(op, requestUrl, body) {
|
||||
const parts = ["curl", "-X", op.method, JSON.stringify(window.location.origin + requestUrl)];
|
||||
if (body !== null && body !== undefined && body !== "") {
|
||||
parts.push("-H", "\"Content-Type: application/json\"");
|
||||
parts.push("--data-raw", JSON.stringify(body));
|
||||
}
|
||||
return parts.join(" ");
|
||||
}
|
||||
|
||||
/** 渲染参数输入表单，返回收集函数。 */
|
||||
function renderParameterRows(container, op, savedValues) {
|
||||
if (op.parameters.length === 0) return function () { return {}; };
|
||||
|
||||
const inputs = {};
|
||||
op.parameters.forEach(function (param) {
|
||||
const row = document.createElement("div");
|
||||
row.className = "form-row";
|
||||
|
||||
const nameNode = document.createElement("div");
|
||||
nameNode.className = "name";
|
||||
nameNode.textContent = param.name + " (" + param.in + ")";
|
||||
if (param.required) {
|
||||
const requiredMark = document.createElement("span");
|
||||
requiredMark.className = "required";
|
||||
requiredMark.textContent = "*";
|
||||
nameNode.appendChild(requiredMark);
|
||||
}
|
||||
row.appendChild(nameNode);
|
||||
|
||||
const kind = inputKindForType(param.schema);
|
||||
const inputNode = document.createElement("input");
|
||||
inputNode.type = kind.kind;
|
||||
if (kind.kind === "checkbox") {
|
||||
inputNode.checked = savedValues && Object.prototype.hasOwnProperty.call(savedValues, param.name)
|
||||
? Boolean(savedValues[param.name])
|
||||
: Boolean(param.schema && param.schema.default);
|
||||
} else {
|
||||
let initial = "";
|
||||
if (savedValues && Object.prototype.hasOwnProperty.call(savedValues, param.name)) {
|
||||
initial = String(savedValues[param.name]);
|
||||
} else if (param.schema && param.schema.default !== undefined) {
|
||||
initial = String(param.schema.default);
|
||||
}
|
||||
inputNode.value = initial;
|
||||
if (kind.kind === "number") inputNode.step = "any";
|
||||
}
|
||||
row.appendChild(inputNode);
|
||||
|
||||
const typeNode = document.createElement("div");
|
||||
typeNode.className = "type";
|
||||
typeNode.textContent = kind.label;
|
||||
row.appendChild(typeNode);
|
||||
|
||||
container.appendChild(row);
|
||||
inputs[param.name] = { node: inputNode, kind: kind.kind, schema: param.schema };
|
||||
});
|
||||
|
||||
return function collect() {
|
||||
const collected = {};
|
||||
Object.keys(inputs).forEach(function (key) {
|
||||
const item = inputs[key];
|
||||
if (item.kind === "checkbox") {
|
||||
collected[key] = item.node.checked;
|
||||
} else {
|
||||
const raw = item.node.value;
|
||||
if (raw === "") {
|
||||
collected[key] = "";
|
||||
} else if (item.kind === "number") {
|
||||
const num = Number(raw);
|
||||
collected[key] = Number.isNaN(num) ? raw : num;
|
||||
} else {
|
||||
collected[key] = raw;
|
||||
}
|
||||
}
|
||||
});
|
||||
return collected;
|
||||
};
|
||||
}
|
||||
|
||||
/** 渲染请求体编辑器，返回收集函数。 */
|
||||
function renderBodyEditor(container, op, savedBody) {
|
||||
if (!op.bodySchema) return function () { return null; };
|
||||
|
||||
const block = document.createElement("div");
|
||||
block.className = "body-block";
|
||||
|
||||
const labelRow = document.createElement("div");
|
||||
labelRow.className = "body-label";
|
||||
const left = document.createElement("div");
|
||||
left.className = "left";
|
||||
left.textContent = "请求体 (application/json)";
|
||||
labelRow.appendChild(left);
|
||||
|
||||
const formatBtn = document.createElement("button");
|
||||
formatBtn.type = "button";
|
||||
formatBtn.className = "secondary";
|
||||
formatBtn.textContent = "格式化 JSON";
|
||||
labelRow.appendChild(formatBtn);
|
||||
|
||||
block.appendChild(labelRow);
|
||||
|
||||
const textarea = document.createElement("textarea");
|
||||
textarea.className = "body-editor";
|
||||
textarea.spellcheck = false;
|
||||
let initialText;
|
||||
if (savedBody !== undefined && savedBody !== null) {
|
||||
initialText = typeof savedBody === "string" ? savedBody : JSON.stringify(savedBody, null, 2);
|
||||
} else {
|
||||
const sample = getRequestBodySample(op) || buildSampleFromSchema(op.bodySchema, 0);
|
||||
initialText = sample === null ? "" : JSON.stringify(sample, null, 2);
|
||||
}
|
||||
textarea.value = initialText;
|
||||
block.appendChild(textarea);
|
||||
|
||||
formatBtn.addEventListener("click", function () {
|
||||
try {
|
||||
const parsed = JSON.parse(textarea.value || "null");
|
||||
textarea.value = parsed === null ? "" : JSON.stringify(parsed, null, 2);
|
||||
} catch (e) {
|
||||
window.alert("JSON 解析失败: " + e.message);
|
||||
}
|
||||
});
|
||||
|
||||
container.appendChild(block);
|
||||
|
||||
return function collect() {
|
||||
return textarea.value;
|
||||
};
|
||||
}
|
||||
|
||||
/** 把 HTTP 状态码翻译成颜色徽标 class。 */
|
||||
function statusBadgeClass(status) {
|
||||
if (status >= 200 && status < 300) return "s2xx";
|
||||
if (status >= 300 && status < 400) return "s3xx";
|
||||
if (status >= 400 && status < 500) return "s4xx";
|
||||
if (status >= 500) return "s5xx";
|
||||
return "error";
|
||||
}
|
||||
|
||||
/** 把响应头展开成可读字符串。 */
|
||||
function formatHeaders(headers) {
|
||||
const lines = [];
|
||||
headers.forEach(function (value, key) { lines.push(key + ": " + value); });
|
||||
return lines.join("\n");
|
||||
}
|
||||
|
||||
/** 在历史面板顶部追加一条记录，超过上限则丢弃尾部。 */
|
||||
function pushHistory(entry) {
|
||||
state.history.unshift(entry);
|
||||
if (state.history.length > HISTORY_LIMIT) state.history.length = HISTORY_LIMIT;
|
||||
renderHistory();
|
||||
}
|
||||
|
||||
function renderHistory() {
|
||||
const list = document.getElementById("history-list");
|
||||
list.innerHTML = "";
|
||||
if (state.history.length === 0) {
|
||||
const empty = document.createElement("li");
|
||||
empty.textContent = "暂无调用记录";
|
||||
empty.style.color = "var(--muted)";
|
||||
empty.style.gridTemplateColumns = "1fr";
|
||||
list.appendChild(empty);
|
||||
return;
|
||||
}
|
||||
state.history.forEach(function (entry) {
|
||||
const li = document.createElement("li");
|
||||
const method = document.createElement("span");
|
||||
method.className = "h-method";
|
||||
method.textContent = entry.method;
|
||||
method.style.color = entry.method === "GET" ? "var(--get)" : entry.method === "POST" ? "var(--post)" : "var(--muted)";
|
||||
const path = document.createElement("span");
|
||||
path.className = "h-path";
|
||||
path.title = entry.url;
|
||||
path.textContent = entry.url;
|
||||
const meta = document.createElement("span");
|
||||
meta.style.color = "var(--muted)";
|
||||
meta.textContent = (entry.status || "ERR") + " · " + entry.elapsedMs + "ms";
|
||||
li.appendChild(method);
|
||||
li.appendChild(path);
|
||||
li.appendChild(meta);
|
||||
list.appendChild(li);
|
||||
});
|
||||
}
|
||||
|
||||
/** 渲染单个端点的卡片。 */
|
||||
function renderOperationCard(op) {
|
||||
const card = document.createElement("section");
|
||||
card.className = "card collapsed";
|
||||
|
||||
const head = document.createElement("div");
|
||||
head.className = "card-head";
|
||||
|
||||
const badge = document.createElement("span");
|
||||
badge.className = "badge " + (["GET", "POST", "PUT", "DELETE"].indexOf(op.method) >= 0 ? op.method : "OTHER");
|
||||
badge.textContent = op.method;
|
||||
head.appendChild(badge);
|
||||
|
||||
const path = document.createElement("span");
|
||||
path.className = "card-path";
|
||||
path.textContent = op.path;
|
||||
head.appendChild(path);
|
||||
|
||||
const summary = document.createElement("span");
|
||||
summary.className = "card-summary";
|
||||
summary.textContent = op.summary;
|
||||
summary.title = op.summary;
|
||||
head.appendChild(summary);
|
||||
|
||||
const toggle = document.createElement("span");
|
||||
toggle.className = "card-toggle";
|
||||
toggle.textContent = "展开 ▾";
|
||||
head.appendChild(toggle);
|
||||
|
||||
head.addEventListener("click", function () {
|
||||
const collapsed = card.classList.toggle("collapsed");
|
||||
toggle.textContent = collapsed ? "展开 ▾" : "收起 ▴";
|
||||
});
|
||||
card.appendChild(head);
|
||||
|
||||
const body = document.createElement("div");
|
||||
body.className = "card-body";
|
||||
|
||||
// 描述（来自 XML summary）独立成一段
|
||||
if (op.summary) {
|
||||
const desc = document.createElement("div");
|
||||
desc.style.color = "var(--muted)";
|
||||
desc.style.marginBottom = "10px";
|
||||
desc.style.fontSize = "13px";
|
||||
desc.textContent = op.summary;
|
||||
body.appendChild(desc);
|
||||
}
|
||||
|
||||
const saved = loadInputs(op) || {};
|
||||
|
||||
// 参数区
|
||||
let collectParams = function () { return {}; };
|
||||
if (op.parameters.length > 0) {
|
||||
const paramsContainer = document.createElement("div");
|
||||
paramsContainer.className = "params";
|
||||
body.appendChild(paramsContainer);
|
||||
collectParams = renderParameterRows(paramsContainer, op, saved.params);
|
||||
}
|
||||
|
||||
// 请求体区
|
||||
const collectBody = renderBodyEditor(body, op, saved.body);
|
||||
|
||||
// 操作按钮
|
||||
const buttonRow = document.createElement("div");
|
||||
buttonRow.className = "button-row";
|
||||
|
||||
const sendBtn = document.createElement("button");
|
||||
sendBtn.type = "button";
|
||||
sendBtn.textContent = "发送";
|
||||
|
||||
const resetBtn = document.createElement("button");
|
||||
resetBtn.type = "button";
|
||||
resetBtn.className = "secondary";
|
||||
resetBtn.textContent = "重置";
|
||||
|
||||
const curlBtn = document.createElement("button");
|
||||
curlBtn.type = "button";
|
||||
curlBtn.className = "secondary";
|
||||
curlBtn.textContent = "复制 curl";
|
||||
|
||||
buttonRow.appendChild(sendBtn);
|
||||
buttonRow.appendChild(resetBtn);
|
||||
buttonRow.appendChild(curlBtn);
|
||||
body.appendChild(buttonRow);
|
||||
|
||||
// 响应面板
|
||||
const responseBlock = document.createElement("div");
|
||||
responseBlock.className = "response-block";
|
||||
responseBlock.style.display = "none";
|
||||
body.appendChild(responseBlock);
|
||||
|
||||
function renderResponse(payload) {
|
||||
responseBlock.style.display = "block";
|
||||
responseBlock.innerHTML = "";
|
||||
|
||||
const summaryRow = document.createElement("div");
|
||||
summaryRow.className = "response-summary";
|
||||
|
||||
const statusBadge = document.createElement("span");
|
||||
statusBadge.className = "status-badge " + statusBadgeClass(payload.status || 0);
|
||||
statusBadge.textContent = payload.status ? payload.status + " " + (payload.statusText || "") : "请求失败";
|
||||
summaryRow.appendChild(statusBadge);
|
||||
|
||||
const elapsed = document.createElement("span");
|
||||
elapsed.style.color = "var(--muted)";
|
||||
elapsed.textContent = payload.elapsedMs + " ms · " + payload.url;
|
||||
summaryRow.appendChild(elapsed);
|
||||
|
||||
responseBlock.appendChild(summaryRow);
|
||||
|
||||
if (payload.error) {
|
||||
const pre = document.createElement("pre");
|
||||
pre.className = "response-body";
|
||||
pre.textContent = payload.error;
|
||||
responseBlock.appendChild(pre);
|
||||
return;
|
||||
}
|
||||
|
||||
const headersDetails = document.createElement("details");
|
||||
const headersSummary = document.createElement("summary");
|
||||
headersSummary.textContent = "响应头";
|
||||
headersDetails.appendChild(headersSummary);
|
||||
const headersPre = document.createElement("pre");
|
||||
headersPre.className = "response-headers";
|
||||
headersPre.textContent = payload.headers;
|
||||
headersDetails.appendChild(headersPre);
|
||||
responseBlock.appendChild(headersDetails);
|
||||
|
||||
const bodyPre = document.createElement("pre");
|
||||
bodyPre.className = "response-body";
|
||||
bodyPre.textContent = payload.bodyText;
|
||||
responseBlock.appendChild(bodyPre);
|
||||
}
|
||||
|
||||
sendBtn.addEventListener("click", async function () {
|
||||
sendBtn.disabled = true;
|
||||
const params = collectParams();
|
||||
const rawBody = collectBody();
|
||||
saveInputs(op, { params: params, body: rawBody });
|
||||
|
||||
const requestUrl = buildRequestUrl(op, params);
|
||||
const init = { method: op.method, headers: {} };
|
||||
|
||||
// 仅 POST/PUT/PATCH/DELETE 才认为可能携带 body；对没有 bodySchema 的方法直接跳过。
|
||||
const methodAllowsBody = ["POST", "PUT", "PATCH", "DELETE"].indexOf(op.method) >= 0;
|
||||
if (methodAllowsBody && op.bodySchema && rawBody !== null && rawBody !== undefined && rawBody !== "") {
|
||||
init.headers["Content-Type"] = "application/json";
|
||||
init.body = rawBody;
|
||||
}
|
||||
|
||||
const startedAt = performance.now();
|
||||
try {
|
||||
const response = await fetch(requestUrl, init);
|
||||
const elapsedMs = Math.round(performance.now() - startedAt);
|
||||
const text = await response.text();
|
||||
const contentType = response.headers.get("content-type") || "";
|
||||
let bodyText = text;
|
||||
if (contentType.indexOf("application/json") >= 0) {
|
||||
try {
|
||||
bodyText = JSON.stringify(JSON.parse(text), null, 2);
|
||||
} catch (e) {
|
||||
bodyText = text;
|
||||
}
|
||||
}
|
||||
renderResponse({
|
||||
status: response.status,
|
||||
statusText: response.statusText,
|
||||
headers: formatHeaders(response.headers),
|
||||
bodyText: bodyText,
|
||||
url: requestUrl,
|
||||
elapsedMs: elapsedMs
|
||||
});
|
||||
pushHistory({ method: op.method, url: requestUrl, status: response.status, elapsedMs: elapsedMs });
|
||||
} catch (err) {
|
||||
const elapsedMs = Math.round(performance.now() - startedAt);
|
||||
renderResponse({
|
||||
error: String(err && err.message ? err.message : err),
|
||||
url: requestUrl,
|
||||
elapsedMs: elapsedMs
|
||||
});
|
||||
pushHistory({ method: op.method, url: requestUrl, status: 0, elapsedMs: elapsedMs });
|
||||
} finally {
|
||||
sendBtn.disabled = false;
|
||||
}
|
||||
});
|
||||
|
||||
resetBtn.addEventListener("click", function () {
|
||||
try { window.localStorage.removeItem(storageKey(op)); } catch (e) { /* 忽略 */ }
|
||||
// 直接重新渲染当前卡片：替换原 DOM 节点。
|
||||
const refreshed = renderOperationCard(op);
|
||||
refreshed.classList.remove("collapsed");
|
||||
refreshed.querySelector(".card-toggle").textContent = "收起 ▴";
|
||||
card.parentNode.replaceChild(refreshed, card);
|
||||
});
|
||||
|
||||
curlBtn.addEventListener("click", function () {
|
||||
const params = collectParams();
|
||||
const rawBody = collectBody();
|
||||
const requestUrl = buildRequestUrl(op, params);
|
||||
const methodAllowsBody = ["POST", "PUT", "PATCH", "DELETE"].indexOf(op.method) >= 0;
|
||||
const bodyForCurl = methodAllowsBody && op.bodySchema ? rawBody : null;
|
||||
const command = buildCurlCommand(op, requestUrl, bodyForCurl);
|
||||
if (navigator.clipboard && navigator.clipboard.writeText) {
|
||||
navigator.clipboard.writeText(command).then(function () {
|
||||
curlBtn.textContent = "已复制 ✓";
|
||||
window.setTimeout(function () { curlBtn.textContent = "复制 curl"; }, 1500);
|
||||
}).catch(function () {
|
||||
window.prompt("复制失败，手动复制：", command);
|
||||
});
|
||||
} else {
|
||||
window.prompt("复制失败，手动复制：", command);
|
||||
}
|
||||
});
|
||||
|
||||
card.appendChild(body);
|
||||
return card;
|
||||
}
|
||||
|
||||
/** 把 operation 列表按分组渲染到主区域。 */
|
||||
function renderGroups(operations) {
|
||||
const root = document.getElementById("debug-console-app");
|
||||
root.innerHTML = "";
|
||||
if (operations.length === 0) {
|
||||
const empty = document.createElement("div");
|
||||
empty.className = "empty-hint";
|
||||
empty.textContent = "OpenAPI 文档中没有任何端点。";
|
||||
root.appendChild(empty);
|
||||
return;
|
||||
}
|
||||
|
||||
const grouped = new Map();
|
||||
operations.forEach(function (op) {
|
||||
const groupTitle = pickGroup(op);
|
||||
if (!grouped.has(groupTitle)) grouped.set(groupTitle, []);
|
||||
grouped.get(groupTitle).push(op);
|
||||
});
|
||||
|
||||
// 固定输出顺序：基础与状态在前，ControllerClient 兼容在后，其余按字典序。
|
||||
const orderedTitles = [];
|
||||
["基础与状态", "ControllerClient 兼容"].forEach(function (title) {
|
||||
if (grouped.has(title)) orderedTitles.push(title);
|
||||
});
|
||||
Array.from(grouped.keys()).sort().forEach(function (title) {
|
||||
if (orderedTitles.indexOf(title) < 0) orderedTitles.push(title);
|
||||
});
|
||||
|
||||
orderedTitles.forEach(function (title) {
|
||||
const ops = grouped.get(title);
|
||||
ops.sort(function (a, b) {
|
||||
if (a.path === b.path) return a.method.localeCompare(b.method);
|
||||
return a.path.localeCompare(b.path);
|
||||
});
|
||||
|
||||
const section = document.createElement("section");
|
||||
section.className = "group";
|
||||
const heading = document.createElement("h2");
|
||||
heading.textContent = title + " (" + ops.length + ")";
|
||||
section.appendChild(heading);
|
||||
ops.forEach(function (op) { section.appendChild(renderOperationCard(op)); });
|
||||
root.appendChild(section);
|
||||
});
|
||||
}
|
||||
|
||||
/** 加载 OpenAPI 文档并渲染。 */
|
||||
async function loadSpecAndRender() {
|
||||
const metaSpec = document.getElementById("meta-spec-url");
|
||||
const metaCount = document.getElementById("meta-operation-count");
|
||||
const metaStatus = document.getElementById("meta-status");
|
||||
|
||||
metaSpec.textContent = "正在读取调试配置...";
|
||||
metaStatus.textContent = "正在拉取 OpenAPI 文档...";
|
||||
metaStatus.className = "";
|
||||
|
||||
try {
|
||||
const configResponse = await fetch(DEBUG_CONFIG_URL, { cache: "no-store" });
|
||||
if (!configResponse.ok) throw new Error("调试配置 HTTP " + configResponse.status + " " + configResponse.statusText);
|
||||
const config = await configResponse.json();
|
||||
const swaggerJsonUrl = config.swaggerJsonUrl;
|
||||
if (!swaggerJsonUrl) throw new Error("调试配置缺少 swaggerJsonUrl");
|
||||
metaSpec.textContent = swaggerJsonUrl;
|
||||
|
||||
const response = await fetch(swaggerJsonUrl, { cache: "no-store" });
|
||||
if (!response.ok) throw new Error("HTTP " + response.status + " " + response.statusText);
|
||||
const spec = await response.json();
|
||||
state.spec = spec;
|
||||
state.operations = extractOperations(spec);
|
||||
metaCount.textContent = state.operations.length;
|
||||
metaStatus.textContent = "已加载";
|
||||
metaStatus.className = "good";
|
||||
renderGroups(state.operations);
|
||||
} catch (err) {
|
||||
metaStatus.textContent = "加载失败: " + (err && err.message ? err.message : err);
|
||||
metaStatus.className = "bad";
|
||||
metaCount.textContent = "0";
|
||||
const root = document.getElementById("debug-console-app");
|
||||
root.innerHTML = "";
|
||||
const errBlock = document.createElement("div");
|
||||
errBlock.className = "empty-hint";
|
||||
errBlock.textContent = "无法加载 OpenAPI 文档，请确认 Swagger:Enabled = true 且 " + DEBUG_CONFIG_URL + " 可访问。";
|
||||
root.appendChild(errBlock);
|
||||
}
|
||||
}
|
||||
|
||||
document.getElementById("reload-spec").addEventListener("click", loadSpecAndRender);
|
||||
document.getElementById("history-clear").addEventListener("click", function () {
|
||||
state.history.length = 0;
|
||||
renderHistory();
|
||||
});
|
||||
|
||||
renderHistory();
|
||||
loadSpecAndRender();
|
||||
526
src/Flyshot.Server.Host/wwwroot/assets/status.css
Normal file
526
src/Flyshot.Server.Host/wwwroot/assets/status.css
Normal file
@@ -0,0 +1,526 @@
|
||||
:root {
|
||||
color-scheme: light;
|
||||
--bg: #f5f7fb;
|
||||
--surface: #ffffff;
|
||||
--line: #d8dee9;
|
||||
--text: #172033;
|
||||
--muted: #5b667a;
|
||||
--accent: #007c89;
|
||||
--accent-dark: #005d67;
|
||||
--steel: #2f3b4f;
|
||||
--steel-soft: #e7ebf1;
|
||||
--good: #12805c;
|
||||
--warn: #b7791f;
|
||||
--bad: #b42318;
|
||||
}
|
||||
|
||||
* {
|
||||
box-sizing: border-box;
|
||||
}
|
||||
|
||||
body {
|
||||
margin: 0;
|
||||
min-height: 100vh;
|
||||
background: var(--bg);
|
||||
color: var(--text);
|
||||
font-family: "Segoe UI", "Microsoft YaHei", Arial, sans-serif;
|
||||
font-size: 15px;
|
||||
letter-spacing: 0;
|
||||
}
|
||||
|
||||
header {
|
||||
border-bottom: 1px solid var(--line);
|
||||
background: var(--surface);
|
||||
}
|
||||
|
||||
.topbar {
|
||||
display: flex;
|
||||
align-items: center;
|
||||
justify-content: space-between;
|
||||
gap: 16px;
|
||||
width: min(1180px, calc(100% - 32px));
|
||||
margin: 0 auto;
|
||||
padding: 18px 0;
|
||||
}
|
||||
|
||||
h1 {
|
||||
margin: 0;
|
||||
font-size: 22px;
|
||||
font-weight: 650;
|
||||
}
|
||||
|
||||
button {
|
||||
min-height: 36px;
|
||||
padding: 0 14px;
|
||||
border: 1px solid var(--accent);
|
||||
border-radius: 6px;
|
||||
background: var(--accent);
|
||||
color: #ffffff;
|
||||
font: inherit;
|
||||
cursor: pointer;
|
||||
}
|
||||
|
||||
button:disabled {
|
||||
opacity: 0.6;
|
||||
cursor: default;
|
||||
}
|
||||
|
||||
/* 顶部操作区按钮和外链按钮共用同一组视觉样式，便于现场顺手跳转。 */
|
||||
.actions {
|
||||
display: flex;
|
||||
align-items: center;
|
||||
gap: 10px;
|
||||
}
|
||||
|
||||
.link-button {
|
||||
display: inline-flex;
|
||||
align-items: center;
|
||||
min-height: 36px;
|
||||
padding: 0 14px;
|
||||
border: 1px solid var(--accent);
|
||||
border-radius: 6px;
|
||||
background: transparent;
|
||||
color: var(--accent);
|
||||
font: inherit;
|
||||
text-decoration: none;
|
||||
}
|
||||
|
||||
.link-button:hover {
|
||||
background: rgba(0, 124, 137, 0.08);
|
||||
}
|
||||
|
||||
main {
|
||||
width: min(1180px, calc(100% - 32px));
|
||||
margin: 22px auto;
|
||||
}
|
||||
|
||||
.summary {
|
||||
display: grid;
|
||||
grid-template-columns: repeat(4, minmax(0, 1fr));
|
||||
gap: 12px;
|
||||
margin-bottom: 16px;
|
||||
}
|
||||
|
||||
.metric,
|
||||
section {
|
||||
border: 1px solid var(--line);
|
||||
border-radius: 8px;
|
||||
background: var(--surface);
|
||||
}
|
||||
|
||||
.metric {
|
||||
min-height: 86px;
|
||||
padding: 14px;
|
||||
}
|
||||
|
||||
.label {
|
||||
color: var(--muted);
|
||||
font-size: 13px;
|
||||
}
|
||||
|
||||
.value {
|
||||
margin-top: 8px;
|
||||
overflow-wrap: anywhere;
|
||||
font-size: 24px;
|
||||
font-weight: 650;
|
||||
}
|
||||
|
||||
.status-row {
|
||||
display: flex;
|
||||
align-items: center;
|
||||
gap: 8px;
|
||||
}
|
||||
|
||||
.dot {
|
||||
width: 12px;
|
||||
height: 12px;
|
||||
flex: 0 0 12px;
|
||||
border-radius: 999px;
|
||||
background: var(--warn);
|
||||
}
|
||||
|
||||
.dot.good {
|
||||
background: var(--good);
|
||||
}
|
||||
|
||||
.dot.bad {
|
||||
background: var(--bad);
|
||||
}
|
||||
|
||||
.grid {
|
||||
display: grid;
|
||||
grid-template-columns: repeat(2, minmax(0, 1fr));
|
||||
gap: 16px;
|
||||
}
|
||||
|
||||
section h2 {
|
||||
margin: 0;
|
||||
padding: 14px 16px;
|
||||
border-bottom: 1px solid var(--line);
|
||||
font-size: 16px;
|
||||
font-weight: 650;
|
||||
}
|
||||
|
||||
dl {
|
||||
display: grid;
|
||||
grid-template-columns: 160px minmax(0, 1fr);
|
||||
gap: 0;
|
||||
margin: 0;
|
||||
padding: 4px 16px 12px;
|
||||
}
|
||||
|
||||
dt,
|
||||
dd {
|
||||
min-height: 36px;
|
||||
margin: 0;
|
||||
padding: 9px 0;
|
||||
border-bottom: 1px solid #edf1f7;
|
||||
}
|
||||
|
||||
dt {
|
||||
color: var(--muted);
|
||||
}
|
||||
|
||||
dd {
|
||||
overflow-wrap: anywhere;
|
||||
font-family: Consolas, "Cascadia Mono", monospace;
|
||||
}
|
||||
|
||||
.empty {
|
||||
color: var(--muted);
|
||||
font-family: inherit;
|
||||
}
|
||||
|
||||
.jog-panel {
|
||||
grid-column: 1 / -1;
|
||||
overflow: hidden;
|
||||
}
|
||||
|
||||
.jog-toolbar {
|
||||
display: grid;
|
||||
grid-template-columns: repeat(2, minmax(180px, 1fr)) minmax(220px, 1fr);
|
||||
gap: 12px;
|
||||
padding: 16px;
|
||||
border-bottom: 1px solid var(--line);
|
||||
background: #fbfcfe;
|
||||
}
|
||||
|
||||
.step-field {
|
||||
display: grid;
|
||||
gap: 6px;
|
||||
color: var(--muted);
|
||||
font-size: 13px;
|
||||
}
|
||||
|
||||
.step-field input {
|
||||
min-height: 38px;
|
||||
width: 100%;
|
||||
padding: 0 10px;
|
||||
border: 1px solid var(--line);
|
||||
border-radius: 6px;
|
||||
background: #ffffff;
|
||||
color: var(--text);
|
||||
font: inherit;
|
||||
}
|
||||
|
||||
.jog-body {
|
||||
display: grid;
|
||||
grid-template-columns: minmax(0, 1fr) minmax(0, 1fr);
|
||||
gap: 18px;
|
||||
padding: 16px;
|
||||
}
|
||||
|
||||
.stick-card {
|
||||
min-width: 0;
|
||||
border: 1px solid #ccd4df;
|
||||
border-radius: 8px;
|
||||
background: linear-gradient(180deg, #ffffff 0%, #f3f6fa 100%);
|
||||
box-shadow: inset 0 1px 0 rgba(255, 255, 255, 0.9);
|
||||
}
|
||||
|
||||
.stick-head {
|
||||
display: flex;
|
||||
align-items: center;
|
||||
justify-content: space-between;
|
||||
gap: 12px;
|
||||
padding: 14px 16px 0;
|
||||
}
|
||||
|
||||
.stick-head h3 {
|
||||
margin: 0;
|
||||
font-size: 16px;
|
||||
font-weight: 650;
|
||||
}
|
||||
|
||||
.stick-head span {
|
||||
color: var(--muted);
|
||||
font-family: Consolas, "Cascadia Mono", monospace;
|
||||
font-size: 13px;
|
||||
}
|
||||
|
||||
.stick-console {
|
||||
display: grid;
|
||||
grid-template-columns: 72px 1fr 72px;
|
||||
grid-template-rows: 72px 72px 72px;
|
||||
gap: 8px;
|
||||
width: min(100%, 344px);
|
||||
margin: 18px auto 16px;
|
||||
padding: 14px;
|
||||
border: 1px solid #c2cad7;
|
||||
border-radius: 999px;
|
||||
background:
|
||||
radial-gradient(circle at 50% 50%, #ffffff 0 26%, #e6ebf2 27% 48%, #cdd5e0 49% 50%, #edf1f6 51% 100%);
|
||||
}
|
||||
|
||||
.jog-button {
|
||||
min-width: 44px;
|
||||
min-height: 44px;
|
||||
color: var(--steel);
|
||||
font-weight: 650;
|
||||
touch-action: none;
|
||||
user-select: none;
|
||||
}
|
||||
|
||||
.axis-button,
|
||||
.mini-button {
|
||||
border: 1px solid #99a8ba;
|
||||
border-radius: 999px;
|
||||
background: linear-gradient(180deg, #ffffff 0%, #e5ebf2 100%);
|
||||
box-shadow: 0 2px 0 #b7c1ce;
|
||||
}
|
||||
|
||||
.axis-button {
|
||||
min-width: 0;
|
||||
min-height: 64px;
|
||||
padding: 0 8px;
|
||||
}
|
||||
|
||||
.mini-button {
|
||||
padding: 0 10px;
|
||||
background: var(--surface);
|
||||
}
|
||||
|
||||
.axis-button:hover,
|
||||
.axis-button.active {
|
||||
border-color: var(--accent);
|
||||
background: linear-gradient(180deg, #10a4b3 0%, var(--accent) 100%);
|
||||
color: #ffffff;
|
||||
box-shadow: 0 2px 0 var(--accent-dark);
|
||||
}
|
||||
|
||||
.mini-button:hover,
|
||||
.mini-button.active {
|
||||
border-color: var(--warn);
|
||||
background: var(--warn);
|
||||
color: #ffffff;
|
||||
box-shadow: 0 2px 0 #8a5a13;
|
||||
}
|
||||
|
||||
.jog-button:focus-visible {
|
||||
outline: 3px solid rgba(0, 124, 137, 0.3);
|
||||
outline-offset: 2px;
|
||||
}
|
||||
|
||||
.jog-button.active {
|
||||
transform: translateY(1px);
|
||||
}
|
||||
|
||||
.axis-button.up {
|
||||
grid-column: 2;
|
||||
grid-row: 1;
|
||||
}
|
||||
|
||||
.axis-button.left {
|
||||
grid-column: 1;
|
||||
grid-row: 2;
|
||||
}
|
||||
|
||||
.axis-button.right {
|
||||
grid-column: 3;
|
||||
grid-row: 2;
|
||||
}
|
||||
|
||||
.axis-button.down {
|
||||
grid-column: 2;
|
||||
grid-row: 3;
|
||||
}
|
||||
|
||||
.center-stack {
|
||||
display: grid;
|
||||
grid-column: 2;
|
||||
grid-row: 2;
|
||||
grid-template-columns: 1fr 1fr;
|
||||
gap: 6px;
|
||||
place-items: center;
|
||||
padding: 8px;
|
||||
border-radius: 999px;
|
||||
background: rgba(47, 59, 79, 0.08);
|
||||
}
|
||||
|
||||
.hint-row {
|
||||
display: grid;
|
||||
grid-template-columns: repeat(3, minmax(0, 1fr));
|
||||
gap: 8px;
|
||||
padding: 0 16px 16px;
|
||||
color: var(--muted);
|
||||
font-size: 13px;
|
||||
text-align: center;
|
||||
}
|
||||
|
||||
.hint-row span {
|
||||
min-height: 28px;
|
||||
padding: 5px 8px;
|
||||
border-radius: 999px;
|
||||
background: var(--steel-soft);
|
||||
white-space: nowrap;
|
||||
}
|
||||
|
||||
.jog-status {
|
||||
display: flex;
|
||||
align-items: center;
|
||||
justify-content: flex-end;
|
||||
min-height: 38px;
|
||||
color: var(--muted);
|
||||
font-family: Consolas, "Cascadia Mono", monospace;
|
||||
text-align: right;
|
||||
}
|
||||
|
||||
.jog-status.error {
|
||||
color: var(--bad);
|
||||
}
|
||||
|
||||
.log-panel {
|
||||
margin: 0 16px 16px;
|
||||
overflow: hidden;
|
||||
border: 1px solid var(--line);
|
||||
border-radius: 8px;
|
||||
background: #fbfcfe;
|
||||
}
|
||||
|
||||
.log-head {
|
||||
display: flex;
|
||||
align-items: center;
|
||||
justify-content: space-between;
|
||||
gap: 12px;
|
||||
padding: 12px 14px;
|
||||
border-bottom: 1px solid var(--line);
|
||||
background: var(--surface);
|
||||
}
|
||||
|
||||
.log-head h3 {
|
||||
margin: 0;
|
||||
font-size: 15px;
|
||||
font-weight: 650;
|
||||
}
|
||||
|
||||
.clear-button {
|
||||
min-height: 34px;
|
||||
padding: 0 12px;
|
||||
border-color: var(--line);
|
||||
background: #ffffff;
|
||||
color: var(--muted);
|
||||
}
|
||||
|
||||
.clear-button:hover {
|
||||
border-color: var(--accent);
|
||||
color: var(--accent);
|
||||
}
|
||||
|
||||
.request-log {
|
||||
display: grid;
|
||||
gap: 8px;
|
||||
max-height: 300px;
|
||||
min-height: 54px;
|
||||
margin: 0;
|
||||
padding: 12px 14px 14px;
|
||||
overflow: auto;
|
||||
font-family: Consolas, "Cascadia Mono", monospace;
|
||||
font-size: 13px;
|
||||
}
|
||||
|
||||
.log-entry {
|
||||
display: grid;
|
||||
grid-template-columns: 88px 96px minmax(0, 1fr);
|
||||
gap: 10px;
|
||||
align-items: start;
|
||||
padding: 9px 10px;
|
||||
border: 1px solid #e1e7ef;
|
||||
border-radius: 6px;
|
||||
background: #ffffff;
|
||||
}
|
||||
|
||||
.log-entry:first-child {
|
||||
border-color: rgba(0, 124, 137, 0.35);
|
||||
background: rgba(0, 124, 137, 0.04);
|
||||
}
|
||||
|
||||
.log-entry.error {
|
||||
border-color: rgba(180, 35, 24, 0.35);
|
||||
background: rgba(180, 35, 24, 0.06);
|
||||
}
|
||||
|
||||
.log-time,
|
||||
.log-axis {
|
||||
color: var(--muted);
|
||||
white-space: nowrap;
|
||||
}
|
||||
|
||||
.log-payload {
|
||||
overflow-wrap: anywhere;
|
||||
color: var(--text);
|
||||
}
|
||||
|
||||
.log-empty {
|
||||
color: var(--muted);
|
||||
}
|
||||
|
||||
@media (max-width: 820px) {
|
||||
.topbar {
|
||||
align-items: flex-start;
|
||||
flex-direction: column;
|
||||
}
|
||||
|
||||
.summary,
|
||||
.grid {
|
||||
grid-template-columns: 1fr;
|
||||
}
|
||||
|
||||
.jog-toolbar,
|
||||
.jog-body {
|
||||
grid-template-columns: 1fr;
|
||||
}
|
||||
|
||||
.jog-status {
|
||||
justify-content: flex-start;
|
||||
text-align: left;
|
||||
}
|
||||
|
||||
dl {
|
||||
grid-template-columns: 1fr;
|
||||
}
|
||||
|
||||
dt {
|
||||
border-bottom: 0;
|
||||
padding-bottom: 2px;
|
||||
}
|
||||
|
||||
dd {
|
||||
padding-top: 2px;
|
||||
}
|
||||
}
|
||||
|
||||
@media (max-width: 520px) {
|
||||
.stick-console {
|
||||
grid-template-columns: 60px 1fr 60px;
|
||||
grid-template-rows: 60px 60px 60px;
|
||||
padding: 10px;
|
||||
}
|
||||
|
||||
.axis-button {
|
||||
min-height: 56px;
|
||||
}
|
||||
|
||||
.log-entry {
|
||||
grid-template-columns: 1fr;
|
||||
}
|
||||
}
|
||||
333
src/Flyshot.Server.Host/wwwroot/assets/status.js
Normal file
333
src/Flyshot.Server.Host/wwwroot/assets/status.js
Normal file
@@ -0,0 +1,333 @@
|
||||
const fields = {
|
||||
connectionState: document.getElementById("connection-state"),
|
||||
stateDot: document.getElementById("state-dot"),
|
||||
robotName: document.getElementById("robot-name"),
|
||||
speedRatio: document.getElementById("speed-ratio"),
|
||||
motionState: document.getElementById("motion-state"),
|
||||
serverVersion: document.getElementById("server-version"),
|
||||
clientVersion: document.getElementById("client-version"),
|
||||
setupState: document.getElementById("setup-state"),
|
||||
enabledState: document.getElementById("enabled-state"),
|
||||
j519Status: document.getElementById("j519-status"),
|
||||
j519Sequence: document.getElementById("j519-sequence"),
|
||||
capturedAt: document.getElementById("captured-at"),
|
||||
dof: document.getElementById("dof"),
|
||||
joints: document.getElementById("joints"),
|
||||
pose: document.getElementById("pose"),
|
||||
trajectories: document.getElementById("trajectories"),
|
||||
refresh: document.getElementById("refresh"),
|
||||
linearStep: document.getElementById("linear-step"),
|
||||
angularStep: document.getElementById("angular-step"),
|
||||
jogStatus: document.getElementById("jog-status"),
|
||||
requestLog: document.getElementById("request-log"),
|
||||
clearLog: document.getElementById("clear-log"),
|
||||
jogButtons: Array.from(document.querySelectorAll(".jog-button"))
|
||||
};
|
||||
|
||||
const axisIndexes = {
|
||||
x: 0,
|
||||
y: 1,
|
||||
z: 2,
|
||||
w: 3,
|
||||
p: 4,
|
||||
r: 5
|
||||
};
|
||||
|
||||
// 点动状态集中保存，确保按住按钮时不会并发发送多条 MovePose 请求。
|
||||
const jogState = {
|
||||
timer: null,
|
||||
activeButton: null,
|
||||
inFlight: false,
|
||||
lastSnapshot: null,
|
||||
sequence: 0,
|
||||
logs: []
|
||||
};
|
||||
|
||||
const maxJogLogs = 80;
|
||||
const jogIntervalMs = 250;
|
||||
|
||||
function formatArray(values) {
|
||||
if (!Array.isArray(values) || values.length === 0) {
|
||||
return "--";
|
||||
}
|
||||
|
||||
return values.map(value => Number(value).toFixed(4)).join(", ");
|
||||
}
|
||||
|
||||
function formatNullableBool(value) {
|
||||
if (value === true) {
|
||||
return "是";
|
||||
}
|
||||
|
||||
if (value === false) {
|
||||
return "否";
|
||||
}
|
||||
|
||||
return "--";
|
||||
}
|
||||
|
||||
function formatJ519Status(snapshot) {
|
||||
if (snapshot.j519Status === null || snapshot.j519Status === undefined) {
|
||||
return "--";
|
||||
}
|
||||
|
||||
const status = Number(snapshot.j519Status).toString(16).padStart(2, "0").toUpperCase();
|
||||
return `0x${status} accept=${formatNullableBool(snapshot.j519AcceptsCommand)} received=${formatNullableBool(snapshot.j519ReceivedCommand)} sysrdy=${formatNullableBool(snapshot.j519SystemReady)} motion=${formatNullableBool(snapshot.j519RobotInMotion)}`;
|
||||
}
|
||||
|
||||
function setDot(connectionState) {
|
||||
fields.stateDot.className = "dot";
|
||||
if (connectionState === "Connected") {
|
||||
fields.stateDot.classList.add("good");
|
||||
} else if (connectionState === "NotConfigured") {
|
||||
fields.stateDot.classList.add("bad");
|
||||
}
|
||||
}
|
||||
|
||||
function clampStep(input, min, max) {
|
||||
const value = Number(input.value);
|
||||
if (!Number.isFinite(value)) {
|
||||
input.value = String(min);
|
||||
return min;
|
||||
}
|
||||
|
||||
const clamped = Math.min(Math.max(value, min), max);
|
||||
input.value = String(clamped);
|
||||
return clamped;
|
||||
}
|
||||
|
||||
function getStepForAxis(axis) {
|
||||
// 平移和姿态使用不同单位，但共享 0.1 到 20 的现场可调范围。
|
||||
return axis === "x" || axis === "y" || axis === "z"
|
||||
? clampStep(fields.linearStep, 0.1, 20)
|
||||
: clampStep(fields.angularStep, 0.1, 20);
|
||||
}
|
||||
|
||||
function setJogStatus(message, isError) {
|
||||
fields.jogStatus.textContent = message;
|
||||
fields.jogStatus.classList.toggle("error", Boolean(isError));
|
||||
}
|
||||
|
||||
function formatDirection(axis, direction) {
|
||||
return `${axis.toUpperCase()}${direction > 0 ? "+" : "-"}`;
|
||||
}
|
||||
|
||||
function renderJogLogs() {
|
||||
fields.requestLog.replaceChildren();
|
||||
|
||||
if (jogState.logs.length === 0) {
|
||||
const empty = document.createElement("div");
|
||||
empty.className = "log-empty";
|
||||
empty.textContent = "暂无请求记录";
|
||||
fields.requestLog.appendChild(empty);
|
||||
return;
|
||||
}
|
||||
|
||||
jogState.logs.forEach(function (entry) {
|
||||
const row = document.createElement("div");
|
||||
row.className = `log-entry${entry.ok ? "" : " error"}`;
|
||||
|
||||
const time = document.createElement("span");
|
||||
time.className = "log-time";
|
||||
time.textContent = entry.time;
|
||||
|
||||
const axis = document.createElement("span");
|
||||
axis.className = "log-axis";
|
||||
axis.textContent = `#${entry.sequence} ${entry.direction}`;
|
||||
|
||||
const payload = document.createElement("span");
|
||||
payload.className = "log-payload";
|
||||
payload.textContent = `step=${entry.step} ${entry.unit}; body=${entry.body}; ${entry.result}`;
|
||||
|
||||
row.append(time, axis, payload);
|
||||
fields.requestLog.appendChild(row);
|
||||
});
|
||||
}
|
||||
|
||||
function addJogLog(entry) {
|
||||
jogState.logs.unshift(entry);
|
||||
if (jogState.logs.length > maxJogLogs) {
|
||||
jogState.logs.length = maxJogLogs;
|
||||
}
|
||||
|
||||
renderJogLogs();
|
||||
}
|
||||
|
||||
async function loadSnapshotForJog() {
|
||||
const response = await fetch("/api/status/snapshot", { cache: "no-store" });
|
||||
if (!response.ok) {
|
||||
throw new Error(`状态快照读取失败: HTTP ${response.status}`);
|
||||
}
|
||||
|
||||
const payload = await response.json();
|
||||
jogState.lastSnapshot = payload.snapshot;
|
||||
return payload.snapshot;
|
||||
}
|
||||
|
||||
function buildJogPose(snapshot, axis, direction) {
|
||||
// 每次点动都从最新 TCP 位姿出发，只修改一个轴，避免连续按压时累积本地误差。
|
||||
const sourcePose = Array.isArray(snapshot.cartesianPose) ? snapshot.cartesianPose : [];
|
||||
if (sourcePose.length < 6) {
|
||||
throw new Error("当前 TCP 位姿不足 6 维，无法点动。");
|
||||
}
|
||||
|
||||
const pose = sourcePose.slice(0, 6).map(Number);
|
||||
if (pose.some(value => !Number.isFinite(value))) {
|
||||
throw new Error("当前 TCP 位姿包含非数值，无法点动。");
|
||||
}
|
||||
|
||||
const axisIndex = axisIndexes[axis];
|
||||
pose[axisIndex] = Number((pose[axisIndex] + direction * getStepForAxis(axis)).toFixed(6));
|
||||
return {
|
||||
x: pose[0],
|
||||
y: pose[1],
|
||||
z: pose[2],
|
||||
w: pose[3],
|
||||
p: pose[4],
|
||||
r: pose[5]
|
||||
};
|
||||
}
|
||||
|
||||
async function sendJog(button) {
|
||||
// MovePose 底层会生成完整直角轨迹；前端这里只负责构造增量目标位姿。
|
||||
if (jogState.inFlight) {
|
||||
return;
|
||||
}
|
||||
|
||||
const axis = button.dataset.axis;
|
||||
const direction = Number(button.dataset.direction);
|
||||
if (!Object.prototype.hasOwnProperty.call(axisIndexes, axis) || !Number.isFinite(direction)) {
|
||||
return;
|
||||
}
|
||||
|
||||
jogState.inFlight = true;
|
||||
const step = getStepForAxis(axis);
|
||||
const directionLabel = formatDirection(axis, direction);
|
||||
const sequence = ++jogState.sequence;
|
||||
let bodyText = "{}";
|
||||
let resultText = "";
|
||||
let ok = false;
|
||||
|
||||
try {
|
||||
const snapshot = await loadSnapshotForJog();
|
||||
const pose = buildJogPose(snapshot, axis, direction);
|
||||
bodyText = JSON.stringify(pose);
|
||||
const response = await fetch("/move_pose/", {
|
||||
method: "POST",
|
||||
headers: { "Content-Type": "application/json" },
|
||||
body: bodyText
|
||||
});
|
||||
|
||||
if (!response.ok) {
|
||||
throw new Error(`MovePose 调用失败: HTTP ${response.status}`);
|
||||
}
|
||||
|
||||
ok = true;
|
||||
resultText = `HTTP ${response.status}`;
|
||||
setJogStatus(`${directionLabel} 已发送`, false);
|
||||
await refreshStatus();
|
||||
} catch (error) {
|
||||
resultText = error instanceof Error ? error.message : "点动失败";
|
||||
setJogStatus(resultText, true);
|
||||
stopJog();
|
||||
} finally {
|
||||
addJogLog({
|
||||
time: new Date().toLocaleTimeString(),
|
||||
sequence,
|
||||
direction: directionLabel,
|
||||
step,
|
||||
unit: axis === "x" || axis === "y" || axis === "z" ? "mm" : "deg",
|
||||
body: bodyText,
|
||||
result: resultText,
|
||||
ok
|
||||
});
|
||||
jogState.inFlight = false;
|
||||
}
|
||||
}
|
||||
|
||||
function startJog(event) {
|
||||
const button = event.currentTarget;
|
||||
if (!button || button.disabled) {
|
||||
return;
|
||||
}
|
||||
|
||||
event.preventDefault();
|
||||
stopJog();
|
||||
jogState.activeButton = button;
|
||||
button.classList.add("active");
|
||||
sendJog(button);
|
||||
jogState.timer = window.setInterval(function () {
|
||||
sendJog(button);
|
||||
}, jogIntervalMs);
|
||||
}
|
||||
|
||||
function stopJog() {
|
||||
if (jogState.timer !== null) {
|
||||
window.clearInterval(jogState.timer);
|
||||
jogState.timer = null;
|
||||
}
|
||||
|
||||
if (jogState.activeButton) {
|
||||
jogState.activeButton.classList.remove("active");
|
||||
jogState.activeButton = null;
|
||||
}
|
||||
}
|
||||
|
||||
async function refreshStatus() {
|
||||
fields.refresh.disabled = true;
|
||||
try {
|
||||
const response = await fetch("/api/status/snapshot", { cache: "no-store" });
|
||||
const payload = await response.json();
|
||||
const snapshot = payload.snapshot;
|
||||
jogState.lastSnapshot = snapshot;
|
||||
|
||||
fields.connectionState.textContent = snapshot.connectionState;
|
||||
fields.robotName.textContent = payload.robotName || "--";
|
||||
fields.speedRatio.textContent = Number(snapshot.speedRatio).toFixed(2);
|
||||
fields.motionState.textContent = snapshot.isInMotion ? "是" : "否";
|
||||
fields.serverVersion.textContent = payload.serverVersion;
|
||||
fields.clientVersion.textContent = payload.clientVersion;
|
||||
fields.setupState.textContent = payload.isSetup ? "是" : "否";
|
||||
fields.enabledState.textContent = snapshot.isEnabled ? "是" : "否";
|
||||
fields.j519Status.textContent = formatJ519Status(snapshot);
|
||||
fields.j519Sequence.textContent = snapshot.j519Sequence ?? "--";
|
||||
fields.capturedAt.textContent = new Date(snapshot.capturedAt).toLocaleString();
|
||||
fields.dof.textContent = payload.degreesOfFreedom;
|
||||
fields.joints.textContent = formatArray(snapshot.jointPositions);
|
||||
fields.pose.textContent = formatArray(snapshot.cartesianPose);
|
||||
fields.trajectories.textContent = payload.uploadedTrajectories.length > 0
|
||||
? payload.uploadedTrajectories.join(", ")
|
||||
: "--";
|
||||
fields.trajectories.classList.toggle("empty", payload.uploadedTrajectories.length === 0);
|
||||
setDot(snapshot.connectionState);
|
||||
} finally {
|
||||
fields.refresh.disabled = false;
|
||||
}
|
||||
}
|
||||
|
||||
fields.refresh.addEventListener("click", refreshStatus);
|
||||
fields.linearStep.addEventListener("change", function () {
|
||||
clampStep(fields.linearStep, 0.1, 20);
|
||||
});
|
||||
fields.angularStep.addEventListener("change", function () {
|
||||
clampStep(fields.angularStep, 0.1, 20);
|
||||
});
|
||||
fields.clearLog.addEventListener("click", function () {
|
||||
jogState.logs = [];
|
||||
renderJogLogs();
|
||||
});
|
||||
fields.jogButtons.forEach(function (button) {
|
||||
button.addEventListener("pointerdown", startJog);
|
||||
button.addEventListener("pointerup", stopJog);
|
||||
button.addEventListener("pointercancel", stopJog);
|
||||
button.addEventListener("pointerleave", stopJog);
|
||||
});
|
||||
window.addEventListener("blur", stopJog);
|
||||
window.addEventListener("keyup", function (event) {
|
||||
if (event.key === "Escape") {
|
||||
stopJog();
|
||||
}
|
||||
});
|
||||
renderJogLogs();
|
||||
refreshStatus();
|
||||
window.setInterval(refreshStatus, 2000);
|
||||
44
src/Flyshot.Server.Host/wwwroot/debug.html
Normal file
44
src/Flyshot.Server.Host/wwwroot/debug.html
Normal file
@@ -0,0 +1,44 @@
|
||||
<!doctype html>
|
||||
<html lang="zh-CN">
|
||||
<head>
|
||||
<meta charset="utf-8">
|
||||
<meta name="viewport" content="width=device-width, initial-scale=1">
|
||||
<title>Flyshot Replacement 接口调试</title>
|
||||
<link rel="stylesheet" href="/assets/debug.css">
|
||||
</head>
|
||||
<body>
|
||||
<header>
|
||||
<div class="topbar">
|
||||
<h1>Flyshot Replacement 接口调试</h1>
|
||||
<div class="actions">
|
||||
<a class="link-button" href="/status.html">回到状态页</a>
|
||||
<a class="link-button" href="/swagger" target="_blank" rel="noopener">Swagger UI</a>
|
||||
<button id="reload-spec" type="button">重新加载 OpenAPI</button>
|
||||
</div>
|
||||
</div>
|
||||
</header>
|
||||
<main>
|
||||
<section class="meta">
|
||||
<dl>
|
||||
<dt>OpenAPI 文档</dt>
|
||||
<dd id="meta-spec-url">--</dd>
|
||||
<dt>API 数量</dt>
|
||||
<dd id="meta-operation-count">--</dd>
|
||||
<dt>加载状态</dt>
|
||||
<dd id="meta-status">初始化中...</dd>
|
||||
</dl>
|
||||
</section>
|
||||
<div id="debug-console-app">
|
||||
<div class="empty-hint">正在加载接口列表...</div>
|
||||
</div>
|
||||
</main>
|
||||
<aside class="history" id="history-panel">
|
||||
<h3>
|
||||
<span>调用历史 (本次会话)</span>
|
||||
<button type="button" id="history-clear" class="secondary">清空</button>
|
||||
</h3>
|
||||
<ul id="history-list"></ul>
|
||||
</aside>
|
||||
<script src="/assets/debug.js" defer></script>
|
||||
</body>
|
||||
</html>
|
||||
131
src/Flyshot.Server.Host/wwwroot/status.html
Normal file
131
src/Flyshot.Server.Host/wwwroot/status.html
Normal file
@@ -0,0 +1,131 @@
|
||||
<!doctype html>
|
||||
<html lang="zh-CN">
|
||||
<head>
|
||||
<meta charset="utf-8">
|
||||
<meta name="viewport" content="width=device-width, initial-scale=1">
|
||||
<title>Flyshot Replacement 状态监控</title>
|
||||
<link rel="stylesheet" href="/assets/status.css">
|
||||
</head>
|
||||
<body>
|
||||
<header>
|
||||
<div class="topbar">
|
||||
<h1>Flyshot Replacement 状态监控</h1>
|
||||
<div class="actions">
|
||||
<a class="link-button" href="/debug.html" target="_blank" rel="noopener">调试接口</a>
|
||||
<button id="refresh" type="button">刷新</button>
|
||||
</div>
|
||||
</div>
|
||||
</header>
|
||||
<main>
|
||||
<div class="summary">
|
||||
<div class="metric">
|
||||
<div class="label">连接状态</div>
|
||||
<div class="value status-row"><span id="state-dot" class="dot"></span><span id="connection-state">--</span></div>
|
||||
</div>
|
||||
<div class="metric">
|
||||
<div class="label">机器人</div>
|
||||
<div id="robot-name" class="value">--</div>
|
||||
</div>
|
||||
<div class="metric">
|
||||
<div class="label">速度倍率</div>
|
||||
<div id="speed-ratio" class="value">--</div>
|
||||
</div>
|
||||
<div class="metric">
|
||||
<div class="label">运动中</div>
|
||||
<div id="motion-state" class="value">--</div>
|
||||
</div>
|
||||
</div>
|
||||
<div class="grid">
|
||||
<section>
|
||||
<h2>控制器</h2>
|
||||
<dl>
|
||||
<dt>服务端版本</dt><dd id="server-version">--</dd>
|
||||
<dt>客户端版本</dt><dd id="client-version">--</dd>
|
||||
<dt>已初始化</dt><dd id="setup-state">--</dd>
|
||||
<dt>已使能</dt><dd id="enabled-state">--</dd>
|
||||
<dt>J519 状态</dt><dd id="j519-status">--</dd>
|
||||
<dt>J519 序号</dt><dd id="j519-sequence">--</dd>
|
||||
<dt>采样时间</dt><dd id="captured-at">--</dd>
|
||||
</dl>
|
||||
</section>
|
||||
<section>
|
||||
<h2>机器人</h2>
|
||||
<dl>
|
||||
<dt>自由度</dt><dd id="dof">--</dd>
|
||||
<dt>关节位置</dt><dd id="joints">--</dd>
|
||||
<dt>TCP 位姿</dt><dd id="pose">--</dd>
|
||||
<dt>已上传轨迹</dt><dd id="trajectories" class="empty">--</dd>
|
||||
</dl>
|
||||
</section>
|
||||
<section class="jog-panel">
|
||||
<h2>直角坐标点动</h2>
|
||||
<div class="jog-toolbar">
|
||||
<label class="step-field" for="linear-step">
|
||||
<span>平移步长 mm</span>
|
||||
<input id="linear-step" type="number" min="0.1" max="20" step="0.1" value="1">
|
||||
</label>
|
||||
<label class="step-field" for="angular-step">
|
||||
<span>姿态步长 deg</span>
|
||||
<input id="angular-step" type="number" min="0.1" max="20" step="0.1" value="1">
|
||||
</label>
|
||||
<div id="jog-status" class="jog-status">等待点动输入</div>
|
||||
</div>
|
||||
<div class="jog-body">
|
||||
<article class="stick-card">
|
||||
<div class="stick-head">
|
||||
<h3>XYZ 平移摇杆</h3>
|
||||
<span>mm</span>
|
||||
</div>
|
||||
<div class="stick-console" aria-label="XYZ 平移摇杆">
|
||||
<button class="jog-button axis-button up" type="button" data-axis="y" data-direction="1">Y+</button>
|
||||
<button class="jog-button axis-button left" type="button" data-axis="x" data-direction="-1">X-</button>
|
||||
<div class="center-stack">
|
||||
<button class="jog-button mini-button" type="button" data-axis="z" data-direction="1">Z+</button>
|
||||
<button class="jog-button mini-button" type="button" data-axis="z" data-direction="-1">Z-</button>
|
||||
</div>
|
||||
<button class="jog-button axis-button right" type="button" data-axis="x" data-direction="1">X+</button>
|
||||
<button class="jog-button axis-button down" type="button" data-axis="y" data-direction="-1">Y-</button>
|
||||
</div>
|
||||
<div class="hint-row">
|
||||
<span>X 左右</span>
|
||||
<span>Y 上下</span>
|
||||
<span>Z 中心</span>
|
||||
</div>
|
||||
</article>
|
||||
<article class="stick-card">
|
||||
<div class="stick-head">
|
||||
<h3>WPR 姿态摇杆</h3>
|
||||
<span>deg</span>
|
||||
</div>
|
||||
<div class="stick-console" aria-label="WPR 姿态摇杆">
|
||||
<button class="jog-button axis-button up" type="button" data-axis="p" data-direction="1">P+</button>
|
||||
<button class="jog-button axis-button left" type="button" data-axis="w" data-direction="-1">W-</button>
|
||||
<div class="center-stack">
|
||||
<button class="jog-button mini-button" type="button" data-axis="r" data-direction="-1">R-</button>
|
||||
<button class="jog-button mini-button" type="button" data-axis="r" data-direction="1">R+</button>
|
||||
</div>
|
||||
<button class="jog-button axis-button right" type="button" data-axis="w" data-direction="1">W+</button>
|
||||
<button class="jog-button axis-button down" type="button" data-axis="p" data-direction="-1">P-</button>
|
||||
</div>
|
||||
<div class="hint-row">
|
||||
<span>W 左右</span>
|
||||
<span>P 上下</span>
|
||||
<span>R 中心</span>
|
||||
</div>
|
||||
</article>
|
||||
</div>
|
||||
<div class="log-panel">
|
||||
<div class="log-head">
|
||||
<h3>请求日志</h3>
|
||||
<button id="clear-log" class="clear-button" type="button">清空</button>
|
||||
</div>
|
||||
<div id="request-log" class="request-log" aria-live="polite">
|
||||
<div class="log-empty">暂无请求记录</div>
|
||||
</div>
|
||||
</div>
|
||||
</section>
|
||||
</div>
|
||||
</main>
|
||||
<script src="/assets/status.js" defer></script>
|
||||
</body>
|
||||
</html>
|
||||
Reference in New Issue
Block a user