xsxiaosa/FlyShotHost
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

✨ feat(fanuc): 添加直角坐标点动功能与相关接口

Browse Source 
* 新增 `MovePose` 方法，支持以直角坐标执行点到点移动。
* 引入 `LegacyCartesianPoseRequest` 类，处理直角位姿请求体的解析与验证。
* 更新 `LegacyHttpApiController`，实现 `/move_pose/` 路由以支持新功能。
* 增强状态快照元数据，提供机器人初始化状态与已上传轨迹信息。
* 更新前端状态页面，增加直角坐标点动控制面板与步长设置选项。
* 相关文档与测试用例同步更新，确保新功能的完整性与稳定性。
This commit is contained in:
yunxiao.zhu
2026-05-14 17:46:42 +08:00
parent d120ef2a39
commit 2cd42f04e5
22 changed files with 2062 additions and 104 deletions
Download Patch File Download Diff File Expand all files Collapse all files

334
src/Flyshot.ControllerClientCompat/ControllerClientCompatService.cs
View File

@@ -12,6 +12,7 @@ namespace Flyshot.ControllerClientCompat;
public sealed class ControllerClientCompatService : IControllerClientCompatService
{
private readonly object _stateLock = new();
private readonly object _motionLock = new();
private readonly Dictionary<string, ControllerClientCompatUploadedTrajectory> _uploadedTrajectories = new(StringComparer.Ordinal);
private readonly ControllerClientCompatOptions _options;
private readonly ControllerClientCompatRobotCatalog _robotCatalog;
@@ -197,8 +198,9 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi
lock (_stateLock)
{
EnsureRobotSetup();
_runtime.SetActiveController(sim);
}
_runtime.SetActiveController(sim);
}
/// <inheritdoc />
@@ -214,9 +216,10 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi
lock (_stateLock)
{
EnsureRobotSetup();
_runtime.Connect(robotIp);
}
_runtime.Connect(robotIp);
_logger?.LogInformation("Connect 完成: robotIp={RobotIp}", robotIp);
}
@@ -226,8 +229,9 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi
lock (_stateLock)
{
EnsureRobotSetup();
_runtime.Disconnect();
}
_runtime.Disconnect();
}
/// <inheritdoc />
@@ -237,8 +241,8 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi
lock (_stateLock)
{
EnsureRobotSetup();
_runtime.EnableRobot(bufferSize);
}
_runtime.EnableRobot(bufferSize);
_logger?.LogInformation("EnableRobot 完成");
}
@@ -249,8 +253,10 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi
lock (_stateLock)
{
EnsureRobotSetup();
_runtime.DisableRobot();
}
// DisableRobot 是中断/控制命令，不能被长时间运动串行锁挡住。
_runtime.DisableRobot();
_logger?.LogInformation("DisableRobot 完成");
}
@@ -261,8 +267,10 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi
lock (_stateLock)
{
EnsureRobotSetup();
_runtime.StopMove();
}
// StopMove 必须能在运动执行期间直接进入 runtime 取消发送任务。
_runtime.StopMove();
_logger?.LogInformation("StopMove 完成");
}
@@ -272,14 +280,31 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi
return _runtime.GetSnapshot();
}
/// <inheritdoc />
public ControllerClientStatusSnapshotMetadata GetStatusSnapshotMetadata()
{
lock (_stateLock)
{
var isSetup = _activeRobotProfile is not null;
return new ControllerClientStatusSnapshotMetadata(
isSetup,
isSetup ? _configuredRobotName : null,
isSetup ? _activeRobotProfile!.DegreesOfFreedom : 0,
isSetup ? _uploadedTrajectories.Keys.ToArray() : Array.Empty<string>(),
GetServerVersion(),
GetClientVersion());
}
}
/// <inheritdoc />
public double GetSpeedRatio()
{
lock (_stateLock)
{
EnsureRobotSetup();
return _runtime.GetSpeedRatio();
}
return _runtime.GetSpeedRatio();
}
/// <inheritdoc />
@@ -288,8 +313,9 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi
lock (_stateLock)
{
EnsureRobotSetup();
_runtime.SetSpeedRatio(ratio);
}
_runtime.SetSpeedRatio(ratio);
}
/// <inheritdoc />
@@ -298,8 +324,9 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi
lock (_stateLock)
{
EnsureRobotSetup();
_runtime.SetIo(port, value, ioType);
}
_runtime.SetIo(port, value, ioType);
}
/// <inheritdoc />
@@ -308,8 +335,9 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi
lock (_stateLock)
{
EnsureRobotSetup();
return _runtime.GetIo(port, ioType);
}
return _runtime.GetIo(port, ioType);
}
/// <inheritdoc />
@@ -341,8 +369,9 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi
lock (_stateLock)
{
EnsureRobotSetup();
_runtime.SetTcp(x, y, z);
}
_runtime.SetTcp(x, y, z);
}
/// <inheritdoc />
@@ -351,8 +380,9 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi
lock (_stateLock)
{
EnsureRobotSetup();
return _runtime.GetTcp();
}
return _runtime.GetTcp();
}
/// <inheritdoc />
@@ -361,8 +391,9 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi
lock (_stateLock)
{
EnsureRobotSetup();
return _runtime.GetJointPositions();
}
return _runtime.GetJointPositions();
}
/// <inheritdoc />
@@ -373,16 +404,44 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi
_logger?.LogInformation("MoveJoint 开始: 目标关节数={JointCount}", jointPositions.Count);
_logger?.LogDebug("MoveJoint 目标关节: {Joints}", string.Join(", ", jointPositions.Select(j => j.ToString("F4"))));
RobotProfile robot;
lock (_stateLock)
{
var robot = RequireActiveRobot();
robot = RequireActiveRobot();
EnsureRuntimeEnabled();
ExecuteMoveJointAndWaitLocked(robot, jointPositions, "MoveJoint");
}
lock (_motionLock)
{
ExecuteMoveJointAndWait(robot, jointPositions, "MoveJoint");
}
_logger?.LogInformation("MoveJoint 完成");
}
/// <inheritdoc />
public void MovePose(IReadOnlyList<double> pose)
{
ArgumentNullException.ThrowIfNull(pose);
_logger?.LogInformation("MovePose 开始: 目标位姿维度={PoseCount}", pose.Count);
_logger?.LogDebug("MovePose 目标位姿: {Pose}", string.Join(", ", pose.Select(v => v.ToString("F4"))));
RobotProfile robot;
lock (_stateLock)
{
robot = RequireActiveRobot();
EnsureRuntimeEnabled();
}
lock (_motionLock)
{
ExecuteMovePoseAndWait(robot, pose, "MovePose");
}
_logger?.LogInformation("MovePose 完成");
}
/// <inheritdoc />
public void ExecuteTrajectory(IReadOnlyList<IReadOnlyList<double>> waypoints, TrajectoryExecutionOptions? options = null)
{
@@ -398,13 +457,19 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi
_logger?.LogDebug("ExecuteTrajectory 路点详情: {Waypoints}",
string.Join(" | ", waypoints.Select(wp => $"[{string.Join(", ", wp.Select(j => j.ToString("F4")))}]")));
RobotProfile robot;
CompatibilityRobotSettings settings;
lock (_stateLock)
{
var robot = RequireActiveRobot();
robot = RequireActiveRobot();
EnsureRuntimeEnabled();
settings = RequireRobotSettings();
}
lock (_motionLock)
{
// 普通轨迹必须按调用方指定 method 规划，再把规划结果交给运行时执行。
var planningSpeedScale = RequireRobotSettings().PlanningSpeedScale;
var planningSpeedScale = settings.PlanningSpeedScale;
var speedRatio = _runtime.GetSnapshot().SpeedRatio;
var bundle = _trajectoryOrchestrator.PlanOrdinaryTrajectory(robot, waypoints, options, planningSpeedScale, speedRatio);
_logger?.LogInformation(
@@ -428,8 +493,9 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi
lock (_stateLock)
{
EnsureRobotSetup();
return _runtime.GetPose();
}
return _runtime.GetPose();
}
/// <inheritdoc />
@@ -443,16 +509,20 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi
trajectory.Waypoints.Count,
trajectory.ShotFlags.Count(static f => f));
string robotName;
CompatibilityRobotSettings settings;
lock (_stateLock)
{
EnsureRuntimeEnabled();
_uploadedTrajectories[trajectory.Name] = trajectory;
var robotName = _configuredRobotName ?? throw new InvalidOperationException("Robot has not been setup.");
var settings = _robotSettings ?? CreateDefaultRobotSettings();
_trajectoryStore.Save(robotName, settings, trajectory);
robotName = _configuredRobotName ?? throw new InvalidOperationException("Robot has not been setup.");
settings = _robotSettings ?? CreateDefaultRobotSettings();
}
// RobotConfig.json 持久化是文件 I/O，放在状态锁外，避免状态页轮询被磁盘写入拖住。
_trajectoryStore.Save(robotName, settings, trajectory);
_logger?.LogInformation("UploadTrajectory 完成: name={Name}", trajectory.Name);
}
@@ -478,25 +548,34 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi
"ExecuteTrajectoryByName 开始: name={Name}, method={Method}, moveToStart={MoveToStart}, useCache={UseCache}, wait={Wait}",
name, options.Method, options.MoveToStart, options.UseCache, options.Wait);
RobotProfile robot;
CompatibilityRobotSettings settings;
ControllerClientCompatUploadedTrajectory trajectory;
lock (_stateLock)
{
var robot = RequireActiveRobot();
robot = RequireActiveRobot();
EnsureRuntimeEnabled();
if (!_uploadedTrajectories.TryGetValue(name, out var trajectory))
if (!_uploadedTrajectories.TryGetValue(name, out var uploadedTrajectory))
{
_logger?.LogWarning("ExecuteTrajectoryByName 失败: 轨迹不存在 name={Name}", name);
throw new InvalidOperationException("FlyShot trajectory does not exist.");
}
// 飞拍执行只拿上传轨迹的瞬时副本，后续规划/导出都不再依赖字典锁。
trajectory = CloneUploadedTrajectory(uploadedTrajectory);
if (trajectory.Waypoints.Count == 0)
{
_logger?.LogWarning("ExecuteTrajectoryByName 失败: 轨迹无路点 name={Name}", name);
throw new InvalidOperationException("FlyShot trajectory contains no waypoints.");
}
settings = RequireRobotSettings();
}
lock (_motionLock)
{
// 已上传飞拍轨迹必须按调用方指定 method 生成 shot timeline 后再交给运行时。
var settings = RequireRobotSettings();
var speedRatio = _runtime.GetSnapshot().SpeedRatio;
var bundle = _trajectoryOrchestrator.PlanUploadedFlyshot(robot, trajectory, options, settings, settings.PlanningSpeedScale, speedRatio);
bundle = PrepareFlyshotExecutionBundle(robot, bundle, speedRatio);
@@ -523,7 +602,7 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi
if (options.MoveToStart)
{
_logger?.LogInformation("ExecuteTrajectoryByName 先移动到起点");
ExecuteMoveJointAndWaitLocked(robot, bundle.PlannedTrajectory.PlannedWaypoints[0].Positions, "ExecuteTrajectoryByName.move_to_start");
ExecuteMoveJointAndWait(robot, bundle.PlannedTrajectory.PlannedWaypoints[0].Positions, "ExecuteTrajectoryByName.move_to_start");
EnsureFeedbackNearFlyshotStart(bundle.PlannedTrajectory.PlannedWaypoints[0].Positions, name);
}
else
@@ -555,7 +634,7 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi
/// <param name="robot">当前机器人模型。</param>
/// <param name="targetJointPositions">目标关节位置，单位为弧度。</param>
/// <param name="operationName">用于日志和超时异常的操作名。</param>
private void ExecuteMoveJointAndWaitLocked(RobotProfile robot, IReadOnlyList<double> targetJointPositions, string operationName)
private void ExecuteMoveJointAndWait(RobotProfile robot, IReadOnlyList<double> targetJointPositions, string operationName)
{
var currentJointPositions = _runtime.GetJointPositions();
EnsureJointVector(currentJointPositions, robot.DegreesOfFreedom, nameof(currentJointPositions));
@@ -574,6 +653,30 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi
WaitForRuntimeMotionComplete(operationName, moveResult.Duration);
}
/// <summary>
/// 从当前 TCP 位姿生成临时直角 PTP 稠密轨迹并阻塞等待运行时完成。
/// </summary>
/// <param name="robot">当前机器人模型，用于取得 J519 伺服周期。</param>
/// <param name="targetPose">目标位姿 [x,y,z,w,p,r]，单位为 mm/deg。</param>
/// <param name="operationName">用于日志和超时异常的操作名。</param>
private void ExecuteMovePoseAndWait(RobotProfile robot, IReadOnlyList<double> targetPose, string operationName)
{
var currentPose = NormalizeRuntimePose(_runtime.GetPose());
EnsurePoseVector(targetPose, nameof(targetPose));
var speedRatio = _runtime.GetSnapshot().SpeedRatio;
var moveResult = MovePoseTrajectoryGenerator.CreateResult(currentPose, targetPose, robot.ServoPeriod, speedRatio, _logger);
_logger?.LogInformation(
"{OperationName} 直角PTP规划完成: 当前速度倍率={SpeedRatio}, 规划时长={Duration}s, 采样点数={SampleCount}",
operationName,
speedRatio,
moveResult.Duration.TotalSeconds,
moveResult.DenseCartesianTrajectory?.Count ?? 0);
_runtime.ExecuteCartesianTrajectory(moveResult, targetPose);
WaitForRuntimeMotionComplete(operationName, moveResult.Duration);
}
/// <summary>
/// 校验当前反馈是否接近飞拍起点；不接近时直接抛出兼容错误。
/// </summary>
@@ -649,43 +752,45 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi
_logger?.LogInformation("SaveTrajectoryInfo 开始: name={Name}, method={Method}", name, method);
RobotProfile robot;
CompatibilityRobotSettings planningSettings;
ControllerClientCompatUploadedTrajectory trajectory;
lock (_stateLock)
{
var robot = RequireActiveRobot();
if (!_uploadedTrajectories.TryGetValue(name, out var trajectory))
robot = RequireActiveRobot();
if (!_uploadedTrajectories.TryGetValue(name, out var uploadedTrajectory))
{
_logger?.LogWarning("SaveTrajectoryInfo 失败: 轨迹不存在 name={Name}", name);
throw new InvalidOperationException("FlyShot trajectory does not exist.");
}
// 先通过规划校验避免静默接受非法参数，同时把轨迹信息强制刷写到本地 JSON。
var planningSettings = RequireRobotSettings();
var speedRatio = _runtime.GetSnapshot().SpeedRatio;
var bundle = _trajectoryOrchestrator.PlanUploadedFlyshot(
robot,
trajectory,
new FlyshotExecutionOptions(useCache:false,saveTrajectory: true, method: method),
planningSettings,
planningSettings.PlanningSpeedScale,
speedRatio);
bundle = PrepareFlyshotExecutionBundle(robot, bundle, speedRatio);
_logger?.LogInformation("SaveTrajectoryInfo 规划完成记录到本地");
ExportFlyshotArtifactsIfRequested(
name,
saveTrajectory: true,
robot,
trajectory,
new FlyshotExecutionOptions(useCache: false, saveTrajectory: true, method: method),
planningSettings,
bundle,
planningSettings.PlanningSpeedScale,
speedRatio);
// var robotName = _configuredRobotName ?? throw new InvalidOperationException("Robot has not been setup.");
// var settings = _robotSettings ?? CreateDefaultRobotSettings();
// _trajectoryStore.Save(robotName, settings, trajectory);
// 保存轨迹信息会执行规划和文件导出，先复制上传数据再释放状态锁。
trajectory = CloneUploadedTrajectory(uploadedTrajectory);
planningSettings = RequireRobotSettings();
}
var speedRatio = _runtime.GetSnapshot().SpeedRatio;
var bundle = _trajectoryOrchestrator.PlanUploadedFlyshot(
robot,
trajectory,
new FlyshotExecutionOptions(useCache: false, saveTrajectory: true, method: method),
planningSettings,
planningSettings.PlanningSpeedScale,
speedRatio);
bundle = PrepareFlyshotExecutionBundle(robot, bundle, speedRatio);
_logger?.LogInformation("SaveTrajectoryInfo 规划完成记录到本地");
ExportFlyshotArtifactsIfRequested(
name,
saveTrajectory: true,
robot,
trajectory,
new FlyshotExecutionOptions(useCache: false, saveTrajectory: true, method: method),
planningSettings,
bundle,
planningSettings.PlanningSpeedScale,
speedRatio);
_logger?.LogInformation("SaveTrajectoryInfo 完成: name={Name}", name);
}
@@ -699,42 +804,48 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi
_logger?.LogInformation("IsFlyshotTrajectoryValid 开始: name={Name}, method={Method}", name, method);
RobotProfile robot;
CompatibilityRobotSettings planningSettings;
ControllerClientCompatUploadedTrajectory trajectory;
lock (_stateLock)
{
var robot = RequireActiveRobot();
if (!_uploadedTrajectories.TryGetValue(name, out var trajectory))
robot = RequireActiveRobot();
if (!_uploadedTrajectories.TryGetValue(name, out var uploadedTrajectory))
{
_logger?.LogWarning("IsFlyshotTrajectoryValid 失败: 轨迹不存在 name={Name}", name);
throw new InvalidOperationException("FlyShot trajectory does not exist.");
}
var planningSettings = RequireRobotSettings();
var speedRatio = _runtime.GetSnapshot().SpeedRatio;
var bundle = _trajectoryOrchestrator.PlanUploadedFlyshot(
robot,
trajectory,
new FlyshotExecutionOptions(method: method, saveTrajectory: saveTrajectory),
planningSettings,
planningSettings.PlanningSpeedScale,
speedRatio);
bundle = PrepareFlyshotExecutionBundle(robot, bundle, speedRatio);
ExportFlyshotArtifactsIfRequested(
name,
saveTrajectory,
robot,
trajectory,
new FlyshotExecutionOptions(method: method, saveTrajectory: saveTrajectory),
planningSettings,
bundle,
planningSettings.PlanningSpeedScale,
speedRatio);
duration = bundle.Result.Duration;
_logger?.LogInformation(
"IsFlyshotTrajectoryValid 结果: name={Name}, valid={Valid}, duration={Duration}s",
name, bundle.Result.IsValid, duration.TotalSeconds);
return bundle.Result.IsValid;
// 有效性检查只消费当前快照，不要求和后续上传/删除形成长事务。
trajectory = CloneUploadedTrajectory(uploadedTrajectory);
planningSettings = RequireRobotSettings();
}
var speedRatio = _runtime.GetSnapshot().SpeedRatio;
var bundle = _trajectoryOrchestrator.PlanUploadedFlyshot(
robot,
trajectory,
new FlyshotExecutionOptions(method: method, saveTrajectory: saveTrajectory),
planningSettings,
planningSettings.PlanningSpeedScale,
speedRatio);
bundle = PrepareFlyshotExecutionBundle(robot, bundle, speedRatio);
ExportFlyshotArtifactsIfRequested(
name,
saveTrajectory,
robot,
trajectory,
new FlyshotExecutionOptions(method: method, saveTrajectory: saveTrajectory),
planningSettings,
bundle,
planningSettings.PlanningSpeedScale,
speedRatio);
duration = bundle.Result.Duration;
_logger?.LogInformation(
"IsFlyshotTrajectoryValid 结果: name={Name}, valid={Valid}, duration={Duration}s",
name, bundle.Result.IsValid, duration.TotalSeconds);
return bundle.Result.IsValid;
}
/// <inheritdoc />
@@ -747,6 +858,7 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi
_logger?.LogInformation("DeleteTrajectory 开始: name={Name}", name);
string robotName;
lock (_stateLock)
{
if (!_uploadedTrajectories.Remove(name))
@@ -755,10 +867,12 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi
throw new InvalidOperationException("DeleteFlyShotTraj failed");
}
var robotName = _configuredRobotName ?? throw new InvalidOperationException("Robot has not been setup.");
_trajectoryStore.Delete(robotName, name);
robotName = _configuredRobotName ?? throw new InvalidOperationException("Robot has not been setup.");
}
// 删除持久化文件不占用状态锁，状态页只需要看到内存字典的即时快照。
_trajectoryStore.Delete(robotName, name);
_logger?.LogInformation("DeleteTrajectory 完成: name={Name}", name);
}
@@ -798,6 +912,21 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi
return _robotSettings ?? CreateDefaultRobotSettings();
}
/// <summary>
/// 复制一份上传轨迹快照，避免锁外规划期间观察到可变集合引用。
/// </summary>
/// <param name="trajectory">待复制的上传轨迹。</param>
/// <returns>上传轨迹副本。</returns>
private static ControllerClientCompatUploadedTrajectory CloneUploadedTrajectory(ControllerClientCompatUploadedTrajectory trajectory)
{
return new ControllerClientCompatUploadedTrajectory(
trajectory.Name,
trajectory.Waypoints,
trajectory.ShotFlags,
trajectory.OffsetValues,
trajectory.AddressGroups);
}
/// <summary>
/// 校验机器人已经完成初始化。
/// </summary>
@@ -841,6 +970,45 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi
}
}
/// <summary>
/// 校验直角位姿为 [x,y,z,w,p,r] 六维有限数。
/// </summary>
/// <param name="pose">待校验位姿，单位为 mm/deg。</param>
/// <param name="paramName">调用方参数名。</param>
private static void EnsurePoseVector(IReadOnlyList<double> pose, string paramName)
{
if (pose.Count != 6)
{
throw new ArgumentException("位姿必须为 [x,y,z,w,p,r] 六维数组。", paramName);
}
for (var index = 0; index < pose.Count; index++)
{
var value = pose[index];
if (double.IsNaN(value) || double.IsInfinity(value))
{
throw new ArgumentOutOfRangeException(paramName, $"第 {index} 个位姿值必须是有限数值。");
}
}
}
/// <summary>
/// 将运行时位姿快照归一化为 J519 直角命令需要的 [x,y,z,w,p,r] 六维。
/// </summary>
/// <param name="pose">运行时返回的位姿数组。</param>
/// <returns>六维直角位姿。</returns>
private static IReadOnlyList<double> NormalizeRuntimePose(IReadOnlyList<double> pose)
{
if (pose.Count < 6)
{
throw new InvalidOperationException("Runtime pose must contain at least [x,y,z,w,p,r].");
}
var normalized = pose.Take(6).ToArray();
EnsurePoseVector(normalized, nameof(pose));
return normalized;
}
/// <summary>
/// 根据 saveTrajectory 参数把规划结果点位写入运行目录 Config/Data/name。
/// </summary>