diff --git a/README.md b/README.md index a5f9a40..c2af03a 100644 --- a/README.md +++ b/README.md @@ -24,7 +24,9 @@ - 2026-05-11 对 `UTTC_MS11` 的独立 `ConfigRoot` 重新生成确认:flyshot 链路若继续用离散 `jerk` 作为阻断条件,会把新结果拉长到约 `16.576s / 33.152s`;只保留速度、加速度校验并跳过 flyshot jerk 阻断后,新 `1.0x / 0.5x` 时长回落到约 `7.552s / 15.104s`,与旧抓包主段 `7.408s / 14.808s` 的差异已很小,当前按可忽略处理。详见 `docs/uttc-ms11-jerk-validation-decision-20260511.md`。 - `MoveJoint` 已按 `2026042802-mvpoint*.pcap` 复刻为点到点临时轨迹:当前关节到目标关节的关节空间直线,五次 smoothstep 起停,按 `status=15` 运动窗口复现 `40/55/77` 点,并由 J519 层完成 `rad -> deg` 下发。 - 单程序只对应一台机器人,上传/删除/恢复飞拍轨迹统一读写运行目录 `Config/RobotConfig.json`,不再创建独立轨迹存储文件。 -- 服务启动时支持自动初始化机器人:在 `appsettings.json` 的 `ControllerClientCompat.AutoInit` 节配置 `RobotName` 和 `RobotIp`,启动后会自动执行 `SetUpRobot → SetActiveController → Connect → EnableRobot`;配置缺失或连接失败不会阻止服务启动。 +- 服务启动时支持真机优先初始化:在 `ControllerClientCompat.AutoInit` 配置 `RobotName` 和 `RobotIp`,启动后执行 `SetUpRobot → Connect → EnableRobot(8)`;连接失败后进入 `OfflineReady`,模型仍可用于离线飞拍规划。 +- 飞拍资产与机器人型号解耦:未加载模型时也可上传、列出和删除;模型自由度与关节限制延迟到规划阶段校验。 +- `/save_traj_info/` 已成为离线规划入口,返回最终 8ms 发送点列、ShotEvents、触发绑定和全部导出文件绝对路径;`/execute_flyshot/` 仍只允许 `RealReady`。 单位约定总览: @@ -74,6 +76,8 @@ - [x] 保留新 HTTP 接口路线,明确不再实现旧 `50001/TCP+JSON` 网关 - [x] 将飞拍轨迹持久化收敛到运行目录 `Config/RobotConfig.json` - [x] 服务启动自动初始化机器人:通过 `ControllerClientCompat.AutoInit` 配置节指定 `RobotName` 和 `RobotIp`,启动时自动执行初始化链路 +- [x] 完成真机优先与离线飞拍规划改造:`RealReady / OfflineReady / ModelLoadFailed` 状态、固定轨迹资产、完整离线规划响应和 Debug 页面 +- [x] 删除生产仿真切换 HTTP 入口及旧 `/init_mpc_robt`,统一使用 `/init_mpc_robot` 剩余 Todo: diff --git a/docs/fanuc-field-runtime-workflow.md b/docs/fanuc-field-runtime-workflow.md index b616689..10ffa59 100644 --- a/docs/fanuc-field-runtime-workflow.md +++ b/docs/fanuc-field-runtime-workflow.md @@ -7,32 +7,27 @@ 推荐使用聚合端点完成当前现场的一次性初始化: ```bash -POST /init_mpc_robt +POST /init_mpc_robot { - "server_ip": "127.0.0.1", - "port": 50001, "robot_name": "FANUC_LR_Mate_200iD", - "robot_ip": "192.168.10.11", - "sim": false + "robot_ip": "192.168.10.11" } ``` 该端点内部顺序: -1. `ConnectServer(server_ip, port)`:兼容旧参数形状,仅记录服务连接语义。 -2. `SetUpRobot(robot_name)`:加载机器人配置、关节限制和伺服周期。 -3. `SetActiveController(sim)`:选择仿真或 FANUC 真机运行时。 -4. `Connect(robot_ip)`:真机模式下依次建立 `TCP 10010` 状态通道、`TCP 10012` 命令通道、`UDP 60015` J519 运动通道。 -5. `EnableRobot(2)`:真机模式下执行 `StopProg("RVBUSTSM") -> Reset -> GetProgStatus("RVBUSTSM") -> StartProg("RVBUSTSM")`,随后允许 J519 在收到机器人 UDP status 包后回发下一帧命令。 +1. `SetUpRobot(robot_name)`:加载机器人配置、关节限制和伺服周期,状态进入 `OfflineReady`。 +2. `Connect(robot_ip)`:依次建立 `TCP 10010` 状态通道、`TCP 10012` 命令通道、`UDP 60015` J519 运动通道。 +3. `EnableRobot(8)`:启动 `RVBUSTSM` 和 J519 运动循环,成功后进入 `RealReady`。 + +连接或使能失败时接口返回 400,但模型保持加载,状态进入 `OfflineReady`;只有模型加载失败才进入 `ModelLoadFailed`。 也可以使用拆分端点按同样顺序调用: ```text -POST /connect_server/?server_ip=127.0.0.1&port=50001 POST /setup_robot/?robot_name=FANUC_LR_Mate_200iD -POST /set_active_controller/?sim=false POST /connect_robot/?ip=192.168.10.11 -GET /enable_robot/?buffer_size=2 +GET /enable_robot/?buffer_size=8 ``` ## 2. 参数设置 @@ -98,6 +93,8 @@ POST /move_joint/ ## 4. 飞拍轨迹 +飞拍资产固定保存在 `Config/RobotConfig.json`。上传、列出和删除不要求模型已加载,也不要求真机连接;`setup_robot` 不会清空已上传轨迹。 + 上传: ```bash @@ -122,6 +119,18 @@ POST /is_flyShotTrajValid/ } ``` +离线生成最终发送轨迹: + +```bash +POST /save_traj_info/ +{ + "name": "UTTC_MS11", + "method": "self-adapt-icsp" +} +``` + +该接口只要求模型已加载,不要求连接或使能。响应包含规划时长、最终发送时长、关键/稠密点数量、完整最终 8ms 点列、IO mask/value、ShotEvents、触发采样点绑定和全部导出文件绝对路径。`/debug` 中的“离线飞拍规划”面板提供同一能力,并明确不会向机器人发送命令。 + 执行: ```bash @@ -142,7 +151,7 @@ POST /execute_flyshot/ 2. 使用 `icsp` 或 `self-adapt-icsp` 规划关节轨迹;规划阶段先应用 `acc_limit / jerk_limit`,再应用 replacement-only 的规划加速度校准参数。 3. 生成 `TrajectoryDoEvent`,把拍照触发绑定到轨迹时间。 4. 若 `move_to_start=true`,先从运行时当前关节位置生成临时 PTP 稠密轨迹移动到规划轨迹起点,并等待运行时 `IsInMotion=false` 后再启动飞拍轨迹,避免第一帧 J519 目标从当前位置跳到起点。 -5. 真机模式下把规划输出的 `rad` 稠密轨迹按 J519 轨迹时间步长重采样并转成 `deg`,命令实际发包由机器人 UDP status 包驱动。 +5. 把规划输出的 `rad` 连续轨迹构建为最终 8ms 发送队列并转成 `deg`;离线规划和真机执行复用同一份结果。 6. 若 `wait=true`,正式飞拍轨迹启动后继续等待运行时 `IsInMotion=false`,机器人执行完整条飞拍轨迹后 HTTP 才返回;`wait=false` 时启动后立即返回。 7. 启动前若已有 J519 响应且 `accept_cmd` 或 `sysrdy` 未就绪,则拒绝执行。 8. 周期命令中嵌入 IO 脉冲;当前 UTTC 抓包确认 mask 集合为 `10/12/14`,共 17 个 set 脉冲和 17 个 clear 帧。 @@ -157,7 +166,7 @@ GET /disable_robot/ POST /disconnect_robot/ ``` -真机模式下: +真机运行时: - 正常飞拍、MoveJoint、MovePose 结束时,默认在 J519 队列末尾追加 `LastData=1` 结束帧;结束帧发出后进入仅监控状态,继续接收 J519 状态包,不再回发保姿命令。 - `StopMove()` 取消当前稠密轨迹生成任务并走强制停止路径,可停止 J519 状态包驱动发送。 diff --git a/docs/superpowers/plans/2026-06-23-hardware-first-offline-flyshot-planning.md b/docs/superpowers/plans/2026-06-23-hardware-first-offline-flyshot-planning.md new file mode 100644 index 0000000..d6a92d3 --- /dev/null +++ b/docs/superpowers/plans/2026-06-23-hardware-first-offline-flyshot-planning.md @@ -0,0 +1,32 @@ +# 真机优先与离线飞拍规划改造计划 + +## 已实施目标 + +- 启动及 `/init_mpc_robot` 固定执行“加载模型 → 连接真机 → `EnableRobot(8)`”。 +- 模型加载成功但连接或使能失败时进入 `OfflineReady`,HTTP 宿主继续运行。 +- 飞拍资产固定保存到 `Config/RobotConfig.json`,上传、列出、删除不依赖模型或真机状态。 +- `/save_traj_info/` 只要求模型已加载,返回完整最终发送点列、触发绑定和导出文件绝对路径。 +- `/execute_flyshot/` 仍要求 `RealReady`,离线规划不会伪造机器人连接或使能。 +- `/debug` 提供模型加载、轨迹上传/选择、离线生成、分页点列和导出路径查看。 + +## 状态约定 + +- `LoadingModel`:正在加载机器人模型。 +- `Connecting`:模型已加载,正在连接或使能真机。 +- `RealReady`:真机已连接且使能,允许真实运动和 IO。 +- `OfflineReady`:模型已加载但真机不可用,允许离线规划。 +- `ModelLoadFailed`:机器人模型不可用,禁止规划和执行。 + +## HTTP 约定 + +- 保留 `/setup_robot/`,仅加载模型,不连接、不使能,也不清空飞拍资产。 +- 新增 `/init_mpc_robot`,请求体只包含 `robot_name`、`robot_ip`。 +- 删除 `/init_mpc_robt`、`/connect_server/`、版本查询、环境文件、TCP 显示、`is_setup` 和仿真控制器切换路由。 +- `/upload_flyshot/` 在上传阶段只校验资产结构和有限数值;自由度与模型限制在规划阶段校验。 +- `/save_traj_info/` 返回规划摘要、关键/稠密轨迹、最终发送点、ShotEvents、触发绑定和导出路径。 + +## 验证 + +- 核心测试覆盖离线资产生命周期、模型重载保留资产、连接失败降级和完整离线规划结果。 +- 集成测试覆盖删除路由 404、新初始化接口、状态快照和 Debug 离线规划页面。 +- 最终执行核心测试、集成测试和解决方案构建。 diff --git a/src/Flyshot.ControllerClientCompat/ControllerClientCompatOptions.cs b/src/Flyshot.ControllerClientCompat/ControllerClientCompatOptions.cs index 27b26e6..87e4a4e 100644 --- a/src/Flyshot.ControllerClientCompat/ControllerClientCompatOptions.cs +++ b/src/Flyshot.ControllerClientCompat/ControllerClientCompatOptions.cs @@ -61,7 +61,7 @@ public sealed class AutoInitOptions public string RobotName { get; set; } = string.Empty; /// - /// 获取或设置机器人 IP 地址,用于连接真机或仿真控制器。 + /// 获取或设置机器人 IP 地址,用于连接真实 FANUC 控制器。 /// public string RobotIp { get; set; } = string.Empty; } diff --git a/src/Flyshot.ControllerClientCompat/ControllerClientCompatService.cs b/src/Flyshot.ControllerClientCompat/ControllerClientCompatService.cs index 66f0f6b..3cf539a 100644 --- a/src/Flyshot.ControllerClientCompat/ControllerClientCompatService.cs +++ b/src/Flyshot.ControllerClientCompat/ControllerClientCompatService.cs @@ -25,11 +25,12 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi private RobotProfile? _activeRobotProfile; private string? _configuredRobotName; private CompatibilityRobotSettings? _robotSettings; - private string? _connectedServerIp; - private int _connectedServerPort; - private bool _showTcp = true; - private double _showTcpAxisLength = 0.1; - private int _showTcpAxisSize = 2; + private string _operatingMode = "ModelLoadFailed"; + private string _initializationStage = "NotStarted"; + private string? _robotIp; + private string? _lastConnectionError; + private DateTimeOffset? _lastConnectionErrorAt; + private DateTimeOffset? _lastInitializationAttemptAt; /// /// 初始化一份 HTTP-only 的 ControllerClient 兼容服务。 @@ -60,6 +61,15 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi _artifactWriter = artifactWriter ?? new FlyshotTrajectoryArtifactWriter(_options, new RobotModelLoader()); _trajectoryStore = trajectoryStore ?? new JsonFlyshotTrajectoryStore(_options, _configLoader); _logger = logger; + + // 飞拍轨迹是固定 RobotConfig.json 中的独立资产,服务构造后立即加载,不依赖机器人模型。 + var savedTrajectories = _trajectoryStore.LoadAll(out var savedSettings); + foreach (var saved in savedTrajectories) + { + _uploadedTrajectories[saved.Key] = saved.Value; + } + + _robotSettings = savedSettings; } /// @@ -82,125 +92,111 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi /// public bool IsInMotion => _runtime.GetSnapshot().IsInMotion; - /// - public void ConnectServer(string serverIp, int port) - { - if (string.IsNullOrWhiteSpace(serverIp)) - { - throw new ArgumentException("服务端 IP 不能为空。", nameof(serverIp)); - } - - if (port <= 0) - { - throw new ArgumentOutOfRangeException(nameof(port), "端口必须大于 0。"); - } - - lock (_stateLock) - { - // HTTP-only 阶段仍记录旧客户端期望的 50001 地址,便于后续 TCP 入口恢复时复用状态。 - _connectedServerIp = serverIp; - _connectedServerPort = port; - } - - _logger?.LogInformation("ConnectServer 完成: {ServerIp}:{Port}", serverIp, port); - } - - /// - public string GetServerVersion() - { - return ServerVersion; - } - - /// - public string GetClientVersion() - { - return "flyshot-replacement-controller-client-compat/0.1.0"; - } - /// public void SetUpRobot(string robotName) { _logger?.LogInformation("SetUpRobot 开始: robotName={RobotName}", robotName); - var robotSettings = TryLoadRobotSettings() ?? CreateDefaultRobotSettings(); - var robotProfile = _robotCatalog.LoadProfile( - robotName, - robotSettings.AccLimitScale, - robotSettings.JerkLimitScale); - lock (_stateLock) { - // 机器人重新初始化时同步重置运行时和上传轨迹目录,保持旧服务初始化语义。 - _configuredRobotName = robotName; - _activeRobotProfile = robotProfile; - _uploadedTrajectories.Clear(); - _runtime.ResetRobot(robotProfile, robotName); - // RobotConfig.json 的 speed_ratio 是启动默认值;在线 set_speedRatio 仍可覆盖后续执行。 - _runtime.SetSpeedRatio(robotSettings.SpeedRatio); - _robotSettings = robotSettings; + _initializationStage = "LoadingModel"; + } - // 从持久化存储恢复该机器人名下之前已上传的轨迹。 - var savedTrajectories = _trajectoryStore.LoadAll(robotName, out _); - foreach (var saved in savedTrajectories) + try + { + var robotSettings = TryLoadRobotSettings() ?? _robotSettings ?? CreateDefaultRobotSettings(); + var robotProfile = _robotCatalog.LoadProfile( + robotName, + robotSettings.AccLimitScale, + robotSettings.JerkLimitScale); + + lock (_stateLock) { - _uploadedTrajectories[saved.Key] = saved.Value; + // 重新加载模型只重置 Runtime;飞拍资产继续保留在固定 RobotConfig.json 中。 + _configuredRobotName = robotName; + _activeRobotProfile = robotProfile; + _runtime.ResetRobot(robotProfile, robotName); + _runtime.SetSpeedRatio(robotSettings.SpeedRatio); + _robotSettings = robotSettings; + _operatingMode = "OfflineReady"; + _initializationStage = "ModelLoaded"; + } + + _logger?.LogInformation( + "SetUpRobot 完成: robotName={RobotName}, dof={Dof}, accLimit={AccLimit}, jerkLimit={JerkLimit}, speedRatio={SpeedRatio}, 保留轨迹数={TrajCount}", + robotName, + robotProfile.DegreesOfFreedom, + robotSettings.AccLimitScale, + robotSettings.JerkLimitScale, + robotSettings.SpeedRatio, + _uploadedTrajectories.Count); + } + catch + { + try + { + _runtime.Disconnect(); + } + catch + { + // 模型切换失败时优先清理上层状态;底层若尚未初始化,Disconnect 失败可忽略。 + } + + lock (_stateLock) + { + _activeRobotProfile = null; + _configuredRobotName = null; + _operatingMode = "ModelLoadFailed"; + _initializationStage = "ModelLoadFailed"; + } + + throw; + } + } + + /// + public void InitializeRobot(string robotName, string robotIp, int bufferSize = 8) + { + lock (_stateLock) + { + _lastInitializationAttemptAt = DateTimeOffset.UtcNow; + _robotIp = robotIp; + _lastConnectionError = null; + _lastConnectionErrorAt = null; + } + + SetUpRobot(robotName); + + try + { + lock (_stateLock) + { + _initializationStage = "Connecting"; + } + + // 真机优先初始化固定执行连接和使能,不再选择生产仿真控制器。 + Connect(robotIp); + EnableRobot(bufferSize); + + lock (_stateLock) + { + _operatingMode = "RealReady"; + _initializationStage = "RealReady"; } } - - _logger?.LogInformation( - "SetUpRobot 完成: robotName={RobotName}, dof={Dof}, accLimit={AccLimit}, jerkLimit={JerkLimit}, speedRatio={SpeedRatio}, 恢复轨迹数={TrajCount}", - robotName, - robotProfile.DegreesOfFreedom, - robotSettings.AccLimitScale, - robotSettings.JerkLimitScale, - robotSettings.SpeedRatio, - _uploadedTrajectories.Count); - } - - /// - public void SetUpRobotFromEnv(string envFile) - { - if (string.IsNullOrWhiteSpace(envFile)) + catch (Exception exception) { - throw new ArgumentException("环境文件路径不能为空。", nameof(envFile)); + lock (_stateLock) + { + // 模型已加载时,真机失败必须保留离线规划能力。 + _operatingMode = _activeRobotProfile is null ? "ModelLoadFailed" : "OfflineReady"; + _initializationStage = "OfflineReady"; + _lastConnectionError = exception.Message; + _lastConnectionErrorAt = DateTimeOffset.UtcNow; + } + + throw; } - - throw new NotSupportedException("SetUpRobotFromEnv 尚未接入环境文件解析。"); - } - - /// - public void SetShowTcp(bool isShow, double axisLength, int axisSize) - { - if (axisLength <= 0.0) - { - throw new ArgumentOutOfRangeException(nameof(axisLength), "TCP 坐标轴长度必须大于 0。"); - } - - if (axisSize <= 0) - { - throw new ArgumentOutOfRangeException(nameof(axisSize), "TCP 坐标轴线宽必须大于 0。"); - } - - lock (_stateLock) - { - EnsureRobotSetup(); - - // 当前无 GUI 渲染层,先保存显示参数,保证旧 SDK 参数不会在 HTTP 边界丢失。 - _showTcp = isShow; - _showTcpAxisLength = axisLength; - _showTcpAxisSize = axisSize; - } - } - - /// - public void SetActiveController(bool sim) - { - lock (_stateLock) - { - EnsureRobotSetup(); - } - - _runtime.SetActiveController(sim); } /// @@ -216,6 +212,8 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi lock (_stateLock) { EnsureRobotSetup(); + _robotIp = robotIp; + _initializationStage = "Connecting"; } _runtime.Connect(robotIp); @@ -232,6 +230,11 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi } _runtime.Disconnect(); + lock (_stateLock) + { + _operatingMode = _activeRobotProfile is null ? "ModelLoadFailed" : "OfflineReady"; + _initializationStage = _activeRobotProfile is null ? "ModelLoadFailed" : "OfflineReady"; + } } /// @@ -243,6 +246,13 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi EnsureRobotSetup(); } _runtime.EnableRobot(bufferSize); + lock (_stateLock) + { + _operatingMode = "RealReady"; + _initializationStage = "RealReady"; + _lastConnectionError = null; + _lastConnectionErrorAt = null; + } _logger?.LogInformation("EnableRobot 完成"); } @@ -257,6 +267,11 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi // DisableRobot 是中断/控制命令,不能被长时间运动串行锁挡住。 _runtime.DisableRobot(); + lock (_stateLock) + { + _operatingMode = _activeRobotProfile is null ? "ModelLoadFailed" : "OfflineReady"; + _initializationStage = _activeRobotProfile is null ? "ModelLoadFailed" : "OfflineReady"; + } _logger?.LogInformation("DisableRobot 完成"); } @@ -290,9 +305,18 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi isSetup, isSetup ? _configuredRobotName : null, isSetup ? _activeRobotProfile!.DegreesOfFreedom : 0, - isSetup ? _uploadedTrajectories.Keys.ToArray() : Array.Empty(), - GetServerVersion(), - GetClientVersion()); + _uploadedTrajectories.Keys.ToArray(), + ServerVersion, + "flyshot-replacement-controller-client-compat/0.1.0", + _operatingMode, + _initializationStage, + _robotIp, + _lastConnectionError, + _lastConnectionErrorAt, + _lastInitializationAttemptAt, + canPlanOffline: isSetup, + canExecuteRealMotion: string.Equals(_operatingMode, "RealReady", StringComparison.Ordinal) + && _runtime.GetSnapshot().IsEnabled); } } @@ -509,19 +533,16 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi trajectory.Waypoints.Count, trajectory.ShotFlags.Count(static f => f)); - string robotName; CompatibilityRobotSettings settings; lock (_stateLock) { - EnsureRuntimeEnabled(); + ValidateUploadedTrajectoryAsset(trajectory); _uploadedTrajectories[trajectory.Name] = trajectory; - - robotName = _configuredRobotName ?? throw new InvalidOperationException("Robot has not been setup."); settings = _robotSettings ?? CreateDefaultRobotSettings(); } // RobotConfig.json 持久化是文件 I/O,放在状态锁外,避免状态页轮询被磁盘写入拖住。 - _trajectoryStore.Save(robotName, settings, trajectory); + _trajectoryStore.Save(settings, trajectory); _logger?.LogInformation("UploadTrajectory 完成: name={Name}", trajectory.Name); } @@ -743,7 +764,7 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi } /// - public void SaveTrajectoryInfo(string name, string method = "icsp") + public OfflineFlyshotPlanningResult SaveTrajectoryInfo(string name, string method = "icsp") { if (string.IsNullOrWhiteSpace(name)) { @@ -792,6 +813,7 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi speedRatio); _logger?.LogInformation("SaveTrajectoryInfo 完成: name={Name}", name); + return CreateOfflinePlanningResult(name, method, robot, bundle, speedRatio); } /// @@ -858,7 +880,6 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi _logger?.LogInformation("DeleteTrajectory 开始: name={Name}", name); - string robotName; lock (_stateLock) { if (!_uploadedTrajectories.Remove(name)) @@ -867,11 +888,10 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi throw new InvalidOperationException("DeleteFlyShotTraj failed"); } - robotName = _configuredRobotName ?? throw new InvalidOperationException("Robot has not been setup."); } // 删除持久化文件不占用状态锁,状态页只需要看到内存字典的即时快照。 - _trajectoryStore.Delete(robotName, name); + _trajectoryStore.Delete(name); _logger?.LogInformation("DeleteTrajectory 完成: name={Name}", name); } @@ -1066,8 +1086,7 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi bundle.ExecutionTrajectory, bundle.Result, robot.ServoPeriod.TotalSeconds, - requestedSpeedRatio: speedRatio, - samplingSpeedRatio: 1.0); + requestedSpeedRatio: speedRatio); var preparedResult = new TrajectoryResult( programName: bundle.Result.ProgramName, method: bundle.Result.Method, @@ -1146,4 +1165,248 @@ public sealed class ControllerClientCompatService : IControllerClientCompatServi adaptIcspTryNum: 5); } + /// + /// 校验上传资产自身结构,不在上传阶段绑定机器人自由度或关节限制。 + /// + /// 待保存的飞拍资产。 + private static void ValidateUploadedTrajectoryAsset(ControllerClientCompatUploadedTrajectory trajectory) + { + if (trajectory.Waypoints.Count == 0) + { + throw new ArgumentException("飞拍轨迹至少需要一个路点。", nameof(trajectory)); + } + + if (trajectory.ShotFlags.Count != trajectory.Waypoints.Count + || trajectory.OffsetValues.Count != trajectory.Waypoints.Count + || trajectory.AddressGroups.Count != trajectory.Waypoints.Count) + { + throw new ArgumentException("飞拍轨迹的辅助数组长度必须与路点数量一致。", nameof(trajectory)); + } + + for (var waypointIndex = 0; waypointIndex < trajectory.Waypoints.Count; waypointIndex++) + { + if (trajectory.Waypoints[waypointIndex].Length == 0) + { + throw new ArgumentException($"第 {waypointIndex} 个路点不能为空。", nameof(trajectory)); + } + + for (var jointIndex = 0; jointIndex < trajectory.Waypoints[waypointIndex].Length; jointIndex++) + { + var value = trajectory.Waypoints[waypointIndex][jointIndex]; + if (double.IsNaN(value) || double.IsInfinity(value)) + { + throw new ArgumentOutOfRangeException( + nameof(trajectory), + $"第 {waypointIndex} 个路点的第 {jointIndex} 个值必须是有限数值。"); + } + } + } + } + + /// + /// 把内部规划结果转换成供 HTTP 和 Debug 页面直接消费的完整离线结果。 + /// + /// 轨迹名称。 + /// 规划方法。 + /// 当前机器人模型。 + /// 已经完成最终发送队列构建的结果包。 + /// 本次速度倍率。 + /// 完整离线规划结果。 + private OfflineFlyshotPlanningResult CreateOfflinePlanningResult( + string name, + string method, + RobotProfile robot, + PlannedExecutionBundle bundle, + double speedRatio) + { + var prepared = bundle.PreparedExecution + ?? throw new InvalidOperationException("离线规划结果缺少最终发送队列。"); + var ioBySampleIndex = BuildIoTimeline(prepared); + var finalSendPoints = prepared.Samples.Select((sample, index) => + { + ioBySampleIndex.TryGetValue(index, out var io); + return new OfflineFlyshotSendPoint( + sample.SampleIndex, + sample.SendTime, + sample.TrajectoryTime, + sample.JointsDegrees, + io.Mask, + io.Value); + }).ToArray(); + var triggerBindings = prepared.TriggerBindings.Select(binding => + { + var ioValue = ComputeIoValue(binding.Trigger.AddressGroup); + return new OfflineFlyshotTriggerBinding( + binding.Trigger.WaypointIndex, + binding.Trigger.TriggerTime, + binding.SampleIndex, + binding.Sample.SampleIndex, + binding.FoundInWindow, + ioValue, + ioValue); + }).ToArray(); + var plannedRows = bundle.ExecutionTrajectory.PlannedWaypoints + .Select((waypoint, index) => + (IReadOnlyList)[bundle.ExecutionTrajectory.WaypointTimes[index], .. waypoint.Positions]) + .ToArray(); + var artifactDirectory = Path.Combine(_options.ResolveConfigRoot(), "Data", SanitizeDirectoryName(name)); + var artifactPaths = new[] + { + "JointTraj.txt", + "JointDetialTraj.txt", + "CartTraj.txt", + "CartDetialTraj.txt", + "ActualSendJointTraj.txt", + "ActualSendTiming.txt", + "ActualSendJerkStats.txt", + "ShotEvents.json" + }.ToDictionary( + static fileName => fileName, + fileName => Path.GetFullPath(Path.Combine(artifactDirectory, fileName)), + StringComparer.Ordinal); + var cartesianTrajectory = ReadNumericRows(artifactPaths["CartDetialTraj.txt"]); + var diagnostics = CalculateDiagnostics(prepared.Samples); + + return new OfflineFlyshotPlanningResult( + name, + method, + bundle.Result.IsValid, + bundle.Result.Duration.TotalSeconds, + prepared.FinalDurationSeconds, + bundle.Result.OriginalWaypointCount, + bundle.Result.PlannedWaypointCount, + bundle.Result.DenseJointTrajectory?.Count ?? 0, + speedRatio, + robot.ServoPeriod.TotalSeconds, + plannedRows, + bundle.Result.DenseJointTrajectory ?? Array.Empty>(), + cartesianTrajectory, + finalSendPoints, + bundle.Result.ShotEvents, + triggerBindings, + artifactPaths, + diagnostics); + } + + /// + /// 根据触发绑定和保持周期构造每个最终发送点的 IO 掩码和值。 + /// + /// 最终发送结果。 + /// 按发送点索引保存的 IO 数据。 + private static Dictionary BuildIoTimeline(FlyshotPreparedExecution prepared) + { + var result = new Dictionary(); + foreach (var binding in prepared.TriggerBindings) + { + var ioValue = ComputeIoValue(binding.Trigger.AddressGroup); + var holdCount = Math.Max(1, binding.Trigger.HoldCycles); + for (var offset = 0; offset < holdCount && binding.SampleIndex + offset < prepared.Samples.Count; offset++) + { + result[binding.SampleIndex + offset] = (ioValue, ioValue); + } + } + + return result; + } + + /// + /// 把 IO 地址组转换为 16 位写出掩码。 + /// + /// IO 地址组。 + /// 写出掩码和值。 + private static ushort ComputeIoValue(IoAddressGroup group) + { + ushort value = 0; + foreach (var address in group.Addresses) + { + if (address is >= 1 and <= 16) + { + value |= (ushort)(1 << (address - 1)); + } + } + + return value; + } + + /// + /// 清理轨迹名称中的非法路径字符。 + /// + /// 原始轨迹名称。 + /// 可安全用于目录名的字符串。 + private static string SanitizeDirectoryName(string name) + { + var invalidChars = Path.GetInvalidFileNameChars(); + return new string(name.Select(character => invalidChars.Contains(character) ? '_' : character).ToArray()); + } + + /// + /// 读取空格分隔的数值轨迹文件。 + /// + /// 待读取文件绝对路径。 + /// 数值行集合;文件不存在时返回空集合。 + private static IReadOnlyList> ReadNumericRows(string path) + { + if (!File.Exists(path)) + { + return Array.Empty>(); + } + + return File.ReadLines(path) + .Where(static line => !string.IsNullOrWhiteSpace(line)) + .Select(line => (IReadOnlyList)line + .Split(' ', StringSplitOptions.RemoveEmptyEntries) + .Select(value => double.Parse(value, System.Globalization.CultureInfo.InvariantCulture)) + .ToArray()) + .ToArray(); + } + + /// + /// 从最终发送点列计算最大绝对速度、加速度和跃度。 + /// + /// 最终发送采样点。 + /// 离散运动学诊断统计。 + private static OfflineFlyshotDiagnostics CalculateDiagnostics(IReadOnlyList samples) + { + var maxVelocity = 0.0; + var maxAcceleration = 0.0; + var maxJerk = 0.0; + double[]? previousVelocity = null; + double[]? previousAcceleration = null; + + for (var sampleIndex = 1; sampleIndex < samples.Count; sampleIndex++) + { + var dt = samples[sampleIndex].SendTime - samples[sampleIndex - 1].SendTime; + if (dt <= 0.0) + { + continue; + } + + var velocity = new double[samples[sampleIndex].JointsDegrees.Count]; + var acceleration = new double[velocity.Length]; + for (var jointIndex = 0; jointIndex < velocity.Length; jointIndex++) + { + velocity[jointIndex] = + (samples[sampleIndex].JointsDegrees[jointIndex] - samples[sampleIndex - 1].JointsDegrees[jointIndex]) / dt; + maxVelocity = Math.Max(maxVelocity, Math.Abs(velocity[jointIndex])); + + if (previousVelocity is not null) + { + acceleration[jointIndex] = (velocity[jointIndex] - previousVelocity[jointIndex]) / dt; + maxAcceleration = Math.Max(maxAcceleration, Math.Abs(acceleration[jointIndex])); + } + + if (previousAcceleration is not null) + { + var jerk = (acceleration[jointIndex] - previousAcceleration[jointIndex]) / dt; + maxJerk = Math.Max(maxJerk, Math.Abs(jerk)); + } + } + + previousVelocity = velocity; + previousAcceleration = acceleration; + } + + return new OfflineFlyshotDiagnostics(maxVelocity, maxAcceleration, maxJerk); + } + } diff --git a/src/Flyshot.ControllerClientCompat/ControllerClientStatusSnapshotMetadata.cs b/src/Flyshot.ControllerClientCompat/ControllerClientStatusSnapshotMetadata.cs index 09d86d1..51432b9 100644 --- a/src/Flyshot.ControllerClientCompat/ControllerClientStatusSnapshotMetadata.cs +++ b/src/Flyshot.ControllerClientCompat/ControllerClientStatusSnapshotMetadata.cs @@ -20,7 +20,15 @@ public sealed record ControllerClientStatusSnapshotMetadata int degreesOfFreedom, IReadOnlyList uploadedTrajectories, string serverVersion, - string clientVersion) + string clientVersion, + string operatingMode = "ModelLoadFailed", + string initializationStage = "NotStarted", + string? robotIp = null, + string? lastConnectionError = null, + DateTimeOffset? lastConnectionErrorAt = null, + DateTimeOffset? lastInitializationAttemptAt = null, + bool canPlanOffline = false, + bool canExecuteRealMotion = false) { IsSetup = isSetup; RobotName = robotName; @@ -28,6 +36,14 @@ public sealed record ControllerClientStatusSnapshotMetadata UploadedTrajectories = uploadedTrajectories.ToArray(); ServerVersion = serverVersion; ClientVersion = clientVersion; + OperatingMode = operatingMode; + InitializationStage = initializationStage; + RobotIp = robotIp; + LastConnectionError = lastConnectionError; + LastConnectionErrorAt = lastConnectionErrorAt; + LastInitializationAttemptAt = lastInitializationAttemptAt; + CanPlanOffline = canPlanOffline; + CanExecuteRealMotion = canExecuteRealMotion; } /// @@ -59,4 +75,28 @@ public sealed record ControllerClientStatusSnapshotMetadata /// 获取兼容客户端版本。 /// public string ClientVersion { get; } + + /// 获取当前运行模式。 + public string OperatingMode { get; } + + /// 获取当前初始化阶段。 + public string InitializationStage { get; } + + /// 获取最近尝试连接的机器人 IP。 + public string? RobotIp { get; } + + /// 获取最后一次真机连接或使能错误。 + public string? LastConnectionError { get; } + + /// 获取最后一次真机连接或使能错误发生时间。 + public DateTimeOffset? LastConnectionErrorAt { get; } + + /// 获取最后一次初始化尝试时间。 + public DateTimeOffset? LastInitializationAttemptAt { get; } + + /// 获取当前是否允许离线规划。 + public bool CanPlanOffline { get; } + + /// 获取当前是否允许真机执行。 + public bool CanExecuteRealMotion { get; } } diff --git a/src/Flyshot.ControllerClientCompat/FlyshotTrajectoryStore.cs b/src/Flyshot.ControllerClientCompat/FlyshotTrajectoryStore.cs index 6cadd92..d4c071a 100644 --- a/src/Flyshot.ControllerClientCompat/FlyshotTrajectoryStore.cs +++ b/src/Flyshot.ControllerClientCompat/FlyshotTrajectoryStore.cs @@ -34,14 +34,14 @@ public sealed class JsonFlyshotTrajectoryStore /// 当前已初始化的机器人名称,仅用于日志诊断。 /// 当前机器人级兼容配置。 /// 要保存的已上传轨迹。 - public void Save(string robotName, CompatibilityRobotSettings settings, ControllerClientCompatUploadedTrajectory trajectory) + public void Save(CompatibilityRobotSettings settings, ControllerClientCompatUploadedTrajectory trajectory) { ArgumentNullException.ThrowIfNull(settings); ArgumentNullException.ThrowIfNull(trajectory); _logger?.LogInformation( "RobotConfig 保存轨迹: robot={RobotName}, name={TrajectoryName}, waypoints={WaypointCount}", - robotName, + "fixed-config", trajectory.Name, trajectory.Waypoints.Count); @@ -82,19 +82,30 @@ public sealed class JsonFlyshotTrajectoryStore _logger?.LogInformation("RobotConfig 轨迹已保存到 {Path}", path); } + /// + /// 兼容旧调用签名;机器人名称不再参与固定配置文件分区。 + /// + /// 仅用于兼容的机器人名称。 + /// 机器人级兼容配置。 + /// 待保存轨迹。 + public void Save(string robotName, CompatibilityRobotSettings settings, ControllerClientCompatUploadedTrajectory trajectory) + { + Save(settings, trajectory); + } + /// /// 从统一 RobotConfig.json 删除指定名称的轨迹。 /// /// 当前已初始化的机器人名称,仅用于日志诊断。 /// 要删除的轨迹名称。 - public void Delete(string robotName, string trajectoryName) + public void Delete(string trajectoryName) { if (string.IsNullOrWhiteSpace(trajectoryName)) { throw new ArgumentException("轨迹名称不能为空。", nameof(trajectoryName)); } - _logger?.LogInformation("RobotConfig 删除轨迹: robot={RobotName}, name={TrajectoryName}", robotName, trajectoryName); + _logger?.LogInformation("RobotConfig 删除轨迹: name={TrajectoryName}", trajectoryName); var path = ResolveStorePath(); if (!File.Exists(path)) @@ -131,13 +142,23 @@ public sealed class JsonFlyshotTrajectoryStore } } + /// + /// 兼容旧调用签名;机器人名称不再参与固定配置文件分区。 + /// + /// 仅用于兼容的机器人名称。 + /// 待删除轨迹名称。 + public void Delete(string robotName, string trajectoryName) + { + Delete(trajectoryName); + } + /// /// 加载统一 RobotConfig.json 中的所有轨迹,并回传机器人配置。 /// /// 当前已初始化的机器人名称,仅用于日志诊断。 /// 输出 RobotConfig.json 中的机器人配置;若文件不存在或解析失败则为 null。 /// 按轨迹名称索引的已上传轨迹集合。 - public IReadOnlyDictionary LoadAll(string robotName, out CompatibilityRobotSettings? settings) + public IReadOnlyDictionary LoadAll(out CompatibilityRobotSettings? settings) { var path = ResolveStorePath(); if (!File.Exists(path)) @@ -168,7 +189,7 @@ public sealed class JsonFlyshotTrajectoryStore _logger?.LogInformation( "RobotConfig 加载完成: robot={RobotName}, 轨迹数={Count}, useDo={UseDo}, ioKeepCycles={IoKeepCycles}", - robotName, + "fixed-config", dict.Count, settings?.UseDo, settings?.IoKeepCycles); @@ -183,6 +204,19 @@ public sealed class JsonFlyshotTrajectoryStore } } + /// + /// 兼容旧调用签名;机器人名称不再参与固定配置文件分区。 + /// + /// 仅用于兼容的机器人名称。 + /// 输出机器人配置。 + /// 固定 RobotConfig.json 中的全部飞拍资产。 + public IReadOnlyDictionary LoadAll( + string robotName, + out CompatibilityRobotSettings? settings) + { + return LoadAll(out settings); + } + /// /// 把机器人兼容配置序列化为 JSON 对象,字段名与旧版 RobotConfig.json 一致。 /// diff --git a/src/Flyshot.ControllerClientCompat/IControllerClientCompatService.cs b/src/Flyshot.ControllerClientCompat/IControllerClientCompatService.cs index 3236873..69a0598 100644 --- a/src/Flyshot.ControllerClientCompat/IControllerClientCompatService.cs +++ b/src/Flyshot.ControllerClientCompat/IControllerClientCompatService.cs @@ -17,25 +17,6 @@ public interface IControllerClientCompatService /// bool IsSetUp { get; } - /// - /// 保存当前调用方期望连接的 replacement 服务端地址。 - /// - /// 客户端传入的服务端 IP。 - /// 客户端传入的服务端端口。 - void ConnectServer(string serverIp, int port); - - /// - /// 获取兼容服务端版本号。 - /// - /// 服务端版本号。 - string GetServerVersion(); - - /// - /// 获取兼容客户端版本号。 - /// - /// 客户端版本号。 - string GetClientVersion(); - /// /// 根据旧客户端使用的机器人名称完成机器人初始化。 /// @@ -43,24 +24,12 @@ public interface IControllerClientCompatService void SetUpRobot(string robotName); /// - /// 根据旧客户端传入的环境文件完成机器人初始化。 + /// 按“加载模型、连接真机、使能机器人”的固定顺序执行初始化。 /// - /// 环境文件路径。 - void SetUpRobotFromEnv(string envFile); - - /// - /// 设置是否显示 TCP 坐标轴。 - /// - /// 是否显示 TCP。 - /// 坐标轴长度。 - /// 坐标轴线宽。 - void SetShowTcp(bool isShow, double axisLength, int axisSize); - - /// - /// 记录当前激活的控制器类型。 - /// - /// 是否为仿真控制器。 - void SetActiveController(bool sim); + /// 机器人名称。 + /// 机器人控制器 IP。 + /// J519 发送缓冲区大小。 + void InitializeRobot(string robotName, string robotIp, int bufferSize = 8); /// /// 记录当前控制器已经建立连接。 @@ -206,7 +175,7 @@ public interface IControllerClientCompatService /// /// 轨迹名称。 /// 轨迹生成方法。 - void SaveTrajectoryInfo(string name, string method = "icsp"); + OfflineFlyshotPlanningResult SaveTrajectoryInfo(string name, string method = "icsp"); /// /// 检查指定飞拍轨迹是否可执行。 diff --git a/src/Flyshot.ControllerClientCompat/OfflineFlyshotPlanningResult.cs b/src/Flyshot.ControllerClientCompat/OfflineFlyshotPlanningResult.cs new file mode 100644 index 0000000..9f359f5 --- /dev/null +++ b/src/Flyshot.ControllerClientCompat/OfflineFlyshotPlanningResult.cs @@ -0,0 +1,174 @@ +using Flyshot.Core.Domain; + +namespace Flyshot.ControllerClientCompat; + +/// +/// 表示离线飞拍规划返回给 HTTP、Debug 页面和导出链路的完整结果。 +/// +public sealed class OfflineFlyshotPlanningResult +{ + /// + /// 初始化一份离线飞拍规划结果。 + /// + /// 轨迹名称。 + /// 规划方法。 + /// 规划结果是否有效。 + /// 连续规划轨迹时长,单位为秒。 + /// 最终发送时长,单位为秒。 + /// 原始示教路点数量。 + /// 规划关键路点数量。 + /// 稠密轨迹点数量。 + /// 本次最终发送使用的速度倍率。 + /// 最终发送采样周期,单位为秒。 + /// 关键关节轨迹,每行为时间加弧度制关节值。 + /// 稠密关节轨迹,每行为时间加弧度制关节值。 + /// 稠密笛卡尔轨迹。 + /// 最终 8ms 发送点列。 + /// 理论拍照事件。 + /// 触发事件与最终发送点的绑定结果。 + /// 导出文件名与绝对路径。 + /// 最终发送点列的速度、加速度和跃度统计。 + public OfflineFlyshotPlanningResult( + string trajectoryName, + string method, + bool isValid, + double planningDurationSeconds, + double finalSendDurationSeconds, + int originalWaypointCount, + int plannedWaypointCount, + int densePointCount, + double speedRatio, + double servoPeriodSeconds, + IEnumerable> plannedJointTrajectory, + IEnumerable> denseJointTrajectory, + IEnumerable> cartesianTrajectory, + IEnumerable finalSendPoints, + IEnumerable shotEvents, + IEnumerable triggerBindings, + IReadOnlyDictionary artifactPaths, + OfflineFlyshotDiagnostics diagnostics) + { + TrajectoryName = trajectoryName; + Method = method; + IsValid = isValid; + PlanningDurationSeconds = planningDurationSeconds; + FinalSendDurationSeconds = finalSendDurationSeconds; + OriginalWaypointCount = originalWaypointCount; + PlannedWaypointCount = plannedWaypointCount; + DensePointCount = densePointCount; + SpeedRatio = speedRatio; + ServoPeriodSeconds = servoPeriodSeconds; + PlannedJointTrajectory = plannedJointTrajectory.Select(static row => row.ToArray()).ToArray(); + DenseJointTrajectory = denseJointTrajectory.Select(static row => row.ToArray()).ToArray(); + CartesianTrajectory = cartesianTrajectory.Select(static row => row.ToArray()).ToArray(); + FinalSendPoints = finalSendPoints.ToArray(); + ShotEvents = shotEvents.ToArray(); + TriggerBindings = triggerBindings.ToArray(); + ArtifactPaths = new Dictionary(artifactPaths, StringComparer.Ordinal); + Diagnostics = diagnostics; + } + + /// 获取轨迹名称。 + public string TrajectoryName { get; } + + /// 获取规划方法。 + public string Method { get; } + + /// 获取规划结果是否有效。 + public bool IsValid { get; } + + /// 获取连续规划轨迹时长,单位为秒。 + public double PlanningDurationSeconds { get; } + + /// 获取最终发送时长,单位为秒。 + public double FinalSendDurationSeconds { get; } + + /// 获取原始示教路点数量。 + public int OriginalWaypointCount { get; } + + /// 获取规划关键路点数量。 + public int PlannedWaypointCount { get; } + + /// 获取稠密轨迹点数量。 + public int DensePointCount { get; } + + /// 获取最终发送点数量。 + public int FinalSendPointCount => FinalSendPoints.Count; + + /// 获取本次最终发送使用的速度倍率。 + public double SpeedRatio { get; } + + /// 获取最终发送采样周期,单位为秒。 + public double ServoPeriodSeconds { get; } + + /// 获取关键关节轨迹。 + public IReadOnlyList> PlannedJointTrajectory { get; } + + /// 获取稠密关节轨迹。 + public IReadOnlyList> DenseJointTrajectory { get; } + + /// 获取稠密笛卡尔轨迹。 + public IReadOnlyList> CartesianTrajectory { get; } + + /// 获取最终 8ms 发送点列。 + public IReadOnlyList FinalSendPoints { get; } + + /// 获取理论拍照事件。 + public IReadOnlyList ShotEvents { get; } + + /// 获取触发事件与最终发送点的绑定结果。 + public IReadOnlyList TriggerBindings { get; } + + /// 获取导出文件名与绝对路径。 + public IReadOnlyDictionary ArtifactPaths { get; } + + /// 获取最终发送点列的速度、加速度和跃度统计。 + public OfflineFlyshotDiagnostics Diagnostics { get; } +} + +/// +/// 表示离线规划结果中的单个最终 J519 发送点。 +/// +/// 发送周期序号。 +/// 物理发送时间,单位为秒。 +/// 映射到连续轨迹的采样时间,单位为秒。 +/// 角度制关节目标。 +/// 该点写出的 IO 掩码。 +/// 该点写出的 IO 值。 +public sealed record OfflineFlyshotSendPoint( + long SampleIndex, + double SendTime, + double TrajectoryTime, + IReadOnlyList JointsDegrees, + ushort IoMask, + ushort IoValue); + +/// +/// 表示离线规划结果中的触发采样点绑定。 +/// +/// 原始示教路点索引。 +/// 理论触发时间,单位为秒。 +/// 最终发送点数组索引。 +/// 最终发送序号。 +/// 是否在理论搜索窗口内完成绑定。 +/// 触发 IO 掩码。 +/// 触发 IO 值。 +public sealed record OfflineFlyshotTriggerBinding( + int WaypointIndex, + double TriggerTime, + int SampleIndex, + long SampleSequence, + bool FoundInWindow, + ushort IoMask, + ushort IoValue); + +/// +/// 表示最终发送点列的离散运动学诊断统计。 +/// +/// 最大绝对速度,单位 deg/s。 +/// 最大绝对加速度,单位 deg/s²。 +/// 最大绝对跃度,单位 deg/s³。 +public sealed record OfflineFlyshotDiagnostics( + double MaxAbsVelocityDegreesPerSecond, + double MaxAbsAccelerationDegreesPerSecondSquared, + double MaxAbsJerkDegreesPerSecondCubed); diff --git a/src/Flyshot.Core.Planning/Sampling/FlyshotExecutionSendSequenceBuilder.cs b/src/Flyshot.Core.Planning/Sampling/FlyshotExecutionSendSequenceBuilder.cs index a29c776..758b618 100644 --- a/src/Flyshot.Core.Planning/Sampling/FlyshotExecutionSendSequenceBuilder.cs +++ b/src/Flyshot.Core.Planning/Sampling/FlyshotExecutionSendSequenceBuilder.cs @@ -39,7 +39,8 @@ public static class FlyshotExecutionSendSequenceBuilder throw new ArgumentOutOfRangeException(nameof(servoPeriodSeconds), "伺服周期必须是有限正数。"); } - var finalSpeedRatio = samplingSpeedRatio ?? 1.0; + // 未显式指定独立采样倍率时,最终发送队列必须原样使用请求倍率。 + var finalSpeedRatio = samplingSpeedRatio ?? requestedSpeedRatio; if (finalSpeedRatio <= 0.0 || double.IsNaN(finalSpeedRatio) || double.IsInfinity(finalSpeedRatio)) { throw new ArgumentOutOfRangeException(nameof(samplingSpeedRatio), "最终队列采样倍率必须是有限正数。"); @@ -56,7 +57,7 @@ public static class FlyshotExecutionSendSequenceBuilder robot, samples, trajectoryName: result.ProgramName, - validateJerk: false, + validateJerk: true, strictJerkTolerance: true); } catch (InvalidOperationException exception) diff --git a/src/Flyshot.Runtime.Common/IControllerRuntime.cs b/src/Flyshot.Runtime.Common/IControllerRuntime.cs index c3ae7e5..1d97833 100644 --- a/src/Flyshot.Runtime.Common/IControllerRuntime.cs +++ b/src/Flyshot.Runtime.Common/IControllerRuntime.cs @@ -14,12 +14,6 @@ public interface IControllerRuntime /// 兼容层传入的机器人名称。 void ResetRobot(RobotProfile robot, string robotName); - /// - /// 选择当前活动控制器类型。 - /// - /// 是否使用仿真控制器。 - void SetActiveController(bool sim); - /// /// 建立到控制器 IP 的连接。 /// diff --git a/src/Flyshot.Runtime.Fanuc/FanucControllerRuntime.cs b/src/Flyshot.Runtime.Fanuc/FanucControllerRuntime.cs index dcebca9..bc87462 100644 --- a/src/Flyshot.Runtime.Fanuc/FanucControllerRuntime.cs +++ b/src/Flyshot.Runtime.Fanuc/FanucControllerRuntime.cs @@ -12,7 +12,6 @@ namespace Flyshot.Runtime.Fanuc; /// /// FANUC 控制器运行时,将上层兼容层指令转换为三条真实 Socket 通道的交互。 -/// 仿真模式下仍保持内存桩行为,便于离线测试与回退。 /// public sealed class FanucControllerRuntime : IControllerRuntime, IDisposable { @@ -25,7 +24,6 @@ public sealed class FanucControllerRuntime : IControllerRuntime, IDisposable private RobotProfile? _robot; private string? _robotName; - private bool? _activeControllerIsSimulation; private string? _connectedRobotIp; private bool _isEnabled; private bool _isInMotion; @@ -88,7 +86,6 @@ public sealed class FanucControllerRuntime : IControllerRuntime, IDisposable DisconnectClients(); _robot = robot; _robotName = robotName; - _activeControllerIsSimulation = null; _connectedRobotIp = null; _isEnabled = false; _isInMotion = false; @@ -101,21 +98,6 @@ public sealed class FanucControllerRuntime : IControllerRuntime, IDisposable } } - /// - public void SetActiveController(bool sim) - { - _logger?.LogInformation("SetActiveController: sim={Sim}", sim); - lock (_stateLock) - { - EnsureRobotSetup(); - DisconnectClients(); - _activeControllerIsSimulation = sim; - _connectedRobotIp = null; - _isEnabled = false; - _isInMotion = false; - } - } - /// public void Connect(string robotIp) { @@ -124,21 +106,12 @@ public sealed class FanucControllerRuntime : IControllerRuntime, IDisposable throw new ArgumentException("控制器 IP 不能为空。", nameof(robotIp)); } - _logger?.LogInformation("Connect 开始: robotIp={RobotIp}, 仿真={IsSim}", robotIp, _activeControllerIsSimulation); + _logger?.LogInformation("Connect 开始: robotIp={RobotIp}", robotIp); lock (_stateLock) { - EnsureActiveControllerSelected(); - if (_activeControllerIsSimulation!.Value) - { - _connectedRobotIp = robotIp; - _isEnabled = false; - _isInMotion = false; - _logger?.LogInformation("Connect 完成(仿真): robotIp={RobotIp}", robotIp); - return; - } - - // 真机模式:顺序建立三条通道 — 状态 → 命令 → 运动。 + EnsureRobotSetup(); + // 顺序建立三条真机通道:状态、命令、运动。 _stateClient.ConnectAsync(robotIp, 10010).GetAwaiter().GetResult(); _commandClient.ConnectAsync(robotIp, 10012).GetAwaiter().GetResult(); _j519Client.ConnectAsync(robotIp, 60015).GetAwaiter().GetResult(); @@ -175,7 +148,7 @@ public sealed class FanucControllerRuntime : IControllerRuntime, IDisposable throw new ArgumentOutOfRangeException(nameof(bufferSize), "buffer_size 必须大于 0。"); } - _logger?.LogInformation("EnableRobot 开始: bufferSize={BufferSize}, 仿真={IsSim}", bufferSize, _activeControllerIsSimulation); + _logger?.LogInformation("EnableRobot 开始: bufferSize={BufferSize}", bufferSize); lock (_stateLock) { @@ -183,14 +156,7 @@ public sealed class FanucControllerRuntime : IControllerRuntime, IDisposable _bufferSize = bufferSize; _j519Client.SetSequenceBufferSize(bufferSize); - if (IsSimulationMode) - { - _isEnabled = true; - _logger?.LogInformation("EnableRobot 完成(仿真)"); - return; - } - - // 真机模式:按 all-reconnect.pcap 的重连序列启动 RVBUSTSM,暂不发送 StopProg。 + // 按 all-reconnect.pcap 的重连序列启动 RVBUSTSM。 _commandClient.StopProgramAsync("RVBUSTSM").GetAwaiter().GetResult(); _commandClient.ResetRobotAsync().GetAwaiter().GetResult(); //_commandClient.GetProgramStatusAsync("RVBUSTSM").GetAwaiter().GetResult(); @@ -211,11 +177,8 @@ public sealed class FanucControllerRuntime : IControllerRuntime, IDisposable { EnsureRobotSetup(); CancelSendTaskLocked(); - if (!IsSimulationMode) - { - _j519Client.StopMotionAsync().GetAwaiter().GetResult(); - _commandClient.StopProgramAsync("RVBUSTSM").GetAwaiter().GetResult(); - } + _j519Client.StopMotionAsync().GetAwaiter().GetResult(); + _commandClient.StopProgramAsync("RVBUSTSM").GetAwaiter().GetResult(); _isEnabled = false; _isInMotion = false; @@ -231,10 +194,7 @@ public sealed class FanucControllerRuntime : IControllerRuntime, IDisposable { EnsureRobotSetup(); CancelSendTaskLocked(); - if (!IsSimulationMode) - { - _j519Client.StopMotionAsync().GetAwaiter().GetResult(); - } + _j519Client.StopMotionAsync().GetAwaiter().GetResult(); _isInMotion = false; } @@ -247,11 +207,8 @@ public sealed class FanucControllerRuntime : IControllerRuntime, IDisposable lock (_stateLock) { EnsureConnected(); - if (!IsSimulationMode) - { - var response = _commandClient.GetSpeedRatioAsync().GetAwaiter().GetResult(); - _speedRatio = response.Ratio; - } + var response = _commandClient.GetSpeedRatioAsync().GetAwaiter().GetResult(); + _speedRatio = response.Ratio; return _speedRatio; } @@ -271,11 +228,6 @@ public sealed class FanucControllerRuntime : IControllerRuntime, IDisposable { //EnsureConnected(); var clampedRatio = Math.Clamp(ratio, 0.0, 1.0); - if (!IsSimulationMode) - { - //_commandClient.SetSpeedRatioAsync(clampedRatio).GetAwaiter().GetResult(); - } - _speedRatio = clampedRatio; } @@ -288,7 +240,7 @@ public sealed class FanucControllerRuntime : IControllerRuntime, IDisposable lock (_stateLock) { EnsureRobotSetup(); - if (_activeControllerIsSimulation is false && !string.IsNullOrWhiteSpace(_connectedRobotIp)) + if (!string.IsNullOrWhiteSpace(_connectedRobotIp)) { var response = _commandClient.GetTcpAsync(1).GetAwaiter().GetResult(); _tcp = response.Pose.Take(3).ToArray(); @@ -304,11 +256,8 @@ public sealed class FanucControllerRuntime : IControllerRuntime, IDisposable lock (_stateLock) { EnsureRobotSetup(); - if (_activeControllerIsSimulation is false) - { - EnsureConnected(); - _commandClient.SetTcpAsync(1, CreateTcpPose(x, y, z)).GetAwaiter().GetResult(); - } + EnsureConnected(); + _commandClient.SetTcpAsync(1, CreateTcpPose(x, y, z)).GetAwaiter().GetResult(); _tcp = [x, y, z]; } @@ -327,14 +276,9 @@ public sealed class FanucControllerRuntime : IControllerRuntime, IDisposable lock (_stateLock) { EnsureConnected(); - if (!IsSimulationMode) - { - var response = _commandClient.GetIoAsync(port, normalizedIoType).GetAwaiter().GetResult(); - _ioValues[(normalizedIoType, port)] = response.Value; - return response.Value; - } - - return _ioValues.TryGetValue((normalizedIoType, port), out var value) && value; + var response = _commandClient.GetIoAsync(port, normalizedIoType).GetAwaiter().GetResult(); + _ioValues[(normalizedIoType, port)] = response.Value; + return response.Value; } } @@ -351,10 +295,7 @@ public sealed class FanucControllerRuntime : IControllerRuntime, IDisposable lock (_stateLock) { EnsureEnabled(); - if (!IsSimulationMode) - { - _commandClient.SetIoAsync(port, value, normalizedIoType).GetAwaiter().GetResult(); - } + _commandClient.SetIoAsync(port, value, normalizedIoType).GetAwaiter().GetResult(); _ioValues[(normalizedIoType, port)] = value; } @@ -366,7 +307,7 @@ public sealed class FanucControllerRuntime : IControllerRuntime, IDisposable lock (_stateLock) { EnsureRobotSetup(); - if (!IsSimulationMode) + if (!string.IsNullOrWhiteSpace(_connectedRobotIp)) { var frame = GetFreshStateFrame(); if (frame?.JointRadians.Count >= _jointPositions.Length) @@ -385,7 +326,7 @@ public sealed class FanucControllerRuntime : IControllerRuntime, IDisposable lock (_stateLock) { EnsureRobotSetup(); - if (!IsSimulationMode) + if (!string.IsNullOrWhiteSpace(_connectedRobotIp)) { var frame = GetFreshStateFrame(); if (frame?.CartesianPose.Count >= 6) @@ -414,7 +355,7 @@ public sealed class FanucControllerRuntime : IControllerRuntime, IDisposable bool? j519SystemReady = null; bool? j519RobotInMotion = null; - if (!IsSimulationMode) + if (!string.IsNullOrWhiteSpace(_connectedRobotIp)) { var frame = GetFreshStateFrame(); if (frame is not null) @@ -485,11 +426,11 @@ public sealed class FanucControllerRuntime : IControllerRuntime, IDisposable EnsureJointCount(finalJointPositions.Count); CancelSendTaskLocked(); - if (!IsSimulationMode && result.DenseJointTrajectory is not null) + if (result.DenseJointTrajectory is not null) { EnsureJ519ReadyForDenseExecution(); - // 真机模式且存在稠密路点:准备可被 StopMove 取消的同步发送任务。 + // 存在稠密路点时,准备可被 StopMove 取消的同步发送任务。 _isInMotion = true; _sendCts = new CancellationTokenSource(); denseSendCancellationSource = _sendCts; @@ -500,14 +441,11 @@ public sealed class FanucControllerRuntime : IControllerRuntime, IDisposable } else { - if (!IsSimulationMode) - { - // 真机模式无稠密路点:回退到单点收敛。 - var command = new FanucJ519Command( - sequence: 0, - targetJoints: finalJointPositions.Select(j => (double)j).ToArray()); - _j519Client.UpdateCommand(command); - } + // 无稠密路点时回退到单点收敛。 + var command = new FanucJ519Command( + sequence: 0, + targetJoints: finalJointPositions.Select(j => (double)j).ToArray()); + _j519Client.UpdateCommand(command); _isInMotion = true; _jointPositions = finalJointPositions.ToArray(); @@ -558,11 +496,11 @@ public sealed class FanucControllerRuntime : IControllerRuntime, IDisposable EnsurePoseCount(finalPose.Count); CancelSendTaskLocked(); - if (!IsSimulationMode && result.DenseCartesianTrajectory is not null) + if (result.DenseCartesianTrajectory is not null) { EnsureJ519ReadyForDenseExecution(); - // 真机直角模式同样预装完整队列,由机器人状态包节拍驱动出队。 + // 直角模式同样预装完整队列,由机器人状态包节拍驱动出队。 _isInMotion = true; _sendCts = new CancellationTokenSource(); denseSendCancellationSource = _sendCts; @@ -573,14 +511,11 @@ public sealed class FanucControllerRuntime : IControllerRuntime, IDisposable } else { - if (!IsSimulationMode) - { - var command = new FanucJ519Command( - sequence: 0, - targetValues: BuildCartesianTargetValues(finalPose), - dataStyle: 0); - _j519Client.UpdateCommand(command); - } + var command = new FanucJ519Command( + sequence: 0, + targetValues: BuildCartesianTargetValues(finalPose), + dataStyle: 0); + _j519Client.UpdateCommand(command); _isInMotion = true; _pose = MergeFinalCartesianPose(finalPose); @@ -1066,7 +1001,7 @@ public sealed class FanucControllerRuntime : IControllerRuntime, IDisposable } /// - /// 将六维 WPR 直角目标合并回运行时缓存,保留旧 GetPose 仿真路径的 7 维外形。 + /// 将六维 WPR 直角目标合并回运行时缓存,保留 GetPose 的 7 维兼容外形。 /// private double[] MergeFinalCartesianPose(IReadOnlyList pose) { @@ -1179,22 +1114,6 @@ public sealed class FanucControllerRuntime : IControllerRuntime, IDisposable return value; } - /// - /// 判断当前是否处于仿真模式;若尚未选择控制器则抛出异常。 - /// - private bool IsSimulationMode - { - get - { - if (_activeControllerIsSimulation is null) - { - return false; - } - - return _activeControllerIsSimulation.Value; - } - } - /// /// 断开所有真实 Socket 通道,不影响内存状态。 /// @@ -1252,9 +1171,7 @@ public sealed class FanucControllerRuntime : IControllerRuntime, IDisposable return "Disconnected"; } - return _activeControllerIsSimulation is false - ? ResolveRealConnectionState(_stateClient.GetStatus()) - : "Connected"; + return ResolveRealConnectionState(_stateClient.GetStatus()); } /// @@ -1327,24 +1244,12 @@ public sealed class FanucControllerRuntime : IControllerRuntime, IDisposable } } - /// - /// 校验活动控制器已经被选择。 - /// - private void EnsureActiveControllerSelected() - { - EnsureRobotSetup(); - if (_activeControllerIsSimulation is null) - { - throw new InvalidOperationException("Active controller has not been selected."); - } - } - /// /// 校验控制器已经建立连接。 /// private void EnsureConnected() { - EnsureActiveControllerSelected(); + EnsureRobotSetup(); if (string.IsNullOrWhiteSpace(_connectedRobotIp)) { throw new InvalidOperationException("Controller has not been connected."); diff --git a/src/Flyshot.Server.Host/Controllers/LegacyHttpApiController.cs b/src/Flyshot.Server.Host/Controllers/LegacyHttpApiController.cs index 38a60d2..7cce389 100644 --- a/src/Flyshot.Server.Host/Controllers/LegacyHttpApiController.cs +++ b/src/Flyshot.Server.Host/Controllers/LegacyHttpApiController.cs @@ -25,59 +25,6 @@ public sealed class LegacyHttpApiController : ControllerBase _logger = logger ?? throw new ArgumentNullException(nameof(logger)); } - /// - /// 兼容旧根路径探活接口。 - /// - /// 旧 HTTP 服务约定的 Hello World 响应。 - [HttpGet("/")] - public IActionResult Root() - { - return Ok(new { message = "Hello World" }); - } - - /// - /// 兼容旧 `/connect_server/` 路由;在 replacement 宿主中仅记录调用方期望连接的地址。 - /// - /// 旧客户端传入的服务端 IP。 - /// 旧客户端传入的服务端端口。 - /// 与旧 FastAPI 层一致的状态响应。 - [HttpPost("/connect_server/")] - public IActionResult ConnectServer([FromQuery] string server_ip, [FromQuery] int port) - { - _logger.LogInformation("ConnectServer 调用: server_ip={ServerIp}, port={Port}", server_ip, port); - try - { - _compatService.ConnectServer(server_ip, port); - _logger.LogInformation("ConnectServer 成功: server_ip={ServerIp}, port={Port}", server_ip, port); - return Ok(new { status = "connected" }); - } - catch (Exception exception) - { - _logger.LogError(exception, "ConnectServer 失败: server_ip={ServerIp}, port={Port}", server_ip, port); - return LegacyBadRequest("Connect Server failed"); - } - } - - /// - /// 兼容旧 `GetServerVersion` 版本查询语义。 - /// - /// 服务端版本号。 - [HttpGet("/get_server_version/")] - public IActionResult GetServerVersion() - { - return Ok(new { server_version = _compatService.GetServerVersion() }); - } - - /// - /// 兼容旧 `GetClientVersion` 版本查询语义。 - /// - /// 客户端版本号。 - [HttpGet("/get_client_version/")] - public IActionResult GetClientVersion() - { - return Ok(new { client_version = _compatService.GetClientVersion() }); - } - /// /// 兼容旧 `/setup_robot/` 路由。 /// @@ -100,59 +47,6 @@ public sealed class LegacyHttpApiController : ControllerBase } } - /// - /// 兼容旧 `SetUpRobotFromEnv(env_file)` 参数形状。 - /// - /// 环境文件路径。 - /// 旧 FastAPI 层风格的状态响应。 - [HttpPost("/setup_robot_from_env/")] - public IActionResult SetupRobotFromEnv([FromQuery] string env_file) - { - try - { - _compatService.SetUpRobotFromEnv(env_file); - return Ok(new { status = "robot setup" }); - } - catch - { - return LegacyBadRequest("SetUpRobotFromEnv failed"); - } - } - - /// - /// 兼容旧 `SetShowTCP(is_show, axis_length, axis_size)` 参数形状。 - /// - /// 是否显示 TCP。 - /// 坐标轴长度。 - /// 坐标轴线宽。 - /// 旧 FastAPI 层风格的状态响应。 - [HttpPost("/set_show_tcp/")] - public IActionResult SetShowTcp( - [FromQuery] bool is_show = true, - [FromQuery] double axis_length = 0.1, - [FromQuery] int axis_size = 2) - { - try - { - _compatService.SetShowTcp(is_show, axis_length, axis_size); - return Ok(new { status = "show TCP set" }); - } - catch - { - return LegacyBadRequest("SetShowTCP failed"); - } - } - - /// - /// 兼容旧 `/is_setup/` 路由。 - /// - /// 当前机器人是否完成初始化。 - [HttpGet("/is_setup/")] - public IActionResult IsSetup() - { - return Ok(new { is_setup = _compatService.IsSetUp }); - } - /// /// 兼容旧 `EnableRobot(buffer_size=4)` 参数形状。 /// @@ -217,28 +111,6 @@ public sealed class LegacyHttpApiController : ControllerBase } } - /// - /// 兼容旧 `/set_active_controller/` 路由。 - /// - /// 是否切到仿真控制器。 - /// 旧 FastAPI 层风格的状态响应。 - [HttpPost("/set_active_controller/")] - public IActionResult SetActiveController([FromQuery] bool sim) - { - _logger.LogInformation("SetActiveController 调用: sim={Sim}", sim); - try - { - _compatService.SetActiveController(sim); - _logger.LogInformation("SetActiveController 成功: sim={Sim}", sim); - return Ok(new { status = "active controller set" }); - } - catch (Exception exception) - { - _logger.LogError(exception, "SetActiveController 失败: sim={Sim}", sim); - return LegacyBadRequest("SetActiveController failed"); - } - } - /// /// 兼容旧 `/connect_robot/` 路由。 /// @@ -487,11 +359,6 @@ public sealed class LegacyHttpApiController : ControllerBase public IActionResult ListFlyshotTrajectories() { var names = _compatService.ListTrajectoryNames(); - if (names.Count == 0) - { - return LegacyBadRequest("ListFlyShotTraj failed"); - } - return Ok(new { flyshot_trajs = names }); } @@ -623,9 +490,9 @@ public sealed class LegacyHttpApiController : ControllerBase _logger.LogInformation("SaveTrajectoryInfo 调用: name={Name}, method={Method}", request.name, request.method); try { - _compatService.SaveTrajectoryInfo(request.name, request.method); + var result = _compatService.SaveTrajectoryInfo(request.name, request.method); _logger.LogInformation("SaveTrajectoryInfo 成功: name={Name}", request.name); - return Ok(new { status = "trajectory info saved", success = true }); + return Ok(result); } catch (NotSupportedException exception) { @@ -723,36 +590,26 @@ public sealed class LegacyHttpApiController : ControllerBase } /// - /// 兼容旧 `/init_mpc_robt` 路由,保留历史拼写。 + /// 使用机器人名称和 IP 执行真机优先初始化。 /// /// 初始化请求体。 /// 旧 FastAPI 层风格的初始化结果。 - [HttpPost("/init_mpc_robt")] - public IActionResult InitMpcRobot([FromBody] LegacyInitMpcRobotRequest data) + [HttpPost("/init_mpc_robot")] + public IActionResult InitMpcRobot([FromBody] InitMpcRobotRequest data) { _logger.LogInformation( - "InitMpcRobot 调用: robot_name={RobotName}, robot_ip={RobotIp}, sim={Sim}, server={ServerIp}:{Port}", - data.robot_name, data.robot_ip, data.sim, data.server_ip, data.port); + "InitMpcRobot 调用: robot_name={RobotName}, robot_ip={RobotIp}", + data.robot_name, data.robot_ip); try { - - _compatService.SetUpRobot(data.robot_name); - if (!_compatService.IsSetUp) - { - _logger.LogWarning("InitMpcRobot 失败: Robot not setup"); - return LegacyBadRequest("Robot not setup"); - } - - _compatService.SetActiveController(data.sim); - _compatService.Connect(data.robot_ip); - _compatService.EnableRobot(8); + _compatService.InitializeRobot(data.robot_name, data.robot_ip, bufferSize: 8); _logger.LogInformation("InitMpcRobot 成功: robot_name={RobotName}", data.robot_name); return Ok(new { message = "init_Success", returnCode = 0 }); } catch (Exception exception) { _logger.LogError(exception, "InitMpcRobot 失败"); - return LegacyBadRequest("Connect Server failed"); + return BadRequest(new { detail = exception.Message }); } } @@ -1173,20 +1030,10 @@ public sealed class LegacySpeedRatioRequest } /// -/// 表示旧 `/init_mpc_robt` 路由使用的初始化请求体。 +/// 表示 `/init_mpc_robot` 路由使用的真机初始化请求体。 /// -public sealed class LegacyInitMpcRobotRequest +public sealed class InitMpcRobotRequest { - /// - /// 获取或设置目标服务端 IP。 - /// - public string server_ip { get; init; } = string.Empty; - - /// - /// 获取或设置目标服务端端口。 - /// - public int port { get; init; } - /// /// 获取或设置机器人名称。 /// @@ -1197,8 +1044,4 @@ public sealed class LegacyInitMpcRobotRequest /// public string robot_ip { get; init; } = string.Empty; - /// - /// 获取或设置是否使用仿真控制器;默认 false 连接真机。 - /// - public bool sim { get; init; } } diff --git a/src/Flyshot.Server.Host/Controllers/StatusController.cs b/src/Flyshot.Server.Host/Controllers/StatusController.cs index 7e466b3..e47d875 100644 --- a/src/Flyshot.Server.Host/Controllers/StatusController.cs +++ b/src/Flyshot.Server.Host/Controllers/StatusController.cs @@ -61,6 +61,15 @@ public sealed class StatusController : ControllerBase RobotName = metadata.RobotName, DegreesOfFreedom = metadata.DegreesOfFreedom, UploadedTrajectories = metadata.UploadedTrajectories, + OperatingMode = metadata.OperatingMode, + InitializationStage = metadata.InitializationStage, + ModelLoaded = metadata.IsSetup, + RobotIp = metadata.RobotIp, + LastConnectionError = metadata.LastConnectionError, + LastConnectionErrorAt = metadata.LastConnectionErrorAt, + LastInitializationAttemptAt = metadata.LastInitializationAttemptAt, + CanPlanOffline = metadata.CanPlanOffline, + CanExecuteRealMotion = metadata.CanExecuteRealMotion, Snapshot = snapshot }); } diff --git a/src/Flyshot.Server.Host/Services/AutoInitRobotHostedService.cs b/src/Flyshot.Server.Host/Services/AutoInitRobotHostedService.cs index 266710f..c9f3fd4 100644 --- a/src/Flyshot.Server.Host/Services/AutoInitRobotHostedService.cs +++ b/src/Flyshot.Server.Host/Services/AutoInitRobotHostedService.cs @@ -5,7 +5,7 @@ namespace Flyshot.Server.Host.Services; /// /// 服务启动时自动初始化机器人的托管服务。 -/// 从配置文件读取 robot_name 和 robot_ip,依次执行 SetUpRobot、SetActiveController、Connect、EnableRobot。 +/// 从配置文件读取 robot_name 和 robot_ip,执行统一的真机优先初始化流程。 /// 初始化失败不会阻止服务启动,仅记录警告日志。 /// public sealed class AutoInitRobotHostedService : IHostedService @@ -54,22 +54,8 @@ public sealed class AutoInitRobotHostedService : IHostedService try { - // 步骤1:加载机器人配置 - _compatService.SetUpRobot(autoInit.RobotName); - if (!_compatService.IsSetUp) - { - _logger.LogWarning("自动初始化失败: SetUpRobot 返回 IsSetUp=false,robot_name={RobotName}", autoInit.RobotName); - return Task.CompletedTask; - } - - // 步骤2:设置控制器模式(默认非仿真) - _compatService.SetActiveController(sim: false); - - // 步骤3:连接机器人控制器 - _compatService.Connect(autoInit.RobotIp); - - // 步骤4:使能机器人(上伺服) - _compatService.EnableRobot(8); + // 启动与人工初始化复用同一编排,确保状态阶段和失败降级语义一致。 + _compatService.InitializeRobot(autoInit.RobotName, autoInit.RobotIp, bufferSize: 8); _logger.LogInformation("自动初始化成功: robot_name={RobotName}", autoInit.RobotName); } diff --git a/src/Flyshot.Server.Host/wwwroot/assets/debug.css b/src/Flyshot.Server.Host/wwwroot/assets/debug.css index 03f6c3d..0e383a4 100644 --- a/src/Flyshot.Server.Host/wwwroot/assets/debug.css +++ b/src/Flyshot.Server.Host/wwwroot/assets/debug.css @@ -422,3 +422,64 @@ details > summary { max-height: none; } } +.offline-panel { + margin: 16px 0 24px; + padding: 18px; + border: 1px solid rgba(56, 189, 248, 0.35); + border-radius: 12px; + background: rgba(15, 23, 42, 0.72); +} + +.offline-warning { + color: #fbbf24; +} + +.offline-grid { + display: grid; + grid-template-columns: minmax(220px, 1fr) auto minmax(220px, 1fr) auto; + gap: 10px; + align-items: end; +} + +.offline-grid label { + display: grid; + gap: 6px; + color: var(--muted); +} + +.offline-grid input, +.offline-grid select { + min-height: 38px; +} + +.offline-table-wrap { + max-height: 520px; + overflow: auto; + margin-top: 12px; +} + +#offline-send-table { + width: 100%; + border-collapse: collapse; + font-size: 12px; +} + +#offline-send-table th, +#offline-send-table td { + padding: 7px; + border-bottom: 1px solid rgba(148, 163, 184, 0.18); + text-align: left; + white-space: nowrap; +} + +.offline-summary { + display: grid; + grid-template-columns: max-content 1fr; + gap: 6px 16px; +} + +@media (max-width: 900px) { + .offline-grid { + grid-template-columns: 1fr; + } +} diff --git a/src/Flyshot.Server.Host/wwwroot/assets/debug.js b/src/Flyshot.Server.Host/wwwroot/assets/debug.js index 82f87c4..3299079 100644 --- a/src/Flyshot.Server.Host/wwwroot/assets/debug.js +++ b/src/Flyshot.Server.Host/wwwroot/assets/debug.js @@ -5,10 +5,10 @@ const HISTORY_LIMIT = 10; const STATIC_DOCS_ELEMENT_ID = "debug-static-docs"; const groupTitleByPrefix = [ - // 基础与状态分组:探活和状态快照两个固定 API 路径 - { match: function (op) { return op.path === "/healthz" || op.path === "/api/status/snapshot"; }, title: "基础与状态" }, - // 默认兜底:剩余全部走 ControllerClient 兼容分组 - { match: function () { return true; }, title: "ControllerClient 兼容" } + { match: function (op) { return ["/healthz", "/status", "/debug", "/api/status/snapshot"].indexOf(op.path) >= 0; }, title: "基础状态" }, + { match: function (op) { return ["/setup_robot/", "/init_mpc_robot", "/connect_robot/", "/enable_robot/", "/disable_robot/", "/disconnect_robot/", "/stop_move/"].indexOf(op.path) >= 0; }, title: "初始化与维护" }, + { match: function (op) { return ["/move_joint/", "/move_pose/", "/set_tcp/", "/get_tcp/", "/set_io/", "/get_io/", "/get_joint_position/", "/get_pose", "/robot_info/"].indexOf(op.path) >= 0; }, title: "机器人操作" }, + { match: function () { return true; }, title: "轨迹与飞拍" } ]; const state = { @@ -19,6 +19,10 @@ const state = { }; const requestBodySamples = { + "POST /init_mpc_robot": { + robot_name: "FANUC_LR_Mate_200iD", + robot_ip: "192.168.10.101" + }, "POST /move_pose/": { x: 100.0, y: 200.0, @@ -26,9 +30,19 @@ const requestBodySamples = { w: 0.0, p: 45.0, r: 0.0 + }, + "POST /save_traj_info/": { + name: "demo-flyshot", + method: "icsp" } }; +const offlinePlanningState = { + result: null, + page: 1, + pageSize: 50 +}; + /** 简单的 escape:把任意字符串安全嵌入 textContent 之外的位置时使用。 */ function escapeHtml(value) { return String(value).replace(/[&<>"']/g, function (ch) { @@ -781,9 +795,9 @@ function renderGroups(operations) { grouped.get(groupTitle).push(op); }); - // 固定输出顺序:基础与状态在前,ControllerClient 兼容在后,其余按字典序。 + // 固定输出顺序与现场操作顺序一致。 const orderedTitles = []; - ["基础与状态", "ControllerClient 兼容"].forEach(function (title) { + ["基础状态", "初始化与维护", "机器人操作", "轨迹与飞拍"].forEach(function (title) { if (grouped.has(title)) orderedTitles.push(title); }); Array.from(grouped.keys()).sort().forEach(function (title) { @@ -807,6 +821,144 @@ function renderGroups(operations) { }); } +/** 调用 JSON API,并在非 2xx 时把服务端 detail 作为异常抛出。 */ +async function requestJson(url, init) { + const response = await fetch(url, init || {}); + const text = await response.text(); + let payload = null; + if (text) { + try { payload = JSON.parse(text); } catch (error) { payload = { detail: text }; } + } + if (!response.ok) { + throw new Error(payload && payload.detail ? payload.detail : "HTTP " + response.status); + } + return payload; +} + +/** 刷新固定 RobotConfig.json 中的飞拍轨迹下拉列表。 */ +async function refreshOfflineTrajectoryList() { + const select = document.getElementById("offline-trajectory-select"); + const payload = await requestJson("/list_flyShotTraj/"); + const names = payload.flyshot_trajs || []; + select.innerHTML = ""; + names.forEach(function (name) { + const option = document.createElement("option"); + option.value = name; + option.textContent = name; + select.appendChild(option); + }); +} + +/** 按页渲染最终发送点,避免大点列一次性创建过多 DOM。 */ +function renderOfflineSendPage() { + const result = offlinePlanningState.result; + const body = document.querySelector("#offline-send-table tbody"); + body.innerHTML = ""; + if (!result) { + document.getElementById("offline-page-info").textContent = "0 / 0"; + return; + } + + const points = result.finalSendPoints || []; + const pageCount = Math.max(1, Math.ceil(points.length / offlinePlanningState.pageSize)); + offlinePlanningState.page = Math.min(Math.max(1, offlinePlanningState.page), pageCount); + const start = (offlinePlanningState.page - 1) * offlinePlanningState.pageSize; + points.slice(start, start + offlinePlanningState.pageSize).forEach(function (point) { + const row = document.createElement("tr"); + [ + point.sampleIndex, + Number(point.sendTime).toFixed(6), + Number(point.trajectoryTime).toFixed(6), + (point.jointsDegrees || []).map(function (value) { return Number(value).toFixed(4); }).join(", "), + point.ioMask, + point.ioValue + ].forEach(function (value) { + const cell = document.createElement("td"); + cell.textContent = value; + row.appendChild(cell); + }); + body.appendChild(row); + }); + document.getElementById("offline-page-info").textContent = offlinePlanningState.page + " / " + pageCount; +} + +/** 展示离线规划摘要、触发绑定、最终发送点和导出路径。 */ +function renderOfflinePlanningResult(result) { + offlinePlanningState.result = result; + offlinePlanningState.page = 1; + document.getElementById("offline-planning-summary").innerHTML = + "
" + + "
运行结果
" + escapeHtml(result.isValid ? "有效" : "无效") + "
" + + "
规划时长
" + Number(result.planningDurationSeconds).toFixed(6) + " s
" + + "
最终发送时长
" + Number(result.finalSendDurationSeconds).toFixed(6) + " s
" + + "
关键/稠密/发送点
" + result.plannedWaypointCount + " / " + result.densePointCount + " / " + result.finalSendPointCount + "
" + + "
速度倍率
" + result.speedRatio + "
" + + "
"; + document.getElementById("offline-shot-events").textContent = + "ShotEvents / TriggerBindings\n" + JSON.stringify({ + shotEvents: result.shotEvents, + triggerBindings: result.triggerBindings + }, null, 2); + document.getElementById("offline-artifact-paths").textContent = + "导出文件\n" + JSON.stringify(result.artifactPaths, null, 2); + renderOfflineSendPage(); +} + +/** 初始化离线规划操作面板。 */ +function initializeOfflinePlanningPanel() { + const status = document.getElementById("offline-planning-status"); + document.getElementById("offline-load-model").addEventListener("click", async function () { + try { + const robotName = document.getElementById("offline-robot-name").value.trim(); + await requestJson("/setup_robot/?robot_name=" + encodeURIComponent(robotName), { method: "POST" }); + status.textContent = "模型已加载,当前可离线规划。"; + } catch (error) { + status.textContent = "模型加载失败: " + error.message; + } + }); + document.getElementById("offline-refresh-trajectories").addEventListener("click", async function () { + try { await refreshOfflineTrajectoryList(); status.textContent = "轨迹列表已刷新。"; } + catch (error) { status.textContent = "刷新失败: " + error.message; } + }); + document.getElementById("offline-upload").addEventListener("click", async function () { + try { + const body = document.getElementById("offline-upload-json").value; + await requestJson("/upload_flyshot/", { method: "POST", headers: { "Content-Type": "application/json" }, body: body }); + await refreshOfflineTrajectoryList(); + status.textContent = "轨迹已上传到固定 RobotConfig.json。"; + } catch (error) { + status.textContent = "上传失败: " + error.message; + } + }); + document.getElementById("offline-generate").addEventListener("click", async function () { + try { + const name = document.getElementById("offline-trajectory-select").value; + const method = document.getElementById("offline-planning-method").value; + status.textContent = "正在生成最终发送轨迹..."; + const result = await requestJson("/save_traj_info/", { + method: "POST", + headers: { "Content-Type": "application/json" }, + body: JSON.stringify({ name: name, method: method }) + }); + renderOfflinePlanningResult(result); + status.textContent = "离线规划完成,未向机器人发送命令。"; + } catch (error) { + status.textContent = "离线规划失败: " + error.message; + } + }); + document.getElementById("offline-prev-page").addEventListener("click", function () { + offlinePlanningState.page--; + renderOfflineSendPage(); + }); + document.getElementById("offline-next-page").addEventListener("click", function () { + offlinePlanningState.page++; + renderOfflineSendPage(); + }); + refreshOfflineTrajectoryList().catch(function (error) { + status.textContent = "轨迹列表加载失败: " + error.message; + }); +} + /** 加载 OpenAPI 文档并渲染。 */ async function loadSpecAndRender() { const metaSpec = document.getElementById("meta-spec-url"); @@ -854,4 +1006,5 @@ document.getElementById("history-clear").addEventListener("click", function () { }); renderHistory(); +initializeOfflinePlanningPanel(); loadSpecAndRender(); diff --git a/src/Flyshot.Server.Host/wwwroot/debug.html b/src/Flyshot.Server.Host/wwwroot/debug.html index 8f8859d..2ef4c4f 100644 --- a/src/Flyshot.Server.Host/wwwroot/debug.html +++ b/src/Flyshot.Server.Host/wwwroot/debug.html @@ -28,6 +28,58 @@
初始化中...
+
+

离线飞拍规划

+

生成轨迹不会向机器人发送命令;只有 RealReady 状态才能执行真实运动。

+
+ + + + + + +
+
+ 上传飞拍轨迹 JSON + + +
+
等待离线规划操作。
+
+
+ + + + + +
序号发送时间轨迹时间关节角(deg)IO MaskIO Value
+
+
+ + 0 / 0 + +
+

+      

+    
正在加载接口列表...
@@ -41,11 +93,6 @@