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

✨ feat(compat): 补齐飞拍执行等待与 FANUC 状态驱动链路

Browse Source 
- 为 ExecuteFlyShotTraj 补齐 wait 语义，并让 move_to_start
  先完成临时 PTP 运动后再启动正式飞拍轨迹
- 将 J519 命令发送改为由机器人 UDP status sequence 驱动，
  避免在未收到状态包时主动发周期命令
- 将 10010 状态通道关节字段统一按 JointRadians 命名，
  同步更新运行时读取逻辑与协议测试
- 新增 FANUC 10010 状态帧、流运动手册和 Python client
  逆向文档，并更新 README 与兼容需求说明
- 补充兼容层编排测试与 HTTP 集成测试，覆盖 wait 和
  move_to_start 串行化行为
This commit is contained in:
yunxiao.zhu
2026-05-03 19:29:31 +08:00
parent 91c1494cde
commit af65ca03a0
17 changed files with 1694 additions and 214 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -373,18 +373,7 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi
{
var robot = RequireActiveRobot();
EnsureRuntimeEnabled();
var currentJointPositions = _runtime.GetJointPositions();
EnsureJointVector(currentJointPositions, robot.DegreesOfFreedom, nameof(currentJointPositions));
EnsureJointVector(jointPositions, robot.DegreesOfFreedom, nameof(jointPositions));
var speedRatio = _runtime.GetSnapshot().SpeedRatio;
var moveResult = MoveJointTrajectoryGenerator.CreateResult(robot, currentJointPositions, jointPositions, speedRatio, _logger);
_logger?.LogInformation(
"MoveJoint 规划完成: 当前速度倍率={SpeedRatio}, 规划时长={Duration}s, 采样点数={SampleCount}",
speedRatio,
moveResult.Duration.TotalSeconds,
moveResult.DenseJointTrajectory?.Count ?? 0);
_runtime.ExecuteTrajectory(moveResult, jointPositions);
ExecuteMoveJointAndWaitLocked(robot, jointPositions, "MoveJoint");
}
_logger?.LogInformation("MoveJoint 完成");
@@ -480,8 +469,8 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi
}
_logger?.LogInformation(
"ExecuteTrajectoryByName 开始: name={Name}, method={Method}, moveToStart={MoveToStart}, useCache={UseCache}",
name, options.Method, options.MoveToStart, options.UseCache);
"ExecuteTrajectoryByName 开始: name={Name}, method={Method}, moveToStart={MoveToStart}, useCache={UseCache}, wait={Wait}",
name, options.Method, options.MoveToStart, options.UseCache, options.Wait);
lock (_stateLock)
{
@@ -516,16 +505,83 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi
if (options.MoveToStart)
{
_logger?.LogInformation("ExecuteTrajectoryByName 先移动到起点");
_runtime.ExecuteTrajectory(CreateImmediateMoveResult(), bundle.PlannedTrajectory.PlannedWaypoints[0].Positions);
ExecuteMoveJointAndWaitLocked(robot, bundle.PlannedTrajectory.PlannedWaypoints[0].Positions, "ExecuteTrajectoryByName.move_to_start");
}
var finalJointPositions = bundle.PlannedTrajectory.PlannedWaypoints[^1].Positions;
_runtime.ExecuteTrajectory(bundle.Result, finalJointPositions);
if (options.Wait)
{
WaitForRuntimeMotionComplete("ExecuteTrajectoryByName.flyshot", bundle.Result.Duration);
}
}
_logger?.LogInformation("ExecuteTrajectoryByName 完成: name={Name}", name);
}
/// <summary>
/// 从当前关节位置生成临时 PTP 稠密轨迹并阻塞等待运行时完成，避免后续 J519 目标发生突变。
/// </summary>
/// <param name="robot">当前机器人模型。</param>
/// <param name="targetJointPositions">目标关节位置，单位为弧度。</param>
/// <param name="operationName">用于日志和超时异常的操作名。</param>
private void ExecuteMoveJointAndWaitLocked(RobotProfile robot, IReadOnlyList<double> targetJointPositions, string operationName)
{
var currentJointPositions = _runtime.GetJointPositions();
EnsureJointVector(currentJointPositions, robot.DegreesOfFreedom, nameof(currentJointPositions));
EnsureJointVector(targetJointPositions, robot.DegreesOfFreedom, nameof(targetJointPositions));
var speedRatio = _runtime.GetSnapshot().SpeedRatio;
var moveResult = MoveJointTrajectoryGenerator.CreateResult(robot, currentJointPositions, targetJointPositions, speedRatio, _logger);
_logger?.LogInformation(
"{OperationName} PTP规划完成: 当前速度倍率={SpeedRatio}, 规划时长={Duration}s, 采样点数={SampleCount}",
operationName,
speedRatio,
moveResult.Duration.TotalSeconds,
moveResult.DenseJointTrajectory?.Count ?? 0);
_runtime.ExecuteTrajectory(moveResult, targetJointPositions);
WaitForRuntimeMotionComplete(operationName, moveResult.Duration);
}
/// <summary>
/// 等待运行时报告当前运动结束，用于把 move_to_start 与正式飞拍轨迹串行化。
/// </summary>
/// <param name="operationName">用于日志和超时异常的操作名。</param>
/// <param name="plannedDuration">规划运动时长。</param>
private void WaitForRuntimeMotionComplete(string operationName, TimeSpan plannedDuration)
{
var timeout = ResolveMotionCompletionTimeout(plannedDuration);
var deadline = DateTimeOffset.UtcNow.Add(timeout);
while (true)
{
if (!_runtime.GetSnapshot().IsInMotion)
{
_logger?.LogInformation("{OperationName} 运动完成", operationName);
return;
}
if (DateTimeOffset.UtcNow >= deadline)
{
throw new TimeoutException($"{operationName} 等待运动完成超时，planned={plannedDuration.TotalSeconds:F3}s, timeout={timeout.TotalSeconds:F3}s。");
}
Thread.Sleep(TimeSpan.FromMilliseconds(10));
}
}
/// <summary>
/// 根据规划时长推导等待超时，给真机通信和状态更新留出余量。
/// </summary>
/// <param name="plannedDuration">规划运动时长。</param>
/// <returns>等待运行时完成的最大时长。</returns>
private static TimeSpan ResolveMotionCompletionTimeout(TimeSpan plannedDuration)
{
var timeoutSeconds = Math.Max(5.0, plannedDuration.TotalSeconds * 3.0 + 2.0);
return TimeSpan.FromSeconds(timeoutSeconds);
}
/// <inheritdoc />
public void SaveTrajectoryInfo(string name, string method = "icsp")
{
@@ -703,26 +759,6 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi
}
}
/// <summary>
/// 构造无需稠密轨迹的最小合法结果，供仍需单点状态更新的兼容路径使用。
/// </summary>
/// <returns>可立即执行的轨迹结果。</returns>
private static TrajectoryResult CreateImmediateMoveResult()
{
return new TrajectoryResult(
programName: "move-joint",
method: PlanningMethod.Icsp,
isValid: true,
duration: TimeSpan.Zero,
shotEvents: Array.Empty<ShotEvent>(),
triggerTimeline: Array.Empty<TrajectoryDoEvent>(),
artifacts: Array.Empty<TrajectoryArtifact>(),
failureReason: null,
usedCache: false,
originalWaypointCount: 1,
plannedWaypointCount: 1);
}
/// <summary>
/// 根据 saveTrajectory 参数把规划结果点位写入运行目录 Config/Data/name。
/// </summary>