diff --git a/analysis/read_fanuc_allowable_limits.py b/analysis/read_fanuc_allowable_limits.py new file mode 100644 index 0000000..6b8e2b7 --- /dev/null +++ b/analysis/read_fanuc_allowable_limits.py @@ -0,0 +1,200 @@ +#!/usr/bin/env python3 +"""读取 FANUC J519 允许速度/加速度/jerk 上限表。 + +本脚本只做 UDP packet type=3 查询,不启动运行时服务,也不写入机器人参数。 +响应中的 limit 值保留为控制器原始单位:速度 deg/s、加速度 deg/s^2、jerk deg/s^3。 +""" + +from __future__ import annotations + +import argparse +import csv +import socket +import struct +import sys +from dataclasses import dataclass +from pathlib import Path + + +LIMIT_TYPES = { + 0: ("velocity", "deg/s"), + 1: ("acceleration", "deg/s^2"), + 2: ("jerk", "deg/s^3"), +} + + +@dataclass(frozen=True) +class LimitTable: + """保存单个轴、单种限制类型的 20 档 no-payload / max-payload 上限表。""" + + axis: int + limit_type: int + vmax_mm_s: int + intermediate_check_s: int + no_payload: tuple[float, ...] + max_payload: tuple[float, ...] + + +def pack_request(axis: int, limit_type: int) -> bytes: + """按手册 Table 5 封装允许上限表查询请求。""" + + return struct.pack(">IIII", 3, 1, axis, limit_type) + + +def parse_response(packet: bytes, expected_axis: int, expected_type: int) -> LimitTable: + """按手册 Table 6 解析允许上限表响应。""" + + if len(packet) != 184: + raise ValueError(f"响应长度应为 184 字节,实际为 {len(packet)} 字节。") + + packet_type, version, axis, limit_type, vmax, check_time = struct.unpack_from(">IIIIII", packet, 0) + if packet_type != 3 or version != 1: + raise ValueError(f"响应头不正确:packet_type={packet_type}, version={version}。") + if axis != expected_axis or limit_type != expected_type: + raise ValueError( + f"响应与请求不匹配:期望 axis={expected_axis}, type={expected_type}," + f"实际 axis={axis}, type={limit_type}。" + ) + + values = struct.unpack_from(">40f", packet, 24) + return LimitTable( + axis=axis, + limit_type=limit_type, + vmax_mm_s=vmax, + intermediate_check_s=check_time, + no_payload=tuple(values[:20]), + max_payload=tuple(values[20:]), + ) + + +def query_table(sock: socket.socket, robot_ip: str, port: int, axis: int, limit_type: int) -> LimitTable: + """发送单个查询请求并等待对应响应。""" + + sock.sendto(pack_request(axis, limit_type), (robot_ip, port)) + packet, address = sock.recvfrom(2048) + if address[0] != robot_ip: + raise ValueError(f"收到非目标机器人响应:{address[0]}:{address[1]}。") + return parse_response(packet, axis, limit_type) + + +def iter_requested_tables( + robot_ip: str, + port: int, + timeout_s: float, + axes: range, + limit_types: tuple[int, ...], +) -> list[LimitTable]: + """按轴和限制类型顺序读取所有上限表。""" + + tables: list[LimitTable] = [] + with socket.socket(socket.AF_INET, socket.SOCK_DGRAM) as sock: + sock.settimeout(timeout_s) + for axis in axes: + for limit_type in limit_types: + tables.append(query_table(sock, robot_ip, port, axis, limit_type)) + return tables + + +def write_csv(path: Path, tables: list[LimitTable]) -> None: + """把 20 档速度区间和两套 payload 表写成扁平 CSV。""" + + with path.open("w", newline="", encoding="utf-8") as file: + writer = csv.writer(file) + writer.writerow( + [ + "axis", + "limit_type", + "unit", + "vmax_mm_s", + "intermediate_check_s", + "speed_bin_index", + "speed_bin_upper_mm_s", + "no_payload_limit", + "max_payload_limit", + ] + ) + for table in tables: + limit_name, unit = LIMIT_TYPES[table.limit_type] + for index, (no_payload, max_payload) in enumerate(zip(table.no_payload, table.max_payload), start=1): + writer.writerow( + [ + table.axis, + limit_name, + unit, + table.vmax_mm_s, + table.intermediate_check_s, + index, + table.vmax_mm_s * index / 20.0, + f"{no_payload:.9g}", + f"{max_payload:.9g}", + ] + ) + + +def print_summary(tables: list[LimitTable]) -> None: + """打印每张表的首末档,便于现场快速确认返回值是否正常。""" + + for table in tables: + limit_name, unit = LIMIT_TYPES[table.limit_type] + print( + f"axis={table.axis} type={limit_name:<12} unit={unit:<8} " + f"vmax={table.vmax_mm_s}mm/s check={table.intermediate_check_s}s " + f"no_payload[1]={table.no_payload[0]:.6g} no_payload[20]={table.no_payload[-1]:.6g} " + f"max_payload[1]={table.max_payload[0]:.6g} max_payload[20]={table.max_payload[-1]:.6g}" + ) + + +def parse_args() -> argparse.Namespace: + """解析命令行参数。""" + + parser = argparse.ArgumentParser( + description="Read FANUC J519 packet type=3 allowable upper limit tables.", + ) + parser.add_argument("robot_ip", help="机器人控制柜 IP,例如 192.168.10.11。") + parser.add_argument("--port", type=int, default=60015, help="J519 UDP 端口,默认 60015。") + parser.add_argument("--timeout", type=float, default=2.0, help="单次响应超时秒数,默认 2.0。") + parser.add_argument("--axis-start", type=int, default=1, help="起始轴号,默认 1。") + parser.add_argument("--axis-end", type=int, default=9, help="结束轴号,默认 9。") + parser.add_argument( + "--types", + default="0,1,2", + help="限制类型列表:0=velocity, 1=acceleration, 2=jerk;默认 0,1,2。", + ) + parser.add_argument( + "--csv", + type=Path, + default=Path("analysis/fanuc_allowable_limit_tables.csv"), + help="CSV 输出路径。", + ) + return parser.parse_args() + + +def main() -> int: + """脚本入口。""" + + args = parse_args() + limit_types = tuple(int(item.strip()) for item in args.types.split(",") if item.strip()) + unknown_types = [item for item in limit_types if item not in LIMIT_TYPES] + if unknown_types: + print(f"未知限制类型:{unknown_types}", file=sys.stderr) + return 2 + if not 1 <= args.axis_start <= args.axis_end <= 9: + print("轴范围必须满足 1 <= axis-start <= axis-end <= 9。", file=sys.stderr) + return 2 + + tables = iter_requested_tables( + args.robot_ip, + args.port, + args.timeout, + range(args.axis_start, args.axis_end + 1), + limit_types, + ) + args.csv.parent.mkdir(parents=True, exist_ok=True) + write_csv(args.csv, tables) + print_summary(tables) + print(f"\nCSV 已写入:{args.csv}") + return 0 + + +if __name__ == "__main__": + raise SystemExit(main()) diff --git a/docs/0.7x.pcap b/docs/0.7x.pcap new file mode 100644 index 0000000..a966039 Binary files /dev/null and b/docs/0.7x.pcap differ diff --git a/src/Flyshot.ControllerClientCompat/ControllerClientCompatService.cs b/src/Flyshot.ControllerClientCompat/ControllerClientCompatService.cs index 9389c3f..66f0f6b 100644 --- a/src/Flyshot.ControllerClientCompat/ControllerClientCompatService.cs +++ b/src/Flyshot.ControllerClientCompat/ControllerClientCompatService.cs @@ -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 _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); } /// @@ -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(); } /// @@ -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(); } + /// + 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(), + GetServerVersion(), + GetClientVersion()); + } + } + /// public double GetSpeedRatio() { lock (_stateLock) { EnsureRobotSetup(); - return _runtime.GetSpeedRatio(); } + + return _runtime.GetSpeedRatio(); } /// @@ -288,8 +313,9 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi lock (_stateLock) { EnsureRobotSetup(); - _runtime.SetSpeedRatio(ratio); } + + _runtime.SetSpeedRatio(ratio); } /// @@ -298,8 +324,9 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi lock (_stateLock) { EnsureRobotSetup(); - _runtime.SetIo(port, value, ioType); } + + _runtime.SetIo(port, value, ioType); } /// @@ -308,8 +335,9 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi lock (_stateLock) { EnsureRobotSetup(); - return _runtime.GetIo(port, ioType); } + + return _runtime.GetIo(port, ioType); } /// @@ -341,8 +369,9 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi lock (_stateLock) { EnsureRobotSetup(); - _runtime.SetTcp(x, y, z); } + + _runtime.SetTcp(x, y, z); } /// @@ -351,8 +380,9 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi lock (_stateLock) { EnsureRobotSetup(); - return _runtime.GetTcp(); } + + return _runtime.GetTcp(); } /// @@ -361,8 +391,9 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi lock (_stateLock) { EnsureRobotSetup(); - return _runtime.GetJointPositions(); } + + return _runtime.GetJointPositions(); } /// @@ -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 完成"); } + /// + public void MovePose(IReadOnlyList 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 完成"); + } + /// public void ExecuteTrajectory(IReadOnlyList> 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(); } /// @@ -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 /// 当前机器人模型。 /// 目标关节位置,单位为弧度。 /// 用于日志和超时异常的操作名。 - private void ExecuteMoveJointAndWaitLocked(RobotProfile robot, IReadOnlyList targetJointPositions, string operationName) + private void ExecuteMoveJointAndWait(RobotProfile robot, IReadOnlyList 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); } + /// + /// 从当前 TCP 位姿生成临时直角 PTP 稠密轨迹并阻塞等待运行时完成。 + /// + /// 当前机器人模型,用于取得 J519 伺服周期。 + /// 目标位姿 [x,y,z,w,p,r],单位为 mm/deg。 + /// 用于日志和超时异常的操作名。 + private void ExecuteMovePoseAndWait(RobotProfile robot, IReadOnlyList 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); + } + /// /// 校验当前反馈是否接近飞拍起点;不接近时直接抛出兼容错误。 /// @@ -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; } /// @@ -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(); } + /// + /// 复制一份上传轨迹快照,避免锁外规划期间观察到可变集合引用。 + /// + /// 待复制的上传轨迹。 + /// 上传轨迹副本。 + private static ControllerClientCompatUploadedTrajectory CloneUploadedTrajectory(ControllerClientCompatUploadedTrajectory trajectory) + { + return new ControllerClientCompatUploadedTrajectory( + trajectory.Name, + trajectory.Waypoints, + trajectory.ShotFlags, + trajectory.OffsetValues, + trajectory.AddressGroups); + } + /// /// 校验机器人已经完成初始化。 /// @@ -841,6 +970,45 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi } } + /// + /// 校验直角位姿为 [x,y,z,w,p,r] 六维有限数。 + /// + /// 待校验位姿,单位为 mm/deg。 + /// 调用方参数名。 + private static void EnsurePoseVector(IReadOnlyList 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} 个位姿值必须是有限数值。"); + } + } + } + + /// + /// 将运行时位姿快照归一化为 J519 直角命令需要的 [x,y,z,w,p,r] 六维。 + /// + /// 运行时返回的位姿数组。 + /// 六维直角位姿。 + private static IReadOnlyList NormalizeRuntimePose(IReadOnlyList 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; + } + /// /// 根据 saveTrajectory 参数把规划结果点位写入运行目录 Config/Data/name。 /// diff --git a/src/Flyshot.ControllerClientCompat/ControllerClientStatusSnapshotMetadata.cs b/src/Flyshot.ControllerClientCompat/ControllerClientStatusSnapshotMetadata.cs new file mode 100644 index 0000000..09d86d1 --- /dev/null +++ b/src/Flyshot.ControllerClientCompat/ControllerClientStatusSnapshotMetadata.cs @@ -0,0 +1,62 @@ +namespace Flyshot.ControllerClientCompat; + +/// +/// 保存状态页需要的 ControllerClient 兼容层元数据快照,避免状态页连续读取多个受保护字段。 +/// +public sealed record ControllerClientStatusSnapshotMetadata +{ + /// + /// 初始化一份兼容层状态元数据快照。 + /// + /// 当前是否已经完成机器人初始化。 + /// 当前机器人名称;未初始化时为空。 + /// 当前机器人自由度;未初始化时为 0。 + /// 当前已上传轨迹名称快照。 + /// 兼容服务端版本。 + /// 兼容客户端版本。 + public ControllerClientStatusSnapshotMetadata( + bool isSetup, + string? robotName, + int degreesOfFreedom, + IReadOnlyList uploadedTrajectories, + string serverVersion, + string clientVersion) + { + IsSetup = isSetup; + RobotName = robotName; + DegreesOfFreedom = degreesOfFreedom; + UploadedTrajectories = uploadedTrajectories.ToArray(); + ServerVersion = serverVersion; + ClientVersion = clientVersion; + } + + /// + /// 获取当前是否已经完成机器人初始化。 + /// + public bool IsSetup { get; } + + /// + /// 获取当前机器人名称;未初始化时为空。 + /// + public string? RobotName { get; } + + /// + /// 获取当前机器人自由度;未初始化时为 0。 + /// + public int DegreesOfFreedom { get; } + + /// + /// 获取已上传轨迹名称数组快照。 + /// + public IReadOnlyList UploadedTrajectories { get; } + + /// + /// 获取兼容服务端版本。 + /// + public string ServerVersion { get; } + + /// + /// 获取兼容客户端版本。 + /// + public string ClientVersion { get; } +} diff --git a/src/Flyshot.ControllerClientCompat/IControllerClientCompatService.cs b/src/Flyshot.ControllerClientCompat/IControllerClientCompatService.cs index 0e8573e..3236873 100644 --- a/src/Flyshot.ControllerClientCompat/IControllerClientCompatService.cs +++ b/src/Flyshot.ControllerClientCompat/IControllerClientCompatService.cs @@ -95,6 +95,12 @@ public interface IControllerClientCompatService /// 控制器运行时状态快照。 ControllerStateSnapshot GetControllerSnapshot(); + /// + /// 读取状态页所需的兼容层元数据快照。 + /// + /// 兼容层元数据快照。 + ControllerClientStatusSnapshotMetadata GetStatusSnapshotMetadata(); + /// /// 获取当前速度倍率。 /// @@ -157,6 +163,12 @@ public interface IControllerClientCompatService /// 目标关节位置。 void MoveJoint(IReadOnlyList jointPositions); + /// + /// 按直角坐标移动到目标 TCP 位姿。 + /// + /// 目标位姿 [x,y,z,w,p,r],单位为 mm/deg。 + void MovePose(IReadOnlyList pose); + /// /// 执行普通轨迹。 /// diff --git a/src/Flyshot.ControllerClientCompat/MovePoseTrajectoryGenerator.cs b/src/Flyshot.ControllerClientCompat/MovePoseTrajectoryGenerator.cs new file mode 100644 index 0000000..4a148cc --- /dev/null +++ b/src/Flyshot.ControllerClientCompat/MovePoseTrajectoryGenerator.cs @@ -0,0 +1,353 @@ +using Flyshot.Core.Domain; +using Microsoft.Extensions.Logging; + +namespace Flyshot.ControllerClientCompat; + +/// +/// MovePose 直角坐标轨迹生成器。 +/// 将起始 TCP 位姿到目标 TCP 位姿的单段运动,按保守直角速度、加速度和 jerk 限制生成 8ms 稠密点。 +/// +internal static class MovePoseTrajectoryGenerator +{ + /// + /// 直角位置轴最大速度,单位为 mm/s。 + /// + private const double LinearVelocityLimit = 50.0; + + /// + /// 直角位置轴最大加速度,单位为 mm/s^2。 + /// + private const double LinearAccelerationLimit = 250.0; + + /// + /// 直角位置轴最大 jerk,单位为 mm/s^3。 + /// + private const double LinearJerkLimit = 1250.0; + + /// + /// 姿态轴最大速度,单位为 deg/s。 + /// + private const double AngularVelocityLimit = 10.0; + + /// + /// 姿态轴最大加速度,单位为 deg/s^2。 + /// + private const double AngularAccelerationLimit = 50.0; + + /// + /// 姿态轴最大 jerk,单位为 deg/s^3。 + /// + private const double AngularJerkLimit = 250.0; + + /// + /// 7 阶平滑点到点时间律的一阶导数最大值。 + /// + private const double SmoothPtpVelocityShapeCoefficient = 2.1875; + + /// + /// 7 阶平滑点到点时间律的二阶导数最大值。 + /// + private const double SmoothPtpAccelerationShapeCoefficient = 7.513188404399293; + + /// + /// 7 阶平滑点到点时间律的三阶导数最大值。 + /// + private const double SmoothPtpJerkShapeCoefficient = 52.5; + + /// + /// 单次 MovePose 最大采样点数上限,避免极低速度倍率生成过大的队列。 + /// + private const int MaxMovePoseSampleCount = 1_000_000; + + /// + /// 离散限位校验允许的浮点容差。 + /// + private const double DiscreteLimitTolerance = 1.000001; + + /// + /// 离散限位校验失败时最多拉长的采样周期次数。 + /// + private const int MaxDiscreteLimitStretchIterations = 10_000; + + /// + /// 计算 MovePose 轨迹的完整结果。 + /// + /// 起始位姿 [x,y,z,w,p,r],单位为 mm/deg。 + /// 目标位姿 [x,y,z,w,p,r],单位为 mm/deg。 + /// J519 伺服发送周期。 + /// 速度倍率,必须大于 0。 + /// 可选诊断日志。 + /// 包含稠密直角轨迹的规划结果。 + public static TrajectoryResult CreateResult( + IReadOnlyList startPose, + IReadOnlyList 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(), + triggerTimeline: Array.Empty(), + artifacts: Array.Empty(), + failureReason: null, + usedCache: false, + originalWaypointCount: 2, + plannedWaypointCount: denseCartesianTrajectory.Count, + denseCartesianTrajectory: denseCartesianTrajectory); + } + + /// + /// 检查稠密直角轨迹是否满足默认六维速度、加速度和 jerk 限制。 + /// + /// 稠密轨迹行,每行为 [time,x,y,z,w,p,r]。 + /// 生成该轨迹时使用的速度倍率。 + /// 满足限制时返回 true,否则返回 false。 + public static bool SatisfiesDefaultCartesianLimits(IReadOnlyList> 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; + } + + /// + /// 根据 7 阶平滑点到点时间律和直角六维限制计算理论最短时长。 + /// + private static double ResolveDurationSeconds(IReadOnlyList startPose, IReadOnlyList 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; + } + + /// + /// 生成从起始位姿到目标位姿的稠密等时间隔直角轨迹点序列。 + /// + private static IReadOnlyList> GenerateDenseTrajectory( + IReadOnlyList startPose, + IReadOnlyList targetPose, + double durationSeconds, + double samplePeriodSeconds) + { + var sampleCount = ResolveSampleIntervalCount(durationSeconds, samplePeriodSeconds) + 1; + var rows = new List>(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; + } + + /// + /// 将请求时长向上对齐到整数个伺服周期。 + /// + private static double AlignDurationToServoStep(double durationSeconds, double samplePeriodSeconds) + { + return ResolveSampleIntervalCount(durationSeconds, samplePeriodSeconds) * samplePeriodSeconds; + } + + /// + /// 计算时长对应的采样间隔数,轨迹至少包含起点和终点两帧。 + /// + 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; + } + + /// + /// 构造单个轨迹行:[time_seconds,x,y,z,w,p,r]。 + /// + private static IReadOnlyList CreateRow( + double timeSeconds, + double durationSeconds, + IReadOnlyList startPose, + IReadOnlyList 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; + } + + /// + /// 获取指定直角轴在当前速度倍率下的有效限制。 + /// + 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); + } + + /// + /// 校验位姿向量必须为六维有限数。 + /// + private static void EnsurePoseVector(IReadOnlyList 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); + } + } + + /// + /// 表示单个直角轴的有效速度、加速度和 jerk 限制。 + /// + private readonly record struct CartesianAxisLimit(double Velocity, double Acceleration, double Jerk); +} diff --git a/src/Flyshot.Core.Domain/TrajectoryResult.cs b/src/Flyshot.Core.Domain/TrajectoryResult.cs index e80f373..d90e515 100644 --- a/src/Flyshot.Core.Domain/TrajectoryResult.cs +++ b/src/Flyshot.Core.Domain/TrajectoryResult.cs @@ -24,7 +24,8 @@ public sealed class TrajectoryResult int plannedWaypointCount, int triggerSampleIndexOffsetCycles = 0, IEnumerable>? denseJointTrajectory = null, - FlyshotPreparedExecution? preparedFlyshotExecution = null) + FlyshotPreparedExecution? preparedFlyshotExecution = null, + IEnumerable>? denseCartesianTrajectory = null) { if (string.IsNullOrWhiteSpace(programName)) { @@ -60,6 +61,7 @@ public sealed class TrajectoryResult 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; @@ -74,6 +76,7 @@ public sealed class TrajectoryResult PlannedWaypointCount = plannedWaypointCount; TriggerSampleIndexOffsetCycles = triggerSampleIndexOffsetCycles; DenseJointTrajectory = copiedDenseJointTrajectory; + DenseCartesianTrajectory = copiedDenseCartesianTrajectory; PreparedFlyshotExecution = preparedFlyshotExecution; } @@ -156,6 +159,13 @@ public sealed class TrajectoryResult [JsonPropertyName("denseJointTrajectory")] public IReadOnlyList>? DenseJointTrajectory { get; } + /// + /// 获取稠密直角坐标轨迹采样点,每行格式为 [time, x, y, z, w, p, r]。 + /// Null 表示本结果不是直角流式运动。 + /// + [JsonPropertyName("denseCartesianTrajectory")] + public IReadOnlyList>? DenseCartesianTrajectory { get; } + /// /// Gets the prepared flyshot execution queue when the flyshot chain has already built the final 8ms send sequence. /// diff --git a/src/Flyshot.Runtime.Common/IControllerRuntime.cs b/src/Flyshot.Runtime.Common/IControllerRuntime.cs index d71421b..c3ae7e5 100644 --- a/src/Flyshot.Runtime.Common/IControllerRuntime.cs +++ b/src/Flyshot.Runtime.Common/IControllerRuntime.cs @@ -113,4 +113,11 @@ public interface IControllerRuntime /// 规划结果。 /// 轨迹执行结束后的关节位置。 void ExecuteTrajectory(TrajectoryResult result, IReadOnlyList finalJointPositions); + + /// + /// 执行一条已经完成规划的直角坐标轨迹,并更新最终 TCP 位姿。 + /// + /// 规划结果,必须包含稠密直角坐标轨迹。 + /// 轨迹执行结束后的 [x,y,z,w,p,r] 位姿。 + void ExecuteCartesianTrajectory(TrajectoryResult result, IReadOnlyList finalPose); } diff --git a/src/Flyshot.Runtime.Fanuc/FanucControllerRuntime.cs b/src/Flyshot.Runtime.Fanuc/FanucControllerRuntime.cs index 69697f0..b8a86b1 100644 --- a/src/Flyshot.Runtime.Fanuc/FanucControllerRuntime.cs +++ b/src/Flyshot.Runtime.Fanuc/FanucControllerRuntime.cs @@ -538,6 +538,78 @@ public sealed class FanucControllerRuntime : IControllerRuntime, IDisposable } } + /// + public void ExecuteCartesianTrajectory(TrajectoryResult result, IReadOnlyList finalPose) + { + ArgumentNullException.ThrowIfNull(result); + ArgumentNullException.ThrowIfNull(finalPose); + CancellationToken denseSendCancellationToken = default; + CancellationTokenSource? denseSendCancellationSource = null; + var shouldRunDenseTrajectory = false; + + _logger?.LogInformation( + "ExecuteCartesianTrajectory 开始: program={ProgramName}, 时长={Duration}s, 稠密采样={HasDense}, speedRatio={SpeedRatio}", + result.ProgramName, result.Duration.TotalSeconds, result.DenseCartesianTrajectory is not null, _speedRatio); + + lock (_stateLock) + { + EnsureEnabled(); + EnsureValidTrajectory(result); + EnsurePoseCount(finalPose.Count); + CancelSendTaskLocked(); + + if (!IsSimulationMode && result.DenseCartesianTrajectory is not null) + { + EnsureJ519ReadyForDenseExecution(); + + // 真机直角模式同样预装完整队列,由机器人状态包节拍驱动出队。 + _isInMotion = true; + _sendCts = new CancellationTokenSource(); + denseSendCancellationSource = _sendCts; + denseSendCancellationToken = _sendCts.Token; + shouldRunDenseTrajectory = true; + + _logger?.LogInformation("ExecuteCartesianTrajectory 开始同步直角稠密发送任务"); + } + else + { + if (!IsSimulationMode) + { + var command = new FanucJ519Command( + sequence: 0, + targetValues: BuildCartesianTargetValues(finalPose), + dataStyle: 0); + _j519Client.UpdateCommand(command); + } + + _isInMotion = true; + _pose = MergeFinalCartesianPose(finalPose); + _isInMotion = false; + _logger?.LogInformation("ExecuteCartesianTrajectory 完成(单点模式)"); + } + } + + if (shouldRunDenseTrajectory) + { + try + { + SendDenseCartesianTrajectory(result, finalPose, denseSendCancellationToken); + _logger?.LogInformation("ExecuteCartesianTrajectory 完成(稠密模式)"); + } + finally + { + lock (_stateLock) + { + if (ReferenceEquals(_sendCts, denseSendCancellationSource)) + { + denseSendCancellationSource?.Dispose(); + _sendCts = null; + } + } + } + } + } + /// /// 释放运行时持有的所有 Socket 客户端。 /// @@ -759,6 +831,71 @@ public sealed class FanucControllerRuntime : IControllerRuntime, IDisposable } } + /// + /// 直角稠密轨迹发送任务:预生成 Data format=0 的 J519 命令队列,并等待状态包驱动执行完成。 + /// + private void SendDenseCartesianTrajectory(TrajectoryResult result, IReadOnlyList finalPose, CancellationToken cancellationToken) + { + var rows = result.DenseCartesianTrajectory ?? throw new InvalidOperationException("Cartesian trajectory requires dense Cartesian samples."); + var commands = new List(rows.Count); + var sentPoseRows = new List>(rows.Count); + var outputDir = CreateDenseSendOutputDirectory(result.ProgramName); + var stopwatch = Stopwatch.StartNew(); + + try + { + foreach (var row in rows) + { + cancellationToken.ThrowIfCancellationRequested(); + if (row.Count != 7) + { + throw new InvalidOperationException("Cartesian dense trajectory rows must contain time plus six pose values."); + } + + var pose = row.Skip(1).Take(6).ToArray(); + var command = new FanucJ519Command( + sequence: 0, + targetValues: BuildCartesianTargetValues(pose), + dataStyle: 0, + writeIoType: 2, + writeIoIndex: 1, + writeIoMask: 0, + writeIoValue: 0); + + commands.Add(command); + sentPoseRows.Add(BuildDenseSendPoseRow(row[0], pose)); + } + + TryWriteDenseCartesianSendArtifacts(outputDir, sentPoseRows); + + _j519Client.LoadCommandQueue(commands); + if (_j519Client.IsConnected) + { + _j519Client.WaitForCommandQueueDrainedAsync(cancellationToken).GetAwaiter().GetResult(); + } + + _logger?.LogInformation( + "SendDenseCartesianTrajectory 正常完成: 采样数={SampleCount}, 队列装载耗时={ElapsedMs}ms", + commands.Count, + stopwatch.ElapsedMilliseconds); + } + catch (OperationCanceledException) + { + _logger?.LogWarning( + "SendDenseCartesianTrajectory 被取消: 已生成 {Current}/{Total} 条命令", + commands.Count, + rows.Count); + } + finally + { + lock (_stateLock) + { + _isInMotion = false; + _pose = MergeFinalCartesianPose(finalPose); + } + } + } + /// /// 若已有 J519 响应,则在启动稠密轨迹前检查伺服侧是否接受命令并处于系统就绪状态。 /// @@ -879,6 +1016,41 @@ public sealed class FanucControllerRuntime : IControllerRuntime, IDisposable return row; } + /// + /// 构造实际发送直角位姿文本行,格式为 send_time + X/Y/Z/W/P/R。 + /// + private static IReadOnlyList BuildDenseSendPoseRow(double sendTime, IReadOnlyList pose) + { + var row = new double[pose.Count + 1]; + row[0] = Math.Round(sendTime, 6); + for (var index = 0; index < pose.Count; index++) + { + row[index + 1] = Math.Round(pose[index], 6); + } + + return row; + } + + /// + /// 构造 J519 直角命令的 9 个目标槽位,当前现场无扩展轴时 E1/E2/E3 补零。 + /// + private static double[] BuildCartesianTargetValues(IReadOnlyList pose) + { + EnsurePoseCount(pose.Count); + return [pose[0], pose[1], pose[2], pose[3], pose[4], pose[5], 0.0, 0.0, 0.0]; + } + + /// + /// 将六维 WPR 直角目标合并回运行时缓存,保留旧 GetPose 仿真路径的 7 维外形。 + /// + private double[] MergeFinalCartesianPose(IReadOnlyList pose) + { + EnsurePoseCount(pose.Count); + return _pose.Length >= 7 + ? [pose[0], pose[1], pose[2], pose[3], pose[4], pose[5], _pose[6]] + : pose.ToArray(); + } + /// /// 尝试把实际发送点位、时间映射和跃度统计写入纯文本文件;若落盘失败,只记录日志,不影响运动主流程。 /// @@ -910,6 +1082,25 @@ public sealed class FanucControllerRuntime : IControllerRuntime, IDisposable } } + /// + /// 尝试把直角实际发送点位写入文本文件;若落盘失败,只记录日志,不影响运动主流程。 + /// + private void TryWriteDenseCartesianSendArtifacts(string outputDir, IReadOnlyList> sentPoseRows) + { + try + { + WriteDenseRows(Path.Combine(outputDir, "ActualSendPoseTraj.txt"), sentPoseRows); + _logger?.LogInformation( + "SendDenseCartesianTrajectory 已写出实际发送记录: outputDir={OutputDir}, poseRows={PoseRows}", + outputDir, + sentPoseRows.Count); + } + catch (Exception exception) + { + _logger?.LogWarning(exception, "SendDenseCartesianTrajectory 写出实际发送记录失败: outputDir={OutputDir}", outputDir); + } + } + /// /// 以旧轨迹文本兼容的空格分隔格式写出数值行。 /// @@ -1089,6 +1280,17 @@ public sealed class FanucControllerRuntime : IControllerRuntime, IDisposable } } + /// + /// 校验直角位姿固定为 [x,y,z,w,p,r] 六维。 + /// + private static void EnsurePoseCount(int poseCount) + { + if (poseCount != 6) + { + throw new InvalidOperationException($"Expected 6 Cartesian pose values but received {poseCount}."); + } + } + /// /// 校验机器人已经完成初始化。 /// diff --git a/src/Flyshot.Runtime.Fanuc/Protocol/FanucJ519Protocol.cs b/src/Flyshot.Runtime.Fanuc/Protocol/FanucJ519Protocol.cs index cb13f70..4d29a95 100644 --- a/src/Flyshot.Runtime.Fanuc/Protocol/FanucJ519Protocol.cs +++ b/src/Flyshot.Runtime.Fanuc/Protocol/FanucJ519Protocol.cs @@ -7,7 +7,7 @@ namespace Flyshot.Runtime.Fanuc.Protocol; /// public sealed class FanucJ519Command { - private readonly double[] _targetJoints; + private readonly double[] _targetValues; /// /// 初始化 J519 命令数据。 @@ -35,11 +35,53 @@ public sealed class FanucJ519Command ushort writeIoIndex = 1, ushort writeIoMask = 0, ushort writeIoValue = 0) + : this( + sequence, + targetValues: targetJoints, + lastData, + readIoType, + readIoIndex, + readIoMask, + dataStyle, + writeIoType, + writeIoIndex, + writeIoMask, + writeIoValue) { - ArgumentNullException.ThrowIfNull(targetJoints); - if (targetJoints.Count is <= 0 or > 9) + } + + /// + /// 初始化 J519 命令数据。 + /// + /// 命令序号。 + /// J519 目标槽位,直角模式为 X/Y/Z/W/P/R/E1/E2/E3,关节模式为 J1..J9。 + /// 是否为最后一帧数据。 + /// 读取 IO 类型。 + /// 读取 IO 起始索引。 + /// 读取 IO 掩码。 + /// 目标数据类型,0 为直角坐标,1 为关节坐标。 + /// 写入 IO 类型。 + /// 写入 IO 起始索引。 + /// 写入 IO 掩码。 + /// 写入 IO 数值。 + public FanucJ519Command( + uint sequence, + IEnumerable 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(targetJoints), "J519 目标数据必须包含 1 到 9 个槽位。"); + throw new ArgumentOutOfRangeException(nameof(targetValues), "J519 目标数据必须包含 1 到 9 个槽位。"); } Sequence = sequence; @@ -52,7 +94,7 @@ public sealed class FanucJ519Command WriteIoIndex = writeIoIndex; WriteIoMask = writeIoMask; WriteIoValue = writeIoValue; - _targetJoints = targetJoints.ToArray(); + _targetValues = copiedTargetValues; } /// @@ -108,7 +150,12 @@ public sealed class FanucJ519Command /// /// 获取目标关节或扩展轴数据。 /// - public IReadOnlyList TargetJoints => _targetJoints; + public IReadOnlyList TargetJoints => _targetValues; + + /// + /// 获取 J519 目标槽位,直角模式为 X/Y/Z/W/P/R/E1/E2/E3,关节模式为 J1..J9。 + /// + public IReadOnlyList TargetValues => _targetValues; } /// diff --git a/src/Flyshot.Server.Host/Controllers/LegacyHttpApiController.cs b/src/Flyshot.Server.Host/Controllers/LegacyHttpApiController.cs index 483ba07..38a60d2 100644 --- a/src/Flyshot.Server.Host/Controllers/LegacyHttpApiController.cs +++ b/src/Flyshot.Server.Host/Controllers/LegacyHttpApiController.cs @@ -431,6 +431,32 @@ public sealed class LegacyHttpApiController : ControllerBase } } + /// + /// 以直角坐标 `[x,y,z,w,p,r]` 执行点到点移动。 + /// + /// 直角位姿请求体。 + /// 旧 FastAPI 层风格的状态响应。 + [HttpPost("/move_pose/")] + public IActionResult MovePose([FromBody] JsonElement pose_data) + { + try + { + var poseRequest = LegacyCartesianPoseRequest.FromJson(pose_data); + var pose = poseRequest.ToPoseArray(); + _logger.LogInformation("MovePose 调用: x={X}, y={Y}, z={Z}, w={W}, p={P}, r={R}", + poseRequest.x, poseRequest.y, poseRequest.z, poseRequest.w, poseRequest.p, poseRequest.r); + + _compatService.MovePose(pose); + _logger.LogInformation("MovePose 成功"); + return Ok(new { status = "robot moved" }); + } + catch (Exception exception) + { + _logger.LogError(exception, "MovePose 失败"); + return LegacyBadRequest("MovePose failed"); + } + } + /// /// 兼容旧 `GetNearestIK(pose, seed, ik)` 参数形状。 /// @@ -874,6 +900,136 @@ public sealed class LegacyJointPositionRequest public List joints { get; init; } = []; } +/// +/// 表示 `/move_pose/` 路由使用的直角位姿请求体。 +/// +public sealed class LegacyCartesianPoseRequest +{ + /// + /// MovePose 第一版入口硬限制:TCP X/Y 最大绝对值,单位 mm。 + /// + private const double MaxHorizontalMillimeters = 1000.0; + + /// + /// MovePose 第一版入口硬限制:TCP Z 最小值,单位 mm。 + /// + private const double MinZMillimeters = 0.0; + + /// + /// MovePose 第一版入口硬限制:TCP Z 最大值,单位 mm。 + /// + private const double MaxZMillimeters = 1200.0; + + /// + /// MovePose 第一版入口硬限制:W/R 姿态角最大绝对值,单位 deg。 + /// + private const double MaxRollYawDegrees = 180.0; + + /// + /// MovePose 第一版入口硬限制:P 姿态角最大绝对值,单位 deg。 + /// + private const double MaxPitchDegrees = 90.0; + + /// + /// 获取或设置 TCP X,单位为 mm。 + /// + public double x { get; init; } + + /// + /// 获取或设置 TCP Y,单位为 mm。 + /// + public double y { get; init; } + + /// + /// 获取或设置 TCP Z,单位为 mm。 + /// + public double z { get; init; } + + /// + /// 获取或设置姿态 W,单位为 deg。 + /// + public double w { get; init; } + + /// + /// 获取或设置姿态 P,单位为 deg。 + /// + public double p { get; init; } + + /// + /// 获取或设置姿态 R,单位为 deg。 + /// + public double r { get; init; } + + /// + /// 从原始 JSON 请求体解析直角位姿,并显式拒绝缺字段、null 和非有限数。 + /// + /// 原始 JSON 请求体。 + /// 解析后的直角位姿请求。 + public static LegacyCartesianPoseRequest FromJson(JsonElement json) + { + if (json.ValueKind != JsonValueKind.Object) + { + throw new ArgumentException("MovePose request body must be an object."); + } + + // 旧接口要求请求体必须完整提供 x/y/z/w/p/r,不能让模型绑定把缺字段静默补 0。 + return new LegacyCartesianPoseRequest + { + x = ReadRequiredFiniteDouble(json, "x", -MaxHorizontalMillimeters, MaxHorizontalMillimeters), + y = ReadRequiredFiniteDouble(json, "y", -MaxHorizontalMillimeters, MaxHorizontalMillimeters), + z = ReadRequiredFiniteDouble(json, "z", MinZMillimeters, MaxZMillimeters), + w = ReadRequiredFiniteDouble(json, "w", -MaxRollYawDegrees, MaxRollYawDegrees), + p = ReadRequiredFiniteDouble(json, "p", -MaxPitchDegrees, MaxPitchDegrees), + r = ReadRequiredFiniteDouble(json, "r", -MaxRollYawDegrees, MaxRollYawDegrees) + }; + } + + /// + /// 转换为兼容层使用的六维位姿数组。 + /// + /// [x,y,z,w,p,r] 数组。 + public IReadOnlyList ToPoseArray() + { + return [x, y, z, w, p, r]; + } + + /// + /// 读取必填有限数值字段。 + /// + /// 请求体 JSON 对象。 + /// 字段名。 + /// 允许的最小值。 + /// 允许的最大值。 + /// 字段对应的有限 double 数值。 + private static double ReadRequiredFiniteDouble( + JsonElement json, + string propertyName, + double minInclusive, + double maxInclusive) + { + if (!json.TryGetProperty(propertyName, out var property) || property.ValueKind != JsonValueKind.Number) + { + throw new ArgumentException($"MovePose request field '{propertyName}' is required and must be a number."); + } + + var value = property.GetDouble(); + if (double.IsNaN(value) || double.IsInfinity(value)) + { + throw new ArgumentOutOfRangeException(propertyName, "MovePose request values must be finite."); + } + + if (value < minInclusive || value > maxInclusive) + { + throw new ArgumentOutOfRangeException( + propertyName, + value, + $"MovePose request field '{propertyName}' must be between {minInclusive} and {maxInclusive}."); + } + + return value; + } +} + /// /// 表示旧 `/upload_flyshot/` 路由使用的飞拍上传请求体。 /// diff --git a/src/Flyshot.Server.Host/Controllers/StatusController.cs b/src/Flyshot.Server.Host/Controllers/StatusController.cs index b641850..7e466b3 100644 --- a/src/Flyshot.Server.Host/Controllers/StatusController.cs +++ b/src/Flyshot.Server.Host/Controllers/StatusController.cs @@ -49,23 +49,18 @@ public sealed class StatusController : ControllerBase public IActionResult GetSnapshot() { var snapshot = _compatService.GetControllerSnapshot(); - var isSetup = _compatService.IsSetUp; - - // 状态页需要在机器人未初始化时仍能打开,因此只有初始化后才读取机器人元数据。 - var robotName = isSetup ? _compatService.GetRobotName() : null; - var degreesOfFreedom = isSetup ? _compatService.GetDegreesOfFreedom() : 0; - var uploadedTrajectories = isSetup ? _compatService.ListTrajectoryNames() : Array.Empty(); + var metadata = _compatService.GetStatusSnapshotMetadata(); return Ok(new { Status = "ok", Service = "flyshot-server-host", - ServerVersion = _compatService.GetServerVersion(), - ClientVersion = _compatService.GetClientVersion(), - IsSetup = isSetup, - RobotName = robotName, - DegreesOfFreedom = degreesOfFreedom, - UploadedTrajectories = uploadedTrajectories, + ServerVersion = metadata.ServerVersion, + ClientVersion = metadata.ClientVersion, + IsSetup = metadata.IsSetup, + RobotName = metadata.RobotName, + DegreesOfFreedom = metadata.DegreesOfFreedom, + UploadedTrajectories = metadata.UploadedTrajectories, Snapshot = snapshot }); } diff --git a/src/Flyshot.Server.Host/wwwroot/assets/debug.js b/src/Flyshot.Server.Host/wwwroot/assets/debug.js index 1efef3f..c1175ad 100644 --- a/src/Flyshot.Server.Host/wwwroot/assets/debug.js +++ b/src/Flyshot.Server.Host/wwwroot/assets/debug.js @@ -16,6 +16,17 @@ const state = { 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) { @@ -137,6 +148,11 @@ 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 { @@ -281,7 +297,7 @@ function renderBodyEditor(container, op, savedBody) { if (savedBody !== undefined && savedBody !== null) { initialText = typeof savedBody === "string" ? savedBody : JSON.stringify(savedBody, null, 2); } else { - const sample = buildSampleFromSchema(op.bodySchema, 0); + const sample = getRequestBodySample(op) || buildSampleFromSchema(op.bodySchema, 0); initialText = sample === null ? "" : JSON.stringify(sample, null, 2); } textarea.value = initialText; diff --git a/src/Flyshot.Server.Host/wwwroot/assets/status.css b/src/Flyshot.Server.Host/wwwroot/assets/status.css index afc0ccb..4206ed3 100644 --- a/src/Flyshot.Server.Host/wwwroot/assets/status.css +++ b/src/Flyshot.Server.Host/wwwroot/assets/status.css @@ -188,6 +188,68 @@ dd { font-family: inherit; } +.jog-panel { + grid-column: 1 / -1; +} + +.jog-content { + padding: 16px; +} + +.jog-settings { + display: grid; + grid-template-columns: repeat(2, minmax(180px, 1fr)); + gap: 12px; + margin-bottom: 14px; +} + +.jog-settings label { + display: grid; + gap: 6px; + color: var(--muted); + font-size: 13px; +} + +.jog-settings input { + min-height: 36px; + width: 100%; + padding: 0 10px; + border: 1px solid var(--line); + border-radius: 6px; + background: #ffffff; + color: var(--text); + font: inherit; +} + +.jog-grid { + display: grid; + grid-template-columns: repeat(6, minmax(64px, 1fr)); + gap: 8px; +} + +.jog-button { + min-height: 44px; + padding: 0 8px; + font-weight: 650; + touch-action: none; + user-select: none; +} + +.jog-button.active { + filter: brightness(0.88); +} + +.jog-status { + min-height: 24px; + margin-top: 12px; + color: var(--muted); + font-family: Consolas, "Cascadia Mono", monospace; +} + +.jog-status.error { + color: var(--bad); +} + @media (max-width: 820px) { .topbar { align-items: flex-start; @@ -199,6 +261,11 @@ dd { grid-template-columns: 1fr; } + .jog-settings, + .jog-grid { + grid-template-columns: repeat(2, minmax(0, 1fr)); + } + dl { grid-template-columns: 1fr; } diff --git a/src/Flyshot.Server.Host/wwwroot/assets/status.js b/src/Flyshot.Server.Host/wwwroot/assets/status.js index 417c431..7720c75 100644 --- a/src/Flyshot.Server.Host/wwwroot/assets/status.js +++ b/src/Flyshot.Server.Host/wwwroot/assets/status.js @@ -15,7 +15,28 @@ const fields = { joints: document.getElementById("joints"), pose: document.getElementById("pose"), trajectories: document.getElementById("trajectories"), - refresh: document.getElementById("refresh") + refresh: document.getElementById("refresh"), + linearStep: document.getElementById("linear-step"), + angularStep: document.getElementById("angular-step"), + jogStatus: document.getElementById("jog-status"), + 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 }; function formatArray(values) { @@ -56,12 +77,136 @@ function setDot(connectionState) { } } +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 到 10 的现场可调范围。 + return axis === "x" || axis === "y" || axis === "z" + ? clampStep(fields.linearStep, 0.1, 10) + : clampStep(fields.angularStep, 0.1, 10); +} + +function setJogStatus(message, isError) { + fields.jogStatus.textContent = message; + fields.jogStatus.classList.toggle("error", Boolean(isError)); +} + +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; + try { + const snapshot = await loadSnapshotForJog(); + const pose = buildJogPose(snapshot, axis, direction); + const response = await fetch("/move_pose/", { + method: "POST", + headers: { "Content-Type": "application/json" }, + body: JSON.stringify(pose) + }); + + if (!response.ok) { + throw new Error(`MovePose 调用失败: HTTP ${response.status}`); + } + + setJogStatus(`${axis.toUpperCase()}${direction > 0 ? "+" : "-"} 已发送`, false); + await refreshStatus(); + } catch (error) { + setJogStatus(error instanceof Error ? error.message : "点动失败", true); + stopJog(); + } finally { + 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); + }, 250); +} + +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 || "--"; @@ -88,5 +233,23 @@ async function refreshStatus() { } fields.refresh.addEventListener("click", refreshStatus); +fields.linearStep.addEventListener("change", function () { + clampStep(fields.linearStep, 0.1, 10); +}); +fields.angularStep.addEventListener("change", function () { + clampStep(fields.angularStep, 0.1, 10); +}); +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(); + } +}); refreshStatus(); window.setInterval(refreshStatus, 2000); diff --git a/src/Flyshot.Server.Host/wwwroot/status.html b/src/Flyshot.Server.Host/wwwroot/status.html index df326a8..5366dd1 100644 --- a/src/Flyshot.Server.Host/wwwroot/status.html +++ b/src/Flyshot.Server.Host/wwwroot/status.html @@ -57,6 +57,36 @@
已上传轨迹
--
+
+

直角坐标点动

+
+
+ + +
+
+ + + + + + + + + + + + +
+
--
+
+
diff --git a/tests/Flyshot.Core.Tests/FanucControllerRuntimeDenseTests.cs b/tests/Flyshot.Core.Tests/FanucControllerRuntimeDenseTests.cs index 57a4fbc..02d35a0 100644 --- a/tests/Flyshot.Core.Tests/FanucControllerRuntimeDenseTests.cs +++ b/tests/Flyshot.Core.Tests/FanucControllerRuntimeDenseTests.cs @@ -555,6 +555,30 @@ public sealed class FanucControllerRuntimeDenseTests Assert.True(speed05.Duration.TotalSeconds >= ExpectedSmoothPtpDuration(robot, startJoints, targetJoints, speedRatio: 0.5)); } + /// + /// 验证 MovePose 低速倍率仍保持固定伺服周期,并通过拉长时长降低直角运动速度。 + /// + [Fact] + public void MovePoseTrajectoryGenerator_LowerSpeedUsesFixedServoPeriodAndLongerPlannedDuration() + { + var servoPeriod = TimeSpan.FromMilliseconds(8); + var startPose = new[] { 100.0, 200.0, 300.0, 1.0, 2.0, 3.0 }; + var targetPose = new[] { 140.0, 260.0, 330.0, 8.0, 10.0, 12.0 }; + + var fullSpeed = MovePoseTrajectoryGenerator.CreateResult(startPose, targetPose, servoPeriod, speedRatio: 1.0); + var speed07 = MovePoseTrajectoryGenerator.CreateResult(startPose, targetPose, servoPeriod, speedRatio: 0.7); + var speed05 = MovePoseTrajectoryGenerator.CreateResult(startPose, targetPose, servoPeriod, speedRatio: 0.5); + + Assert.True(speed07.DenseCartesianTrajectory!.Count > fullSpeed.DenseCartesianTrajectory!.Count); + Assert.True(speed05.DenseCartesianTrajectory!.Count > speed07.DenseCartesianTrajectory!.Count); + AssertDenseRowsUseServoPeriod(fullSpeed.DenseCartesianTrajectory, servoPeriod.TotalSeconds); + AssertDenseRowsUseServoPeriod(speed07.DenseCartesianTrajectory, servoPeriod.TotalSeconds); + AssertDenseRowsUseServoPeriod(speed05.DenseCartesianTrajectory, servoPeriod.TotalSeconds); + AssertPoseEqual(startPose, fullSpeed.DenseCartesianTrajectory[0].Skip(1).ToArray()); + AssertPoseEqual(targetPose, fullSpeed.DenseCartesianTrajectory[^1].Skip(1).ToArray()); + Assert.True(MovePoseTrajectoryGenerator.SatisfiesDefaultCartesianLimits(speed05.DenseCartesianTrajectory, speedRatio: 0.5)); + } + [Fact] public void MoveJoint_RealMode_LeavesFinalTargetForHoldStreaming() { @@ -578,6 +602,41 @@ public sealed class FanucControllerRuntimeDenseTests AssertJointDegreesEqual(targetJoints, currentCommand.TargetJoints); } + /// + /// 验证 MovePose 会生成直角坐标 J519 队列,并使用 Data format=0 下发 X/Y/Z/W/P/R。 + /// + [Fact] + public void MovePose_RealMode_GeneratesCartesianJ519Queue() + { + using var commandClient = new FanucCommandClient(); + using var stateClient = new FanucStateClient(); + using var j519Client = new FanucJ519Client(); + using var runtime = new FanucControllerRuntime(commandClient, stateClient, j519Client); + var service = CreateCompatService(runtime); + var startPose = new[] { 100.0, 200.0, 300.0, 1.0, 2.0, 3.0 }; + var targetPose = new[] { 110.0, 220.0, 315.0, 4.0, 5.0, 6.0 }; + + service.SetUpRobot("FANUC_LR_Mate_200iD"); + j519Client.EnableCommandHistoryForTests(); + ForceRealModeEnabled(runtime, speedRatio: 1.0); + SetPrivateField(runtime, "_pose", startPose); + + service.MovePose(targetPose); + WaitUntilIdle(runtime); + + var commands = j519Client.GetCommandHistoryForTests(); + Assert.NotEmpty(commands); + Assert.All(commands, static command => Assert.Equal(0, command.DataStyle)); + AssertPoseEqual(startPose, commands[0].TargetValues.Take(6).ToArray()); + AssertPoseEqual(targetPose, commands[^1].TargetValues.Take(6).ToArray()); + Assert.All(commands, static command => + { + Assert.Equal(0.0, command.TargetValues[6], precision: 6); + Assert.Equal(0.0, command.TargetValues[7], precision: 6); + Assert.Equal(0.0, command.TargetValues[8], precision: 6); + }); + } + /// /// 验证运行时稠密发送不再依赖当前 speed_ratio;倍率合法性应在上游规划/生成阶段处理。 /// @@ -1103,6 +1162,15 @@ public sealed class FanucControllerRuntimeDenseTests } } + private static void AssertPoseEqual(IReadOnlyList expected, IReadOnlyList actual) + { + Assert.Equal(expected.Count, actual.Count); + for (var index = 0; index < expected.Count; index++) + { + Assert.Equal(expected[index], actual[index], precision: 6); + } + } + /// /// 创建用于就绪状态测试的最小 J519 响应。 /// diff --git a/tests/Flyshot.Core.Tests/FanucJ519ClientTests.cs b/tests/Flyshot.Core.Tests/FanucJ519ClientTests.cs index 8fcb0a9..f026b2b 100644 --- a/tests/Flyshot.Core.Tests/FanucJ519ClientTests.cs +++ b/tests/Flyshot.Core.Tests/FanucJ519ClientTests.cs @@ -163,6 +163,28 @@ public sealed class FanucJ519ClientTests : IDisposable await client.StopMotionAsync(_cts.Token); } + /// + /// 验证直角坐标命令会把 Data format 写为 0,并按通用目标槽位写入 X/Y/Z/W/P/R。 + /// + [Fact] + public void PackCommandPacket_WritesCartesianDataFormatAndTargetValues() + { + var command = new FanucJ519Command( + sequence: 7, + targetValues: [100.0, 200.0, 300.0, 1.0, 2.0, 3.0, 0.0, 0.0, 0.0], + dataStyle: 0); + + var packet = FanucJ519Protocol.PackCommandPacket(command); + + Assert.Equal(0, packet[0x12]); + Assert.Equal(100.0f, BinaryPrimitives.ReadSingleBigEndian(packet.AsSpan(0x1c, 4))); + Assert.Equal(200.0f, BinaryPrimitives.ReadSingleBigEndian(packet.AsSpan(0x20, 4))); + Assert.Equal(300.0f, BinaryPrimitives.ReadSingleBigEndian(packet.AsSpan(0x24, 4))); + Assert.Equal(1.0f, BinaryPrimitives.ReadSingleBigEndian(packet.AsSpan(0x28, 4))); + Assert.Equal(2.0f, BinaryPrimitives.ReadSingleBigEndian(packet.AsSpan(0x2c, 4))); + Assert.Equal(3.0f, BinaryPrimitives.ReadSingleBigEndian(packet.AsSpan(0x30, 4))); + } + /// /// 验证配置 J519 buffer size 后,实际回发命令序号会在状态包序号基础上增加该缓冲深度。 /// diff --git a/tests/Flyshot.Core.Tests/RuntimeOrchestrationTests.cs b/tests/Flyshot.Core.Tests/RuntimeOrchestrationTests.cs index 6aa94bd..ec8c673 100644 --- a/tests/Flyshot.Core.Tests/RuntimeOrchestrationTests.cs +++ b/tests/Flyshot.Core.Tests/RuntimeOrchestrationTests.cs @@ -956,6 +956,111 @@ public sealed class RuntimeOrchestrationTests } } + /// + /// 验证飞拍执行阻塞在运行时时,状态页元数据快照仍能通过短锁快速返回。 + /// + [Fact] + public async Task ControllerClientCompatService_GetStatusSnapshotMetadata_DoesNotWaitForRunningFlyshot() + { + var configRoot = CreateTempConfigRoot(); + try + { + WriteRobotConfigWithDemoTrajectory(configRoot); + var options = new ControllerClientCompatOptions + { + ConfigRoot = configRoot + }; + var runtime = new BlockingExecutionControllerRuntime([0.0, 0.0, 0.0, 0.0, 0.0, 0.0]); + var service = new ControllerClientCompatService( + options, + new ControllerClientCompatRobotCatalog(options, new RobotModelLoader()), + runtime, + new ControllerClientTrajectoryOrchestrator(), + new RobotConfigLoader()); + + service.SetUpRobot("FANUC_LR_Mate_200iD"); + service.SetActiveController(sim: false); + service.Connect("192.168.10.101"); + service.EnableRobot(2); + service.UploadTrajectory(TestRobotFactory.CreateUploadedTrajectoryWithSingleShot()); + + var executing = Task.Run(() => service.ExecuteTrajectoryByName( + "demo-flyshot", + new FlyshotExecutionOptions(moveToStart: false, method: "icsp", saveTrajectory: false, useCache: false, wait: true))); + Assert.True(runtime.WaitForExecutionStarted(TimeSpan.FromSeconds(2))); + + var metadataTask = Task.Run(() => service.GetStatusSnapshotMetadata()); + var completed = await Task.WhenAny(metadataTask, Task.Delay(TimeSpan.FromMilliseconds(150))); + + runtime.ReleaseExecution(); + await executing; + + Assert.Same(metadataTask, completed); + var metadata = await metadataTask; + Assert.True(metadata.IsSetup); + Assert.Equal("FANUC_LR_Mate_200iD", metadata.RobotName); + Assert.Equal(["demo-flyshot"], metadata.UploadedTrajectories); + } + finally + { + Directory.Delete(configRoot, recursive: true); + } + } + + /// + /// 验证两个飞拍执行命令必须串行进入 runtime,避免 J519 队列被并发执行覆盖。 + /// + [Fact] + public async Task ControllerClientCompatService_ExecuteTrajectoryByName_SerializesConcurrentExecutionCommands() + { + var configRoot = CreateTempConfigRoot(); + try + { + WriteRobotConfigWithDemoTrajectory(configRoot); + var options = new ControllerClientCompatOptions + { + ConfigRoot = configRoot + }; + var runtime = new BlockingExecutionControllerRuntime([0.0, 0.0, 0.0, 0.0, 0.0, 0.0]); + var service = new ControllerClientCompatService( + options, + new ControllerClientCompatRobotCatalog(options, new RobotModelLoader()), + runtime, + new ControllerClientTrajectoryOrchestrator(), + new RobotConfigLoader()); + + service.SetUpRobot("FANUC_LR_Mate_200iD"); + service.SetActiveController(sim: false); + service.Connect("192.168.10.101"); + service.EnableRobot(2); + service.UploadTrajectory(TestRobotFactory.CreateUploadedTrajectoryWithSingleShot()); + + var first = Task.Run(() => service.ExecuteTrajectoryByName( + "demo-flyshot", + new FlyshotExecutionOptions(moveToStart: false, method: "icsp", saveTrajectory: false, useCache: false, wait: true))); + Assert.True(runtime.WaitForExecutionStarted(TimeSpan.FromSeconds(2))); + + var second = Task.Run(() => service.ExecuteTrajectoryByName( + "demo-flyshot", + new FlyshotExecutionOptions(moveToStart: false, method: "icsp", saveTrajectory: false, useCache: false, wait: true))); + await Task.Delay(TimeSpan.FromMilliseconds(100)); + + Assert.Equal(1, runtime.ExecuteCallCount); + + runtime.ReleaseExecution(); + await first; + Assert.True(runtime.WaitForExecutionStarted(TimeSpan.FromSeconds(2), expectedCallCount: 2)); + runtime.ReleaseExecution(); + await second; + + Assert.Equal(2, runtime.ExecuteCallCount); + } + finally + { + Directory.Delete(configRoot, recursive: true); + } + } + /// /// 验证飞拍链路在进入运行时前就会准备最终发送队列,而不是把 speedRatio 重采样留给运行时临场处理。 /// @@ -2005,6 +2110,12 @@ internal sealed class RecordingControllerRuntime : IControllerRuntime { LastExecutedResult = result; } + + /// + public void ExecuteCartesianTrajectory(TrajectoryResult result, IReadOnlyList finalPose) + { + LastExecutedResult = result; + } } /// @@ -2166,6 +2277,182 @@ internal sealed class DelayedCompletionControllerRuntime : IControllerRuntime _jointPositions = finalJointPositions.ToArray(); } } + + /// + public void ExecuteCartesianTrajectory(TrajectoryResult result, IReadOnlyList finalPose) + { + } +} + +/// +/// 模拟 runtime 执行入口长期占用的测试运行时,用于验证兼容层锁边界。 +/// +internal sealed class BlockingExecutionControllerRuntime : IControllerRuntime +{ + private readonly object _lock = new(); + private readonly ManualResetEventSlim _executionStarted = new(false); + private readonly ManualResetEventSlim _releaseExecution = new(false); + private readonly double[] _jointPositions; + private bool _isEnabled; + private bool _isInMotion; + private int _executeCallCount; + + /// + /// 初始化一份会阻塞 ExecuteTrajectory 的测试运行时。 + /// + /// 初始关节反馈。 + public BlockingExecutionControllerRuntime(IReadOnlyList initialJointPositions) + { + _jointPositions = initialJointPositions.ToArray(); + } + + /// + /// 获取 runtime 执行入口被调用的次数。 + /// + public int ExecuteCallCount + { + get + { + lock (_lock) + { + return _executeCallCount; + } + } + } + + /// + /// 等待指定序号的执行调用进入 runtime。 + /// + /// 最长等待时间。 + /// 期望已经进入的执行次数。 + /// 是否在超时前等到。 + public bool WaitForExecutionStarted(TimeSpan timeout, int expectedCallCount = 1) + { + var deadline = DateTimeOffset.UtcNow.Add(timeout); + while (DateTimeOffset.UtcNow < deadline) + { + lock (_lock) + { + if (_executeCallCount >= expectedCallCount) + { + return true; + } + } + + _executionStarted.Wait(TimeSpan.FromMilliseconds(10)); + } + + return false; + } + + /// + /// 释放当前阻塞的执行调用。 + /// + public void ReleaseExecution() + { + _releaseExecution.Set(); + } + + public void ResetRobot(RobotProfile robot, string robotName) + { + } + + public void SetActiveController(bool sim) + { + } + + public void Connect(string robotIp) + { + } + + public void Disconnect() + { + } + + public void EnableRobot(int bufferSize) + { + _isEnabled = true; + } + + public void DisableRobot() + { + _isEnabled = false; + } + + public void StopMove() + { + lock (_lock) + { + _isInMotion = false; + } + + ReleaseExecution(); + } + + public double GetSpeedRatio() => 1.0; + + public void SetSpeedRatio(double ratio) + { + } + + public IReadOnlyList GetTcp() => [0.0, 0.0, 0.0]; + + public void SetTcp(double x, double y, double z) + { + } + + public bool GetIo(int port, string ioType) => false; + + public void SetIo(int port, bool value, string ioType) + { + } + + public IReadOnlyList GetJointPositions() + { + return _jointPositions.ToArray(); + } + + public IReadOnlyList GetPose() => [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]; + + public ControllerStateSnapshot GetSnapshot() + { + lock (_lock) + { + return new ControllerStateSnapshot( + capturedAt: DateTimeOffset.UtcNow, + connectionState: "Connected", + isEnabled: _isEnabled, + isInMotion: _isInMotion, + speedRatio: 1.0, + jointPositions: _jointPositions.ToArray(), + cartesianPose: Array.Empty(), + activeAlarms: Array.Empty()); + } + } + + public void ExecuteTrajectory(TrajectoryResult result, IReadOnlyList finalJointPositions) + { + lock (_lock) + { + _executeCallCount++; + _isInMotion = true; + _executionStarted.Set(); + _releaseExecution.Reset(); + } + + _releaseExecution.Wait(); + + lock (_lock) + { + _isInMotion = false; + _executionStarted.Reset(); + } + } + + public void ExecuteCartesianTrajectory(TrajectoryResult result, IReadOnlyList finalPose) + { + ExecuteTrajectory(result, _jointPositions); + } } /// @@ -2292,4 +2579,8 @@ internal sealed class StickyFeedbackControllerRuntime : IControllerRuntime _jointPositions = finalJointPositions.ToArray(); } } + + public void ExecuteCartesianTrajectory(TrajectoryResult result, IReadOnlyList finalPose) + { + } } diff --git a/tests/Flyshot.Server.IntegrationTests/DebugConsoleEndpointTests.cs b/tests/Flyshot.Server.IntegrationTests/DebugConsoleEndpointTests.cs index 601d581..0fcf321 100644 --- a/tests/Flyshot.Server.IntegrationTests/DebugConsoleEndpointTests.cs +++ b/tests/Flyshot.Server.IntegrationTests/DebugConsoleEndpointTests.cs @@ -44,6 +44,28 @@ public sealed class DebugConsoleEndpointTests(FlyshotServerFactory factory) : IC Assert.Contains("/api/debug/config", script, StringComparison.Ordinal); } + /// + /// 调试页应当为 MovePose 提供可直接发送的六字段请求体模板。 + /// + [Fact] + public async Task GetDebugScript_ContainsMovePoseRequestSample() + { + using var configuredFactory = CreateFactoryWithSwaggerEnabled(true); + using var client = configuredFactory.CreateClient(); + + using var scriptResponse = await client.GetAsync("/assets/debug.js"); + Assert.Equal(HttpStatusCode.OK, scriptResponse.StatusCode); + + var script = await scriptResponse.Content.ReadAsStringAsync(); + Assert.Contains("/move_pose/", script, StringComparison.Ordinal); + Assert.Contains("x: 100.0", script, StringComparison.Ordinal); + Assert.Contains("y: 200.0", script, StringComparison.Ordinal); + Assert.Contains("z: 300.0", script, StringComparison.Ordinal); + Assert.Contains("w: 0.0", script, StringComparison.Ordinal); + Assert.Contains("p: 45.0", script, StringComparison.Ordinal); + Assert.Contains("r: 0.0", script, StringComparison.Ordinal); + } + /// /// 当 Swagger 启用时,调试配置 API 应当返回实际 Swagger JSON 地址。 /// diff --git a/tests/Flyshot.Server.IntegrationTests/LegacyHttpApiCompatibilityTests.cs b/tests/Flyshot.Server.IntegrationTests/LegacyHttpApiCompatibilityTests.cs index 89ff82e..0777072 100644 --- a/tests/Flyshot.Server.IntegrationTests/LegacyHttpApiCompatibilityTests.cs +++ b/tests/Flyshot.Server.IntegrationTests/LegacyHttpApiCompatibilityTests.cs @@ -1,5 +1,6 @@ using System.Net; using System.Net.Http.Json; +using System.Text; using System.Text.Json; namespace Flyshot.Server.IntegrationTests; @@ -131,6 +132,13 @@ public sealed class LegacyHttpApiCompatibilityTests(FlyshotServerFactory factory Assert.Equal("robot moved", moveJointJson.RootElement.GetProperty("status").GetString()); } + using (var movePoseResponse = await client.PostAsJsonAsync("/move_pose/", new { x = 100.0, y = 200.0, z = 300.0, w = 1.0, p = 2.0, r = 3.0 })) + { + Assert.Equal(HttpStatusCode.OK, movePoseResponse.StatusCode); + using var movePoseJson = await ReadJsonAsync(movePoseResponse); + Assert.Equal("robot moved", movePoseJson.RootElement.GetProperty("status").GetString()); + } + using (var getJointPositionResponse = await client.GetAsync("/get_joint_position/")) { Assert.Equal(HttpStatusCode.OK, getJointPositionResponse.StatusCode); @@ -165,6 +173,32 @@ public sealed class LegacyHttpApiCompatibilityTests(FlyshotServerFactory factory } } + /// + /// 验证 MovePose 请求必须显式提供六个有限直角坐标字段,避免缺字段被模型绑定静默补 0。 + /// + [Theory] + [InlineData("""{"x":100.0,"y":200.0,"z":300.0,"w":1.0,"p":2.0}""")] + [InlineData("null")] + [InlineData("""{"x":1e999,"y":200.0,"z":300.0,"w":1.0,"p":2.0,"r":3.0}""")] + [InlineData("""{"x":1000.1,"y":200.0,"z":300.0,"w":1.0,"p":2.0,"r":3.0}""")] + [InlineData("""{"x":100.0,"y":-1000.1,"z":300.0,"w":1.0,"p":2.0,"r":3.0}""")] + [InlineData("""{"x":100.0,"y":200.0,"z":-0.1,"w":1.0,"p":2.0,"r":3.0}""")] + [InlineData("""{"x":100.0,"y":200.0,"z":300.0,"w":-180.1,"p":2.0,"r":3.0}""")] + [InlineData("""{"x":100.0,"y":200.0,"z":300.0,"w":1.0,"p":90.1,"r":3.0}""")] + [InlineData("""{"x":100.0,"y":200.0,"z":300.0,"w":1.0,"p":2.0,"r":180.1}""")] + public async Task MovePose_InvalidPayload_ReturnsLegacyBadRequest(string payload) + { + using var client = factory.CreateClient(); + await InitializeRobotAsync(client); + + using var content = new StringContent(payload, Encoding.UTF8, "application/json"); + using var response = await client.PostAsync("/move_pose/", content); + + Assert.Equal(HttpStatusCode.BadRequest, response.StatusCode); + using var json = await ReadJsonAsync(response); + Assert.Equal("MovePose failed", json.RootElement.GetProperty("detail").GetString()); + } + /// /// 验证飞拍 HTTP 接口可以按旧 API 层的路径和字段完成上传、列出、执行与删除。 /// diff --git a/tests/Flyshot.Server.IntegrationTests/StatusEndpointTests.cs b/tests/Flyshot.Server.IntegrationTests/StatusEndpointTests.cs index cfcdcdc..2c8c3f9 100644 --- a/tests/Flyshot.Server.IntegrationTests/StatusEndpointTests.cs +++ b/tests/Flyshot.Server.IntegrationTests/StatusEndpointTests.cs @@ -34,6 +34,39 @@ public sealed class StatusEndpointTests(FlyshotServerFactory factory) : IClassFi Assert.Contains("/api/status/snapshot", script, StringComparison.Ordinal); } + /// + /// 状态页应当提供直角坐标点动按钮,并复用现有 MovePose HTTP 接口。 + /// + [Fact] + public async Task GetStatusPageAssets_ExposeCartesianJogControls() + { + using var client = factory.CreateClient(); + + using var htmlResponse = await client.GetAsync("/status.html"); + Assert.Equal(HttpStatusCode.OK, htmlResponse.StatusCode); + + var html = await htmlResponse.Content.ReadAsStringAsync(); + Assert.Contains("直角坐标点动", html, StringComparison.Ordinal); + Assert.Contains("id=\"linear-step\"", html, StringComparison.Ordinal); + Assert.Contains("id=\"angular-step\"", html, StringComparison.Ordinal); + + foreach (var axis in new[] { "x", "y", "z", "w", "p", "r" }) + { + Assert.Contains($"data-axis=\"{axis}\"", html, StringComparison.Ordinal); + Assert.Contains($"data-axis=\"{axis}\" data-direction=\"1\"", html, StringComparison.Ordinal); + Assert.Contains($"data-axis=\"{axis}\" data-direction=\"-1\"", html, StringComparison.Ordinal); + } + + using var scriptResponse = await client.GetAsync("/assets/status.js"); + Assert.Equal(HttpStatusCode.OK, scriptResponse.StatusCode); + + var script = await scriptResponse.Content.ReadAsStringAsync(); + Assert.Contains("/move_pose/", script, StringComparison.Ordinal); + Assert.Contains("cartesianPose", script, StringComparison.Ordinal); + Assert.Contains("pointerdown", script, StringComparison.Ordinal); + Assert.Contains("pointerup", script, StringComparison.Ordinal); + } + /// /// 验证状态快照 API 会返回运行时连接、使能、速度和机器人元数据。 ///