diff --git a/cmvr-es/common/types/agv/agv_types.h b/cmvr-es/common/types/agv/agv_types.h index 5c7ef69c..0e01a02f 100644 --- a/cmvr-es/common/types/agv/agv_types.h +++ b/cmvr-es/common/types/agv/agv_types.h @@ -2,6 +2,7 @@ #define CMVR_ES_AGV_TYPES_H #include +#include #include #include #include @@ -114,7 +115,13 @@ struct AgvMotionOptions { double reach_distance{0.0}; double reach_angle{0.0}; double speed_ratio{1.0}; - bool asynchronous{true}; + // 导航默认同步阻塞；调用方只有显式设为 true 才在任务接受后立即返回。 + bool asynchronous{false}; + int wait_timeout_ms{0}; + int poll_interval_ms{0}; + // 不带 RPC 框架依赖的取消检查。同步导航等待期间可由 + // 上层绑定 deadline/cancel；驱动不得在函数返回后保留该回调。 + std::function cancellation_requested; }; /** @@ -220,7 +227,7 @@ struct AgvPathSegment { /** * @brief AGV 扫图过程中产生的数据文件。 * - * content 可保存控制器返回的二进制内容，例如 SRC1100 的 rawmap zip 包。 + * content 可保存控制器返回的二进制内容，例如 SEER Robokit 的 rawmap zip 包。 */ struct AgvMappingDataFile { std::string name; diff --git a/cmvr-es/config/devices/agv/src1100.pb.txt b/cmvr-es/config/devices/agv/seer_robokit.pb.txt similarity index 91% rename from cmvr-es/config/devices/agv/src1100.pb.txt rename to cmvr-es/config/devices/agv/seer_robokit.pb.txt index 16867ce7..a22209e9 100644 --- a/cmvr-es/config/devices/agv/src1100.pb.txt +++ b/cmvr-es/config/devices/agv/seer_robokit.pb.txt @@ -8,8 +8,9 @@ agv { } agvs { + # 当前部署的控制器型号为 SRC1100；该值是设备实例 ID，不是后端类型名。 id: "src1100" - src1100_agv { + seer_robokit_agv { ip: "192.168.192.5" port_status: 19204 port_control: 19205 diff --git a/cmvr-es/config/manager/device_manager.pb.txt b/cmvr-es/config/manager/device_manager.pb.txt index de5de1da..07204d9f 100644 --- a/cmvr-es/config/manager/device_manager.pb.txt +++ b/cmvr-es/config/manager/device_manager.pb.txt @@ -136,9 +136,10 @@ device_manager { } devices { + # 当前部署的控制器型号为 SRC1100；该值是设备实例 ID，不是后端类型名。 id: "src1100" type: DEVICE_TYPE_AGV - config_file: "devices/agv/src1100.pb.txt" + config_file: "devices/agv/seer_robokit.pb.txt" enable: false } diff --git a/cmvr-es/devices/README.md b/cmvr-es/devices/README.md index 1fa6ac7c..2642c4f6 100644 --- a/cmvr-es/devices/README.md +++ b/cmvr-es/devices/README.md @@ -36,7 +36,7 @@ config/cmvr_es.pb.txt | 大类 | 抽象接口 | 类别工厂 | 当前可选后端 | | --- | --- | --- | --- | | Camera | [`camera/abstract_camera.h`](camera/abstract_camera.h) | [`camera/camera_factory.h`](camera/camera_factory.h) | UVC、RealSense、Hikvision | -| AGV | [`agv/abstract_agv.h`](agv/abstract_agv.h) | [`agv/agv_factory.h`](agv/agv_factory.h) | MyAgv、SRC1100 | +| AGV | [`agv/abstract_agv.h`](agv/abstract_agv.h) | [`agv/agv_factory.h`](agv/agv_factory.h) | MyAgv、SEER Robokit | | RobotArm | [`arm/robot_arm.h`](arm/robot_arm.h) | [`arm/robot_arm_factory.h`](arm/robot_arm_factory.h) | MotorRobotArm、[AUBO](arm/aubo_arm/README.md)、Huayan、UME | | DexHand | [`dexhand/abstract_dexhand.h`](dexhand/abstract_dexhand.h) | [`dexhand/dexhand_factory.h`](dexhand/dexhand_factory.h) | RH56DFTP、PX6AXGen3 | | Microphone | [`microphone/abstract_microphone.h`](microphone/abstract_microphone.h) | [`microphone/microphone_factory.h`](microphone/microphone_factory.h) | FFmpeg | diff --git a/cmvr-es/devices/agv/CMakeLists.txt b/cmvr-es/devices/agv/CMakeLists.txt index a0ca27e4..fe998a13 100644 --- a/cmvr-es/devices/agv/CMakeLists.txt +++ b/cmvr-es/devices/agv/CMakeLists.txt @@ -1,5 +1,5 @@ add_subdirectory(my_agv) -add_subdirectory(src1100) +add_subdirectory(seer_robokit) add_library(agv INTERFACE) @@ -8,7 +8,7 @@ target_include_directories(agv INTERFACE ${CMAKE_CURRENT_SOURCE_DIR}) target_link_libraries(agv INTERFACE cmvr_es::device::my_agv - cmvr_es::device::src1100_agv + cmvr_es::device::seer_robokit_agv cmvr_es::proto ) diff --git a/cmvr-es/devices/agv/abstract_agv.h b/cmvr-es/devices/agv/abstract_agv.h index b8ad4a61..643fa974 100644 --- a/cmvr-es/devices/agv/abstract_agv.h +++ b/cmvr-es/devices/agv/abstract_agv.h @@ -85,11 +85,25 @@ public: /** * @brief 发起显式站点到站点路径导航任务。 */ - virtual AgvResult followPath(const std::vector& path) + virtual AgvResult followPath( + const std::vector& path) { (void)path; return AgvResult::failure(AgvErrorCode::UnsupportedCommand, "followPath not implemented"); } + + /** + * @brief 发起显式站点到站点路径导航任务，并指定同步/异步选项。 + * + * 保留单参数虚函数以兼容已有派生类；旧实现会由本重载转发。 + */ + virtual AgvResult followPath( + const std::vector& path, + const AgvMotionOptions& options) + { + (void)options; + return followPath(path); + } /** * @brief 暂停当前导航任务，如果设备支持。 diff --git a/cmvr-es/devices/agv/agv_factory.h b/cmvr-es/devices/agv/agv_factory.h index 79b2f0a3..00e46415 100644 --- a/cmvr-es/devices/agv/agv_factory.h +++ b/cmvr-es/devices/agv/agv_factory.h @@ -7,7 +7,7 @@ #include "common/base/logging/logger.h" #include "devices/agv/abstract_agv.h" #include "devices/agv/my_agv/include/my_agv.h" -#include "devices/agv/src1100/include/src1100_agv.h" +#include "seer_robokit_agv.h" namespace cmvr::device { @@ -31,15 +31,15 @@ public: backend.set_id(cfg.id()); return std::make_shared(backend); } - case config::AGVDeviceConfig::kSrc1100Agv: + case config::AGVDeviceConfig::kSeerRobokitAgv: { - if (!cfg.src1100_agv().id().empty() && cfg.src1100_agv().id() != cfg.id()) { + if (!cfg.seer_robokit_agv().id().empty() && cfg.seer_robokit_agv().id() != cfg.id()) { CMVR_LOG(ERROR) << "[AGVFactory]: AGV id does not match backend id: " << cfg.id(); return nullptr; } - auto backend = cfg.src1100_agv(); + auto backend = cfg.seer_robokit_agv(); backend.set_id(cfg.id()); - return std::make_shared(backend); + return std::make_shared(backend); } case config::AGVDeviceConfig::BACKEND_NOT_SET: diff --git a/cmvr-es/devices/agv/seer_robokit/CMakeLists.txt b/cmvr-es/devices/agv/seer_robokit/CMakeLists.txt new file mode 100644 index 00000000..35ce8bad --- /dev/null +++ b/cmvr-es/devices/agv/seer_robokit/CMakeLists.txt @@ -0,0 +1,57 @@ +add_library(seer_robokit_agv SHARED + src/seer_robokit_agv.cpp + src/seer_robokit_transport.cpp + src/seer_robokit_control.cpp + src/seer_robokit_status.cpp + src/seer_robokit_navigation.cpp + src/seer_robokit_navigation_wait.cpp + src/seer_robokit_map.cpp + include/seer_robokit_agv.h + include/seer_robokit_protocol.h + include/seer_robokit_utils.h + include/seer_robokit_navigation_utils.h + include/seer_robokit_pgv_utils.h +) + +target_include_directories(seer_robokit_agv + PUBLIC + ${CMAKE_CURRENT_SOURCE_DIR}/include + ${PROJECT_SOURCE_DIR}/cmvr-es +) + +target_link_libraries(seer_robokit_agv + PUBLIC + cmvr_es::proto + jsoncpp +) + +add_library(cmvr_es::device::seer_robokit_agv ALIAS seer_robokit_agv) +install(TARGETS seer_robokit_agv LIBRARY DESTINATION lib) + +if(BUILD_TESTING) + add_executable(seer_robokit_control_authority_test + tests/seer_robokit_control_authority_test.cpp + ) + target_link_libraries(seer_robokit_control_authority_test + PRIVATE + cmvr_es::device::seer_robokit_agv + gtest + gtest_main + pthread + ) + add_test( + NAME seer_robokit_control_authority_test + COMMAND seer_robokit_control_authority_test + ) + set(_seer_robokit_control_authority_test_environment + "LD_LIBRARY_PATH=${CMVR_TEST_EXTERNAL_LIBRARY_PATH}" + ) + if(CMVR_TEST_SYSTEM_LIBSTDCXX) + list(APPEND _seer_robokit_control_authority_test_environment + "LD_PRELOAD=${CMVR_TEST_SYSTEM_LIBSTDCXX}") + endif() + set_tests_properties(seer_robokit_control_authority_test PROPERTIES + TIMEOUT 180 + ENVIRONMENT "${_seer_robokit_control_authority_test_environment}" + ) +endif() diff --git a/cmvr-es/devices/agv/seer_robokit/README.md b/cmvr-es/devices/agv/seer_robokit/README.md new file mode 100644 index 00000000..bd0b9460 --- /dev/null +++ b/cmvr-es/devices/agv/seer_robokit/README.md @@ -0,0 +1,394 @@ +# 仙工 SEER Robokit AGV 适配器 + +`SeerRobokitAgv` 将仙工 SEER Robokit TCP/IP API 适配为 CMVR 的通用 +`AbstractAGV`/`cmvr.api.AgvService`。厂商命令号、端口、抢占控制权、状态轮询、 +地图格式转换和错误码解析都封装在本目录内。 + +本项目现场使用的控制器型号仍是 SRC1100，所以设备实例 ID 保持为 +`src1100`；它只用于配置关联和 gRPC 路由，不再作为驱动实现名称。后端配置字段 +使用 `seer_robokit_agv`，目录、类、库和测试统一使用 `seer_robokit` / +`SeerRobokitAgv` 命名。 + +从旧版本升级时，外部部署配置必须同步使用 `seer_robokit_agv { ... }`，并把 +配置路径更新为 `devices/agv/seer_robokit.pb.txt`；设备实例 ID 保持不变。程序、 +外部配置和部署脚本需要原子升级，不能把旧字段或旧路径与新二进制混用。 + +返回 [Devices 模块指南](../../README.md) 或 [项目总览](../../../../README.md)。 + +## 代码与配置 + +所有驱动头文件统一放在 `include/`，实现文件统一放在 `src/`；测试源码独立放在 +`tests/`。除 `seer_robokit_agv.h` 外，其余头文件均为驱动内部实现细节。 + +- 公共类声明：[`include/seer_robokit_agv.h`](include/seer_robokit_agv.h) +- 导航轮询工具： + [`include/seer_robokit_navigation_utils.h`](include/seer_robokit_navigation_utils.h) +- PGV 参数转换： + [`include/seer_robokit_pgv_utils.h`](include/seer_robokit_pgv_utils.h) +- 协议常量：[`include/seer_robokit_protocol.h`](include/seer_robokit_protocol.h) +- 通用解析工具：[`include/seer_robokit_utils.h`](include/seer_robokit_utils.h) +- 生命周期和连接：[`src/seer_robokit_agv.cpp`](src/seer_robokit_agv.cpp) +- TCP 帧与收发：[`src/seer_robokit_transport.cpp`](src/seer_robokit_transport.cpp) +- 控制权与受控命令：[`src/seer_robokit_control.cpp`](src/seer_robokit_control.cpp) +- 状态与推送缓存：[`src/seer_robokit_status.cpp`](src/seer_robokit_status.cpp) +- 导航命令：[`src/seer_robokit_navigation.cpp`](src/seer_robokit_navigation.cpp) +- 阻塞等待与停车确认： + [`src/seer_robokit_navigation_wait.cpp`](src/seer_robokit_navigation_wait.cpp) +- 地图和建图：[`src/seer_robokit_map.cpp`](src/seer_robokit_map.cpp) +- 假控制器测试： + [`tests/seer_robokit_control_authority_test.cpp`](tests/seer_robokit_control_authority_test.cpp) +- 设备配置： + [`../../../config/devices/agv/seer_robokit.pb.txt`](../../../config/devices/agv/seer_robokit.pb.txt) +- DeviceManager 配置： + [`../../../config/manager/device_manager.pb.txt`](../../../config/manager/device_manager.pb.txt) +- gRPC API： + [`../../../../protos/cmvr/api/agv_service.proto`](../../../../protos/cmvr/api/agv_service.proto)、 + [`../../../../protos/cmvr/api/agv_command.proto`](../../../../protos/cmvr/api/agv_command.proto) + +## 配置和启动 + +现场配置至少需要修改控制器 IP；端口通常保持仙工默认值： + +```textproto +agv { + agvs { + id: "src1100" + seer_robokit_agv { + ip: "192.168.192.5" + port_status: 19204 + port_control: 19205 + port_nav: 19206 + port_config: 19207 + port_other: 19210 + port_push: 19301 + recv_timeout_ms: 1000 + control_nick_name: "cmvr-es" + enable_state_push: true + state_push_interval_ms: 200 + enable_map_update: true + map_update_interval_ms: 1000 + map_update_history_size: 8 + } + } +} +``` + +还要在 `device_manager.pb.txt` 中确认同一个设备 id，并在完成现场安全检查后把 +`enable` 改为 `true`。源码默认配置故意保持关闭。 + +```textproto +devices { + id: "src1100" + type: DEVICE_TYPE_AGV + config_file: "devices/agv/seer_robokit.pb.txt" + enable: true +} +``` + +构建、安装并启动： + +```bash +cmake -S . -B build -DCMAKE_BUILD_TYPE=Release +cmake --build build -j2 +cmake --install build +./output/bin/cmvr_es +``` + +`output/bin/cmvr_es` 默认读取 `output/bin/config/`。修改源码配置后需要重新安装， +或通过程序支持的外部配置入口启动，不能只修改源码文件后继续使用旧的 +`output/` 配置。 + +## 控制器端口和命令 + +| 端口 | 主要用途 | 当前使用的命令 | +| --- | --- | --- | +| `19204` | 状态、站点、地图和建图文件 | `1004`、`1007`、`1020`、`1101`、`1110`、`1300`、`1301`、`1780`、`1800` | +| `19205` | 底盘控制 | `2000`、`2010`、`2022` | +| `19206` | 导航任务 | `3001`、`3002`、`3003`、`3051`、`3066`、`3067` | +| `19207` | 控制权、地图上传下载 | `4005`、`4010`、`4011` | +| `19210` | 开始/停止建图 | `6100`、`6101` | +| `19301` | 机器人状态推送 | `9300`/`19300` 配置，`19301` 推送 | + +所有会改变机器人或控制器状态的调用都在 SEER Robokit 子类内部先通过 `4005` +抢权，负载为稳定的 `nick_name`，成功后才发送实际命令。普通命令集中走 +`sendControlledCommand_`；`emergencyStop` 为保证 `2000` 和导航取消之间不被 +插入其他命令，会在同一个控制序列锁内只抢一次权。只读查询不抢权。不要在 +gRPC 客户端另做一套租约逻辑。 + +## gRPC 接口概览 + +默认示例端点为 `127.0.0.1:50052`；远程部署时替换为 CMVR 服务所在主机， +不是 SEER Robokit 原生 TCP 端口。 + +| gRPC 方法 | SEER Robokit 行为 | 说明 | +| --- | --- | --- | +| `getRuntimeState` | 推送缓存，缺失时查询 `1004/1007/1300` | 只读 | +| `getNavigationStatus` | 跟踪任务查询 `1110`，无精确上下文时回退 `1020` | 只读；同步等待另用 `1101` 确认停车 | +| `emergencyStop` | `2000`，再执行 `3003` 或 `3067` | 软件停止，不替代硬件急停 | +| `clearFault` | 未实现 | 返回 `UnsupportedCommand` | +| `navigateToPose` | `3051` + `freeGo` | 地图绝对位姿，仅双轮差速底盘 | +| `navigateToStation` | `3051` | 站点路径导航；PGV 二次定位也使用此方法 | +| `followPath` | `3066` | 仙工“指定路径导航”，与 `3051` 不同 | +| `pauseNavigation` / `resumeNavigation` | `3001` / `3002` | 导航控制 | +| `cancelNavigation` | `3003`，路径队列使用 `3067` | 取消当前跟踪任务 | +| `setVelocity` / `stopVelocityControl` | `2010` | 车体速度；停止时发送全零速度 | +| `listMaps` / `listStations` | `1300` / `1301` | 只读 | +| `switchMap` | `2022` | 会改变定位所用地图 | +| `uploadMap` / `downloadMap` | `4010` / `4011` | 上传会抢权，下载只读 | +| `startMapping` / `stopMapping` | `6100` / `6101` | 建图控制 | +| `streamMap` | `1780/1800` 加内部解析和缓存 | 对外发送统一 2D/3D 地图，不暴露 `.smap` 原始格式 | + +查询运行状态： + +```bash +grpcurl -plaintext \ + -d '{"header":{"deviceId":"src1100"}}' \ + 127.0.0.1:50052 \ + cmvr.api.AgvService/getRuntimeState +``` + +查询导航状态： + +```bash +grpcurl -plaintext \ + -d '{"header":{"deviceId":"src1100"}}' \ + 127.0.0.1:50052 \ + cmvr.api.AgvService/getNavigationStatus +``` + +列出地图和当前地图站点： + +```bash +grpcurl -plaintext \ + -d '{"header":{"deviceId":"src1100"}}' \ + 127.0.0.1:50052 \ + cmvr.api.AgvService/listMaps + +grpcurl -plaintext \ + -d '{"header":{"deviceId":"src1100"}}' \ + 127.0.0.1:50052 \ + cmvr.api.AgvService/listStations +``` + +## 导航的同步语义 + +`navigateToPose`、`navigateToStation` 和 `followPath` 默认同步阻塞。控制器接受 +命令后，适配器继续轮询精确任务状态，并结合 `1101` 状态确认底盘已经停车； +到达、失败、取消、遇障停止或超时后才返回。`waitTimeoutMs` 为 `0` 时使用 +适配器默认值，当前为 10 分钟；`pollIntervalMs` 为 `0` 时当前使用 200 ms。 + +连续观察到障碍阻挡且底盘已经停止后，适配器会主动取消该导航；清理结果不明确 +时还可能发送软件停止。任务不会在障碍消失后由本次调用自动恢复。等待超时、 +RPC cancel 和 deadline 到期也会进入安全取消及停车确认，因此函数返回时间可能 +晚于最初发现障碍或取消请求的时刻。 + +调用方的 gRPC deadline 必须大于预计行程时间和 `waitTimeoutMs`。RPC 被取消或 +deadline 到期时，适配器会进入安全取消/停车确认流程。显式设置 +`"asynchronous":true` 后不会等待任务终态：站点导航和指定路径导航在控制器 +接受后返回；自由导航仍会做最长约 1.5 秒的启动确认。异步成功不代表已经到点。 + +`AgvMotionOptions` 中，SEER Robokit 的 `3051` 导航当前支持： + +| gRPC 字段 | 控制器字段 | 单位 | +| --- | --- | --- | +| `maxSpeed` | `max_speed` | m/s | +| `maxAngularSpeed` | `max_wspeed` | rad/s | +| `maxAcceleration` | `max_acc` | m/s² | +| `maxAngularAcceleration` | `max_wacc` | rad/s² | +| `reachDistance` | `reach_dist` | m | +| `reachAngle` | `reach_angle` | rad | + +`asynchronous`、`waitTimeoutMs` 和 `pollIntervalMs` 由适配器本地执行。 +`speedRatio` 当前没有对应的 SEER Robokit 序列化字段。`followPath` 的运动选项当前只 +控制同步/异步等待、超时和轮询；在没有确认 `3066` 的速度字段前，不会猜测性地 +写入每个路径段。 + +## 固定路径导航的 PGV 二次定位 + +仙工文档 [“路径导航 / 2. 固定路径导航 PGV 二次定位调整”](https://seer-group.feishu.cn/wiki/Q26SwaNoGisuLWk2vCxcPfVWn2e) +说明 PGV 参数是 `3051 / robot_task_gotarget_req` 的顶层可选字段。因此在 CMVR +中应调用 `navigateToStation`，不是 `followPath`。后者对应另一条 +`3066 / 指定路径导航` 协议，现有仙工资料和仓库历史都没有证明 `3066` 支持 +PGV 字段。 + +PGV 参数通过 `adapterParams.values` 传入。protobuf map 的值是字符串， +SEER Robokit 适配器会在任何状态查询、抢权和运动命令之前完成校验，再转换为控制器 +要求的 JSON `bool`/`number`： + +| `adapterParams.values` 键 | 输出 JSON 类型 | 含义 | +| --- | --- | --- | +| `use_pgv` | `bool` | 使用上视 PGV | +| `use_down_pgv` | `bool` | 使用下视 PGV | +| `pgv_adjust_dist` | `number` | 最大调整半径，必须为有限非负数；用于仙工第 3/4 种调整方式 | +| `pgv_adjust_cx` | `number` | 调整范围圆心在二维码坐标系下的 X 偏移；用于第 4 种方式 | +| `pgv_adjust_cy` | `number` | 调整范围圆心在二维码坐标系下的 Y 偏移；用于第 4 种方式 | +| `pgv_x_adjust` | `number` | 仅调整小车 X 方向误差；用于第 2 种方式 | + +所有数字都必须是完整、有限的数字字符串；偏移量允许正负。适配器不臆造 +调整半径上限，也不假定上视和下视一定互斥，这些约束应由实际 PGV 安装、标定和 +当前控制器版本确定。显式的 `"false"` 和 `"0"` 仍会作为原生布尔值和数值 +发给控制器；没有给出的字段不会发送。第 2/3/4 种方式由控制器和站点配置决定， +本接口只传递与所选方式匹配的调整参数。 + +一旦请求中出现任意 PGV 键，适配器只允许同时出现 `source_id`、`task_id` 和 +上述 PGV 字段；`operation`、`jack_height`、脚本名或未知扩展字段都会在状态 +查询和抢权前被拒绝，避免一次 PGV 导航意外夹带顶升、货叉、IO 或脚本动作。 +没有 PGV 键的既有站点导航扩展语义保持不变。 + +上视 PGV 示例。该命令会让机器人导航到 `AP1`，只能在确认地图、站点、PGV +标定、行驶区域和急停人员后执行： + +```bash +grpcurl -plaintext \ + -d '{ + "header":{"deviceId":"src1100"}, + "stationId":"AP1", + "options":{ + "maxSpeed":0.15, + "maxAcceleration":0.15, + "asynchronous":false, + "waitTimeoutMs":300000, + "pollIntervalMs":200 + }, + "adapterParams":{"values":{ + "use_pgv":"true", + "pgv_adjust_dist":"0.3", + "pgv_adjust_cx":"-0.3", + "pgv_adjust_cy":"0" + }} + }' \ + 127.0.0.1:50052 \ + cmvr.api.AgvService/navigateToStation +``` + +下视 PGV 使用同一接口，把 `use_down_pgv` 设为字符串 `"true"`；其他调整 +字段是否需要传入取决于现场定位方案。如果控制器版本要求明确起点，可在同一个 +map 中增加 `"source_id":"实际起点站点"`；默认起点为 `SELF_POSITION`。 + +仙工在线文档当前有两处拼写不一致： + +- 代码块出现了损坏字段 `pgv_adjustuse_pgv_dist`；适配器会拒绝它，正确字段是 + `pgv_adjust_dist`； +- 表格写成 `pgv_ajdust_cy`，而示例和仓库旧版序列化代码使用 + `pgv_adjust_cy`。适配器兼容接收前者，但只向控制器输出规范字段 + `pgv_adjust_cy`；两个拼写同时出现会因歧义被拒绝。 + +C++ 调用同样复用通用扩展参数： + +```cpp +cmvr::device::AgvMotionOptions options; +options.max_speed = 0.15; +options.max_acceleration = 0.15; + +cmvr::device::AgvAdapterParams adapter; +adapter.values["use_pgv"] = "true"; +adapter.values["pgv_adjust_dist"] = "0.3"; +adapter.values["pgv_adjust_cx"] = "-0.3"; +adapter.values["pgv_adjust_cy"] = "0"; + +const auto result = agv.navigateToStation("AP1", options, adapter); +``` + +## 其他导航和控制示例 + +自由导航使用地图绝对坐标，不是“相对当前位置移动多少米”。示例只展示请求 +结构，发送前必须读取当前位姿并确认目标在同一地图的安全区域： + +```bash +grpcurl -plaintext \ + -d '{ + "header":{"deviceId":"src1100"}, + "pose":{"x":1.0,"y":0.0,"theta":0.0}, + "options":{"maxSpeed":0.15,"maxAcceleration":0.15} + }' \ + 127.0.0.1:50052 \ + cmvr.api.AgvService/navigateToPose +``` + +显式站点路径使用 `3066`： + +```bash +grpcurl -plaintext \ + -d '{ + "header":{"deviceId":"src1100"}, + "path":[ + {"sourceStation":"LM1","targetStation":"LM2"}, + {"sourceStation":"LM2","targetStation":"AP1"} + ] + }' \ + 127.0.0.1:50052 \ + cmvr.api.AgvService/followPath +``` + +暂停、继续和取消的请求体直接是 `CommandHeader.Request`，没有外层 `header`： + +```bash +grpcurl -plaintext -d '{"deviceId":"src1100"}' \ + 127.0.0.1:50052 cmvr.api.AgvService/pauseNavigation + +grpcurl -plaintext -d '{"deviceId":"src1100"}' \ + 127.0.0.1:50052 cmvr.api.AgvService/resumeNavigation + +grpcurl -plaintext -d '{"deviceId":"src1100"}' \ + 127.0.0.1:50052 cmvr.api.AgvService/cancelNavigation +``` + +差速底盘的 `vy` 应保持 `0`。低层速度控制不等价于导航，并可能与已有任务 +冲突；只应在专门的速度控制测试流程中使用： + +```bash +grpcurl -plaintext \ + -d '{"header":{"deviceId":"src1100"},"velocity":{"vx":0.05,"vy":0,"wz":0}}' \ + 127.0.0.1:50052 \ + cmvr.api.AgvService/setVelocity + +grpcurl -plaintext -d '{"deviceId":"src1100"}' \ + 127.0.0.1:50052 cmvr.api.AgvService/stopVelocityControl +``` + +## 错误返回 + +控制器响应中的非零 `ret_code` 和 `err_msg` 会保留在 `AgvResult.message`，并由 +gRPC 同时写入 transport status message 和反馈头的 `errorMessage`。非 OK RPC +下，标准客户端通常不会交付响应体，因此跨客户端应以 transport status message +为准，不要依赖反馈头仍然可见。例如： + +```text +SEER Robokit command failed: ret_code=43051, err_msg=planner_rejected_pose +``` + +控制器仅返回“已接收”不等于导航完成；同步接口仍要等待精确任务终态和停车 +确认。若发送后连接中断且控制器是否执行已无法确定，错误会明确提示 outcome +unknown，调用方不能自动重发运动命令，应先查询状态并取消或停止。 + +## 安全边界 + +- 仙工文档明确把 `3051` 定位为任务链或验证测试等单车场景接口；不要把它当作 + 多车调度接口，否则可能出现路径/速度不连续等危险行为。 +- `emergencyStop` 是控制器软件停止，不是功能安全急停；真实系统必须保留可达的 + 硬件急停、安全激光、碰撞条和独立安全链。 +- 首次 PGV 测试应在低速、空载、隔离区域进行，并先核对二维码坐标系、传感器 + 上/下视方向、调整半径和中心偏移的标定值。 +- PGV 同步成功目前能证明精确 `3051` 任务进入终态，并连续确认两次零速度； + 仙工文档没有明确 `Completed` 是否一定覆盖 PGV 二次调整的全部阶段，仍需实机 + 验证后才能据此联动机械臂。异步成功更不代表 PGV 调整完成。 +- 地图切换、地图上传和开始建图会改变控制器状态，也会先抢占控制权；不要和 + 现场调度系统并行操作。 +- 本目录的假控制器测试验证软件协议、错误路径和并发逻辑，不代表真实 SEER Robokit、 + 底盘、PGV、地图或安全链已经验收。 + +## 测试 + +```bash +cmake --build build \ + --target seer_robokit_control_authority_test grpc_agv_service_test \ + -j2 + +ctest --test-dir build \ + -R '^(seer_robokit_control_authority_test|grpc_agv_service_test)$' \ + --output-on-failure +``` + +`seer_robokit_control_authority_test` 使用本机回环 TCP 假控制器，需要允许本地 +bind/listen。受限沙箱若禁止创建 socket，只能证明编译通过，不能把未执行的 +fake-controller 场景报告为测试通过。测试过程不会连接真实 AGV。 diff --git a/cmvr-es/devices/agv/src1100/include/src1100_agv.h b/cmvr-es/devices/agv/seer_robokit/include/seer_robokit_agv.h similarity index 67% rename from cmvr-es/devices/agv/src1100/include/src1100_agv.h rename to cmvr-es/devices/agv/seer_robokit/include/seer_robokit_agv.h index e9fa3a82..37028a0f 100644 --- a/cmvr-es/devices/agv/src1100/include/src1100_agv.h +++ b/cmvr-es/devices/agv/seer_robokit/include/seer_robokit_agv.h @@ -1,5 +1,5 @@ -#ifndef CMVR_ES_SRC1100_AGV_H -#define CMVR_ES_SRC1100_AGV_H +#ifndef CMVR_ES_SEER_ROBOKIT_AGV_H +#define CMVR_ES_SEER_ROBOKIT_AGV_H #include #include @@ -18,14 +18,14 @@ namespace cmvr::device { -class Src1100AgvTestPeer; +class SeerRobokitAgvTestPeer; -class Src1100Agv final : public AbstractAGV { +class SeerRobokitAgv final : public AbstractAGV { public: - explicit Src1100Agv(const config::Src1100AgvConfig& cfg); - ~Src1100Agv() override; + explicit SeerRobokitAgv(const config::SeerRobokitAgvConfig& cfg); + ~SeerRobokitAgv() override; - std::string typeName() const override { return "Src1100Agv"; } + std::string typeName() const override { return "SeerRobokitAgv"; } bool init() override; bool start() override; @@ -46,7 +46,11 @@ public: const std::string& station_id, const AgvMotionOptions& options = {}, const AgvAdapterParams& adapter_params = AgvAdapterParams{}) override; - AgvResult followPath(const std::vector& path) override; + AgvResult followPath( + const std::vector& path) override; + AgvResult followPath( + const std::vector& path, + const AgvMotionOptions& options) override; AgvResult pauseNavigation() override; AgvResult resumeNavigation() override; AgvResult cancelNavigation() override; @@ -67,7 +71,7 @@ public: AgvResult stopMapping() override; private: - friend class Src1100AgvTestPeer; + friend class SeerRobokitAgvTestPeer; struct Ports { int status{19204}; @@ -82,10 +86,46 @@ private: bool found{false}; int state{0}; int type{0}; + bool type_present{false}; double progress{0.0}; std::string detail; }; + struct NavigationSnapshot { + int task_status{0}; + int task_type{0}; + bool task_status_present{false}; + bool task_type_present{false}; + bool blocked{false}; + bool blocked_present{false}; + int block_reason{-1}; + std::string block_reason_raw; + bool velocity_present{false}; + double vx{0.0}; + double vy{0.0}; + double w{0.0}; + bool emergency{false}; + std::string target_id; + std::string active_faults; + std::string detail; + }; + + enum class CommandTransmissionState { + NotSent, + PossiblySent, + }; + + struct TrackedNavigationContext { + std::string token; + std::vector task_ids; + AgvTaskType type{AgvTaskType::None}; + std::string target_id; + std::vector target_ids; + std::uint64_t navigation_generation{0}; + std::chrono::steady_clock::time_point accepted_at{}; + bool synchronous_wait{false}; + }; + struct PoseTaskContext { std::string task_id; math::Pose2d target{}; @@ -99,6 +139,8 @@ private: AgvResult connect_(); AgvResult disconnect_(); + AgvResult emergencyStopTrackedNavigation_( + const TrackedNavigationContext* expected_navigation); AgvResult connectSocket_(int& sock, int port); AgvResult ensureOtherSocket_(); void closeSocket_(int& sock) const; @@ -106,10 +148,35 @@ private: AgvResult acquireControl_() const; AgvResult confirmPoseNavigationStarted_( - const PoseTaskContext& context) const; + const PoseTaskContext& context, + bool accept_paused, + const AgvMotionOptions& options) const; + AgvResult waitForPoseNavigationTerminal_( + const PoseTaskContext& pose_context, + const TrackedNavigationContext& navigation_context, + const AgvMotionOptions& options); + AgvResult waitForTrackedNavigationTerminal_( + const TrackedNavigationContext& context, + const AgvMotionOptions& options); + AgvResult queryNavigationSnapshot_(NavigationSnapshot& snapshot) const; + AgvResult cancelTrackedNavigation_( + const TrackedNavigationContext& context, + std::uint64_t& accepted_generation); + AgvResult waitForCanceledTaskToStop_( + const TrackedNavigationContext& context, + const AgvMotionOptions& options, + const std::string& reason); + AgvResult failAndCancelTrackedNavigation_( + const TrackedNavigationContext& context, + const AgvMotionOptions& options, + AgvErrorCode error_code, + const std::string& reason); AgvResult queryPoseTaskStatus_( const std::string& task_id, PoseTaskStatus& status) const; + AgvResult queryTaskStatuses_( + const std::vector& task_ids, + std::vector& statuses) const; bool poseTargetReached_( const PoseTaskContext& context, std::string& detail) const; @@ -127,7 +194,16 @@ private: void advancePoseTaskControlAttempt_( std::uint64_t control_attempt_sequence) const; void clearPoseTask_(std::uint64_t navigation_generation) const; + void clearPoseTaskIfTaskId_(const std::string& task_id) const; bool currentPoseTask_(PoseTaskContext& context) const; + void rememberTrackedNavigation_( + const TrackedNavigationContext& context) const; + void advanceTrackedNavigationGeneration_( + std::uint64_t navigation_generation) const; + void clearTrackedNavigation_(std::uint64_t navigation_generation) const; + void clearTrackedNavigationIfToken_(const std::string& token) const; + bool currentTrackedNavigation_( + TrackedNavigationContext& context) const; AgvResult sendControlledCommand_(int sock, std::uint16_t command, const Json::Value& payload, @@ -136,15 +212,22 @@ private: std::uint64_t* controller_fault_sequence_at_attempt = nullptr, std::uint64_t* control_attempt_sequence = nullptr, PoseTaskContext* pose_context_to_publish = nullptr, - bool reject_if_active_controller_fault = false) const; + bool reject_if_active_controller_fault = false, + TrackedNavigationContext* navigation_context_to_publish = nullptr, + bool preserve_tracked_navigation = false, + const std::string* expected_navigation_token = nullptr, + const std::function* cancellation_requested = nullptr, + const TrackedNavigationContext* expected_active_navigation = nullptr) const; AgvResult sendCommand_(int sock, std::uint16_t command, const Json::Value& payload, - Json::Value* response) const; + Json::Value* response, + CommandTransmissionState* transmission_state = nullptr) const; AgvResult sendCommandRaw_(int sock, std::uint16_t command, const Json::Value& payload, - std::string* response_payload) const; + std::string* response_payload, + CommandTransmissionState* transmission_state = nullptr) const; AgvResult sendCommandNoResponse_(int sock, std::uint16_t command, const Json::Value& payload) const; AgvResult configurePush_(); void startPushThread_(); @@ -162,17 +245,17 @@ private: const std::string& content, const AgvMapStreamOptions& options, std::vector& updates) const; - AgvResult parseSrc1100MapArchive_( + AgvResult parseSeerRobokitMapArchive_( const std::string& file_name, const std::string& content, const AgvMapStreamOptions& options, std::vector& updates) const; - AgvResult parseSrc1100Map2D_( + AgvResult parseSeerRobokitMap2D_( const std::string& file_name, const std::string& content, const AgvMapStreamOptions& options, AgvUnifiedMapUpdate& update) const; - AgvResult parseSrc1100Map3D_( + AgvResult parseSeerRobokitMap3D_( const std::string& file_name, const std::string& content, const AgvMapStreamOptions& options, @@ -198,7 +281,7 @@ private: static void applyAdapterParams_(Json::Value& payload, const AgvAdapterParams& params); static AgvResult resultFromResponse_(const Json::Value& response); - config::Src1100AgvConfig config_; + config::SeerRobokitAgvConfig config_; std::string ip_; std::string control_nick_name_; int recv_timeout_ms_{1000}; @@ -217,6 +300,8 @@ private: mutable std::atomic controller_fault_channel_epoch_{0}; mutable std::mutex pose_task_mutex_; mutable PoseTaskContext pose_task_context_; + mutable std::mutex tracked_navigation_mutex_; + mutable TrackedNavigationContext tracked_navigation_context_; mutable int sock_status_{-1}; mutable int sock_control_{-1}; mutable int sock_navigation_{-1}; @@ -252,4 +337,4 @@ private: } // namespace cmvr::device -#endif // CMVR_ES_SRC1100_AGV_H +#endif // CMVR_ES_SEER_ROBOKIT_AGV_H diff --git a/cmvr-es/devices/agv/seer_robokit/include/seer_robokit_navigation_utils.h b/cmvr-es/devices/agv/seer_robokit/include/seer_robokit_navigation_utils.h new file mode 100644 index 00000000..04a08780 --- /dev/null +++ b/cmvr-es/devices/agv/seer_robokit/include/seer_robokit_navigation_utils.h @@ -0,0 +1,257 @@ +#ifndef CMVR_ES_SEER_ROBOKIT_NAVIGATION_UTILS_H +#define CMVR_ES_SEER_ROBOKIT_NAVIGATION_UTILS_H + +#include +#include +#include +#include +#include +#include +#include + +#include "devices/agv/abstract_agv.h" + +namespace cmvr::device::seer_robokit::navigation { + +constexpr auto kPoseNavigationStartTimeout = std::chrono::milliseconds(1500); +constexpr auto kPoseNavigationPollInterval = std::chrono::milliseconds(50); +constexpr int kPoseNavigationRequiredRunningSamples = 2; +constexpr auto kDefaultNavigationWaitTimeout = + std::chrono::milliseconds(600000); +constexpr auto kDefaultNavigationPollInterval = + std::chrono::milliseconds(200); +constexpr auto kMaximumNavigationPollInterval = + std::chrono::milliseconds(5000); +constexpr auto kNavigationCancellationCheckInterval = + std::chrono::milliseconds(50); +constexpr auto kNavigationCancelPollInterval = + std::chrono::milliseconds(100); +constexpr auto kNavigationCancelConfirmationTimeout = + std::chrono::milliseconds(3000); +constexpr int kRequiredBlockedStopSamples = 2; +constexpr int kRequiredCompletedStopSamples = 2; +constexpr double kNavigationStopVelocityTolerance = 0.005; +constexpr int kMinimumControllerFaultCaptureGraceMs = 250; +constexpr int kMaximumControllerFaultCaptureGraceMs = 5000; +constexpr int kDefaultControllerFaultPushIntervalMs = 1000; +constexpr int kControllerFaultPushJitterMs = 100; +constexpr int kMinimumControllerFaultStateMaxAgeMs = 2000; +constexpr int kControllerFaultStateMaxAgeIntervals = 5; +constexpr double kDefaultPoseReachDistance = 0.05; +constexpr double kDefaultPoseReachAngle = 0.10; +constexpr double kTwoPi = 6.28318530717958647692; + +static inline bool exactTaskStateIsActive(const int state) +{ + return state >= 1 && state <= 3; +} + +static inline bool exactTaskStateIsKnownTerminal(const int state) +{ + return state >= 4 && state <= 7; +} + +static inline bool globalTaskStateIsKnownTerminal(const int state) +{ + return state == 0 || exactTaskStateIsKnownTerminal(state); +} + +static inline double angleDistance(const double lhs, const double rhs) +{ + return std::abs(std::remainder(lhs - rhs, kTwoPi)); +} + +static inline AgvResult withUnknownControllerOutcome(AgvResult result) +{ + const auto code = result.ok() ? AgvErrorCode::CommandFailed : result.code; + std::string detail = result.message.empty() ? "unknown transport or protocol error" : result.message; + detail += + "; SEER Robokit controller outcome is unknown after the command attempt; " + "the command may already have taken effect; do not issue another motion " + "command automatically; query status and cancel or stop first"; + return AgvResult::failure(code, detail); +} + +static inline std::string makePoseTaskId( + const std::string& device_id, + const std::uint64_t task_sequence) +{ + const auto timestamp = std::chrono::duration_cast( + std::chrono::system_clock::now().time_since_epoch()).count(); + const std::string prefix = device_id.empty() ? "cmvr-es" : device_id; + return prefix + "_pose_" + std::to_string(timestamp) + + "_" + std::to_string(task_sequence); +} + +static inline std::string makeNavigationTaskId( + const std::string& device_id, + const char* kind, + const std::uint64_t task_sequence) +{ + const auto timestamp = std::chrono::duration_cast( + std::chrono::system_clock::now().time_since_epoch()).count(); + const std::string prefix = device_id.empty() ? "cmvr-es" : device_id; + return prefix + "_" + kind + "_" + std::to_string(timestamp) + + "_" + std::to_string(task_sequence); +} + +static inline const char* blockReasonName(const int reason) +{ + switch (reason) { + case 0: + return "ultrasonic"; + case 1: + return "laser"; + case 2: + return "fallingdown"; + case 3: + return "collision"; + case 4: + return "infrared"; + case 5: + return "locked"; + default: + return "unknown"; + } +} + +static inline std::string invalidMotionOption(const AgvMotionOptions& options) +{ + const auto non_negative_error = [](const double value, const char* field) { + if (!std::isfinite(value)) { + return std::string(field) + " must be finite"; + } + if (value < 0.0) { + return std::string(field) + " must be non-negative"; + } + return std::string{}; + }; + + if (auto error = non_negative_error(options.max_speed, "max_speed"); + !error.empty()) return error; + if (auto error = non_negative_error( + options.max_angular_speed, + "max_angular_speed"); + !error.empty()) return error; + if (auto error = non_negative_error( + options.max_acceleration, + "max_acceleration"); + !error.empty()) return error; + if (auto error = non_negative_error( + options.max_angular_acceleration, + "max_angular_acceleration"); + !error.empty()) return error; + if (auto error = non_negative_error( + options.reach_distance, + "reach_distance"); + !error.empty()) return error; + if (auto error = non_negative_error(options.reach_angle, "reach_angle"); + !error.empty()) return error; + if (auto error = non_negative_error(options.speed_ratio, "speed_ratio"); + !error.empty()) return error; + if (options.wait_timeout_ms < 0) { + return "wait_timeout_ms must be non-negative"; + } + if (options.poll_interval_ms < 0) { + return "poll_interval_ms must be non-negative"; + } + if (options.poll_interval_ms + > kMaximumNavigationPollInterval.count()) { + return "poll_interval_ms must not exceed " + + std::to_string(kMaximumNavigationPollInterval.count()); + } + if (options.wait_timeout_ms > 0 + && options.poll_interval_ms > options.wait_timeout_ms) { + return "poll_interval_ms must not exceed wait_timeout_ms"; + } + return {}; +} + +static inline std::chrono::milliseconds navigationWaitTimeout( + const AgvMotionOptions& options) +{ + return options.wait_timeout_ms > 0 + ? std::chrono::milliseconds(options.wait_timeout_ms) + : kDefaultNavigationWaitTimeout; +} + +static inline std::chrono::milliseconds navigationPollInterval( + const AgvMotionOptions& options) +{ + if (options.poll_interval_ms <= 0) { + return kDefaultNavigationPollInterval; + } + return std::chrono::milliseconds( + std::max(options.poll_interval_ms, 20)); +} + +static inline bool navigationCancellationRequested(const AgvMotionOptions& options) +{ + return options.cancellation_requested + && options.cancellation_requested(); +} + +static inline void sleepForNavigationPoll( + const std::chrono::milliseconds poll_interval, + const std::chrono::steady_clock::time_point overall_deadline, + const AgvMotionOptions& options) +{ + const auto poll_deadline = std::min( + overall_deadline, + std::chrono::steady_clock::now() + poll_interval); + while (std::chrono::steady_clock::now() < poll_deadline + && !navigationCancellationRequested(options)) { + const auto remaining = std::chrono::duration_cast( + poll_deadline - std::chrono::steady_clock::now()); + if (remaining <= std::chrono::milliseconds::zero()) { + break; + } + std::this_thread::sleep_for(std::min( + kNavigationCancellationCheckInterval, + remaining)); + } +} + +template +static inline bool navigationStopped(const Snapshot& snapshot) +{ + return snapshot.velocity_present + && std::abs(snapshot.vx) <= kNavigationStopVelocityTolerance + && std::abs(snapshot.vy) <= kNavigationStopVelocityTolerance + && std::abs(snapshot.w) <= kNavigationStopVelocityTolerance; +} + +static inline AgvResult reconciledNavigationResult( + const AgvResult& command_result, + AgvResult terminal_result) +{ + if (command_result.ok()) { + return terminal_result; + } + if (terminal_result.ok()) { + terminal_result.message = + "SEER Robokit navigation completed after an indeterminate command " + "acknowledgment; initial_detail=" + command_result.message; + return terminal_result; + } + terminal_result.message = + "SEER Robokit navigation command acknowledgment was indeterminate: " + + command_result.message + "; status reconciliation: " + + terminal_result.message; + return terminal_result; +} + +static inline bool parseFiniteDouble(const std::string& value, double& parsed) +{ + std::size_t consumed = 0; + try { + parsed = std::stod(value, &consumed); + } catch (...) { + return false; + } + return consumed == value.size() && std::isfinite(parsed); +} + +} // namespace cmvr::device::seer_robokit::navigation + +#endif // CMVR_ES_SEER_ROBOKIT_NAVIGATION_UTILS_H diff --git a/cmvr-es/devices/agv/seer_robokit/include/seer_robokit_pgv_utils.h b/cmvr-es/devices/agv/seer_robokit/include/seer_robokit_pgv_utils.h new file mode 100644 index 00000000..37dbbec7 --- /dev/null +++ b/cmvr-es/devices/agv/seer_robokit/include/seer_robokit_pgv_utils.h @@ -0,0 +1,141 @@ +#ifndef CMVR_ES_SEER_ROBOKIT_PGV_UTILS_H +#define CMVR_ES_SEER_ROBOKIT_PGV_UTILS_H + +#include + +#include + +#include "devices/agv/abstract_agv.h" +#include "seer_robokit_navigation_utils.h" +#include "seer_robokit_utils.h" + +namespace cmvr::device::seer_robokit::pgv { + +constexpr char kUsePgv[] = "use_pgv"; +constexpr char kPgvAdjustDist[] = "pgv_adjust_dist"; +constexpr char kPgvAdjustCx[] = "pgv_adjust_cx"; +constexpr char kPgvAdjustCy[] = "pgv_adjust_cy"; +constexpr char kPgvXAdjust[] = "pgv_x_adjust"; +constexpr char kUseDownPgv[] = "use_down_pgv"; + +// These spellings currently appear in the vendor document, but conflict with +// its own field table/example and the repository's older working serializer. +constexpr char kMalformedAdjustDist[] = "pgv_adjustuse_pgv_dist"; +constexpr char kAdjustCyDocumentAlias[] = "pgv_ajdust_cy"; + +static inline bool isPgvAdjustmentKey(const std::string& key) +{ + return key == kUsePgv + || key == kPgvAdjustDist + || key == kPgvAdjustCx + || key == kPgvAdjustCy + || key == kPgvXAdjust + || key == kUseDownPgv + || key == kMalformedAdjustDist + || key == kAdjustCyDocumentAlias; +} + +static inline bool hasPgvAdjustmentParams(const AgvAdapterParams& params) +{ + for (const auto& [key, value] : params.values) { + (void)value; + if (isPgvAdjustmentKey(key)) { + return true; + } + } + return false; +} + +/** + * Parse the string-valued generic adapter parameters into the native JSON + * types required by SEER Robokit API 3051. Returns an error string without + * modifying controller state; an empty string means success. + */ +static inline std::string applyPgvAdjustmentParams( + Json::Value& payload, + const AgvAdapterParams& params) +{ + if (params.getString(kMalformedAdjustDist)) { + return std::string(kMalformedAdjustDist) + + " is a vendor-document typo; use " + kPgvAdjustDist; + } + if (hasPgvAdjustmentParams(params)) { + for (const auto& [key, value] : params.values) { + (void)value; + if (!isPgvAdjustmentKey(key) + && key != "source_id" + && key != "task_id") { + return "PGV adjustment must not be combined with adapter " + "field " + key; + } + } + } + const auto adjust_cy = params.getString(kPgvAdjustCy); + const auto adjust_cy_alias = params.getString(kAdjustCyDocumentAlias); + if (adjust_cy && adjust_cy_alias) { + return std::string(kPgvAdjustCy) + " and its vendor-document alias " + + kAdjustCyDocumentAlias + " must not both be set"; + } + + const auto apply_bool = [&payload, ¶ms](const char* key) { + if (!params.getString(key)) { + return std::string{}; + } + const auto parsed = params.getBool(key); + if (!parsed) { + return std::string(key) + + " must be a boolean string such as true or false"; + } + detail::jsonMember(payload, key) = *parsed; + return std::string{}; + }; + if (auto error = apply_bool(kUsePgv); !error.empty()) { + return error; + } + if (auto error = apply_bool(kUseDownPgv); !error.empty()) { + return error; + } + + const auto apply_number = [&payload, ¶ms]( + const char* key, + const bool non_negative) { + const auto raw = params.getString(key); + if (!raw) { + return std::string{}; + } + double parsed = 0.0; + if (!navigation::parseFiniteDouble(*raw, parsed)) { + return std::string(key) + " must be a complete finite number"; + } + if (non_negative && parsed < 0.0) { + return std::string(key) + " must be non-negative"; + } + detail::jsonMember(payload, key) = parsed; + return std::string{}; + }; + if (auto error = apply_number(kPgvAdjustDist, true); !error.empty()) { + return error; + } + if (auto error = apply_number(kPgvAdjustCx, false); !error.empty()) { + return error; + } + if (adjust_cy_alias) { + double parsed = 0.0; + if (!navigation::parseFiniteDouble(*adjust_cy_alias, parsed)) { + return std::string(kAdjustCyDocumentAlias) + + " must be a complete finite number"; + } + detail::jsonMember(payload, kPgvAdjustCy) = parsed; + } else if (auto error = apply_number(kPgvAdjustCy, false); + !error.empty()) { + return error; + } + if (auto error = apply_number(kPgvXAdjust, false); !error.empty()) { + return error; + } + return {}; +} + +} // namespace cmvr::device::seer_robokit::pgv + +#endif // CMVR_ES_SEER_ROBOKIT_PGV_UTILS_H diff --git a/cmvr-es/devices/agv/seer_robokit/include/seer_robokit_protocol.h b/cmvr-es/devices/agv/seer_robokit/include/seer_robokit_protocol.h new file mode 100644 index 00000000..2d581cb5 --- /dev/null +++ b/cmvr-es/devices/agv/seer_robokit/include/seer_robokit_protocol.h @@ -0,0 +1,38 @@ +#ifndef CMVR_ES_SEER_ROBOKIT_PROTOCOL_H +#define CMVR_ES_SEER_ROBOKIT_PROTOCOL_H + +#include + +namespace cmvr::device::seer_robokit::protocol { + +constexpr std::uint16_t kRobotStatusLoc = 1004; +constexpr std::uint16_t kRobotStatusBattery = 1007; +constexpr std::uint16_t kRobotStatusAll2 = 1101; +constexpr std::uint16_t kRobotStatusTask = 1020; +constexpr std::uint16_t kRobotStatusTaskPackage = 1110; +constexpr std::uint16_t kRobotStatusMap = 1300; +constexpr std::uint16_t kRobotStatusStation = 1301; +constexpr std::uint16_t kRobotStatusMappingFileList = 1780; +constexpr std::uint16_t kRobotStatusDownloadFile = 1800; +constexpr std::uint16_t kRobotControlStop = 2000; +constexpr std::uint16_t kRobotControlMotion = 2010; +constexpr std::uint16_t kRobotControlLoadMap = 2022; +constexpr std::uint16_t kRobotTaskPause = 3001; +constexpr std::uint16_t kRobotTaskResume = 3002; +constexpr std::uint16_t kRobotTaskCancel = 3003; +constexpr std::uint16_t kRobotTaskGoTarget = 3051; +constexpr std::uint16_t kRobotTaskGoTargetList = 3066; +constexpr std::uint16_t kRobotTaskClearTargetList = 3067; +constexpr std::uint16_t kRobotConfigLock = 4005; +constexpr std::uint16_t kRobotConfigUploadMap = 4010; +constexpr std::uint16_t kRobotConfigDownloadMap = 4011; +constexpr std::uint16_t kRobotOtherStartMapping = 6100; +constexpr std::uint16_t kRobotOtherStopMapping = 6101; +constexpr std::uint16_t kRobotPushConfigReq = 9300; +constexpr std::uint16_t kRobotPushConfigRes = 19300; +constexpr std::uint16_t kRobotPush = 19301; +constexpr std::uint32_t kMaxFramePayloadBytes = 512U * 1024U * 1024U; + +} // namespace cmvr::device::seer_robokit::protocol + +#endif // CMVR_ES_SEER_ROBOKIT_PROTOCOL_H diff --git a/cmvr-es/devices/agv/seer_robokit/include/seer_robokit_utils.h b/cmvr-es/devices/agv/seer_robokit/include/seer_robokit_utils.h new file mode 100644 index 00000000..12095921 --- /dev/null +++ b/cmvr-es/devices/agv/seer_robokit/include/seer_robokit_utils.h @@ -0,0 +1,92 @@ +#ifndef CMVR_ES_SEER_ROBOKIT_UTILS_H +#define CMVR_ES_SEER_ROBOKIT_UTILS_H + +#include +#include +#include +#include + +#include + +namespace cmvr::device::seer_robokit::detail { + +static inline std::string systemError() +{ + return std::strerror(errno); +} + +static inline Json::Value& jsonMember( + Json::Value& value, + const char* key) +{ + return *value.demand(key, key + std::strlen(key)); +} + +static inline Json::Value& jsonMember( + Json::Value& value, + const std::string& key) +{ + return *value.demand(key.data(), key.data() + key.size()); +} + +static inline const Json::Value* jsonFind( + const Json::Value& value, + const char* key) +{ + return value.find(key, key + std::strlen(key)); +} + +static inline Json::Value jsonGet( + const Json::Value& value, + const char* key, + const Json::Value& fallback) +{ + const auto* found = jsonFind(value, key); + return found ? *found : fallback; +} + +static inline double nowSeconds() +{ + const auto now = std::chrono::system_clock::now().time_since_epoch(); + return std::chrono::duration(now).count(); +} + +static inline bool jsonHas( + const Json::Value& value, + const char* key) +{ + return jsonFind(value, key) != nullptr; +} + +static inline bool hasNumericControllerRetCode( + const Json::Value& response) +{ + const auto* ret_code = jsonFind(response, "ret_code"); + return ret_code + && (ret_code->isInt() + || ret_code->isUInt() + || ret_code->isInt64() + || ret_code->isUInt64()); +} + +static inline std::string jsonValueToString(const Json::Value& value) +{ + if (value.isString()) return value.asString(); + if (value.isBool()) return value.asBool() ? "true" : "false"; + if (value.isInt64() || value.isInt()) { + return std::to_string(value.asInt64()); + } + if (value.isUInt64() || value.isUInt()) { + return std::to_string(value.asUInt64()); + } + if (value.isDouble()) return std::to_string(value.asDouble()); + if (value.isNull()) return {}; + + Json::StreamWriterBuilder builder; + builder["indentation"] = ""; + return Json::writeString(builder, value); +} + +} // namespace cmvr::device::seer_robokit::detail + +#endif // CMVR_ES_SEER_ROBOKIT_UTILS_H diff --git a/cmvr-es/devices/agv/seer_robokit/src/seer_robokit_agv.cpp b/cmvr-es/devices/agv/seer_robokit/src/seer_robokit_agv.cpp new file mode 100644 index 00000000..03457dd3 --- /dev/null +++ b/cmvr-es/devices/agv/seer_robokit/src/seer_robokit_agv.cpp @@ -0,0 +1,175 @@ +#include "seer_robokit_agv.h" + +#include +#include +#include + +#include "common/base/logging/logger.h" + +namespace cmvr::device { + +namespace { + +constexpr int kDefaultMapUpdateIntervalMs = 1000; +constexpr std::size_t kDefaultMapUpdateHistorySize = 8; + +} // namespace + +SeerRobokitAgv::SeerRobokitAgv(const config::SeerRobokitAgvConfig& cfg) + : config_(cfg), + ip_(cfg.ip()), + control_nick_name_( + cfg.control_nick_name().empty() + ? (cfg.id().empty() ? "cmvr-es" : "cmvr-es:" + cfg.id()) + : cfg.control_nick_name()), + recv_timeout_ms_(cfg.recv_timeout_ms() > 0 ? cfg.recv_timeout_ms() : 1000), + state_push_enabled_(cfg.enable_state_push()), + map_update_enabled_(cfg.enable_map_update()), + map_update_interval_ms_(cfg.map_update_interval_ms() > 0 ? cfg.map_update_interval_ms() : kDefaultMapUpdateIntervalMs), + map_update_history_size_(cfg.map_update_history_size() > 0 ? cfg.map_update_history_size() : kDefaultMapUpdateHistorySize) +{ + id_ = cfg.id(); + if (cfg.port_status() > 0) ports_.status = cfg.port_status(); + if (cfg.port_control() > 0) ports_.control = cfg.port_control(); + if (cfg.port_nav() > 0) ports_.navigation = cfg.port_nav(); + if (cfg.port_config() > 0) ports_.config = cfg.port_config(); + if (cfg.port_other() > 0) ports_.other = cfg.port_other(); + if (cfg.port_push() > 0) ports_.push = cfg.port_push(); + + const auto result = connect_(); + if (!result.ok()) { + CMVR_LOG(ERROR) << "[SeerRobokitAgv] Auto connect failed" + << ", id=" << id_ + << ", ip=" << ip_ + << ", error=" << result.message; + } +} + +SeerRobokitAgv::~SeerRobokitAgv() +{ + (void)disconnect_(); +} + +bool SeerRobokitAgv::init() +{ + return !id_.empty() && !ip_.empty(); +} + +bool SeerRobokitAgv::start() +{ + return true; +} + +bool SeerRobokitAgv::stop() +{ + return true; +} + +bool SeerRobokitAgv::update() +{ + return true; +} + +AgvResult SeerRobokitAgv::connect_() +{ + const auto lifecycle_generation = + navigation_generation_.fetch_add(1, std::memory_order_relaxed) + 1; + clearPoseTask_(lifecycle_generation); + clearTrackedNavigation_(lifecycle_generation); + stopPushThread_(); + stopMapUpdateThread_(); + + { + // Status requests may wait for a controller receive timeout without + // holding mutex_. Serialize lifecycle changes with that channel before + // replacing or closing its descriptor. + std::lock_guard status_io_lock(status_io_mutex_); + std::lock_guard lock(mutex_); + closeSocket_(sock_status_); + closeSocket_(sock_control_); + closeSocket_(sock_navigation_); + closeSocket_(sock_config_); + closeSocket_(sock_other_); + closeSocket_(sock_push_); + + if (ip_.empty()) { + return AgvResult::failure(AgvErrorCode::InvalidArgument, "SEER Robokit AGV ip is empty"); + } + + const auto close_all = [this]() { + closeSocket_(sock_status_); + closeSocket_(sock_control_); + closeSocket_(sock_navigation_); + closeSocket_(sock_config_); + closeSocket_(sock_other_); + closeSocket_(sock_push_); + }; + + if (auto result = connectSocket_(sock_status_, ports_.status); !result.ok()) { + close_all(); + return result; + } + if (auto result = connectSocket_(sock_control_, ports_.control); !result.ok()) { + close_all(); + return result; + } + if (auto result = connectSocket_(sock_navigation_, ports_.navigation); !result.ok()) { + close_all(); + return result; + } + if (auto result = connectSocket_(sock_config_, ports_.config); !result.ok()) { + close_all(); + return result; + } + + if (state_push_enabled_) { + const auto result = connectSocket_(sock_push_, ports_.push); + if (!result.ok()) { + CMVR_LOG(ERROR) << "[SeerRobokitAgv] Connect push port failed" + << ", id=" << id_ + << ", port=" << ports_.push + << ", error=" << result.message; + closeSocket_(sock_push_); + } + } + last_error_.clear(); + } + + if (state_push_enabled_ && sock_push_ >= 0) { + const auto result = configurePush_(); + if (result.ok()) { + startPushThread_(); + } else { + CMVR_LOG(ERROR) << "[SeerRobokitAgv] Configure push failed" + << ", id=" << id_ + << ", error=" << result.message; + std::lock_guard lock(mutex_); + closeSocket_(sock_push_); + } + } + if (map_update_enabled_) { + startMapUpdateThread_(); + } + return AgvResult::success(); +} + +AgvResult SeerRobokitAgv::disconnect_() +{ + const auto lifecycle_generation = + navigation_generation_.fetch_add(1, std::memory_order_relaxed) + 1; + clearPoseTask_(lifecycle_generation); + clearTrackedNavigation_(lifecycle_generation); + stopMapUpdateThread_(); + stopPushThread_(); + std::lock_guard status_io_lock(status_io_mutex_); + std::lock_guard lock(mutex_); + closeSocket_(sock_status_); + closeSocket_(sock_control_); + closeSocket_(sock_navigation_); + closeSocket_(sock_config_); + closeSocket_(sock_other_); + closeSocket_(sock_push_); + return AgvResult::success(); +} + +} // namespace cmvr::device diff --git a/cmvr-es/devices/agv/seer_robokit/src/seer_robokit_control.cpp b/cmvr-es/devices/agv/seer_robokit/src/seer_robokit_control.cpp new file mode 100644 index 00000000..9f3f0b21 --- /dev/null +++ b/cmvr-es/devices/agv/seer_robokit/src/seer_robokit_control.cpp @@ -0,0 +1,378 @@ +#include "seer_robokit_agv.h" +#include "seer_robokit_navigation_utils.h" +#include "seer_robokit_protocol.h" +#include "seer_robokit_utils.h" + +#include +#include +#include +#include +#include +#include +#include +#include + +namespace cmvr::device { + +using namespace seer_robokit::navigation; +using namespace seer_robokit::protocol; +using namespace seer_robokit::detail; + +AgvResult SeerRobokitAgv::acquireControl_() const +{ + Json::Value payload(Json::objectValue); + jsonMember(payload, "nick_name") = control_nick_name_; + + Json::Value response; + auto result = sendCommand_(sock_config_, kRobotConfigLock, payload, &response); + return result.ok() ? resultFromResponse_(response) : result; +} + +AgvResult SeerRobokitAgv::sendControlledCommand_( + const int sock, + const std::uint16_t command, + const Json::Value& payload, + Json::Value* response, + std::uint64_t* accepted_navigation_generation, + std::uint64_t* controller_fault_sequence_at_attempt, + std::uint64_t* control_attempt_sequence, + PoseTaskContext* pose_context_to_publish, + const bool reject_if_active_controller_fault, + TrackedNavigationContext* navigation_context_to_publish, + const bool preserve_tracked_navigation, + const std::string* expected_navigation_token, + const std::function* cancellation_requested, + const TrackedNavigationContext* expected_active_navigation) const +{ + const auto canceled_before_send = [cancellation_requested]() { + return cancellation_requested + && *cancellation_requested + && (*cancellation_requested)(); + }; + if (canceled_before_send()) { + return AgvResult::failure( + AgvErrorCode::TaskCanceled, + "SEER Robokit command was not sent because the caller canceled the " + "operation before control authority was acquired"); + } + + const auto expected_context_is_current = + [this, expected_active_navigation]() { + if (!expected_active_navigation) { + return true; + } + TrackedNavigationContext active_context; + return currentTrackedNavigation_(active_context) + && active_context.token + == expected_active_navigation->token + && active_context.navigation_generation + == expected_active_navigation->navigation_generation + && active_context.type + == expected_active_navigation->type + && navigation_generation_.load(std::memory_order_relaxed) + == expected_active_navigation->navigation_generation; + }; + + // Conditional cancel ownership checks are deliberately performed without + // the control sequencing mutex. A slow 1110/1101 response must never + // prevent emergencyStop() from acquiring authority and sending 2000. + // The exact local token/generation/type is revalidated under the control + // lock both before and after authority acquisition below. + if (expected_active_navigation) { + if (!expected_context_is_current()) { + return AgvResult::failure( + AgvErrorCode::TaskCanceled, + "SEER Robokit did not start conditional navigation cancel " + "preflight because the tracked task was already replaced or " + "ended"); + } + + std::vector statuses; + const auto exact_result = queryTaskStatuses_( + expected_active_navigation->task_ids, + statuses); + if (!exact_result.ok()) { + return AgvResult::failure( + exact_result.code, + "SEER Robokit did not send the conditional navigation cancel " + "because exact task ownership preflight failed: " + + exact_result.message); + } + const bool all_exact_tasks_terminal = !statuses.empty() + && std::all_of( + statuses.begin(), + statuses.end(), + [](const PoseTaskStatus& status) { + return status.found + && exactTaskStateIsKnownTerminal(status.state); + }); + if (all_exact_tasks_terminal) { + return AgvResult::success(); + } + + const bool exact_task_still_active = std::any_of( + statuses.begin(), + statuses.end(), + [](const PoseTaskStatus& status) { + return status.found + && exactTaskStateIsActive(status.state); + }); + + NavigationSnapshot snapshot; + const auto snapshot_result = queryNavigationSnapshot_(snapshot); + if (!snapshot_result.ok()) { + return AgvResult::failure( + snapshot_result.code, + "SEER Robokit did not send the conditional navigation cancel " + "because 1101 ownership preflight was unavailable: " + + snapshot_result.message); + } + const int expected_type = expected_active_navigation->type + == AgvTaskType::NavigateToPose + ? 1 + : (expected_active_navigation->type + == AgvTaskType::NavigateToStation + ? 2 + : 3); + const bool global_active = + exactTaskStateIsActive(snapshot.task_status); + const bool target_conflicts = global_active + && !snapshot.target_id.empty() + && !expected_active_navigation->target_ids.empty() + && std::find( + expected_active_navigation->target_ids.begin(), + expected_active_navigation->target_ids.end(), + snapshot.target_id) + == expected_active_navigation->target_ids.end(); + if (global_active + && (snapshot.task_type != expected_type + || target_conflicts)) { + return AgvResult::failure( + AgvErrorCode::TaskCanceled, + "SEER Robokit did not send the conditional navigation cancel " + "because 1101 reports another active task: " + + snapshot.detail); + } + + if (!exact_task_still_active) { + const bool clearing_path_queue = + expected_active_navigation->type + == AgvTaskType::FollowPath; + const bool terminal_target_matches = + expected_active_navigation->type + != AgvTaskType::NavigateToStation + || (!snapshot.target_id.empty() + && snapshot.target_id + == expected_active_navigation->target_id); + if (globalTaskStateIsKnownTerminal(snapshot.task_status) + && snapshot.task_status != 0 + && !clearing_path_queue + && snapshot.task_type == expected_type + && terminal_target_matches) { + return AgvResult::success(); + } + } + } + + // Keep the permission acquisition and the following write ordered with + // respect to other control RPCs in this process. Channel I/O serialization + // is separate, so this must remain a distinct lock. + std::lock_guard sequence_lock(control_sequence_mutex_); + if (expected_navigation_token) { + TrackedNavigationContext active_context; + const bool has_active_context = + currentTrackedNavigation_(active_context); + const bool expected_context_matches = expected_active_navigation + ? (has_active_context + && active_context.token + == expected_active_navigation->token + && active_context.navigation_generation + == expected_active_navigation->navigation_generation + && active_context.type + == expected_active_navigation->type + && navigation_generation_.load(std::memory_order_relaxed) + == expected_active_navigation->navigation_generation) + : (has_active_context + && active_context.token == *expected_navigation_token); + if (!expected_context_matches) { + return AgvResult::failure( + AgvErrorCode::TaskCanceled, + "SEER Robokit did not send the conditional navigation cancel " + "because the tracked task was already replaced or ended; " + "expected_token=" + *expected_navigation_token + + (active_context.token.empty() + ? std::string(", active_token=") + : ", active_token=" + active_context.token)); + } + } + const auto attempt_sequence = + control_attempt_sequence_.fetch_add( + 1, + std::memory_order_relaxed) + 1; + if (control_attempt_sequence) { + *control_attempt_sequence = attempt_sequence; + } + if (pose_context_to_publish) { + pose_context_to_publish->control_attempt_sequence_at_start = + attempt_sequence; + } + + const auto authority = acquireControl_(); + if (!authority.ok()) { + const std::string detail = authority.message.empty() ? "unknown error" : authority.message; + return AgvResult::failure( + authority.code, + "SEER Robokit acquire control authority failed: " + detail); + } + if (expected_active_navigation && !expected_context_is_current()) { + return AgvResult::failure( + AgvErrorCode::TaskCanceled, + "SEER Robokit did not send the conditional navigation cancel because " + "the tracked token, generation, or type changed while control " + "authority was being acquired"); + } + if (canceled_before_send()) { + return AgvResult::failure( + AgvErrorCode::TaskCanceled, + "SEER Robokit command was not sent because the caller canceled the " + "operation while control authority was being acquired"); + } + std::string controller_fault_gate_error; + if (controller_fault_sequence_at_attempt + || pose_context_to_publish + || reject_if_active_controller_fault) { + std::lock_guard lock(runtime_state_mutex_); + if (controller_fault_sequence_at_attempt) { + *controller_fault_sequence_at_attempt = + controller_fault_sequence_; + } + if (pose_context_to_publish) { + pose_context_to_publish->controller_fault_sequence_at_start = + controller_fault_sequence_; + pose_context_to_publish + ->controller_fault_channel_epoch_at_start = + controller_fault_channel_epoch_.load( + std::memory_order_relaxed); + } + if (reject_if_active_controller_fault) { + if (!state_push_enabled_) { + controller_fault_gate_error = + "controller fault state is unavailable because state push " + "is disabled"; + } else if (!active_controller_fault_detail_.empty()) { + controller_fault_gate_error = + "the controller reported a fault or invalid fault state: " + + active_controller_fault_detail_; + } else if (!controller_fault_state_observed_) { + controller_fault_gate_error = + "no state push containing fatals/errors has been observed"; + } else { + const auto fault_state_age = + std::chrono::duration_cast( + std::chrono::steady_clock::now() + - controller_fault_state_observed_at_) + .count(); + if (fault_state_age > controllerFaultStateMaxAgeMs_()) { + controller_fault_gate_error = + "the most recent fatals/errors state push is stale " + "(age_ms=" + std::to_string(fault_state_age) + + ", max_age_ms=" + + std::to_string(controllerFaultStateMaxAgeMs_()) + + ")"; + } + } + } + } + if (!controller_fault_gate_error.empty()) { + return AgvResult::failure( + AgvErrorCode::Fault, + "SEER Robokit free-navigation command was not sent because " + + controller_fault_gate_error); + } + if (canceled_before_send()) { + return AgvResult::failure( + AgvErrorCode::TaskCanceled, + "SEER Robokit command was not sent because the caller canceled the " + "operation before the controller command write"); + } + if (canceled_before_send()) { + return AgvResult::failure( + AgvErrorCode::TaskCanceled, + "SEER Robokit command was not sent because the caller canceled the " + "operation immediately before the controller command write"); + } + const auto publish_navigation_generation = + [this, + accepted_navigation_generation, + pose_context_to_publish, + navigation_context_to_publish, + preserve_tracked_navigation]() { + const auto generation = + navigation_generation_.fetch_add( + 1, + std::memory_order_relaxed) + 1; + *accepted_navigation_generation = generation; + if (pose_context_to_publish) { + pose_context_to_publish->navigation_generation = generation; + rememberPoseTask_(*pose_context_to_publish); + } + if (navigation_context_to_publish) { + navigation_context_to_publish->navigation_generation = + generation; + navigation_context_to_publish->accepted_at = + std::chrono::steady_clock::now(); + rememberTrackedNavigation_( + *navigation_context_to_publish); + } else if (preserve_tracked_navigation) { + advanceTrackedNavigationGeneration_(generation); + } else { + clearTrackedNavigation_(generation); + } + }; + CommandTransmissionState transmission_state = + CommandTransmissionState::NotSent; + auto result = sendCommand_( + sock, + command, + payload, + response, + &transmission_state); + if (!result.ok()) { + if (accepted_navigation_generation + && transmission_state + == CommandTransmissionState::PossiblySent) { + // Once the control write has been attempted, a timeout, disconnect, + // wrong response opcode, or malformed JSON cannot prove rejection: + // the controller may already have executed the command. + publish_navigation_generation(); + return withUnknownControllerOutcome(std::move(result)); + } + return result; + } + if (!accepted_navigation_generation) { + return result; + } + if (!response) { + publish_navigation_generation(); + return withUnknownControllerOutcome(AgvResult::failure( + AgvErrorCode::CommandFailed, + "SEER Robokit cannot confirm navigation command without a response")); + } + if (!hasNumericControllerRetCode(*response)) { + publish_navigation_generation(); + return withUnknownControllerOutcome(resultFromResponse_(*response)); + } + result = resultFromResponse_(*response); + if (!result.ok()) { + return result; + } + + // Advance only after the controller accepted the command, and do it before + // releasing control_sequence_mutex_. This prevents a failed cancel/pause or + // failed authority acquisition from falsely reporting a pose task canceled, + // while preserving the controller's actual command order under concurrency. + publish_navigation_generation(); + return result; +} + +} // namespace cmvr::device diff --git a/cmvr-es/devices/agv/seer_robokit/src/seer_robokit_map.cpp b/cmvr-es/devices/agv/seer_robokit/src/seer_robokit_map.cpp new file mode 100644 index 00000000..d520d462 --- /dev/null +++ b/cmvr-es/devices/agv/seer_robokit/src/seer_robokit_map.cpp @@ -0,0 +1,1016 @@ +#include "seer_robokit_agv.h" +#include "seer_robokit_protocol.h" +#include "seer_robokit_utils.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "common/base/logging/logger.h" +#include "rbk/protocol/seer_robokit_map3d.pb.h" + +namespace cmvr::device { + +using namespace seer_robokit::protocol; +using namespace seer_robokit::detail; + +namespace { + +namespace fs = std::filesystem; + +constexpr std::uint64_t kMapSnapshotSequenceStart = 1; + +bool wants2D(const AgvMapDimension dimension) +{ + return dimension == AgvMapDimension::Unspecified + || dimension == AgvMapDimension::Map2D + || dimension == AgvMapDimension::Map2DAnd3D; +} + +bool wants3D(const AgvMapDimension dimension) +{ + return dimension == AgvMapDimension::Unspecified + || dimension == AgvMapDimension::Map3D + || dimension == AgvMapDimension::Map2DAnd3D; +} + +bool contentLooksLikeZip(const std::string& content) +{ + return content.size() >= 4 + && static_cast(content[0]) == 0x50U + && static_cast(content[1]) == 0x4BU + && static_cast(content[2]) == 0x03U + && static_cast(content[3]) == 0x04U; +} + +bool contentLooksLikeJson(const std::string& content) +{ + const auto pos = content.find_first_not_of(" \t\r\n"); + return pos != std::string::npos && (content[pos] == '{' || content[pos] == '['); +} + +std::string shellQuote(const std::string& value) +{ + std::string quoted = "'"; + for (const char ch : value) { + if (ch == '\'') { + quoted += "'\\''"; + } else { + quoted += ch; + } + } + quoted += "'"; + return quoted; +} + +bool writeBinaryFile(const fs::path& path, const std::string& content) +{ + std::ofstream output(path, std::ios::binary); + if (!output) { + return false; + } + output.write(content.data(), static_cast(content.size())); + return output.good(); +} + +bool readBinaryFile(const fs::path& path, std::string& content) +{ + std::ifstream input(path, std::ios::binary); + if (!input) { + return false; + } + std::ostringstream buffer; + buffer << input.rdbuf(); + content = buffer.str(); + return true; +} + +fs::path makeTempDirectory() +{ + auto pattern = fs::temp_directory_path() / "cmvr_seer_robokit_map_XXXXXX"; + std::string path = pattern.string(); + char* created = ::mkdtemp(path.data()); + if (!created) { + return {}; + } + return fs::path(created); +} + +void putPropertyIfPresent( + std::unordered_map& properties, + const Json::Value& value, + const char* json_key, + const char* property_key) +{ + const auto* found = jsonFind(value, json_key); + if (!found || found->isNull()) { + return; + } + properties[property_key] = jsonValueToString(*found); +} + +void appendMapProperties( + std::unordered_map& properties, + const Json::Value& value, + const char* key) +{ + const auto* list = jsonFind(value, key); + if (!list || !list->isArray()) { + return; + } + + for (const auto& item : *list) { + const std::string property_key = jsonGet(item, "key", "").asString(); + if (property_key.empty()) { + continue; + } + + const char* value_keys[] = { + "string_value", + "bool_value", + "int32_value", + "uint32_value", + "int64_value", + "uint64_value", + "float_value", + "double_value", + "bytes_value", + "value" + }; + for (const char* value_key : value_keys) { + const auto* found = jsonFind(item, value_key); + if (found && !found->isNull()) { + properties[property_key] = jsonValueToString(*found); + break; + } + } + } +} + +AgvMapPoint3D jsonPoint3D(const Json::Value& value) +{ + AgvMapPoint3D point; + point.x = jsonGet(value, "x", 0.0).asDouble(); + point.y = jsonGet(value, "y", 0.0).asDouble(); + point.z = jsonGet(value, "z", 0.0).asDouble(); + return point; +} + +void appendObject( + AgvUnifiedMap2D& map, + std::string id, + const AgvMapObjectType type, + std::vector points, + const double heading, + const Json::Value& source) +{ + AgvMapObject object; + object.id = std::move(id); + object.type = type; + object.points = std::move(points); + object.heading = heading; + putPropertyIfPresent(object.properties, source, "class_name", "class_name"); + putPropertyIfPresent(object.properties, source, "type", "type"); + putPropertyIfPresent(object.properties, source, "description", "description"); + appendMapProperties(object.properties, source, "property"); + map.objects.push_back(std::move(object)); +} + +} // namespace + +AgvResult SeerRobokitAgv::listMaps(std::vector& maps) const +{ + Json::Value response; + auto result = sendCommand_(sock_status_, kRobotStatusMap, Json::Value(Json::objectValue), &response); + if (!result.ok()) return result; + maps.clear(); + if (const auto* values = jsonFind(response, "maps"); values && values->isArray()) { + for (const auto& value : *values) { + maps.push_back(value.asString()); + } + } + return resultFromResponse_(response); +} + +AgvResult SeerRobokitAgv::listStations(std::vector& stations) const +{ + Json::Value response; + auto result = sendCommand_(sock_status_, kRobotStatusStation, Json::Value(Json::objectValue), &response); + if (!result.ok()) return result; + stations.clear(); + if (const auto* values = jsonFind(response, "stations"); values && values->isArray()) { + for (const auto& value : *values) { + AgvStation station; + station.id = jsonGet(value, "id", "").asString(); + station.type = jsonGet(value, "type", "").asString(); + station.pose.x = jsonGet(value, "x", 0.0).asDouble(); + station.pose.y = jsonGet(value, "y", 0.0).asDouble(); + station.pose.theta = jsonGet(value, "r", 0.0).asDouble(); + station.description = jsonGet(value, "desc", "").asString(); + stations.push_back(station); + } + } + return resultFromResponse_(response); +} + +AgvResult SeerRobokitAgv::switchMap(const std::string& map_name) +{ + Json::Value payload(Json::objectValue); + jsonMember(payload, "map_name") = map_name; + Json::Value response; + std::uint64_t accepted_generation = 0; + auto result = sendControlledCommand_( + sock_control_, + kRobotControlLoadMap, + payload, + &response, + &accepted_generation); + if (accepted_generation != 0) { + clearPoseTask_(accepted_generation); + } + return result; +} + +AgvResult SeerRobokitAgv::uploadMap(const std::string& map_name, const std::string& content) +{ + Json::Value payload(Json::objectValue); + jsonMember(payload, "map_name") = map_name; + jsonMember(payload, "map_content") = content; + Json::Value response; + auto result = sendControlledCommand_(sock_config_, kRobotConfigUploadMap, payload, &response); + return result.ok() ? resultFromResponse_(response) : result; +} + +AgvResult SeerRobokitAgv::downloadMap(const std::string& map_name, std::string& content) const +{ + Json::Value payload(Json::objectValue); + jsonMember(payload, "map_name") = map_name; + Json::Value response; + auto result = sendCommand_(sock_config_, kRobotConfigDownloadMap, payload, &response); + if (!result.ok()) return result; + content = jsonGet(response, "map_content", jsonGet(response, "content", "")).asString(); + return resultFromResponse_(response); +} + +AgvResult SeerRobokitAgv::startMapping(const AgvMappingOptions& options) +{ + auto result = ensureOtherSocket_(); + if (!result.ok()) return result; + + Json::Value payload(Json::objectValue); + jsonMember(payload, "slam_type") = options.dimension == AgvMapDimension::Map2D ? 2 : 4; + jsonMember(payload, "real_time") = options.real_time; + if (!options.map_name.empty()) { + jsonMember(payload, "map_name") = options.map_name; + } + + Json::Value response; + std::uint64_t accepted_generation = 0; + result = sendControlledCommand_( + sock_other_, + kRobotOtherStartMapping, + payload, + &response, + &accepted_generation); + if (accepted_generation != 0) { + clearPoseTask_(accepted_generation); + } + if (result.ok()) { + { + std::lock_guard lock(map_update_mutex_); + cached_map_updates_.clear(); + next_mapping_index_ = 0; + last_map_content_hash_ = 0; + map_sequence_ = 0; + map_session_id_ = id_ + "_mapping_" + std::to_string(static_cast(nowSeconds() * 1000.0)); + } + if (map_update_enabled_ || options.real_time) { + startMapUpdateThread_(); + } + } + return result; +} + +AgvResult SeerRobokitAgv::getMappingData(const int start_index, AgvMappingData& data) const +{ + if (start_index < 0) { + return AgvResult::failure(AgvErrorCode::InvalidArgument, "mapping data start_index must be >= 0"); + } + + Json::Value list_payload(Json::objectValue); + jsonMember(list_payload, "index") = start_index; + + Json::Value list_response; + auto result = sendCommand_(sock_status_, kRobotStatusMappingFileList, list_payload, &list_response); + if (!result.ok()) return result; + result = resultFromResponse_(list_response); + if (!result.ok()) return result; + + data = {}; + data.start_index = start_index; + data.next_index = start_index; + + const auto* list = jsonFind(list_response, "list"); + if (!list || !list->isArray()) { + return AgvResult::success(); + } + + for (const auto& item : *list) { + const std::string file_name = item.asString(); + if (file_name.empty()) { + continue; + } + + Json::Value download_payload(Json::objectValue); + jsonMember(download_payload, "type") = "users"; + jsonMember(download_payload, "file_path") = file_name; + + std::string content; + result = sendCommandRaw_(sock_status_, kRobotStatusDownloadFile, download_payload, &content); + if (!result.ok()) return result; + + Json::Value maybe_error; + std::string parse_error; + if (parseJson_(content, maybe_error, parse_error) && maybe_error.isObject()) { + result = resultFromResponse_(maybe_error); + if (!result.ok()) return result; + content = jsonGet(maybe_error, "content", jsonGet(maybe_error, "file_content", content)).asString(); + } + + AgvMappingDataFile file; + file.name = file_name; + file.content = std::move(content); + data.files.push_back(std::move(file)); + } + + data.next_index = data.start_index + static_cast(data.files.size()); + return AgvResult::success(); +} + +AgvResult SeerRobokitAgv::getUnifiedMapUpdate( + const std::uint64_t after_sequence, + const AgvMapStreamOptions& options, + AgvUnifiedMapUpdate& update) const +{ + if (findCachedMapUpdate_(after_sequence, options, update)) { + return AgvResult::success(); + } + + const auto refresh_result = refreshMapCacheOnce_(options); + if (findCachedMapUpdate_(after_sequence, options, update)) { + return AgvResult::success(); + } + if (!refresh_result.ok() && refresh_result.code != AgvErrorCode::Timeout) { + return refresh_result; + } + + const auto wait_ms = options.wait_timeout_ms > 0 ? options.wait_timeout_ms : 1000; + std::unique_lock lock(map_update_mutex_); + const auto effective_after = [&]() { + if (after_sequence != 0 || options.resume_token.empty()) { + return after_sequence; + } + try { + return static_cast(std::stoull(options.resume_token)); + } catch (...) { + return std::uint64_t{0}; + } + }(); + const auto find_locked = [&]() { + for (const auto& candidate : cached_map_updates_) { + if (candidate.sequence > effective_after && mapUpdateMatches_(candidate, options)) { + update = candidate; + return true; + } + } + return false; + }; + + if (find_locked()) { + return AgvResult::success(); + } + const bool ready = map_update_cv_.wait_for( + lock, + std::chrono::milliseconds(wait_ms), + find_locked); + if (ready) { + return AgvResult::success(); + } + return AgvResult::failure(AgvErrorCode::Timeout, "SEER Robokit unified map update timeout"); +} + +void SeerRobokitAgv::startMapUpdateThread_() +{ + if (map_update_running_.exchange(true)) { + return; + } + map_update_thread_ = std::thread(&SeerRobokitAgv::mapUpdateLoop_, this); +} + +void SeerRobokitAgv::stopMapUpdateThread_() +{ + const bool was_running = map_update_running_.exchange(false); + if (was_running) { + map_update_cv_.notify_all(); + } + if (map_update_thread_.joinable()) { + map_update_thread_.join(); + } +} + +void SeerRobokitAgv::mapUpdateLoop_() +{ + while (map_update_running_) { + AgvMapStreamOptions options; + options.dimension = AgvMapDimension::Map2DAnd3D; + options.snapshot = true; + options.incremental = true; + options.wait_timeout_ms = 0; + + const auto result = refreshMapCacheOnce_(options); + if (!result.ok() && result.code != AgvErrorCode::Timeout) { + std::lock_guard lock(mutex_); + last_error_ = result.message; + } + + std::unique_lock lock(map_update_mutex_); + map_update_cv_.wait_for( + lock, + std::chrono::milliseconds(map_update_interval_ms_), + [this]() { return !map_update_running_; }); + } +} + +AgvResult SeerRobokitAgv::refreshMapCacheOnce_(const AgvMapStreamOptions& options) const +{ + int start_index = 0; + { + std::lock_guard lock(map_update_mutex_); + start_index = next_mapping_index_; + } + + AgvMappingData mapping_data; + auto result = getMappingData(start_index, mapping_data); + if (result.ok() && !mapping_data.files.empty()) { + std::vector updates; + for (const auto& file : mapping_data.files) { + std::vector file_updates; + const auto parse_result = parseMapFileToUpdates_(file.name, file.content, options, file_updates); + if (!parse_result.ok()) { + CMVR_LOG(ERROR) << "[SeerRobokitAgv] Parse mapping file failed" + << ", id=" << id_ + << ", file=" << file.name + << ", error=" << parse_result.message; + continue; + } + updates.insert( + updates.end(), + std::make_move_iterator(file_updates.begin()), + std::make_move_iterator(file_updates.end())); + } + { + std::lock_guard lock(map_update_mutex_); + next_mapping_index_ = std::max(next_mapping_index_, mapping_data.next_index); + } + if (!updates.empty()) { + cacheMapUpdates_(std::move(updates)); + return AgvResult::success(); + } + } + + std::string map_name = options.map_name; + if (map_name.empty()) { + const auto state = runtimeState(); + map_name = state.current_map; + } + if (map_name.empty()) { + std::vector maps; + if (listMaps(maps).ok() && !maps.empty()) { + map_name = maps.back(); + } + } + if (map_name.empty()) { + return result.ok() + ? AgvResult::failure(AgvErrorCode::Timeout, "SEER Robokit no map file is available") + : result; + } + + std::string content; + result = downloadMap(map_name, content); + if (!result.ok()) { + return result; + } + const auto content_hash = std::hash{}(content); + std::size_t last_map_content_hash = 0; + { + std::lock_guard lock(map_update_mutex_); + last_map_content_hash = last_map_content_hash_; + } + AgvUnifiedMapUpdate cached; + if (content_hash == last_map_content_hash && findCachedMapUpdate_(0, options, cached)) { + return AgvResult::success(); + } + + std::vector updates; + result = parseMapFileToUpdates_(map_name, content, options, updates); + if (!result.ok()) { + return result; + } + if (updates.empty()) { + return AgvResult::failure(AgvErrorCode::Timeout, "SEER Robokit map file has no requested dimension"); + } + + { + std::lock_guard lock(map_update_mutex_); + last_map_content_hash_ = content_hash; + } + cacheMapUpdates_(std::move(updates)); + return AgvResult::success(); +} + +AgvResult SeerRobokitAgv::parseMapFileToUpdates_( + const std::string& file_name, + const std::string& content, + const AgvMapStreamOptions& options, + std::vector& updates) const +{ + if (content.empty()) { + return AgvResult::failure(AgvErrorCode::InvalidArgument, "SEER Robokit map file is empty: " + file_name); + } + + if (contentLooksLikeZip(content)) { + return parseSeerRobokitMapArchive_(file_name, content, options, updates); + } + + if (contentLooksLikeJson(content)) { + if (wants2D(options.dimension)) { + AgvUnifiedMapUpdate update; + const auto result = parseSeerRobokitMap2D_(file_name, content, options, update); + if (!result.ok()) { + return result; + } + updates.push_back(std::move(update)); + } + return AgvResult::success(); + } + + if (wants3D(options.dimension)) { + AgvUnifiedMapUpdate update; + const auto result = parseSeerRobokitMap3D_(file_name, content, options, update); + if (!result.ok()) { + return result; + } + updates.push_back(std::move(update)); + return AgvResult::success(); + } + + return AgvResult::success(); +} + +AgvResult SeerRobokitAgv::parseSeerRobokitMapArchive_( + const std::string& file_name, + const std::string& content, + const AgvMapStreamOptions& options, + std::vector& updates) const +{ + const auto temp_dir = makeTempDirectory(); + if (temp_dir.empty()) { + return AgvResult::failure(AgvErrorCode::CommandFailed, "create temporary map directory failed: " + systemError()); + } + + const auto archive_path = temp_dir / "map.smap"; + if (!writeBinaryFile(archive_path, content)) { + fs::remove_all(temp_dir); + return AgvResult::failure(AgvErrorCode::CommandFailed, "write temporary map archive failed"); + } + + const std::string command = "unzip -qq -o " + + shellQuote(archive_path.string()) + + " -d " + + shellQuote(temp_dir.string()); + const int unzip_result = std::system(command.c_str()); + if (unzip_result != 0) { + fs::remove_all(temp_dir); + return AgvResult::failure(AgvErrorCode::CommandFailed, "unzip SEER Robokit smap archive failed: " + file_name); + } + + if (wants2D(options.dimension)) { + std::string map2d_content; + if (readBinaryFile(temp_dir / "0.smap", map2d_content)) { + AgvUnifiedMapUpdate update; + const auto result = parseSeerRobokitMap2D_(file_name, map2d_content, options, update); + if (result.ok()) { + updates.push_back(std::move(update)); + } else { + CMVR_LOG(ERROR) << "[SeerRobokitAgv] Parse 0.smap failed" + << ", id=" << id_ + << ", file=" << file_name + << ", error=" << result.message; + } + } + } + + if (wants3D(options.dimension)) { + std::string map3d_content; + if (readBinaryFile(temp_dir / "0.3dsmap", map3d_content)) { + AgvUnifiedMapUpdate update; + const auto result = parseSeerRobokitMap3D_(file_name, map3d_content, options, update); + if (result.ok()) { + updates.push_back(std::move(update)); + } else { + CMVR_LOG(ERROR) << "[SeerRobokitAgv] Parse 0.3dsmap failed" + << ", id=" << id_ + << ", file=" << file_name + << ", error=" << result.message; + } + } + } + + fs::remove_all(temp_dir); + return updates.empty() + ? AgvResult::failure(AgvErrorCode::CommandFailed, "SEER Robokit smap archive has no requested map data: " + file_name) + : AgvResult::success(); +} + +AgvResult SeerRobokitAgv::parseSeerRobokitMap2D_( + const std::string& file_name, + const std::string& content, + const AgvMapStreamOptions& options, + AgvUnifiedMapUpdate& update) const +{ + Json::Value root; + std::string error; + if (!parseJson_(content, root, error)) { + return AgvResult::failure(AgvErrorCode::CommandFailed, "parse SEER Robokit 2D map json failed: " + error); + } + if (!root.isObject()) { + return AgvResult::failure(AgvErrorCode::CommandFailed, "SEER Robokit 2D map json root is not object"); + } + + const auto* header_ptr = jsonFind(root, "header"); + const Json::Value& header = header_ptr && header_ptr->isObject() ? *header_ptr : root; + + AgvUnifiedMap2D map; + map.frame_id = "map"; + map.timestamp = nowSeconds(); + map.resolution = jsonGet(header, "resolution", 0.0).asDouble(); + if (const auto* min_pos = jsonFind(header, "min_pos")) { + map.origin.x = jsonGet(*min_pos, "x", 0.0).asDouble(); + map.origin.y = jsonGet(*min_pos, "y", 0.0).asDouble(); + map.origin.theta = 0.0; + } + if (const auto* max_pos = jsonFind(header, "max_pos"); + max_pos && map.resolution > 0.0) { + const double width_m = jsonGet(*max_pos, "x", map.origin.x).asDouble() - map.origin.x; + const double height_m = jsonGet(*max_pos, "y", map.origin.y).asDouble() - map.origin.y; + if (width_m > 0.0 && height_m > 0.0) { + map.width = static_cast(std::ceil(width_m / map.resolution)); + map.height = static_cast(std::ceil(height_m / map.resolution)); + } + } + + const auto make_id = [](const Json::Value& value, const char* prefix, const int index) { + std::string id = jsonGet(value, "instance_name", "").asString(); + if (id.empty()) id = jsonGet(value, "id", "").asString(); + if (id.empty()) id = jsonGet(value, "name", "").asString(); + if (id.empty()) id = jsonGet(value, "point_name", "").asString(); + if (id.empty() && jsonHas(value, "tag_value")) { + id = std::to_string(jsonGet(value, "tag_value", 0).asUInt()); + } + if (id.empty()) id = std::string(prefix) + "_" + std::to_string(index); + return id; + }; + + if (const auto* list = jsonFind(root, "advanced_point_list"); list && list->isArray()) { + int index = 0; + for (const auto& item : *list) { + const auto* pos = jsonFind(item, "pos"); + appendObject( + map, + make_id(item, "station", index++), + AgvMapObjectType::Station, + pos ? std::vector{jsonPoint3D(*pos)} : std::vector{}, + jsonGet(item, "dir", 0.0).asDouble(), + item); + } + } + + if (const auto* list = jsonFind(root, "normal_line_list"); list && list->isArray()) { + int index = 0; + for (const auto& item : *list) { + std::vector points; + if (const auto* start = jsonFind(item, "start_pos")) points.push_back(jsonPoint3D(*start)); + if (const auto* end = jsonFind(item, "end_pos")) points.push_back(jsonPoint3D(*end)); + appendObject(map, make_id(item, "normal_line", index++), AgvMapObjectType::Line, std::move(points), 0.0, item); + } + } + + if (const auto* list = jsonFind(root, "advanced_line_list"); list && list->isArray()) { + int index = 0; + for (const auto& item : *list) { + std::vector points; + if (const auto* line = jsonFind(item, "line")) { + if (const auto* start = jsonFind(*line, "start_pos")) points.push_back(jsonPoint3D(*start)); + if (const auto* end = jsonFind(*line, "end_pos")) points.push_back(jsonPoint3D(*end)); + } + appendObject(map, make_id(item, "line", index++), AgvMapObjectType::Line, std::move(points), 0.0, item); + } + } + + if (const auto* list = jsonFind(root, "advanced_curve_list"); list && list->isArray()) { + int index = 0; + for (const auto& item : *list) { + std::vector points; + if (const auto* start = jsonFind(item, "start_pos")) { + if (const auto* pos = jsonFind(*start, "pos")) points.push_back(jsonPoint3D(*pos)); + } + if (const auto* control = jsonFind(item, "control_pos1")) points.push_back(jsonPoint3D(*control)); + if (const auto* control = jsonFind(item, "control_pos2")) points.push_back(jsonPoint3D(*control)); + if (const auto* control = jsonFind(item, "control_pos3")) points.push_back(jsonPoint3D(*control)); + if (const auto* control = jsonFind(item, "control_pos4")) points.push_back(jsonPoint3D(*control)); + if (const auto* end = jsonFind(item, "end_pos")) { + if (const auto* pos = jsonFind(*end, "pos")) points.push_back(jsonPoint3D(*pos)); + } + appendObject(map, make_id(item, "curve", index++), AgvMapObjectType::Line, std::move(points), 0.0, item); + } + } + + if (const auto* list = jsonFind(root, "advanced_area_list"); list && list->isArray()) { + int index = 0; + for (const auto& item : *list) { + std::vector points; + if (const auto* pos_group = jsonFind(item, "pos_group"); pos_group && pos_group->isArray()) { + for (const auto& pos : *pos_group) points.push_back(jsonPoint3D(pos)); + } + appendObject( + map, + make_id(item, "area", index++), + AgvMapObjectType::Area, + std::move(points), + jsonGet(item, "dir", 0.0).asDouble(), + item); + } + } + + if (const auto* list = jsonFind(root, "reflector_pos_list"); list && list->isArray()) { + int index = 0; + for (const auto& item : *list) { + appendObject( + map, + make_id(item, "reflector", index++), + AgvMapObjectType::Reflector, + {jsonPoint3D(item)}, + 0.0, + item); + } + } + + if (const auto* list = jsonFind(root, "tag_pos_list"); list && list->isArray()) { + int index = 0; + for (const auto& item : *list) { + appendObject( + map, + make_id(item, "tag", index++), + AgvMapObjectType::QrTag, + {jsonPoint3D(item)}, + jsonGet(item, "angle", 0.0).asDouble(), + item); + } + } + + if (const auto* list = jsonFind(root, "external_device_list"); list && list->isArray()) { + int index = 0; + for (const auto& item : *list) { + appendObject( + map, + make_id(item, "external_device", index++), + AgvMapObjectType::ExternalDevice, + {}, + 0.0, + item); + } + } + + if (const auto* groups = jsonFind(root, "bin_locations_list"); groups && groups->isArray()) { + int index = 0; + for (const auto& group : *groups) { + const auto* list = jsonFind(group, "bin_location_list"); + if (!list || !list->isArray()) { + continue; + } + for (const auto& item : *list) { + const auto* pos = jsonFind(item, "pos"); + appendObject( + map, + make_id(item, "bin_location", index++), + AgvMapObjectType::BinLocation, + pos ? std::vector{jsonPoint3D(*pos)} : std::vector{}, + 0.0, + item); + } + } + } + + std::string map_id = options.map_name; + if (map_id.empty()) map_id = jsonGet(header, "map_name", "").asString(); + if (map_id.empty()) map_id = file_name; + + update = {}; + update.map_id = map_id; + update.dimension = AgvMapDimension::Map2D; + update.update_type = AgvMapUpdateType::Snapshot; + update.frame_id = map.frame_id; + update.timestamp = map.timestamp; + update.snapshot_begin = true; + update.snapshot_end = true; + update.chunk_index = 0; + update.chunk_count = 1; + update.map_2d = std::move(map); + return AgvResult::success(); +} + +AgvResult SeerRobokitAgv::parseSeerRobokitMap3D_( + const std::string& file_name, + const std::string& content, + const AgvMapStreamOptions& options, + AgvUnifiedMapUpdate& update) const +{ + rbk::protocol::Message_Map3D src; + if (!src.ParseFromString(content)) { + return AgvResult::failure(AgvErrorCode::CommandFailed, "parse SEER Robokit 3D map protobuf failed: " + file_name); + } + + AgvUnifiedMap3D map; + map.frame_id = "map"; + map.timestamp = nowSeconds(); + if (src.has_feature_map_3d() && src.feature_map_3d().has_params()) { + map.voxel_resolution = src.feature_map_3d().params().max_voxel_size(); + } else if (src.has_header()) { + map.voxel_resolution = src.header().resolution(); + } + + map.points.reserve(static_cast(src.normal_pos3d_list_size())); + for (const auto& point : src.normal_pos3d_list()) { + AgvMapPointSample3D sample; + sample.x = point.x(); + sample.y = point.y(); + sample.z = point.z(); + map.points.push_back(sample); + } + + if (src.has_feature_map_3d()) { + const auto& feature_map = src.feature_map_3d(); + map.planes.reserve(static_cast(feature_map.planes_size())); + for (const auto& plane : feature_map.planes()) { + AgvMapPlane3D dst; + dst.center = {plane.center().x(), plane.center().y(), plane.center().z()}; + dst.normal = {plane.normal().x(), plane.normal().y(), plane.normal().z()}; + dst.d = plane.d(); + dst.radius = plane.radius(); + map.planes.push_back(dst); + } + + map.voxels.reserve(static_cast(feature_map.voxel_locs_size())); + for (const auto& voxel : feature_map.voxel_locs()) { + AgvMapVoxel3D dst; + dst.x = voxel.x(); + dst.y = voxel.y(); + dst.z = voxel.z(); + dst.probability = 1.0F; + map.voxels.push_back(dst); + } + } + + std::string map_id = options.map_name; + if (map_id.empty() && src.has_header()) map_id = src.header().map_name(); + if (map_id.empty()) map_id = src.map_directory(); + if (map_id.empty()) map_id = file_name; + + update = {}; + update.map_id = map_id; + update.dimension = AgvMapDimension::Map3D; + update.update_type = AgvMapUpdateType::Snapshot; + update.frame_id = map.frame_id; + update.timestamp = map.timestamp; + update.snapshot_begin = true; + update.snapshot_end = true; + update.chunk_index = 0; + update.chunk_count = 1; + update.map_3d = std::move(map); + return AgvResult::success(); +} + +void SeerRobokitAgv::cacheMapUpdates_(std::vector updates) const +{ + if (updates.empty()) { + return; + } + + { + std::lock_guard lock(map_update_mutex_); + if (map_session_id_.empty()) { + map_session_id_ = id_ + "_map"; + } + if (map_sequence_ == 0) { + map_sequence_ = kMapSnapshotSequenceStart - 1; + } + for (auto& update : updates) { + update.sequence = ++map_sequence_; + update.session_id = map_session_id_; + update.resume_token = std::to_string(update.sequence); + if (update.timestamp <= 0.0) update.timestamp = nowSeconds(); + if (update.frame_id.empty()) update.frame_id = "map"; + if (update.map_id.empty()) update.map_id = id_; + if (update.update_type == AgvMapUpdateType::Unspecified) { + update.update_type = AgvMapUpdateType::Snapshot; + } + cached_map_updates_.push_back(std::move(update)); + } + while (cached_map_updates_.size() > map_update_history_size_) { + cached_map_updates_.pop_front(); + } + } + map_update_cv_.notify_all(); +} + +bool SeerRobokitAgv::findCachedMapUpdate_( + const std::uint64_t after_sequence, + const AgvMapStreamOptions& options, + AgvUnifiedMapUpdate& update) const +{ + std::uint64_t effective_after = after_sequence; + if (effective_after == 0 && !options.resume_token.empty()) { + try { + effective_after = static_cast(std::stoull(options.resume_token)); + } catch (...) { + effective_after = 0; + } + } + + std::lock_guard lock(map_update_mutex_); + for (const auto& candidate : cached_map_updates_) { + if (candidate.sequence > effective_after && mapUpdateMatches_(candidate, options)) { + update = candidate; + return true; + } + } + return false; +} + +bool SeerRobokitAgv::mapUpdateMatches_( + const AgvUnifiedMapUpdate& update, + const AgvMapStreamOptions& options) const +{ + if (!options.map_name.empty() && update.map_id != options.map_name) { + return false; + } + + switch (options.dimension) { + case AgvMapDimension::Map2D: + return update.dimension == AgvMapDimension::Map2D && update.map_2d.has_value(); + case AgvMapDimension::Map3D: + return update.dimension == AgvMapDimension::Map3D && update.map_3d.has_value(); + case AgvMapDimension::Map2DAnd3D: + return (update.dimension == AgvMapDimension::Map2D && update.map_2d.has_value()) + || (update.dimension == AgvMapDimension::Map3D && update.map_3d.has_value()); + case AgvMapDimension::Unspecified: + default: + return update.map_2d.has_value() || update.map_3d.has_value(); + } +} + +AgvResult SeerRobokitAgv::stopMapping() +{ + auto result = ensureOtherSocket_(); + if (!result.ok()) return result; + + Json::Value response; + std::uint64_t accepted_generation = 0; + result = sendControlledCommand_( + sock_other_, + kRobotOtherStopMapping, + Json::Value(Json::objectValue), + &response, + &accepted_generation); + if (accepted_generation != 0) { + clearPoseTask_(accepted_generation); + } + return result; +} + +} // namespace cmvr::device diff --git a/cmvr-es/devices/agv/seer_robokit/src/seer_robokit_navigation.cpp b/cmvr-es/devices/agv/seer_robokit/src/seer_robokit_navigation.cpp new file mode 100644 index 00000000..39b1ebeb --- /dev/null +++ b/cmvr-es/devices/agv/seer_robokit/src/seer_robokit_navigation.cpp @@ -0,0 +1,1378 @@ +#include "seer_robokit_agv.h" +#include "seer_robokit_navigation_utils.h" +#include "seer_robokit_pgv_utils.h" +#include "seer_robokit_protocol.h" +#include "seer_robokit_utils.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace cmvr::device { + +using namespace seer_robokit::navigation; +using namespace seer_robokit::protocol; +using namespace seer_robokit::detail; + +AgvResult SeerRobokitAgv::emergencyStop() +{ + return emergencyStopTrackedNavigation_(nullptr); +} + +AgvResult SeerRobokitAgv::emergencyStopTrackedNavigation_( + const TrackedNavigationContext* expected_navigation) +{ + // This is a controller-level software stop, not a substitute for the + // physical emergency-stop circuit. Keep both stop commands under one + // authority acquisition so no other command from this process can + // interleave between them. + std::lock_guard sequence_lock(control_sequence_mutex_); + TrackedNavigationContext tracked_navigation; + const bool has_tracked_navigation = + currentTrackedNavigation_(tracked_navigation); + if (expected_navigation) { + const bool same_navigation_identity = has_tracked_navigation + && tracked_navigation.token == expected_navigation->token + && tracked_navigation.type == expected_navigation->type + && tracked_navigation.task_ids == expected_navigation->task_ids + && tracked_navigation.target_id == expected_navigation->target_id + && tracked_navigation.target_ids + == expected_navigation->target_ids; + if (!same_navigation_identity) { + return AgvResult::failure( + AgvErrorCode::TaskCanceled, + "SEER Robokit did not issue the tracked fail-safe software stop " + "because the navigation identity was replaced before the " + "control lock was acquired"); + } + } + const bool clearing_path_queue = has_tracked_navigation + && tracked_navigation.type == AgvTaskType::FollowPath; + const bool preserve_synchronous_wait = has_tracked_navigation + && tracked_navigation.synchronous_wait; + const auto navigation_stop_command = clearing_path_queue + ? kRobotTaskClearTargetList + : kRobotTaskCancel; + const auto stop_attempt_sequence = + control_attempt_sequence_.fetch_add( + 1, + std::memory_order_relaxed) + 1; + + const auto authority = acquireControl_(); + if (!authority.ok()) { + const std::string detail = authority.message.empty() ? "unknown error" : authority.message; + return AgvResult::failure( + authority.code, + "SEER Robokit acquire control authority failed: " + detail); + } + + struct StopOutcome { + AgvResult result; + bool controller_outcome_unknown{false}; + }; + const auto send_stop = [this](const int sock, const std::uint16_t command) { + Json::Value response; + auto result = sendCommand_( + sock, + command, + Json::Value(Json::objectValue), + &response); + if (!result.ok()) { + return StopOutcome{ + withUnknownControllerOutcome(std::move(result)), + true}; + } + if (!hasNumericControllerRetCode(response)) { + return StopOutcome{ + withUnknownControllerOutcome(resultFromResponse_(response)), + true}; + } + return StopOutcome{resultFromResponse_(response), false}; + }; + + bool generation_advanced = false; + const auto advance_generation_if_needed = [this, &generation_advanced]( + const StopOutcome& outcome) { + if (!generation_advanced + && (outcome.result.ok() || outcome.controller_outcome_unknown)) { + // Publish immediately after the first accepted or indeterminate stop + // outcome. Waiting for the second stop response would leave a window + // in which pose-start confirmation could incorrectly return success. + navigation_generation_.fetch_add(1, std::memory_order_relaxed); + generation_advanced = true; + } + }; + + const auto motion_stop = send_stop(sock_control_, kRobotControlStop); + advance_generation_if_needed(motion_stop); + const auto navigation_cancel = send_stop( + sock_navigation_, + navigation_stop_command); + advance_generation_if_needed(navigation_cancel); + if (generation_advanced) { + const auto stop_generation = + navigation_generation_.load(std::memory_order_relaxed); + if (preserve_synchronous_wait) { + advancePoseTaskGeneration_( + stop_generation, + stop_attempt_sequence); + advanceTrackedNavigationGeneration_(stop_generation); + } else { + clearPoseTask_(stop_generation); + clearTrackedNavigation_(stop_generation); + } + } + + if (!motion_stop.result.ok()) { + const std::string detail = motion_stop.result.message.empty() + ? "unknown error" + : motion_stop.result.message; + if (!navigation_cancel.result.ok()) { + const std::string cancel_detail = navigation_cancel.result.message.empty() + ? "unknown error" + : navigation_cancel.result.message; + return AgvResult::failure( + motion_stop.result.code, + "SEER Robokit software stop failed: control stop: " + detail + + "; cancel navigation: " + cancel_detail); + } + return AgvResult::failure( + motion_stop.result.code, + "SEER Robokit software stop failed: control stop: " + detail); + } + if (!navigation_cancel.result.ok()) { + const std::string detail = navigation_cancel.result.message.empty() + ? "unknown error" + : navigation_cancel.result.message; + return AgvResult::failure( + navigation_cancel.result.code, + "SEER Robokit software stop failed: cancel navigation: " + detail); + } + return AgvResult::success(); +} + +AgvResult SeerRobokitAgv::clearFault() +{ + return AgvResult::failure( + AgvErrorCode::UnsupportedCommand, + "SEER Robokit clearFault command is not implemented"); +} + +AgvResult SeerRobokitAgv::navigateToPose( + const math::Pose2d& pose, + const AgvMotionOptions& options, + const AgvAdapterParams& adapter_params) +{ + if (!std::isfinite(pose.x) + || !std::isfinite(pose.y) + || !std::isfinite(pose.theta)) { + return AgvResult::failure( + AgvErrorCode::InvalidArgument, + "SEER Robokit free-navigation pose x, y, and theta must be finite"); + } + if (const std::string error = invalidMotionOption(options); + !error.empty()) { + return AgvResult::failure( + AgvErrorCode::InvalidArgument, + "SEER Robokit free-navigation motion option " + error); + } + if (navigationCancellationRequested(options)) { + return AgvResult::failure( + AgvErrorCode::TaskCanceled, + "SEER Robokit free-navigation command was not sent because the caller " + "had already canceled the operation"); + } + // This SEER Robokit firmware exposes arbitrary-pose navigation through the + // vendor-specific freeGo extension of API 3051. The empty target id and + // GotoSpecifiedPose skill are part of the controller payload that was + // validated on the differential-drive chassis. + std::string source_id = adapter_params.getString("source_id").value_or("SELF_POSITION"); + if (source_id.empty()) { + source_id = "SELF_POSITION"; + } + if (source_id != "SELF_POSITION") { + return AgvResult::failure( + AgvErrorCode::InvalidArgument, + "SEER Robokit free navigation source_id must be SELF_POSITION"); + } + const std::string requested_target_id = + adapter_params.getString("target_id").value_or(""); + if (!requested_target_id.empty() + && requested_target_id != "SELF_POSITION") { + return AgvResult::failure( + AgvErrorCode::InvalidArgument, + "SEER Robokit free navigation target_id must be empty or SELF_POSITION so a " + "malformed freeGo request cannot fall back to station navigation"); + } + const std::string target_id; + + std::string skill_name = + adapter_params.getString("skill_name").value_or("GotoSpecifiedPose"); + if (skill_name.empty()) { + skill_name = "GotoSpecifiedPose"; + } + if (skill_name != "GotoSpecifiedPose") { + return AgvResult::failure( + AgvErrorCode::InvalidArgument, + "SEER Robokit free navigation skill_name must be GotoSpecifiedPose"); + } + if (!options.asynchronous) { + NavigationSnapshot snapshot; + const auto snapshot_result = queryNavigationSnapshot_(snapshot); + if (!snapshot_result.ok()) { + return AgvResult::failure( + snapshot_result.code, + "SEER Robokit free-navigation command was not sent because the " + "required 1101 safety/status preflight failed: " + + snapshot_result.message); + } + if (snapshot.blocked || snapshot.emergency + || !snapshot.active_faults.empty()) { + return AgvResult::failure( + snapshot.emergency + ? AgvErrorCode::EmergencyStopped + : AgvErrorCode::Fault, + "SEER Robokit free-navigation command was not sent because the " + "controller is not safe to start: " + snapshot.detail); + } + } + + const auto task_sequence = + pose_task_sequence_.fetch_add(1, std::memory_order_relaxed) + 1; + std::string task_id_prefix = + adapter_params.getString("task_id").value_or(id_); + if (task_id_prefix.empty()) { + task_id_prefix = id_; + } + const std::string task_id = + makePoseTaskId(task_id_prefix, task_sequence); + + Json::Value payload(Json::objectValue); + jsonMember(payload, "source_id") = source_id; + jsonMember(payload, "id") = target_id; + jsonMember(payload, "task_id") = task_id; + jsonMember(payload, "skill_name") = skill_name; + + auto& free_go = jsonMember(payload, "freeGo"); + jsonMember(free_go, "x") = pose.x; + jsonMember(free_go, "y") = pose.y; + jsonMember(free_go, "theta") = pose.theta; + + // Only strongly typed motion fields and the string whitelist above are + // accepted here. Generic adapter passthrough could inject unrelated 3051 + // operations such as lift, fork, script, or digital-I/O actions. + applyMotionOptions_(payload, options); + + PoseTaskContext context; + context.task_id = task_id; + context.target = pose; + context.reach_distance = options.reach_distance > 0.0 + ? options.reach_distance + : kDefaultPoseReachDistance; + context.reach_angle = options.reach_angle > 0.0 + ? options.reach_angle + : kDefaultPoseReachAngle; + TrackedNavigationContext navigation_context; + navigation_context.token = task_id; + navigation_context.task_ids = {task_id}; + navigation_context.type = AgvTaskType::NavigateToPose; + navigation_context.synchronous_wait = !options.asynchronous; + + Json::Value response; + std::uint64_t navigation_generation = 0; + auto result = sendControlledCommand_( + sock_navigation_, + kRobotTaskGoTarget, + payload, + &response, + &navigation_generation, + nullptr, + nullptr, + &context, + true, + &navigation_context, + false, + nullptr, + &options.cancellation_requested); + const AgvResult command_result = result; + const bool reconcile_indeterminate_command = !result.ok() + && navigation_generation != 0 + && !options.asynchronous; + if (!result.ok() && !reconcile_indeterminate_command) { + return result; + } + result = confirmPoseNavigationStarted_( + context, + !options.asynchronous, + options); + if (!result.ok()) { + TrackedNavigationContext active_navigation; + const bool same_token_still_current = + currentTrackedNavigation_(active_navigation) + && active_navigation.token == navigation_context.token; + if (!same_token_still_current) { + // A pause, resume, explicit cancel, emergency stop, or replacement + // navigation has already ordered the controller state after this + // command. Never let the older confirmation path issue another + // global cancellation against that newer state. + return reconciledNavigationResult( + command_result, + std::move(result)); + } + if (active_navigation.navigation_generation + != navigation_context.navigation_generation + || navigation_generation_.load(std::memory_order_relaxed) + != navigation_context.navigation_generation) { + // A cancel/stop/pause command preserved this exact token but + // advanced its control epoch. A synchronous caller must reconcile + // the exact task and two zero-velocity samples instead of issuing + // a late second cancel. Preserve the established asynchronous + // contract: start confirmation returns superseded immediately. + if (options.asynchronous) { + return reconciledNavigationResult( + command_result, + std::move(result)); + } + return reconciledNavigationResult( + command_result, + waitForPoseNavigationTerminal_( + context, + active_navigation, + options)); + } + auto failure = failAndCancelTrackedNavigation_( + navigation_context, + options, + result.code, + "SEER Robokit free-navigation start confirmation failed after the " + "controller accepted the command: " + + result.message); + return reconciledNavigationResult(command_result, std::move(failure)); + } + if (options.asynchronous) { + return result; + } + TrackedNavigationContext terminal_navigation_context = + navigation_context; + TrackedNavigationContext latest_navigation_context; + if (currentTrackedNavigation_(latest_navigation_context) + && latest_navigation_context.token == navigation_context.token) { + terminal_navigation_context = latest_navigation_context; + } + return reconciledNavigationResult( + command_result, + waitForPoseNavigationTerminal_( + context, + terminal_navigation_context, + options)); +} + +AgvResult SeerRobokitAgv::navigateToStation( + const std::string& station_id, + const AgvMotionOptions& options, + const AgvAdapterParams& adapter_params) +{ + if (station_id.empty()) { + return AgvResult::failure( + AgvErrorCode::InvalidArgument, + "SEER Robokit station-navigation station_id must not be empty"); + } + if (const std::string error = invalidMotionOption(options); + !error.empty()) { + return AgvResult::failure( + AgvErrorCode::InvalidArgument, + "SEER Robokit station-navigation motion option " + error); + } + if (navigationCancellationRequested(options)) { + return AgvResult::failure( + AgvErrorCode::TaskCanceled, + "SEER Robokit station-navigation command was not sent because the " + "caller had already canceled the operation"); + } + if (const auto jack_height = adapter_params.getString("jack_height")) { + double parsed_jack_height = 0.0; + if (!parseFiniteDouble(*jack_height, parsed_jack_height)) { + return AgvResult::failure( + AgvErrorCode::InvalidArgument, + "SEER Robokit station-navigation adapter jack_height must be a " + "complete finite number"); + } + } + + Json::Value payload(Json::objectValue); + if (const auto error = seer_robokit::pgv::applyPgvAdjustmentParams( + payload, + adapter_params); + !error.empty()) { + return AgvResult::failure( + AgvErrorCode::InvalidArgument, + "SEER Robokit station-navigation PGV adapter parameter " + error); + } + if (!options.asynchronous) { + NavigationSnapshot snapshot; + const auto snapshot_result = queryNavigationSnapshot_(snapshot); + if (!snapshot_result.ok()) { + return AgvResult::failure( + snapshot_result.code, + "SEER Robokit station-navigation command was not sent because " + "the required 1101 safety/status preflight failed: " + + snapshot_result.message); + } + if (snapshot.blocked || snapshot.emergency + || !snapshot.active_faults.empty()) { + return AgvResult::failure( + snapshot.emergency + ? AgvErrorCode::EmergencyStopped + : AgvErrorCode::Fault, + "SEER Robokit station-navigation command was not sent because " + "the controller is not safe to start: " + snapshot.detail); + } + } + + jsonMember(payload, "source_id") = adapter_params.getString("source_id").value_or("SELF_POSITION"); + jsonMember(payload, "id") = station_id; + applyAdapterParams_(payload, adapter_params); + const auto task_sequence = + pose_task_sequence_.fetch_add(1, std::memory_order_relaxed) + 1; + std::string task_id_prefix = + adapter_params.getString("task_id").value_or(id_); + if (task_id_prefix.empty()) { + task_id_prefix = id_; + } + const std::string task_id = makeNavigationTaskId( + task_id_prefix, + "station", + task_sequence); + // Always replace a caller-supplied reusable id with a unique id derived + // from it so 1110 cannot report a stale completion from an older request. + jsonMember(payload, "task_id") = task_id; + // Canonical typed motion options must win over string-valued adapter + // extensions so the SRC controller receives JSON numbers. + applyMotionOptions_(payload, options); + Json::Value response; + std::uint64_t accepted_generation = 0; + TrackedNavigationContext navigation_context; + navigation_context.token = task_id; + navigation_context.task_ids = {task_id}; + navigation_context.type = AgvTaskType::NavigateToStation; + navigation_context.target_id = station_id; + navigation_context.target_ids = {station_id}; + navigation_context.synchronous_wait = !options.asynchronous; + auto result = sendControlledCommand_( + sock_navigation_, + kRobotTaskGoTarget, + payload, + &response, + &accepted_generation, + nullptr, + nullptr, + nullptr, + false, + &navigation_context, + false, + nullptr, + &options.cancellation_requested); + if (accepted_generation != 0) { + clearPoseTask_(accepted_generation); + } + const AgvResult command_result = result; + const bool reconcile_indeterminate_command = !result.ok() + && accepted_generation != 0 + && !options.asynchronous; + if ((!result.ok() && !reconcile_indeterminate_command) + || options.asynchronous) { + return result; + } + return reconciledNavigationResult( + command_result, + waitForTrackedNavigationTerminal_( + navigation_context, + options)); +} + +AgvResult SeerRobokitAgv::followPath( + const std::vector& path) +{ + return followPath(path, AgvMotionOptions{}); +} + +AgvResult SeerRobokitAgv::followPath( + const std::vector& path, + const AgvMotionOptions& options) +{ + if (path.empty()) { + return AgvResult::failure( + AgvErrorCode::InvalidArgument, + "SEER Robokit path navigation requires at least one segment"); + } + if (const std::string error = invalidMotionOption(options); + !error.empty()) { + return AgvResult::failure( + AgvErrorCode::InvalidArgument, + "SEER Robokit path-navigation motion option " + error); + } + if (navigationCancellationRequested(options)) { + return AgvResult::failure( + AgvErrorCode::TaskCanceled, + "SEER Robokit path-navigation command was not sent because the caller " + "had already canceled the operation"); + } + for (const auto& segment : path) { + if (segment.source_station.empty() + || segment.target_station.empty()) { + return AgvResult::failure( + AgvErrorCode::InvalidArgument, + "SEER Robokit path-navigation source and target station ids " + "must not be empty"); + } + } + if (!options.asynchronous) { + NavigationSnapshot snapshot; + const auto snapshot_result = queryNavigationSnapshot_(snapshot); + if (!snapshot_result.ok()) { + return AgvResult::failure( + snapshot_result.code, + "SEER Robokit path-navigation command was not sent because the " + "required 1101 safety/status preflight failed: " + + snapshot_result.message); + } + if (snapshot.blocked || snapshot.emergency + || !snapshot.active_faults.empty()) { + return AgvResult::failure( + snapshot.emergency + ? AgvErrorCode::EmergencyStopped + : AgvErrorCode::Fault, + "SEER Robokit path-navigation command was not sent because the " + "controller is not safe to start: " + snapshot.detail); + } + } + + const auto task_sequence = + pose_task_sequence_.fetch_add(1, std::memory_order_relaxed) + 1; + const std::string batch_id = makeNavigationTaskId( + id_, + "path", + task_sequence); + Json::Value payload(Json::objectValue); + Json::Value tasks(Json::arrayValue); + std::vector task_ids; + task_ids.reserve(path.size()); + std::size_t index = 0; + for (const auto& segment : path) { + Json::Value task(Json::objectValue); + const std::string task_id = + batch_id + "_segment_" + std::to_string(index++); + jsonMember(task, "task_id") = task_id; + jsonMember(task, "source_id") = segment.source_station; + jsonMember(task, "id") = segment.target_station; + tasks.append(task); + task_ids.push_back(task_id); + } + jsonMember(payload, "move_task_list") = tasks; + Json::Value response; + std::uint64_t accepted_generation = 0; + TrackedNavigationContext navigation_context; + navigation_context.token = batch_id; + navigation_context.task_ids = task_ids; + navigation_context.type = AgvTaskType::FollowPath; + navigation_context.target_id = path.back().target_station; + navigation_context.target_ids.reserve(path.size()); + for (const auto& segment : path) { + navigation_context.target_ids.push_back(segment.target_station); + } + navigation_context.synchronous_wait = !options.asynchronous; + auto result = sendControlledCommand_( + sock_navigation_, + kRobotTaskGoTargetList, + payload, + &response, + &accepted_generation, + nullptr, + nullptr, + nullptr, + false, + &navigation_context, + false, + nullptr, + &options.cancellation_requested); + if (accepted_generation != 0) { + clearPoseTask_(accepted_generation); + } + const AgvResult command_result = result; + const bool reconcile_indeterminate_command = !result.ok() + && accepted_generation != 0 + && !options.asynchronous; + if ((!result.ok() && !reconcile_indeterminate_command) + || options.asynchronous) { + return result; + } + return reconciledNavigationResult( + command_result, + waitForTrackedNavigationTerminal_( + navigation_context, + options)); +} + +AgvResult SeerRobokitAgv::pauseNavigation() +{ + Json::Value response; + std::uint64_t accepted_generation = 0; + std::uint64_t control_attempt_sequence = 0; + auto result = sendControlledCommand_( + sock_navigation_, + kRobotTaskPause, + Json::Value(Json::objectValue), + &response, + &accepted_generation, + nullptr, + &control_attempt_sequence, + nullptr, + false, + nullptr, + true); + if (accepted_generation != 0) { + advancePoseTaskGeneration_( + accepted_generation, + control_attempt_sequence); + } + return result; +} + +AgvResult SeerRobokitAgv::resumeNavigation() +{ + Json::Value response; + std::uint64_t accepted_generation = 0; + std::uint64_t control_attempt_sequence = 0; + auto result = sendControlledCommand_( + sock_navigation_, + kRobotTaskResume, + Json::Value(Json::objectValue), + &response, + &accepted_generation, + nullptr, + &control_attempt_sequence, + nullptr, + false, + nullptr, + true); + if (accepted_generation != 0) { + advancePoseTaskGeneration_( + accepted_generation, + control_attempt_sequence); + } + return result; +} + +AgvResult SeerRobokitAgv::cancelNavigation() +{ + Json::Value response; + std::uint64_t accepted_generation = 0; + std::uint64_t control_attempt_sequence = 0; + TrackedNavigationContext tracked_navigation; + const bool has_tracked_navigation = + currentTrackedNavigation_(tracked_navigation); + const auto cancel_command = has_tracked_navigation + && tracked_navigation.type == AgvTaskType::FollowPath + ? kRobotTaskClearTargetList + : kRobotTaskCancel; + auto result = sendControlledCommand_( + sock_navigation_, + cancel_command, + Json::Value(Json::objectValue), + &response, + &accepted_generation, + nullptr, + &control_attempt_sequence, + nullptr, + false, + nullptr, + true, + has_tracked_navigation ? &tracked_navigation.token : nullptr, + nullptr, + has_tracked_navigation ? &tracked_navigation : nullptr); + if (accepted_generation != 0) { + advancePoseTaskGeneration_( + accepted_generation, + control_attempt_sequence); + } + return result; +} + +AgvResult SeerRobokitAgv::setVelocity(const AgvVelocity& velocity) +{ + if (!std::isfinite(velocity.vx) + || !std::isfinite(velocity.vy) + || !std::isfinite(velocity.wz)) { + return AgvResult::failure( + AgvErrorCode::InvalidArgument, + "SEER Robokit velocity vx, vy, and wz must be finite"); + } + + Json::Value payload(Json::objectValue); + jsonMember(payload, "vx") = velocity.vx; + jsonMember(payload, "vy") = velocity.vy; + jsonMember(payload, "w") = velocity.wz; + Json::Value response; + const bool stop_velocity = + velocity.vx == 0.0 && velocity.vy == 0.0 && velocity.wz == 0.0; + if (stop_velocity) { + std::uint64_t control_attempt_sequence = 0; + auto result = sendControlledCommand_( + sock_control_, + kRobotControlMotion, + payload, + &response, + nullptr, + nullptr, + &control_attempt_sequence); + result = result.ok() ? resultFromResponse_(response) : result; + if (result.ok()) { + advancePoseTaskControlAttempt_(control_attempt_sequence); + } + return result; + } + + std::uint64_t accepted_generation = 0; + auto result = sendControlledCommand_( + sock_control_, + kRobotControlMotion, + payload, + &response, + &accepted_generation); + if (accepted_generation != 0) { + clearPoseTask_(accepted_generation); + } + return result; +} + +void SeerRobokitAgv::rememberPoseTask_(const PoseTaskContext& context) const +{ + std::lock_guard lock(pose_task_mutex_); + if (context.navigation_generation + < pose_task_context_.navigation_generation) { + return; + } + pose_task_context_ = context; +} + +void SeerRobokitAgv::advancePoseTaskGeneration_( + const std::uint64_t navigation_generation, + const std::uint64_t control_attempt_sequence) const +{ + std::lock_guard lock(pose_task_mutex_); + if (navigation_generation + < pose_task_context_.navigation_generation) { + return; + } + pose_task_context_.navigation_generation = navigation_generation; + pose_task_context_.control_attempt_sequence_at_start = + control_attempt_sequence; +} + +void SeerRobokitAgv::advancePoseTaskControlAttempt_( + const std::uint64_t control_attempt_sequence) const +{ + std::lock_guard lock(pose_task_mutex_); + if (pose_task_context_.task_id.empty() + || control_attempt_sequence + < pose_task_context_.control_attempt_sequence_at_start) { + return; + } + pose_task_context_.control_attempt_sequence_at_start = + control_attempt_sequence; +} + +void SeerRobokitAgv::clearPoseTask_( + const std::uint64_t navigation_generation) const +{ + std::lock_guard lock(pose_task_mutex_); + if (navigation_generation < pose_task_context_.navigation_generation) { + return; + } + pose_task_context_ = PoseTaskContext{}; + pose_task_context_.navigation_generation = navigation_generation; +} + +void SeerRobokitAgv::clearPoseTaskIfTaskId_(const std::string& task_id) const +{ + std::lock_guard lock(pose_task_mutex_); + if (pose_task_context_.task_id != task_id) { + return; + } + const auto generation = pose_task_context_.navigation_generation; + pose_task_context_ = PoseTaskContext{}; + pose_task_context_.navigation_generation = generation; +} + +bool SeerRobokitAgv::currentPoseTask_(PoseTaskContext& context) const +{ + std::lock_guard lock(pose_task_mutex_); + if (pose_task_context_.task_id.empty()) { + return false; + } + context = pose_task_context_; + return true; +} + +void SeerRobokitAgv::rememberTrackedNavigation_( + const TrackedNavigationContext& context) const +{ + std::lock_guard lock(tracked_navigation_mutex_); + if (context.navigation_generation + < tracked_navigation_context_.navigation_generation) { + return; + } + tracked_navigation_context_ = context; +} + +void SeerRobokitAgv::advanceTrackedNavigationGeneration_( + const std::uint64_t navigation_generation) const +{ + std::lock_guard lock(tracked_navigation_mutex_); + if (navigation_generation + < tracked_navigation_context_.navigation_generation) { + return; + } + tracked_navigation_context_.navigation_generation = + navigation_generation; +} + +void SeerRobokitAgv::clearTrackedNavigation_( + const std::uint64_t navigation_generation) const +{ + std::lock_guard lock(tracked_navigation_mutex_); + if (navigation_generation + < tracked_navigation_context_.navigation_generation) { + return; + } + tracked_navigation_context_ = TrackedNavigationContext{}; + tracked_navigation_context_.navigation_generation = + navigation_generation; +} + +void SeerRobokitAgv::clearTrackedNavigationIfToken_( + const std::string& token) const +{ + std::lock_guard lock(tracked_navigation_mutex_); + if (tracked_navigation_context_.token != token) { + return; + } + const auto generation = + tracked_navigation_context_.navigation_generation; + tracked_navigation_context_ = TrackedNavigationContext{}; + tracked_navigation_context_.navigation_generation = generation; +} + +bool SeerRobokitAgv::currentTrackedNavigation_( + TrackedNavigationContext& context) const +{ + std::lock_guard lock(tracked_navigation_mutex_); + if (tracked_navigation_context_.token.empty()) { + return false; + } + context = tracked_navigation_context_; + return true; +} + +std::string SeerRobokitAgv::cachedControllerFaultDetail_( + const std::uint64_t after_sequence, + const int wait_ms, + std::uint64_t* associated_control_attempt) const +{ + std::unique_lock lock(runtime_state_mutex_); + const auto has_matching_fault = [this, after_sequence]() { + return !last_controller_fault_detail_.empty() + && controller_fault_sequence_ > after_sequence; + }; + if (!has_matching_fault() + && wait_ms > 0 + && state_push_enabled_) { + runtime_state_cv_.wait_for( + lock, + std::chrono::milliseconds(wait_ms), + has_matching_fault); + } + if (!has_matching_fault()) { + return {}; + } + if (associated_control_attempt) { + *associated_control_attempt = + last_controller_fault_control_attempt_; + } + return "cached_controller_fault_at=" + + std::to_string(last_controller_fault_timestamp_) + + ", " + last_controller_fault_detail_; +} + +int SeerRobokitAgv::controllerFaultCaptureGraceMs_() const +{ + const int configured_interval = config_.state_push_interval_ms(); + const int effective_interval = configured_interval > 0 + ? configured_interval + : kDefaultControllerFaultPushIntervalMs; + const auto configured_grace = + static_cast(effective_interval) + + kControllerFaultPushJitterMs; + return static_cast(std::min( + std::max( + configured_grace, + static_cast(kMinimumControllerFaultCaptureGraceMs)), + static_cast(kMaximumControllerFaultCaptureGraceMs))); +} + +int SeerRobokitAgv::controllerFaultStateMaxAgeMs_() const +{ + const int configured_interval = config_.state_push_interval_ms(); + const int effective_interval = configured_interval > 0 + ? std::min( + configured_interval, + kMaximumControllerFaultCaptureGraceMs + - kControllerFaultPushJitterMs) + : kDefaultControllerFaultPushIntervalMs; + const auto max_age = + static_cast(effective_interval) + * kControllerFaultStateMaxAgeIntervals + + kControllerFaultPushJitterMs; + return static_cast(std::max( + max_age, + static_cast(kMinimumControllerFaultStateMaxAgeMs))); +} + +std::string SeerRobokitAgv::freeNavigationFaultStateUnavailableDetail_() const +{ + std::lock_guard lock(runtime_state_mutex_); + if (!state_push_enabled_) { + return "controller fault state is unavailable because state push is " + "disabled"; + } + // A newly reported active fault is handled through the sequenced fault + // cache, including its raw fatals/errors payload. Do not replace that + // diagnostic with the less specific "incomplete push" message. + if (!active_controller_fault_detail_.empty()) { + return {}; + } + if (!controller_fault_state_observed_) { + return "no complete state push containing fatals/errors is currently " + "available"; + } + const auto fault_state_age = + std::chrono::duration_cast( + std::chrono::steady_clock::now() + - controller_fault_state_observed_at_) + .count(); + const int max_age_ms = controllerFaultStateMaxAgeMs_(); + if (fault_state_age > max_age_ms) { + return "the most recent fatals/errors state push is stale (age_ms=" + + std::to_string(fault_state_age) + + ", max_age_ms=" + std::to_string(max_age_ms) + ")"; + } + return {}; +} + +AgvResult SeerRobokitAgv::queryPoseTaskStatus_( + const std::string& task_id, + PoseTaskStatus& status) const +{ + std::vector statuses; + const auto result = queryTaskStatuses_({task_id}, statuses); + if (!result.ok()) { + status = PoseTaskStatus{}; + return result; + } + status = std::move(statuses.front()); + return result; +} + +AgvResult SeerRobokitAgv::queryTaskStatuses_( + const std::vector& requested_task_ids, + std::vector& statuses) const +{ + statuses.assign(requested_task_ids.size(), PoseTaskStatus{}); + if (requested_task_ids.empty()) { + return AgvResult::failure( + AgvErrorCode::InvalidArgument, + "SEER Robokit 1110 task status query requires at least one task id"); + } + + Json::Value payload(Json::objectValue); + Json::Value task_ids(Json::arrayValue); + for (const auto& task_id : requested_task_ids) { + task_ids.append(task_id); + } + jsonMember(payload, "task_ids") = std::move(task_ids); + + Json::Value response; + auto result = sendCommand_( + sock_status_, + kRobotStatusTaskPackage, + payload, + &response); + if (!result.ok()) { + return result; + } + result = resultFromResponse_(response); + if (!result.ok()) { + return result; + } + + const auto* package = jsonFind(response, "task_status_package"); + const double progress = package + ? jsonGet(*package, "percentage", 0.0).asDouble() + : 0.0; + if (package) { + if (const auto* status_list = jsonFind(*package, "task_status_list"); + status_list && status_list->isArray()) { + for (const auto& item : *status_list) { + const std::string returned_task_id = + jsonGet(item, "task_id", "").asString(); + const auto requested = std::find( + requested_task_ids.begin(), + requested_task_ids.end(), + returned_task_id); + if (requested == requested_task_ids.end()) { + continue; + } + const auto index = static_cast( + std::distance(requested_task_ids.begin(), requested)); + auto& status = statuses[index]; + status.found = true; + status.state = jsonGet(item, "status", 0).asInt(); + if (const auto* type = jsonFind(item, "type"); + type && type->isNumeric()) { + status.type = type->asInt(); + status.type_present = true; + } + } + } + } + + std::ostringstream common_detail; + if (const auto* ret_code = jsonFind(response, "ret_code")) { + common_detail << ", status_query_ret_code=" + << jsonValueToString(*ret_code); + } + const auto append_field = [&common_detail]( + const Json::Value& object, + const char* key, + const char* label) { + const auto* value = jsonFind(object, key); + if (!value || value->isNull()) { + return; + } + const std::string text = jsonValueToString(*value); + if (!text.empty()) { + common_detail << ", " << label << "=" << text; + } + }; + if (package) { + append_field(*package, "info", "info"); + append_field(*package, "closest_target", "closest_target"); + append_field(*package, "source_name", "source_name"); + append_field(*package, "target_name", "target_name"); + append_field(*package, "percentage", "percentage"); + append_field(*package, "distance", "distance"); + } + append_field(response, "create_on", "create_on"); + append_field(response, "err_msg", "status_query_err_msg"); + + for (std::size_t index = 0; index < requested_task_ids.size(); ++index) { + auto& status = statuses[index]; + status.progress = progress; + std::ostringstream detail; + detail << "task_id=" << requested_task_ids[index]; + if (status.found) { + detail << ", task_status=" << status.state; + if (status.type_present) { + detail << ", task_type=" << status.type; + } else { + detail << ", task_type="; + } + } else { + detail << " not present in task_status_package"; + } + detail << common_detail.str(); + status.detail = detail.str(); + } + return AgvResult::success(); +} + +AgvResult SeerRobokitAgv::queryNavigationSnapshot_( + NavigationSnapshot& snapshot) const +{ + snapshot = NavigationSnapshot{}; + + Json::Value response; + auto result = sendCommand_( + sock_status_, + kRobotStatusAll2, + Json::Value(Json::objectValue), + &response); + if (!result.ok()) { + return result; + } + result = resultFromResponse_(response); + if (!result.ok()) { + return result; + } + + const auto* task_status = jsonFind(response, "task_status"); + const auto* task_type = jsonFind(response, "task_type"); + const auto* blocked = jsonFind(response, "blocked"); + const auto* vx = jsonFind(response, "vx"); + const auto* vy = jsonFind(response, "vy"); + const auto* w = jsonFind(response, "w"); + if (!w) { + w = jsonFind(response, "wz"); + } + + if (!task_status || !task_status->isNumeric() + || !task_type || !task_type->isNumeric()) { + return AgvResult::failure( + AgvErrorCode::CommandFailed, + "SEER Robokit 1101 navigation snapshot did not contain numeric " + "task_status/task_type"); + } + if (!blocked || !(blocked->isBool() || blocked->isNumeric())) { + return AgvResult::failure( + AgvErrorCode::CommandFailed, + "SEER Robokit 1101 navigation snapshot did not contain blocked"); + } + if (!vx || !vx->isNumeric() + || !vy || !vy->isNumeric() + || !w || !w->isNumeric()) { + return AgvResult::failure( + AgvErrorCode::CommandFailed, + "SEER Robokit 1101 navigation snapshot did not contain numeric " + "vx/vy/w required to confirm that navigation has stopped"); + } + + snapshot.task_status = task_status->asInt(); + snapshot.task_type = task_type->asInt(); + snapshot.task_status_present = true; + snapshot.task_type_present = true; + snapshot.blocked = blocked->asBool(); + snapshot.blocked_present = true; + snapshot.vx = vx->asDouble(); + snapshot.vy = vy->asDouble(); + snapshot.w = w->asDouble(); + snapshot.velocity_present = + std::isfinite(snapshot.vx) + && std::isfinite(snapshot.vy) + && std::isfinite(snapshot.w); + if (!snapshot.velocity_present) { + return AgvResult::failure( + AgvErrorCode::CommandFailed, + "SEER Robokit 1101 navigation snapshot contained non-finite vx/vy/w"); + } + + if (const auto* reason = jsonFind(response, "block_reason"); + reason && !reason->isNull()) { + snapshot.block_reason_raw = jsonValueToString(*reason); + if (reason->isNumeric()) { + snapshot.block_reason = reason->asInt(); + } + } + if (const auto* target_id = jsonFind(response, "target_id"); + target_id && target_id->isString()) { + snapshot.target_id = target_id->asString(); + } + if (const auto* emergency = jsonFind(response, "emergency"); + emergency && (emergency->isBool() || emergency->isNumeric())) { + snapshot.emergency = emergency->asBool(); + } + + std::ostringstream faults; + const auto append_faults = [&response, &faults](const char* key) { + const auto* value = jsonFind(response, key); + if (!value) { + return; + } + const bool malformed = !value->isArray(); + if (!malformed && value->empty()) { + return; + } + if (faults.tellp() > 0) { + faults << ", "; + } + faults << key << "=" + << (value->isNull() + ? std::string("null") + : jsonValueToString(*value)); + if (malformed) { + faults << "(malformed; expected array)"; + } + }; + append_faults("fatals"); + append_faults("errors"); + snapshot.active_faults = faults.str(); + + std::ostringstream detail; + detail << "1101 task_status=" << snapshot.task_status + << ", task_type=" << snapshot.task_type + << ", blocked=" << (snapshot.blocked ? "true" : "false") + << ", velocity=(" << snapshot.vx << "," << snapshot.vy + << "," << snapshot.w << ")"; + if (!snapshot.target_id.empty()) { + detail << ", target_id=" << snapshot.target_id; + } + if (!snapshot.block_reason_raw.empty()) { + detail << ", block_reason=" << snapshot.block_reason_raw; + if (snapshot.block_reason >= 0) { + detail << "(" << blockReasonName(snapshot.block_reason) << ")"; + } + } + const auto append_detail_field = [&response, &detail](const char* key) { + const auto* value = jsonFind(response, key); + if (!value || value->isNull()) { + return; + } + const std::string text = jsonValueToString(*value); + if (!text.empty()) { + detail << ", " << key << "=" << text; + } + }; + append_detail_field("block_x"); + append_detail_field("block_y"); + append_detail_field("block_di"); + append_detail_field("block_ultrasonic_id"); + append_detail_field("move_status_info"); + append_detail_field("err_msg"); + append_detail_field("warnings"); + if (!snapshot.active_faults.empty()) { + detail << ", " << snapshot.active_faults; + } + if (snapshot.emergency) { + detail << ", emergency=true"; + } + snapshot.detail = detail.str(); + return AgvResult::success(); +} + +bool SeerRobokitAgv::poseTargetReached_( + const PoseTaskContext& context, + std::string& detail) const +{ + math::Pose2d current_pose; + bool current_pose_available = false; + std::string pose_source; + std::string query_error; + + Json::Value response; + auto result = sendCommand_( + sock_status_, + kRobotStatusLoc, + Json::Value(Json::objectValue), + &response); + if (result.ok()) { + result = resultFromResponse_(response); + } + const auto* x = jsonFind(response, "x"); + const auto* y = jsonFind(response, "y"); + const auto* angle = jsonFind(response, "angle"); + if (result.ok() + && x && x->isNumeric() + && y && y->isNumeric() + && angle && angle->isNumeric()) { + current_pose.x = x->asDouble(); + current_pose.y = y->asDouble(); + current_pose.theta = angle->asDouble(); + if (std::isfinite(current_pose.x) + && std::isfinite(current_pose.y) + && std::isfinite(current_pose.theta)) { + current_pose_available = true; + pose_source = "controller_1004"; + } else { + query_error = "SEER Robokit 1004 response contained non-finite x/y/angle"; + } + } else if (!result.ok()) { + query_error = result.message; + } else { + query_error = + "SEER Robokit 1004 response did not contain numeric x/y/angle"; + } + + if (!current_pose_available) { + detail = "target pose could not be verified"; + if (!query_error.empty()) { + detail += ": " + query_error; + } + return false; + } + + const double distance_error = std::hypot( + current_pose.x - context.target.x, + current_pose.y - context.target.y); + const double angle_error = angleDistance( + current_pose.theta, + context.target.theta); + std::ostringstream description; + description << "pose_source=" << pose_source + << ", current_pose=(" << current_pose.x + << "," << current_pose.y + << "," << current_pose.theta + << "), target_pose=(" << context.target.x + << "," << context.target.y + << "," << context.target.theta + << "), distance_error=" << distance_error + << ", distance_tolerance=" << context.reach_distance + << ", angle_error=" << angle_error + << ", angle_tolerance=" << context.reach_angle; + if (!query_error.empty()) { + description << ", 1004_query_error=" << query_error; + } + detail = description.str(); + return distance_error <= context.reach_distance + && angle_error <= context.reach_angle; +} + +void SeerRobokitAgv::applyMotionOptions_( + Json::Value& payload, + const AgvMotionOptions& options, + const bool include_reach_options) +{ + if (options.max_speed > 0.0) jsonMember(payload, "max_speed") = options.max_speed; + if (options.max_angular_speed > 0.0) jsonMember(payload, "max_wspeed") = options.max_angular_speed; + if (options.max_acceleration > 0.0) jsonMember(payload, "max_acc") = options.max_acceleration; + if (options.max_angular_acceleration > 0.0) jsonMember(payload, "max_wacc") = options.max_angular_acceleration; + if (include_reach_options) { + if (options.reach_distance > 0.0) jsonMember(payload, "reach_dist") = options.reach_distance; + if (options.reach_angle > 0.0) jsonMember(payload, "reach_angle") = options.reach_angle; + } +} + +void SeerRobokitAgv::applyAdapterParams_(Json::Value& payload, const AgvAdapterParams& params) +{ + for (const auto& [key, value] : params.values) { + if (seer_robokit::pgv::isPgvAdjustmentKey(key) + || key.rfind("port_", 0) == 0 + || key == "target_id" + || key == "id" + || key == "x" + || key == "y" + || key == "angle" + || key == "freeGo" + || key == "max_speed" + || key == "max_wspeed" + || key == "max_acc" + || key == "max_wacc" + || key == "reach_dist" + || key == "reach_angle" + || key == "jack_height") { + continue; + } + jsonMember(payload, key) = value; + } + if (const auto jack_height = params.getDouble("jack_height")) { + jsonMember(payload, "jack_height") = *jack_height; + } +} + +} // namespace cmvr::device diff --git a/cmvr-es/devices/agv/seer_robokit/src/seer_robokit_navigation_wait.cpp b/cmvr-es/devices/agv/seer_robokit/src/seer_robokit_navigation_wait.cpp new file mode 100644 index 00000000..fc9cf13f --- /dev/null +++ b/cmvr-es/devices/agv/seer_robokit/src/seer_robokit_navigation_wait.cpp @@ -0,0 +1,1554 @@ +#include "seer_robokit_agv.h" +#include "seer_robokit_navigation_utils.h" +#include "seer_robokit_protocol.h" +#include "seer_robokit_utils.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace cmvr::device { + +using namespace seer_robokit::navigation; +using namespace seer_robokit::protocol; +using namespace seer_robokit::detail; + +AgvResult SeerRobokitAgv::confirmPoseNavigationStarted_( + const PoseTaskContext& context, + const bool accept_paused, + const AgvMotionOptions& options) const +{ + const auto deadline = std::chrono::steady_clock::now() + kPoseNavigationStartTimeout; + std::string last_status = "no task status received"; + int consecutive_running_samples = 0; + bool matching_task_observed = false; + int last_matching_state = 0; + bool last_poll_matched = false; + bool running_stability_window_active = false; + std::chrono::steady_clock::time_point running_stable_at{}; + const auto running_stability_window = std::chrono::milliseconds( + controllerFaultCaptureGraceMs_()); + const auto hard_deadline = deadline + running_stability_window; + + const auto superseded = [this, &context, accept_paused]() { + if (!accept_paused) { + return navigation_generation_.load(std::memory_order_relaxed) + != context.navigation_generation; + } + TrackedNavigationContext active_context; + return !currentTrackedNavigation_(active_context) + || active_context.token != context.task_id; + }; + const auto superseded_result = []() { + return AgvResult::failure( + AgvErrorCode::CommandFailed, + "SEER Robokit free-navigation start confirmation was superseded by " + "another accepted navigation, velocity, pause, or stop command; " + "the controller task state is unknown, so do not retry automatically " + "before querying or canceling navigation"); + }; + const auto fault_monitoring_unavailable = + [this, &context]() { + if (controller_fault_channel_epoch_.load( + std::memory_order_relaxed) + != context.controller_fault_channel_epoch_at_start) { + return std::string( + "the controller fault push channel changed or was " + "invalidated after the free-navigation command was " + "accepted"); + } + return freeNavigationFaultStateUnavailableDetail_(); + }; + const auto fault_monitoring_unavailable_result = + [](const std::string& detail) { + return AgvResult::failure( + AgvErrorCode::Fault, + "SEER Robokit accepted the free-navigation command, but controller " + "fault monitoring became unavailable during start " + "confirmation: " + detail + + "; the task state is unsafe to accept, so query the " + "controller and cancel or stop before another motion " + "command"); + }; + const auto fault_attribution_is_ambiguous = + [&context](const std::uint64_t associated_control_attempt) { + return associated_control_attempt != 0 + && associated_control_attempt + != context.control_attempt_sequence_at_start; + }; + const auto ambiguous_fault_result = + [](const std::string& task_detail, const std::string& fault) { + return AgvResult::failure( + AgvErrorCode::Fault, + "SEER Robokit reported a controller fault after another control " + "command attempt had begun; the fault " + "cannot be attributed to the tracked free-navigation task: " + + task_detail + ", " + fault + + "; query navigation status and cancel or stop before " + "another motion command"); + }; + const auto supersededWithFault = [&]() { + std::uint64_t fault_control_attempt = 0; + const std::string fault = cachedControllerFaultDetail_( + context.controller_fault_sequence_at_start, + 0, + &fault_control_attempt); + if (!fault.empty() + && fault_attribution_is_ambiguous(fault_control_attempt)) { + return ambiguous_fault_result(last_status, fault); + } + return superseded_result(); + }; + + while (true) { + if (navigationCancellationRequested(options)) { + return AgvResult::failure( + AgvErrorCode::TaskCanceled, + "SEER Robokit free-navigation start confirmation was canceled by " + "the caller"); + } + if (superseded()) { + std::uint64_t fault_control_attempt = 0; + const std::string fault = cachedControllerFaultDetail_( + context.controller_fault_sequence_at_start, + 0, + &fault_control_attempt); + if (!fault.empty() + && fault_attribution_is_ambiguous( + fault_control_attempt)) { + return ambiguous_fault_result(last_status, fault); + } + return supersededWithFault(); + } + if (const std::string unavailable = + fault_monitoring_unavailable(); + !unavailable.empty()) { + return fault_monitoring_unavailable_result(unavailable); + } + + PoseTaskStatus task_status; + const auto query_result = queryPoseTaskStatus_( + context.task_id, + task_status); + if (!query_result.ok()) { + const std::string detail = query_result.message.empty() + ? "unknown error" + : query_result.message; + return AgvResult::failure( + query_result.code, + "SEER Robokit accepted the free-navigation command, but task start " + "could not be verified: " + detail + + "; do not retry automatically before checking or canceling navigation"); + } + + if (superseded()) { + return supersededWithFault(); + } + if (const std::string unavailable = + fault_monitoring_unavailable(); + !unavailable.empty()) { + return fault_monitoring_unavailable_result(unavailable); + } + + last_status = task_status.detail; + const bool task_present = + task_status.found && task_status.state != 404; + last_poll_matched = task_present; + if (task_present) { + matching_task_observed = true; + last_matching_state = task_status.state; + if (task_status.type_present && task_status.type != 1) { + return AgvResult::failure( + AgvErrorCode::TaskRejected, + "SEER Robokit created an unexpected task type for free navigation: " + + last_status); + } + + if (task_status.state == 2) { + const auto now = std::chrono::steady_clock::now(); + if (!running_stability_window_active) { + running_stability_window_active = true; + running_stable_at = now + running_stability_window; + } + ++consecutive_running_samples; + std::uint64_t fault_control_attempt = 0; + const std::string fault = cachedControllerFaultDetail_( + context.controller_fault_sequence_at_start, + 0, + &fault_control_attempt); + if (!fault.empty()) { + if (superseded()) { + return supersededWithFault(); + } + if (fault_attribution_is_ambiguous( + fault_control_attempt)) { + return ambiguous_fault_result(last_status, fault); + } + return AgvResult::failure( + AgvErrorCode::Fault, + "SEER Robokit free-navigation task reached Running state, " + "but the controller reported a new fault during start " + "confirmation: " + last_status + ", " + fault + + "; do not retry automatically; cancel or stop the " + "task before another motion command"); + } + if (consecutive_running_samples + >= kPoseNavigationRequiredRunningSamples + && now >= running_stable_at) { + if (superseded()) { + return supersededWithFault(); + } + if (const std::string unavailable = + fault_monitoring_unavailable(); + !unavailable.empty()) { + return fault_monitoring_unavailable_result( + unavailable); + } + return AgvResult::success(); + } + } else { + consecutive_running_samples = 0; + running_stability_window_active = false; + } + + if (task_status.state == 3) { + std::uint64_t fault_control_attempt = 0; + const std::string fault = cachedControllerFaultDetail_( + context.controller_fault_sequence_at_start, + controllerFaultCaptureGraceMs_(), + &fault_control_attempt); + if (superseded()) { + return supersededWithFault(); + } + if (!fault.empty()) { + if (fault_attribution_is_ambiguous( + fault_control_attempt)) { + return ambiguous_fault_result(last_status, fault); + } + return AgvResult::failure( + AgvErrorCode::Fault, + "SEER Robokit free-navigation task was established but " + "paused while the controller reported a new fault: " + + last_status + ", " + fault + + "; do not retry automatically before querying or " + "canceling it"); + } + if (const std::string unavailable = + fault_monitoring_unavailable(); + !unavailable.empty()) { + return fault_monitoring_unavailable_result(unavailable); + } + if (accept_paused) { + return AgvResult::success(); + } + return AgvResult::failure( + AgvErrorCode::TaskRejected, + "SEER Robokit free-navigation task was established but is paused: " + + last_status + + "; do not retry automatically before querying or canceling it"); + } + if (task_status.state == 4) { + std::uint64_t fault_control_attempt = 0; + const std::string fault = cachedControllerFaultDetail_( + context.controller_fault_sequence_at_start, + controllerFaultCaptureGraceMs_(), + &fault_control_attempt); + if (superseded()) { + return supersededWithFault(); + } + if (const std::string unavailable = + fault_monitoring_unavailable(); + !unavailable.empty()) { + return fault_monitoring_unavailable_result(unavailable); + } + if (!fault.empty()) { + if (fault_attribution_is_ambiguous( + fault_control_attempt)) { + return ambiguous_fault_result(last_status, fault); + } + return AgvResult::failure( + AgvErrorCode::Fault, + "SEER Robokit free-navigation task reported Completed, but " + "the controller reported a new fault during completion " + "confirmation: " + last_status + ", " + fault + + "; do not retry automatically; cancel or stop " + "the task before another motion command"); + } + if (accept_paused) { + // Synchronous callers perform the authoritative pose + // check only after two zero-velocity 1101 samples. A + // Completed task may still be decelerating here. + return AgvResult::success(); + } + std::string pose_detail; + const bool target_reached = + poseTargetReached_(context, pose_detail); + if (superseded()) { + return supersededWithFault(); + } + std::uint64_t post_pose_fault_control_attempt = 0; + const std::string post_pose_fault = + cachedControllerFaultDetail_( + context.controller_fault_sequence_at_start, + 0, + &post_pose_fault_control_attempt); + if (!post_pose_fault.empty()) { + if (fault_attribution_is_ambiguous( + post_pose_fault_control_attempt)) { + return ambiguous_fault_result( + last_status, + post_pose_fault); + } + return AgvResult::failure( + AgvErrorCode::Fault, + "SEER Robokit free-navigation task reported Completed, but " + "the controller reported a new fault during target " + "verification: " + last_status + ", " + + post_pose_fault + + "; do not retry automatically; cancel or stop " + "the task before another motion command"); + } + if (const std::string unavailable = + fault_monitoring_unavailable(); + !unavailable.empty()) { + return fault_monitoring_unavailable_result(unavailable); + } + if (target_reached) { + return AgvResult::success(); + } + return AgvResult::failure( + AgvErrorCode::TaskRejected, + "SEER Robokit free-navigation task completed before a stable running " + "state, but the requested target was not reached: " + + last_status + ", " + pose_detail + + "; check the freeGo payload and controller alarms before retrying"); + } + if (task_status.state == 5 + || task_status.state == 6 + || task_status.state == 7) { + std::string detail = last_status; + std::uint64_t fault_control_attempt = 0; + const std::string fault = cachedControllerFaultDetail_( + context.controller_fault_sequence_at_start, + controllerFaultCaptureGraceMs_(), + &fault_control_attempt); + if (superseded()) { + return supersededWithFault(); + } + if (!fault.empty()) { + if (fault_attribution_is_ambiguous( + fault_control_attempt)) { + detail += + ", controller_fault_attribution=ambiguous because " + "the fault was observed after another control " + "command attempt had begun"; + } + detail += ", " + fault; + } + return AgvResult::failure( + task_status.state == 5 || task_status.state == 7 + ? AgvErrorCode::TaskFailed + : AgvErrorCode::TaskCanceled, + task_status.state == 5 || task_status.state == 7 + ? "SEER Robokit free-navigation task failed: " + detail + : "SEER Robokit free-navigation task was canceled: " + detail); + } + if (task_status.state < 1 || task_status.state > 7) { + return AgvResult::failure( + AgvErrorCode::TaskFailed, + "SEER Robokit free-navigation task reported an unsupported " + "terminal or vendor-specific status: " + last_status); + } + } else { + consecutive_running_samples = 0; + running_stability_window_active = false; + } + + const auto now = std::chrono::steady_clock::now(); + if (now >= hard_deadline + || (now >= deadline && !running_stability_window_active)) { + break; + } + sleepForNavigationPoll( + kPoseNavigationPollInterval, + hard_deadline, + options); + } + + if (matching_task_observed + && last_poll_matched + && last_matching_state == 1) { + // A matching Waiting task has been accepted by the controller and may + // legitimately remain queued. Returning a rejection here would invite + // a duplicate command while the original task can still start later. + std::uint64_t fault_control_attempt = 0; + const std::string fault = cachedControllerFaultDetail_( + context.controller_fault_sequence_at_start, + controllerFaultCaptureGraceMs_(), + &fault_control_attempt); + if (superseded()) { + return supersededWithFault(); + } + if (const std::string unavailable = + fault_monitoring_unavailable(); + !unavailable.empty()) { + return fault_monitoring_unavailable_result(unavailable); + } + if (!fault.empty()) { + if (fault_attribution_is_ambiguous( + fault_control_attempt)) { + return ambiguous_fault_result(last_status, fault); + } + return AgvResult::failure( + AgvErrorCode::Fault, + "SEER Robokit free-navigation task was accepted and remains active, " + "but the controller reported a new fault: " + + last_status + ", " + fault + + "; the task may still start later, so do not retry " + "automatically; cancel or stop it before another motion command"); + } + return AgvResult::success(); + } + + std::string detail = last_status; + std::uint64_t fault_control_attempt = 0; + const std::string fault = cachedControllerFaultDetail_( + context.controller_fault_sequence_at_start, + 0, + &fault_control_attempt); + if (superseded()) { + return supersededWithFault(); + } + if (!fault.empty()) { + if (fault_attribution_is_ambiguous( + fault_control_attempt)) { + detail += + ", controller_fault_attribution=ambiguous because the fault " + "was observed after another control command attempt had begun"; + } + detail += ", " + fault; + } + return AgvResult::failure( + AgvErrorCode::TaskRejected, + "SEER Robokit accepted the free-navigation command, but no stable matching " + "pose task was established within " + + std::to_string(kPoseNavigationStartTimeout.count()) + + " ms; last " + detail + + "; do not retry automatically before checking or canceling navigation"); +} + +AgvResult SeerRobokitAgv::cancelTrackedNavigation_( + const TrackedNavigationContext& context, + std::uint64_t& accepted_generation) +{ + accepted_generation = 0; + Json::Value response; + const auto cancel_command = context.type == AgvTaskType::FollowPath + ? kRobotTaskClearTargetList + : kRobotTaskCancel; + auto result = sendControlledCommand_( + sock_navigation_, + cancel_command, + Json::Value(Json::objectValue), + &response, + &accepted_generation, + nullptr, + nullptr, + nullptr, + false, + nullptr, + true, + &context.token, + nullptr, + &context); + if (accepted_generation != 0) { + clearPoseTask_(accepted_generation); + } + return result; +} + +AgvResult SeerRobokitAgv::waitForCanceledTaskToStop_( + const TrackedNavigationContext& context, + const AgvMotionOptions& options, + const std::string& reason) +{ + if (context.task_ids.empty()) { + return AgvResult::failure( + AgvErrorCode::CommandFailed, + "SEER Robokit cannot confirm cancellation without a tracked task id"); + } + + (void)options; + const auto confirmation_window = kNavigationCancelConfirmationTimeout; + const auto deadline = + std::chrono::steady_clock::now() + confirmation_window; + int stopped_samples = 0; + std::string last_detail = "no post-cancel status received"; + while (std::chrono::steady_clock::now() < deadline) { + std::vector task_statuses; + const auto task_result = queryTaskStatuses_( + context.task_ids, + task_statuses); + if (!task_result.ok()) { + return AgvResult::failure( + task_result.code, + "SEER Robokit navigation cancel was sent, but exact task " + "termination could not be queried: " + task_result.message); + } + + std::ostringstream exact_detail; + bool all_exact_tasks_terminal = !task_statuses.empty(); + bool any_exact_task_active = false; + for (std::size_t index = 0; index < task_statuses.size(); ++index) { + if (index > 0) { + exact_detail << "; "; + } + const auto& status = task_statuses[index]; + exact_detail << status.detail; + if (!status.found + || !exactTaskStateIsKnownTerminal(status.state)) { + all_exact_tasks_terminal = false; + } + if (status.found && exactTaskStateIsActive(status.state)) { + any_exact_task_active = true; + } + } + last_detail = exact_detail.str(); + + TrackedNavigationContext active_context; + const bool another_local_navigation_started = + currentTrackedNavigation_(active_context) + && active_context.token != context.token; + if (all_exact_tasks_terminal && another_local_navigation_started) { + // A later navigation is allowed to move after this exact task has + // reached a terminal state. Its velocity must not keep the older + // waiter alive or make it cancel the newer task. + for (const auto& task_id : context.task_ids) { + clearPoseTaskIfTaskId_(task_id); + } + return { + AgvErrorCode::OK, + "old exact task ids are terminal; global stopped state was " + "not inspected because a newer local navigation owns 1101"}; + } + if (another_local_navigation_started) { + return AgvResult::failure( + AgvErrorCode::CommandFailed, + "SEER Robokit conditional navigation cancel could not confirm all " + "exact task ids terminal before a newer local navigation " + "started; the newer task was not inspected or canceled; " + + last_detail); + } + + NavigationSnapshot snapshot; + const auto snapshot_result = queryNavigationSnapshot_(snapshot); + if (!snapshot_result.ok()) { + return AgvResult::failure( + snapshot_result.code, + "SEER Robokit navigation cancel was sent, but stopped state " + "could not be confirmed with 1101: " + + snapshot_result.message); + } + last_detail += ", " + snapshot.detail; + const int expected_global_type = + context.type == AgvTaskType::NavigateToPose + ? 1 + : (context.type == AgvTaskType::NavigateToStation ? 2 : 3); + const bool global_target_matches = snapshot.target_id.empty() + || context.target_ids.empty() + || std::find( + context.target_ids.begin(), + context.target_ids.end(), + snapshot.target_id) + != context.target_ids.end(); + const bool global_terminal = snapshot.task_status == 0 + || (exactTaskStateIsKnownTerminal(snapshot.task_status) + && snapshot.task_type == expected_global_type + && global_target_matches); + + if ((all_exact_tasks_terminal + || (!any_exact_task_active && global_terminal)) + && navigationStopped(snapshot)) { + ++stopped_samples; + if (stopped_samples >= kRequiredCompletedStopSamples) { + for (const auto& task_id : context.task_ids) { + clearPoseTaskIfTaskId_(task_id); + } + clearTrackedNavigationIfToken_(context.token); + return { + AgvErrorCode::OK, + "stopped state was confirmed from task termination and " + "two zero-velocity samples"}; + } + } else { + stopped_samples = 0; + } + const auto now = std::chrono::steady_clock::now(); + if (now < deadline) { + std::this_thread::sleep_for(std::min( + kNavigationCancelPollInterval, + std::chrono::duration_cast( + deadline - now))); + } + } + + return AgvResult::failure( + AgvErrorCode::Timeout, + "SEER Robokit accepted the conditional navigation cancel, but task " + "termination and stopped velocity were not confirmed after " + + std::to_string(confirmation_window.count()) + " ms; reason=" + + reason + ", last_status=" + last_detail + + "; the robot state must be checked before another motion command"); +} + +AgvResult SeerRobokitAgv::failAndCancelTrackedNavigation_( + const TrackedNavigationContext& context, + const AgvMotionOptions& options, + const AgvErrorCode error_code, + const std::string& reason) +{ + const auto same_navigation_identity = []( + const TrackedNavigationContext& lhs, + const TrackedNavigationContext& rhs) { + return lhs.token == rhs.token + && lhs.type == rhs.type + && lhs.task_ids == rhs.task_ids + && lhs.target_id == rhs.target_id + && lhs.target_ids == rhs.target_ids; + }; + + TrackedNavigationContext cancel_context = context; + std::uint64_t cancel_generation = 0; + auto cancel_result = cancelTrackedNavigation_( + cancel_context, + cancel_generation); + + // A public cancel, pause/resume, or emergency stop can advance the control + // generation of this exact logical task while its synchronous waiter is + // handling an error. Refresh only an identical token/type/task/target + // identity; a replacement navigation must remain impossible to cancel. + bool same_task_generation_advanced = false; + for (int refresh_attempt = 0; + !cancel_result.ok() + && cancel_generation == 0 + && cancel_result.code == AgvErrorCode::TaskCanceled + && refresh_attempt < 3; + ++refresh_attempt) { + TrackedNavigationContext latest_context; + if (!currentTrackedNavigation_(latest_context) + || !same_navigation_identity(context, latest_context)) { + break; + } + if (latest_context.navigation_generation + <= cancel_context.navigation_generation) { + // sendControlledCommand_ publishes the global generation just + // before updating the tracked context. Yield across that tiny + // window, but keep the retry bounded. + if (navigation_generation_.load(std::memory_order_relaxed) + > cancel_context.navigation_generation) { + same_task_generation_advanced = true; + std::this_thread::yield(); + continue; + } + break; + } + same_task_generation_advanced = true; + cancel_context = std::move(latest_context); + cancel_generation = 0; + cancel_result = cancelTrackedNavigation_( + cancel_context, + cancel_generation); + } + + bool fail_safe_stop_sent = false; + const auto issue_tracked_fail_safe_stop = [this, + &cancel_context, + &fail_safe_stop_sent]() { + const auto stop_result = + emergencyStopTrackedNavigation_(&cancel_context); + fail_safe_stop_sent = stop_result.ok(); + return stop_result; + }; + + // If 1110/1101 ownership preflight is unavailable, a bare global + // 3003/3067 based only on stale local state could cancel another client's + // task. Escalate explicitly to the controller's software-stop sequence + // (2000 plus the matching navigation cancel), protected by a complete + // navigation-identity check under the control mutex. + const bool ownership_status_unavailable = + cancel_result.code == AgvErrorCode::NotConnected + || cancel_result.code == AgvErrorCode::Timeout + || cancel_result.code == AgvErrorCode::CommandFailed; + if (!cancel_result.ok() && cancel_generation == 0 + && (ownership_status_unavailable || same_task_generation_advanced)) { + const auto stop_result = issue_tracked_fail_safe_stop(); + if (!stop_result.ok()) { + return AgvResult::failure( + error_code, + reason + "; conditional cancel ownership could not be " + "confirmed: " + cancel_result.message + + "; tracked fail-safe software stop/cancel failed: " + + stop_result.message + + "; the stopped state is unconfirmed, so do not retry " + "motion automatically"); + } + } + if (!cancel_result.ok() && cancel_generation == 0 + && !fail_safe_stop_sent) { + return AgvResult::failure( + error_code, + reason + "; conditional cancel was not accepted: " + + cancel_result.message + + "; the original navigation task may still be active or may " + "have been replaced, so do not retry motion automatically"); + } + + auto stopped_result = waitForCanceledTaskToStop_( + cancel_context, + options, + reason); + if (!stopped_result.ok() && !fail_safe_stop_sent) { + // Exact task terminal state is not proof that a differential chassis + // has stopped. If 1101 cannot confirm two zero-velocity samples after + // a normal cancel (or after observing an already-terminal task), issue + // a task-identity-protected software stop before returning an error. + const auto stop_result = issue_tracked_fail_safe_stop(); + if (!stop_result.ok()) { + return AgvResult::failure( + error_code, + reason + "; stopped state could not be confirmed: " + + stopped_result.message + + "; tracked fail-safe software stop/cancel failed: " + + stop_result.message + + "; the stopped state remains unconfirmed, so do not " + "retry motion automatically"); + } + stopped_result = waitForCanceledTaskToStop_( + cancel_context, + options, + reason); + } + if (!stopped_result.ok()) { + const std::string cancel_detail = fail_safe_stop_sent + ? "; tracked fail-safe software stop and navigation cancel were issued" + : (cancel_generation != 0 + ? "; controller accepted conditional cancel" + : (cancel_result.ok() + ? "; exact task was already terminal, so global cancel was not sent" + : "; conditional cancel response was indeterminate: " + + cancel_result.message)); + return AgvResult::failure( + error_code, + reason + cancel_detail + "; stopped state remains unconfirmed: " + + stopped_result.message); + } + + const std::string cancel_detail = fail_safe_stop_sent + ? "; a tracked fail-safe software stop and navigation cancel were issued" + : (cancel_generation != 0 + ? "; the tracked navigation task was conditionally canceled" + : (cancel_result.ok() + ? "; the exact task was already terminal and no global cancel was sent" + : "; cancel response was indeterminate, but the exact task subsequently terminated")); + return AgvResult::failure( + error_code, + reason + cancel_detail + "; " + stopped_result.message); +} + +AgvResult SeerRobokitAgv::waitForTrackedNavigationTerminal_( + const TrackedNavigationContext& context, + const AgvMotionOptions& options) +{ + if (context.task_ids.empty()) { + return AgvResult::failure( + AgvErrorCode::CommandFailed, + "SEER Robokit synchronous navigation has no task id to track"); + } + + const auto poll_interval = navigationPollInterval(options); + const auto deadline = std::chrono::steady_clock::now() + + navigationWaitTimeout(options); + const auto accepted_at = context.accepted_at + == std::chrono::steady_clock::time_point{} + ? std::chrono::steady_clock::now() + : context.accepted_at; + const auto start_deadline = accepted_at + kPoseNavigationStartTimeout; + bool any_task_observed = false; + bool final_completion_observed = false; + AgvErrorCode terminal_error = AgvErrorCode::OK; + std::string terminal_reason; + int blocked_stopped_samples = 0; + int terminal_stopped_samples = 0; + std::string last_detail = "no task status received"; + bool superseded_wait_active = false; + std::chrono::steady_clock::time_point superseded_deadline; + + while (std::chrono::steady_clock::now() < deadline) { + TrackedNavigationContext active_context; + const bool still_current = currentTrackedNavigation_(active_context) + && active_context.token == context.token; + if (navigationCancellationRequested(options)) { + if (!still_current) { + return AgvResult::failure( + AgvErrorCode::TaskCanceled, + "SEER Robokit synchronous navigation wait was canceled after " + "the tracked task had already been replaced; the newer " + "task was not canceled"); + } + return failAndCancelTrackedNavigation_( + context, + options, + AgvErrorCode::TaskCanceled, + "SEER Robokit synchronous navigation wait was canceled by the caller"); + } + + std::vector task_statuses; + const auto task_result = queryTaskStatuses_( + context.task_ids, + task_statuses); + if (!task_result.ok()) { + if (!still_current) { + return AgvResult::failure( + AgvErrorCode::CommandFailed, + "SEER Robokit superseded navigation exact task query failed; " + "the newer task was not canceled: " + task_result.message); + } + return failAndCancelTrackedNavigation_( + context, + options, + task_result.code, + "SEER Robokit synchronous navigation exact task query failed: " + + task_result.message); + } + std::ostringstream exact_detail; + bool any_status_found_now = false; + bool any_exact_active = false; + bool all_exact_terminal_now = !task_statuses.empty(); + for (std::size_t index = 0; index < task_statuses.size(); ++index) { + auto& task_status = task_statuses[index]; + if (index > 0) { + exact_detail << "; "; + } + exact_detail << task_status.detail; + if (!task_status.found || task_status.state == 404) { + all_exact_terminal_now = false; + continue; + } + any_status_found_now = true; + any_task_observed = true; + if (task_status.state >= 1 && task_status.state <= 3) { + any_exact_active = true; + } + if (!exactTaskStateIsKnownTerminal(task_status.state)) { + all_exact_terminal_now = false; + } + if (task_status.type_present) { + const bool expected_type = + context.type == AgvTaskType::NavigateToStation + ? task_status.type == 2 + : (task_status.type == 2 || task_status.type == 3); + if (!expected_type) { + return failAndCancelTrackedNavigation_( + context, + options, + AgvErrorCode::TaskRejected, + "SEER Robokit exact task id reported an unexpected task " + "type: " + task_status.detail); + } + } + if (task_status.state == 5 || task_status.state == 7) { + terminal_error = AgvErrorCode::TaskFailed; + terminal_reason = + "SEER Robokit tracked navigation segment failed: " + + task_status.detail; + } else if (task_status.state == 6 + && terminal_error == AgvErrorCode::OK) { + terminal_error = AgvErrorCode::TaskCanceled; + terminal_reason = + "SEER Robokit tracked navigation segment was canceled: " + + task_status.detail; + } else if (task_status.state == 4 + && index + 1 == task_statuses.size()) { + final_completion_observed = true; + } else if (task_status.state < 1 + || task_status.state > 7) { + return failAndCancelTrackedNavigation_( + context, + options, + AgvErrorCode::TaskFailed, + "SEER Robokit tracked navigation reported an unsupported " + "terminal or vendor-specific state: " + task_status.detail); + } + } + last_detail = exact_detail.str(); + + TrackedNavigationContext post_query_context; + const bool still_current_after_query = + currentTrackedNavigation_(post_query_context) + && post_query_context.token == context.token; + + if (!any_task_observed + && std::chrono::steady_clock::now() >= start_deadline) { + if (!still_current_after_query) { + return AgvResult::failure( + AgvErrorCode::CommandFailed, + "SEER Robokit navigation was superseded before any exact task " + "id was observed; the newer task was not canceled; " + + last_detail); + } + return failAndCancelTrackedNavigation_( + context, + options, + AgvErrorCode::TaskRejected, + "SEER Robokit accepted the navigation command, but its exact task " + "id was not established within " + + std::to_string(kPoseNavigationStartTimeout.count()) + + " ms: " + last_detail); + } + + // Once another local control command has replaced this token, never + // consume global 1101 state (it can belong to the newer command) and + // never issue a global cancel. Exact terminal state is sufficient to + // finish the older waiter; otherwise wait briefly for 1110 to settle. + if (!still_current_after_query) { + if (terminal_error != AgvErrorCode::OK) { + return AgvResult::failure(terminal_error, terminal_reason); + } + if (final_completion_observed) { + return AgvResult::failure( + AgvErrorCode::CommandFailed, + "SEER Robokit navigation final task reported Completed, but a " + "newer local task replaced it before stopped state could " + "be confirmed; the newer task was not canceled"); + } + if (any_task_observed && !any_status_found_now) { + return AgvResult::failure( + AgvErrorCode::CommandFailed, + "SEER Robokit navigation was superseded and its exact task " + "status disappeared; the newer task was not canceled; " + + last_detail); + } + if (!superseded_wait_active) { + superseded_wait_active = true; + superseded_deadline = std::chrono::steady_clock::now() + + kNavigationCancelConfirmationTimeout; + } + if (std::chrono::steady_clock::now() >= superseded_deadline) { + return AgvResult::failure( + AgvErrorCode::CommandFailed, + "SEER Robokit superseded navigation exact task remained " + "non-terminal after the replacement grace window; the " + "newer task was not inspected or canceled; " + last_detail); + } + sleepForNavigationPoll( + poll_interval, + std::min(deadline, superseded_deadline), + options); + continue; + } + + const bool failed_path_may_have_hidden_queued_segments = + terminal_error != AgvErrorCode::OK + && context.type == AgvTaskType::FollowPath + && !all_exact_terminal_now; + if (terminal_error != AgvErrorCode::OK + && (any_exact_active + || failed_path_may_have_hidden_queued_segments)) { + return failAndCancelTrackedNavigation_( + context, + options, + terminal_error, + terminal_reason + + "; another exact path segment is still active or remains " + "hidden in the queued path and must be cleared before " + "any global 1101 attribution; " + + last_detail); + } + + NavigationSnapshot snapshot; + const auto snapshot_result = queryNavigationSnapshot_(snapshot); + if (!snapshot_result.ok()) { + if (terminal_error != AgvErrorCode::OK + || final_completion_observed) { + last_detail += ", 1101 detail unavailable: " + + snapshot_result.message; + sleepForNavigationPoll(poll_interval, deadline, options); + continue; + } + return failAndCancelTrackedNavigation_( + context, + options, + snapshot_result.code, + "SEER Robokit synchronous navigation safety snapshot failed: " + + snapshot_result.message); + } + last_detail += ", " + snapshot.detail; + + TrackedNavigationContext post_snapshot_context; + if (!currentTrackedNavigation_(post_snapshot_context) + || post_snapshot_context.token != context.token) { + // A concurrent navigation may have replaced this task while 1101 + // was in flight. Discard that global snapshot because it may + // already describe the replacement. + if (terminal_error != AgvErrorCode::OK) { + return AgvResult::failure(terminal_error, terminal_reason); + } + if (final_completion_observed) { + return AgvResult::failure( + AgvErrorCode::CommandFailed, + "SEER Robokit navigation final task reported Completed, but " + "the 1101 snapshot was superseded before stopped state " + "could be confirmed; the newer task was not canceled"); + } + if (!superseded_wait_active) { + superseded_wait_active = true; + superseded_deadline = std::chrono::steady_clock::now() + + kNavigationCancelConfirmationTimeout; + } + if (std::chrono::steady_clock::now() >= superseded_deadline) { + return AgvResult::failure( + AgvErrorCode::CommandFailed, + "SEER Robokit superseded navigation did not expose an exact " + "terminal state during the replacement grace window; the " + "newer task was not inspected or canceled; " + last_detail); + } + sleepForNavigationPoll( + poll_interval, + std::min(deadline, superseded_deadline), + options); + continue; + } + + const int expected_global_type = + context.type == AgvTaskType::NavigateToStation ? 2 : 3; + const bool global_active = snapshot.task_status >= 1 + && snapshot.task_status <= 3; + // 1110 and 1101 are separate controller publications. During the + // task-establishment window, even a newly visible exact task id can + // be paired with an older global snapshot. Until that window closes, + // use 1101 only for physical safety (faults, emergency, blockage and + // velocity), never for ownership or task-terminal attribution. + const bool global_attribution_ready = any_task_observed + && std::chrono::steady_clock::now() >= start_deadline; + const bool attributed_global_active = global_attribution_ready + && global_active; + const bool global_type_matches = + snapshot.task_type == expected_global_type; + const bool global_target_matches = + context.type == AgvTaskType::NavigateToStation + ? (!snapshot.target_id.empty() + && snapshot.target_id == context.target_id) + : (context.type == AgvTaskType::FollowPath + ? (!snapshot.target_id.empty() + && std::find( + context.target_ids.begin(), + context.target_ids.end(), + snapshot.target_id) + != context.target_ids.end()) + : true); + const bool global_matches_context = global_type_matches + && global_target_matches; + const bool attributed_matching_global_active = + attributed_global_active && global_matches_context; + if (snapshot.emergency || !snapshot.active_faults.empty()) { + return failAndCancelTrackedNavigation_( + context, + options, + snapshot.emergency + ? AgvErrorCode::EmergencyStopped + : AgvErrorCode::Fault, + "SEER Robokit controller reported a navigation safety fault: " + + last_detail); + } + if (terminal_error == AgvErrorCode::OK + && !any_exact_active + && global_attribution_ready + && global_matches_context + && (snapshot.task_status == 5 + || snapshot.task_status == 7)) { + terminal_error = AgvErrorCode::TaskFailed; + terminal_reason = + "SEER Robokit controller reported navigation failure: " + + last_detail; + } else if (terminal_error == AgvErrorCode::OK + && !any_exact_active + && global_attribution_ready + && global_matches_context + && snapshot.task_status == 6) { + terminal_error = AgvErrorCode::TaskCanceled; + terminal_reason = + "SEER Robokit controller reported navigation cancellation: " + + last_detail; + } + if (terminal_error != AgvErrorCode::OK + && (any_exact_active + || attributed_matching_global_active)) { + return failAndCancelTrackedNavigation_( + context, + options, + terminal_error, + terminal_reason + + "; another segment or the global navigation task is " + "still active and must be canceled before returning; " + + last_detail); + } + if (snapshot.blocked && navigationStopped(snapshot)) { + ++blocked_stopped_samples; + } else { + blocked_stopped_samples = 0; + } + if (blocked_stopped_samples >= kRequiredBlockedStopSamples) { + return failAndCancelTrackedNavigation_( + context, + options, + AgvErrorCode::TaskFailed, + "SEER Robokit navigation remained blocked while stopped for " + + std::to_string(blocked_stopped_samples) + + " consecutive 1101 samples: " + last_detail); + } + + bool terminal_candidate = terminal_error != AgvErrorCode::OK; + if (global_attribution_ready + && final_completion_observed + && !any_exact_active) { + const bool expected_completed_snapshot = + snapshot.task_status == 0 + || (snapshot.task_status == 4 + && snapshot.task_type == expected_global_type + && (context.target_id.empty() + || snapshot.target_id.empty() + || snapshot.target_id == context.target_id)); + if (expected_completed_snapshot) { + terminal_candidate = true; + } + } + if (terminal_candidate && navigationStopped(snapshot)) { + ++terminal_stopped_samples; + } else { + terminal_stopped_samples = 0; + } + if (terminal_stopped_samples >= kRequiredCompletedStopSamples) { + clearTrackedNavigationIfToken_(context.token); + if (terminal_error != AgvErrorCode::OK) { + return AgvResult::failure( + terminal_error, + terminal_reason + "; stopped velocity confirmed; " + + last_detail); + } + return AgvResult::success(); + } + + sleepForNavigationPoll(poll_interval, deadline, options); + } + + TrackedNavigationContext active_context; + if (!currentTrackedNavigation_(active_context) + || active_context.token != context.token) { + return AgvResult::failure( + AgvErrorCode::CommandFailed, + "SEER Robokit superseded navigation did not expose an exact terminal " + "state before the wait timeout; the newer task was not canceled; " + "last_status=" + last_detail); + } + if (terminal_error != AgvErrorCode::OK + || final_completion_observed) { + // A terminal task status is not proof that a differential chassis has + // stopped. Keep ownership and enter the same bounded cancellation / + // stop-confirmation path used by all other unsafe exits. If stopped + // state still cannot be established, tracking remains published so a + // later explicit cancel can use the correct 3003/3067 command. + return failAndCancelTrackedNavigation_( + context, + options, + terminal_error != AgvErrorCode::OK + ? terminal_error + : AgvErrorCode::Timeout, + "SEER Robokit navigation reached a terminal task state, but stopped " + "velocity could not be confirmed before timeout; last_status=" + + last_detail); + } + return failAndCancelTrackedNavigation_( + context, + options, + AgvErrorCode::Timeout, + "SEER Robokit synchronous navigation did not reach a terminal stopped " + "state within " + std::to_string(navigationWaitTimeout(options).count()) + + " ms; last_status=" + last_detail); +} + +AgvResult SeerRobokitAgv::waitForPoseNavigationTerminal_( + const PoseTaskContext& pose_context, + const TrackedNavigationContext& navigation_context, + const AgvMotionOptions& options) +{ + const auto poll_interval = navigationPollInterval(options); + const auto deadline = std::chrono::steady_clock::now() + + navigationWaitTimeout(options); + const auto accepted_at = navigation_context.accepted_at + == std::chrono::steady_clock::time_point{} + ? std::chrono::steady_clock::now() + : navigation_context.accepted_at; + const auto global_attribution_deadline = + accepted_at + kPoseNavigationStartTimeout; + bool completion_observed = false; + AgvErrorCode terminal_error = AgvErrorCode::OK; + std::string terminal_reason; + int blocked_stopped_samples = 0; + int terminal_stopped_samples = 0; + std::string last_detail = "free-navigation task start was confirmed"; + bool superseded_wait_active = false; + std::chrono::steady_clock::time_point superseded_deadline; + + while (std::chrono::steady_clock::now() < deadline) { + TrackedNavigationContext active_context; + const bool still_current = currentTrackedNavigation_(active_context) + && active_context.token == navigation_context.token; + if (navigationCancellationRequested(options)) { + if (!still_current) { + return AgvResult::failure( + AgvErrorCode::TaskCanceled, + "SEER Robokit synchronous free-navigation wait was canceled " + "after its task had been replaced; the newer task was not " + "canceled"); + } + return failAndCancelTrackedNavigation_( + navigation_context, + options, + AgvErrorCode::TaskCanceled, + "SEER Robokit synchronous free-navigation wait was canceled by " + "the caller"); + } + + PoseTaskStatus exact_status; + const auto exact_result = queryPoseTaskStatus_( + pose_context.task_id, + exact_status); + if (!exact_result.ok()) { + if (!still_current) { + return AgvResult::failure( + AgvErrorCode::CommandFailed, + "SEER Robokit superseded free-navigation exact task query " + "failed; the newer task was not canceled: " + + exact_result.message); + } + return failAndCancelTrackedNavigation_( + navigation_context, + options, + exact_result.code, + "SEER Robokit synchronous free-navigation exact task query " + "failed: " + exact_result.message); + } + last_detail = exact_status.detail; + if (!exact_status.found || exact_status.state == 404) { + if (!still_current) { + return AgvResult::failure( + AgvErrorCode::CommandFailed, + "SEER Robokit free-navigation task was superseded and its " + "exact status disappeared; the newer task was not " + "canceled: " + last_detail); + } + return failAndCancelTrackedNavigation_( + navigation_context, + options, + AgvErrorCode::CommandFailed, + "SEER Robokit exact free-navigation task disappeared after its " + "start was confirmed: " + last_detail); + } + if (exact_status.type_present && exact_status.type != 1) { + if (!still_current) { + return AgvResult::failure( + AgvErrorCode::CommandFailed, + "SEER Robokit superseded free-navigation task id reported an " + "unexpected type; the newer task was not canceled: " + + last_detail); + } + return failAndCancelTrackedNavigation_( + navigation_context, + options, + AgvErrorCode::TaskRejected, + "SEER Robokit exact free-navigation task reported an unexpected " + "type: " + last_detail); + } + if (exact_status.state == 4 && !completion_observed + && terminal_error == AgvErrorCode::OK) { + // A Completed task can still be decelerating. Defer pose + // acceptance until two stopped 1101 samples have been observed; + // an eager 1004 check here can reject a task that settles inside + // tolerance or accept one that later drifts outside it. + completion_observed = true; + } else if ((exact_status.state == 5 || exact_status.state == 7) + && terminal_error == AgvErrorCode::OK) { + terminal_error = AgvErrorCode::TaskFailed; + terminal_reason = + "SEER Robokit free-navigation task failed: " + last_detail; + } else if (exact_status.state == 6 + && terminal_error == AgvErrorCode::OK) { + terminal_error = AgvErrorCode::TaskCanceled; + terminal_reason = + "SEER Robokit free-navigation task was canceled: " + last_detail; + } else if (exact_status.state < 1 || exact_status.state > 7) { + if (!still_current) { + return AgvResult::failure( + AgvErrorCode::CommandFailed, + "SEER Robokit superseded free-navigation task reported an " + "unsupported state; the newer task was not canceled: " + + last_detail); + } + return failAndCancelTrackedNavigation_( + navigation_context, + options, + AgvErrorCode::TaskFailed, + "SEER Robokit exact free-navigation task reported an unsupported " + "state: " + last_detail); + } + + // Re-read the token after the exact 1110 query. A concurrent + // pose command may have replaced the active context while those I/O + // operations were in flight; global 1101 must never be attributed to + // the older waiter in that case. + TrackedNavigationContext post_query_context; + const bool still_current_after_query = + currentTrackedNavigation_(post_query_context) + && post_query_context.token == navigation_context.token; + if (!still_current_after_query) { + if (terminal_error != AgvErrorCode::OK) { + clearPoseTaskIfTaskId_(pose_context.task_id); + return AgvResult::failure(terminal_error, terminal_reason); + } + if (completion_observed) { + clearPoseTaskIfTaskId_(pose_context.task_id); + return AgvResult::failure( + AgvErrorCode::CommandFailed, + "SEER Robokit free-navigation task reported Completed, but a " + "newer local task replaced it before final stopped-pose " + "verification; the newer task was not canceled"); + } + if (!superseded_wait_active) { + superseded_wait_active = true; + superseded_deadline = std::chrono::steady_clock::now() + + kNavigationCancelConfirmationTimeout; + } + if (std::chrono::steady_clock::now() >= superseded_deadline) { + return AgvResult::failure( + AgvErrorCode::CommandFailed, + "SEER Robokit superseded free-navigation task remained " + "non-terminal after the replacement grace window; the " + "newer task was not inspected or canceled; " + last_detail); + } + sleepForNavigationPoll( + poll_interval, + std::min(deadline, superseded_deadline), + options); + continue; + } + + NavigationSnapshot snapshot; + const auto snapshot_result = queryNavigationSnapshot_(snapshot); + if (!snapshot_result.ok()) { + if (terminal_error != AgvErrorCode::OK || completion_observed) { + last_detail += ", 1101 detail unavailable: " + + snapshot_result.message; + sleepForNavigationPoll(poll_interval, deadline, options); + continue; + } + return failAndCancelTrackedNavigation_( + navigation_context, + options, + snapshot_result.code, + "SEER Robokit synchronous free-navigation safety snapshot " + "failed: " + snapshot_result.message); + } + last_detail += ", " + snapshot.detail; + + TrackedNavigationContext post_snapshot_context; + if (!currentTrackedNavigation_(post_snapshot_context) + || post_snapshot_context.token != navigation_context.token) { + // The snapshot may describe the replacement task; discard it. + if (terminal_error != AgvErrorCode::OK) { + clearPoseTaskIfTaskId_(pose_context.task_id); + return AgvResult::failure(terminal_error, terminal_reason); + } + if (completion_observed) { + clearPoseTaskIfTaskId_(pose_context.task_id); + return AgvResult::failure( + AgvErrorCode::CommandFailed, + "SEER Robokit free-navigation task reported Completed, but the " + "1101 snapshot was superseded before final stopped-pose " + "verification; the newer task was not canceled"); + } + if (!superseded_wait_active) { + superseded_wait_active = true; + superseded_deadline = std::chrono::steady_clock::now() + + kNavigationCancelConfirmationTimeout; + } + if (std::chrono::steady_clock::now() >= superseded_deadline) { + return AgvResult::failure( + AgvErrorCode::CommandFailed, + "SEER Robokit superseded free navigation did not expose an " + "exact terminal state during the replacement grace " + "window; the newer task was not inspected or canceled; " + + last_detail); + } + sleepForNavigationPoll( + poll_interval, + std::min(deadline, superseded_deadline), + options); + continue; + } + + const bool global_active = exactTaskStateIsActive( + snapshot.task_status); + const bool global_attribution_ready = + std::chrono::steady_clock::now() + >= global_attribution_deadline; + const bool attributed_global_active = global_attribution_ready + && global_active; + const bool attributed_matching_global_active = + attributed_global_active && snapshot.task_type == 1; + + if (snapshot.emergency || !snapshot.active_faults.empty()) { + return failAndCancelTrackedNavigation_( + navigation_context, + options, + snapshot.emergency + ? AgvErrorCode::EmergencyStopped + : AgvErrorCode::Fault, + "SEER Robokit controller reported a free-navigation safety " + "fault: " + last_detail); + } + + if (terminal_error == AgvErrorCode::OK + && !exactTaskStateIsActive(exact_status.state) + && global_attribution_ready + && snapshot.task_type == 1 + && (snapshot.task_status == 5 + || snapshot.task_status == 7)) { + terminal_error = AgvErrorCode::TaskFailed; + terminal_reason = + "SEER Robokit controller reported free-navigation failure: " + + last_detail; + } else if (terminal_error == AgvErrorCode::OK + && !exactTaskStateIsActive(exact_status.state) + && global_attribution_ready + && snapshot.task_type == 1 + && snapshot.task_status == 6) { + terminal_error = AgvErrorCode::TaskCanceled; + terminal_reason = + "SEER Robokit controller reported free-navigation cancellation: " + + last_detail; + } + if (terminal_error != AgvErrorCode::OK + && (exactTaskStateIsActive(exact_status.state) + || attributed_matching_global_active)) { + return failAndCancelTrackedNavigation_( + navigation_context, + options, + terminal_error, + terminal_reason + + "; the exact or global free-navigation task is still " + "active and must be canceled before returning; " + + last_detail); + } + + if (snapshot.blocked && navigationStopped(snapshot)) { + ++blocked_stopped_samples; + } else { + blocked_stopped_samples = 0; + } + if (blocked_stopped_samples >= kRequiredBlockedStopSamples) { + return failAndCancelTrackedNavigation_( + navigation_context, + options, + AgvErrorCode::TaskFailed, + "SEER Robokit free navigation remained blocked while stopped for " + + std::to_string(blocked_stopped_samples) + + " consecutive 1101 samples: " + last_detail); + } + + const bool expected_completed_snapshot = global_attribution_ready + && completion_observed + && exact_status.state == 4 + && (snapshot.task_status == 0 + || (snapshot.task_status == 4 + && snapshot.task_type == 1)); + if ((terminal_error != AgvErrorCode::OK + || expected_completed_snapshot) + && navigationStopped(snapshot)) { + ++terminal_stopped_samples; + } else { + terminal_stopped_samples = 0; + } + if (terminal_stopped_samples >= kRequiredCompletedStopSamples) { + if (terminal_error != AgvErrorCode::OK) { + clearPoseTaskIfTaskId_(pose_context.task_id); + clearTrackedNavigationIfToken_(navigation_context.token); + return AgvResult::failure( + terminal_error, + terminal_reason + "; stopped velocity confirmed; " + + last_detail); + } + + std::string final_pose_detail; + const bool final_pose_reached = + poseTargetReached_(pose_context, final_pose_detail); + TrackedNavigationContext post_pose_context; + if (!currentTrackedNavigation_(post_pose_context) + || post_pose_context.token != navigation_context.token) { + return AgvResult::failure( + AgvErrorCode::CommandFailed, + "SEER Robokit free-navigation completion was superseded during " + "the final stopped-pose verification; the newer task was " + "not canceled; " + final_pose_detail); + } + clearPoseTaskIfTaskId_(pose_context.task_id); + clearTrackedNavigationIfToken_(navigation_context.token); + if (!final_pose_reached) { + return AgvResult::failure( + AgvErrorCode::TaskRejected, + "SEER Robokit free-navigation task stopped after reporting " + "Completed, but the final pose was outside the requested " + "tolerance: " + final_pose_detail); + } + return AgvResult::success(); + } + + sleepForNavigationPoll(poll_interval, deadline, options); + } + + TrackedNavigationContext active_context; + if (!currentTrackedNavigation_(active_context) + || active_context.token != navigation_context.token) { + return AgvResult::failure( + AgvErrorCode::CommandFailed, + "SEER Robokit superseded free navigation did not expose an exact " + "terminal state before timeout; the newer task was not canceled; " + "last_status=" + last_detail); + } + if (terminal_error != AgvErrorCode::OK || completion_observed) { + return failAndCancelTrackedNavigation_( + navigation_context, + options, + terminal_error != AgvErrorCode::OK + ? terminal_error + : AgvErrorCode::Timeout, + "SEER Robokit free-navigation task reached a terminal state, but " + "stopped velocity could not be confirmed before timeout; " + "last_status=" + last_detail); + } + return failAndCancelTrackedNavigation_( + navigation_context, + options, + AgvErrorCode::Timeout, + "SEER Robokit synchronous free navigation did not reach its verified " + "target and stop within " + + std::to_string(navigationWaitTimeout(options).count()) + + " ms; last_status=" + last_detail); +} + +} // namespace cmvr::device diff --git a/cmvr-es/devices/agv/seer_robokit/src/seer_robokit_status.cpp b/cmvr-es/devices/agv/seer_robokit/src/seer_robokit_status.cpp new file mode 100644 index 00000000..475d4648 --- /dev/null +++ b/cmvr-es/devices/agv/seer_robokit/src/seer_robokit_status.cpp @@ -0,0 +1,725 @@ +#include "seer_robokit_agv.h" +#include "seer_robokit_protocol.h" +#include "seer_robokit_utils.h" + +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +namespace cmvr::device { + +using namespace seer_robokit::protocol; +using namespace seer_robokit::detail; + +namespace { + +bool hasFaultArray(const Json::Value& value, const char* key) +{ + const auto* found = jsonFind(value, key); + return found && found->isArray() && !found->empty(); +} + +void appendStringArray(Json::Value& value, const char* key, const google::protobuf::RepeatedPtrField& strings) +{ + if (strings.empty()) { + return; + } + Json::Value array(Json::arrayValue); + for (const auto& item : strings) { + array.append(item); + } + jsonMember(value, key) = array; +} + +AgvMode modeFromTaskState(const int state) +{ + switch (state) { + case 2: + return AgvMode::Auto; + case 3: + return AgvMode::Paused; + case 5: + return AgvMode::Fault; + case 6: + return AgvMode::Stopped; + default: + return AgvMode::Idle; + } +} + +AgvTaskState toTaskState(const int value) +{ + switch (value) { + case 1: + return AgvTaskState::Waiting; + case 2: + return AgvTaskState::Running; + case 3: + return AgvTaskState::Paused; + case 4: + return AgvTaskState::Completed; + case 5: + case 7: + return AgvTaskState::Failed; + case 6: + return AgvTaskState::Canceled; + case 0: + default: + return AgvTaskState::None; + } +} + +AgvTaskType toTaskType(const int value) +{ + switch (value) { + case 1: + return AgvTaskType::NavigateToPose; + case 2: + return AgvTaskType::NavigateToStation; + case 3: + return AgvTaskType::FollowPath; + case 100: + return AgvTaskType::Custom; + default: + return AgvTaskType::None; + } +} + +} // namespace + +AgvRuntimeState SeerRobokitAgv::runtimeState() const +{ + AgvRuntimeState cached_state; + bool has_cached_state = false; + if (state_push_enabled_) { + std::lock_guard lock(runtime_state_mutex_); + if (cached_runtime_state_valid_) { + cached_state = cached_runtime_state_; + has_cached_state = true; + } + } + + if (has_cached_state) { + std::string adapter_error; + { + std::lock_guard lock(mutex_); + cached_state.connected = connected_(); + adapter_error = last_error_; + } + if (!adapter_error.empty()) { + if (cached_state.last_error.empty()) { + cached_state.last_error = adapter_error; + } else if (cached_state.last_error != adapter_error) { + cached_state.last_error += "; adapter_error=" + adapter_error; + } + } + if (!cached_state.connected) { + cached_state.mode = AgvMode::Disconnected; + } + return cached_state; + } + + return queryRuntimeState_(); +} + +AgvRuntimeState SeerRobokitAgv::queryRuntimeState_() const +{ + AgvRuntimeState state; + { + std::lock_guard lock(mutex_); + state.connected = connected_(); + state.last_error = last_error_; + } + state.mode = state.connected ? AgvMode::Idle : AgvMode::Disconnected; + + Json::Value loc; + if (sendCommand_(sock_status_, kRobotStatusLoc, Json::Value(Json::objectValue), &loc).ok()) { + state.pose.x = jsonGet(loc, "x", 0.0).asDouble(); + state.pose.y = jsonGet(loc, "y", 0.0).asDouble(); + state.pose.theta = jsonGet(loc, "angle", 0.0).asDouble(); + state.localized = jsonGet(loc, "confidence", 0.0).asDouble() > 0.0; + state.current_station = jsonGet(loc, "current_station", "").asString(); + } + + Json::Value battery; + if (sendCommand_(sock_status_, kRobotStatusBattery, Json::Value(Json::objectValue), &battery).ok()) { + state.battery.percentage = jsonGet(battery, "battery_level", 0.0).asDouble(); + state.battery.temperature = jsonGet(battery, "battery_temp", 0.0).asDouble(); + state.battery.charging = jsonGet(battery, "charging", false).asBool(); + state.battery.voltage = jsonGet(battery, "voltage", 0.0).asDouble(); + state.battery.current = jsonGet(battery, "current", 0.0).asDouble(); + } + + Json::Value map; + if (sendCommand_(sock_status_, kRobotStatusMap, Json::Value(Json::objectValue), &map).ok()) { + state.current_map = jsonGet(map, "current_map", "").asString(); + } + + const auto nav = navigationStatus(); + state.moving = nav.state == AgvTaskState::Running; + state.fault = nav.state == AgvTaskState::Failed; + state.mode = state.fault ? AgvMode::Fault : modeFromTaskState(static_cast(nav.state)); + return state; +} + +AgvNavigationStatus SeerRobokitAgv::navigationStatus() const +{ + AgvNavigationStatus status; + std::string missing_pose_task_detail; + for (int attempt = 0; attempt < 2; ++attempt) { + PoseTaskContext pose_context; + if (!currentPoseTask_(pose_context)) { + break; + } + const auto observed_navigation_generation = + navigation_generation_.load(std::memory_order_relaxed); + if (pose_context.navigation_generation + != observed_navigation_generation) { + continue; + } + + PoseTaskStatus task_status; + const auto result = queryPoseTaskStatus_(pose_context.task_id, task_status); + PoseTaskContext latest_context; + if (navigation_generation_.load(std::memory_order_relaxed) + != observed_navigation_generation + || !currentPoseTask_(latest_context) + || latest_context.navigation_generation + != pose_context.navigation_generation + || latest_context.task_id != pose_context.task_id) { + continue; + } + + status.type = AgvTaskType::NavigateToPose; + const auto fault_monitoring_unavailable = + [this, &pose_context]() { + if (controller_fault_channel_epoch_.load( + std::memory_order_relaxed) + != pose_context + .controller_fault_channel_epoch_at_start) { + return std::string( + "the controller fault push channel changed or was " + "invalidated after the free-navigation command was " + "accepted"); + } + return freeNavigationFaultStateUnavailableDetail_(); + }; + if (!result.ok()) { + status.state = AgvTaskState::Failed; + status.message = result.message; + return status; + } + if (!task_status.found || task_status.state == 404) { + std::uint64_t missing_task_fault_control_attempt = 0; + const std::string missing_task_fault = + cachedControllerFaultDetail_( + pose_context.controller_fault_sequence_at_start, + controllerFaultCaptureGraceMs_(), + &missing_task_fault_control_attempt); + PoseTaskContext post_missing_context; + if (navigation_generation_.load(std::memory_order_relaxed) + != observed_navigation_generation + || !currentPoseTask_(post_missing_context) + || post_missing_context.navigation_generation + != pose_context.navigation_generation + || post_missing_context.task_id != pose_context.task_id) { + continue; + } + if (!missing_task_fault.empty()) { + std::string attribution; + if (missing_task_fault_control_attempt != 0 + && missing_task_fault_control_attempt + != pose_context.control_attempt_sequence_at_start) { + attribution = + "controller_fault_attribution=ambiguous because the " + "fault was observed after another control command " + "attempt had begun, "; + } + status.state = AgvTaskState::Failed; + status.message = + "SEER Robokit tracked free-navigation task disappeared from " + "1110 task_status_package while a new controller fault " + "was observed: " + task_status.detail + ", " + + attribution + missing_task_fault; + clearPoseTask_(pose_context.navigation_generation); + return status; + } + if (const std::string unavailable = + fault_monitoring_unavailable(); + !unavailable.empty()) { + status.state = AgvTaskState::Failed; + status.message = + "SEER Robokit tracked free-navigation status is unsafe to " + "accept because controller fault monitoring is " + "unavailable: " + unavailable + + "; query the controller and cancel or stop before " + "another motion command"; + return status; + } + missing_pose_task_detail = task_status.detail; + clearPoseTask_(pose_context.navigation_generation); + break; + } + if (task_status.type_present && task_status.type != 1) { + status.state = AgvTaskState::Failed; + status.type = toTaskType(task_status.type); + status.message = + "SEER Robokit returned an unexpected task type for the tracked " + "free-navigation task: " + task_status.detail; + clearPoseTask_(pose_context.navigation_generation); + return status; + } + status.state = toTaskState(task_status.state); + status.progress = task_status.progress; + status.message = task_status.detail; + const auto controller_reported_state = status.state; + const bool controller_state_terminal = + controller_reported_state == AgvTaskState::Completed + || controller_reported_state == AgvTaskState::Failed + || controller_reported_state == AgvTaskState::Canceled; + std::uint64_t fault_control_attempt = 0; + const std::string fault = cachedControllerFaultDetail_( + pose_context.controller_fault_sequence_at_start, + controller_reported_state == AgvTaskState::Completed + || controller_reported_state == AgvTaskState::Failed + ? controllerFaultCaptureGraceMs_() + : 0, + &fault_control_attempt); + PoseTaskContext post_fault_context; + if (navigation_generation_.load(std::memory_order_relaxed) + != observed_navigation_generation + || !currentPoseTask_(post_fault_context) + || post_fault_context.navigation_generation + != pose_context.navigation_generation + || post_fault_context.task_id != pose_context.task_id) { + continue; + } + const std::string unavailable = + fault_monitoring_unavailable(); + if (!fault.empty()) { + std::string attribution; + if (fault_control_attempt != 0 + && fault_control_attempt + != pose_context.control_attempt_sequence_at_start) { + attribution = + "controller_fault_attribution=ambiguous because the " + "fault was observed after another control command " + "attempt had begun, "; + } + status.state = AgvTaskState::Failed; + status.message = + "SEER Robokit reported a new controller fault while the tracked " + "free-navigation task had controller_task_state=" + + std::to_string(task_status.state) + ": " + + task_status.detail + ", " + attribution + fault; + if (!unavailable.empty()) { + status.message += + ", controller_fault_monitoring_unavailable=" + + unavailable; + } + } else if (!unavailable.empty()) { + if (controller_reported_state == AgvTaskState::Failed + || controller_reported_state == AgvTaskState::Canceled) { + status.message += + ", controller_fault_monitoring_unavailable=" + + unavailable; + } else { + status.state = AgvTaskState::Failed; + status.message = + "SEER Robokit tracked free-navigation state is unsafe to accept " + "because controller fault monitoring became unavailable: " + + unavailable + + "; query the controller and cancel or stop before " + "another motion command"; + return status; + } + } + if (status.state == AgvTaskState::Completed) { + std::string pose_detail; + const bool target_reached = + poseTargetReached_(pose_context, pose_detail); + PoseTaskContext post_pose_context; + if (navigation_generation_.load(std::memory_order_relaxed) + != observed_navigation_generation + || !currentPoseTask_(post_pose_context) + || post_pose_context.navigation_generation + != pose_context.navigation_generation + || post_pose_context.task_id != pose_context.task_id) { + continue; + } + std::uint64_t post_pose_fault_control_attempt = 0; + const std::string post_pose_fault = + cachedControllerFaultDetail_( + pose_context.controller_fault_sequence_at_start, + 0, + &post_pose_fault_control_attempt); + if (!post_pose_fault.empty()) { + status.state = AgvTaskState::Failed; + std::string attribution; + if (post_pose_fault_control_attempt != 0 + && post_pose_fault_control_attempt + != pose_context + .control_attempt_sequence_at_start) { + attribution = + "controller_fault_attribution=ambiguous because the " + "fault was observed after another control command " + "attempt had begun, "; + } + status.message = + "SEER Robokit reported the tracked free-navigation task " + "Completed, but a new controller fault was observed during " + "target verification: " + task_status.detail + ", " + + attribution + post_pose_fault; + } else if (const std::string post_pose_unavailable = + fault_monitoring_unavailable(); + !post_pose_unavailable.empty()) { + status.state = AgvTaskState::Failed; + status.message = + "SEER Robokit tracked free-navigation completion is unsafe to " + "accept because controller fault monitoring became " + "unavailable: " + post_pose_unavailable + + "; query the controller and cancel or stop before " + "another motion command"; + return status; + } else if (!target_reached) { + status.state = AgvTaskState::Failed; + status.message = + "SEER Robokit reported the tracked free-navigation task " + "Completed, but the requested target was not reached: " + + task_status.detail + ", " + pose_detail; + } else { + status.message += ", target_verified: " + pose_detail; + } + } + if (controller_state_terminal) { + clearPoseTask_(pose_context.navigation_generation); + } + return status; + } + + PoseTaskContext changed_context; + if (currentPoseTask_(changed_context)) { + status.state = AgvTaskState::Waiting; + status.type = AgvTaskType::NavigateToPose; + status.message = + "SEER Robokit free-navigation task changed while its status was being " + "queried; query navigation status again"; + return status; + } + + Json::Value payload(Json::objectValue); + jsonMember(payload, "simple") = false; + + Json::Value response; + const auto result = sendCommand_(sock_status_, kRobotStatusTask, payload, &response); + if (!result.ok()) { + status.state = AgvTaskState::Failed; + status.message = missing_pose_task_detail.empty() + ? result.message + : missing_pose_task_detail + "; 1020 status query failed: " + + result.message; + return status; + } + const auto controller_result = resultFromResponse_(response); + if (!controller_result.ok()) { + status.state = AgvTaskState::Failed; + status.message = missing_pose_task_detail.empty() + ? controller_result.message + : missing_pose_task_detail + "; 1020 status query failed: " + + controller_result.message; + return status; + } + + status.state = toTaskState(jsonGet(response, "task_status", 0).asInt()); + status.type = toTaskType(jsonGet(response, "task_type", 0).asInt()); + status.message = jsonGet(response, "move_status_info", jsonGet(response, "err_msg", "")).asString(); + if (!missing_pose_task_detail.empty()) { + status.message = missing_pose_task_detail + + "; fallback_1020_status=" + std::to_string( + jsonGet(response, "task_status", 0).asInt()) + + ", fallback_1020_type=" + std::to_string( + jsonGet(response, "task_type", 0).asInt()) + + (status.message.empty() ? std::string{} : ", " + status.message); + } + if (const auto* task_status_package = jsonFind(response, "task_status_package")) { + status.progress = jsonGet(*task_status_package, "percentage", 0.0).asDouble(); + } + return status; +} + +AgvResult SeerRobokitAgv::configurePush_() +{ + if (config_.state_push_included_fields_size() > 0 && config_.state_push_excluded_fields_size() > 0) { + return AgvResult::failure( + AgvErrorCode::InvalidArgument, + "SEER Robokit push included_fields and excluded_fields cannot both be set"); + } + + Json::Value payload(Json::objectValue); + if (config_.state_push_interval_ms() > 0) { + jsonMember(payload, "interval") = config_.state_push_interval_ms(); + } + appendStringArray(payload, "included_fields", config_.state_push_included_fields()); + appendStringArray(payload, "excluded_fields", config_.state_push_excluded_fields()); + + if (payload.empty()) { + return AgvResult::success(); + } + + const std::string payload_text = toJsonString_(payload); + const auto frame = buildFrame_(kRobotPushConfigReq, payload_text); + + std::lock_guard lock(mutex_); + if (sock_push_ < 0) { + return AgvResult::failure(AgvErrorCode::NotConnected, "SEER Robokit push socket not connected"); + } + if (::send(sock_push_, frame.data(), frame.size(), MSG_NOSIGNAL) != static_cast(frame.size())) { + return AgvResult::failure(AgvErrorCode::CommandFailed, "SEER Robokit send push config failed: " + systemError()); + } + + while (true) { + std::uint16_t command = 0; + std::string response_payload; + const auto result = receiveFrame_(sock_push_, command, response_payload); + if (!result.ok()) { + return result; + } + + Json::Value response; + std::string error; + if (!response_payload.empty() && !parseJson_(response_payload, response, error)) { + return AgvResult::failure(AgvErrorCode::CommandFailed, error); + } + + if (command == kRobotPushConfigRes) { + return resultFromResponse_(response); + } + if (command == kRobotPush && response.isObject()) { + updateCachedRuntimeState_(response); + } + } +} + +void SeerRobokitAgv::startPushThread_() +{ + if (!state_push_enabled_) { + return; + } + if (push_running_.exchange(true)) { + return; + } + if (sock_push_ < 0) { + push_running_ = false; + return; + } + push_thread_ = std::thread(&SeerRobokitAgv::pushLoop_, this); +} + +void SeerRobokitAgv::stopPushThread_() +{ + const bool was_running = push_running_.exchange(false); + if (was_running) { + int sock = -1; + { + std::lock_guard lock(mutex_); + sock = sock_push_; + } + if (sock >= 0) { + ::shutdown(sock, SHUT_RDWR); + } + } + if (push_thread_.joinable()) { + push_thread_.join(); + } + invalidateControllerFaultState_(); +} + +void SeerRobokitAgv::pushLoop_() +{ + while (push_running_) { + int sock = -1; + { + std::lock_guard lock(mutex_); + sock = sock_push_; + } + if (sock < 0) { + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + continue; + } + + std::uint16_t command = 0; + std::string payload; + const auto result = receiveFrame_(sock, command, payload); + if (!push_running_) { + break; + } + if (!result.ok()) { + if (result.code != AgvErrorCode::Timeout) { + invalidateControllerFaultState_(); + std::lock_guard lock(mutex_); + last_error_ = result.message; + closeSocket_(sock_push_); + } + continue; + } + if (command != kRobotPush || payload.empty()) { + continue; + } + + Json::Value parsed; + std::string error; + if (!parseJson_(payload, parsed, error)) { + invalidateControllerFaultState_(); + std::lock_guard lock(mutex_); + last_error_ = error; + continue; + } + updateCachedRuntimeState_(parsed); + } +} + +void SeerRobokitAgv::invalidateControllerFaultState_() +{ + std::lock_guard lock(runtime_state_mutex_); + controller_fault_channel_epoch_.fetch_add( + 1, + std::memory_order_relaxed); + controller_fault_state_observed_ = false; + controller_fault_state_observed_at_ = {}; + active_controller_fault_detail_.clear(); + runtime_state_cv_.notify_all(); +} + +void SeerRobokitAgv::updateCachedRuntimeState_(const Json::Value& payload) +{ + std::lock_guard lock(runtime_state_mutex_); + auto state = cached_runtime_state_valid_ ? cached_runtime_state_ : AgvRuntimeState{}; + state.timestamp = nowSeconds(); + state.connected = true; + + if (jsonHas(payload, "x")) state.pose.x = jsonGet(payload, "x", state.pose.x).asDouble(); + if (jsonHas(payload, "y")) state.pose.y = jsonGet(payload, "y", state.pose.y).asDouble(); + if (jsonHas(payload, "angle")) state.pose.theta = jsonGet(payload, "angle", state.pose.theta).asDouble(); + if (jsonHas(payload, "vx")) state.velocity.vx = jsonGet(payload, "vx", state.velocity.vx).asDouble(); + if (jsonHas(payload, "vy")) state.velocity.vy = jsonGet(payload, "vy", state.velocity.vy).asDouble(); + if (jsonHas(payload, "w")) state.velocity.wz = jsonGet(payload, "w", state.velocity.wz).asDouble(); + if (jsonHas(payload, "battery_level")) { + state.battery.percentage = jsonGet(payload, "battery_level", state.battery.percentage).asDouble(); + } + if (jsonHas(payload, "battery_temp")) { + state.battery.temperature = jsonGet(payload, "battery_temp", state.battery.temperature).asDouble(); + } + if (jsonHas(payload, "charging")) { + state.battery.charging = jsonGet(payload, "charging", state.battery.charging).asBool(); + } + if (jsonHas(payload, "voltage")) { + state.battery.voltage = jsonGet(payload, "voltage", state.battery.voltage).asDouble(); + } + if (jsonHas(payload, "current")) { + state.battery.current = jsonGet(payload, "current", state.battery.current).asDouble(); + } + if (jsonHas(payload, "current_map")) { + state.current_map = jsonGet(payload, "current_map", state.current_map).asString(); + } + if (jsonHas(payload, "current_station")) { + state.current_station = jsonGet(payload, "current_station", state.current_station).asString(); + } + if (jsonHas(payload, "confidence")) { + state.localized = jsonGet(payload, "confidence", 0.0).asDouble() > 0.0; + } + if (jsonHas(payload, "emergency")) { + state.emergency_stopped = jsonGet(payload, "emergency", state.emergency_stopped).asBool(); + } + + state.moving = std::hypot(state.velocity.vx, state.velocity.vy) > 1e-4 || std::abs(state.velocity.wz) > 1e-4; + const bool has_fatals = jsonHas(payload, "fatals"); + const bool has_errors = jsonHas(payload, "errors"); + const bool has_fault_fields = has_fatals || has_errors; + if (has_fault_fields) { + const auto* fatals = jsonFind(payload, "fatals"); + const auto* errors = jsonFind(payload, "errors"); + const bool valid_fatals = !has_fatals + || (fatals && fatals->isArray()); + const bool valid_errors = !has_errors + || (errors && errors->isArray()); + const bool complete_fault_state = + has_fatals && has_errors && valid_fatals && valid_errors; + if (complete_fault_state) { + controller_fault_state_observed_ = true; + controller_fault_state_observed_at_ = + std::chrono::steady_clock::now(); + } else { + controller_fault_state_observed_ = false; + controller_fault_state_observed_at_ = {}; + } + + const bool reported_fault = + hasFaultArray(payload, "fatals") + || hasFaultArray(payload, "errors"); + const bool invalid_or_incomplete_fault_state = + !complete_fault_state && !reported_fault; + state.fault = reported_fault + || invalid_or_incomplete_fault_state; + if (state.fault) { + std::ostringstream detail; + detail << (reported_fault + ? "SEER Robokit controller fault" + : "SEER Robokit controller fault state is incomplete or malformed"); + if (fatals + && (!fatals->isArray() + || !fatals->empty() + || !complete_fault_state)) { + detail << ": fatals=" + << (fatals->isNull() + ? std::string("null") + : jsonValueToString(*fatals)); + } + if (errors + && (!errors->isArray() + || !errors->empty() + || !complete_fault_state)) { + detail << ": errors=" + << (errors->isNull() + ? std::string("null") + : jsonValueToString(*errors)); + } + state.last_error = detail.str(); + if (state.last_error != active_controller_fault_detail_) { + active_controller_fault_detail_ = state.last_error; + ++controller_fault_sequence_; + last_controller_fault_timestamp_ = state.timestamp; + last_controller_fault_detail_ = state.last_error; + last_controller_fault_control_attempt_ = + control_attempt_sequence_.load( + std::memory_order_acquire); + } + } else { + state.last_error.clear(); + active_controller_fault_detail_.clear(); + } + } + if (state.emergency_stopped) { + state.mode = AgvMode::EmergencyStop; + } else if (state.fault) { + state.mode = AgvMode::Fault; + } else if (state.battery.charging) { + state.mode = AgvMode::Charging; + } else if (state.moving) { + state.mode = AgvMode::Auto; + } else { + state.mode = AgvMode::Idle; + } + + cached_runtime_state_ = state; + cached_runtime_state_valid_ = true; + runtime_state_cv_.notify_all(); +} + +} // namespace cmvr::device diff --git a/cmvr-es/devices/agv/seer_robokit/src/seer_robokit_transport.cpp b/cmvr-es/devices/agv/seer_robokit/src/seer_robokit_transport.cpp new file mode 100644 index 00000000..97b7a515 --- /dev/null +++ b/cmvr-es/devices/agv/seer_robokit/src/seer_robokit_transport.cpp @@ -0,0 +1,362 @@ +#include "seer_robokit_agv.h" +#include "seer_robokit_protocol.h" +#include "seer_robokit_utils.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace cmvr::device { + +using namespace seer_robokit::protocol; +using namespace seer_robokit::detail; + +AgvResult SeerRobokitAgv::connectSocket_(int& sock, const int port) +{ + sock = ::socket(AF_INET, SOCK_STREAM, 0); + if (sock < 0) { + last_error_ = "create socket failed: " + systemError(); + return AgvResult::failure(AgvErrorCode::ConnectionFailed, last_error_); + } + + sockaddr_in address{}; + address.sin_family = AF_INET; + address.sin_port = htons(static_cast(port)); + if (::inet_pton(AF_INET, ip_.c_str(), &address.sin_addr) <= 0) { + closeSocket_(sock); + last_error_ = "invalid SEER Robokit ip: " + ip_; + return AgvResult::failure(AgvErrorCode::InvalidArgument, last_error_); + } + + if (::connect(sock, reinterpret_cast(&address), sizeof(address)) < 0) { + closeSocket_(sock); + last_error_ = "connect SEER Robokit port " + std::to_string(port) + " failed: " + systemError(); + return AgvResult::failure(AgvErrorCode::ConnectionFailed, last_error_); + } + + timeval timeout{}; + timeout.tv_sec = recv_timeout_ms_ / 1000; + timeout.tv_usec = (recv_timeout_ms_ % 1000) * 1000; + ::setsockopt(sock, SOL_SOCKET, SO_RCVTIMEO, &timeout, sizeof(timeout)); + return AgvResult::success(); +} + +AgvResult SeerRobokitAgv::ensureOtherSocket_() +{ + std::lock_guard lock(mutex_); + if (sock_other_ >= 0) { + return AgvResult::success(); + } + return connectSocket_(sock_other_, ports_.other); +} + +void SeerRobokitAgv::closeSocket_(int& sock) const +{ + if (sock >= 0) { + ::close(sock); + sock = -1; + } +} + +bool SeerRobokitAgv::connected_() const +{ + return sock_status_ >= 0 && sock_control_ >= 0 && sock_navigation_ >= 0 && sock_config_ >= 0; +} + +AgvResult SeerRobokitAgv::sendCommand_( + const int sock, + const std::uint16_t command, + const Json::Value& payload, + Json::Value* response, + CommandTransmissionState* transmission_state) const +{ + std::string response_payload; + auto result = sendCommandRaw_( + sock, + command, + payload, + &response_payload, + transmission_state); + if (!result.ok()) { + return result; + } + if (!response) { + return AgvResult::success(); + } + + Json::Value parsed; + std::string error; + if (!parseJson_(response_payload, parsed, error)) { + const std::string json_text = extractJson_(response_payload); + if (json_text.empty() || !parseJson_(json_text, parsed, error)) { + return AgvResult::failure(AgvErrorCode::CommandFailed, error); + } + } + + *response = std::move(parsed); + return AgvResult::success(); +} + +AgvResult SeerRobokitAgv::sendCommandRaw_( + const int sock, + const std::uint16_t command, + const Json::Value& payload, + std::string* response_payload, + CommandTransmissionState* transmission_state) const +{ + if (transmission_state) { + *transmission_state = CommandTransmissionState::NotSent; + } + const auto exchange = [&]() { + const std::string payload_text = payload.empty() ? std::string{} : toJsonString_(payload); + const auto frame = buildFrame_(command, payload_text); + const auto sent = ::send( + sock, + frame.data(), + frame.size(), + MSG_NOSIGNAL); + if (sent > 0 && transmission_state) { + *transmission_state = CommandTransmissionState::PossiblySent; + } + if (sent != static_cast(frame.size())) { + return AgvResult::failure( + AgvErrorCode::CommandFailed, + "SEER Robokit send command failed: " + systemError()); + } + + std::uint16_t response_command = 0; + std::string payload_text_response; + const auto result = receiveFrame_(sock, response_command, payload_text_response); + if (!result.ok()) { + return result; + } + const auto expected_response_command = static_cast( + command + 10000U); + if (response_command != expected_response_command) { + return AgvResult::failure( + AgvErrorCode::CommandFailed, + "SEER Robokit response command mismatch: expected=" + + std::to_string(expected_response_command) + + ", actual=" + std::to_string(response_command)); + } + if (response_payload) { + *response_payload = std::move(payload_text_response); + } + return AgvResult::success(); + }; + const auto close_matching_socket_locked = [this, sock]() { + if (sock == sock_status_) { + closeSocket_(sock_status_); + } else if (sock == sock_control_) { + closeSocket_(sock_control_); + } else if (sock == sock_navigation_) { + closeSocket_(sock_navigation_); + } else if (sock == sock_config_) { + closeSocket_(sock_config_); + } else if (sock == sock_other_) { + closeSocket_(sock_other_); + } + }; + const auto mark_channel_desynchronized = [](AgvResult result) { + std::string detail = result.message.empty() + ? "unknown transport or frame error" + : result.message; + detail += + "; SEER Robokit channel closed because the response stream may be " + "desynchronized; reconnect before sending another command"; + return AgvResult::failure(result.code, detail); + }; + + bool is_status_socket = false; + { + std::lock_guard lock(mutex_); + if (sock < 0) { + return AgvResult::failure( + AgvErrorCode::NotConnected, + "SEER Robokit socket not connected"); + } + is_status_socket = sock == sock_status_; + } + + if (is_status_socket) { + // A slow 1110 status response must never hold the lifecycle/global I/O + // mutex needed by cancelNavigation() or emergencyStop(). The dedicated + // status lock still serializes requests on port 19204. connect_() and + // disconnect_() take this lock before changing the descriptor. + std::lock_guard status_lock(status_io_mutex_); + { + std::lock_guard lock(mutex_); + if (sock < 0 || sock != sock_status_) { + return AgvResult::failure( + AgvErrorCode::NotConnected, + "SEER Robokit status socket is no longer connected"); + } + } + auto result = exchange(); + if (!result.ok()) { + std::lock_guard lock(mutex_); + close_matching_socket_locked(); + return mark_channel_desynchronized(std::move(result)); + } + return result; + } + + std::lock_guard lock(mutex_); + if (sock < 0 + || (sock != sock_control_ + && sock != sock_navigation_ + && sock != sock_config_ + && sock != sock_other_)) { + return AgvResult::failure( + AgvErrorCode::NotConnected, + "SEER Robokit socket is no longer connected"); + } + auto result = exchange(); + if (!result.ok()) { + close_matching_socket_locked(); + return mark_channel_desynchronized(std::move(result)); + } + return result; +} + +AgvResult SeerRobokitAgv::sendCommandNoResponse_( + const int sock, + const std::uint16_t command, + const Json::Value& payload) const +{ + return sendCommand_(sock, command, payload, nullptr); +} + +std::vector SeerRobokitAgv::buildFrame_( + const std::uint16_t command, + const std::string& payload) +{ + std::vector frame(16 + payload.size(), 0); + frame[0] = 0x5A; + frame[1] = 0x01; + frame[2] = 0x00; + frame[3] = 0x01; + const auto length = static_cast(payload.size()); + frame[4] = static_cast((length >> 24U) & 0xFFU); + frame[5] = static_cast((length >> 16U) & 0xFFU); + frame[6] = static_cast((length >> 8U) & 0xFFU); + frame[7] = static_cast(length & 0xFFU); + frame[8] = static_cast((command >> 8U) & 0xFFU); + frame[9] = static_cast(command & 0xFFU); + std::copy(payload.begin(), payload.end(), frame.begin() + 16); + return frame; +} + +std::string SeerRobokitAgv::toJsonString_(const Json::Value& value) +{ + Json::StreamWriterBuilder builder; + builder["indentation"] = ""; + return Json::writeString(builder, value); +} + +bool SeerRobokitAgv::parseJson_(const std::string& input, Json::Value& output, std::string& error) +{ + Json::CharReaderBuilder builder; + std::unique_ptr reader(builder.newCharReader()); + return reader->parse(input.data(), input.data() + input.size(), &output, &error); +} + +std::string SeerRobokitAgv::extractJson_(const std::string& raw) +{ + const auto begin = raw.find('{'); + const auto end = raw.rfind('}'); + if (begin == std::string::npos || end == std::string::npos || end < begin) { + return {}; + } + return raw.substr(begin, end - begin + 1); +} + +AgvResult SeerRobokitAgv::receiveFrame_(const int sock, std::uint16_t& command, std::string& payload) +{ + const auto recv_exact = [](const int fd, std::uint8_t* data, const std::size_t size) -> AgvResult { + std::size_t offset = 0; + while (offset < size) { + const ssize_t count = ::recv(fd, data + offset, size - offset, 0); + if (count > 0) { + offset += static_cast(count); + continue; + } + if (count == 0) { + return AgvResult::failure(AgvErrorCode::NotConnected, "SEER Robokit socket closed"); + } + if (errno == EINTR) { + continue; + } + if (errno == EAGAIN || errno == EWOULDBLOCK) { + return AgvResult::failure(AgvErrorCode::Timeout, "SEER Robokit receive timeout"); + } + return AgvResult::failure(AgvErrorCode::CommandFailed, "SEER Robokit receive failed: " + systemError()); + } + return AgvResult::success(); + }; + + std::uint8_t header[16]{}; + auto result = recv_exact(sock, header, sizeof(header)); + if (!result.ok()) { + return result; + } + if (header[0] != 0x5A) { + return AgvResult::failure(AgvErrorCode::CommandFailed, "SEER Robokit frame header is invalid"); + } + + const auto length = (static_cast(header[4]) << 24U) + | (static_cast(header[5]) << 16U) + | (static_cast(header[6]) << 8U) + | static_cast(header[7]); + command = static_cast((static_cast(header[8]) << 8U) | header[9]); + payload.clear(); + if (length == 0) { + return AgvResult::success(); + } + if (length > kMaxFramePayloadBytes) { + return AgvResult::failure(AgvErrorCode::CommandFailed, "SEER Robokit frame payload is too large"); + } + + std::vector buffer(length); + result = recv_exact(sock, buffer.data(), buffer.size()); + if (!result.ok()) { + return result; + } + payload.assign(reinterpret_cast(buffer.data()), buffer.size()); + return AgvResult::success(); +} + + +AgvResult SeerRobokitAgv::resultFromResponse_(const Json::Value& response) +{ + if (!hasNumericControllerRetCode(response)) { + return AgvResult::failure( + AgvErrorCode::CommandFailed, + "SEER Robokit controller response is missing a numeric ret_code"); + } + const auto* ret_code_value = jsonFind(response, "ret_code"); + const bool success = ret_code_value->isUInt() || ret_code_value->isUInt64() + ? ret_code_value->asUInt64() == 0 + : ret_code_value->asInt64() == 0; + const std::string ret_code = jsonValueToString(*ret_code_value); + const std::string message = jsonGet(response, "err_msg", "").asString(); + if (success) { + return AgvResult::success(); + } + std::string detail = "SEER Robokit command failed: ret_code=" + ret_code; + if (!message.empty()) { + detail += ", err_msg=" + message; + } + return AgvResult::failure(AgvErrorCode::CommandFailed, detail); +} + +} // namespace cmvr::device diff --git a/cmvr-es/devices/agv/seer_robokit/tests/seer_robokit_control_authority_test.cpp b/cmvr-es/devices/agv/seer_robokit/tests/seer_robokit_control_authority_test.cpp new file mode 100644 index 00000000..905b9105 --- /dev/null +++ b/cmvr-es/devices/agv/seer_robokit/tests/seer_robokit_control_authority_test.cpp @@ -0,0 +1,5403 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include +#include + +#include "seer_robokit_agv.h" + +namespace cmvr::device { + +class SeerRobokitAgvTestPeer { +public: + static void installSockets( + SeerRobokitAgv& agv, + const int status, + const int control, + const int navigation, + const int config, + const int other) + { + agv.sock_status_ = status; + agv.sock_control_ = control; + agv.sock_navigation_ = navigation; + agv.sock_config_ = config; + agv.sock_other_ = other; + } + + static void cacheRuntimeState(SeerRobokitAgv& agv, const Json::Value& payload) + { + agv.updateCachedRuntimeState_(payload); + } + + static void setAdapterError(SeerRobokitAgv& agv, std::string error) + { + std::lock_guard lock(agv.mutex_); + agv.last_error_ = std::move(error); + } + + static void setFaultStateUnknown(SeerRobokitAgv& agv) + { + agv.invalidateControllerFaultState_(); + } + + static void setFaultStateAge( + SeerRobokitAgv& agv, + const std::chrono::milliseconds age) + { + std::lock_guard lock(agv.runtime_state_mutex_); + agv.controller_fault_state_observed_ = true; + agv.controller_fault_state_observed_at_ = + std::chrono::steady_clock::now() - age; + agv.active_controller_fault_detail_.clear(); + } + + static bool hasTrackedPoseTask(const SeerRobokitAgv& agv) + { + SeerRobokitAgv::PoseTaskContext context; + return agv.currentPoseTask_(context); + } + + static bool hasTrackedNavigation( + const SeerRobokitAgv& agv, + const AgvTaskType expected_type) + { + SeerRobokitAgv::TrackedNavigationContext context; + return agv.currentTrackedNavigation_(context) + && context.type == expected_type; + } + + static AgvResult disconnect(SeerRobokitAgv& agv) + { + return agv.disconnect_(); + } + + static void setNavigationReceiveTimeout( + SeerRobokitAgv& agv, + const std::chrono::milliseconds timeout) + { + timeval value{}; + value.tv_sec = static_cast(timeout.count() / 1000); + value.tv_usec = static_cast( + (timeout.count() % 1000) * 1000); + ASSERT_EQ( + ::setsockopt( + agv.sock_navigation_, + SOL_SOCKET, + SO_RCVTIMEO, + &value, + sizeof(value)), + 0); + } + + static void setStatusReceiveTimeout( + SeerRobokitAgv& agv, + const std::chrono::milliseconds timeout) + { + timeval value{}; + value.tv_sec = static_cast(timeout.count() / 1000); + value.tv_usec = static_cast( + (timeout.count() % 1000) * 1000); + ASSERT_EQ( + ::setsockopt( + agv.sock_status_, + SOL_SOCKET, + SO_RCVTIMEO, + &value, + sizeof(value)), + 0); + } + + static void closeNavigationSocket(SeerRobokitAgv& agv) + { + std::lock_guard lock(agv.mutex_); + agv.closeSocket_(agv.sock_navigation_); + } +}; + +namespace { + +constexpr std::uint16_t kRobotStatusTask = 1020; +constexpr std::uint16_t kRobotStatusLoc = 1004; +constexpr std::uint16_t kRobotStatusAll2 = 1101; +constexpr std::uint16_t kRobotStatusTaskPackage = 1110; +constexpr std::uint16_t kRobotControlStop = 2000; +constexpr std::uint16_t kRobotControlMotion = 2010; +constexpr std::uint16_t kRobotControlLoadMap = 2022; +constexpr std::uint16_t kRobotTaskPause = 3001; +constexpr std::uint16_t kRobotTaskResume = 3002; +constexpr std::uint16_t kRobotTaskCancel = 3003; +constexpr std::uint16_t kRobotTaskGoTarget = 3051; +constexpr std::uint16_t kRobotTaskGoTargetList = 3066; +constexpr std::uint16_t kRobotTaskClearTargetList = 3067; +constexpr std::uint16_t kRobotConfigLock = 4005; +constexpr std::uint16_t kRobotConfigUploadMap = 4010; +constexpr std::uint16_t kRobotConfigDownloadMap = 4011; +constexpr std::uint16_t kRobotOtherStartMapping = 6100; +constexpr std::uint16_t kRobotOtherStopMapping = 6101; + +enum class Channel : std::size_t { + Status = 0, + Control, + Navigation, + Config, + Other, + Count +}; + +struct CommandRecord { + std::uint16_t command{0}; + std::string payload; +}; + +Json::Value parsePayload(const CommandRecord& record) +{ + Json::Value payload; + Json::CharReaderBuilder builder; + std::string error; + std::unique_ptr reader(builder.newCharReader()); + if (!reader->parse( + record.payload.data(), + record.payload.data() + record.payload.size(), + &payload, + &error)) { + ADD_FAILURE() << "Failed to parse command " << record.command + << " payload: " << error; + } + return payload; +} + +const Json::Value& payloadValue(const Json::Value& payload, const char* key) +{ + const auto* value = payload.find(key, key + std::strlen(key)); + if (!value) { + ADD_FAILURE() << "Missing JSON field: " << key; + static const Json::Value null_value; + return null_value; + } + return *value; +} + +bool payloadHas(const Json::Value& payload, const char* key) +{ + return payload.find(key, key + std::strlen(key)) != nullptr; +} + +AgvMotionOptions asynchronousMotionOptions() +{ + AgvMotionOptions options; + options.asynchronous = true; + return options; +} + +bool receiveExact(const int fd, void* output, const std::size_t size) +{ + auto* bytes = static_cast(output); + std::size_t offset = 0; + while (offset < size) { + const auto count = ::recv(fd, bytes + offset, size - offset, 0); + if (count > 0) { + offset += static_cast(count); + continue; + } + if (count < 0 && errno == EINTR) { + continue; + } + return false; + } + return true; +} + +bool sendAll(const int fd, const std::vector& data) +{ + std::size_t offset = 0; + while (offset < data.size()) { + const auto count = ::send( + fd, + data.data() + offset, + data.size() - offset, + MSG_NOSIGNAL); + if (count > 0) { + offset += static_cast(count); + continue; + } + if (count < 0 && errno == EINTR) { + continue; + } + return false; + } + return true; +} + +std::vector responseFrame( + const std::uint16_t response_command, + const std::string& payload) +{ + std::vector frame(16 + payload.size(), 0); + frame[0] = 0x5A; + frame[1] = 0x01; + frame[3] = 0x01; + const auto length = static_cast(payload.size()); + frame[4] = static_cast((length >> 24U) & 0xFFU); + frame[5] = static_cast((length >> 16U) & 0xFFU); + frame[6] = static_cast((length >> 8U) & 0xFFU); + frame[7] = static_cast(length & 0xFFU); + frame[8] = static_cast((response_command >> 8U) & 0xFFU); + frame[9] = static_cast(response_command & 0xFFU); + std::copy(payload.begin(), payload.end(), frame.begin() + 16); + return frame; +} + +std::string injectRequestedTaskId( + std::string response_payload, + const std::string& request_payload) +{ + constexpr char kTaskIdToken[] = "${TASK_ID}"; + Json::Value request; + Json::CharReaderBuilder builder; + std::string error; + std::unique_ptr reader(builder.newCharReader()); + if (!reader->parse( + request_payload.data(), + request_payload.data() + request_payload.size(), + &request, + &error)) { + return response_payload; + } + const auto* task_ids = request.find("task_ids", "task_ids" + std::strlen("task_ids")); + if (!task_ids || !task_ids->isArray() || task_ids->empty()) { + return response_payload; + } + + const auto replace_all = [&response_payload]( + const std::string& token, + const std::string& value) { + std::size_t position = 0; + while ((position = response_payload.find(token, position)) + != std::string::npos) { + response_payload.replace(position, token.size(), value); + position += value.size(); + } + }; + for (Json::ArrayIndex index = 0; index < task_ids->size(); ++index) { + replace_all( + "${TASK_ID_" + std::to_string(index) + "}", + (*task_ids)[index].asString()); + } + replace_all(kTaskIdToken, (*task_ids)[0].asString()); + return response_payload; +} + +class FakeSeerRobokitController { +public: + FakeSeerRobokitController() + { + for (auto& endpoint : endpoints_) { + int pair[2]{-1, -1}; + if (::socketpair(AF_UNIX, SOCK_STREAM, 0, pair) != 0) { + throw std::runtime_error("socketpair failed"); + } + endpoint.client = pair[0]; + endpoint.server = pair[1]; + } + for (std::size_t index = 0; index < endpoints_.size(); ++index) { + endpoints_[index].worker = std::thread( + &FakeSeerRobokitController::serve, + this, + index); + } + } + + ~FakeSeerRobokitController() + { + for (auto& endpoint : endpoints_) { + if (endpoint.client >= 0) { + ::shutdown(endpoint.client, SHUT_RDWR); + ::close(endpoint.client); + endpoint.client = -1; + } + if (endpoint.server >= 0) { + ::shutdown(endpoint.server, SHUT_RDWR); + } + } + for (auto& endpoint : endpoints_) { + if (endpoint.worker.joinable()) { + endpoint.worker.join(); + } + if (endpoint.server >= 0) { + ::close(endpoint.server); + endpoint.server = -1; + } + } + } + + int takeClient(const Channel channel) + { + auto& endpoint = endpoints_[static_cast(channel)]; + const int client = endpoint.client; + endpoint.client = -1; + return client; + } + + void setResponseCode(const std::uint16_t command, const int ret_code) + { + std::lock_guard lock(response_codes_mutex_); + response_codes_[command] = ret_code; + response_payloads_.erase(command); + } + + void setResponsePayload(const std::uint16_t command, std::string payload) + { + std::lock_guard lock(response_codes_mutex_); + response_codes_.erase(command); + response_payloads_[command] = {std::move(payload)}; + } + + void queueResponsePayload(const std::uint16_t command, std::string payload) + { + std::lock_guard lock(response_codes_mutex_); + response_codes_.erase(command); + response_payloads_[command].push_back(std::move(payload)); + } + + void setResponseDelay( + const std::uint16_t command, + const std::chrono::milliseconds delay) + { + std::lock_guard lock(response_codes_mutex_); + response_delays_[command] = delay; + } + + void setResponseCommand( + const std::uint16_t request_command, + const std::uint16_t response_command) + { + std::lock_guard lock(response_codes_mutex_); + response_commands_[request_command] = response_command; + } + + void clearRecords() + { + std::lock_guard lock(records_mutex_); + records_.clear(); + } + + std::vector records() const + { + std::lock_guard lock(records_mutex_); + return records_; + } + +private: + struct Endpoint { + int client{-1}; + int server{-1}; + std::thread worker; + }; + + void serve(const std::size_t index) + { + const int fd = endpoints_[index].server; + while (true) { + std::array header{}; + if (!receiveExact(fd, header.data(), header.size())) { + return; + } + const auto length = (static_cast(header[4]) << 24U) + | (static_cast(header[5]) << 16U) + | (static_cast(header[6]) << 8U) + | static_cast(header[7]); + const auto command = static_cast( + (static_cast(header[8]) << 8U) | header[9]); + std::string payload(length, '\0'); + if (length > 0 && !receiveExact(fd, payload.data(), payload.size())) { + return; + } + { + std::lock_guard lock(records_mutex_); + records_.push_back({command, payload}); + } + int ret_code = 0; + std::string response_payload; + std::chrono::milliseconds response_delay{0}; + std::uint16_t response_command = static_cast( + command + 10000U); + { + std::lock_guard lock(response_codes_mutex_); + const auto payloads = response_payloads_.find(command); + if (payloads != response_payloads_.end() && !payloads->second.empty()) { + response_payload = payloads->second.front(); + if (payloads->second.size() > 1U) { + payloads->second.pop_front(); + } + } + const auto response_code = response_codes_.find(command); + if (response_code != response_codes_.end()) { + ret_code = response_code->second; + } + const auto delay = response_delays_.find(command); + if (delay != response_delays_.end()) { + response_delay = delay->second; + } + const auto response_command_override = response_commands_.find(command); + if (response_command_override != response_commands_.end()) { + response_command = response_command_override->second; + } + } + if (response_delay.count() > 0) { + std::this_thread::sleep_for(response_delay); + } + if (response_payload.empty()) { + response_payload = ret_code == 0 + ? R"({"ret_code":0,"err_msg":""})" + : "{\"ret_code\":" + std::to_string(ret_code) + + R"(,"err_msg":"simulated command failure"})"; + } + response_payload = injectRequestedTaskId( + std::move(response_payload), + payload); + if (!sendAll(fd, responseFrame(response_command, response_payload))) { + return; + } + } + } + + std::array(Channel::Count)> endpoints_; + mutable std::mutex records_mutex_; + std::vector records_; + std::mutex response_codes_mutex_; + std::unordered_map response_codes_; + std::unordered_map> response_payloads_; + std::unordered_map response_delays_; + std::unordered_map response_commands_; +}; + +class SeerRobokitControlAuthorityTest : public ::testing::Test { +protected: + void SetUp() override + { + config::SeerRobokitAgvConfig cfg; + cfg.set_id("src1100"); + cfg.set_ip("invalid-ip"); + cfg.set_recv_timeout_ms(100); + cfg.set_control_nick_name("cmvr-test"); + cfg.set_enable_state_push(true); + cfg.set_state_push_interval_ms(200); + controller_.setResponsePayload( + kRobotStatusTask, + R"({"ret_code":0,"err_msg":"","task_status":2,"task_type":1,"target_point":[1.0,2.0,0.5]})"); + controller_.setResponsePayload( + kRobotStatusTaskPackage, + R"({"ret_code":0,"err_msg":"","task_status_package":{"task_status_list":[{"task_id":"${TASK_ID}","status":2,"type":1}]}})"); + controller_.setResponsePayload( + kRobotStatusAll2, + R"({"ret_code":0,"err_msg":"","task_status":0,"task_type":0,"blocked":false,"vx":0.0,"vy":0.0,"w":0.0,"fatals":[],"errors":[],"warnings":[]})"); + agv_ = std::make_unique(cfg); + const int status_socket = controller_.takeClient(Channel::Status); + const int control_socket = controller_.takeClient(Channel::Control); + const int navigation_socket = controller_.takeClient(Channel::Navigation); + const int config_socket = controller_.takeClient(Channel::Config); + const int other_socket = controller_.takeClient(Channel::Other); + SeerRobokitAgvTestPeer::installSockets( + *agv_, + status_socket, + control_socket, + navigation_socket, + config_socket, + other_socket); + Json::Value fault_state_push(Json::objectValue); + *fault_state_push.demand( + "errors", + "errors" + std::strlen("errors")) = + Json::Value(Json::arrayValue); + *fault_state_push.demand( + "fatals", + "fatals" + std::strlen("fatals")) = + Json::Value(Json::arrayValue); + SeerRobokitAgvTestPeer::cacheRuntimeState(*agv_, fault_state_push); + } + + void TearDown() override + { + agv_.reset(); + } + + void expectControlledSequence( + const std::vector& commands, + const std::function& invoke) + { + controller_.clearRecords(); + const auto result = invoke(); + ASSERT_TRUE(result.ok()) << result.message; + + const auto records = controller_.records(); + ASSERT_EQ(records.size(), commands.size() + 1U); + EXPECT_EQ(records[0].command, kRobotConfigLock); + for (std::size_t index = 0; index < commands.size(); ++index) { + EXPECT_EQ(records[index + 1U].command, commands[index]); + } + + Json::Value lock_payload; + Json::CharReaderBuilder builder; + std::string error; + std::unique_ptr reader(builder.newCharReader()); + ASSERT_TRUE(reader->parse( + records[0].payload.data(), + records[0].payload.data() + records[0].payload.size(), + &lock_payload, + &error)) << error; + constexpr char kNickName[] = "nick_name"; + const auto* nick_name = lock_payload.find( + kNickName, + kNickName + std::strlen(kNickName)); + ASSERT_NE(nick_name, nullptr); + EXPECT_EQ(nick_name->asString(), "cmvr-test"); + } + + void expectControlled( + const std::uint16_t command, + const std::function& invoke) + { + expectControlledSequence({command}, invoke); + } + + bool waitForCommandCount( + const std::uint16_t command, + const std::size_t expected_count, + const int max_attempts = 3000) const + { + for (int attempt = 0; attempt < max_attempts; ++attempt) { + const auto records = controller_.records(); + const auto count = static_cast(std::count_if( + records.begin(), + records.end(), + [command](const CommandRecord& record) { + return record.command == command; + })); + if (count >= expected_count) { + return true; + } + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + } + return false; + } + + FakeSeerRobokitController controller_; + std::unique_ptr agv_; +}; + +TEST_F(SeerRobokitControlAuthorityTest, EveryImplementedMutatingOperationAcquiresAuthorityFirst) +{ + controller_.clearRecords(); + const auto pose_result = + agv_->navigateToPose(math::Pose2d{1.0, 2.0, 0.5}, asynchronousMotionOptions()); + ASSERT_TRUE(pose_result.ok()) << pose_result.message; + const auto pose_records = controller_.records(); + ASSERT_GE(pose_records.size(), 4U); + EXPECT_EQ(pose_records[0].command, kRobotConfigLock); + EXPECT_EQ(pose_records[1].command, kRobotTaskGoTarget); + for (std::size_t index = 2; index < pose_records.size(); ++index) { + EXPECT_EQ(pose_records[index].command, kRobotStatusTaskPackage); + } + expectControlled(kRobotTaskGoTarget, [this]() { + return agv_->navigateToStation("station-1", asynchronousMotionOptions()); + }); + expectControlled(kRobotTaskGoTargetList, [this]() { + return agv_->followPath( + {AgvPathSegment{"station-1", "station-2"}}, + asynchronousMotionOptions()); + }); + expectControlled(kRobotTaskPause, [this]() { + return agv_->pauseNavigation(); + }); + expectControlled(kRobotTaskResume, [this]() { + return agv_->resumeNavigation(); + }); + controller_.clearRecords(); + const auto cancel_result = agv_->cancelNavigation(); + ASSERT_TRUE(cancel_result.ok()) << cancel_result.message; + const auto cancel_records = controller_.records(); + ASSERT_EQ(cancel_records.size(), 4U); + EXPECT_EQ(cancel_records[0].command, kRobotStatusTaskPackage); + EXPECT_EQ(cancel_records[1].command, kRobotStatusAll2); + EXPECT_EQ(cancel_records[2].command, kRobotConfigLock); + EXPECT_EQ(cancel_records[3].command, kRobotTaskClearTargetList); + expectControlledSequence( + {kRobotControlStop, kRobotTaskClearTargetList}, + [this]() { + return agv_->emergencyStop(); + }); + expectControlled(kRobotControlMotion, [this]() { + return agv_->setVelocity(AgvVelocity{0.1, 0.0, 0.2}); + }); + expectControlled(kRobotControlMotion, [this]() { + return agv_->stopVelocityControl(); + }); + expectControlled(kRobotControlLoadMap, [this]() { + return agv_->switchMap("map-1"); + }); + expectControlled(kRobotConfigUploadMap, [this]() { + return agv_->uploadMap("map-1", "{}"); + }); + expectControlled(kRobotOtherStartMapping, [this]() { + return agv_->startMapping(); + }); + expectControlled(kRobotOtherStopMapping, [this]() { + return agv_->stopMapping(); + }); +} + +TEST_F( + SeerRobokitControlAuthorityTest, + SynchronousPoseNavigationWaitsForVerifiedCompletionAndStoppedVelocity) +{ + controller_.setResponsePayload( + kRobotStatusAll2, + R"({"ret_code":0,"task_status":0,"task_type":0,"blocked":false,"vx":0.0,"vy":0.0,"w":0.0,"fatals":[],"errors":[]})"); + controller_.queueResponsePayload( + kRobotStatusAll2, + R"({"ret_code":0,"task_status":2,"task_type":1,"blocked":false,"vx":0.1,"vy":0.0,"w":0.0,"fatals":[],"errors":[]})"); + controller_.setResponsePayload( + kRobotStatusTaskPackage, + R"({"ret_code":0,"task_status_package":{"task_status_list":[{"task_id":"${TASK_ID}","status":2,"type":1}]}})"); + controller_.setResponsePayload( + kRobotStatusLoc, + R"({"ret_code":0,"x":1.0,"y":2.0,"angle":0.5,"confidence":1.0})"); + AgvMotionOptions options; + options.asynchronous = false; + options.wait_timeout_ms = 3000; + options.poll_interval_ms = 20; + std::atomic finished{false}; + AgvResult result; + controller_.clearRecords(); + + std::thread navigation_thread([this, &options, &finished, &result]() { + result = agv_->navigateToPose( + math::Pose2d{1.0, 2.0, 0.5}, + options); + finished.store(true, std::memory_order_release); + }); + + const bool terminal_wait_observed = waitForCommandCount( + kRobotStatusAll2, + 2); + EXPECT_TRUE(terminal_wait_observed); + EXPECT_FALSE(finished.load(std::memory_order_acquire)); + + controller_.setResponsePayload( + kRobotStatusTaskPackage, + R"({"ret_code":0,"task_status_package":{"task_status_list":[{"task_id":"${TASK_ID}","status":4,"type":1}]}})"); + controller_.setResponsePayload( + kRobotStatusAll2, + R"({"ret_code":0,"task_status":4,"task_type":1,"blocked":false,"vx":0.0,"vy":0.0,"w":0.0,"fatals":[],"errors":[]})"); + navigation_thread.join(); + + ASSERT_TRUE(result.ok()) << result.message; + EXPECT_TRUE(finished.load(std::memory_order_acquire)); + const auto records = controller_.records(); + EXPECT_GE( + static_cast(std::count_if( + records.begin(), + records.end(), + [](const CommandRecord& record) { + return record.command == kRobotStatusAll2; + })), + 3U); +} + +TEST_F( + SeerRobokitControlAuthorityTest, + SynchronousPoseRechecksTargetAfterCompletedTaskStops) +{ + controller_.setResponsePayload( + kRobotStatusAll2, + R"({"ret_code":0,"task_status":0,"task_type":0,"blocked":false,"vx":0.0,"vy":0.0,"w":0.0,"fatals":[],"errors":[]})"); + controller_.queueResponsePayload( + kRobotStatusAll2, + R"({"ret_code":0,"task_status":4,"task_type":1,"blocked":false,"vx":0.05,"vy":0.0,"w":0.0,"fatals":[],"errors":[]})"); + controller_.setResponsePayload( + kRobotStatusTaskPackage, + R"({"ret_code":0,"task_status_package":{"task_status_list":[{"task_id":"${TASK_ID}","status":4,"type":1}]}})"); + controller_.setResponsePayload( + kRobotStatusLoc, + R"({"ret_code":0,"x":1.2,"y":2.0,"angle":0.5})"); + AgvMotionOptions options; + options.wait_timeout_ms = 2000; + options.poll_interval_ms = 20; + std::atomic finished{false}; + AgvResult result; + controller_.clearRecords(); + + std::thread navigation_thread([this, &options, &finished, &result]() { + result = agv_->navigateToPose( + math::Pose2d{1.0, 2.0, 0.5}, + options); + finished.store(true, std::memory_order_release); + }); + + const bool moving_completion_observed = + waitForCommandCount(kRobotStatusAll2, 2); + EXPECT_TRUE(moving_completion_observed); + EXPECT_FALSE(finished.load(std::memory_order_acquire)); + const auto moving_records = controller_.records(); + EXPECT_EQ( + std::count_if( + moving_records.begin(), + moving_records.end(), + [](const CommandRecord& record) { + return record.command == kRobotStatusLoc; + }), + 0); + controller_.setResponsePayload( + kRobotStatusAll2, + R"({"ret_code":0,"task_status":4,"task_type":1,"blocked":false,"vx":0.0,"vy":0.0,"w":0.0,"fatals":[],"errors":[]})"); + navigation_thread.join(); + + EXPECT_FALSE(result.ok()); + EXPECT_EQ(result.code, AgvErrorCode::TaskRejected); + EXPECT_NE( + result.message.find("final pose was outside"), + std::string::npos); + EXPECT_NE( + result.message.find("distance_error=0.2"), + std::string::npos); +} + +TEST_F( + SeerRobokitControlAuthorityTest, + SynchronousPoseReconcilesIndeterminateCommandAcknowledgment) +{ + controller_.setResponsePayload( + kRobotStatusAll2, + R"({"ret_code":0,"task_status":0,"task_type":0,"blocked":false,"vx":0.0,"vy":0.0,"w":0.0,"fatals":[],"errors":[]})"); + controller_.queueResponsePayload( + kRobotStatusAll2, + R"({"ret_code":0,"task_status":2,"task_type":1,"blocked":false,"vx":0.05,"vy":0.0,"w":0.0,"fatals":[],"errors":[]})"); + controller_.setResponsePayload( + kRobotStatusTaskPackage, + R"({"ret_code":0,"task_status_package":{"task_status_list":[{"task_id":"${TASK_ID}","status":2,"type":1}]}})"); + controller_.setResponsePayload( + kRobotTaskGoTarget, + R"({"err_msg":"pose acknowledgment lost"})"); + controller_.setResponsePayload( + kRobotStatusLoc, + R"({"ret_code":0,"x":1.0,"y":2.0,"angle":0.5})"); + AgvMotionOptions options; + options.wait_timeout_ms = 3000; + options.poll_interval_ms = 20; + std::atomic finished{false}; + AgvResult result; + controller_.clearRecords(); + + std::thread navigation_thread([this, &options, &finished, &result]() { + result = agv_->navigateToPose( + math::Pose2d{1.0, 2.0, 0.5}, + options); + finished.store(true, std::memory_order_release); + }); + + const bool pose_status_observed = waitForCommandCount( + kRobotStatusTaskPackage, + 3, + 1000); + EXPECT_TRUE(pose_status_observed); + EXPECT_FALSE(finished.load(std::memory_order_acquire)); + controller_.setResponsePayload( + kRobotStatusTaskPackage, + R"({"ret_code":0,"task_status_package":{"task_status_list":[{"task_id":"${TASK_ID}","status":4,"type":1}]}})"); + controller_.setResponsePayload( + kRobotStatusAll2, + R"({"ret_code":0,"task_status":4,"task_type":1,"blocked":false,"vx":0.0,"vy":0.0,"w":0.0,"fatals":[],"errors":[]})"); + navigation_thread.join(); + + ASSERT_TRUE(result.ok()) << result.message; + EXPECT_NE( + result.message.find("indeterminate command acknowledgment"), + std::string::npos); + EXPECT_NE( + result.message.find("missing a numeric ret_code"), + std::string::npos); +} + +TEST_F( + SeerRobokitControlAuthorityTest, + PoseWaitDoesNotLetGlobalTerminalOverrideExactRunningAfterGrace) +{ + controller_.setResponsePayload( + kRobotStatusAll2, + R"({"ret_code":0,"task_status":0,"task_type":0,"blocked":false,"vx":0.0,"vy":0.0,"w":0.0,"fatals":[],"errors":[]})"); + controller_.queueResponsePayload( + kRobotStatusAll2, + R"({"ret_code":0,"task_status":5,"task_type":1,"blocked":false,"vx":0.0,"vy":0.0,"w":0.0,"fatals":[],"errors":[]})"); + controller_.setResponsePayload( + kRobotStatusTaskPackage, + R"({"ret_code":0,"task_status_package":{"task_status_list":[{"task_id":"${TASK_ID}","status":2,"type":1}]}})"); + controller_.setResponsePayload( + kRobotStatusLoc, + R"({"ret_code":0,"x":1.0,"y":2.0,"angle":0.5})"); + AgvMotionOptions options; + options.wait_timeout_ms = 3000; + options.poll_interval_ms = 20; + std::atomic finished{false}; + AgvResult result; + controller_.clearRecords(); + + std::thread navigation_thread([this, &options, &finished, &result]() { + result = agv_->navigateToPose( + math::Pose2d{1.0, 2.0, 0.5}, + options); + finished.store(true, std::memory_order_release); + }); + + const bool grace_elapsed_while_polling = waitForCommandCount( + kRobotStatusTaskPackage, + 90, + 2500); + EXPECT_TRUE(grace_elapsed_while_polling); + EXPECT_FALSE(finished.load(std::memory_order_acquire)); + const auto records_before_completion = controller_.records(); + EXPECT_EQ( + std::count_if( + records_before_completion.begin(), + records_before_completion.end(), + [](const CommandRecord& record) { + return record.command == kRobotTaskCancel; + }), + 0); + controller_.setResponsePayload( + kRobotStatusTaskPackage, + R"({"ret_code":0,"task_status_package":{"task_status_list":[{"task_id":"${TASK_ID}","status":4,"type":1}]}})"); + controller_.setResponsePayload( + kRobotStatusAll2, + R"({"ret_code":0,"task_status":4,"task_type":1,"blocked":false,"vx":0.0,"vy":0.0,"w":0.0,"fatals":[],"errors":[]})"); + navigation_thread.join(); + + ASSERT_TRUE(result.ok()) << result.message; +} + +TEST_F( + SeerRobokitControlAuthorityTest, + SynchronousStationNavigationWaitsForExactTaskCompletion) +{ + controller_.setResponsePayload( + kRobotStatusAll2, + R"({"ret_code":0,"task_status":0,"task_type":0,"blocked":false,"vx":0.0,"vy":0.0,"w":0.0,"fatals":[],"errors":[]})"); + controller_.queueResponsePayload( + kRobotStatusAll2, + R"({"ret_code":0,"task_status":2,"task_type":2,"target_id":"station-2","blocked":false,"vx":0.1,"vy":0.0,"w":0.0,"fatals":[],"errors":[]})"); + controller_.setResponsePayload( + kRobotStatusTaskPackage, + R"({"ret_code":0,"task_status_package":{"task_status_list":[{"task_id":"${TASK_ID}","status":2,"type":2}]}})"); + AgvMotionOptions options; + options.asynchronous = false; + options.wait_timeout_ms = 3000; + options.poll_interval_ms = 20; + std::atomic finished{false}; + AgvResult result; + controller_.clearRecords(); + + std::thread navigation_thread([this, &options, &finished, &result]() { + result = agv_->navigateToStation("station-2", options); + finished.store(true, std::memory_order_release); + }); + + const bool terminal_wait_observed = waitForCommandCount( + kRobotStatusAll2, + 2); + EXPECT_TRUE(terminal_wait_observed); + EXPECT_FALSE(finished.load(std::memory_order_acquire)); + + controller_.setResponsePayload( + kRobotStatusTaskPackage, + R"({"ret_code":0,"task_status_package":{"task_status_list":[{"task_id":"${TASK_ID}","status":4,"type":2}]}})"); + controller_.setResponsePayload( + kRobotStatusAll2, + R"({"ret_code":0,"task_status":4,"task_type":2,"target_id":"station-2","blocked":false,"vx":0.0,"vy":0.0,"w":0.0,"fatals":[],"errors":[]})"); + navigation_thread.join(); + + ASSERT_TRUE(result.ok()) << result.message; + const auto records = controller_.records(); + const auto command = std::find_if( + records.begin(), + records.end(), + [](const CommandRecord& record) { + return record.command == kRobotTaskGoTarget; + }); + ASSERT_NE(command, records.end()); + const auto payload = parsePayload(*command); + EXPECT_FALSE(payloadValue(payload, "task_id").asString().empty()); +} + +TEST_F( + SeerRobokitControlAuthorityTest, + SynchronousStationAcceptsExactCompletionAfterGlobalStateClears) +{ + controller_.setResponsePayload( + kRobotStatusTaskPackage, + R"({"ret_code":0,"task_status_package":{"task_status_list":[{"task_id":"${TASK_ID}","status":4,"type":2}]}})"); + AgvMotionOptions options; + options.wait_timeout_ms = 2500; + options.poll_interval_ms = 20; + + const auto result = agv_->navigateToStation( + "station-cleared", + options); + + ASSERT_TRUE(result.ok()) << result.message; +} + +TEST_F( + SeerRobokitControlAuthorityTest, + StationWaitIgnoresStaleGlobalTaskUntilExactTaskIdAppears) +{ + controller_.setResponsePayload( + kRobotStatusAll2, + R"({"ret_code":0,"task_status":0,"task_type":0,"blocked":false,"vx":0.0,"vy":0.0,"w":0.0,"fatals":[],"errors":[]})"); + controller_.queueResponsePayload( + kRobotStatusAll2, + R"({"ret_code":0,"task_status":2,"task_type":1,"blocked":false,"vx":0.1,"vy":0.0,"w":0.0,"fatals":[],"errors":[]})"); + controller_.queueResponsePayload( + kRobotStatusAll2, + R"({"ret_code":0,"task_status":5,"task_type":1,"blocked":false,"vx":0.0,"vy":0.0,"w":0.0,"fatals":[],"errors":[]})"); + controller_.queueResponsePayload( + kRobotStatusAll2, + R"({"ret_code":0,"task_status":2,"task_type":2,"target_id":"station-delayed","blocked":false,"vx":0.1,"vy":0.0,"w":0.0,"fatals":[],"errors":[]})"); + controller_.setResponsePayload( + kRobotStatusTaskPackage, + R"({"ret_code":0,"task_status_package":{"task_status_list":[]}})"); + controller_.queueResponsePayload( + kRobotStatusTaskPackage, + R"({"ret_code":0,"task_status_package":{"task_status_list":[{"task_id":"${TASK_ID}","status":2,"type":2}]}})"); + AgvMotionOptions options; + options.wait_timeout_ms = 2000; + options.poll_interval_ms = 20; + std::atomic finished{false}; + AgvResult result; + controller_.clearRecords(); + + std::thread navigation_thread([this, &options, &finished, &result]() { + result = agv_->navigateToStation("station-delayed", options); + finished.store(true, std::memory_order_release); + }); + + const bool exact_task_appeared = waitForCommandCount( + kRobotStatusTaskPackage, + 3, + 1000); + EXPECT_TRUE(exact_task_appeared); + EXPECT_FALSE(finished.load(std::memory_order_acquire)); + const auto records_before_completion = controller_.records(); + EXPECT_EQ( + std::count_if( + records_before_completion.begin(), + records_before_completion.end(), + [](const CommandRecord& record) { + return record.command == kRobotTaskCancel; + }), + 0); + controller_.setResponsePayload( + kRobotStatusTaskPackage, + R"({"ret_code":0,"task_status_package":{"task_status_list":[{"task_id":"${TASK_ID}","status":4,"type":2}]}})"); + controller_.setResponsePayload( + kRobotStatusAll2, + R"({"ret_code":0,"task_status":4,"task_type":2,"target_id":"station-delayed","blocked":false,"vx":0.0,"vy":0.0,"w":0.0,"fatals":[],"errors":[]})"); + navigation_thread.join(); + + ASSERT_TRUE(result.ok()) << result.message; + const auto records = controller_.records(); + EXPECT_EQ( + std::count_if( + records.begin(), + records.end(), + [](const CommandRecord& record) { + return record.command == kRobotTaskCancel; + }), + 0); +} + +TEST_F( + SeerRobokitControlAuthorityTest, + SynchronousStationReconcilesIndeterminateCommandAcknowledgment) +{ + controller_.setResponsePayload( + kRobotStatusAll2, + R"({"ret_code":0,"task_status":0,"task_type":0,"blocked":false,"vx":0.0,"vy":0.0,"w":0.0,"fatals":[],"errors":[]})"); + controller_.queueResponsePayload( + kRobotStatusAll2, + R"({"ret_code":0,"task_status":2,"task_type":2,"target_id":"station-ack","blocked":false,"vx":0.05,"vy":0.0,"w":0.0,"fatals":[],"errors":[]})"); + controller_.setResponsePayload( + kRobotStatusTaskPackage, + R"({"ret_code":0,"task_status_package":{"task_status_list":[{"task_id":"${TASK_ID}","status":2,"type":2}]}})"); + controller_.setResponsePayload( + kRobotTaskGoTarget, + R"({"err_msg":"station acknowledgment lost"})"); + AgvMotionOptions options; + options.wait_timeout_ms = 3000; + options.poll_interval_ms = 20; + std::atomic finished{false}; + AgvResult result; + controller_.clearRecords(); + + std::thread navigation_thread([this, &options, &finished, &result]() { + result = agv_->navigateToStation("station-ack", options); + finished.store(true, std::memory_order_release); + }); + + const bool station_status_observed = waitForCommandCount( + kRobotStatusTaskPackage, + 3, + 1000); + EXPECT_TRUE(station_status_observed); + EXPECT_FALSE(finished.load(std::memory_order_acquire)); + controller_.setResponsePayload( + kRobotStatusTaskPackage, + R"({"ret_code":0,"task_status_package":{"task_status_list":[{"task_id":"${TASK_ID}","status":4,"type":2}]}})"); + controller_.setResponsePayload( + kRobotStatusAll2, + R"({"ret_code":0,"task_status":4,"task_type":2,"target_id":"station-ack","blocked":false,"vx":0.0,"vy":0.0,"w":0.0,"fatals":[],"errors":[]})"); + navigation_thread.join(); + + ASSERT_TRUE(result.ok()) << result.message; + EXPECT_NE( + result.message.find("indeterminate command acknowledgment"), + std::string::npos); +} + +TEST_F( + SeerRobokitControlAuthorityTest, + StationWaitDoesNotLetGlobalTerminalOverrideExactRunningAfterGrace) +{ + controller_.setResponsePayload( + kRobotStatusAll2, + R"({"ret_code":0,"task_status":0,"task_type":0,"blocked":false,"vx":0.0,"vy":0.0,"w":0.0,"fatals":[],"errors":[]})"); + controller_.queueResponsePayload( + kRobotStatusAll2, + R"({"ret_code":0,"task_status":5,"task_type":2,"target_id":"station-current","blocked":false,"vx":0.0,"vy":0.0,"w":0.0,"fatals":[],"errors":[]})"); + controller_.setResponsePayload( + kRobotStatusTaskPackage, + R"({"ret_code":0,"task_status_package":{"task_status_list":[{"task_id":"${TASK_ID}","status":2,"type":2}]}})"); + AgvMotionOptions options; + options.wait_timeout_ms = 3000; + options.poll_interval_ms = 20; + std::atomic finished{false}; + AgvResult result; + controller_.clearRecords(); + + std::thread navigation_thread([this, &options, &finished, &result]() { + result = agv_->navigateToStation("station-current", options); + finished.store(true, std::memory_order_release); + }); + + const bool grace_elapsed_while_polling = waitForCommandCount( + kRobotStatusTaskPackage, + 90, + 2500); + EXPECT_TRUE(grace_elapsed_while_polling); + EXPECT_FALSE(finished.load(std::memory_order_acquire)); + const auto records_before_completion = controller_.records(); + EXPECT_EQ( + std::count_if( + records_before_completion.begin(), + records_before_completion.end(), + [](const CommandRecord& record) { + return record.command == kRobotTaskCancel; + }), + 0); + controller_.setResponsePayload( + kRobotStatusTaskPackage, + R"({"ret_code":0,"task_status_package":{"task_status_list":[{"task_id":"${TASK_ID}","status":4,"type":2}]}})"); + controller_.setResponsePayload( + kRobotStatusAll2, + R"({"ret_code":0,"task_status":4,"task_type":2,"target_id":"station-current","blocked":false,"vx":0.0,"vy":0.0,"w":0.0,"fatals":[],"errors":[]})"); + navigation_thread.join(); + + ASSERT_TRUE(result.ok()) << result.message; +} + +TEST_F( + SeerRobokitControlAuthorityTest, + SynchronousStationMapsTaskStatusSevenToFailedAfterStop) +{ + controller_.setResponsePayload( + kRobotStatusAll2, + R"({"ret_code":0,"task_status":0,"task_type":0,"blocked":false,"vx":0.0,"vy":0.0,"w":0.0,"fatals":[],"errors":[]})"); + controller_.queueResponsePayload( + kRobotStatusAll2, + R"({"ret_code":0,"task_status":7,"task_type":2,"target_id":"station-timeout","blocked":false,"vx":0.0,"vy":0.0,"w":0.0,"fatals":[],"errors":[]})"); + controller_.setResponsePayload( + kRobotStatusTaskPackage, + R"({"ret_code":0,"task_status_package":{"task_status_list":[{"task_id":"${TASK_ID}","status":7,"type":2}]}})"); + AgvMotionOptions options; + options.wait_timeout_ms = 1000; + options.poll_interval_ms = 20; + controller_.clearRecords(); + + const auto result = agv_->navigateToStation( + "station-timeout", + options); + + EXPECT_FALSE(result.ok()); + EXPECT_EQ(result.code, AgvErrorCode::TaskFailed); + EXPECT_NE(result.message.find("task_status=7"), std::string::npos); + EXPECT_NE( + result.message.find("stopped velocity confirmed"), + std::string::npos); + const auto records = controller_.records(); + EXPECT_EQ( + std::count_if( + records.begin(), + records.end(), + [](const CommandRecord& record) { + return record.command == kRobotTaskCancel; + }), + 0); +} + +TEST_F( + SeerRobokitControlAuthorityTest, + SynchronousFollowPathWaitsForFinalExactTaskAndStoppedVelocity) +{ + controller_.setResponsePayload( + kRobotStatusAll2, + R"({"ret_code":0,"task_status":0,"task_type":0,"blocked":false,"vx":0.0,"vy":0.0,"w":0.0,"fatals":[],"errors":[]})"); + controller_.queueResponsePayload( + kRobotStatusAll2, + R"({"ret_code":0,"task_status":2,"task_type":3,"target_id":"station-3","blocked":false,"vx":0.1,"vy":0.0,"w":0.0,"fatals":[],"errors":[]})"); + controller_.setResponsePayload( + kRobotStatusTaskPackage, + R"({"ret_code":0,"task_status_package":{"task_status_list":[{"task_id":"${TASK_ID_0}","status":2},{"task_id":"${TASK_ID_1}","status":1}]}})"); + AgvMotionOptions options; + options.asynchronous = false; + options.wait_timeout_ms = 3000; + options.poll_interval_ms = 20; + std::atomic finished{false}; + AgvResult result; + controller_.clearRecords(); + + std::thread navigation_thread([this, &options, &finished, &result]() { + result = agv_->followPath( + { + AgvPathSegment{"station-1", "station-2"}, + AgvPathSegment{"station-2", "station-3"}, + }, + options); + finished.store(true, std::memory_order_release); + }); + + const bool terminal_wait_observed = waitForCommandCount( + kRobotStatusAll2, + 2); + EXPECT_TRUE(terminal_wait_observed); + EXPECT_FALSE(finished.load(std::memory_order_acquire)); + + const auto records_before_completion = controller_.records(); + const auto all2_count_before_completion = static_cast( + std::count_if( + records_before_completion.begin(), + records_before_completion.end(), + [](const CommandRecord& record) { + return record.command == kRobotStatusAll2; + })); + controller_.setResponsePayload( + kRobotStatusTaskPackage, + R"({"ret_code":0,"task_status_package":{"task_status_list":[{"task_id":"${TASK_ID_0}","status":4},{"task_id":"${TASK_ID_1}","status":4}]}})"); + controller_.setResponsePayload( + kRobotStatusAll2, + R"({"ret_code":0,"task_status":4,"task_type":3,"target_id":"station-3","blocked":false,"vx":0.05,"vy":0.0,"w":0.0,"fatals":[],"errors":[]})"); + controller_.queueResponsePayload( + kRobotStatusAll2, + R"({"ret_code":0,"task_status":4,"task_type":3,"target_id":"station-3","blocked":false,"vx":0.05,"vy":0.0,"w":0.0,"fatals":[],"errors":[]})"); + const bool completed_moving_sample_observed = waitForCommandCount( + kRobotStatusAll2, + all2_count_before_completion + 2U); + EXPECT_TRUE(completed_moving_sample_observed); + EXPECT_FALSE(finished.load(std::memory_order_acquire)); + + controller_.setResponsePayload( + kRobotStatusAll2, + R"({"ret_code":0,"task_status":4,"task_type":3,"target_id":"station-3","blocked":false,"vx":0.0,"vy":0.0,"w":0.0,"fatals":[],"errors":[]})"); + navigation_thread.join(); + + ASSERT_TRUE(result.ok()) << result.message; + const auto records = controller_.records(); + const auto command = std::find_if( + records.begin(), + records.end(), + [](const CommandRecord& record) { + return record.command == kRobotTaskGoTargetList; + }); + ASSERT_NE(command, records.end()); + const auto payload = parsePayload(*command); + const auto& tasks = payloadValue(payload, "move_task_list"); + ASSERT_TRUE(tasks.isArray()); + ASSERT_EQ(tasks.size(), 2U); + const std::string first_task_id = + payloadValue(tasks[0], "task_id").asString(); + const std::string final_task_id = + payloadValue(tasks[1], "task_id").asString(); + EXPECT_FALSE(first_task_id.empty()); + EXPECT_FALSE(final_task_id.empty()); + EXPECT_NE(first_task_id, final_task_id); + + const auto status_query = std::find_if( + records.begin(), + records.end(), + [](const CommandRecord& record) { + return record.command == kRobotStatusTaskPackage; + }); + ASSERT_NE(status_query, records.end()); + const auto status_payload = parsePayload(*status_query); + const auto& requested_ids = payloadValue(status_payload, "task_ids"); + ASSERT_TRUE(requested_ids.isArray()); + ASSERT_EQ(requested_ids.size(), 2U); + EXPECT_EQ(requested_ids[0].asString(), first_task_id); + EXPECT_EQ(requested_ids[1].asString(), final_task_id); +} + +TEST_F( + SeerRobokitControlAuthorityTest, + FollowPathDoesNotFinishWhileEarlierExactSegmentIsStillActive) +{ + controller_.setResponsePayload( + kRobotStatusAll2, + R"({"ret_code":0,"task_status":0,"task_type":0,"blocked":false,"vx":0.0,"vy":0.0,"w":0.0,"fatals":[],"errors":[]})"); + controller_.queueResponsePayload( + kRobotStatusAll2, + R"({"ret_code":0,"task_status":4,"task_type":3,"target_id":"station-3","blocked":false,"vx":0.0,"vy":0.0,"w":0.0,"fatals":[],"errors":[]})"); + controller_.setResponsePayload( + kRobotStatusTaskPackage, + R"({"ret_code":0,"task_status_package":{"task_status_list":[{"task_id":"${TASK_ID_0}","status":2},{"task_id":"${TASK_ID_1}","status":4}]}})"); + AgvMotionOptions options; + options.wait_timeout_ms = 2000; + options.poll_interval_ms = 20; + std::atomic finished{false}; + AgvResult result; + controller_.clearRecords(); + + std::thread navigation_thread([this, &options, &finished, &result]() { + result = agv_->followPath( + { + AgvPathSegment{"station-1", "station-2"}, + AgvPathSegment{"station-2", "station-3"}, + }, + options); + finished.store(true, std::memory_order_release); + }); + + const bool continued_polling = waitForCommandCount( + kRobotStatusTaskPackage, + 5, + 1000); + EXPECT_TRUE(continued_polling); + EXPECT_FALSE(finished.load(std::memory_order_acquire)); + controller_.setResponsePayload( + kRobotStatusTaskPackage, + R"({"ret_code":0,"task_status_package":{"task_status_list":[{"task_id":"${TASK_ID_0}","status":4},{"task_id":"${TASK_ID_1}","status":4}]}})"); + navigation_thread.join(); + + ASSERT_TRUE(result.ok()) << result.message; +} + +TEST_F( + SeerRobokitControlAuthorityTest, + SynchronousFollowPathReconcilesIndeterminateCommandAcknowledgment) +{ + controller_.setResponsePayload( + kRobotStatusAll2, + R"({"ret_code":0,"task_status":0,"task_type":0,"blocked":false,"vx":0.0,"vy":0.0,"w":0.0,"fatals":[],"errors":[]})"); + controller_.queueResponsePayload( + kRobotStatusAll2, + R"({"ret_code":0,"task_status":2,"task_type":3,"target_id":"station-3","blocked":false,"vx":0.05,"vy":0.0,"w":0.0,"fatals":[],"errors":[]})"); + controller_.setResponsePayload( + kRobotStatusTaskPackage, + R"({"ret_code":0,"task_status_package":{"task_status_list":[{"task_id":"${TASK_ID_0}","status":2},{"task_id":"${TASK_ID_1}","status":1}]}})"); + controller_.setResponsePayload( + kRobotTaskGoTargetList, + R"({"err_msg":"path acknowledgment lost"})"); + AgvMotionOptions options; + options.wait_timeout_ms = 3000; + options.poll_interval_ms = 20; + std::atomic finished{false}; + AgvResult result; + controller_.clearRecords(); + + std::thread navigation_thread([this, &options, &finished, &result]() { + result = agv_->followPath( + { + AgvPathSegment{"station-1", "station-2"}, + AgvPathSegment{"station-2", "station-3"}, + }, + options); + finished.store(true, std::memory_order_release); + }); + + const bool path_status_observed = waitForCommandCount( + kRobotStatusTaskPackage, + 3, + 1000); + EXPECT_TRUE(path_status_observed); + EXPECT_FALSE(finished.load(std::memory_order_acquire)); + controller_.setResponsePayload( + kRobotStatusTaskPackage, + R"({"ret_code":0,"task_status_package":{"task_status_list":[{"task_id":"${TASK_ID_0}","status":4},{"task_id":"${TASK_ID_1}","status":4}]}})"); + controller_.setResponsePayload( + kRobotStatusAll2, + R"({"ret_code":0,"task_status":4,"task_type":3,"target_id":"station-3","blocked":false,"vx":0.0,"vy":0.0,"w":0.0,"fatals":[],"errors":[]})"); + navigation_thread.join(); + + ASSERT_TRUE(result.ok()) << result.message; + EXPECT_NE( + result.message.find("indeterminate command acknowledgment"), + std::string::npos); +} + +TEST_F( + SeerRobokitControlAuthorityTest, + TerminalPathTimeoutRetainsTrackingUntilStoppedVelocityIsConfirmed) +{ + controller_.setResponsePayload( + kRobotStatusAll2, + R"({"ret_code":0,"task_status":0,"task_type":0,"blocked":false,"vx":0.0,"vy":0.0,"w":0.0,"fatals":[],"errors":[]})"); + controller_.queueResponsePayload( + kRobotStatusAll2, + R"({"ret_code":0,"task_status":4,"task_type":3,"target_id":"station-3","blocked":false,"vx":0.05,"vy":0.0,"w":0.0,"fatals":[],"errors":[]})"); + controller_.setResponsePayload( + kRobotStatusTaskPackage, + R"({"ret_code":0,"task_status_package":{"task_status_list":[{"task_id":"${TASK_ID_0}","status":4},{"task_id":"${TASK_ID_1}","status":4}]}})"); + AgvMotionOptions options; + options.wait_timeout_ms = 100; + options.poll_interval_ms = 20; + std::atomic finished{false}; + AgvResult result; + controller_.clearRecords(); + + std::thread navigation_thread([this, &options, &finished, &result]() { + result = agv_->followPath( + { + AgvPathSegment{"station-1", "station-2"}, + AgvPathSegment{"station-2", "station-3"}, + }, + options); + finished.store(true, std::memory_order_release); + }); + + const bool timeout_cleanup_started = waitForCommandCount( + kRobotStatusAll2, + 2, + 1000); + EXPECT_TRUE(timeout_cleanup_started); + std::this_thread::sleep_for(std::chrono::milliseconds(150)); + EXPECT_FALSE(finished.load(std::memory_order_acquire)); + EXPECT_TRUE(SeerRobokitAgvTestPeer::hasTrackedNavigation( + *agv_, + AgvTaskType::FollowPath)); + controller_.setResponsePayload( + kRobotStatusAll2, + R"({"ret_code":0,"task_status":4,"task_type":3,"target_id":"station-3","blocked":false,"vx":0.0,"vy":0.0,"w":0.0,"fatals":[],"errors":[]})"); + navigation_thread.join(); + + EXPECT_EQ(result.code, AgvErrorCode::Timeout); + EXPECT_NE( + result.message.find("stopped state was confirmed"), + std::string::npos); + EXPECT_FALSE(SeerRobokitAgvTestPeer::hasTrackedNavigation( + *agv_, + AgvTaskType::FollowPath)); + const auto records = controller_.records(); + EXPECT_EQ( + std::count_if( + records.begin(), + records.end(), + [](const CommandRecord& record) { + return record.command == kRobotTaskClearTargetList; + }), + 0); +} + +TEST_F( + SeerRobokitControlAuthorityTest, + TerminalPoseTimeoutRetainsTrackingUntilStoppedVelocityIsConfirmed) +{ + controller_.setResponsePayload( + kRobotStatusAll2, + R"({"ret_code":0,"task_status":0,"task_type":0,"blocked":false,"vx":0.0,"vy":0.0,"w":0.0,"fatals":[],"errors":[]})"); + controller_.queueResponsePayload( + kRobotStatusAll2, + R"({"ret_code":0,"task_status":4,"task_type":1,"blocked":false,"vx":0.05,"vy":0.0,"w":0.0,"fatals":[],"errors":[]})"); + controller_.setResponsePayload( + kRobotStatusTaskPackage, + R"({"ret_code":0,"task_status_package":{"task_status_list":[{"task_id":"${TASK_ID}","status":4,"type":1}]}})"); + controller_.setResponsePayload( + kRobotStatusLoc, + R"({"ret_code":0,"x":1.0,"y":2.0,"angle":0.5})"); + AgvMotionOptions options; + options.wait_timeout_ms = 100; + options.poll_interval_ms = 20; + std::atomic finished{false}; + AgvResult result; + controller_.clearRecords(); + + std::thread navigation_thread([this, &options, &finished, &result]() { + result = agv_->navigateToPose( + math::Pose2d{1.0, 2.0, 0.5}, + options); + finished.store(true, std::memory_order_release); + }); + + const bool timeout_cleanup_started = waitForCommandCount( + kRobotStatusAll2, + 2, + 1000); + EXPECT_TRUE(timeout_cleanup_started); + std::this_thread::sleep_for(std::chrono::milliseconds(150)); + EXPECT_FALSE(finished.load(std::memory_order_acquire)); + EXPECT_TRUE(SeerRobokitAgvTestPeer::hasTrackedPoseTask(*agv_)); + EXPECT_TRUE(SeerRobokitAgvTestPeer::hasTrackedNavigation( + *agv_, + AgvTaskType::NavigateToPose)); + controller_.setResponsePayload( + kRobotStatusAll2, + R"({"ret_code":0,"task_status":4,"task_type":1,"blocked":false,"vx":0.0,"vy":0.0,"w":0.0,"fatals":[],"errors":[]})"); + navigation_thread.join(); + + EXPECT_EQ(result.code, AgvErrorCode::Timeout); + EXPECT_NE( + result.message.find("stopped state was confirmed"), + std::string::npos); + EXPECT_FALSE(SeerRobokitAgvTestPeer::hasTrackedPoseTask(*agv_)); + EXPECT_FALSE(SeerRobokitAgvTestPeer::hasTrackedNavigation( + *agv_, + AgvTaskType::NavigateToPose)); +} + +TEST_F( + SeerRobokitControlAuthorityTest, + PersistentBlockedNavigationConditionallyCancelsAndConfirmsStoppedTask) +{ + controller_.setResponsePayload( + kRobotStatusAll2, + R"({"ret_code":0,"task_status":0,"task_type":0,"blocked":false,"vx":0.0,"vy":0.0,"w":0.0,"fatals":[],"errors":[]})"); + controller_.queueResponsePayload( + kRobotStatusAll2, + R"({"ret_code":0,"task_status":2,"task_type":2,"target_id":"station-blocked","blocked":true,"block_reason":3,"block_x":1.2,"block_y":0.1,"vx":0.0,"vy":0.0,"w":0.0,"fatals":[],"errors":[]})"); + controller_.setResponsePayload( + kRobotStatusTaskPackage, + R"({"ret_code":0,"task_status_package":{"task_status_list":[{"task_id":"${TASK_ID}","status":2,"type":2}]}})"); + controller_.setResponseDelay( + kRobotTaskCancel, + std::chrono::milliseconds(50)); + AgvMotionOptions options; + options.asynchronous = false; + options.wait_timeout_ms = 3000; + options.poll_interval_ms = 20; + std::atomic finished{false}; + AgvResult result; + controller_.clearRecords(); + + std::thread navigation_thread([this, &options, &finished, &result]() { + result = agv_->navigateToStation("station-blocked", options); + finished.store(true, std::memory_order_release); + }); + + const bool cancel_observed = waitForCommandCount(kRobotTaskCancel, 1); + EXPECT_TRUE(cancel_observed); + EXPECT_FALSE(finished.load(std::memory_order_acquire)); + controller_.setResponsePayload( + kRobotStatusTaskPackage, + R"({"ret_code":0,"task_status_package":{"task_status_list":[{"task_id":"${TASK_ID}","status":6,"type":2}]}})"); + controller_.setResponsePayload( + kRobotStatusAll2, + R"({"ret_code":0,"task_status":6,"task_type":2,"target_id":"station-blocked","blocked":false,"vx":0.0,"vy":0.0,"w":0.0,"fatals":[],"errors":[]})"); + navigation_thread.join(); + + EXPECT_FALSE(result.ok()); + EXPECT_EQ(result.code, AgvErrorCode::TaskFailed); + EXPECT_NE(result.message.find("block_reason=3(collision)"), std::string::npos); + EXPECT_NE(result.message.find("canceled"), std::string::npos); + const auto records = controller_.records(); + EXPECT_EQ( + std::count_if( + records.begin(), + records.end(), + [](const CommandRecord& record) { + return record.command == kRobotTaskCancel; + }), + 1); + EXPECT_EQ( + std::count_if( + records.begin(), + records.end(), + [](const CommandRecord& record) { + return record.command == kRobotConfigLock; + }), + 2); +} + +TEST_F( + SeerRobokitControlAuthorityTest, + FollowPathUsesUniqueTaskIdsAcrossCalls) +{ + controller_.clearRecords(); + const auto first_result = agv_->followPath( + {AgvPathSegment{"station-1", "station-2"}}, + asynchronousMotionOptions()); + ASSERT_TRUE(first_result.ok()) << first_result.message; + const auto first_records = controller_.records(); + const auto first_command = std::find_if( + first_records.begin(), + first_records.end(), + [](const CommandRecord& record) { + return record.command == kRobotTaskGoTargetList; + }); + ASSERT_NE(first_command, first_records.end()); + const auto first_payload = parsePayload(*first_command); + const std::string first_task_id = payloadValue( + payloadValue(first_payload, "move_task_list")[0], + "task_id").asString(); + + controller_.clearRecords(); + const auto second_result = agv_->followPath( + {AgvPathSegment{"station-1", "station-2"}}, + asynchronousMotionOptions()); + ASSERT_TRUE(second_result.ok()) << second_result.message; + const auto second_records = controller_.records(); + const auto second_command = std::find_if( + second_records.begin(), + second_records.end(), + [](const CommandRecord& record) { + return record.command == kRobotTaskGoTargetList; + }); + ASSERT_NE(second_command, second_records.end()); + const auto second_payload = parsePayload(*second_command); + const std::string second_task_id = payloadValue( + payloadValue(second_payload, "move_task_list")[0], + "task_id").asString(); + + EXPECT_FALSE(first_task_id.empty()); + EXPECT_FALSE(second_task_id.empty()); + EXPECT_NE(first_task_id, second_task_id); +} + +TEST_F( + SeerRobokitControlAuthorityTest, + ExactTaskStatusWithoutTypeStillConfirmsAsynchronousPoseStart) +{ + controller_.setResponsePayload( + kRobotStatusTaskPackage, + R"({"ret_code":0,"task_status_package":{"task_status_list":[{"task_id":"${TASK_ID}","status":2}]}})"); + + const auto result = agv_->navigateToPose( + math::Pose2d{1.0, 2.0, 0.5}, + asynchronousMotionOptions()); + + ASSERT_TRUE(result.ok()) << result.message; +} + +TEST_F( + SeerRobokitControlAuthorityTest, + PoseStartTreatsTaskStatus404AsNotYetPresent) +{ + controller_.setResponsePayload( + kRobotStatusTaskPackage, + R"({"ret_code":0,"task_status_package":{"task_status_list":[{"task_id":"${TASK_ID}","status":404,"type":1}]}})"); + controller_.queueResponsePayload( + kRobotStatusTaskPackage, + R"({"ret_code":0,"task_status_package":{"task_status_list":[{"task_id":"${TASK_ID}","status":2,"type":1}]}})"); + controller_.clearRecords(); + + const auto result = agv_->navigateToPose( + math::Pose2d{1.0, 2.0, 0.5}, + asynchronousMotionOptions()); + + ASSERT_TRUE(result.ok()) << result.message; + const auto records = controller_.records(); + EXPECT_GE( + std::count_if( + records.begin(), + records.end(), + [](const CommandRecord& record) { + return record.command == kRobotStatusTaskPackage; + }), + 3); +} + +TEST_F( + SeerRobokitControlAuthorityTest, + PoseStartMapsControllerTaskStatusSevenToTaskFailed) +{ + controller_.setResponsePayload( + kRobotStatusTaskPackage, + R"({"ret_code":0,"task_status_package":{"info":"controller terminated free navigation","task_status_list":[{"task_id":"${TASK_ID}","status":7,"type":1}]}})"); + controller_.clearRecords(); + + const auto result = agv_->navigateToPose( + math::Pose2d{1.0, 2.0, 0.5}, + asynchronousMotionOptions()); + + EXPECT_FALSE(result.ok()); + EXPECT_EQ(result.code, AgvErrorCode::TaskFailed); + EXPECT_NE(result.message.find("controller terminated free navigation"), std::string::npos); + EXPECT_NE(result.message.find("task_status=7"), std::string::npos); + const auto records = controller_.records(); + EXPECT_EQ( + std::count_if( + records.begin(), + records.end(), + [](const CommandRecord& record) { + return record.command == kRobotTaskCancel; + }), + 0); +} + +TEST_F( + SeerRobokitControlAuthorityTest, + CancelNavigationClearsTrackedAsynchronousPathWithCommand3067) +{ + const auto path_result = agv_->followPath( + {AgvPathSegment{"station-1", "station-2"}}, + asynchronousMotionOptions()); + ASSERT_TRUE(path_result.ok()) << path_result.message; + ASSERT_TRUE(SeerRobokitAgvTestPeer::hasTrackedNavigation( + *agv_, + AgvTaskType::FollowPath)); + controller_.clearRecords(); + + const auto cancel_result = agv_->cancelNavigation(); + + ASSERT_TRUE(cancel_result.ok()) << cancel_result.message; + const auto records = controller_.records(); + EXPECT_EQ( + std::count_if( + records.begin(), + records.end(), + [](const CommandRecord& record) { + return record.command == kRobotTaskClearTargetList; + }), + 1); + EXPECT_EQ( + std::count_if( + records.begin(), + records.end(), + [](const CommandRecord& record) { + return record.command == kRobotTaskCancel; + }), + 0); +} + +TEST_F( + SeerRobokitControlAuthorityTest, + NavigationDefaultsToSynchronousAndRejectsUnboundedPolling) +{ + EXPECT_FALSE(AgvMotionOptions{}.asynchronous); + + AgvMotionOptions options; + options.poll_interval_ms = 5001; + controller_.clearRecords(); + const auto result = agv_->navigateToStation("station-1", options); + + EXPECT_FALSE(result.ok()); + EXPECT_EQ(result.code, AgvErrorCode::InvalidArgument); + EXPECT_NE(result.message.find("must not exceed 5000"), std::string::npos); + EXPECT_TRUE(controller_.records().empty()); +} + +TEST_F( + SeerRobokitControlAuthorityTest, + PreCanceledNavigationDoesNotAcquireAuthorityOrSendMotion) +{ + AgvMotionOptions options; + options.cancellation_requested = []() { return true; }; + + controller_.clearRecords(); + const auto pose_result = agv_->navigateToPose( + math::Pose2d{1.0, 2.0, 0.5}, + options); + EXPECT_EQ(pose_result.code, AgvErrorCode::TaskCanceled); + EXPECT_TRUE(controller_.records().empty()); + + const auto station_result = + agv_->navigateToStation("station-1", options); + EXPECT_EQ(station_result.code, AgvErrorCode::TaskCanceled); + EXPECT_TRUE(controller_.records().empty()); + + const auto path_result = agv_->followPath( + {AgvPathSegment{"station-1", "station-2"}}, + options); + EXPECT_EQ(path_result.code, AgvErrorCode::TaskCanceled); + EXPECT_TRUE(controller_.records().empty()); +} + +TEST_F( + SeerRobokitControlAuthorityTest, + CancellationDuringAuthorityAcquisitionPreventsNavigationWrite) +{ + std::atomic canceled{false}; + AgvMotionOptions options = asynchronousMotionOptions(); + options.cancellation_requested = [&canceled]() { + return canceled.load(std::memory_order_acquire); + }; + controller_.setResponseDelay( + kRobotConfigLock, + std::chrono::milliseconds(100)); + controller_.clearRecords(); + AgvResult result; + + std::thread navigation_thread([this, &options, &result]() { + result = agv_->navigateToStation("station-1", options); + }); + ASSERT_TRUE(waitForCommandCount(kRobotConfigLock, 1)); + canceled.store(true, std::memory_order_release); + navigation_thread.join(); + + EXPECT_EQ(result.code, AgvErrorCode::TaskCanceled); + const auto records = controller_.records(); + EXPECT_EQ( + std::count_if( + records.begin(), + records.end(), + [](const CommandRecord& record) { + return record.command == kRobotTaskGoTarget; + }), + 0); +} + +TEST_F( + SeerRobokitControlAuthorityTest, + SynchronousStationPublicCancelStillWaitsForTerminalZeroVelocity) +{ + controller_.setResponsePayload( + kRobotStatusAll2, + R"({"ret_code":0,"task_status":0,"task_type":0,"blocked":false,"vx":0.0,"vy":0.0,"w":0.0,"fatals":[],"errors":[]})"); + controller_.queueResponsePayload( + kRobotStatusAll2, + R"({"ret_code":0,"task_status":2,"task_type":2,"target_id":"station-public-cancel","blocked":false,"vx":0.05,"vy":0.0,"w":0.0,"fatals":[],"errors":[]})"); + controller_.setResponsePayload( + kRobotStatusTaskPackage, + R"({"ret_code":0,"task_status_package":{"task_status_list":[{"task_id":"${TASK_ID}","status":2,"type":2}]}})"); + std::atomic caller_canceled{false}; + AgvMotionOptions options; + options.wait_timeout_ms = 2500; + options.poll_interval_ms = 20; + options.cancellation_requested = [&caller_canceled]() { + return caller_canceled.load(std::memory_order_acquire); + }; + std::atomic finished{false}; + AgvResult result; + controller_.clearRecords(); + + std::thread navigation_thread( + [this, &options, &finished, &result]() { + result = agv_->navigateToStation( + "station-public-cancel", + options); + finished.store(true, std::memory_order_release); + }); + + const bool terminal_wait_started = waitForCommandCount( + kRobotStatusAll2, + 2, + 1500); + const auto public_cancel_result = agv_->cancelNavigation(); + caller_canceled.store(true, std::memory_order_release); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + const bool returned_while_exact_task_was_active = + finished.load(std::memory_order_acquire); + + controller_.setResponsePayload( + kRobotStatusTaskPackage, + R"({"ret_code":0,"task_status_package":{"task_status_list":[{"task_id":"${TASK_ID}","status":6,"type":2}]}})"); + controller_.setResponsePayload( + kRobotStatusAll2, + R"({"ret_code":0,"task_status":6,"task_type":2,"target_id":"station-public-cancel","blocked":false,"vx":0.0,"vy":0.0,"w":0.0,"fatals":[],"errors":[]})"); + navigation_thread.join(); + + EXPECT_TRUE(terminal_wait_started); + ASSERT_TRUE(public_cancel_result.ok()) + << public_cancel_result.message; + EXPECT_FALSE(returned_while_exact_task_was_active); + EXPECT_EQ(result.code, AgvErrorCode::TaskCanceled); + EXPECT_NE( + result.message.find("stopped state was confirmed"), + std::string::npos); + const auto records = controller_.records(); + EXPECT_GE( + std::count_if( + records.begin(), + records.end(), + [](const CommandRecord& record) { + return record.command == kRobotTaskCancel; + }), + 1); +} + +TEST_F( + SeerRobokitControlAuthorityTest, + TerminalTaskWithoutVelocityStatusEscalatesToTrackedFailSafeStop) +{ + controller_.setResponsePayload( + kRobotStatusAll2, + R"({"ret_code":0,"task_status":0,"task_type":0,"blocked":false,"vx":0.0,"vy":0.0,"w":0.0,"fatals":[],"errors":[]})"); + controller_.queueResponsePayload( + kRobotStatusAll2, + R"({"ret_code":41101,"err_msg":"1101 unavailable after terminal task"})"); + controller_.setResponsePayload( + kRobotStatusTaskPackage, + R"({"ret_code":0,"task_status_package":{"task_status_list":[{"task_id":"${TASK_ID}","status":4,"type":2}]}})"); + AgvMotionOptions options; + options.wait_timeout_ms = 100; + options.poll_interval_ms = 20; + controller_.clearRecords(); + + const auto result = agv_->navigateToStation( + "station-terminal-no-velocity", + options); + + EXPECT_FALSE(result.ok()); + EXPECT_EQ(result.code, AgvErrorCode::Timeout); + EXPECT_NE( + result.message.find("tracked fail-safe software stop"), + std::string::npos); + EXPECT_NE( + result.message.find("stopped state remains unconfirmed"), + std::string::npos); + const auto records = controller_.records(); + EXPECT_EQ( + std::count_if( + records.begin(), + records.end(), + [](const CommandRecord& record) { + return record.command == kRobotControlStop; + }), + 1); + EXPECT_EQ( + std::count_if( + records.begin(), + records.end(), + [](const CommandRecord& record) { + return record.command == kRobotTaskCancel; + }), + 1); +} + +TEST_F( + SeerRobokitControlAuthorityTest, + SynchronousPosePublicCancelStillWaitsForTerminalZeroVelocity) +{ + controller_.setResponsePayload( + kRobotStatusAll2, + R"({"ret_code":0,"task_status":0,"task_type":0,"blocked":false,"vx":0.0,"vy":0.0,"w":0.0,"fatals":[],"errors":[]})"); + controller_.queueResponsePayload( + kRobotStatusAll2, + R"({"ret_code":0,"task_status":2,"task_type":1,"blocked":false,"vx":0.05,"vy":0.0,"w":0.0,"fatals":[],"errors":[]})"); + controller_.setResponsePayload( + kRobotStatusTaskPackage, + R"({"ret_code":0,"task_status_package":{"task_status_list":[{"task_id":"${TASK_ID}","status":2,"type":1}]}})"); + std::atomic caller_canceled{false}; + AgvMotionOptions options; + options.wait_timeout_ms = 2500; + options.poll_interval_ms = 20; + options.cancellation_requested = [&caller_canceled]() { + return caller_canceled.load(std::memory_order_acquire); + }; + std::atomic finished{false}; + AgvResult result; + controller_.clearRecords(); + + std::thread navigation_thread( + [this, &options, &finished, &result]() { + result = agv_->navigateToPose( + math::Pose2d{1.0, 2.0, 0.5}, + options); + finished.store(true, std::memory_order_release); + }); + + const bool terminal_wait_started = waitForCommandCount( + kRobotStatusAll2, + 2, + 1500); + const auto public_cancel_result = agv_->cancelNavigation(); + caller_canceled.store(true, std::memory_order_release); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + const bool returned_while_exact_task_was_active = + finished.load(std::memory_order_acquire); + + controller_.setResponsePayload( + kRobotStatusTaskPackage, + R"({"ret_code":0,"task_status_package":{"task_status_list":[{"task_id":"${TASK_ID}","status":6,"type":1}]}})"); + controller_.setResponsePayload( + kRobotStatusAll2, + R"({"ret_code":0,"task_status":6,"task_type":1,"blocked":false,"vx":0.0,"vy":0.0,"w":0.0,"fatals":[],"errors":[]})"); + navigation_thread.join(); + + EXPECT_TRUE(terminal_wait_started); + ASSERT_TRUE(public_cancel_result.ok()) + << public_cancel_result.message; + EXPECT_FALSE(returned_while_exact_task_was_active); + EXPECT_EQ(result.code, AgvErrorCode::TaskCanceled); + EXPECT_NE( + result.message.find("stopped state was confirmed"), + std::string::npos); + const auto records = controller_.records(); + EXPECT_GE( + std::count_if( + records.begin(), + records.end(), + [](const CommandRecord& record) { + return record.command == kRobotTaskCancel; + }), + 1); +} + +TEST_F( + SeerRobokitControlAuthorityTest, + SynchronousStationEmergencyStopStillWaitsForTerminalZeroVelocity) +{ + controller_.setResponsePayload( + kRobotStatusAll2, + R"({"ret_code":0,"task_status":0,"task_type":0,"blocked":false,"vx":0.0,"vy":0.0,"w":0.0,"fatals":[],"errors":[]})"); + controller_.queueResponsePayload( + kRobotStatusAll2, + R"({"ret_code":0,"task_status":2,"task_type":2,"target_id":"station-emergency-stop","blocked":false,"vx":0.05,"vy":0.0,"w":0.0,"fatals":[],"errors":[]})"); + controller_.setResponsePayload( + kRobotStatusTaskPackage, + R"({"ret_code":0,"task_status_package":{"task_status_list":[{"task_id":"${TASK_ID}","status":2,"type":2}]}})"); + AgvMotionOptions options; + options.wait_timeout_ms = 2500; + options.poll_interval_ms = 20; + std::atomic finished{false}; + AgvResult result; + + std::thread navigation_thread( + [this, &options, &finished, &result]() { + result = agv_->navigateToStation( + "station-emergency-stop", + options); + finished.store(true, std::memory_order_release); + }); + + const bool terminal_wait_started = waitForCommandCount( + kRobotStatusAll2, + 2, + 1500); + const auto emergency_stop_result = agv_->emergencyStop(); + const auto records_before_terminal = controller_.records(); + const auto exact_queries_before_terminal = + static_cast(std::count_if( + records_before_terminal.begin(), + records_before_terminal.end(), + [](const CommandRecord& record) { + return record.command == kRobotStatusTaskPackage; + })); + + controller_.setResponsePayload( + kRobotStatusTaskPackage, + R"({"ret_code":0,"task_status_package":{"task_status_list":[{"task_id":"${TASK_ID}","status":6,"type":2}]}})"); + controller_.setResponsePayload( + kRobotStatusAll2, + R"({"ret_code":0,"task_status":6,"task_type":2,"target_id":"station-emergency-stop","blocked":false,"vx":0.05,"vy":0.0,"w":0.0,"fatals":[],"errors":[]})"); + const bool moving_terminal_observed = waitForCommandCount( + kRobotStatusTaskPackage, + exact_queries_before_terminal + 1U, + 1000); + std::this_thread::sleep_for(std::chrono::milliseconds(50)); + const bool returned_while_velocity_was_nonzero = + finished.load(std::memory_order_acquire); + + controller_.setResponsePayload( + kRobotStatusAll2, + R"({"ret_code":0,"task_status":6,"task_type":2,"target_id":"station-emergency-stop","blocked":false,"vx":0.0,"vy":0.0,"w":0.0,"fatals":[],"errors":[]})"); + navigation_thread.join(); + + EXPECT_TRUE(terminal_wait_started); + ASSERT_TRUE(emergency_stop_result.ok()) + << emergency_stop_result.message; + EXPECT_TRUE(moving_terminal_observed); + EXPECT_FALSE(returned_while_velocity_was_nonzero); + EXPECT_EQ(result.code, AgvErrorCode::TaskCanceled); + EXPECT_NE( + result.message.find("stopped velocity confirmed"), + std::string::npos); + const auto records = controller_.records(); + EXPECT_EQ( + std::count_if( + records.begin(), + records.end(), + [](const CommandRecord& record) { + return record.command == kRobotControlStop; + }), + 1); + EXPECT_EQ( + std::count_if( + records.begin(), + records.end(), + [](const CommandRecord& record) { + return record.command == kRobotTaskCancel; + }), + 1); +} + +TEST_F( + SeerRobokitControlAuthorityTest, + SynchronousStationStatusTimeoutStillCancelsAndReportsUnconfirmedStop) +{ + controller_.setResponsePayload( + kRobotStatusAll2, + R"({"ret_code":0,"task_status":0,"task_type":0,"blocked":false,"vx":0.0,"vy":0.0,"w":0.0,"fatals":[],"errors":[]})"); + controller_.queueResponsePayload( + kRobotStatusAll2, + R"({"ret_code":0,"task_status":2,"task_type":2,"target_id":"station-status-timeout","blocked":false,"vx":0.05,"vy":0.0,"w":0.0,"fatals":[],"errors":[]})"); + controller_.setResponsePayload( + kRobotStatusTaskPackage, + R"({"ret_code":0,"task_status_package":{"task_status_list":[{"task_id":"${TASK_ID}","status":2,"type":2}]}})"); + SeerRobokitAgvTestPeer::setStatusReceiveTimeout( + *agv_, + std::chrono::milliseconds(50)); + AgvMotionOptions options; + options.wait_timeout_ms = 2000; + options.poll_interval_ms = 100; + AgvResult result; + controller_.clearRecords(); + + std::thread navigation_thread([this, &options, &result]() { + result = agv_->navigateToStation( + "station-status-timeout", + options); + }); + + const bool running_status_observed = waitForCommandCount( + kRobotStatusAll2, + 2, + 1000); + controller_.setResponseDelay( + kRobotStatusTaskPackage, + std::chrono::milliseconds(200)); + navigation_thread.join(); + + EXPECT_TRUE(running_status_observed); + EXPECT_EQ(result.code, AgvErrorCode::Timeout); + EXPECT_NE( + result.message.find("navigation cancel was sent"), + std::string::npos); + EXPECT_NE( + result.message.find("termination could not be queried"), + std::string::npos); + EXPECT_EQ( + result.message.find("stopped state was confirmed"), + std::string::npos); + const auto records = controller_.records(); + EXPECT_GE( + std::count_if( + records.begin(), + records.end(), + [](const CommandRecord& record) { + return record.command == kRobotTaskCancel; + }), + 1); + EXPECT_EQ( + std::count_if( + records.begin(), + records.end(), + [](const CommandRecord& record) { + return record.command == kRobotControlStop; + }), + 1); +} + +TEST_F( + SeerRobokitControlAuthorityTest, + FollowPathAcceptsCurrentSegmentOnlyTaskStatusProgression) +{ + controller_.setResponsePayload( + kRobotStatusAll2, + R"({"ret_code":0,"task_status":0,"task_type":0,"blocked":false,"vx":0.0,"vy":0.0,"w":0.0,"fatals":[],"errors":[]})"); + controller_.queueResponsePayload( + kRobotStatusAll2, + R"({"ret_code":0,"task_status":2,"task_type":3,"target_id":"station-2","blocked":false,"vx":0.1,"vy":0.0,"w":0.0,"fatals":[],"errors":[]})"); + controller_.setResponsePayload( + kRobotStatusTaskPackage, + R"({"ret_code":0,"task_status_package":{"task_status_list":[{"task_id":"${TASK_ID_0}","status":2}]}})"); + controller_.queueResponsePayload( + kRobotStatusTaskPackage, + R"({"ret_code":0,"task_status_package":{"task_status_list":[]}})"); + controller_.queueResponsePayload( + kRobotStatusTaskPackage, + R"({"ret_code":0,"task_status_package":{"task_status_list":[{"task_id":"${TASK_ID_1}","status":2}]}})"); + AgvMotionOptions options; + options.wait_timeout_ms = 3000; + options.poll_interval_ms = 20; + std::atomic finished{false}; + AgvResult result; + controller_.clearRecords(); + + std::thread navigation_thread([this, &options, &finished, &result]() { + result = agv_->followPath( + { + AgvPathSegment{"station-1", "station-2"}, + AgvPathSegment{"station-2", "station-3"}, + }, + options); + finished.store(true, std::memory_order_release); + }); + + ASSERT_TRUE(waitForCommandCount(kRobotStatusTaskPackage, 3)); + EXPECT_FALSE(finished.load(std::memory_order_acquire)); + controller_.setResponsePayload( + kRobotStatusTaskPackage, + R"({"ret_code":0,"task_status_package":{"task_status_list":[{"task_id":"${TASK_ID_1}","status":4}]}})"); + controller_.setResponsePayload( + kRobotStatusAll2, + R"({"ret_code":0,"task_status":4,"task_type":3,"target_id":"station-3","blocked":false,"vx":0.0,"vy":0.0,"w":0.0,"fatals":[],"errors":[]})"); + navigation_thread.join(); + + ASSERT_TRUE(result.ok()) << result.message; + const auto records = controller_.records(); + EXPECT_EQ( + std::count_if( + records.begin(), + records.end(), + [](const CommandRecord& record) { + return record.command == kRobotTaskCancel; + }), + 0); + const auto command = std::find_if( + records.begin(), + records.end(), + [](const CommandRecord& record) { + return record.command == kRobotTaskGoTargetList; + }); + ASSERT_NE(command, records.end()); + const auto command_payload = parsePayload(*command); + const auto& tasks = payloadValue(command_payload, "move_task_list"); + ASSERT_EQ(tasks.size(), 2U); + const std::string first_id = + payloadValue(tasks[0], "task_id").asString(); + const std::string second_id = + payloadValue(tasks[1], "task_id").asString(); + for (const auto& record : records) { + if (record.command != kRobotStatusTaskPackage) { + continue; + } + const auto payload = parsePayload(record); + const auto& requested = payloadValue(payload, "task_ids"); + ASSERT_EQ(requested.size(), 2U); + EXPECT_EQ(requested[0].asString(), first_id); + EXPECT_EQ(requested[1].asString(), second_id); + } +} + +TEST_F( + SeerRobokitControlAuthorityTest, + FailedStationTaskDoesNotReturnUntilStoppedVelocityIsConfirmed) +{ + controller_.setResponsePayload( + kRobotStatusAll2, + R"({"ret_code":0,"task_status":0,"task_type":0,"blocked":false,"vx":0.0,"vy":0.0,"w":0.0,"fatals":[],"errors":[]})"); + controller_.queueResponsePayload( + kRobotStatusAll2, + R"({"ret_code":0,"task_status":5,"task_type":2,"target_id":"station-failed","blocked":false,"vx":0.1,"vy":0.0,"w":0.0,"fatals":[],"errors":[]})"); + controller_.setResponsePayload( + kRobotStatusTaskPackage, + R"({"ret_code":0,"task_status_package":{"task_status_list":[{"task_id":"${TASK_ID}","status":5}]}})"); + AgvMotionOptions options; + options.wait_timeout_ms = 3000; + options.poll_interval_ms = 20; + std::atomic finished{false}; + AgvResult result; + controller_.clearRecords(); + + std::thread navigation_thread([this, &options, &finished, &result]() { + result = agv_->navigateToStation("station-failed", options); + finished.store(true, std::memory_order_release); + }); + + ASSERT_TRUE(waitForCommandCount(kRobotStatusAll2, 2)); + EXPECT_FALSE(finished.load(std::memory_order_acquire)); + controller_.setResponsePayload( + kRobotStatusAll2, + R"({"ret_code":0,"task_status":5,"task_type":2,"target_id":"station-failed","blocked":false,"vx":0.0,"vy":0.0,"w":0.0,"fatals":[],"errors":[]})"); + navigation_thread.join(); + + EXPECT_EQ(result.code, AgvErrorCode::TaskFailed); + EXPECT_NE(result.message.find("stopped velocity confirmed"), std::string::npos); + const auto records = controller_.records(); + EXPECT_EQ( + std::count_if( + records.begin(), + records.end(), + [](const CommandRecord& record) { + return record.command == kRobotTaskCancel; + }), + 0); +} + +TEST_F( + SeerRobokitControlAuthorityTest, + FailedPathSegmentCancelsRemainingActiveSegmentsBeforeReturning) +{ + controller_.setResponsePayload( + kRobotStatusAll2, + R"({"ret_code":0,"task_status":0,"task_type":0,"blocked":false,"vx":0.0,"vy":0.0,"w":0.0,"fatals":[],"errors":[]})"); + controller_.queueResponsePayload( + kRobotStatusAll2, + R"({"ret_code":0,"task_status":2,"task_type":3,"target_id":"station-2","blocked":false,"vx":0.0,"vy":0.0,"w":0.0,"fatals":[],"errors":[]})"); + controller_.setResponsePayload( + kRobotStatusTaskPackage, + R"({"ret_code":0,"task_status_package":{"task_status_list":[{"task_id":"${TASK_ID_0}","status":5},{"task_id":"${TASK_ID_1}","status":1}]}})"); + controller_.setResponseDelay( + kRobotTaskClearTargetList, + std::chrono::milliseconds(50)); + AgvMotionOptions options; + options.wait_timeout_ms = 3000; + options.poll_interval_ms = 20; + std::atomic finished{false}; + AgvResult result; + controller_.clearRecords(); + + std::thread navigation_thread([this, &options, &finished, &result]() { + result = agv_->followPath( + { + AgvPathSegment{"station-1", "station-2"}, + AgvPathSegment{"station-2", "station-3"}, + }, + options); + finished.store(true, std::memory_order_release); + }); + + ASSERT_TRUE(waitForCommandCount(kRobotTaskClearTargetList, 1)); + EXPECT_FALSE(finished.load(std::memory_order_acquire)); + controller_.setResponsePayload( + kRobotStatusTaskPackage, + R"({"ret_code":0,"task_status_package":{"task_status_list":[{"task_id":"${TASK_ID_0}","status":5},{"task_id":"${TASK_ID_1}","status":6}]}})"); + controller_.setResponsePayload( + kRobotStatusAll2, + R"({"ret_code":0,"task_status":6,"task_type":3,"target_id":"station-3","blocked":false,"vx":0.0,"vy":0.0,"w":0.0,"fatals":[],"errors":[]})"); + navigation_thread.join(); + + EXPECT_EQ(result.code, AgvErrorCode::TaskFailed); + EXPECT_NE(result.message.find("still active"), std::string::npos); + EXPECT_NE(result.message.find("conditionally canceled"), std::string::npos); + const auto records = controller_.records(); + EXPECT_EQ( + std::count_if( + records.begin(), + records.end(), + [](const CommandRecord& record) { + return record.command == kRobotTaskClearTargetList; + }), + 1); +} + +TEST_F( + SeerRobokitControlAuthorityTest, + CurrentSegmentOnlyFailureCancelsHiddenPathAndWaitsForGlobalTerminalStop) +{ + controller_.setResponsePayload( + kRobotStatusAll2, + R"({"ret_code":0,"task_status":0,"task_type":0,"blocked":false,"vx":0.0,"vy":0.0,"w":0.0,"fatals":[],"errors":[]})"); + controller_.queueResponsePayload( + kRobotStatusAll2, + R"({"ret_code":0,"task_status":2,"task_type":3,"target_id":"station-2","blocked":false,"vx":0.0,"vy":0.0,"w":0.0,"fatals":[],"errors":[]})"); + // This firmware shape reports only the current segment. The failed first + // segment is visible, while the later queued segment is temporarily absent. + controller_.setResponsePayload( + kRobotStatusTaskPackage, + R"({"ret_code":0,"task_status_package":{"task_status_list":[{"task_id":"${TASK_ID_0}","status":5}]}})"); + controller_.setResponseDelay( + kRobotTaskClearTargetList, + std::chrono::milliseconds(100)); + AgvMotionOptions options; + options.wait_timeout_ms = 3000; + options.poll_interval_ms = 20; + std::atomic finished{false}; + AgvResult result; + controller_.clearRecords(); + + std::thread navigation_thread([this, &options, &finished, &result]() { + result = agv_->followPath( + { + AgvPathSegment{"station-1", "station-2"}, + AgvPathSegment{"station-2", "station-3"}, + }, + options); + finished.store(true, std::memory_order_release); + }); + + const bool cancel_observed = waitForCommandCount( + kRobotTaskClearTargetList, + 1, + 750); + std::size_t all2_count_at_cancel = 0; + if (cancel_observed) { + const auto records = controller_.records(); + all2_count_at_cancel = static_cast(std::count_if( + records.begin(), + records.end(), + [](const CommandRecord& record) { + return record.command == kRobotStatusAll2; + })); + + // The cancel ACK is delayed above, so these become the first + // post-cancel observations: one exact task is canceled, the other is + // still absent, and 1101 still says navigation is active but stationary. + controller_.setResponsePayload( + kRobotStatusTaskPackage, + R"({"ret_code":0,"task_status_package":{"task_status_list":[{"task_id":"${TASK_ID_0}","status":6}]}})"); + controller_.setResponsePayload( + kRobotStatusAll2, + R"({"ret_code":0,"task_status":2,"task_type":3,"target_id":"station-3","blocked":false,"vx":0.0,"vy":0.0,"w":0.0,"fatals":[],"errors":[]})"); + } + + const bool active_zero_samples_observed = cancel_observed + && waitForCommandCount( + kRobotStatusAll2, + all2_count_at_cancel + 2U, + 1500); + const bool returned_before_global_terminal = + finished.load(std::memory_order_acquire); + + // Allow both the correct and the intentionally failing implementation to + // terminate cleanly before joining the test thread. + controller_.setResponsePayload( + kRobotStatusTaskPackage, + R"({"ret_code":0,"task_status_package":{"task_status_list":[{"task_id":"${TASK_ID_0}","status":6}]}})"); + controller_.setResponsePayload( + kRobotStatusAll2, + R"({"ret_code":0,"task_status":6,"task_type":3,"target_id":"station-3","blocked":false,"vx":0.0,"vy":0.0,"w":0.0,"fatals":[],"errors":[]})"); + navigation_thread.join(); + + EXPECT_TRUE(cancel_observed); + EXPECT_TRUE(active_zero_samples_observed); + EXPECT_FALSE(returned_before_global_terminal); + EXPECT_EQ(result.code, AgvErrorCode::TaskFailed); + const auto records = controller_.records(); + EXPECT_EQ( + std::count_if( + records.begin(), + records.end(), + [](const CommandRecord& record) { + return record.command == kRobotTaskClearTargetList; + }), + 1); +} + +TEST_F( + SeerRobokitControlAuthorityTest, + ExactActivePathEscalatesToFailSafeWhenGlobalOwnershipIsUnavailable) +{ + controller_.setResponsePayload( + kRobotStatusAll2, + R"({"ret_code":0,"task_status":0,"task_type":0,"blocked":false,"vx":0.0,"vy":0.0,"w":0.0,"fatals":[],"errors":[]})"); + // If the waiter asks for 1101 after seeing exact states 5 + 1, this queued + // controller error is returned. A bare global 3067 is unsafe without the + // global ownership snapshot, so the adapter must use its explicit + // software-stop escalation sequence. + controller_.queueResponsePayload( + kRobotStatusAll2, + R"({"ret_code":41101,"err_msg":"simulated 1101 failure after exact segment failure"})"); + controller_.setResponsePayload( + kRobotStatusTaskPackage, + R"({"ret_code":0,"task_status_package":{"task_status_list":[{"task_id":"${TASK_ID_0}","status":5},{"task_id":"${TASK_ID_1}","status":1}]}})"); + controller_.setResponseDelay( + kRobotTaskClearTargetList, + std::chrono::milliseconds(100)); + AgvMotionOptions options; + options.wait_timeout_ms = 2000; + options.poll_interval_ms = 20; + AgvResult result; + controller_.clearRecords(); + + std::thread navigation_thread([this, &options, &result]() { + result = agv_->followPath( + { + AgvPathSegment{"station-1", "station-2"}, + AgvPathSegment{"station-2", "station-3"}, + }, + options); + }); + + const bool cancel_observed = waitForCommandCount( + kRobotTaskClearTargetList, + 1, + 750); + // Replace the queued failing 1101 response while the cancel ACK is delayed, + // so a correct implementation can confirm the post-cancel terminal stop. + controller_.setResponsePayload( + kRobotStatusTaskPackage, + R"({"ret_code":0,"task_status_package":{"task_status_list":[{"task_id":"${TASK_ID_0}","status":5},{"task_id":"${TASK_ID_1}","status":6}]}})"); + controller_.setResponsePayload( + kRobotStatusAll2, + R"({"ret_code":0,"task_status":6,"task_type":3,"target_id":"station-3","blocked":false,"vx":0.0,"vy":0.0,"w":0.0,"fatals":[],"errors":[]})"); + navigation_thread.join(); + + EXPECT_TRUE(cancel_observed); + EXPECT_EQ(result.code, AgvErrorCode::TaskFailed); + const auto records = controller_.records(); + EXPECT_EQ( + std::count_if( + records.begin(), + records.end(), + [](const CommandRecord& record) { + return record.command == kRobotTaskClearTargetList; + }), + 1); + EXPECT_EQ( + std::count_if( + records.begin(), + records.end(), + [](const CommandRecord& record) { + return record.command == kRobotControlStop; + }), + 1); +} + +TEST_F( + SeerRobokitControlAuthorityTest, + AsynchronousPoseCancellationAfterAckCancelsAndConfirmsStoppedTask) +{ + controller_.setResponsePayload( + kRobotStatusTaskPackage, + R"({"ret_code":0,"task_status_package":{"task_status_list":[{"task_id":"${TASK_ID}","status":2,"type":1}]}})"); + controller_.setResponseDelay( + kRobotTaskCancel, + std::chrono::milliseconds(100)); + std::atomic canceled{false}; + AgvMotionOptions options = asynchronousMotionOptions(); + options.cancellation_requested = [&canceled]() { + return canceled.load(std::memory_order_acquire); + }; + AgvResult result; + controller_.clearRecords(); + + std::thread navigation_thread([this, &options, &result]() { + result = agv_->navigateToPose( + math::Pose2d{1.0, 2.0, 0.5}, + options); + }); + + // The first 1110 request proves that 3051 already returned its ACK and the + // asynchronous pose start-confirmation phase is in progress. + const bool post_ack_status_observed = waitForCommandCount( + kRobotStatusTaskPackage, + 1, + 750); + canceled.store(true, std::memory_order_release); + const bool cancel_observed = waitForCommandCount( + kRobotTaskCancel, + 1, + 750); + + controller_.setResponsePayload( + kRobotStatusTaskPackage, + R"({"ret_code":0,"task_status_package":{"task_status_list":[{"task_id":"${TASK_ID}","status":6,"type":1}]}})"); + controller_.setResponsePayload( + kRobotStatusAll2, + R"({"ret_code":0,"task_status":6,"task_type":1,"blocked":false,"vx":0.0,"vy":0.0,"w":0.0,"fatals":[],"errors":[]})"); + navigation_thread.join(); + + EXPECT_TRUE(post_ack_status_observed); + EXPECT_TRUE(cancel_observed); + EXPECT_EQ(result.code, AgvErrorCode::TaskCanceled); + EXPECT_NE(result.message.find("conditionally canceled"), std::string::npos); + EXPECT_NE(result.message.find("stopped state was confirmed"), std::string::npos); + const auto records = controller_.records(); + EXPECT_EQ( + std::count_if( + records.begin(), + records.end(), + [](const CommandRecord& record) { + return record.command == kRobotTaskCancel; + }), + 1); +} + +TEST_F( + SeerRobokitControlAuthorityTest, + GlobalTaskStatusSevenDoesNotOverrideExactRunningSynchronousPose) +{ + controller_.setResponsePayload( + kRobotStatusAll2, + R"({"ret_code":0,"task_status":0,"task_type":0,"blocked":false,"vx":0.0,"vy":0.0,"w":0.0,"fatals":[],"errors":[]})"); + controller_.queueResponsePayload( + kRobotStatusAll2, + R"({"ret_code":0,"task_status":7,"task_type":1,"blocked":false,"vx":0.0,"vy":0.0,"w":0.0,"fatals":[],"errors":[]})"); + controller_.setResponsePayload( + kRobotStatusTaskPackage, + R"({"ret_code":0,"task_status_package":{"task_status_list":[{"task_id":"${TASK_ID}","status":2,"type":1}]}})"); + controller_.setResponsePayload( + kRobotStatusLoc, + R"({"ret_code":0,"x":1.0,"y":2.0,"angle":0.5})"); + AgvMotionOptions options; + options.wait_timeout_ms = 3000; + options.poll_interval_ms = 20; + std::atomic finished{false}; + AgvResult result; + controller_.clearRecords(); + + std::thread navigation_thread([this, &options, &finished, &result]() { + result = agv_->navigateToPose( + math::Pose2d{1.0, 2.0, 0.5}, + options); + finished.store(true, std::memory_order_release); + }); + + const bool grace_elapsed_while_exact_running = waitForCommandCount( + kRobotStatusTaskPackage, + 90, + 2500); + EXPECT_TRUE(grace_elapsed_while_exact_running); + EXPECT_FALSE(finished.load(std::memory_order_acquire)); + const auto records_before_completion = controller_.records(); + EXPECT_EQ( + std::count_if( + records_before_completion.begin(), + records_before_completion.end(), + [](const CommandRecord& record) { + return record.command == kRobotTaskCancel; + }), + 0); + controller_.setResponsePayload( + kRobotStatusTaskPackage, + R"({"ret_code":0,"task_status_package":{"task_status_list":[{"task_id":"${TASK_ID}","status":4,"type":1}]}})"); + controller_.setResponsePayload( + kRobotStatusAll2, + R"({"ret_code":0,"task_status":4,"task_type":1,"blocked":false,"vx":0.0,"vy":0.0,"w":0.0,"fatals":[],"errors":[]})"); + navigation_thread.join(); + + ASSERT_TRUE(result.ok()) << result.message; + const auto records = controller_.records(); + EXPECT_EQ( + std::count_if( + records.begin(), + records.end(), + [](const CommandRecord& record) { + return record.command == kRobotTaskCancel; + }), + 0); + EXPECT_EQ( + std::count_if( + records.begin(), + records.end(), + [](const CommandRecord& record) { + return record.command == kRobotTaskClearTargetList; + }), + 0); +} + +TEST_F( + SeerRobokitControlAuthorityTest, + CompletedPoseDoesNotFinishAfterExactTaskReturnsToRunning) +{ + controller_.setResponsePayload( + kRobotStatusAll2, + R"({"ret_code":0,"task_status":0,"task_type":0,"blocked":false,"vx":0.0,"vy":0.0,"w":0.0,"fatals":[],"errors":[]})"); + controller_.setResponsePayload( + kRobotStatusTaskPackage, + R"({"ret_code":0,"task_status_package":{"task_status_list":[{"task_id":"${TASK_ID}","status":4,"type":1}]}})"); + controller_.queueResponsePayload( + kRobotStatusTaskPackage, + R"({"ret_code":0,"task_status_package":{"task_status_list":[{"task_id":"${TASK_ID}","status":2,"type":1}]}})"); + controller_.setResponsePayload( + kRobotStatusLoc, + R"({"ret_code":0,"x":1.0,"y":2.0,"angle":0.5})"); + AgvMotionOptions options; + options.wait_timeout_ms = 3000; + options.poll_interval_ms = 20; + std::atomic finished{false}; + AgvResult result; + controller_.clearRecords(); + + std::thread navigation_thread( + [this, &options, &finished, &result]() { + result = agv_->navigateToPose( + math::Pose2d{1.0, 2.0, 0.5}, + options); + finished.store(true, std::memory_order_release); + }); + + const bool running_after_completed_observed = waitForCommandCount( + kRobotStatusTaskPackage, + 10, + 1500); + EXPECT_TRUE(running_after_completed_observed); + EXPECT_FALSE(finished.load(std::memory_order_acquire)); + controller_.setResponsePayload( + kRobotStatusTaskPackage, + R"({"ret_code":0,"task_status_package":{"task_status_list":[{"task_id":"${TASK_ID}","status":4,"type":1}]}})"); + controller_.setResponsePayload( + kRobotStatusAll2, + R"({"ret_code":0,"task_status":4,"task_type":1,"blocked":false,"vx":0.0,"vy":0.0,"w":0.0,"fatals":[],"errors":[]})"); + navigation_thread.join(); + + ASSERT_TRUE(result.ok()) << result.message; +} + +TEST_F( + SeerRobokitControlAuthorityTest, + NavigationStatusDoesNotSupersedeSynchronousPoseWaiter) +{ + controller_.setResponsePayload( + kRobotStatusAll2, + R"({"ret_code":0,"task_status":0,"task_type":0,"blocked":false,"vx":0.0,"vy":0.0,"w":0.0,"fatals":[],"errors":[]})"); + controller_.queueResponsePayload( + kRobotStatusAll2, + R"({"ret_code":0,"task_status":2,"task_type":1,"blocked":false,"vx":0.05,"vy":0.0,"w":0.0,"fatals":[],"errors":[]})"); + controller_.setResponsePayload( + kRobotStatusTaskPackage, + R"({"ret_code":0,"task_status_package":{"task_status_list":[{"task_id":"${TASK_ID}","status":2,"type":1}]}})"); + controller_.setResponsePayload( + kRobotStatusLoc, + R"({"ret_code":0,"x":1.0,"y":2.0,"angle":0.5})"); + AgvMotionOptions options; + options.wait_timeout_ms = 3000; + options.poll_interval_ms = 20; + std::atomic finished{false}; + AgvResult result; + controller_.clearRecords(); + + std::thread navigation_thread( + [this, &options, &finished, &result]() { + result = agv_->navigateToPose( + math::Pose2d{1.0, 2.0, 0.5}, + options); + finished.store(true, std::memory_order_release); + }); + + const bool terminal_wait_started = waitForCommandCount( + kRobotStatusAll2, + 2, + 2000); + const auto observed_status = agv_->navigationStatus(); + EXPECT_TRUE(terminal_wait_started); + EXPECT_EQ(observed_status.state, AgvTaskState::Running); + EXPECT_FALSE(finished.load(std::memory_order_acquire)); + controller_.setResponsePayload( + kRobotStatusTaskPackage, + R"({"ret_code":0,"task_status_package":{"task_status_list":[{"task_id":"${TASK_ID}","status":4,"type":1}]}})"); + controller_.setResponsePayload( + kRobotStatusAll2, + R"({"ret_code":0,"task_status":4,"task_type":1,"blocked":false,"vx":0.0,"vy":0.0,"w":0.0,"fatals":[],"errors":[]})"); + navigation_thread.join(); + + ASSERT_TRUE(result.ok()) << result.message; + const auto records = controller_.records(); + EXPECT_EQ( + std::count_if( + records.begin(), + records.end(), + [](const CommandRecord& record) { + return record.command == kRobotTaskCancel; + }), + 0); +} + +TEST_F( + SeerRobokitControlAuthorityTest, + ConditionalCancelRefusesDifferentActiveControllerTask) +{ + const auto navigation_result = agv_->navigateToStation( + "station-owned", + asynchronousMotionOptions()); + ASSERT_TRUE(navigation_result.ok()) << navigation_result.message; + controller_.setResponsePayload( + kRobotStatusTaskPackage, + R"({"ret_code":0,"task_status_package":{"task_status_list":[{"task_id":"${TASK_ID}","status":2,"type":2}]}})"); + controller_.setResponsePayload( + kRobotStatusAll2, + R"({"ret_code":0,"task_status":2,"task_type":3,"target_id":"station-external","blocked":false,"vx":0.05,"vy":0.0,"w":0.0,"fatals":[],"errors":[]})"); + controller_.clearRecords(); + + const auto cancel_result = agv_->cancelNavigation(); + + EXPECT_FALSE(cancel_result.ok()); + EXPECT_EQ(cancel_result.code, AgvErrorCode::TaskCanceled); + EXPECT_NE( + cancel_result.message.find("another active task"), + std::string::npos); + const auto records = controller_.records(); + EXPECT_EQ( + std::count_if( + records.begin(), + records.end(), + [](const CommandRecord& record) { + return record.command == kRobotConfigLock + || record.command == kRobotTaskCancel + || record.command == kRobotTaskClearTargetList; + }), + 0); +} + +TEST_F( + SeerRobokitControlAuthorityTest, + CanceledOldPathWithExactTerminalTasksIgnoresReplacementGlobalSnapshot) +{ + controller_.setResponsePayload( + kRobotStatusAll2, + R"({"ret_code":0,"task_status":0,"task_type":0,"blocked":false,"vx":0.0,"vy":0.0,"w":0.0,"fatals":[],"errors":[]})"); + controller_.setResponsePayload( + kRobotStatusTaskPackage, + R"({"ret_code":0,"task_status_package":{"task_status_list":[{"task_id":"${TASK_ID_0}","status":5},{"task_id":"${TASK_ID_1}","status":1}]}})"); + controller_.setResponseDelay( + kRobotTaskClearTargetList, + std::chrono::milliseconds(100)); + AgvMotionOptions options; + options.wait_timeout_ms = 3000; + options.poll_interval_ms = 20; + AgvResult old_result; + controller_.clearRecords(); + + std::thread old_navigation_thread([this, &options, &old_result]() { + old_result = agv_->followPath( + { + AgvPathSegment{"station-1", "station-2"}, + AgvPathSegment{"station-2", "station-3"}, + }, + options); + }); + + const bool cancel_observed = waitForCommandCount( + kRobotTaskClearTargetList, + 1, + 1000); + const auto records_at_cancel = controller_.records(); + const auto all2_count_at_cancel = static_cast(std::count_if( + records_at_cancel.begin(), + records_at_cancel.end(), + [](const CommandRecord& record) { + return record.command == kRobotStatusAll2; + })); + const auto exact_count_at_cancel = static_cast(std::count_if( + records_at_cancel.begin(), + records_at_cancel.end(), + [](const CommandRecord& record) { + return record.command == kRobotStatusTaskPackage; + })); + controller_.setResponsePayload( + kRobotStatusTaskPackage, + R"({"ret_code":0,"task_status_package":{"task_status_list":[{"task_id":"${TASK_ID_0}","status":6},{"task_id":"${TASK_ID_1}","status":6}]}})"); + controller_.setResponseDelay( + kRobotStatusTaskPackage, + std::chrono::milliseconds(300)); + controller_.setResponsePayload( + kRobotStatusAll2, + R"({"ret_code":41101,"err_msg":"replacement task owns global 1101"})"); + const bool post_cancel_exact_query_observed = cancel_observed + && waitForCommandCount( + kRobotStatusTaskPackage, + exact_count_at_cancel + 1U, + 1000); + + const auto replacement_result = agv_->navigateToStation( + "replacement-station", + asynchronousMotionOptions()); + old_navigation_thread.join(); + + EXPECT_TRUE(cancel_observed); + EXPECT_TRUE(post_cancel_exact_query_observed); + ASSERT_TRUE(replacement_result.ok()) << replacement_result.message; + EXPECT_EQ(old_result.code, AgvErrorCode::TaskFailed); + EXPECT_NE( + old_result.message.find("old exact task ids are terminal"), + std::string::npos); + EXPECT_NE( + old_result.message.find("global stopped state was not inspected"), + std::string::npos); + EXPECT_EQ( + old_result.message.find("stopped state was confirmed"), + std::string::npos); + EXPECT_TRUE(SeerRobokitAgvTestPeer::hasTrackedNavigation( + *agv_, + AgvTaskType::NavigateToStation)); + const auto records = controller_.records(); + EXPECT_EQ( + static_cast(std::count_if( + records.begin(), + records.end(), + [](const CommandRecord& record) { + return record.command == kRobotStatusAll2; + })), + all2_count_at_cancel); + EXPECT_EQ( + std::count_if( + records.begin(), + records.end(), + [](const CommandRecord& record) { + return record.command == kRobotTaskClearTargetList; + }), + 1); + EXPECT_EQ( + std::count_if( + records.begin(), + records.end(), + [](const CommandRecord& record) { + return record.command == kRobotTaskCancel; + }), + 0); +} + +TEST_F( + SeerRobokitControlAuthorityTest, + CurrentSegmentFailureClearsPathQueueDuringGlobalIdleTransitionWindow) +{ + controller_.setResponsePayload( + kRobotStatusAll2, + R"({"ret_code":0,"task_status":0,"task_type":0,"blocked":false,"vx":0.0,"vy":0.0,"w":0.0,"fatals":[],"errors":[]})"); + controller_.queueResponsePayload( + kRobotStatusAll2, + R"({"ret_code":0,"task_status":2,"task_type":3,"target_id":"station-3","blocked":false,"vx":0.0,"vy":0.0,"w":0.0,"fatals":[],"errors":[]})"); + controller_.setResponsePayload( + kRobotStatusTaskPackage, + R"({"ret_code":0,"task_status_package":{"task_status_list":[{"task_id":"${TASK_ID_0}","status":5}]}})"); + controller_.setResponseDelay( + kRobotConfigLock, + std::chrono::milliseconds(100)); + controller_.setResponseDelay( + kRobotTaskClearTargetList, + std::chrono::milliseconds(100)); + AgvMotionOptions options; + options.wait_timeout_ms = 3000; + options.poll_interval_ms = 20; + AgvResult result; + controller_.clearRecords(); + + std::thread navigation_thread([this, &options, &result]() { + result = agv_->followPath( + { + AgvPathSegment{"station-1", "station-2"}, + AgvPathSegment{"station-2", "station-3"}, + }, + options); + }); + + const bool cancel_authority_observed = waitForCommandCount( + kRobotConfigLock, + 2, + 1500); + controller_.setResponsePayload( + kRobotStatusAll2, + R"({"ret_code":0,"task_status":0,"task_type":0,"blocked":false,"vx":0.0,"vy":0.0,"w":0.0,"fatals":[],"errors":[]})"); + const bool clear_path_observed = waitForCommandCount( + kRobotTaskClearTargetList, + 1, + 1000); + navigation_thread.join(); + + EXPECT_TRUE(cancel_authority_observed); + EXPECT_TRUE(clear_path_observed); + EXPECT_EQ(result.code, AgvErrorCode::TaskFailed); + EXPECT_NE(result.message.find("conditionally canceled"), std::string::npos); + EXPECT_NE(result.message.find("stopped state was confirmed"), std::string::npos); + const auto records = controller_.records(); + EXPECT_EQ( + std::count_if( + records.begin(), + records.end(), + [](const CommandRecord& record) { + return record.command == kRobotTaskClearTargetList; + }), + 1); + EXPECT_EQ( + std::count_if( + records.begin(), + records.end(), + [](const CommandRecord& record) { + return record.command == kRobotTaskCancel; + }), + 0); +} + +TEST_F(SeerRobokitControlAuthorityTest, AcquisitionFailureDoesNotSendControlCommand) +{ + controller_.setResponseCode(kRobotConfigLock, 40020); + controller_.clearRecords(); + + const auto result = agv_->setVelocity(AgvVelocity{0.1, 0.0, 0.0}); + + EXPECT_FALSE(result.ok()); + EXPECT_EQ(result.code, AgvErrorCode::CommandFailed); + EXPECT_NE(result.message.find("acquire control authority failed"), std::string::npos); + EXPECT_NE(result.message.find("ret_code=40020"), std::string::npos); + const auto records = controller_.records(); + ASSERT_EQ(records.size(), 1U); + EXPECT_EQ(records[0].command, kRobotConfigLock); +} + +TEST_F(SeerRobokitControlAuthorityTest, EmergencyStopAcquisitionFailureDoesNotSendStopCommands) +{ + controller_.setResponseCode(kRobotConfigLock, 40020); + controller_.clearRecords(); + + const auto result = agv_->emergencyStop(); + + EXPECT_FALSE(result.ok()); + EXPECT_EQ(result.code, AgvErrorCode::CommandFailed); + EXPECT_NE(result.message.find("acquire control authority failed"), std::string::npos); + EXPECT_NE(result.message.find("ret_code=40020"), std::string::npos); + const auto records = controller_.records(); + ASSERT_EQ(records.size(), 1U); + EXPECT_EQ(records[0].command, kRobotConfigLock); +} + +TEST_F(SeerRobokitControlAuthorityTest, EmergencyStopAttemptsBothStopsAndAggregatesFailures) +{ + controller_.setResponseCode(kRobotControlStop, 50001); + controller_.setResponseCode(kRobotTaskCancel, 50002); + controller_.clearRecords(); + + const auto result = agv_->emergencyStop(); + + EXPECT_FALSE(result.ok()); + EXPECT_NE(result.message.find("control stop"), std::string::npos); + EXPECT_NE(result.message.find("cancel navigation"), std::string::npos); + EXPECT_NE(result.message.find("ret_code=50001"), std::string::npos); + EXPECT_NE(result.message.find("ret_code=50002"), std::string::npos); + const auto records = controller_.records(); + ASSERT_EQ(records.size(), 3U); + EXPECT_EQ(records[0].command, kRobotConfigLock); + EXPECT_EQ(records[1].command, kRobotControlStop); + EXPECT_EQ(records[2].command, kRobotTaskCancel); +} + +TEST_F(SeerRobokitControlAuthorityTest, UnsupportedClearFaultDoesNotAcquireAuthority) +{ + controller_.clearRecords(); + + const auto result = agv_->clearFault(); + + EXPECT_FALSE(result.ok()); + EXPECT_EQ(result.code, AgvErrorCode::UnsupportedCommand); + EXPECT_TRUE(controller_.records().empty()); +} + +TEST_F(SeerRobokitControlAuthorityTest, ReadOnlyMapDownloadDoesNotAcquireAuthority) +{ + controller_.clearRecords(); + std::string content; + + const auto result = agv_->downloadMap("map-1", content); + + ASSERT_TRUE(result.ok()) << result.message; + const auto records = controller_.records(); + ASSERT_EQ(records.size(), 1U); + EXPECT_EQ(records[0].command, kRobotConfigDownloadMap); +} + +TEST_F( + SeerRobokitControlAuthorityTest, + NavigateToPoseUsesLegacyCompatibleFreeGoPayloadWithTypedMotionLimits) +{ + AgvMotionOptions options; + options.asynchronous = true; + options.max_speed = 0.6; + options.max_angular_speed = 0.7; + options.max_acceleration = 0.8; + options.max_angular_acceleration = 0.9; + options.reach_distance = 0.1; + options.reach_angle = 0.2; + controller_.clearRecords(); + + const auto result = agv_->navigateToPose( + math::Pose2d{1.0, 2.0, 0.5}, + options); + + ASSERT_TRUE(result.ok()) << result.message; + const auto records = controller_.records(); + ASSERT_GE(records.size(), 4U); + EXPECT_EQ(records[0].command, kRobotConfigLock); + EXPECT_EQ(records[1].command, kRobotTaskGoTarget); + for (std::size_t index = 2; index < records.size(); ++index) { + EXPECT_EQ(records[index].command, kRobotStatusTaskPackage); + } + + const auto payload = parsePayload(records[1]); + EXPECT_EQ(payloadValue(payload, "source_id").asString(), "SELF_POSITION"); + EXPECT_EQ(payloadValue(payload, "id").asString(), ""); + EXPECT_FALSE(payloadValue(payload, "task_id").asString().empty()); + const auto& free_go = payloadValue(payload, "freeGo"); + EXPECT_TRUE(payloadValue(free_go, "x").isNumeric()); + EXPECT_TRUE(payloadValue(free_go, "y").isNumeric()); + EXPECT_TRUE(payloadValue(free_go, "theta").isNumeric()); + EXPECT_DOUBLE_EQ(payloadValue(free_go, "x").asDouble(), 1.0); + EXPECT_DOUBLE_EQ(payloadValue(free_go, "y").asDouble(), 2.0); + EXPECT_DOUBLE_EQ(payloadValue(free_go, "theta").asDouble(), 0.5); + EXPECT_DOUBLE_EQ(payloadValue(payload, "max_speed").asDouble(), 0.6); + EXPECT_DOUBLE_EQ(payloadValue(payload, "max_wspeed").asDouble(), 0.7); + EXPECT_DOUBLE_EQ(payloadValue(payload, "max_acc").asDouble(), 0.8); + EXPECT_DOUBLE_EQ(payloadValue(payload, "max_wacc").asDouble(), 0.9); + EXPECT_DOUBLE_EQ(payloadValue(payload, "reach_dist").asDouble(), 0.1); + EXPECT_DOUBLE_EQ(payloadValue(payload, "reach_angle").asDouble(), 0.2); + EXPECT_EQ( + payloadValue(payload, "skill_name").asString(), + "GotoSpecifiedPose"); + EXPECT_FALSE(payloadHas(payload, "x")); + EXPECT_FALSE(payloadHas(payload, "y")); + EXPECT_FALSE(payloadHas(payload, "angle")); + EXPECT_FALSE(payloadHas(payload, "jack_height")); + + const auto status_payload = parsePayload(records[2]); + const auto& requested_task_ids = payloadValue(status_payload, "task_ids"); + ASSERT_TRUE(requested_task_ids.isArray()); + ASSERT_EQ(requested_task_ids.size(), 1U); + ASSERT_TRUE(requested_task_ids[0].isString()); + EXPECT_EQ( + requested_task_ids[0].asString(), + payloadValue(payload, "task_id").asString()); + const auto second_status_payload = parsePayload(records.back()); + const auto& second_requested_task_ids = + payloadValue(second_status_payload, "task_ids"); + ASSERT_TRUE(second_requested_task_ids.isArray()); + ASSERT_EQ(second_requested_task_ids.size(), 1U); + EXPECT_EQ( + second_requested_task_ids[0].asString(), + payloadValue(payload, "task_id").asString()); +} + +TEST_F( + SeerRobokitControlAuthorityTest, + NavigateToPoseUsesExplicitTaskIdAsUniquePrefixAndWhitelistsAdapterFields) +{ + controller_.setResponsePayload( + kRobotStatusTaskPackage, + R"({"ret_code":0,"task_status_package":{"task_status_list":[{"task_id":"${TASK_ID}","status":2,"type":1}]}})"); + AgvAdapterParams adapter_params; + adapter_params.values.emplace("source_id", "SELF_POSITION"); + adapter_params.values.emplace("target_id", "SELF_POSITION"); + adapter_params.values.emplace("task_id", "pose-task"); + adapter_params.values.emplace("skill_name", "GotoSpecifiedPose"); + adapter_params.values.emplace("operation", "JackHeight"); + adapter_params.values.emplace("jack_height", "0.5"); + adapter_params.values.emplace("script_name", "unsafe-script"); + adapter_params.values.emplace("unknown_field", "unsafe-value"); + controller_.clearRecords(); + + const auto result = agv_->navigateToPose( + math::Pose2d{0.0, 0.0, 0.5}, + asynchronousMotionOptions(), + adapter_params); + + ASSERT_TRUE(result.ok()) << result.message; + const auto records = controller_.records(); + ASSERT_GE(records.size(), 4U); + EXPECT_EQ(records[0].command, kRobotConfigLock); + EXPECT_EQ(records[1].command, kRobotTaskGoTarget); + for (std::size_t index = 2; index < records.size(); ++index) { + EXPECT_EQ(records[index].command, kRobotStatusTaskPackage); + } + + const auto payload = parsePayload(records[1]); + const std::string first_task_id = + payloadValue(payload, "task_id").asString(); + EXPECT_EQ(payloadValue(payload, "source_id").asString(), "SELF_POSITION"); + EXPECT_EQ(payloadValue(payload, "id").asString(), ""); + EXPECT_EQ( + first_task_id.find("pose-task_pose_"), + 0U); + EXPECT_EQ(payloadValue(payload, "skill_name").asString(), "GotoSpecifiedPose"); + const auto& free_go = payloadValue(payload, "freeGo"); + EXPECT_DOUBLE_EQ(payloadValue(free_go, "x").asDouble(), 0.0); + EXPECT_DOUBLE_EQ(payloadValue(free_go, "y").asDouble(), 0.0); + EXPECT_DOUBLE_EQ(payloadValue(free_go, "theta").asDouble(), 0.5); + EXPECT_FALSE(payloadHas(payload, "operation")); + EXPECT_FALSE(payloadHas(payload, "jack_height")); + EXPECT_FALSE(payloadHas(payload, "script_name")); + EXPECT_FALSE(payloadHas(payload, "unknown_field")); + + controller_.clearRecords(); + const auto second_result = agv_->navigateToPose( + math::Pose2d{0.0, 0.0, 0.5}, + asynchronousMotionOptions(), + adapter_params); + ASSERT_TRUE(second_result.ok()) << second_result.message; + const auto second_records = controller_.records(); + ASSERT_GE(second_records.size(), 2U); + const auto second_payload = parsePayload(second_records[1]); + const std::string second_task_id = + payloadValue(second_payload, "task_id").asString(); + EXPECT_EQ(second_task_id.find("pose-task_pose_"), 0U); + EXPECT_NE(first_task_id, second_task_id); +} + +TEST_F(SeerRobokitControlAuthorityTest, NavigateToPoseRejectsUnsafeFallbackStationAndSkill) +{ + AgvAdapterParams adapter_params; + adapter_params.values.emplace("source_id", "station-0"); + controller_.clearRecords(); + + auto result = agv_->navigateToPose( + math::Pose2d{1.0, 2.0, 0.5}, + {}, + adapter_params); + + EXPECT_FALSE(result.ok()); + EXPECT_EQ(result.code, AgvErrorCode::InvalidArgument); + EXPECT_NE(result.message.find("source_id must be SELF_POSITION"), std::string::npos); + EXPECT_TRUE(controller_.records().empty()); + + adapter_params.values.clear(); + adapter_params.values.emplace("target_id", "station-1"); + controller_.clearRecords(); + + result = agv_->navigateToPose( + math::Pose2d{1.0, 2.0, 0.5}, + {}, + adapter_params); + + EXPECT_FALSE(result.ok()); + EXPECT_EQ(result.code, AgvErrorCode::InvalidArgument); + EXPECT_NE(result.message.find("target_id must be empty"), std::string::npos); + EXPECT_TRUE(controller_.records().empty()); + + adapter_params.values.clear(); + adapter_params.values.emplace("skill_name", "unsafe-custom-skill"); + controller_.clearRecords(); + + result = agv_->navigateToPose( + math::Pose2d{1.0, 2.0, 0.5}, + {}, + adapter_params); + + EXPECT_FALSE(result.ok()); + EXPECT_EQ(result.code, AgvErrorCode::InvalidArgument); + EXPECT_NE(result.message.find("skill_name must be GotoSpecifiedPose"), std::string::npos); + EXPECT_TRUE(controller_.records().empty()); +} + +TEST_F( + SeerRobokitControlAuthorityTest, + MotionCommandsRejectNonFiniteNumericInputsBeforeAcquiringAuthority) +{ + const double nan = std::numeric_limits::quiet_NaN(); + const double infinity = std::numeric_limits::infinity(); + + controller_.clearRecords(); + auto result = agv_->navigateToPose(math::Pose2d{nan, 0.0, 0.0}, asynchronousMotionOptions()); + EXPECT_FALSE(result.ok()); + EXPECT_EQ(result.code, AgvErrorCode::InvalidArgument); + EXPECT_NE(result.message.find("pose"), std::string::npos); + EXPECT_TRUE(controller_.records().empty()); + + AgvMotionOptions pose_options; + pose_options.reach_distance = infinity; + controller_.clearRecords(); + result = agv_->navigateToPose( + math::Pose2d{0.0, 0.0, 0.0}, + pose_options); + EXPECT_FALSE(result.ok()); + EXPECT_EQ(result.code, AgvErrorCode::InvalidArgument); + EXPECT_NE(result.message.find("reach_distance"), std::string::npos); + EXPECT_TRUE(controller_.records().empty()); + + AgvMotionOptions station_options; + station_options.max_acceleration = nan; + controller_.clearRecords(); + result = agv_->navigateToStation("station-1", station_options); + EXPECT_FALSE(result.ok()); + EXPECT_EQ(result.code, AgvErrorCode::InvalidArgument); + EXPECT_NE(result.message.find("max_acceleration"), std::string::npos); + EXPECT_TRUE(controller_.records().empty()); + + AgvMotionOptions negative_options; + negative_options.max_speed = -0.1; + controller_.clearRecords(); + result = agv_->navigateToPose( + math::Pose2d{0.0, 0.0, 0.0}, + negative_options); + EXPECT_FALSE(result.ok()); + EXPECT_EQ(result.code, AgvErrorCode::InvalidArgument); + EXPECT_NE(result.message.find("non-negative"), std::string::npos); + EXPECT_TRUE(controller_.records().empty()); + + AgvAdapterParams invalid_adapter; + invalid_adapter.values.emplace("jack_height", "inf"); + controller_.clearRecords(); + result = agv_->navigateToStation( + "station-1", + {}, + invalid_adapter); + EXPECT_FALSE(result.ok()); + EXPECT_EQ(result.code, AgvErrorCode::InvalidArgument); + EXPECT_NE(result.message.find("jack_height"), std::string::npos); + EXPECT_TRUE(controller_.records().empty()); + + controller_.clearRecords(); + result = agv_->setVelocity(AgvVelocity{0.0, infinity, 0.0}); + EXPECT_FALSE(result.ok()); + EXPECT_EQ(result.code, AgvErrorCode::InvalidArgument); + EXPECT_NE(result.message.find("velocity"), std::string::npos); + EXPECT_TRUE(controller_.records().empty()); +} + +TEST_F(SeerRobokitControlAuthorityTest, NavigateToPoseIgnoresUncorrelatedStatusUntilPoseTaskAppears) +{ + controller_.setResponsePayload( + kRobotStatusTaskPackage, + R"({"ret_code":0,"task_status_package":{"task_status_list":[{"task_id":"old-pose-task","status":2,"type":1}]}})"); + controller_.queueResponsePayload( + kRobotStatusTaskPackage, + R"({"ret_code":0,"task_status_package":{"task_status_list":[{"task_id":"${TASK_ID}","status":2,"type":1}]}})"); + controller_.clearRecords(); + + const auto result = agv_->navigateToPose(math::Pose2d{1.0, 2.0, 0.5}, asynchronousMotionOptions()); + + ASSERT_TRUE(result.ok()) << result.message; + const auto records = controller_.records(); + ASSERT_GE(records.size(), 5U); + EXPECT_EQ(records[0].command, kRobotConfigLock); + EXPECT_EQ(records[1].command, kRobotTaskGoTarget); + for (std::size_t index = 2; index < records.size(); ++index) { + EXPECT_EQ(records[index].command, kRobotStatusTaskPackage); + } +} + +TEST_F(SeerRobokitControlAuthorityTest, NavigateToPoseWaitsForStableRunningAfterWaiting) +{ + controller_.setResponsePayload( + kRobotStatusTaskPackage, + R"({"ret_code":0,"task_status_package":{"task_status_list":[{"task_id":"${TASK_ID}","status":1,"type":1}]}})"); + controller_.queueResponsePayload( + kRobotStatusTaskPackage, + R"({"ret_code":0,"task_status_package":{"task_status_list":[{"task_id":"${TASK_ID}","status":2,"type":1}]}})"); + controller_.clearRecords(); + + const auto result = agv_->navigateToPose(math::Pose2d{1.0, 2.0, 0.5}, asynchronousMotionOptions()); + + ASSERT_TRUE(result.ok()) << result.message; + const auto records = controller_.records(); + ASSERT_GE(records.size(), 5U); + EXPECT_EQ(records[0].command, kRobotConfigLock); + EXPECT_EQ(records[1].command, kRobotTaskGoTarget); + for (std::size_t index = 2; index < records.size(); ++index) { + EXPECT_EQ(records[index].command, kRobotStatusTaskPackage); + } +} + +TEST_F( + SeerRobokitControlAuthorityTest, + NavigateToPoseDoesNotReturnSuccessBeforeLateRunningFaultPush) +{ + controller_.clearRecords(); + AgvResult result = AgvResult::success(); + + std::thread pose_thread([this, &result]() { + result = agv_->navigateToPose(math::Pose2d{1.0, 2.0, 0.5}, asynchronousMotionOptions()); + }); + + bool status_query_observed = false; + for (int attempt = 0; attempt < 200; ++attempt) { + const auto records = controller_.records(); + status_query_observed = std::any_of( + records.begin(), + records.end(), + [](const CommandRecord& record) { + return record.command == kRobotStatusTaskPackage; + }); + if (status_query_observed) { + break; + } + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + } + EXPECT_TRUE(status_query_observed); + + Json::Value fault_push(Json::objectValue); + Json::Value errors(Json::arrayValue); + errors.append("E_RUNNING_31: safety controller rejected free navigation"); + *fault_push.demand("errors", "errors" + std::strlen("errors")) = errors; + SeerRobokitAgvTestPeer::cacheRuntimeState(*agv_, fault_push); + pose_thread.join(); + + EXPECT_FALSE(result.ok()); + EXPECT_EQ(result.code, AgvErrorCode::Fault); + EXPECT_NE(result.message.find("E_RUNNING_31"), std::string::npos); + EXPECT_NE(result.message.find("Running state"), std::string::npos); + EXPECT_NE( + result.message.find("do not retry automatically"), + std::string::npos); +} + +TEST_F( + SeerRobokitControlAuthorityTest, + NavigateToPoseFailsIfFaultPushChannelInvalidatesDuringStartConfirmation) +{ + controller_.clearRecords(); + AgvResult result = AgvResult::success(); + + std::thread pose_thread([this, &result]() { + result = agv_->navigateToPose(math::Pose2d{1.0, 2.0, 0.5}, asynchronousMotionOptions()); + }); + + bool status_query_observed = false; + for (int attempt = 0; attempt < 200; ++attempt) { + const auto records = controller_.records(); + status_query_observed = std::any_of( + records.begin(), + records.end(), + [](const CommandRecord& record) { + return record.command == kRobotStatusTaskPackage; + }); + if (status_query_observed) { + break; + } + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + } + EXPECT_TRUE(status_query_observed); + + SeerRobokitAgvTestPeer::setFaultStateUnknown(*agv_); + pose_thread.join(); + + EXPECT_FALSE(result.ok()); + EXPECT_EQ(result.code, AgvErrorCode::Fault); + EXPECT_NE( + result.message.find("fault monitoring became unavailable"), + std::string::npos); + EXPECT_NE( + result.message.find("push channel changed or was invalidated"), + std::string::npos); +} + +TEST_F( + SeerRobokitControlAuthorityTest, + NavigateToPoseRejectsCompletedTargetWhenLateControllerFaultArrives) +{ + controller_.setResponsePayload( + kRobotStatusTaskPackage, + R"({"ret_code":0,"task_status_package":{"task_status_list":[{"task_id":"${TASK_ID}","status":4,"type":1}]}})"); + controller_.setResponsePayload( + kRobotStatusLoc, + R"({"ret_code":0,"x":1.0,"y":2.0,"angle":0.5})"); + controller_.clearRecords(); + AgvResult result = AgvResult::success(); + + std::thread pose_thread([this, &result]() { + result = agv_->navigateToPose(math::Pose2d{1.0, 2.0, 0.5}, asynchronousMotionOptions()); + }); + + bool status_query_observed = false; + for (int attempt = 0; attempt < 200; ++attempt) { + const auto records = controller_.records(); + status_query_observed = std::any_of( + records.begin(), + records.end(), + [](const CommandRecord& record) { + return record.command == kRobotStatusTaskPackage; + }); + if (status_query_observed) { + break; + } + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + } + EXPECT_TRUE(status_query_observed); + + Json::Value fault_push(Json::objectValue); + Json::Value errors(Json::arrayValue); + errors.append("E_COMPLETED_45: controller rejected completed pose"); + *fault_push.demand("errors", "errors" + std::strlen("errors")) = errors; + *fault_push.demand("fatals", "fatals" + std::strlen("fatals")) = + Json::Value(Json::arrayValue); + SeerRobokitAgvTestPeer::cacheRuntimeState(*agv_, fault_push); + pose_thread.join(); + + EXPECT_FALSE(result.ok()); + EXPECT_EQ(result.code, AgvErrorCode::Fault); + EXPECT_NE(result.message.find("reported Completed"), std::string::npos); + EXPECT_NE(result.message.find("E_COMPLETED_45"), std::string::npos); + const auto records = controller_.records(); + EXPECT_FALSE(std::any_of( + records.begin(), + records.end(), + [](const CommandRecord& record) { + return record.command == kRobotStatusLoc; + })); +} + +TEST_F( + SeerRobokitControlAuthorityTest, + NavigateToPoseRejectsFaultArrivingDuringCompletedPoseVerification) +{ + controller_.setResponsePayload( + kRobotStatusTaskPackage, + R"({"ret_code":0,"task_status_package":{"task_status_list":[{"task_id":"${TASK_ID}","status":4,"type":1}]}})"); + controller_.setResponsePayload( + kRobotStatusLoc, + R"({"ret_code":0,"x":1.0,"y":2.0,"angle":0.5})"); + controller_.setResponseDelay( + kRobotStatusLoc, + std::chrono::milliseconds(300)); + controller_.clearRecords(); + AgvResult result = AgvResult::success(); + + std::thread pose_thread([this, &result]() { + result = agv_->navigateToPose(math::Pose2d{1.0, 2.0, 0.5}, asynchronousMotionOptions()); + }); + + bool location_query_observed = false; + for (int attempt = 0; attempt < 700; ++attempt) { + const auto records = controller_.records(); + location_query_observed = std::any_of( + records.begin(), + records.end(), + [](const CommandRecord& record) { + return record.command == kRobotStatusLoc; + }); + if (location_query_observed) { + break; + } + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + } + EXPECT_TRUE(location_query_observed); + + Json::Value fault_push(Json::objectValue); + Json::Value errors(Json::arrayValue); + errors.append("E_POSE_VERIFY_46: fault during target verification"); + *fault_push.demand("errors", "errors" + std::strlen("errors")) = errors; + *fault_push.demand("fatals", "fatals" + std::strlen("fatals")) = + Json::Value(Json::arrayValue); + SeerRobokitAgvTestPeer::cacheRuntimeState(*agv_, fault_push); + pose_thread.join(); + + EXPECT_FALSE(result.ok()); + EXPECT_EQ(result.code, AgvErrorCode::Fault); + EXPECT_NE(result.message.find("target verification"), std::string::npos); + EXPECT_NE(result.message.find("E_POSE_VERIFY_46"), std::string::npos); +} + +TEST_F( + SeerRobokitControlAuthorityTest, + NavigateToPoseDoesNotMisattributeFaultFromCommandAwaitingAck) +{ + controller_.clearRecords(); + AgvResult pose_result = AgvResult::success(); + AgvResult station_result = AgvResult::success(); + + std::thread pose_thread([this, &pose_result]() { + pose_result = + agv_->navigateToPose(math::Pose2d{1.0, 2.0, 0.5}, asynchronousMotionOptions()); + }); + + bool status_query_observed = false; + for (int attempt = 0; attempt < 200; ++attempt) { + const auto records = controller_.records(); + status_query_observed = std::any_of( + records.begin(), + records.end(), + [](const CommandRecord& record) { + return record.command == kRobotStatusTaskPackage; + }); + if (status_query_observed) { + break; + } + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + } + EXPECT_TRUE(status_query_observed); + + controller_.setResponseDelay( + kRobotTaskGoTarget, + std::chrono::milliseconds(300)); + std::thread station_thread([this, &station_result]() { + station_result = agv_->navigateToStation("station-1", asynchronousMotionOptions()); + }); + + bool station_command_observed = false; + for (int attempt = 0; attempt < 500; ++attempt) { + const auto records = controller_.records(); + const auto go_target_count = std::count_if( + records.begin(), + records.end(), + [](const CommandRecord& record) { + return record.command == kRobotTaskGoTarget; + }); + station_command_observed = go_target_count >= 2; + if (station_command_observed) { + break; + } + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + } + EXPECT_TRUE(station_command_observed); + + Json::Value fault_push(Json::objectValue); + Json::Value errors(Json::arrayValue); + errors.append("E_STATION_ACK_18: fault from concurrent station command"); + *fault_push.demand("errors", "errors" + std::strlen("errors")) = errors; + SeerRobokitAgvTestPeer::cacheRuntimeState(*agv_, fault_push); + + pose_thread.join(); + station_thread.join(); + + EXPECT_FALSE(pose_result.ok()); + EXPECT_EQ(pose_result.code, AgvErrorCode::Fault) + << pose_result.message; + EXPECT_NE( + pose_result.message.find("another control command attempt"), + std::string::npos); + EXPECT_NE( + pose_result.message.find("cannot be attributed"), + std::string::npos); + EXPECT_NE( + pose_result.message.find("E_STATION_ACK_18"), + std::string::npos); + ASSERT_TRUE(station_result.ok()) << station_result.message; +} + +TEST_F( + SeerRobokitControlAuthorityTest, + NavigateToPoseDoesNotMisattributeFaultObservedAfterAnotherCommandAck) +{ + controller_.clearRecords(); + AgvResult pose_result = AgvResult::success(); + + std::thread pose_thread([this, &pose_result]() { + pose_result = + agv_->navigateToPose(math::Pose2d{1.0, 2.0, 0.5}, asynchronousMotionOptions()); + }); + + bool status_query_observed = false; + for (int attempt = 0; attempt < 200; ++attempt) { + const auto records = controller_.records(); + status_query_observed = std::any_of( + records.begin(), + records.end(), + [](const CommandRecord& record) { + return record.command == kRobotStatusTaskPackage; + }); + if (status_query_observed) { + break; + } + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + } + EXPECT_TRUE(status_query_observed); + + controller_.setResponseCode(kRobotTaskGoTarget, 4188); + const auto station_result = agv_->navigateToStation("station-1", asynchronousMotionOptions()); + EXPECT_FALSE(station_result.ok()); + EXPECT_NE(station_result.message.find("ret_code=4188"), std::string::npos); + + Json::Value fault_push(Json::objectValue); + Json::Value errors(Json::arrayValue); + errors.append("E_AFTER_ACK_19: delayed station command alarm"); + *fault_push.demand("errors", "errors" + std::strlen("errors")) = errors; + *fault_push.demand("fatals", "fatals" + std::strlen("fatals")) = + Json::Value(Json::arrayValue); + SeerRobokitAgvTestPeer::cacheRuntimeState(*agv_, fault_push); + pose_thread.join(); + + EXPECT_FALSE(pose_result.ok()); + EXPECT_EQ(pose_result.code, AgvErrorCode::Fault); + EXPECT_NE( + pose_result.message.find("another control command attempt"), + std::string::npos); + EXPECT_NE( + pose_result.message.find("cannot be attributed"), + std::string::npos); + EXPECT_NE(pose_result.message.find("E_AFTER_ACK_19"), std::string::npos); +} + +TEST_F( + SeerRobokitControlAuthorityTest, + PauseDuringPoseCommandAckPreservesPublishedTaskContext) +{ + controller_.setResponseDelay( + kRobotTaskGoTarget, + std::chrono::milliseconds(300)); + controller_.clearRecords(); + AgvResult pose_result = AgvResult::success(); + AgvResult pause_result = AgvResult::failure( + AgvErrorCode::CommandFailed, + "pause not called"); + + std::thread pose_thread([this, &pose_result]() { + pose_result = + agv_->navigateToPose(math::Pose2d{1.0, 2.0, 0.5}, asynchronousMotionOptions()); + }); + + bool pose_command_observed = false; + for (int attempt = 0; attempt < 200; ++attempt) { + const auto records = controller_.records(); + pose_command_observed = std::any_of( + records.begin(), + records.end(), + [](const CommandRecord& record) { + return record.command == kRobotTaskGoTarget; + }); + if (pose_command_observed) { + break; + } + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + } + EXPECT_TRUE(pose_command_observed); + + std::thread pause_thread([this, &pause_result]() { + pause_result = agv_->pauseNavigation(); + }); + pause_thread.join(); + pose_thread.join(); + + ASSERT_TRUE(pause_result.ok()) << pause_result.message; + EXPECT_FALSE(pose_result.ok()); + EXPECT_NE(pose_result.message.find("superseded"), std::string::npos); + EXPECT_TRUE(SeerRobokitAgvTestPeer::hasTrackedPoseTask(*agv_)); +} + +TEST_F( + SeerRobokitControlAuthorityTest, + NavigateToPoseReturnsAcceptedWhenMatchingTaskRemainsQueued) +{ + controller_.setResponsePayload( + kRobotStatusTaskPackage, + R"({"ret_code":0,"task_status_package":{"info":"queued","task_status_list":[{"task_id":"${TASK_ID}","status":1,"type":1}]}})"); + controller_.clearRecords(); + + const auto result = + agv_->navigateToPose(math::Pose2d{1.0, 2.0, 0.5}, asynchronousMotionOptions()); + + ASSERT_TRUE(result.ok()) << result.message; + const auto records = controller_.records(); + const auto status_query_count = std::count_if( + records.begin(), + records.end(), + [](const CommandRecord& record) { + return record.command == kRobotStatusTaskPackage; + }); + EXPECT_GT(status_query_count, 2); +} + +TEST_F( + SeerRobokitControlAuthorityTest, + NavigateToPoseReturnsControllerFaultWhenQueuedTaskRaisesAlarm) +{ + controller_.setResponsePayload( + kRobotStatusTaskPackage, + R"({"ret_code":0,"task_status_package":{"info":"queued","task_status_list":[{"task_id":"${TASK_ID}","status":1,"type":1}]}})"); + controller_.clearRecords(); + AgvResult result = AgvResult::success(); + + std::thread pose_thread([this, &result]() { + result = agv_->navigateToPose(math::Pose2d{1.0, 2.0, 0.5}, asynchronousMotionOptions()); + }); + + bool status_query_observed = false; + for (int attempt = 0; attempt < 200; ++attempt) { + const auto records = controller_.records(); + status_query_observed = std::any_of( + records.begin(), + records.end(), + [](const CommandRecord& record) { + return record.command == kRobotStatusTaskPackage; + }); + if (status_query_observed) { + break; + } + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + } + EXPECT_TRUE(status_query_observed); + + Json::Value fault_push(Json::objectValue); + Json::Value errors(Json::arrayValue); + errors.append("E_WAIT_19: safety interlock"); + *fault_push.demand("errors", "errors" + std::strlen("errors")) = errors; + SeerRobokitAgvTestPeer::cacheRuntimeState(*agv_, fault_push); + pose_thread.join(); + + EXPECT_FALSE(result.ok()); + EXPECT_EQ(result.code, AgvErrorCode::Fault); + EXPECT_NE(result.message.find("remains active"), std::string::npos); + EXPECT_NE(result.message.find("E_WAIT_19"), std::string::npos); + EXPECT_NE(result.message.find("do not retry automatically"), std::string::npos); +} + +TEST_F( + SeerRobokitControlAuthorityTest, + NavigateToPoseDoesNotReportAcceptedAfterMatchingTaskDisappears) +{ + controller_.setResponsePayload( + kRobotStatusTaskPackage, + R"({"ret_code":0,"task_status_package":{"task_status_list":[{"task_id":"${TASK_ID}","status":1,"type":1}]}})"); + controller_.queueResponsePayload( + kRobotStatusTaskPackage, + R"({"ret_code":0,"task_status_package":{"task_status_list":[]}})"); + controller_.clearRecords(); + + const auto result = + agv_->navigateToPose(math::Pose2d{1.0, 2.0, 0.5}, asynchronousMotionOptions()); + + EXPECT_FALSE(result.ok()); + EXPECT_EQ(result.code, AgvErrorCode::TaskRejected); + EXPECT_NE( + result.message.find("not present in task_status_package"), + std::string::npos); +} + +TEST_F( + SeerRobokitControlAuthorityTest, + NavigateToPoseDoesNotHangWhenRunningTaskDisappears) +{ + controller_.setResponsePayload( + kRobotStatusTaskPackage, + R"({"ret_code":0,"task_status_package":{"task_status_list":[{"task_id":"${TASK_ID}","status":2,"type":1}]}})"); + controller_.queueResponsePayload( + kRobotStatusTaskPackage, + R"({"ret_code":0,"task_status_package":{"task_status_list":[]}})"); + controller_.clearRecords(); + + const auto started_at = std::chrono::steady_clock::now(); + const auto result = + agv_->navigateToPose(math::Pose2d{1.0, 2.0, 0.5}, asynchronousMotionOptions()); + const auto elapsed = std::chrono::duration_cast( + std::chrono::steady_clock::now() - started_at); + + EXPECT_FALSE(result.ok()); + EXPECT_EQ(result.code, AgvErrorCode::TaskRejected); + EXPECT_NE( + result.message.find("not present in task_status_package"), + std::string::npos); + EXPECT_LT(elapsed, std::chrono::seconds(3)); +} + +TEST_F( + SeerRobokitControlAuthorityTest, + NavigateToPoseReturnsFailureAfterWaitingTransitionsToFailed) +{ + controller_.setResponsePayload( + kRobotStatusTaskPackage, + R"({"ret_code":0,"task_status_package":{"task_status_list":[{"task_id":"${TASK_ID}","status":1,"type":1}]}})"); + controller_.queueResponsePayload( + kRobotStatusTaskPackage, + R"({"ret_code":0,"create_on":"2026-07-31T10:00:00Z","err_msg":"controller task failed","task_status_package":{"closest_target":"goal-7","source_name":"SELF_POSITION","target_name":"free-goal","percentage":0.0,"distance":1.4,"info":"planner rejected pose","task_status_list":[{"task_id":"${TASK_ID}","status":5,"type":1}]}})"); + controller_.clearRecords(); + + const auto result = agv_->navigateToPose(math::Pose2d{1.0, 2.0, 0.5}, asynchronousMotionOptions()); + + EXPECT_FALSE(result.ok()); + EXPECT_EQ(result.code, AgvErrorCode::TaskFailed); + EXPECT_NE(result.message.find("planner rejected pose"), std::string::npos); + EXPECT_NE(result.message.find("task_status=5"), std::string::npos); + EXPECT_NE(result.message.find("status_query_ret_code=0"), std::string::npos); + EXPECT_NE(result.message.find("controller task failed"), std::string::npos); + EXPECT_NE(result.message.find("closest_target=goal-7"), std::string::npos); + EXPECT_NE(result.message.find("distance=1.400000"), std::string::npos); + const auto records = controller_.records(); + EXPECT_GE( + std::count_if( + records.begin(), + records.end(), + [](const CommandRecord& record) { + return record.command == kRobotStatusTaskPackage; + }), + 2); + EXPECT_EQ( + std::count_if( + records.begin(), + records.end(), + [](const CommandRecord& record) { + return record.command == kRobotTaskCancel; + }), + 0); +} + +TEST_F( + SeerRobokitControlAuthorityTest, + NavigateToPoseRejectsCompletedTaskWhenRequestedTargetWasNotReached) +{ + controller_.setResponsePayload( + kRobotStatusTaskPackage, + R"({"ret_code":0,"task_status_package":{"task_status_list":[{"task_id":"${TASK_ID}","status":4,"type":1}]}})"); + controller_.setResponsePayload( + kRobotStatusLoc, + R"({"ret_code":0,"x":0.0,"y":0.0,"angle":0.0})"); + controller_.clearRecords(); + + const auto result = agv_->navigateToPose(math::Pose2d{1.0, 0.0, 0.0}, asynchronousMotionOptions()); + + EXPECT_FALSE(result.ok()); + EXPECT_EQ(result.code, AgvErrorCode::TaskRejected); + EXPECT_NE(result.message.find("target was not reached"), std::string::npos); + EXPECT_NE(result.message.find("distance_error=1"), std::string::npos); + const auto records = controller_.records(); + EXPECT_GE( + std::count_if( + records.begin(), + records.end(), + [](const CommandRecord& record) { + return record.command == kRobotStatusTaskPackage; + }), + 2); + EXPECT_GE( + std::count_if( + records.begin(), + records.end(), + [](const CommandRecord& record) { + return record.command == kRobotStatusLoc; + }), + 1); + EXPECT_EQ( + std::count_if( + records.begin(), + records.end(), + [](const CommandRecord& record) { + return record.command == kRobotTaskCancel; + }), + 0); +} + +TEST_F( + SeerRobokitControlAuthorityTest, + NavigateToPoseAcceptsCompletedTaskOnlyWhenRequestedTargetWasReached) +{ + controller_.setResponsePayload( + kRobotStatusTaskPackage, + R"({"ret_code":0,"task_status_package":{"task_status_list":[{"task_id":"${TASK_ID}","status":4,"type":1}]}})"); + controller_.setResponsePayload( + kRobotStatusLoc, + R"({"ret_code":0,"x":1.0,"y":2.0,"angle":0.5})"); + controller_.clearRecords(); + + const auto result = agv_->navigateToPose(math::Pose2d{1.0, 2.0, 0.5}, asynchronousMotionOptions()); + + ASSERT_TRUE(result.ok()) << result.message; + const auto records = controller_.records(); + ASSERT_EQ(records.size(), 4U); + EXPECT_EQ(records[2].command, kRobotStatusTaskPackage); + EXPECT_EQ(records[3].command, kRobotStatusLoc); +} + +TEST_F( + SeerRobokitControlAuthorityTest, + NavigateToPoseDoesNotUseFaultOnlyPushAsAValidCompletedPose) +{ + Json::Value push_payload(Json::objectValue); + Json::Value errors(Json::arrayValue); + errors.append("controller fault without pose fields"); + *push_payload.demand("errors", "errors" + std::strlen("errors")) = errors; + SeerRobokitAgvTestPeer::cacheRuntimeState(*agv_, push_payload); + Json::Value cleared_push(Json::objectValue); + *cleared_push.demand("errors", "errors" + std::strlen("errors")) = + Json::Value(Json::arrayValue); + *cleared_push.demand("fatals", "fatals" + std::strlen("fatals")) = + Json::Value(Json::arrayValue); + SeerRobokitAgvTestPeer::cacheRuntimeState(*agv_, cleared_push); + controller_.setResponsePayload( + kRobotStatusTaskPackage, + R"({"ret_code":0,"task_status_package":{"task_status_list":[{"task_id":"${TASK_ID}","status":4,"type":1}]}})"); + controller_.setResponsePayload( + kRobotStatusLoc, + R"({"ret_code":0,"err_msg":""})"); + controller_.clearRecords(); + + const auto result = agv_->navigateToPose(math::Pose2d{0.0, 0.0, 0.0}, asynchronousMotionOptions()); + + EXPECT_FALSE(result.ok()); + EXPECT_EQ(result.code, AgvErrorCode::TaskRejected); + EXPECT_NE( + result.message.find("target pose could not be verified"), + std::string::npos); + EXPECT_NE( + result.message.find("did not contain numeric x/y/angle"), + std::string::npos); +} + +TEST_F( + SeerRobokitControlAuthorityTest, + NavigateToPoseRejectsCompletedTaskWithNonNumericControllerPose) +{ + controller_.setResponsePayload( + kRobotStatusTaskPackage, + R"({"ret_code":0,"task_status_package":{"task_status_list":[{"task_id":"${TASK_ID}","status":4,"type":1}]}})"); + controller_.setResponsePayload( + kRobotStatusLoc, + R"({"ret_code":0,"x":null,"y":false,"angle":"0.0"})"); + controller_.clearRecords(); + + const auto result = agv_->navigateToPose(math::Pose2d{0.0, 0.0, 0.0}, asynchronousMotionOptions()); + + EXPECT_FALSE(result.ok()); + EXPECT_EQ(result.code, AgvErrorCode::TaskRejected); + EXPECT_NE( + result.message.find("did not contain numeric x/y/angle"), + std::string::npos); +} + +TEST_F(SeerRobokitControlAuthorityTest, NavigateToPoseReturnsAsynchronousControllerFailure) +{ + controller_.setResponsePayload( + kRobotStatusTaskPackage, + R"({"ret_code":0,"task_status_package":{"info":"planner rejected pose","task_status_list":[{"task_id":"${TASK_ID}","status":5,"type":1}]}})"); + controller_.clearRecords(); + + const auto result = agv_->navigateToPose(math::Pose2d{1.0, 2.0, 0.5}, asynchronousMotionOptions()); + + EXPECT_FALSE(result.ok()); + EXPECT_EQ(result.code, AgvErrorCode::TaskFailed); + EXPECT_NE(result.message.find("planner rejected pose"), std::string::npos); + EXPECT_NE(result.message.find("task_status=5"), std::string::npos); + EXPECT_NE(result.message.find("task_type=1"), std::string::npos); +} + +TEST_F(SeerRobokitControlAuthorityTest, NavigateToPosePreservesSynchronousControllerCode) +{ + controller_.setResponseCode(kRobotTaskGoTarget, 43051); + controller_.clearRecords(); + + const auto result = agv_->navigateToPose(math::Pose2d{1.0, 2.0, 0.5}, asynchronousMotionOptions()); + + EXPECT_FALSE(result.ok()); + EXPECT_EQ(result.code, AgvErrorCode::CommandFailed); + EXPECT_NE(result.message.find("ret_code=43051"), std::string::npos); + EXPECT_NE(result.message.find("err_msg=simulated command failure"), std::string::npos); + const auto records = controller_.records(); + ASSERT_EQ(records.size(), 2U); + EXPECT_EQ(records[0].command, kRobotConfigLock); + EXPECT_EQ(records[1].command, kRobotTaskGoTarget); +} + +TEST_F(SeerRobokitControlAuthorityTest, NavigateToPosePreservesStatusQueryControllerCode) +{ + controller_.setResponseCode(kRobotStatusTaskPackage, 41110); + controller_.clearRecords(); + + const auto result = agv_->navigateToPose(math::Pose2d{1.0, 2.0, 0.5}, asynchronousMotionOptions()); + + EXPECT_FALSE(result.ok()); + EXPECT_EQ(result.code, AgvErrorCode::CommandFailed); + EXPECT_NE(result.message.find("ret_code=41110"), std::string::npos); + EXPECT_NE(result.message.find("err_msg=simulated command failure"), std::string::npos); +} + +TEST_F(SeerRobokitControlAuthorityTest, NavigateToPoseRejectsWrongResponseCommand) +{ + controller_.setResponseCommand(kRobotTaskGoTarget, 13052); + controller_.clearRecords(); + + const auto result = agv_->navigateToPose(math::Pose2d{1.0, 2.0, 0.5}, asynchronousMotionOptions()); + + EXPECT_FALSE(result.ok()); + EXPECT_EQ(result.code, AgvErrorCode::CommandFailed); + EXPECT_NE(result.message.find("expected=13051"), std::string::npos); + EXPECT_NE(result.message.find("actual=13052"), std::string::npos); + EXPECT_NE(result.message.find("channel closed"), std::string::npos); + EXPECT_NE(result.message.find("controller outcome is unknown"), std::string::npos); + const auto records = controller_.records(); + ASSERT_EQ(records.size(), 2U); + EXPECT_EQ(records[1].command, kRobotTaskGoTarget); +} + +TEST_F(SeerRobokitControlAuthorityTest, NavigateToPoseRejectsMissingControllerCode) +{ + controller_.setResponsePayload( + kRobotTaskGoTarget, + R"({"err_msg":"missing acknowledgment code"})"); + controller_.clearRecords(); + + const auto result = agv_->navigateToPose(math::Pose2d{1.0, 2.0, 0.5}, asynchronousMotionOptions()); + + EXPECT_FALSE(result.ok()); + EXPECT_EQ(result.code, AgvErrorCode::CommandFailed); + EXPECT_NE(result.message.find("missing a numeric ret_code"), std::string::npos); + EXPECT_NE(result.message.find("controller outcome is unknown"), std::string::npos); + const auto records = controller_.records(); + ASSERT_EQ(records.size(), 2U); + EXPECT_EQ(records[1].command, kRobotTaskGoTarget); +} + +TEST_F(SeerRobokitControlAuthorityTest, NavigateToPoseReportsPausedTaskExplicitly) +{ + controller_.setResponsePayload( + kRobotStatusTaskPackage, + R"({"ret_code":0,"task_status_package":{"info":"safety pause","task_status_list":[{"task_id":"${TASK_ID}","status":3,"type":1}]}})"); + controller_.clearRecords(); + + const auto result = agv_->navigateToPose(math::Pose2d{1.0, 2.0, 0.5}, asynchronousMotionOptions()); + + EXPECT_FALSE(result.ok()); + EXPECT_EQ(result.code, AgvErrorCode::TaskRejected); + EXPECT_NE(result.message.find("established but is paused"), std::string::npos); + EXPECT_NE(result.message.find("safety pause"), std::string::npos); + EXPECT_NE(result.message.find("do not retry automatically"), std::string::npos); +} + +TEST_F( + SeerRobokitControlAuthorityTest, + NavigateToPosePreservesControllerFaultWhenTaskImmediatelyPauses) +{ + controller_.setResponsePayload( + kRobotStatusTaskPackage, + R"({"ret_code":0,"task_status_package":{"info":"safety pause","task_status_list":[{"task_id":"${TASK_ID}","status":3,"type":1}]}})"); + controller_.clearRecords(); + AgvResult result = AgvResult::success(); + + std::thread pose_thread([this, &result]() { + result = agv_->navigateToPose(math::Pose2d{1.0, 2.0, 0.5}, asynchronousMotionOptions()); + }); + + bool status_query_observed = false; + for (int attempt = 0; attempt < 200; ++attempt) { + const auto records = controller_.records(); + status_query_observed = std::any_of( + records.begin(), + records.end(), + [](const CommandRecord& record) { + return record.command == kRobotStatusTaskPackage; + }); + if (status_query_observed) { + break; + } + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + } + EXPECT_TRUE(status_query_observed); + + Json::Value fault_push(Json::objectValue); + Json::Value errors(Json::arrayValue); + errors.append("E_PAUSED_55: safety controller paused failed task"); + *fault_push.demand("errors", "errors" + std::strlen("errors")) = errors; + *fault_push.demand("fatals", "fatals" + std::strlen("fatals")) = + Json::Value(Json::arrayValue); + SeerRobokitAgvTestPeer::cacheRuntimeState(*agv_, fault_push); + pose_thread.join(); + + EXPECT_FALSE(result.ok()); + EXPECT_EQ(result.code, AgvErrorCode::Fault); + EXPECT_NE(result.message.find("paused"), std::string::npos); + EXPECT_NE(result.message.find("E_PAUSED_55"), std::string::npos); +} + +TEST_F( + SeerRobokitControlAuthorityTest, + NavigateToPoseIncludesTaskCorrelatedRawControllerFaultDetail) +{ + controller_.setResponsePayload( + kRobotStatusTaskPackage, + R"({"ret_code":0,"task_status_package":{"task_status_list":[]}})"); + controller_.clearRecords(); + AgvResult result = AgvResult::success(); + + std::thread pose_thread([this, &result]() { + result = agv_->navigateToPose(math::Pose2d{1.0, 2.0, 0.5}, asynchronousMotionOptions()); + }); + + bool status_query_observed = false; + for (int attempt = 0; attempt < 200; ++attempt) { + const auto records = controller_.records(); + status_query_observed = std::any_of( + records.begin(), + records.end(), + [](const CommandRecord& record) { + return record.command == kRobotStatusTaskPackage; + }); + if (status_query_observed) { + break; + } + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + } + EXPECT_TRUE(status_query_observed); + + Json::Value fault_push(Json::objectValue); + Json::Value errors(Json::arrayValue); + errors.append("E_NAV_42: planner alarm"); + *fault_push.demand("errors", "errors" + std::strlen("errors")) = errors; + SeerRobokitAgvTestPeer::cacheRuntimeState(*agv_, fault_push); + + Json::Value cleared_push(Json::objectValue); + *cleared_push.demand("errors", "errors" + std::strlen("errors")) = + Json::Value(Json::arrayValue); + *cleared_push.demand("fatals", "fatals" + std::strlen("fatals")) = + Json::Value(Json::arrayValue); + SeerRobokitAgvTestPeer::cacheRuntimeState(*agv_, cleared_push); + controller_.setResponsePayload( + kRobotStatusTaskPackage, + R"({"ret_code":0,"task_status_package":{"info":"navigation failed","task_status_list":[{"task_id":"${TASK_ID}","status":5,"type":1}]}})"); + pose_thread.join(); + + EXPECT_FALSE(result.ok()); + EXPECT_EQ(result.code, AgvErrorCode::TaskFailed); + EXPECT_NE(result.message.find("E_NAV_42"), std::string::npos); + EXPECT_NE(result.message.find("planner alarm"), std::string::npos); +} + +TEST_F( + SeerRobokitControlAuthorityTest, + NavigateToPoseRejectsPreexistingControllerFaultWithoutSendingTask) +{ + Json::Value push_payload(Json::objectValue); + Json::Value errors(Json::arrayValue); + errors.append("OLD_FAULT_FROM_PREVIOUS_TASK"); + *push_payload.demand("errors", "errors" + std::strlen("errors")) = errors; + SeerRobokitAgvTestPeer::cacheRuntimeState(*agv_, push_payload); + controller_.clearRecords(); + const auto result = + agv_->navigateToPose(math::Pose2d{1.0, 2.0, 0.5}, asynchronousMotionOptions()); + + EXPECT_FALSE(result.ok()); + EXPECT_EQ(result.code, AgvErrorCode::Fault); + EXPECT_NE( + result.message.find("OLD_FAULT_FROM_PREVIOUS_TASK"), + std::string::npos); + EXPECT_NE(result.message.find("was not sent"), std::string::npos); + const auto records = controller_.records(); + ASSERT_EQ(records.size(), 1U); + EXPECT_EQ(records[0].command, kRobotConfigLock); +} + +TEST_F( + SeerRobokitControlAuthorityTest, + NavigateToPoseRejectsUnknownOrStaleFaultStateWithoutSendingTask) +{ + SeerRobokitAgvTestPeer::setFaultStateUnknown(*agv_); + controller_.clearRecords(); + + auto result = + agv_->navigateToPose(math::Pose2d{1.0, 2.0, 0.5}, asynchronousMotionOptions()); + + EXPECT_FALSE(result.ok()); + EXPECT_EQ(result.code, AgvErrorCode::Fault); + EXPECT_NE( + result.message.find("no state push containing fatals/errors"), + std::string::npos); + auto records = controller_.records(); + ASSERT_EQ(records.size(), 1U); + EXPECT_EQ(records[0].command, kRobotConfigLock); + + SeerRobokitAgvTestPeer::setFaultStateAge( + *agv_, + std::chrono::seconds(3)); + controller_.clearRecords(); + + result = agv_->navigateToPose(math::Pose2d{1.0, 2.0, 0.5}, asynchronousMotionOptions()); + + EXPECT_FALSE(result.ok()); + EXPECT_EQ(result.code, AgvErrorCode::Fault); + EXPECT_NE(result.message.find("state push is stale"), std::string::npos); + records = controller_.records(); + ASSERT_EQ(records.size(), 1U); + EXPECT_EQ(records[0].command, kRobotConfigLock); +} + +TEST_F( + SeerRobokitControlAuthorityTest, + NavigateToPoseRejectsMalformedFaultStateWithoutSendingTask) +{ + Json::Value malformed_push(Json::objectValue); + *malformed_push.demand("errors", "errors" + std::strlen("errors")) = + Json::Value(); + *malformed_push.demand("fatals", "fatals" + std::strlen("fatals")) = + Json::Value(Json::arrayValue); + SeerRobokitAgvTestPeer::cacheRuntimeState(*agv_, malformed_push); + controller_.clearRecords(); + + const auto result = + agv_->navigateToPose(math::Pose2d{1.0, 2.0, 0.5}, asynchronousMotionOptions()); + + EXPECT_FALSE(result.ok()); + EXPECT_EQ(result.code, AgvErrorCode::Fault); + EXPECT_NE( + result.message.find("incomplete or malformed"), + std::string::npos); + EXPECT_NE(result.message.find("errors=null"), std::string::npos) + << result.message; + const auto records = controller_.records(); + ASSERT_EQ(records.size(), 1U); + EXPECT_EQ(records[0].command, kRobotConfigLock); +} + +TEST_F( + SeerRobokitControlAuthorityTest, + NavigateToPoseDoesNotAttributeClearedFaultHistoryToNewTask) +{ + Json::Value fault_push(Json::objectValue); + Json::Value errors(Json::arrayValue); + errors.append("OLD_CLEARED_FAULT"); + *fault_push.demand("errors", "errors" + std::strlen("errors")) = errors; + SeerRobokitAgvTestPeer::cacheRuntimeState(*agv_, fault_push); + + Json::Value cleared_push(Json::objectValue); + *cleared_push.demand("errors", "errors" + std::strlen("errors")) = + Json::Value(Json::arrayValue); + *cleared_push.demand("fatals", "fatals" + std::strlen("fatals")) = + Json::Value(Json::arrayValue); + SeerRobokitAgvTestPeer::cacheRuntimeState(*agv_, cleared_push); + controller_.setResponsePayload( + kRobotStatusTaskPackage, + R"({"ret_code":0,"task_status_package":{"info":"new task failed","task_status_list":[{"task_id":"${TASK_ID}","status":5,"type":1}]}})"); + controller_.clearRecords(); + + const auto result = + agv_->navigateToPose(math::Pose2d{1.0, 2.0, 0.5}, asynchronousMotionOptions()); + + EXPECT_FALSE(result.ok()); + EXPECT_EQ(result.code, AgvErrorCode::TaskFailed); + EXPECT_NE(result.message.find("new task failed"), std::string::npos); + EXPECT_EQ(result.message.find("OLD_CLEARED_FAULT"), std::string::npos); +} + +TEST_F( + SeerRobokitControlAuthorityTest, + CancelDoesNotSupersedeAlreadyCompletedPoseDuringLocationVerification) +{ + controller_.setResponsePayload( + kRobotStatusTaskPackage, + R"({"ret_code":0,"task_status_package":{"task_status_list":[{"task_id":"${TASK_ID}","status":4,"type":1}]}})"); + controller_.setResponsePayload( + kRobotStatusLoc, + R"({"ret_code":0,"x":1.0,"y":2.0,"angle":0.5})"); + controller_.setResponseDelay( + kRobotStatusLoc, + std::chrono::milliseconds(300)); + controller_.clearRecords(); + AgvResult pose_result = AgvResult::success(); + + std::thread pose_thread([this, &pose_result]() { + pose_result = agv_->navigateToPose(math::Pose2d{1.0, 2.0, 0.5}, asynchronousMotionOptions()); + }); + + bool location_query_observed = false; + for (int attempt = 0; attempt < 500; ++attempt) { + const auto records = controller_.records(); + location_query_observed = std::any_of( + records.begin(), + records.end(), + [](const CommandRecord& record) { + return record.command == kRobotStatusLoc; + }); + if (location_query_observed) { + break; + } + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + } + EXPECT_TRUE(location_query_observed); + + const auto cancel_result = agv_->cancelNavigation(); + pose_thread.join(); + + ASSERT_TRUE(cancel_result.ok()) << cancel_result.message; + ASSERT_TRUE(pose_result.ok()) << pose_result.message; + const auto records = controller_.records(); + EXPECT_EQ( + std::count_if( + records.begin(), + records.end(), + [](const CommandRecord& record) { + return record.command == kRobotTaskCancel; + }), + 0); +} + +TEST_F(SeerRobokitControlAuthorityTest, CancelSupersedesPoseStartConfirmation) +{ + controller_.setResponsePayload( + kRobotStatusTaskPackage, + R"({"ret_code":0,"task_status_package":{"task_status_list":[]}})"); + controller_.clearRecords(); + AgvResult pose_result = AgvResult::success(); + + std::thread pose_thread([this, &pose_result]() { + pose_result = agv_->navigateToPose(math::Pose2d{1.0, 2.0, 0.5}, asynchronousMotionOptions()); + }); + + bool status_query_observed = false; + for (int attempt = 0; attempt < 200; ++attempt) { + const auto records = controller_.records(); + status_query_observed = std::any_of( + records.begin(), + records.end(), + [](const CommandRecord& record) { + return record.command == kRobotStatusTaskPackage; + }); + if (status_query_observed) { + break; + } + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + } + + const auto cancel_result = agv_->cancelNavigation(); + pose_thread.join(); + + EXPECT_TRUE(status_query_observed); + ASSERT_TRUE(cancel_result.ok()) << cancel_result.message; + EXPECT_FALSE(pose_result.ok()); + EXPECT_EQ(pose_result.code, AgvErrorCode::CommandFailed); + EXPECT_NE(pose_result.message.find("superseded"), std::string::npos); + EXPECT_NE(pose_result.message.find("state is unknown"), std::string::npos); +} + +TEST_F(SeerRobokitControlAuthorityTest, FailedCancelDoesNotSupersedePoseStartConfirmation) +{ + controller_.setResponsePayload( + kRobotStatusTaskPackage, + R"({"ret_code":0,"task_status_package":{"task_status_list":[]}})"); + controller_.clearRecords(); + AgvResult pose_result = AgvResult::success(); + std::atomic_bool pose_finished{false}; + + std::thread pose_thread([this, &pose_result, &pose_finished]() { + pose_result = agv_->navigateToPose(math::Pose2d{1.0, 2.0, 0.5}, asynchronousMotionOptions()); + pose_finished.store(true, std::memory_order_release); + }); + + bool status_query_observed = false; + for (int attempt = 0; attempt < 200; ++attempt) { + const auto records = controller_.records(); + status_query_observed = std::any_of( + records.begin(), + records.end(), + [](const CommandRecord& record) { + return record.command == kRobotStatusTaskPackage; + }); + if (status_query_observed) { + break; + } + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + } + EXPECT_TRUE(status_query_observed); + + controller_.setResponseCode(kRobotConfigLock, 17); + const auto authority_failure = agv_->cancelNavigation(); + EXPECT_FALSE(authority_failure.ok()); + std::this_thread::sleep_for(std::chrono::milliseconds(75)); + EXPECT_FALSE(pose_finished.load(std::memory_order_acquire)); + + controller_.setResponseCode(kRobotConfigLock, 0); + controller_.setResponseCode(kRobotTaskCancel, 23); + const auto command_failure = agv_->cancelNavigation(); + EXPECT_FALSE(command_failure.ok()); + std::this_thread::sleep_for(std::chrono::milliseconds(75)); + EXPECT_FALSE(pose_finished.load(std::memory_order_acquire)); + + controller_.setResponseCode(kRobotTaskCancel, 0); + const auto successful_cancel = agv_->cancelNavigation(); + pose_thread.join(); + + ASSERT_TRUE(successful_cancel.ok()) << successful_cancel.message; + EXPECT_TRUE(pose_finished.load(std::memory_order_acquire)); + EXPECT_FALSE(pose_result.ok()); + EXPECT_EQ(pose_result.code, AgvErrorCode::CommandFailed); + EXPECT_NE(pose_result.message.find("superseded"), std::string::npos); + EXPECT_NE(pose_result.message.find("state is unknown"), std::string::npos); +} + +TEST_F(SeerRobokitControlAuthorityTest, IndeterminateCancelSupersedesPoseStartConfirmation) +{ + controller_.setResponsePayload( + kRobotStatusTaskPackage, + R"({"ret_code":0,"task_status_package":{"task_status_list":[]}})"); + controller_.clearRecords(); + AgvResult pose_result = AgvResult::success(); + + std::thread pose_thread([this, &pose_result]() { + pose_result = agv_->navigateToPose(math::Pose2d{1.0, 2.0, 0.5}, asynchronousMotionOptions()); + }); + + bool status_query_observed = false; + for (int attempt = 0; attempt < 200; ++attempt) { + const auto records = controller_.records(); + status_query_observed = std::any_of( + records.begin(), + records.end(), + [](const CommandRecord& record) { + return record.command == kRobotStatusTaskPackage; + }); + if (status_query_observed) { + break; + } + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + } + EXPECT_TRUE(status_query_observed); + + controller_.setResponsePayload( + kRobotTaskCancel, + R"({"err_msg":"acknowledgment lost"})"); + const auto cancel_result = agv_->cancelNavigation(); + pose_thread.join(); + + EXPECT_FALSE(cancel_result.ok()); + EXPECT_NE(cancel_result.message.find("missing a numeric ret_code"), std::string::npos); + EXPECT_NE(cancel_result.message.find("controller outcome is unknown"), std::string::npos); + EXPECT_NE( + cancel_result.message.find("do not issue another motion command automatically"), + std::string::npos); + EXPECT_FALSE(pose_result.ok()); + EXPECT_EQ(pose_result.code, AgvErrorCode::CommandFailed); + EXPECT_NE(pose_result.message.find("state is unknown"), std::string::npos); +} + +TEST_F(SeerRobokitControlAuthorityTest, TimedOutChannelIsClosedBeforeSameCommandCanRetry) +{ + SeerRobokitAgvTestPeer::setNavigationReceiveTimeout( + *agv_, + std::chrono::milliseconds(50)); + controller_.setResponseDelay( + kRobotTaskCancel, + std::chrono::milliseconds(200)); + controller_.clearRecords(); + + const auto first_result = agv_->cancelNavigation(); + const auto second_result = agv_->cancelNavigation(); + + EXPECT_FALSE(first_result.ok()); + EXPECT_EQ(first_result.code, AgvErrorCode::Timeout); + EXPECT_NE(first_result.message.find("channel closed"), std::string::npos); + EXPECT_NE( + first_result.message.find("controller outcome is unknown"), + std::string::npos); + EXPECT_FALSE(second_result.ok()); + EXPECT_EQ(second_result.code, AgvErrorCode::NotConnected); + EXPECT_EQ( + second_result.message.find("controller outcome is unknown"), + std::string::npos); + + const auto records = controller_.records(); + const auto cancel_count = std::count_if( + records.begin(), + records.end(), + [](const CommandRecord& record) { + return record.command == kRobotTaskCancel; + }); + EXPECT_EQ(cancel_count, 1); +} + +TEST_F( + SeerRobokitControlAuthorityTest, + NavigationWriteNotStartedDoesNotPublishFalseTracking) +{ + SeerRobokitAgvTestPeer::closeNavigationSocket(*agv_); + AgvMotionOptions options; + options.wait_timeout_ms = 500; + options.poll_interval_ms = 20; + controller_.clearRecords(); + + const auto result = agv_->navigateToStation( + "station-not-sent", + options); + + EXPECT_FALSE(result.ok()); + EXPECT_EQ(result.code, AgvErrorCode::NotConnected); + EXPECT_EQ( + result.message.find("controller outcome is unknown"), + std::string::npos); + EXPECT_FALSE(SeerRobokitAgvTestPeer::hasTrackedNavigation( + *agv_, + AgvTaskType::NavigateToStation)); + const auto records = controller_.records(); + EXPECT_EQ( + std::count_if( + records.begin(), + records.end(), + [](const CommandRecord& record) { + return record.command == kRobotTaskGoTarget + || record.command == kRobotStatusTaskPackage + || record.command == kRobotTaskCancel; + }), + 0); +} + +TEST_F(SeerRobokitControlAuthorityTest, SlowTaskStatusDoesNotBlockEmergencyStop) +{ + controller_.setResponsePayload( + kRobotStatusTaskPackage, + R"({"ret_code":0,"task_status_package":{"task_status_list":[]}})"); + controller_.setResponseDelay( + kRobotStatusTaskPackage, + std::chrono::milliseconds(300)); + controller_.clearRecords(); + AgvResult pose_result = AgvResult::success(); + + std::thread pose_thread([this, &pose_result]() { + pose_result = agv_->navigateToPose(math::Pose2d{1.0, 2.0, 0.5}, asynchronousMotionOptions()); + }); + + bool status_query_observed = false; + for (int attempt = 0; attempt < 200; ++attempt) { + const auto records = controller_.records(); + status_query_observed = std::any_of( + records.begin(), + records.end(), + [](const CommandRecord& record) { + return record.command == kRobotStatusTaskPackage; + }); + if (status_query_observed) { + break; + } + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + } + ASSERT_TRUE(status_query_observed); + + const auto start = std::chrono::steady_clock::now(); + const auto stop_result = agv_->emergencyStop(); + const auto elapsed = std::chrono::duration_cast( + std::chrono::steady_clock::now() - start); + pose_thread.join(); + + ASSERT_TRUE(stop_result.ok()) << stop_result.message; + EXPECT_LT(elapsed.count(), 150); + EXPECT_FALSE(pose_result.ok()); + EXPECT_EQ(pose_result.code, AgvErrorCode::CommandFailed); + EXPECT_NE(pose_result.message.find("state is unknown"), std::string::npos); + + const auto records = controller_.records(); + const auto control_stop = std::find_if( + records.begin(), + records.end(), + [](const CommandRecord& record) { + return record.command == kRobotControlStop; + }); + const auto navigation_cancel = std::find_if( + records.begin(), + records.end(), + [](const CommandRecord& record) { + return record.command == kRobotTaskCancel; + }); + ASSERT_NE(control_stop, records.end()); + ASSERT_NE(navigation_cancel, records.end()); + EXPECT_LT(control_stop, navigation_cancel); +} + +TEST_F( + SeerRobokitControlAuthorityTest, + SlowConditionalCancelPreflightDoesNotBlockOrFollowEmergencyStop) +{ + const auto path_result = agv_->followPath( + {AgvPathSegment{"station-1", "station-2"}}, + asynchronousMotionOptions()); + ASSERT_TRUE(path_result.ok()) << path_result.message; + controller_.setResponsePayload( + kRobotStatusTaskPackage, + R"({"ret_code":0,"task_status_package":{"task_status_list":[{"task_id":"${TASK_ID}","status":2,"type":3}]}})"); + controller_.setResponsePayload( + kRobotStatusAll2, + R"({"ret_code":0,"task_status":2,"task_type":3,"target_id":"station-2","blocked":false,"vx":0.05,"vy":0.0,"w":0.0,"fatals":[],"errors":[]})"); + controller_.setResponseDelay( + kRobotStatusTaskPackage, + std::chrono::milliseconds(300)); + controller_.clearRecords(); + AgvResult cancel_result; + + std::thread cancel_thread([this, &cancel_result]() { + cancel_result = agv_->cancelNavigation(); + }); + const bool cancel_preflight_started = waitForCommandCount( + kRobotStatusTaskPackage, + 1, + 1000); + const auto stop_started_at = std::chrono::steady_clock::now(); + const auto stop_result = agv_->emergencyStop(); + const auto stop_elapsed = std::chrono::duration_cast< + std::chrono::milliseconds>( + std::chrono::steady_clock::now() - stop_started_at); + cancel_thread.join(); + + EXPECT_TRUE(cancel_preflight_started); + ASSERT_TRUE(stop_result.ok()) << stop_result.message; + EXPECT_LT(stop_elapsed.count(), 200); + EXPECT_FALSE(cancel_result.ok()); + EXPECT_EQ(cancel_result.code, AgvErrorCode::TaskCanceled); + const auto records = controller_.records(); + EXPECT_EQ( + std::count_if( + records.begin(), + records.end(), + [](const CommandRecord& record) { + return record.command == kRobotControlStop; + }), + 1); + EXPECT_EQ( + std::count_if( + records.begin(), + records.end(), + [](const CommandRecord& record) { + return record.command == kRobotTaskClearTargetList; + }), + 1); +} + +TEST_F(SeerRobokitControlAuthorityTest, EmergencyStopInvalidatesPoseAfterFirstAcceptedStop) +{ + controller_.setResponseDelay( + kRobotStatusTaskPackage, + std::chrono::milliseconds(200)); + controller_.setResponseDelay( + kRobotTaskCancel, + std::chrono::milliseconds(400)); + controller_.clearRecords(); + AgvResult pose_result = AgvResult::success(); + AgvResult stop_result = AgvResult::success(); + std::atomic_bool pose_finished{false}; + std::atomic_bool stop_finished{false}; + + std::thread pose_thread([this, &pose_result, &pose_finished]() { + pose_result = agv_->navigateToPose(math::Pose2d{1.0, 2.0, 0.5}, asynchronousMotionOptions()); + pose_finished.store(true, std::memory_order_release); + }); + + bool status_query_observed = false; + for (int attempt = 0; attempt < 200; ++attempt) { + const auto records = controller_.records(); + status_query_observed = std::any_of( + records.begin(), + records.end(), + [](const CommandRecord& record) { + return record.command == kRobotStatusTaskPackage; + }); + if (status_query_observed) { + break; + } + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + } + EXPECT_TRUE(status_query_observed); + + std::thread stop_thread([this, &stop_result, &stop_finished]() { + stop_result = agv_->emergencyStop(); + stop_finished.store(true, std::memory_order_release); + }); + + bool control_stop_observed = false; + for (int attempt = 0; attempt < 200; ++attempt) { + const auto records = controller_.records(); + control_stop_observed = std::any_of( + records.begin(), + records.end(), + [](const CommandRecord& record) { + return record.command == kRobotControlStop; + }); + if (control_stop_observed) { + break; + } + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + } + EXPECT_TRUE(control_stop_observed); + + for (int attempt = 0; attempt < 350; ++attempt) { + if (pose_finished.load(std::memory_order_acquire)) { + break; + } + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + } + const bool pose_finished_before_navigation_cancel = + pose_finished.load(std::memory_order_acquire); + const bool stop_finished_before_pose_result = + stop_finished.load(std::memory_order_acquire); + stop_thread.join(); + pose_thread.join(); + EXPECT_TRUE(pose_finished_before_navigation_cancel); + EXPECT_FALSE(stop_finished_before_pose_result); + EXPECT_FALSE(pose_result.ok()); + EXPECT_EQ(pose_result.code, AgvErrorCode::CommandFailed); + EXPECT_NE(pose_result.message.find("state is unknown"), std::string::npos); + ASSERT_TRUE(stop_result.ok()) << stop_result.message; +} + +TEST_F(SeerRobokitControlAuthorityTest, NavigateToStationForwardsTypedMotionOptions) +{ + AgvMotionOptions options; + options.asynchronous = true; + options.max_speed = 0.4; + options.max_angular_speed = 0.5; + options.max_acceleration = 0.6; + options.max_angular_acceleration = 0.7; + AgvAdapterParams adapter_params; + adapter_params.values.emplace("id", "wrong-station"); + adapter_params.values.emplace("x", "99.0"); + adapter_params.values.emplace("freeGo", "invalid"); + adapter_params.values.emplace("max_speed", "not-a-number"); + adapter_params.values.emplace("reach_dist", "not-a-number"); + controller_.clearRecords(); + + const auto result = agv_->navigateToStation( + "station-1", + options, + adapter_params); + + ASSERT_TRUE(result.ok()) << result.message; + const auto records = controller_.records(); + ASSERT_EQ(records.size(), 2U); + EXPECT_EQ(records[0].command, kRobotConfigLock); + EXPECT_EQ(records[1].command, kRobotTaskGoTarget); + + const auto payload = parsePayload(records[1]); + EXPECT_EQ(payloadValue(payload, "source_id").asString(), "SELF_POSITION"); + EXPECT_EQ(payloadValue(payload, "id").asString(), "station-1"); + EXPECT_TRUE(payloadValue(payload, "max_speed").isNumeric()); + EXPECT_TRUE(payloadValue(payload, "max_wspeed").isNumeric()); + EXPECT_TRUE(payloadValue(payload, "max_acc").isNumeric()); + EXPECT_TRUE(payloadValue(payload, "max_wacc").isNumeric()); + EXPECT_DOUBLE_EQ(payloadValue(payload, "max_speed").asDouble(), 0.4); + EXPECT_DOUBLE_EQ(payloadValue(payload, "max_wspeed").asDouble(), 0.5); + EXPECT_DOUBLE_EQ(payloadValue(payload, "max_acc").asDouble(), 0.6); + EXPECT_DOUBLE_EQ(payloadValue(payload, "max_wacc").asDouble(), 0.7); + EXPECT_FALSE(payloadHas(payload, "x")); + EXPECT_FALSE(payloadHas(payload, "freeGo")); + EXPECT_FALSE(payloadHas(payload, "reach_dist")); + EXPECT_FALSE(payloadHas(payload, "jack_height")); + EXPECT_FALSE(payloadHas(payload, "use_pgv")); + EXPECT_FALSE(payloadHas(payload, "use_down_pgv")); + EXPECT_FALSE(payloadHas(payload, "pgv_adjust_dist")); + EXPECT_FALSE(payloadHas(payload, "pgv_adjust_cx")); + EXPECT_FALSE(payloadHas(payload, "pgv_adjust_cy")); + EXPECT_FALSE(payloadHas(payload, "pgv_x_adjust")); +} + +TEST_F( + SeerRobokitControlAuthorityTest, + NavigateToStationForwardsPgvAdjustmentUsingNativeJsonTypes) +{ + AgvMotionOptions options; + options.asynchronous = true; + AgvAdapterParams adapter_params; + adapter_params.values.emplace("source_id", "LM2"); + adapter_params.values.emplace("use_pgv", "true"); + adapter_params.values.emplace("pgv_adjust_dist", "0.3"); + adapter_params.values.emplace("pgv_adjust_cx", "-0.3"); + adapter_params.values.emplace("pgv_adjust_cy", "0"); + adapter_params.values.emplace("pgv_x_adjust", "1"); + adapter_params.values.emplace("use_down_pgv", "false"); + controller_.clearRecords(); + + const auto result = agv_->navigateToStation( + "AP1", + options, + adapter_params); + + ASSERT_TRUE(result.ok()) << result.message; + const auto records = controller_.records(); + ASSERT_EQ(records.size(), 2U); + EXPECT_EQ(records[0].command, kRobotConfigLock); + EXPECT_EQ(records[1].command, kRobotTaskGoTarget); + + const auto payload = parsePayload(records[1]); + EXPECT_EQ(payloadValue(payload, "source_id").asString(), "LM2"); + EXPECT_EQ(payloadValue(payload, "id").asString(), "AP1"); + EXPECT_TRUE(payloadValue(payload, "use_pgv").isBool()); + EXPECT_TRUE(payloadValue(payload, "use_pgv").asBool()); + EXPECT_TRUE(payloadValue(payload, "use_down_pgv").isBool()); + EXPECT_FALSE(payloadValue(payload, "use_down_pgv").asBool()); + EXPECT_TRUE(payloadValue(payload, "pgv_adjust_dist").isNumeric()); + EXPECT_TRUE(payloadValue(payload, "pgv_adjust_cx").isNumeric()); + EXPECT_TRUE(payloadValue(payload, "pgv_adjust_cy").isNumeric()); + EXPECT_TRUE(payloadValue(payload, "pgv_x_adjust").isNumeric()); + EXPECT_DOUBLE_EQ(payloadValue(payload, "pgv_adjust_dist").asDouble(), 0.3); + EXPECT_DOUBLE_EQ(payloadValue(payload, "pgv_adjust_cx").asDouble(), -0.3); + EXPECT_DOUBLE_EQ(payloadValue(payload, "pgv_adjust_cy").asDouble(), 0.0); + EXPECT_DOUBLE_EQ(payloadValue(payload, "pgv_x_adjust").asDouble(), 1.0); + EXPECT_FALSE(payloadHas(payload, "pgv_adjustuse_pgv_dist")); + EXPECT_FALSE(payloadHas(payload, "pgv_ajdust_cy")); +} + +TEST_F( + SeerRobokitControlAuthorityTest, + NavigateToStationNormalizesVendorDocumentPgvCyAlias) +{ + AgvMotionOptions options; + options.asynchronous = true; + AgvAdapterParams adapter_params; + adapter_params.values.emplace("use_pgv", "true"); + adapter_params.values.emplace("pgv_ajdust_cy", "-0.2"); + controller_.clearRecords(); + + const auto result = agv_->navigateToStation( + "AP1", + options, + adapter_params); + + ASSERT_TRUE(result.ok()) << result.message; + const auto records = controller_.records(); + ASSERT_EQ(records.size(), 2U); + const auto payload = parsePayload(records[1]); + EXPECT_TRUE(payloadValue(payload, "pgv_adjust_cy").isNumeric()); + EXPECT_DOUBLE_EQ(payloadValue(payload, "pgv_adjust_cy").asDouble(), -0.2); + EXPECT_FALSE(payloadHas(payload, "pgv_ajdust_cy")); +} + +TEST_F( + SeerRobokitControlAuthorityTest, + NavigateToStationRejectsInvalidPgvAdjustmentBeforeAcquiringAuthority) +{ + struct InvalidPgvCase { + const char* key; + const char* value; + const char* expected_detail; + }; + const InvalidPgvCase cases[] = { + {"use_pgv", "enabled", "must be a boolean string"}, + {"pgv_adjust_dist", "-0.1", "must be non-negative"}, + {"pgv_adjust_cx", "nan", "must be a complete finite number"}, + {"pgv_x_adjust", "0.5m", "must be a complete finite number"}, + {"pgv_ajdust_cy", "inf", "must be a complete finite number"}, + {"pgv_adjustuse_pgv_dist", "0.3", "vendor-document typo"}, + }; + + AgvMotionOptions options; + options.asynchronous = true; + for (const auto& test_case : cases) { + SCOPED_TRACE(test_case.key); + AgvAdapterParams adapter_params; + adapter_params.values.emplace(test_case.key, test_case.value); + controller_.clearRecords(); + + const auto result = agv_->navigateToStation( + "AP1", + options, + adapter_params); + + EXPECT_FALSE(result.ok()); + EXPECT_EQ(result.code, AgvErrorCode::InvalidArgument); + EXPECT_NE( + result.message.find(test_case.expected_detail), + std::string::npos); + EXPECT_TRUE(controller_.records().empty()); + } + + AgvAdapterParams ambiguous_adapter_params; + ambiguous_adapter_params.values.emplace("pgv_adjust_cy", "0.1"); + ambiguous_adapter_params.values.emplace("pgv_ajdust_cy", "0.2"); + controller_.clearRecords(); + + const auto ambiguous_result = agv_->navigateToStation( + "AP1", + options, + ambiguous_adapter_params); + + EXPECT_FALSE(ambiguous_result.ok()); + EXPECT_EQ(ambiguous_result.code, AgvErrorCode::InvalidArgument); + EXPECT_NE( + ambiguous_result.message.find("must not both be set"), + std::string::npos); + EXPECT_TRUE(controller_.records().empty()); + + AgvAdapterParams mixed_action_adapter_params; + mixed_action_adapter_params.values.emplace("use_pgv", "true"); + mixed_action_adapter_params.values.emplace("operation", "JackHeight"); + mixed_action_adapter_params.values.emplace("jack_height", "0.5"); + controller_.clearRecords(); + + const auto mixed_action_result = agv_->navigateToStation( + "AP1", + options, + mixed_action_adapter_params); + + EXPECT_FALSE(mixed_action_result.ok()); + EXPECT_EQ(mixed_action_result.code, AgvErrorCode::InvalidArgument); + EXPECT_NE( + mixed_action_result.message.find( + "PGV adjustment must not be combined with adapter field"), + std::string::npos); + EXPECT_TRUE(controller_.records().empty()); +} + +TEST_F(SeerRobokitControlAuthorityTest, SetVelocityUsesOnlyDocumentedNumericFields) +{ + controller_.clearRecords(); + + const auto result = agv_->setVelocity(AgvVelocity{0.1, -0.2, 0.3}); + + ASSERT_TRUE(result.ok()) << result.message; + auto records = controller_.records(); + ASSERT_EQ(records.size(), 2U); + EXPECT_EQ(records[0].command, kRobotConfigLock); + EXPECT_EQ(records[1].command, kRobotControlMotion); + + auto payload = parsePayload(records[1]); + EXPECT_TRUE(payloadValue(payload, "vx").isNumeric()); + EXPECT_TRUE(payloadValue(payload, "vy").isNumeric()); + EXPECT_TRUE(payloadValue(payload, "w").isNumeric()); + EXPECT_DOUBLE_EQ(payloadValue(payload, "vx").asDouble(), 0.1); + EXPECT_DOUBLE_EQ(payloadValue(payload, "vy").asDouble(), -0.2); + EXPECT_DOUBLE_EQ(payloadValue(payload, "w").asDouble(), 0.3); + EXPECT_FALSE(payloadHas(payload, "duration")); + + controller_.clearRecords(); + const auto stop_result = agv_->stopVelocityControl(); + + ASSERT_TRUE(stop_result.ok()) << stop_result.message; + records = controller_.records(); + ASSERT_EQ(records.size(), 2U); + EXPECT_EQ(records[1].command, kRobotControlMotion); + payload = parsePayload(records[1]); + EXPECT_TRUE(payloadValue(payload, "vx").isNumeric()); + EXPECT_TRUE(payloadValue(payload, "vy").isNumeric()); + EXPECT_TRUE(payloadValue(payload, "w").isNumeric()); + EXPECT_DOUBLE_EQ(payloadValue(payload, "vx").asDouble(), 0.0); + EXPECT_DOUBLE_EQ(payloadValue(payload, "vy").asDouble(), 0.0); + EXPECT_DOUBLE_EQ(payloadValue(payload, "w").asDouble(), 0.0); + EXPECT_FALSE(payloadHas(payload, "duration")); +} + +TEST_F(SeerRobokitControlAuthorityTest, ControllerErrorCodeIsPreservedInResultMessage) +{ + controller_.setResponseCode(kRobotControlMotion, 41200); + controller_.clearRecords(); + + const auto result = agv_->setVelocity(AgvVelocity{0.1, 0.0, 0.0}); + + EXPECT_FALSE(result.ok()); + EXPECT_EQ(result.code, AgvErrorCode::CommandFailed); + EXPECT_NE(result.message.find("ret_code=41200"), std::string::npos); + EXPECT_NE(result.message.find("err_msg=simulated command failure"), std::string::npos); +} + +TEST_F( + SeerRobokitControlAuthorityTest, + MapModeCommandsSupersedeTrackedFreeNavigation) +{ + auto result = + agv_->navigateToPose(math::Pose2d{1.0, 2.0, 0.5}, asynchronousMotionOptions()); + ASSERT_TRUE(result.ok()) << result.message; + ASSERT_TRUE(SeerRobokitAgvTestPeer::hasTrackedPoseTask(*agv_)); + + result = agv_->switchMap("map-1"); + ASSERT_TRUE(result.ok()) << result.message; + EXPECT_FALSE(SeerRobokitAgvTestPeer::hasTrackedPoseTask(*agv_)); + + result = agv_->navigateToPose(math::Pose2d{1.0, 2.0, 0.5}, asynchronousMotionOptions()); + ASSERT_TRUE(result.ok()) << result.message; + ASSERT_TRUE(SeerRobokitAgvTestPeer::hasTrackedPoseTask(*agv_)); + + result = agv_->startMapping(); + ASSERT_TRUE(result.ok()) << result.message; + EXPECT_FALSE(SeerRobokitAgvTestPeer::hasTrackedPoseTask(*agv_)); + + result = agv_->navigateToPose(math::Pose2d{1.0, 2.0, 0.5}, asynchronousMotionOptions()); + ASSERT_TRUE(result.ok()) << result.message; + ASSERT_TRUE(SeerRobokitAgvTestPeer::hasTrackedPoseTask(*agv_)); + + result = agv_->stopMapping(); + ASSERT_TRUE(result.ok()) << result.message; + EXPECT_FALSE(SeerRobokitAgvTestPeer::hasTrackedPoseTask(*agv_)); +} + +TEST_F( + SeerRobokitControlAuthorityTest, + PauseResumeAndStopVelocityPreserveTrackedFreeNavigation) +{ + auto result = + agv_->navigateToPose(math::Pose2d{1.0, 2.0, 0.5}, asynchronousMotionOptions()); + ASSERT_TRUE(result.ok()) << result.message; + ASSERT_TRUE(SeerRobokitAgvTestPeer::hasTrackedPoseTask(*agv_)); + + result = agv_->pauseNavigation(); + ASSERT_TRUE(result.ok()) << result.message; + EXPECT_TRUE(SeerRobokitAgvTestPeer::hasTrackedPoseTask(*agv_)); + + result = agv_->resumeNavigation(); + ASSERT_TRUE(result.ok()) << result.message; + EXPECT_TRUE(SeerRobokitAgvTestPeer::hasTrackedPoseTask(*agv_)); + + result = agv_->stopVelocityControl(); + ASSERT_TRUE(result.ok()) << result.message; + EXPECT_TRUE(SeerRobokitAgvTestPeer::hasTrackedPoseTask(*agv_)); + + controller_.clearRecords(); + const auto status = agv_->navigationStatus(); + EXPECT_EQ(status.state, AgvTaskState::Running); + EXPECT_EQ(status.type, AgvTaskType::NavigateToPose); + const auto records = controller_.records(); + ASSERT_EQ(records.size(), 1U); + EXPECT_EQ(records[0].command, kRobotStatusTaskPackage); +} + +TEST_F(SeerRobokitControlAuthorityTest, NavigationStatusQueriesTrackedPoseTaskPackage) +{ + AgvAdapterParams adapter_params; + adapter_params.values.emplace("task_id", "pose-task-current"); + const auto navigate_result = agv_->navigateToPose( + math::Pose2d{1.0, 2.0, 0.5}, + asynchronousMotionOptions(), + adapter_params); + ASSERT_TRUE(navigate_result.ok()) << navigate_result.message; + const auto navigate_records = controller_.records(); + ASSERT_GE(navigate_records.size(), 2U); + const auto navigate_payload = parsePayload(navigate_records[1]); + const std::string generated_task_id = + payloadValue(navigate_payload, "task_id").asString(); + ASSERT_FALSE(generated_task_id.empty()); + + controller_.setResponsePayload( + kRobotStatusTaskPackage, + R"({"ret_code":0,"create_on":"2026-07-31T10:00:01Z","err_msg":"","task_status_package":{"percentage":42.5,"distance":0.7,"info":"operator pause","task_status_list":[{"task_id":"${TASK_ID}","status":3,"type":1}]}})"); + controller_.clearRecords(); + + const auto status = agv_->navigationStatus(); + + EXPECT_EQ(status.state, AgvTaskState::Paused); + EXPECT_EQ(status.type, AgvTaskType::NavigateToPose); + EXPECT_DOUBLE_EQ(status.progress, 42.5); + EXPECT_NE(status.message.find("task_id=" + generated_task_id), std::string::npos); + EXPECT_NE(status.message.find("operator pause"), std::string::npos); + EXPECT_NE(status.message.find("create_on=2026-07-31T10:00:01Z"), std::string::npos); + const auto records = controller_.records(); + ASSERT_EQ(records.size(), 1U); + EXPECT_EQ(records[0].command, kRobotStatusTaskPackage); + const auto payload = parsePayload(records[0]); + const auto& task_ids = payloadValue(payload, "task_ids"); + ASSERT_TRUE(task_ids.isArray()); + ASSERT_EQ(task_ids.size(), 1U); + EXPECT_EQ(task_ids[0].asString(), generated_task_id); +} + +TEST_F( + SeerRobokitControlAuthorityTest, + NavigationStatusMapsControllerTaskStatusSevenToFailed) +{ + const auto navigate_result = agv_->navigateToPose( + math::Pose2d{1.0, 2.0, 0.5}, + asynchronousMotionOptions()); + ASSERT_TRUE(navigate_result.ok()) << navigate_result.message; + controller_.setResponsePayload( + kRobotStatusTaskPackage, + R"({"ret_code":0,"task_status_package":{"info":"controller terminated tracked pose","task_status_list":[{"task_id":"${TASK_ID}","status":7,"type":1}]}})"); + controller_.clearRecords(); + + const auto status = agv_->navigationStatus(); + + EXPECT_EQ(status.state, AgvTaskState::Failed); + EXPECT_EQ(status.type, AgvTaskType::NavigateToPose); + EXPECT_NE(status.message.find("controller terminated tracked pose"), std::string::npos); + EXPECT_NE(status.message.find("task_status=7"), std::string::npos); + EXPECT_FALSE(SeerRobokitAgvTestPeer::hasTrackedPoseTask(*agv_)); + const auto records = controller_.records(); + ASSERT_EQ(records.size(), 1U); + EXPECT_EQ(records[0].command, kRobotStatusTaskPackage); +} + +TEST_F( + SeerRobokitControlAuthorityTest, + NavigationStatusReturnsControllerFaultWhileTaskStillReportsRunning) +{ + const auto navigate_result = + agv_->navigateToPose(math::Pose2d{1.0, 2.0, 0.5}, asynchronousMotionOptions()); + ASSERT_TRUE(navigate_result.ok()) << navigate_result.message; + + Json::Value fault_push(Json::objectValue); + Json::Value errors(Json::arrayValue); + errors.append("E_RUNNING_STATUS_52: collision input active"); + *fault_push.demand("errors", "errors" + std::strlen("errors")) = errors; + *fault_push.demand("fatals", "fatals" + std::strlen("fatals")) = + Json::Value(Json::arrayValue); + SeerRobokitAgvTestPeer::cacheRuntimeState(*agv_, fault_push); + controller_.clearRecords(); + + const auto status = agv_->navigationStatus(); + + EXPECT_EQ(status.state, AgvTaskState::Failed); + EXPECT_EQ(status.type, AgvTaskType::NavigateToPose); + EXPECT_NE(status.message.find("controller_task_state=2"), std::string::npos); + EXPECT_NE(status.message.find("E_RUNNING_STATUS_52"), std::string::npos); + EXPECT_TRUE(SeerRobokitAgvTestPeer::hasTrackedPoseTask(*agv_)); + const auto records = controller_.records(); + ASSERT_EQ(records.size(), 1U); + EXPECT_EQ(records[0].command, kRobotStatusTaskPackage); +} + +TEST_F( + SeerRobokitControlAuthorityTest, + NavigationStatusPreservesFaultWhenTrackedTaskDisappears) +{ + const auto navigate_result = + agv_->navigateToPose(math::Pose2d{1.0, 2.0, 0.5}, asynchronousMotionOptions()); + ASSERT_TRUE(navigate_result.ok()) << navigate_result.message; + controller_.setResponsePayload( + kRobotStatusTaskPackage, + R"({"ret_code":0,"task_status_package":{"info":"task vanished","task_status_list":[]}})"); + controller_.clearRecords(); + AgvNavigationStatus status; + + std::thread status_thread([this, &status]() { + status = agv_->navigationStatus(); + }); + + bool status_query_observed = false; + for (int attempt = 0; attempt < 200; ++attempt) { + const auto records = controller_.records(); + status_query_observed = std::any_of( + records.begin(), + records.end(), + [](const CommandRecord& record) { + return record.command == kRobotStatusTaskPackage; + }); + if (status_query_observed) { + break; + } + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + } + EXPECT_TRUE(status_query_observed); + + Json::Value fault_push(Json::objectValue); + Json::Value errors(Json::arrayValue); + errors.append("E_TASK_GONE_54: controller removed failed task"); + *fault_push.demand("errors", "errors" + std::strlen("errors")) = errors; + *fault_push.demand("fatals", "fatals" + std::strlen("fatals")) = + Json::Value(Json::arrayValue); + SeerRobokitAgvTestPeer::cacheRuntimeState(*agv_, fault_push); + status_thread.join(); + + EXPECT_EQ(status.state, AgvTaskState::Failed); + EXPECT_EQ(status.type, AgvTaskType::NavigateToPose); + EXPECT_NE(status.message.find("task disappeared"), std::string::npos); + EXPECT_NE(status.message.find("E_TASK_GONE_54"), std::string::npos); + EXPECT_FALSE(SeerRobokitAgvTestPeer::hasTrackedPoseTask(*agv_)); + const auto records = controller_.records(); + EXPECT_FALSE(std::any_of( + records.begin(), + records.end(), + [](const CommandRecord& record) { + return record.command == kRobotStatusTask; + })); +} + +TEST_F( + SeerRobokitControlAuthorityTest, + NavigationStatusRejectsFaultArrivingDuringCompletedPoseVerification) +{ + const auto navigate_result = + agv_->navigateToPose(math::Pose2d{1.0, 2.0, 0.5}, asynchronousMotionOptions()); + ASSERT_TRUE(navigate_result.ok()) << navigate_result.message; + controller_.setResponsePayload( + kRobotStatusTaskPackage, + R"({"ret_code":0,"task_status_package":{"info":"controller says complete","task_status_list":[{"task_id":"${TASK_ID}","status":4,"type":1}]}})"); + controller_.setResponsePayload( + kRobotStatusLoc, + R"({"ret_code":0,"x":1.0,"y":2.0,"angle":0.5})"); + controller_.setResponseDelay( + kRobotStatusLoc, + std::chrono::milliseconds(300)); + controller_.clearRecords(); + AgvNavigationStatus status; + + std::thread status_thread([this, &status]() { + status = agv_->navigationStatus(); + }); + + bool location_query_observed = false; + for (int attempt = 0; attempt < 700; ++attempt) { + const auto records = controller_.records(); + location_query_observed = std::any_of( + records.begin(), + records.end(), + [](const CommandRecord& record) { + return record.command == kRobotStatusLoc; + }); + if (location_query_observed) { + break; + } + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + } + EXPECT_TRUE(location_query_observed); + + Json::Value fault_push(Json::objectValue); + Json::Value errors(Json::arrayValue); + errors.append("E_STATUS_VERIFY_53: fault during completed pose check"); + *fault_push.demand("errors", "errors" + std::strlen("errors")) = errors; + *fault_push.demand("fatals", "fatals" + std::strlen("fatals")) = + Json::Value(Json::arrayValue); + SeerRobokitAgvTestPeer::cacheRuntimeState(*agv_, fault_push); + status_thread.join(); + + EXPECT_EQ(status.state, AgvTaskState::Failed); + EXPECT_EQ(status.type, AgvTaskType::NavigateToPose); + EXPECT_NE(status.message.find("target verification"), std::string::npos); + EXPECT_NE(status.message.find("E_STATUS_VERIFY_53"), std::string::npos); + EXPECT_FALSE(SeerRobokitAgvTestPeer::hasTrackedPoseTask(*agv_)); +} + +TEST_F( + SeerRobokitControlAuthorityTest, + NavigationStatusRejectsCompletedPoseWhenTargetWasNotReached) +{ + const auto navigate_result = + agv_->navigateToPose(math::Pose2d{1.0, 2.0, 0.5}, asynchronousMotionOptions()); + ASSERT_TRUE(navigate_result.ok()) << navigate_result.message; + controller_.setResponsePayload( + kRobotStatusTaskPackage, + R"({"ret_code":0,"task_status_package":{"info":"controller says complete","task_status_list":[{"task_id":"${TASK_ID}","status":4,"type":1}]}})"); + controller_.setResponsePayload( + kRobotStatusLoc, + R"({"ret_code":0,"x":0.0,"y":0.0,"angle":0.0})"); + controller_.clearRecords(); + + const auto status = agv_->navigationStatus(); + + EXPECT_EQ(status.state, AgvTaskState::Failed); + EXPECT_EQ(status.type, AgvTaskType::NavigateToPose); + EXPECT_NE( + status.message.find("requested target was not reached"), + std::string::npos); + EXPECT_NE(status.message.find("distance_error"), std::string::npos); + EXPECT_FALSE(SeerRobokitAgvTestPeer::hasTrackedPoseTask(*agv_)); + const auto records = controller_.records(); + ASSERT_EQ(records.size(), 2U); + EXPECT_EQ(records[0].command, kRobotStatusTaskPackage); + EXPECT_EQ(records[1].command, kRobotStatusLoc); +} + +TEST_F( + SeerRobokitControlAuthorityTest, + NavigationStatusWaitsBrieflyForLateControllerFaultDetail) +{ + const auto navigate_result = + agv_->navigateToPose(math::Pose2d{1.0, 2.0, 0.5}, asynchronousMotionOptions()); + ASSERT_TRUE(navigate_result.ok()) << navigate_result.message; + controller_.setResponsePayload( + kRobotStatusTaskPackage, + R"({"ret_code":0,"task_status_package":{"info":"planner failed","task_status_list":[{"task_id":"${TASK_ID}","status":5,"type":1}]}})"); + controller_.clearRecords(); + AgvNavigationStatus status; + + std::thread status_thread([this, &status]() { + status = agv_->navigationStatus(); + }); + + bool status_query_observed = false; + for (int attempt = 0; attempt < 200; ++attempt) { + const auto records = controller_.records(); + status_query_observed = std::any_of( + records.begin(), + records.end(), + [](const CommandRecord& record) { + return record.command == kRobotStatusTaskPackage; + }); + if (status_query_observed) { + break; + } + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + } + EXPECT_TRUE(status_query_observed); + + Json::Value fault_push(Json::objectValue); + Json::Value fatals(Json::arrayValue); + fatals.append("E_LATE_77: localization alarm"); + *fault_push.demand("fatals", "fatals" + std::strlen("fatals")) = fatals; + SeerRobokitAgvTestPeer::cacheRuntimeState(*agv_, fault_push); + status_thread.join(); + + EXPECT_EQ(status.state, AgvTaskState::Failed); + EXPECT_EQ(status.type, AgvTaskType::NavigateToPose); + EXPECT_NE(status.message.find("planner failed"), std::string::npos); + EXPECT_NE(status.message.find("E_LATE_77"), std::string::npos); + EXPECT_NE(status.message.find("localization alarm"), std::string::npos); + EXPECT_FALSE(SeerRobokitAgvTestPeer::hasTrackedPoseTask(*agv_)); +} + +TEST_F( + SeerRobokitControlAuthorityTest, + NavigationStatusDoesNotReturnOldPoseTaskAfterStationSupersedesIt) +{ + const auto navigate_result = + agv_->navigateToPose(math::Pose2d{1.0, 2.0, 0.5}, asynchronousMotionOptions()); + ASSERT_TRUE(navigate_result.ok()) << navigate_result.message; + + controller_.setResponsePayload( + kRobotStatusTaskPackage, + R"({"ret_code":0,"task_status_package":{"info":"old pose paused","task_status_list":[{"task_id":"${TASK_ID}","status":3,"type":1}]}})"); + controller_.setResponseDelay( + kRobotStatusTaskPackage, + std::chrono::milliseconds(300)); + controller_.setResponsePayload( + kRobotStatusTask, + R"({"ret_code":0,"err_msg":"","task_status":2,"task_type":2,"move_status_info":"station task running"})"); + controller_.clearRecords(); + AgvNavigationStatus status; + + std::thread status_thread([this, &status]() { + status = agv_->navigationStatus(); + }); + + bool status_query_observed = false; + for (int attempt = 0; attempt < 500; ++attempt) { + const auto records = controller_.records(); + status_query_observed = std::any_of( + records.begin(), + records.end(), + [](const CommandRecord& record) { + return record.command == kRobotStatusTaskPackage; + }); + if (status_query_observed) { + break; + } + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + } + EXPECT_TRUE(status_query_observed); + + const auto station_result = agv_->navigateToStation("station-1", asynchronousMotionOptions()); + status_thread.join(); + + ASSERT_TRUE(station_result.ok()) << station_result.message; + EXPECT_EQ(status.state, AgvTaskState::Running); + EXPECT_EQ(status.type, AgvTaskType::NavigateToStation); + EXPECT_EQ(status.message, "station task running"); + const auto records = controller_.records(); + EXPECT_TRUE(std::any_of( + records.begin(), + records.end(), + [](const CommandRecord& record) { + return record.command == kRobotStatusTask; + })); +} + +TEST_F(SeerRobokitControlAuthorityTest, DisconnectClearsTrackedPoseTask) +{ + const auto navigate_result = + agv_->navigateToPose(math::Pose2d{1.0, 2.0, 0.5}, asynchronousMotionOptions()); + ASSERT_TRUE(navigate_result.ok()) << navigate_result.message; + ASSERT_TRUE(SeerRobokitAgvTestPeer::hasTrackedPoseTask(*agv_)); + + const auto disconnect_result = SeerRobokitAgvTestPeer::disconnect(*agv_); + + ASSERT_TRUE(disconnect_result.ok()) << disconnect_result.message; + EXPECT_FALSE(SeerRobokitAgvTestPeer::hasTrackedPoseTask(*agv_)); +} + +TEST_F(SeerRobokitControlAuthorityTest, RuntimeStatePreservesCachedControllerFaultDetail) +{ + Json::Value push_payload(Json::objectValue); + Json::Value errors(Json::arrayValue); + Json::Value error(Json::objectValue); + *error.demand("code", "code" + std::strlen("code")) = "E_NAV_42"; + *error.demand("message", "message" + std::strlen("message")) = "planner alarm"; + errors.append(error); + *push_payload.demand("errors", "errors" + std::strlen("errors")) = errors; + SeerRobokitAgvTestPeer::cacheRuntimeState(*agv_, push_payload); + SeerRobokitAgvTestPeer::setAdapterError( + *agv_, + "SEER Robokit map file is empty after stripping the transport header"); + controller_.clearRecords(); + + const auto state = agv_->runtimeState(); + + EXPECT_TRUE(state.connected); + EXPECT_TRUE(state.fault); + EXPECT_EQ(state.mode, AgvMode::Fault); + EXPECT_NE(state.last_error.find("E_NAV_42"), std::string::npos); + EXPECT_NE(state.last_error.find("planner alarm"), std::string::npos); + EXPECT_NE(state.last_error.find("adapter_error="), std::string::npos); + EXPECT_NE(state.last_error.find("map file is empty"), std::string::npos); + EXPECT_TRUE(controller_.records().empty()); +} + +TEST_F(SeerRobokitControlAuthorityTest, NavigationStatusPreservesControllerErrorCode) +{ + controller_.setResponseCode(kRobotStatusTask, 51020); + controller_.clearRecords(); + + const auto status = agv_->navigationStatus(); + + EXPECT_EQ(status.state, AgvTaskState::Failed); + EXPECT_NE(status.message.find("ret_code=51020"), std::string::npos); + EXPECT_NE(status.message.find("err_msg=simulated command failure"), std::string::npos); + const auto records = controller_.records(); + ASSERT_EQ(records.size(), 1U); + EXPECT_EQ(records[0].command, kRobotStatusTask); +} + +} // namespace +} // namespace cmvr::device diff --git a/cmvr-es/devices/agv/src1100/CMakeLists.txt b/cmvr-es/devices/agv/src1100/CMakeLists.txt deleted file mode 100644 index 705542e2..00000000 --- a/cmvr-es/devices/agv/src1100/CMakeLists.txt +++ /dev/null @@ -1,40 +0,0 @@ -add_library(src1100_agv SHARED src/src1100_agv.cpp) - -target_include_directories(src1100_agv PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/include) - -target_link_libraries(src1100_agv - PUBLIC - cmvr_es::proto - jsoncpp -) - -add_library(cmvr_es::device::src1100_agv ALIAS src1100_agv) -install(TARGETS src1100_agv LIBRARY DESTINATION lib) - -if(BUILD_TESTING) - add_executable(src1100_control_authority_test - tests/src1100_control_authority_test.cpp - ) - target_link_libraries(src1100_control_authority_test - PRIVATE - cmvr_es::device::src1100_agv - gtest - gtest_main - pthread - ) - add_test( - NAME src1100_control_authority_test - COMMAND src1100_control_authority_test - ) - set(_src1100_control_authority_test_environment - "LD_LIBRARY_PATH=${CMVR_TEST_EXTERNAL_LIBRARY_PATH}" - ) - if(CMVR_TEST_SYSTEM_LIBSTDCXX) - list(APPEND _src1100_control_authority_test_environment - "LD_PRELOAD=${CMVR_TEST_SYSTEM_LIBSTDCXX}") - endif() - set_tests_properties(src1100_control_authority_test PROPERTIES - TIMEOUT 10 - ENVIRONMENT "${_src1100_control_authority_test_environment}" - ) -endif() diff --git a/cmvr-es/devices/agv/src1100/src/src1100_agv.cpp b/cmvr-es/devices/agv/src1100/src/src1100_agv.cpp deleted file mode 100644 index d523c80d..00000000 --- a/cmvr-es/devices/agv/src1100/src/src1100_agv.cpp +++ /dev/null @@ -1,3604 +0,0 @@ -#include "devices/agv/src1100/include/src1100_agv.h" - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include - -#include "common/base/logging/logger.h" -#include "rbk/protocol/src1100_map3d.pb.h" - -namespace cmvr::device { - -namespace { - -constexpr std::uint16_t kRobotStatusLoc = 1004; -constexpr std::uint16_t kRobotStatusBattery = 1007; -constexpr std::uint16_t kRobotStatusTask = 1020; -constexpr std::uint16_t kRobotStatusTaskPackage = 1110; -constexpr std::uint16_t kRobotStatusMap = 1300; -constexpr std::uint16_t kRobotStatusStation = 1301; -constexpr std::uint16_t kRobotStatusMappingFileList = 1780; -constexpr std::uint16_t kRobotStatusDownloadFile = 1800; -constexpr std::uint16_t kRobotControlStop = 2000; -constexpr std::uint16_t kRobotControlMotion = 2010; -constexpr std::uint16_t kRobotControlLoadMap = 2022; -constexpr std::uint16_t kRobotTaskPause = 3001; -constexpr std::uint16_t kRobotTaskResume = 3002; -constexpr std::uint16_t kRobotTaskCancel = 3003; -constexpr std::uint16_t kRobotTaskGoTarget = 3051; -constexpr std::uint16_t kRobotTaskGoTargetList = 3066; -constexpr std::uint16_t kRobotConfigLock = 4005; -constexpr std::uint16_t kRobotConfigUploadMap = 4010; -constexpr std::uint16_t kRobotConfigDownloadMap = 4011; -constexpr std::uint16_t kRobotOtherStartMapping = 6100; -constexpr std::uint16_t kRobotOtherStopMapping = 6101; -constexpr std::uint16_t kRobotPushConfigReq = 9300; -constexpr std::uint16_t kRobotPushConfigRes = 19300; -constexpr std::uint16_t kRobotPush = 19301; -constexpr std::uint32_t kMaxFramePayloadBytes = 512U * 1024U * 1024U; -constexpr auto kPoseNavigationStartTimeout = std::chrono::milliseconds(1500); -constexpr auto kPoseNavigationPollInterval = std::chrono::milliseconds(50); -constexpr int kPoseNavigationRequiredRunningSamples = 2; -constexpr int kMinimumControllerFaultCaptureGraceMs = 250; -constexpr int kMaximumControllerFaultCaptureGraceMs = 5000; -constexpr int kDefaultControllerFaultPushIntervalMs = 1000; -constexpr int kControllerFaultPushJitterMs = 100; -constexpr int kMinimumControllerFaultStateMaxAgeMs = 2000; -constexpr int kControllerFaultStateMaxAgeIntervals = 5; -constexpr double kDefaultPoseReachDistance = 0.05; -constexpr double kDefaultPoseReachAngle = 0.10; -constexpr double kTwoPi = 6.28318530717958647692; -constexpr int kDefaultMapUpdateIntervalMs = 1000; -constexpr std::size_t kDefaultMapUpdateHistorySize = 8; -constexpr std::uint64_t kMapSnapshotSequenceStart = 1; - -namespace fs = std::filesystem; - -std::string systemError() -{ - return std::strerror(errno); -} - -bool wants2D(const AgvMapDimension dimension) -{ - return dimension == AgvMapDimension::Unspecified - || dimension == AgvMapDimension::Map2D - || dimension == AgvMapDimension::Map2DAnd3D; -} - -bool wants3D(const AgvMapDimension dimension) -{ - return dimension == AgvMapDimension::Unspecified - || dimension == AgvMapDimension::Map3D - || dimension == AgvMapDimension::Map2DAnd3D; -} - -bool contentLooksLikeZip(const std::string& content) -{ - return content.size() >= 4 - && static_cast(content[0]) == 0x50U - && static_cast(content[1]) == 0x4BU - && static_cast(content[2]) == 0x03U - && static_cast(content[3]) == 0x04U; -} - -bool contentLooksLikeJson(const std::string& content) -{ - const auto pos = content.find_first_not_of(" \t\r\n"); - return pos != std::string::npos && (content[pos] == '{' || content[pos] == '['); -} - -std::string shellQuote(const std::string& value) -{ - std::string quoted = "'"; - for (const char ch : value) { - if (ch == '\'') { - quoted += "'\\''"; - } else { - quoted += ch; - } - } - quoted += "'"; - return quoted; -} - -bool writeBinaryFile(const fs::path& path, const std::string& content) -{ - std::ofstream output(path, std::ios::binary); - if (!output) { - return false; - } - output.write(content.data(), static_cast(content.size())); - return output.good(); -} - -bool readBinaryFile(const fs::path& path, std::string& content) -{ - std::ifstream input(path, std::ios::binary); - if (!input) { - return false; - } - std::ostringstream buffer; - buffer << input.rdbuf(); - content = buffer.str(); - return true; -} - -fs::path makeTempDirectory() -{ - auto pattern = fs::temp_directory_path() / "cmvr_src1100_map_XXXXXX"; - std::string path = pattern.string(); - char* created = ::mkdtemp(path.data()); - if (!created) { - return {}; - } - return fs::path(created); -} - -Json::Value& jsonMember(Json::Value& value, const char* key) -{ - return *value.demand(key, key + std::strlen(key)); -} - -Json::Value& jsonMember(Json::Value& value, const std::string& key) -{ - return *value.demand(key.data(), key.data() + key.size()); -} - -const Json::Value* jsonFind(const Json::Value& value, const char* key) -{ - return value.find(key, key + std::strlen(key)); -} - -Json::Value jsonGet(const Json::Value& value, const char* key, const Json::Value& fallback) -{ - const auto* found = jsonFind(value, key); - return found ? *found : fallback; -} - -double nowSeconds() -{ - const auto now = std::chrono::system_clock::now().time_since_epoch(); - return std::chrono::duration(now).count(); -} - -double angleDistance(const double lhs, const double rhs) -{ - return std::abs(std::remainder(lhs - rhs, kTwoPi)); -} - -bool jsonHas(const Json::Value& value, const char* key) -{ - return jsonFind(value, key) != nullptr; -} - -bool hasNumericControllerRetCode(const Json::Value& response) -{ - const auto* ret_code = jsonFind(response, "ret_code"); - return ret_code - && (ret_code->isInt() - || ret_code->isUInt() - || ret_code->isInt64() - || ret_code->isUInt64()); -} - -bool hasFaultArray(const Json::Value& value, const char* key) -{ - const auto* found = jsonFind(value, key); - return found && found->isArray() && !found->empty(); -} - -std::string jsonValueToString(const Json::Value& value) -{ - if (value.isString()) return value.asString(); - if (value.isBool()) return value.asBool() ? "true" : "false"; - if (value.isInt64() || value.isInt()) return std::to_string(value.asInt64()); - if (value.isUInt64() || value.isUInt()) return std::to_string(value.asUInt64()); - if (value.isDouble()) return std::to_string(value.asDouble()); - if (value.isNull()) return {}; - - Json::StreamWriterBuilder builder; - builder["indentation"] = ""; - return Json::writeString(builder, value); -} - -AgvResult withUnknownControllerOutcome(AgvResult result) -{ - const auto code = result.ok() ? AgvErrorCode::CommandFailed : result.code; - std::string detail = result.message.empty() ? "unknown transport or protocol error" : result.message; - detail += - "; SRC1100 controller outcome is unknown after the command attempt; " - "the command may already have taken effect; do not issue another motion " - "command automatically; query status and cancel or stop first"; - return AgvResult::failure(code, detail); -} - -std::string makePoseTaskId( - const std::string& device_id, - const std::uint64_t task_sequence) -{ - const auto timestamp = std::chrono::duration_cast( - std::chrono::system_clock::now().time_since_epoch()).count(); - const std::string prefix = device_id.empty() ? "cmvr-es" : device_id; - return prefix + "_pose_" + std::to_string(timestamp) - + "_" + std::to_string(task_sequence); -} - -void putPropertyIfPresent( - std::unordered_map& properties, - const Json::Value& value, - const char* json_key, - const char* property_key) -{ - const auto* found = jsonFind(value, json_key); - if (!found || found->isNull()) { - return; - } - properties[property_key] = jsonValueToString(*found); -} - -void appendMapProperties( - std::unordered_map& properties, - const Json::Value& value, - const char* key) -{ - const auto* list = jsonFind(value, key); - if (!list || !list->isArray()) { - return; - } - - for (const auto& item : *list) { - const std::string property_key = jsonGet(item, "key", "").asString(); - if (property_key.empty()) { - continue; - } - - const char* value_keys[] = { - "string_value", - "bool_value", - "int32_value", - "uint32_value", - "int64_value", - "uint64_value", - "float_value", - "double_value", - "bytes_value", - "value" - }; - for (const char* value_key : value_keys) { - const auto* found = jsonFind(item, value_key); - if (found && !found->isNull()) { - properties[property_key] = jsonValueToString(*found); - break; - } - } - } -} - -AgvMapPoint3D jsonPoint3D(const Json::Value& value) -{ - AgvMapPoint3D point; - point.x = jsonGet(value, "x", 0.0).asDouble(); - point.y = jsonGet(value, "y", 0.0).asDouble(); - point.z = jsonGet(value, "z", 0.0).asDouble(); - return point; -} - -void appendObject( - AgvUnifiedMap2D& map, - std::string id, - const AgvMapObjectType type, - std::vector points, - const double heading, - const Json::Value& source) -{ - AgvMapObject object; - object.id = std::move(id); - object.type = type; - object.points = std::move(points); - object.heading = heading; - putPropertyIfPresent(object.properties, source, "class_name", "class_name"); - putPropertyIfPresent(object.properties, source, "type", "type"); - putPropertyIfPresent(object.properties, source, "description", "description"); - appendMapProperties(object.properties, source, "property"); - map.objects.push_back(std::move(object)); -} - -void appendStringArray(Json::Value& value, const char* key, const google::protobuf::RepeatedPtrField& strings) -{ - if (strings.empty()) { - return; - } - Json::Value array(Json::arrayValue); - for (const auto& item : strings) { - array.append(item); - } - jsonMember(value, key) = array; -} - -AgvMode modeFromTaskState(const int state) -{ - switch (state) { - case 2: - return AgvMode::Auto; - case 3: - return AgvMode::Paused; - case 5: - return AgvMode::Fault; - case 6: - return AgvMode::Stopped; - default: - return AgvMode::Idle; - } -} - -AgvTaskState toTaskState(const int value) -{ - switch (value) { - case 1: - return AgvTaskState::Waiting; - case 2: - return AgvTaskState::Running; - case 3: - return AgvTaskState::Paused; - case 4: - return AgvTaskState::Completed; - case 5: - return AgvTaskState::Failed; - case 6: - return AgvTaskState::Canceled; - case 0: - default: - return AgvTaskState::None; - } -} - -AgvTaskType toTaskType(const int value) -{ - switch (value) { - case 1: - return AgvTaskType::NavigateToPose; - case 2: - return AgvTaskType::NavigateToStation; - case 3: - return AgvTaskType::FollowPath; - case 100: - return AgvTaskType::Custom; - default: - return AgvTaskType::None; - } -} - -std::string invalidMotionOption(const AgvMotionOptions& options) -{ - const auto non_negative_error = [](const double value, const char* field) { - if (!std::isfinite(value)) { - return std::string(field) + " must be finite"; - } - if (value < 0.0) { - return std::string(field) + " must be non-negative"; - } - return std::string{}; - }; - - if (auto error = non_negative_error(options.max_speed, "max_speed"); - !error.empty()) return error; - if (auto error = non_negative_error( - options.max_angular_speed, - "max_angular_speed"); - !error.empty()) return error; - if (auto error = non_negative_error( - options.max_acceleration, - "max_acceleration"); - !error.empty()) return error; - if (auto error = non_negative_error( - options.max_angular_acceleration, - "max_angular_acceleration"); - !error.empty()) return error; - if (auto error = non_negative_error( - options.reach_distance, - "reach_distance"); - !error.empty()) return error; - if (auto error = non_negative_error(options.reach_angle, "reach_angle"); - !error.empty()) return error; - if (auto error = non_negative_error(options.speed_ratio, "speed_ratio"); - !error.empty()) return error; - return {}; -} - -bool parseFiniteDouble(const std::string& value, double& parsed) -{ - std::size_t consumed = 0; - try { - parsed = std::stod(value, &consumed); - } catch (...) { - return false; - } - return consumed == value.size() && std::isfinite(parsed); -} - -} // namespace - -Src1100Agv::Src1100Agv(const config::Src1100AgvConfig& cfg) - : config_(cfg), - ip_(cfg.ip()), - control_nick_name_( - cfg.control_nick_name().empty() - ? (cfg.id().empty() ? "cmvr-es" : "cmvr-es:" + cfg.id()) - : cfg.control_nick_name()), - recv_timeout_ms_(cfg.recv_timeout_ms() > 0 ? cfg.recv_timeout_ms() : 1000), - state_push_enabled_(cfg.enable_state_push()), - map_update_enabled_(cfg.enable_map_update()), - map_update_interval_ms_(cfg.map_update_interval_ms() > 0 ? cfg.map_update_interval_ms() : kDefaultMapUpdateIntervalMs), - map_update_history_size_(cfg.map_update_history_size() > 0 ? cfg.map_update_history_size() : kDefaultMapUpdateHistorySize) -{ - id_ = cfg.id(); - if (cfg.port_status() > 0) ports_.status = cfg.port_status(); - if (cfg.port_control() > 0) ports_.control = cfg.port_control(); - if (cfg.port_nav() > 0) ports_.navigation = cfg.port_nav(); - if (cfg.port_config() > 0) ports_.config = cfg.port_config(); - if (cfg.port_other() > 0) ports_.other = cfg.port_other(); - if (cfg.port_push() > 0) ports_.push = cfg.port_push(); - - const auto result = connect_(); - if (!result.ok()) { - CMVR_LOG(ERROR) << "[Src1100Agv] Auto connect failed" - << ", id=" << id_ - << ", ip=" << ip_ - << ", error=" << result.message; - } -} - -Src1100Agv::~Src1100Agv() -{ - (void)disconnect_(); -} - -bool Src1100Agv::init() -{ - return !id_.empty() && !ip_.empty(); -} - -bool Src1100Agv::start() -{ - return true; -} - -bool Src1100Agv::stop() -{ - return true; -} - -bool Src1100Agv::update() -{ - return true; -} - -AgvRuntimeState Src1100Agv::runtimeState() const -{ - AgvRuntimeState cached_state; - bool has_cached_state = false; - if (state_push_enabled_) { - std::lock_guard lock(runtime_state_mutex_); - if (cached_runtime_state_valid_) { - cached_state = cached_runtime_state_; - has_cached_state = true; - } - } - - if (has_cached_state) { - std::string adapter_error; - { - std::lock_guard lock(mutex_); - cached_state.connected = connected_(); - adapter_error = last_error_; - } - if (!adapter_error.empty()) { - if (cached_state.last_error.empty()) { - cached_state.last_error = adapter_error; - } else if (cached_state.last_error != adapter_error) { - cached_state.last_error += "; adapter_error=" + adapter_error; - } - } - if (!cached_state.connected) { - cached_state.mode = AgvMode::Disconnected; - } - return cached_state; - } - - return queryRuntimeState_(); -} - -AgvRuntimeState Src1100Agv::queryRuntimeState_() const -{ - AgvRuntimeState state; - { - std::lock_guard lock(mutex_); - state.connected = connected_(); - state.last_error = last_error_; - } - state.mode = state.connected ? AgvMode::Idle : AgvMode::Disconnected; - - Json::Value loc; - if (sendCommand_(sock_status_, kRobotStatusLoc, Json::Value(Json::objectValue), &loc).ok()) { - state.pose.x = jsonGet(loc, "x", 0.0).asDouble(); - state.pose.y = jsonGet(loc, "y", 0.0).asDouble(); - state.pose.theta = jsonGet(loc, "angle", 0.0).asDouble(); - state.localized = jsonGet(loc, "confidence", 0.0).asDouble() > 0.0; - state.current_station = jsonGet(loc, "current_station", "").asString(); - } - - Json::Value battery; - if (sendCommand_(sock_status_, kRobotStatusBattery, Json::Value(Json::objectValue), &battery).ok()) { - state.battery.percentage = jsonGet(battery, "battery_level", 0.0).asDouble(); - state.battery.temperature = jsonGet(battery, "battery_temp", 0.0).asDouble(); - state.battery.charging = jsonGet(battery, "charging", false).asBool(); - state.battery.voltage = jsonGet(battery, "voltage", 0.0).asDouble(); - state.battery.current = jsonGet(battery, "current", 0.0).asDouble(); - } - - Json::Value map; - if (sendCommand_(sock_status_, kRobotStatusMap, Json::Value(Json::objectValue), &map).ok()) { - state.current_map = jsonGet(map, "current_map", "").asString(); - } - - const auto nav = navigationStatus(); - state.moving = nav.state == AgvTaskState::Running; - state.fault = nav.state == AgvTaskState::Failed; - state.mode = state.fault ? AgvMode::Fault : modeFromTaskState(static_cast(nav.state)); - return state; -} - -AgvNavigationStatus Src1100Agv::navigationStatus() const -{ - AgvNavigationStatus status; - std::string missing_pose_task_detail; - for (int attempt = 0; attempt < 2; ++attempt) { - PoseTaskContext pose_context; - if (!currentPoseTask_(pose_context)) { - break; - } - const auto observed_navigation_generation = - navigation_generation_.load(std::memory_order_relaxed); - if (pose_context.navigation_generation - != observed_navigation_generation) { - continue; - } - - PoseTaskStatus task_status; - const auto result = queryPoseTaskStatus_(pose_context.task_id, task_status); - PoseTaskContext latest_context; - if (navigation_generation_.load(std::memory_order_relaxed) - != observed_navigation_generation - || !currentPoseTask_(latest_context) - || latest_context.navigation_generation - != pose_context.navigation_generation - || latest_context.task_id != pose_context.task_id) { - continue; - } - - status.type = AgvTaskType::NavigateToPose; - const auto fault_monitoring_unavailable = - [this, &pose_context]() { - if (controller_fault_channel_epoch_.load( - std::memory_order_relaxed) - != pose_context - .controller_fault_channel_epoch_at_start) { - return std::string( - "the controller fault push channel changed or was " - "invalidated after the free-navigation command was " - "accepted"); - } - return freeNavigationFaultStateUnavailableDetail_(); - }; - if (!result.ok()) { - status.state = AgvTaskState::Failed; - status.message = result.message; - return status; - } - if (!task_status.found) { - std::uint64_t missing_task_fault_control_attempt = 0; - const std::string missing_task_fault = - cachedControllerFaultDetail_( - pose_context.controller_fault_sequence_at_start, - controllerFaultCaptureGraceMs_(), - &missing_task_fault_control_attempt); - PoseTaskContext post_missing_context; - if (navigation_generation_.load(std::memory_order_relaxed) - != observed_navigation_generation - || !currentPoseTask_(post_missing_context) - || post_missing_context.navigation_generation - != pose_context.navigation_generation - || post_missing_context.task_id != pose_context.task_id) { - continue; - } - if (!missing_task_fault.empty()) { - std::string attribution; - if (missing_task_fault_control_attempt != 0 - && missing_task_fault_control_attempt - != pose_context.control_attempt_sequence_at_start) { - attribution = - "controller_fault_attribution=ambiguous because the " - "fault was observed after another control command " - "attempt had begun, "; - } - status.state = AgvTaskState::Failed; - status.message = - "SRC1100 tracked free-navigation task disappeared from " - "1110 task_status_package while a new controller fault " - "was observed: " + task_status.detail + ", " - + attribution + missing_task_fault; - clearPoseTask_(pose_context.navigation_generation); - return status; - } - if (const std::string unavailable = - fault_monitoring_unavailable(); - !unavailable.empty()) { - status.state = AgvTaskState::Failed; - status.message = - "SRC1100 tracked free-navigation status is unsafe to " - "accept because controller fault monitoring is " - "unavailable: " + unavailable - + "; query the controller and cancel or stop before " - "another motion command"; - return status; - } - missing_pose_task_detail = task_status.detail; - clearPoseTask_(pose_context.navigation_generation); - break; - } - if (task_status.type != 1) { - status.state = AgvTaskState::Failed; - status.type = toTaskType(task_status.type); - status.message = - "SRC1100 returned an unexpected task type for the tracked " - "free-navigation task: " + task_status.detail; - clearPoseTask_(pose_context.navigation_generation); - return status; - } - status.state = toTaskState(task_status.state); - status.progress = task_status.progress; - status.message = task_status.detail; - const auto controller_reported_state = status.state; - const bool controller_state_terminal = - controller_reported_state == AgvTaskState::Completed - || controller_reported_state == AgvTaskState::Failed - || controller_reported_state == AgvTaskState::Canceled; - std::uint64_t fault_control_attempt = 0; - const std::string fault = cachedControllerFaultDetail_( - pose_context.controller_fault_sequence_at_start, - controller_reported_state == AgvTaskState::Completed - || controller_reported_state == AgvTaskState::Failed - ? controllerFaultCaptureGraceMs_() - : 0, - &fault_control_attempt); - PoseTaskContext post_fault_context; - if (navigation_generation_.load(std::memory_order_relaxed) - != observed_navigation_generation - || !currentPoseTask_(post_fault_context) - || post_fault_context.navigation_generation - != pose_context.navigation_generation - || post_fault_context.task_id != pose_context.task_id) { - continue; - } - const std::string unavailable = - fault_monitoring_unavailable(); - if (!fault.empty()) { - std::string attribution; - if (fault_control_attempt != 0 - && fault_control_attempt - != pose_context.control_attempt_sequence_at_start) { - attribution = - "controller_fault_attribution=ambiguous because the " - "fault was observed after another control command " - "attempt had begun, "; - } - status.state = AgvTaskState::Failed; - status.message = - "SRC1100 reported a new controller fault while the tracked " - "free-navigation task had controller_task_state=" - + std::to_string(task_status.state) + ": " - + task_status.detail + ", " + attribution + fault; - if (!unavailable.empty()) { - status.message += - ", controller_fault_monitoring_unavailable=" - + unavailable; - } - } else if (!unavailable.empty()) { - if (controller_reported_state == AgvTaskState::Failed - || controller_reported_state == AgvTaskState::Canceled) { - status.message += - ", controller_fault_monitoring_unavailable=" - + unavailable; - } else { - status.state = AgvTaskState::Failed; - status.message = - "SRC1100 tracked free-navigation state is unsafe to accept " - "because controller fault monitoring became unavailable: " - + unavailable - + "; query the controller and cancel or stop before " - "another motion command"; - return status; - } - } - if (status.state == AgvTaskState::Completed) { - std::string pose_detail; - const bool target_reached = - poseTargetReached_(pose_context, pose_detail); - PoseTaskContext post_pose_context; - if (navigation_generation_.load(std::memory_order_relaxed) - != observed_navigation_generation - || !currentPoseTask_(post_pose_context) - || post_pose_context.navigation_generation - != pose_context.navigation_generation - || post_pose_context.task_id != pose_context.task_id) { - continue; - } - std::uint64_t post_pose_fault_control_attempt = 0; - const std::string post_pose_fault = - cachedControllerFaultDetail_( - pose_context.controller_fault_sequence_at_start, - 0, - &post_pose_fault_control_attempt); - if (!post_pose_fault.empty()) { - status.state = AgvTaskState::Failed; - std::string attribution; - if (post_pose_fault_control_attempt != 0 - && post_pose_fault_control_attempt - != pose_context - .control_attempt_sequence_at_start) { - attribution = - "controller_fault_attribution=ambiguous because the " - "fault was observed after another control command " - "attempt had begun, "; - } - status.message = - "SRC1100 reported the tracked free-navigation task " - "Completed, but a new controller fault was observed during " - "target verification: " + task_status.detail + ", " - + attribution + post_pose_fault; - } else if (const std::string post_pose_unavailable = - fault_monitoring_unavailable(); - !post_pose_unavailable.empty()) { - status.state = AgvTaskState::Failed; - status.message = - "SRC1100 tracked free-navigation completion is unsafe to " - "accept because controller fault monitoring became " - "unavailable: " + post_pose_unavailable - + "; query the controller and cancel or stop before " - "another motion command"; - return status; - } else if (!target_reached) { - status.state = AgvTaskState::Failed; - status.message = - "SRC1100 reported the tracked free-navigation task " - "Completed, but the requested target was not reached: " - + task_status.detail + ", " + pose_detail; - } else { - status.message += ", target_verified: " + pose_detail; - } - } - if (controller_state_terminal) { - clearPoseTask_(pose_context.navigation_generation); - } - return status; - } - - PoseTaskContext changed_context; - if (currentPoseTask_(changed_context)) { - status.state = AgvTaskState::Waiting; - status.type = AgvTaskType::NavigateToPose; - status.message = - "SRC1100 free-navigation task changed while its status was being " - "queried; query navigation status again"; - return status; - } - - Json::Value payload(Json::objectValue); - jsonMember(payload, "simple") = false; - - Json::Value response; - const auto result = sendCommand_(sock_status_, kRobotStatusTask, payload, &response); - if (!result.ok()) { - status.state = AgvTaskState::Failed; - status.message = missing_pose_task_detail.empty() - ? result.message - : missing_pose_task_detail + "; 1020 status query failed: " - + result.message; - return status; - } - const auto controller_result = resultFromResponse_(response); - if (!controller_result.ok()) { - status.state = AgvTaskState::Failed; - status.message = missing_pose_task_detail.empty() - ? controller_result.message - : missing_pose_task_detail + "; 1020 status query failed: " - + controller_result.message; - return status; - } - - status.state = toTaskState(jsonGet(response, "task_status", 0).asInt()); - status.type = toTaskType(jsonGet(response, "task_type", 0).asInt()); - status.message = jsonGet(response, "move_status_info", jsonGet(response, "err_msg", "")).asString(); - if (!missing_pose_task_detail.empty()) { - status.message = missing_pose_task_detail - + "; fallback_1020_status=" + std::to_string( - jsonGet(response, "task_status", 0).asInt()) - + ", fallback_1020_type=" + std::to_string( - jsonGet(response, "task_type", 0).asInt()) - + (status.message.empty() ? std::string{} : ", " + status.message); - } - if (const auto* task_status_package = jsonFind(response, "task_status_package")) { - status.progress = jsonGet(*task_status_package, "percentage", 0.0).asDouble(); - } - return status; -} - -AgvResult Src1100Agv::connect_() -{ - const auto lifecycle_generation = - navigation_generation_.fetch_add(1, std::memory_order_relaxed) + 1; - clearPoseTask_(lifecycle_generation); - stopPushThread_(); - stopMapUpdateThread_(); - - { - // Status requests may wait for a controller receive timeout without - // holding mutex_. Serialize lifecycle changes with that channel before - // replacing or closing its descriptor. - std::lock_guard status_io_lock(status_io_mutex_); - std::lock_guard lock(mutex_); - closeSocket_(sock_status_); - closeSocket_(sock_control_); - closeSocket_(sock_navigation_); - closeSocket_(sock_config_); - closeSocket_(sock_other_); - closeSocket_(sock_push_); - - if (ip_.empty()) { - return AgvResult::failure(AgvErrorCode::InvalidArgument, "SRC1100 AGV ip is empty"); - } - - const auto close_all = [this]() { - closeSocket_(sock_status_); - closeSocket_(sock_control_); - closeSocket_(sock_navigation_); - closeSocket_(sock_config_); - closeSocket_(sock_other_); - closeSocket_(sock_push_); - }; - - if (auto result = connectSocket_(sock_status_, ports_.status); !result.ok()) { - close_all(); - return result; - } - if (auto result = connectSocket_(sock_control_, ports_.control); !result.ok()) { - close_all(); - return result; - } - if (auto result = connectSocket_(sock_navigation_, ports_.navigation); !result.ok()) { - close_all(); - return result; - } - if (auto result = connectSocket_(sock_config_, ports_.config); !result.ok()) { - close_all(); - return result; - } - - if (state_push_enabled_) { - const auto result = connectSocket_(sock_push_, ports_.push); - if (!result.ok()) { - CMVR_LOG(ERROR) << "[Src1100Agv] Connect push port failed" - << ", id=" << id_ - << ", port=" << ports_.push - << ", error=" << result.message; - closeSocket_(sock_push_); - } - } - last_error_.clear(); - } - - if (state_push_enabled_ && sock_push_ >= 0) { - const auto result = configurePush_(); - if (result.ok()) { - startPushThread_(); - } else { - CMVR_LOG(ERROR) << "[Src1100Agv] Configure push failed" - << ", id=" << id_ - << ", error=" << result.message; - std::lock_guard lock(mutex_); - closeSocket_(sock_push_); - } - } - if (map_update_enabled_) { - startMapUpdateThread_(); - } - return AgvResult::success(); -} - -AgvResult Src1100Agv::disconnect_() -{ - const auto lifecycle_generation = - navigation_generation_.fetch_add(1, std::memory_order_relaxed) + 1; - clearPoseTask_(lifecycle_generation); - stopMapUpdateThread_(); - stopPushThread_(); - std::lock_guard status_io_lock(status_io_mutex_); - std::lock_guard lock(mutex_); - closeSocket_(sock_status_); - closeSocket_(sock_control_); - closeSocket_(sock_navigation_); - closeSocket_(sock_config_); - closeSocket_(sock_other_); - closeSocket_(sock_push_); - return AgvResult::success(); -} - -AgvResult Src1100Agv::emergencyStop() -{ - // This is a controller-level software stop, not a substitute for the - // physical emergency-stop circuit. Keep both stop commands under one - // authority acquisition so no other command from this process can - // interleave between them. - std::lock_guard sequence_lock(control_sequence_mutex_); - control_attempt_sequence_.fetch_add( - 1, - std::memory_order_relaxed); - - const auto authority = acquireControl_(); - if (!authority.ok()) { - const std::string detail = authority.message.empty() ? "unknown error" : authority.message; - return AgvResult::failure( - authority.code, - "SRC1100 acquire control authority failed: " + detail); - } - - struct StopOutcome { - AgvResult result; - bool controller_outcome_unknown{false}; - }; - const auto send_stop = [this](const int sock, const std::uint16_t command) { - Json::Value response; - auto result = sendCommand_( - sock, - command, - Json::Value(Json::objectValue), - &response); - if (!result.ok()) { - return StopOutcome{ - withUnknownControllerOutcome(std::move(result)), - true}; - } - if (!hasNumericControllerRetCode(response)) { - return StopOutcome{ - withUnknownControllerOutcome(resultFromResponse_(response)), - true}; - } - return StopOutcome{resultFromResponse_(response), false}; - }; - - bool generation_advanced = false; - const auto advance_generation_if_needed = [this, &generation_advanced]( - const StopOutcome& outcome) { - if (!generation_advanced - && (outcome.result.ok() || outcome.controller_outcome_unknown)) { - // Publish immediately after the first accepted or indeterminate stop - // outcome. Waiting for the second stop response would leave a window - // in which pose-start confirmation could incorrectly return success. - navigation_generation_.fetch_add(1, std::memory_order_relaxed); - generation_advanced = true; - } - }; - - const auto motion_stop = send_stop(sock_control_, kRobotControlStop); - advance_generation_if_needed(motion_stop); - const auto navigation_cancel = send_stop(sock_navigation_, kRobotTaskCancel); - advance_generation_if_needed(navigation_cancel); - if (generation_advanced) { - clearPoseTask_( - navigation_generation_.load(std::memory_order_relaxed)); - } - - if (!motion_stop.result.ok()) { - const std::string detail = motion_stop.result.message.empty() - ? "unknown error" - : motion_stop.result.message; - if (!navigation_cancel.result.ok()) { - const std::string cancel_detail = navigation_cancel.result.message.empty() - ? "unknown error" - : navigation_cancel.result.message; - return AgvResult::failure( - motion_stop.result.code, - "SRC1100 software stop failed: control stop: " + detail - + "; cancel navigation: " + cancel_detail); - } - return AgvResult::failure( - motion_stop.result.code, - "SRC1100 software stop failed: control stop: " + detail); - } - if (!navigation_cancel.result.ok()) { - const std::string detail = navigation_cancel.result.message.empty() - ? "unknown error" - : navigation_cancel.result.message; - return AgvResult::failure( - navigation_cancel.result.code, - "SRC1100 software stop failed: cancel navigation: " + detail); - } - return AgvResult::success(); -} - -AgvResult Src1100Agv::clearFault() -{ - return AgvResult::failure( - AgvErrorCode::UnsupportedCommand, - "SRC1100 clearFault command is not implemented"); -} - -AgvResult Src1100Agv::navigateToPose( - const math::Pose2d& pose, - const AgvMotionOptions& options, - const AgvAdapterParams& adapter_params) -{ - if (!std::isfinite(pose.x) - || !std::isfinite(pose.y) - || !std::isfinite(pose.theta)) { - return AgvResult::failure( - AgvErrorCode::InvalidArgument, - "SRC1100 free-navigation pose x, y, and theta must be finite"); - } - if (const std::string error = invalidMotionOption(options); - !error.empty()) { - return AgvResult::failure( - AgvErrorCode::InvalidArgument, - "SRC1100 free-navigation motion option " + error); - } - - // This SRC1100 firmware exposes arbitrary-pose navigation through the - // vendor-specific freeGo extension of API 3051. The empty target id and - // GotoSpecifiedPose skill are part of the controller payload that was - // validated on the differential-drive chassis. - std::string source_id = adapter_params.getString("source_id").value_or("SELF_POSITION"); - if (source_id.empty()) { - source_id = "SELF_POSITION"; - } - if (source_id != "SELF_POSITION") { - return AgvResult::failure( - AgvErrorCode::InvalidArgument, - "SRC1100 free navigation source_id must be SELF_POSITION"); - } - const std::string requested_target_id = - adapter_params.getString("target_id").value_or(""); - if (!requested_target_id.empty() - && requested_target_id != "SELF_POSITION") { - return AgvResult::failure( - AgvErrorCode::InvalidArgument, - "SRC1100 free navigation target_id must be empty or SELF_POSITION so a " - "malformed freeGo request cannot fall back to station navigation"); - } - const std::string target_id; - - std::string skill_name = - adapter_params.getString("skill_name").value_or("GotoSpecifiedPose"); - if (skill_name.empty()) { - skill_name = "GotoSpecifiedPose"; - } - if (skill_name != "GotoSpecifiedPose") { - return AgvResult::failure( - AgvErrorCode::InvalidArgument, - "SRC1100 free navigation skill_name must be GotoSpecifiedPose"); - } - - const auto task_sequence = - pose_task_sequence_.fetch_add(1, std::memory_order_relaxed) + 1; - std::string task_id_prefix = - adapter_params.getString("task_id").value_or(id_); - if (task_id_prefix.empty()) { - task_id_prefix = id_; - } - const std::string task_id = - makePoseTaskId(task_id_prefix, task_sequence); - - Json::Value payload(Json::objectValue); - jsonMember(payload, "source_id") = source_id; - jsonMember(payload, "id") = target_id; - jsonMember(payload, "task_id") = task_id; - jsonMember(payload, "skill_name") = skill_name; - - auto& free_go = jsonMember(payload, "freeGo"); - jsonMember(free_go, "x") = pose.x; - jsonMember(free_go, "y") = pose.y; - jsonMember(free_go, "theta") = pose.theta; - - // Only strongly typed motion fields and the string whitelist above are - // accepted here. Generic adapter passthrough could inject unrelated 3051 - // operations such as lift, fork, script, or digital-I/O actions. - applyMotionOptions_(payload, options); - - PoseTaskContext context; - context.task_id = task_id; - context.target = pose; - context.reach_distance = options.reach_distance > 0.0 - ? options.reach_distance - : kDefaultPoseReachDistance; - context.reach_angle = options.reach_angle > 0.0 - ? options.reach_angle - : kDefaultPoseReachAngle; - - Json::Value response; - std::uint64_t navigation_generation = 0; - auto result = sendControlledCommand_( - sock_navigation_, - kRobotTaskGoTarget, - payload, - &response, - &navigation_generation, - nullptr, - nullptr, - &context, - true); - if (!result.ok()) { - return result; - } - return confirmPoseNavigationStarted_(context); -} - -AgvResult Src1100Agv::navigateToStation( - const std::string& station_id, - const AgvMotionOptions& options, - const AgvAdapterParams& adapter_params) -{ - if (const std::string error = invalidMotionOption(options); - !error.empty()) { - return AgvResult::failure( - AgvErrorCode::InvalidArgument, - "SRC1100 station-navigation motion option " + error); - } - if (const auto jack_height = adapter_params.getString("jack_height")) { - double parsed_jack_height = 0.0; - if (!parseFiniteDouble(*jack_height, parsed_jack_height)) { - return AgvResult::failure( - AgvErrorCode::InvalidArgument, - "SRC1100 station-navigation adapter jack_height must be a " - "complete finite number"); - } - } - - Json::Value payload(Json::objectValue); - jsonMember(payload, "source_id") = adapter_params.getString("source_id").value_or("SELF_POSITION"); - jsonMember(payload, "id") = station_id; - applyAdapterParams_(payload, adapter_params); - // Canonical typed motion options must win over string-valued adapter - // extensions so the SRC controller receives JSON numbers. - applyMotionOptions_(payload, options); - Json::Value response; - std::uint64_t accepted_generation = 0; - auto result = sendControlledCommand_( - sock_navigation_, - kRobotTaskGoTarget, - payload, - &response, - &accepted_generation); - if (accepted_generation != 0) { - clearPoseTask_(accepted_generation); - } - return result; -} - -AgvResult Src1100Agv::followPath(const std::vector& path) -{ - Json::Value payload(Json::objectValue); - Json::Value tasks(Json::arrayValue); - int index = 0; - for (const auto& segment : path) { - Json::Value task(Json::objectValue); - jsonMember(task, "task_id") = id_ + "_path_" + std::to_string(index++); - jsonMember(task, "source_id") = segment.source_station; - jsonMember(task, "id") = segment.target_station; - tasks.append(task); - } - jsonMember(payload, "move_task_list") = tasks; - Json::Value response; - std::uint64_t accepted_generation = 0; - auto result = sendControlledCommand_( - sock_navigation_, - kRobotTaskGoTargetList, - payload, - &response, - &accepted_generation); - if (accepted_generation != 0) { - clearPoseTask_(accepted_generation); - } - return result; -} - -AgvResult Src1100Agv::pauseNavigation() -{ - Json::Value response; - std::uint64_t accepted_generation = 0; - std::uint64_t control_attempt_sequence = 0; - auto result = sendControlledCommand_( - sock_navigation_, - kRobotTaskPause, - Json::Value(Json::objectValue), - &response, - &accepted_generation, - nullptr, - &control_attempt_sequence); - if (accepted_generation != 0) { - advancePoseTaskGeneration_( - accepted_generation, - control_attempt_sequence); - } - return result; -} - -AgvResult Src1100Agv::resumeNavigation() -{ - Json::Value response; - std::uint64_t accepted_generation = 0; - std::uint64_t control_attempt_sequence = 0; - auto result = sendControlledCommand_( - sock_navigation_, - kRobotTaskResume, - Json::Value(Json::objectValue), - &response, - &accepted_generation, - nullptr, - &control_attempt_sequence); - if (accepted_generation != 0) { - advancePoseTaskGeneration_( - accepted_generation, - control_attempt_sequence); - } - return result; -} - -AgvResult Src1100Agv::cancelNavigation() -{ - Json::Value response; - std::uint64_t accepted_generation = 0; - auto result = sendControlledCommand_( - sock_navigation_, - kRobotTaskCancel, - Json::Value(Json::objectValue), - &response, - &accepted_generation); - if (accepted_generation != 0) { - clearPoseTask_(accepted_generation); - } - return result; -} - -AgvResult Src1100Agv::setVelocity(const AgvVelocity& velocity) -{ - if (!std::isfinite(velocity.vx) - || !std::isfinite(velocity.vy) - || !std::isfinite(velocity.wz)) { - return AgvResult::failure( - AgvErrorCode::InvalidArgument, - "SRC1100 velocity vx, vy, and wz must be finite"); - } - - Json::Value payload(Json::objectValue); - jsonMember(payload, "vx") = velocity.vx; - jsonMember(payload, "vy") = velocity.vy; - jsonMember(payload, "w") = velocity.wz; - Json::Value response; - const bool stop_velocity = - velocity.vx == 0.0 && velocity.vy == 0.0 && velocity.wz == 0.0; - if (stop_velocity) { - std::uint64_t control_attempt_sequence = 0; - auto result = sendControlledCommand_( - sock_control_, - kRobotControlMotion, - payload, - &response, - nullptr, - nullptr, - &control_attempt_sequence); - result = result.ok() ? resultFromResponse_(response) : result; - if (result.ok()) { - advancePoseTaskControlAttempt_(control_attempt_sequence); - } - return result; - } - - std::uint64_t accepted_generation = 0; - auto result = sendControlledCommand_( - sock_control_, - kRobotControlMotion, - payload, - &response, - &accepted_generation); - if (accepted_generation != 0) { - clearPoseTask_(accepted_generation); - } - return result; -} - -AgvResult Src1100Agv::listMaps(std::vector& maps) const -{ - Json::Value response; - auto result = sendCommand_(sock_status_, kRobotStatusMap, Json::Value(Json::objectValue), &response); - if (!result.ok()) return result; - maps.clear(); - if (const auto* values = jsonFind(response, "maps"); values && values->isArray()) { - for (const auto& value : *values) { - maps.push_back(value.asString()); - } - } - return resultFromResponse_(response); -} - -AgvResult Src1100Agv::listStations(std::vector& stations) const -{ - Json::Value response; - auto result = sendCommand_(sock_status_, kRobotStatusStation, Json::Value(Json::objectValue), &response); - if (!result.ok()) return result; - stations.clear(); - if (const auto* values = jsonFind(response, "stations"); values && values->isArray()) { - for (const auto& value : *values) { - AgvStation station; - station.id = jsonGet(value, "id", "").asString(); - station.type = jsonGet(value, "type", "").asString(); - station.pose.x = jsonGet(value, "x", 0.0).asDouble(); - station.pose.y = jsonGet(value, "y", 0.0).asDouble(); - station.pose.theta = jsonGet(value, "r", 0.0).asDouble(); - station.description = jsonGet(value, "desc", "").asString(); - stations.push_back(station); - } - } - return resultFromResponse_(response); -} - -AgvResult Src1100Agv::switchMap(const std::string& map_name) -{ - Json::Value payload(Json::objectValue); - jsonMember(payload, "map_name") = map_name; - Json::Value response; - std::uint64_t accepted_generation = 0; - auto result = sendControlledCommand_( - sock_control_, - kRobotControlLoadMap, - payload, - &response, - &accepted_generation); - if (accepted_generation != 0) { - clearPoseTask_(accepted_generation); - } - return result; -} - -AgvResult Src1100Agv::uploadMap(const std::string& map_name, const std::string& content) -{ - Json::Value payload(Json::objectValue); - jsonMember(payload, "map_name") = map_name; - jsonMember(payload, "map_content") = content; - Json::Value response; - auto result = sendControlledCommand_(sock_config_, kRobotConfigUploadMap, payload, &response); - return result.ok() ? resultFromResponse_(response) : result; -} - -AgvResult Src1100Agv::downloadMap(const std::string& map_name, std::string& content) const -{ - Json::Value payload(Json::objectValue); - jsonMember(payload, "map_name") = map_name; - Json::Value response; - auto result = sendCommand_(sock_config_, kRobotConfigDownloadMap, payload, &response); - if (!result.ok()) return result; - content = jsonGet(response, "map_content", jsonGet(response, "content", "")).asString(); - return resultFromResponse_(response); -} - -AgvResult Src1100Agv::startMapping(const AgvMappingOptions& options) -{ - auto result = ensureOtherSocket_(); - if (!result.ok()) return result; - - Json::Value payload(Json::objectValue); - jsonMember(payload, "slam_type") = options.dimension == AgvMapDimension::Map2D ? 2 : 4; - jsonMember(payload, "real_time") = options.real_time; - if (!options.map_name.empty()) { - jsonMember(payload, "map_name") = options.map_name; - } - - Json::Value response; - std::uint64_t accepted_generation = 0; - result = sendControlledCommand_( - sock_other_, - kRobotOtherStartMapping, - payload, - &response, - &accepted_generation); - if (accepted_generation != 0) { - clearPoseTask_(accepted_generation); - } - if (result.ok()) { - { - std::lock_guard lock(map_update_mutex_); - cached_map_updates_.clear(); - next_mapping_index_ = 0; - last_map_content_hash_ = 0; - map_sequence_ = 0; - map_session_id_ = id_ + "_mapping_" + std::to_string(static_cast(nowSeconds() * 1000.0)); - } - if (map_update_enabled_ || options.real_time) { - startMapUpdateThread_(); - } - } - return result; -} - -AgvResult Src1100Agv::getMappingData(const int start_index, AgvMappingData& data) const -{ - if (start_index < 0) { - return AgvResult::failure(AgvErrorCode::InvalidArgument, "mapping data start_index must be >= 0"); - } - - Json::Value list_payload(Json::objectValue); - jsonMember(list_payload, "index") = start_index; - - Json::Value list_response; - auto result = sendCommand_(sock_status_, kRobotStatusMappingFileList, list_payload, &list_response); - if (!result.ok()) return result; - result = resultFromResponse_(list_response); - if (!result.ok()) return result; - - data = {}; - data.start_index = start_index; - data.next_index = start_index; - - const auto* list = jsonFind(list_response, "list"); - if (!list || !list->isArray()) { - return AgvResult::success(); - } - - for (const auto& item : *list) { - const std::string file_name = item.asString(); - if (file_name.empty()) { - continue; - } - - Json::Value download_payload(Json::objectValue); - jsonMember(download_payload, "type") = "users"; - jsonMember(download_payload, "file_path") = file_name; - - std::string content; - result = sendCommandRaw_(sock_status_, kRobotStatusDownloadFile, download_payload, &content); - if (!result.ok()) return result; - - Json::Value maybe_error; - std::string parse_error; - if (parseJson_(content, maybe_error, parse_error) && maybe_error.isObject()) { - result = resultFromResponse_(maybe_error); - if (!result.ok()) return result; - content = jsonGet(maybe_error, "content", jsonGet(maybe_error, "file_content", content)).asString(); - } - - AgvMappingDataFile file; - file.name = file_name; - file.content = std::move(content); - data.files.push_back(std::move(file)); - } - - data.next_index = data.start_index + static_cast(data.files.size()); - return AgvResult::success(); -} - -AgvResult Src1100Agv::getUnifiedMapUpdate( - const std::uint64_t after_sequence, - const AgvMapStreamOptions& options, - AgvUnifiedMapUpdate& update) const -{ - if (findCachedMapUpdate_(after_sequence, options, update)) { - return AgvResult::success(); - } - - const auto refresh_result = refreshMapCacheOnce_(options); - if (findCachedMapUpdate_(after_sequence, options, update)) { - return AgvResult::success(); - } - if (!refresh_result.ok() && refresh_result.code != AgvErrorCode::Timeout) { - return refresh_result; - } - - const auto wait_ms = options.wait_timeout_ms > 0 ? options.wait_timeout_ms : 1000; - std::unique_lock lock(map_update_mutex_); - const auto effective_after = [&]() { - if (after_sequence != 0 || options.resume_token.empty()) { - return after_sequence; - } - try { - return static_cast(std::stoull(options.resume_token)); - } catch (...) { - return std::uint64_t{0}; - } - }(); - const auto find_locked = [&]() { - for (const auto& candidate : cached_map_updates_) { - if (candidate.sequence > effective_after && mapUpdateMatches_(candidate, options)) { - update = candidate; - return true; - } - } - return false; - }; - - if (find_locked()) { - return AgvResult::success(); - } - const bool ready = map_update_cv_.wait_for( - lock, - std::chrono::milliseconds(wait_ms), - find_locked); - if (ready) { - return AgvResult::success(); - } - return AgvResult::failure(AgvErrorCode::Timeout, "SRC1100 unified map update timeout"); -} - -void Src1100Agv::startMapUpdateThread_() -{ - if (map_update_running_.exchange(true)) { - return; - } - map_update_thread_ = std::thread(&Src1100Agv::mapUpdateLoop_, this); -} - -void Src1100Agv::stopMapUpdateThread_() -{ - const bool was_running = map_update_running_.exchange(false); - if (was_running) { - map_update_cv_.notify_all(); - } - if (map_update_thread_.joinable()) { - map_update_thread_.join(); - } -} - -void Src1100Agv::mapUpdateLoop_() -{ - while (map_update_running_) { - AgvMapStreamOptions options; - options.dimension = AgvMapDimension::Map2DAnd3D; - options.snapshot = true; - options.incremental = true; - options.wait_timeout_ms = 0; - - const auto result = refreshMapCacheOnce_(options); - if (!result.ok() && result.code != AgvErrorCode::Timeout) { - std::lock_guard lock(mutex_); - last_error_ = result.message; - } - - std::unique_lock lock(map_update_mutex_); - map_update_cv_.wait_for( - lock, - std::chrono::milliseconds(map_update_interval_ms_), - [this]() { return !map_update_running_; }); - } -} - -AgvResult Src1100Agv::refreshMapCacheOnce_(const AgvMapStreamOptions& options) const -{ - int start_index = 0; - { - std::lock_guard lock(map_update_mutex_); - start_index = next_mapping_index_; - } - - AgvMappingData mapping_data; - auto result = getMappingData(start_index, mapping_data); - if (result.ok() && !mapping_data.files.empty()) { - std::vector updates; - for (const auto& file : mapping_data.files) { - std::vector file_updates; - const auto parse_result = parseMapFileToUpdates_(file.name, file.content, options, file_updates); - if (!parse_result.ok()) { - CMVR_LOG(ERROR) << "[Src1100Agv] Parse mapping file failed" - << ", id=" << id_ - << ", file=" << file.name - << ", error=" << parse_result.message; - continue; - } - updates.insert( - updates.end(), - std::make_move_iterator(file_updates.begin()), - std::make_move_iterator(file_updates.end())); - } - { - std::lock_guard lock(map_update_mutex_); - next_mapping_index_ = std::max(next_mapping_index_, mapping_data.next_index); - } - if (!updates.empty()) { - cacheMapUpdates_(std::move(updates)); - return AgvResult::success(); - } - } - - std::string map_name = options.map_name; - if (map_name.empty()) { - const auto state = runtimeState(); - map_name = state.current_map; - } - if (map_name.empty()) { - std::vector maps; - if (listMaps(maps).ok() && !maps.empty()) { - map_name = maps.back(); - } - } - if (map_name.empty()) { - return result.ok() - ? AgvResult::failure(AgvErrorCode::Timeout, "SRC1100 no map file is available") - : result; - } - - std::string content; - result = downloadMap(map_name, content); - if (!result.ok()) { - return result; - } - const auto content_hash = std::hash{}(content); - std::size_t last_map_content_hash = 0; - { - std::lock_guard lock(map_update_mutex_); - last_map_content_hash = last_map_content_hash_; - } - AgvUnifiedMapUpdate cached; - if (content_hash == last_map_content_hash && findCachedMapUpdate_(0, options, cached)) { - return AgvResult::success(); - } - - std::vector updates; - result = parseMapFileToUpdates_(map_name, content, options, updates); - if (!result.ok()) { - return result; - } - if (updates.empty()) { - return AgvResult::failure(AgvErrorCode::Timeout, "SRC1100 map file has no requested dimension"); - } - - { - std::lock_guard lock(map_update_mutex_); - last_map_content_hash_ = content_hash; - } - cacheMapUpdates_(std::move(updates)); - return AgvResult::success(); -} - -AgvResult Src1100Agv::parseMapFileToUpdates_( - const std::string& file_name, - const std::string& content, - const AgvMapStreamOptions& options, - std::vector& updates) const -{ - if (content.empty()) { - return AgvResult::failure(AgvErrorCode::InvalidArgument, "SRC1100 map file is empty: " + file_name); - } - - if (contentLooksLikeZip(content)) { - return parseSrc1100MapArchive_(file_name, content, options, updates); - } - - if (contentLooksLikeJson(content)) { - if (wants2D(options.dimension)) { - AgvUnifiedMapUpdate update; - const auto result = parseSrc1100Map2D_(file_name, content, options, update); - if (!result.ok()) { - return result; - } - updates.push_back(std::move(update)); - } - return AgvResult::success(); - } - - if (wants3D(options.dimension)) { - AgvUnifiedMapUpdate update; - const auto result = parseSrc1100Map3D_(file_name, content, options, update); - if (!result.ok()) { - return result; - } - updates.push_back(std::move(update)); - return AgvResult::success(); - } - - return AgvResult::success(); -} - -AgvResult Src1100Agv::parseSrc1100MapArchive_( - const std::string& file_name, - const std::string& content, - const AgvMapStreamOptions& options, - std::vector& updates) const -{ - const auto temp_dir = makeTempDirectory(); - if (temp_dir.empty()) { - return AgvResult::failure(AgvErrorCode::CommandFailed, "create temporary map directory failed: " + systemError()); - } - - const auto archive_path = temp_dir / "map.smap"; - if (!writeBinaryFile(archive_path, content)) { - fs::remove_all(temp_dir); - return AgvResult::failure(AgvErrorCode::CommandFailed, "write temporary map archive failed"); - } - - const std::string command = "unzip -qq -o " - + shellQuote(archive_path.string()) - + " -d " - + shellQuote(temp_dir.string()); - const int unzip_result = std::system(command.c_str()); - if (unzip_result != 0) { - fs::remove_all(temp_dir); - return AgvResult::failure(AgvErrorCode::CommandFailed, "unzip SRC1100 smap archive failed: " + file_name); - } - - if (wants2D(options.dimension)) { - std::string map2d_content; - if (readBinaryFile(temp_dir / "0.smap", map2d_content)) { - AgvUnifiedMapUpdate update; - const auto result = parseSrc1100Map2D_(file_name, map2d_content, options, update); - if (result.ok()) { - updates.push_back(std::move(update)); - } else { - CMVR_LOG(ERROR) << "[Src1100Agv] Parse 0.smap failed" - << ", id=" << id_ - << ", file=" << file_name - << ", error=" << result.message; - } - } - } - - if (wants3D(options.dimension)) { - std::string map3d_content; - if (readBinaryFile(temp_dir / "0.3dsmap", map3d_content)) { - AgvUnifiedMapUpdate update; - const auto result = parseSrc1100Map3D_(file_name, map3d_content, options, update); - if (result.ok()) { - updates.push_back(std::move(update)); - } else { - CMVR_LOG(ERROR) << "[Src1100Agv] Parse 0.3dsmap failed" - << ", id=" << id_ - << ", file=" << file_name - << ", error=" << result.message; - } - } - } - - fs::remove_all(temp_dir); - return updates.empty() - ? AgvResult::failure(AgvErrorCode::CommandFailed, "SRC1100 smap archive has no requested map data: " + file_name) - : AgvResult::success(); -} - -AgvResult Src1100Agv::parseSrc1100Map2D_( - const std::string& file_name, - const std::string& content, - const AgvMapStreamOptions& options, - AgvUnifiedMapUpdate& update) const -{ - Json::Value root; - std::string error; - if (!parseJson_(content, root, error)) { - return AgvResult::failure(AgvErrorCode::CommandFailed, "parse SRC1100 2D map json failed: " + error); - } - if (!root.isObject()) { - return AgvResult::failure(AgvErrorCode::CommandFailed, "SRC1100 2D map json root is not object"); - } - - const auto* header_ptr = jsonFind(root, "header"); - const Json::Value& header = header_ptr && header_ptr->isObject() ? *header_ptr : root; - - AgvUnifiedMap2D map; - map.frame_id = "map"; - map.timestamp = nowSeconds(); - map.resolution = jsonGet(header, "resolution", 0.0).asDouble(); - if (const auto* min_pos = jsonFind(header, "min_pos")) { - map.origin.x = jsonGet(*min_pos, "x", 0.0).asDouble(); - map.origin.y = jsonGet(*min_pos, "y", 0.0).asDouble(); - map.origin.theta = 0.0; - } - if (const auto* max_pos = jsonFind(header, "max_pos"); - max_pos && map.resolution > 0.0) { - const double width_m = jsonGet(*max_pos, "x", map.origin.x).asDouble() - map.origin.x; - const double height_m = jsonGet(*max_pos, "y", map.origin.y).asDouble() - map.origin.y; - if (width_m > 0.0 && height_m > 0.0) { - map.width = static_cast(std::ceil(width_m / map.resolution)); - map.height = static_cast(std::ceil(height_m / map.resolution)); - } - } - - const auto make_id = [](const Json::Value& value, const char* prefix, const int index) { - std::string id = jsonGet(value, "instance_name", "").asString(); - if (id.empty()) id = jsonGet(value, "id", "").asString(); - if (id.empty()) id = jsonGet(value, "name", "").asString(); - if (id.empty()) id = jsonGet(value, "point_name", "").asString(); - if (id.empty() && jsonHas(value, "tag_value")) { - id = std::to_string(jsonGet(value, "tag_value", 0).asUInt()); - } - if (id.empty()) id = std::string(prefix) + "_" + std::to_string(index); - return id; - }; - - if (const auto* list = jsonFind(root, "advanced_point_list"); list && list->isArray()) { - int index = 0; - for (const auto& item : *list) { - const auto* pos = jsonFind(item, "pos"); - appendObject( - map, - make_id(item, "station", index++), - AgvMapObjectType::Station, - pos ? std::vector{jsonPoint3D(*pos)} : std::vector{}, - jsonGet(item, "dir", 0.0).asDouble(), - item); - } - } - - if (const auto* list = jsonFind(root, "normal_line_list"); list && list->isArray()) { - int index = 0; - for (const auto& item : *list) { - std::vector points; - if (const auto* start = jsonFind(item, "start_pos")) points.push_back(jsonPoint3D(*start)); - if (const auto* end = jsonFind(item, "end_pos")) points.push_back(jsonPoint3D(*end)); - appendObject(map, make_id(item, "normal_line", index++), AgvMapObjectType::Line, std::move(points), 0.0, item); - } - } - - if (const auto* list = jsonFind(root, "advanced_line_list"); list && list->isArray()) { - int index = 0; - for (const auto& item : *list) { - std::vector points; - if (const auto* line = jsonFind(item, "line")) { - if (const auto* start = jsonFind(*line, "start_pos")) points.push_back(jsonPoint3D(*start)); - if (const auto* end = jsonFind(*line, "end_pos")) points.push_back(jsonPoint3D(*end)); - } - appendObject(map, make_id(item, "line", index++), AgvMapObjectType::Line, std::move(points), 0.0, item); - } - } - - if (const auto* list = jsonFind(root, "advanced_curve_list"); list && list->isArray()) { - int index = 0; - for (const auto& item : *list) { - std::vector points; - if (const auto* start = jsonFind(item, "start_pos")) { - if (const auto* pos = jsonFind(*start, "pos")) points.push_back(jsonPoint3D(*pos)); - } - if (const auto* control = jsonFind(item, "control_pos1")) points.push_back(jsonPoint3D(*control)); - if (const auto* control = jsonFind(item, "control_pos2")) points.push_back(jsonPoint3D(*control)); - if (const auto* control = jsonFind(item, "control_pos3")) points.push_back(jsonPoint3D(*control)); - if (const auto* control = jsonFind(item, "control_pos4")) points.push_back(jsonPoint3D(*control)); - if (const auto* end = jsonFind(item, "end_pos")) { - if (const auto* pos = jsonFind(*end, "pos")) points.push_back(jsonPoint3D(*pos)); - } - appendObject(map, make_id(item, "curve", index++), AgvMapObjectType::Line, std::move(points), 0.0, item); - } - } - - if (const auto* list = jsonFind(root, "advanced_area_list"); list && list->isArray()) { - int index = 0; - for (const auto& item : *list) { - std::vector points; - if (const auto* pos_group = jsonFind(item, "pos_group"); pos_group && pos_group->isArray()) { - for (const auto& pos : *pos_group) points.push_back(jsonPoint3D(pos)); - } - appendObject( - map, - make_id(item, "area", index++), - AgvMapObjectType::Area, - std::move(points), - jsonGet(item, "dir", 0.0).asDouble(), - item); - } - } - - if (const auto* list = jsonFind(root, "reflector_pos_list"); list && list->isArray()) { - int index = 0; - for (const auto& item : *list) { - appendObject( - map, - make_id(item, "reflector", index++), - AgvMapObjectType::Reflector, - {jsonPoint3D(item)}, - 0.0, - item); - } - } - - if (const auto* list = jsonFind(root, "tag_pos_list"); list && list->isArray()) { - int index = 0; - for (const auto& item : *list) { - appendObject( - map, - make_id(item, "tag", index++), - AgvMapObjectType::QrTag, - {jsonPoint3D(item)}, - jsonGet(item, "angle", 0.0).asDouble(), - item); - } - } - - if (const auto* list = jsonFind(root, "external_device_list"); list && list->isArray()) { - int index = 0; - for (const auto& item : *list) { - appendObject( - map, - make_id(item, "external_device", index++), - AgvMapObjectType::ExternalDevice, - {}, - 0.0, - item); - } - } - - if (const auto* groups = jsonFind(root, "bin_locations_list"); groups && groups->isArray()) { - int index = 0; - for (const auto& group : *groups) { - const auto* list = jsonFind(group, "bin_location_list"); - if (!list || !list->isArray()) { - continue; - } - for (const auto& item : *list) { - const auto* pos = jsonFind(item, "pos"); - appendObject( - map, - make_id(item, "bin_location", index++), - AgvMapObjectType::BinLocation, - pos ? std::vector{jsonPoint3D(*pos)} : std::vector{}, - 0.0, - item); - } - } - } - - std::string map_id = options.map_name; - if (map_id.empty()) map_id = jsonGet(header, "map_name", "").asString(); - if (map_id.empty()) map_id = file_name; - - update = {}; - update.map_id = map_id; - update.dimension = AgvMapDimension::Map2D; - update.update_type = AgvMapUpdateType::Snapshot; - update.frame_id = map.frame_id; - update.timestamp = map.timestamp; - update.snapshot_begin = true; - update.snapshot_end = true; - update.chunk_index = 0; - update.chunk_count = 1; - update.map_2d = std::move(map); - return AgvResult::success(); -} - -AgvResult Src1100Agv::parseSrc1100Map3D_( - const std::string& file_name, - const std::string& content, - const AgvMapStreamOptions& options, - AgvUnifiedMapUpdate& update) const -{ - rbk::protocol::Message_Map3D src; - if (!src.ParseFromString(content)) { - return AgvResult::failure(AgvErrorCode::CommandFailed, "parse SRC1100 3D map protobuf failed: " + file_name); - } - - AgvUnifiedMap3D map; - map.frame_id = "map"; - map.timestamp = nowSeconds(); - if (src.has_feature_map_3d() && src.feature_map_3d().has_params()) { - map.voxel_resolution = src.feature_map_3d().params().max_voxel_size(); - } else if (src.has_header()) { - map.voxel_resolution = src.header().resolution(); - } - - map.points.reserve(static_cast(src.normal_pos3d_list_size())); - for (const auto& point : src.normal_pos3d_list()) { - AgvMapPointSample3D sample; - sample.x = point.x(); - sample.y = point.y(); - sample.z = point.z(); - map.points.push_back(sample); - } - - if (src.has_feature_map_3d()) { - const auto& feature_map = src.feature_map_3d(); - map.planes.reserve(static_cast(feature_map.planes_size())); - for (const auto& plane : feature_map.planes()) { - AgvMapPlane3D dst; - dst.center = {plane.center().x(), plane.center().y(), plane.center().z()}; - dst.normal = {plane.normal().x(), plane.normal().y(), plane.normal().z()}; - dst.d = plane.d(); - dst.radius = plane.radius(); - map.planes.push_back(dst); - } - - map.voxels.reserve(static_cast(feature_map.voxel_locs_size())); - for (const auto& voxel : feature_map.voxel_locs()) { - AgvMapVoxel3D dst; - dst.x = voxel.x(); - dst.y = voxel.y(); - dst.z = voxel.z(); - dst.probability = 1.0F; - map.voxels.push_back(dst); - } - } - - std::string map_id = options.map_name; - if (map_id.empty() && src.has_header()) map_id = src.header().map_name(); - if (map_id.empty()) map_id = src.map_directory(); - if (map_id.empty()) map_id = file_name; - - update = {}; - update.map_id = map_id; - update.dimension = AgvMapDimension::Map3D; - update.update_type = AgvMapUpdateType::Snapshot; - update.frame_id = map.frame_id; - update.timestamp = map.timestamp; - update.snapshot_begin = true; - update.snapshot_end = true; - update.chunk_index = 0; - update.chunk_count = 1; - update.map_3d = std::move(map); - return AgvResult::success(); -} - -void Src1100Agv::cacheMapUpdates_(std::vector updates) const -{ - if (updates.empty()) { - return; - } - - { - std::lock_guard lock(map_update_mutex_); - if (map_session_id_.empty()) { - map_session_id_ = id_ + "_map"; - } - if (map_sequence_ == 0) { - map_sequence_ = kMapSnapshotSequenceStart - 1; - } - for (auto& update : updates) { - update.sequence = ++map_sequence_; - update.session_id = map_session_id_; - update.resume_token = std::to_string(update.sequence); - if (update.timestamp <= 0.0) update.timestamp = nowSeconds(); - if (update.frame_id.empty()) update.frame_id = "map"; - if (update.map_id.empty()) update.map_id = id_; - if (update.update_type == AgvMapUpdateType::Unspecified) { - update.update_type = AgvMapUpdateType::Snapshot; - } - cached_map_updates_.push_back(std::move(update)); - } - while (cached_map_updates_.size() > map_update_history_size_) { - cached_map_updates_.pop_front(); - } - } - map_update_cv_.notify_all(); -} - -bool Src1100Agv::findCachedMapUpdate_( - const std::uint64_t after_sequence, - const AgvMapStreamOptions& options, - AgvUnifiedMapUpdate& update) const -{ - std::uint64_t effective_after = after_sequence; - if (effective_after == 0 && !options.resume_token.empty()) { - try { - effective_after = static_cast(std::stoull(options.resume_token)); - } catch (...) { - effective_after = 0; - } - } - - std::lock_guard lock(map_update_mutex_); - for (const auto& candidate : cached_map_updates_) { - if (candidate.sequence > effective_after && mapUpdateMatches_(candidate, options)) { - update = candidate; - return true; - } - } - return false; -} - -bool Src1100Agv::mapUpdateMatches_( - const AgvUnifiedMapUpdate& update, - const AgvMapStreamOptions& options) const -{ - if (!options.map_name.empty() && update.map_id != options.map_name) { - return false; - } - - switch (options.dimension) { - case AgvMapDimension::Map2D: - return update.dimension == AgvMapDimension::Map2D && update.map_2d.has_value(); - case AgvMapDimension::Map3D: - return update.dimension == AgvMapDimension::Map3D && update.map_3d.has_value(); - case AgvMapDimension::Map2DAnd3D: - return (update.dimension == AgvMapDimension::Map2D && update.map_2d.has_value()) - || (update.dimension == AgvMapDimension::Map3D && update.map_3d.has_value()); - case AgvMapDimension::Unspecified: - default: - return update.map_2d.has_value() || update.map_3d.has_value(); - } -} - -AgvResult Src1100Agv::stopMapping() -{ - auto result = ensureOtherSocket_(); - if (!result.ok()) return result; - - Json::Value response; - std::uint64_t accepted_generation = 0; - result = sendControlledCommand_( - sock_other_, - kRobotOtherStopMapping, - Json::Value(Json::objectValue), - &response, - &accepted_generation); - if (accepted_generation != 0) { - clearPoseTask_(accepted_generation); - } - return result; -} - -AgvResult Src1100Agv::connectSocket_(int& sock, const int port) -{ - sock = ::socket(AF_INET, SOCK_STREAM, 0); - if (sock < 0) { - last_error_ = "create socket failed: " + systemError(); - return AgvResult::failure(AgvErrorCode::ConnectionFailed, last_error_); - } - - sockaddr_in address{}; - address.sin_family = AF_INET; - address.sin_port = htons(static_cast(port)); - if (::inet_pton(AF_INET, ip_.c_str(), &address.sin_addr) <= 0) { - closeSocket_(sock); - last_error_ = "invalid SRC1100 ip: " + ip_; - return AgvResult::failure(AgvErrorCode::InvalidArgument, last_error_); - } - - if (::connect(sock, reinterpret_cast(&address), sizeof(address)) < 0) { - closeSocket_(sock); - last_error_ = "connect SRC1100 port " + std::to_string(port) + " failed: " + systemError(); - return AgvResult::failure(AgvErrorCode::ConnectionFailed, last_error_); - } - - timeval timeout{}; - timeout.tv_sec = recv_timeout_ms_ / 1000; - timeout.tv_usec = (recv_timeout_ms_ % 1000) * 1000; - ::setsockopt(sock, SOL_SOCKET, SO_RCVTIMEO, &timeout, sizeof(timeout)); - return AgvResult::success(); -} - -AgvResult Src1100Agv::ensureOtherSocket_() -{ - std::lock_guard lock(mutex_); - if (sock_other_ >= 0) { - return AgvResult::success(); - } - return connectSocket_(sock_other_, ports_.other); -} - -void Src1100Agv::closeSocket_(int& sock) const -{ - if (sock >= 0) { - ::close(sock); - sock = -1; - } -} - -bool Src1100Agv::connected_() const -{ - return sock_status_ >= 0 && sock_control_ >= 0 && sock_navigation_ >= 0 && sock_config_ >= 0; -} - -AgvResult Src1100Agv::acquireControl_() const -{ - Json::Value payload(Json::objectValue); - jsonMember(payload, "nick_name") = control_nick_name_; - - Json::Value response; - auto result = sendCommand_(sock_config_, kRobotConfigLock, payload, &response); - return result.ok() ? resultFromResponse_(response) : result; -} - -void Src1100Agv::rememberPoseTask_(const PoseTaskContext& context) const -{ - std::lock_guard lock(pose_task_mutex_); - if (context.navigation_generation - < pose_task_context_.navigation_generation) { - return; - } - pose_task_context_ = context; -} - -void Src1100Agv::advancePoseTaskGeneration_( - const std::uint64_t navigation_generation, - const std::uint64_t control_attempt_sequence) const -{ - std::lock_guard lock(pose_task_mutex_); - if (navigation_generation - < pose_task_context_.navigation_generation) { - return; - } - pose_task_context_.navigation_generation = navigation_generation; - pose_task_context_.control_attempt_sequence_at_start = - control_attempt_sequence; -} - -void Src1100Agv::advancePoseTaskControlAttempt_( - const std::uint64_t control_attempt_sequence) const -{ - std::lock_guard lock(pose_task_mutex_); - if (pose_task_context_.task_id.empty() - || control_attempt_sequence - < pose_task_context_.control_attempt_sequence_at_start) { - return; - } - pose_task_context_.control_attempt_sequence_at_start = - control_attempt_sequence; -} - -void Src1100Agv::clearPoseTask_( - const std::uint64_t navigation_generation) const -{ - std::lock_guard lock(pose_task_mutex_); - if (navigation_generation < pose_task_context_.navigation_generation) { - return; - } - pose_task_context_ = PoseTaskContext{}; - pose_task_context_.navigation_generation = navigation_generation; -} - -bool Src1100Agv::currentPoseTask_(PoseTaskContext& context) const -{ - std::lock_guard lock(pose_task_mutex_); - if (pose_task_context_.task_id.empty()) { - return false; - } - context = pose_task_context_; - return true; -} - -std::string Src1100Agv::cachedControllerFaultDetail_( - const std::uint64_t after_sequence, - const int wait_ms, - std::uint64_t* associated_control_attempt) const -{ - std::unique_lock lock(runtime_state_mutex_); - const auto has_matching_fault = [this, after_sequence]() { - return !last_controller_fault_detail_.empty() - && controller_fault_sequence_ > after_sequence; - }; - if (!has_matching_fault() - && wait_ms > 0 - && state_push_enabled_) { - runtime_state_cv_.wait_for( - lock, - std::chrono::milliseconds(wait_ms), - has_matching_fault); - } - if (!has_matching_fault()) { - return {}; - } - if (associated_control_attempt) { - *associated_control_attempt = - last_controller_fault_control_attempt_; - } - return "cached_controller_fault_at=" - + std::to_string(last_controller_fault_timestamp_) - + ", " + last_controller_fault_detail_; -} - -int Src1100Agv::controllerFaultCaptureGraceMs_() const -{ - const int configured_interval = config_.state_push_interval_ms(); - const int effective_interval = configured_interval > 0 - ? configured_interval - : kDefaultControllerFaultPushIntervalMs; - const auto configured_grace = - static_cast(effective_interval) - + kControllerFaultPushJitterMs; - return static_cast(std::min( - std::max( - configured_grace, - static_cast(kMinimumControllerFaultCaptureGraceMs)), - static_cast(kMaximumControllerFaultCaptureGraceMs))); -} - -int Src1100Agv::controllerFaultStateMaxAgeMs_() const -{ - const int configured_interval = config_.state_push_interval_ms(); - const int effective_interval = configured_interval > 0 - ? std::min( - configured_interval, - kMaximumControllerFaultCaptureGraceMs - - kControllerFaultPushJitterMs) - : kDefaultControllerFaultPushIntervalMs; - const auto max_age = - static_cast(effective_interval) - * kControllerFaultStateMaxAgeIntervals - + kControllerFaultPushJitterMs; - return static_cast(std::max( - max_age, - static_cast(kMinimumControllerFaultStateMaxAgeMs))); -} - -std::string Src1100Agv::freeNavigationFaultStateUnavailableDetail_() const -{ - std::lock_guard lock(runtime_state_mutex_); - if (!state_push_enabled_) { - return "controller fault state is unavailable because state push is " - "disabled"; - } - // A newly reported active fault is handled through the sequenced fault - // cache, including its raw fatals/errors payload. Do not replace that - // diagnostic with the less specific "incomplete push" message. - if (!active_controller_fault_detail_.empty()) { - return {}; - } - if (!controller_fault_state_observed_) { - return "no complete state push containing fatals/errors is currently " - "available"; - } - const auto fault_state_age = - std::chrono::duration_cast( - std::chrono::steady_clock::now() - - controller_fault_state_observed_at_) - .count(); - const int max_age_ms = controllerFaultStateMaxAgeMs_(); - if (fault_state_age > max_age_ms) { - return "the most recent fatals/errors state push is stale (age_ms=" - + std::to_string(fault_state_age) - + ", max_age_ms=" + std::to_string(max_age_ms) + ")"; - } - return {}; -} - -AgvResult Src1100Agv::queryPoseTaskStatus_( - const std::string& task_id, - PoseTaskStatus& status) const -{ - status = PoseTaskStatus{}; - - Json::Value payload(Json::objectValue); - Json::Value task_ids(Json::arrayValue); - task_ids.append(task_id); - jsonMember(payload, "task_ids") = std::move(task_ids); - - Json::Value response; - auto result = sendCommand_( - sock_status_, - kRobotStatusTaskPackage, - payload, - &response); - if (!result.ok()) { - return result; - } - result = resultFromResponse_(response); - if (!result.ok()) { - return result; - } - - const auto* package = jsonFind(response, "task_status_package"); - if (package) { - status.progress = jsonGet(*package, "percentage", 0.0).asDouble(); - if (const auto* status_list = jsonFind(*package, "task_status_list"); - status_list && status_list->isArray()) { - for (const auto& item : *status_list) { - if (jsonGet(item, "task_id", "").asString() != task_id) { - continue; - } - status.found = true; - status.state = jsonGet(item, "status", 0).asInt(); - status.type = jsonGet(item, "type", 0).asInt(); - break; - } - } - } - - std::ostringstream detail; - detail << "task_id=" << task_id; - if (status.found) { - detail << ", task_status=" << status.state - << ", task_type=" << status.type; - } else { - detail << " not present in task_status_package"; - } - if (const auto* ret_code = jsonFind(response, "ret_code")) { - detail << ", status_query_ret_code=" << jsonValueToString(*ret_code); - } - const auto append_field = [&detail]( - const Json::Value& object, - const char* key, - const char* label) { - const auto* value = jsonFind(object, key); - if (!value || value->isNull()) { - return; - } - const std::string text = jsonValueToString(*value); - if (text.empty()) { - return; - } - detail << ", " << label << "=" << text; - }; - if (package) { - append_field(*package, "info", "info"); - append_field(*package, "closest_target", "closest_target"); - append_field(*package, "source_name", "source_name"); - append_field(*package, "target_name", "target_name"); - append_field(*package, "percentage", "percentage"); - append_field(*package, "distance", "distance"); - } - append_field(response, "create_on", "create_on"); - append_field(response, "err_msg", "status_query_err_msg"); - status.detail = detail.str(); - return AgvResult::success(); -} - -bool Src1100Agv::poseTargetReached_( - const PoseTaskContext& context, - std::string& detail) const -{ - math::Pose2d current_pose; - bool current_pose_available = false; - std::string pose_source; - std::string query_error; - - Json::Value response; - auto result = sendCommand_( - sock_status_, - kRobotStatusLoc, - Json::Value(Json::objectValue), - &response); - if (result.ok()) { - result = resultFromResponse_(response); - } - const auto* x = jsonFind(response, "x"); - const auto* y = jsonFind(response, "y"); - const auto* angle = jsonFind(response, "angle"); - if (result.ok() - && x && x->isNumeric() - && y && y->isNumeric() - && angle && angle->isNumeric()) { - current_pose.x = x->asDouble(); - current_pose.y = y->asDouble(); - current_pose.theta = angle->asDouble(); - if (std::isfinite(current_pose.x) - && std::isfinite(current_pose.y) - && std::isfinite(current_pose.theta)) { - current_pose_available = true; - pose_source = "controller_1004"; - } else { - query_error = "SRC1100 1004 response contained non-finite x/y/angle"; - } - } else if (!result.ok()) { - query_error = result.message; - } else { - query_error = - "SRC1100 1004 response did not contain numeric x/y/angle"; - } - - if (!current_pose_available) { - detail = "target pose could not be verified"; - if (!query_error.empty()) { - detail += ": " + query_error; - } - return false; - } - - const double distance_error = std::hypot( - current_pose.x - context.target.x, - current_pose.y - context.target.y); - const double angle_error = angleDistance( - current_pose.theta, - context.target.theta); - std::ostringstream description; - description << "pose_source=" << pose_source - << ", current_pose=(" << current_pose.x - << "," << current_pose.y - << "," << current_pose.theta - << "), target_pose=(" << context.target.x - << "," << context.target.y - << "," << context.target.theta - << "), distance_error=" << distance_error - << ", distance_tolerance=" << context.reach_distance - << ", angle_error=" << angle_error - << ", angle_tolerance=" << context.reach_angle; - if (!query_error.empty()) { - description << ", 1004_query_error=" << query_error; - } - detail = description.str(); - return distance_error <= context.reach_distance - && angle_error <= context.reach_angle; -} - -AgvResult Src1100Agv::confirmPoseNavigationStarted_( - const PoseTaskContext& context) const -{ - const auto deadline = std::chrono::steady_clock::now() + kPoseNavigationStartTimeout; - std::string last_status = "no task status received"; - int consecutive_running_samples = 0; - bool matching_task_observed = false; - int last_matching_state = 0; - bool last_poll_matched = false; - bool running_stability_window_active = false; - std::chrono::steady_clock::time_point running_stable_at{}; - const auto running_stability_window = std::chrono::milliseconds( - controllerFaultCaptureGraceMs_()); - const auto hard_deadline = deadline + running_stability_window; - - const auto superseded = [this, &context]() { - return navigation_generation_.load(std::memory_order_relaxed) - != context.navigation_generation; - }; - const auto superseded_result = []() { - return AgvResult::failure( - AgvErrorCode::CommandFailed, - "SRC1100 free-navigation start confirmation was superseded by " - "another accepted navigation, velocity, pause, or stop command; " - "the controller task state is unknown, so do not retry automatically " - "before querying or canceling navigation"); - }; - const auto fault_monitoring_unavailable = - [this, &context]() { - if (controller_fault_channel_epoch_.load( - std::memory_order_relaxed) - != context.controller_fault_channel_epoch_at_start) { - return std::string( - "the controller fault push channel changed or was " - "invalidated after the free-navigation command was " - "accepted"); - } - return freeNavigationFaultStateUnavailableDetail_(); - }; - const auto fault_monitoring_unavailable_result = - [](const std::string& detail) { - return AgvResult::failure( - AgvErrorCode::Fault, - "SRC1100 accepted the free-navigation command, but controller " - "fault monitoring became unavailable during start " - "confirmation: " + detail - + "; the task state is unsafe to accept, so query the " - "controller and cancel or stop before another motion " - "command"); - }; - const auto fault_attribution_is_ambiguous = - [&context](const std::uint64_t associated_control_attempt) { - return associated_control_attempt != 0 - && associated_control_attempt - != context.control_attempt_sequence_at_start; - }; - const auto ambiguous_fault_result = - [](const std::string& task_detail, const std::string& fault) { - return AgvResult::failure( - AgvErrorCode::Fault, - "SRC1100 reported a controller fault after another control " - "command attempt had begun; the fault " - "cannot be attributed to the tracked free-navigation task: " - + task_detail + ", " + fault - + "; query navigation status and cancel or stop before " - "another motion command"); - }; - - while (true) { - if (superseded()) { - return superseded_result(); - } - if (const std::string unavailable = - fault_monitoring_unavailable(); - !unavailable.empty()) { - return fault_monitoring_unavailable_result(unavailable); - } - - PoseTaskStatus task_status; - const auto query_result = queryPoseTaskStatus_( - context.task_id, - task_status); - if (!query_result.ok()) { - const std::string detail = query_result.message.empty() - ? "unknown error" - : query_result.message; - return AgvResult::failure( - query_result.code, - "SRC1100 accepted the free-navigation command, but task start " - "could not be verified: " + detail - + "; do not retry automatically before checking or canceling navigation"); - } - - if (superseded()) { - return superseded_result(); - } - if (const std::string unavailable = - fault_monitoring_unavailable(); - !unavailable.empty()) { - return fault_monitoring_unavailable_result(unavailable); - } - - last_status = task_status.detail; - last_poll_matched = task_status.found; - if (task_status.found) { - matching_task_observed = true; - last_matching_state = task_status.state; - if (task_status.type != 1) { - return AgvResult::failure( - AgvErrorCode::TaskRejected, - "SRC1100 created an unexpected task type for free navigation: " - + last_status); - } - - if (task_status.state == 2) { - const auto now = std::chrono::steady_clock::now(); - if (!running_stability_window_active) { - running_stability_window_active = true; - running_stable_at = now + running_stability_window; - } - ++consecutive_running_samples; - std::uint64_t fault_control_attempt = 0; - const std::string fault = cachedControllerFaultDetail_( - context.controller_fault_sequence_at_start, - 0, - &fault_control_attempt); - if (!fault.empty()) { - if (superseded()) { - return superseded_result(); - } - if (fault_attribution_is_ambiguous( - fault_control_attempt)) { - return ambiguous_fault_result(last_status, fault); - } - return AgvResult::failure( - AgvErrorCode::Fault, - "SRC1100 free-navigation task reached Running state, " - "but the controller reported a new fault during start " - "confirmation: " + last_status + ", " + fault - + "; do not retry automatically; cancel or stop the " - "task before another motion command"); - } - if (consecutive_running_samples - >= kPoseNavigationRequiredRunningSamples - && now >= running_stable_at) { - if (superseded()) { - return superseded_result(); - } - if (const std::string unavailable = - fault_monitoring_unavailable(); - !unavailable.empty()) { - return fault_monitoring_unavailable_result( - unavailable); - } - return AgvResult::success(); - } - } else { - consecutive_running_samples = 0; - running_stability_window_active = false; - } - - if (task_status.state == 3) { - std::uint64_t fault_control_attempt = 0; - const std::string fault = cachedControllerFaultDetail_( - context.controller_fault_sequence_at_start, - controllerFaultCaptureGraceMs_(), - &fault_control_attempt); - if (superseded()) { - return superseded_result(); - } - if (!fault.empty()) { - if (fault_attribution_is_ambiguous( - fault_control_attempt)) { - return ambiguous_fault_result(last_status, fault); - } - return AgvResult::failure( - AgvErrorCode::Fault, - "SRC1100 free-navigation task was established but " - "paused while the controller reported a new fault: " - + last_status + ", " + fault - + "; do not retry automatically before querying or " - "canceling it"); - } - if (const std::string unavailable = - fault_monitoring_unavailable(); - !unavailable.empty()) { - return fault_monitoring_unavailable_result(unavailable); - } - return AgvResult::failure( - AgvErrorCode::TaskRejected, - "SRC1100 free-navigation task was established but is paused: " - + last_status - + "; do not retry automatically before querying or canceling it"); - } - if (task_status.state == 4) { - std::uint64_t fault_control_attempt = 0; - const std::string fault = cachedControllerFaultDetail_( - context.controller_fault_sequence_at_start, - controllerFaultCaptureGraceMs_(), - &fault_control_attempt); - if (superseded()) { - return superseded_result(); - } - if (const std::string unavailable = - fault_monitoring_unavailable(); - !unavailable.empty()) { - return fault_monitoring_unavailable_result(unavailable); - } - if (!fault.empty()) { - if (fault_attribution_is_ambiguous( - fault_control_attempt)) { - return ambiguous_fault_result(last_status, fault); - } - return AgvResult::failure( - AgvErrorCode::Fault, - "SRC1100 free-navigation task reported Completed, but " - "the controller reported a new fault during completion " - "confirmation: " + last_status + ", " + fault - + "; do not retry automatically; cancel or stop " - "the task before another motion command"); - } - std::string pose_detail; - const bool target_reached = - poseTargetReached_(context, pose_detail); - if (superseded()) { - return superseded_result(); - } - std::uint64_t post_pose_fault_control_attempt = 0; - const std::string post_pose_fault = - cachedControllerFaultDetail_( - context.controller_fault_sequence_at_start, - 0, - &post_pose_fault_control_attempt); - if (!post_pose_fault.empty()) { - if (fault_attribution_is_ambiguous( - post_pose_fault_control_attempt)) { - return ambiguous_fault_result( - last_status, - post_pose_fault); - } - return AgvResult::failure( - AgvErrorCode::Fault, - "SRC1100 free-navigation task reported Completed, but " - "the controller reported a new fault during target " - "verification: " + last_status + ", " - + post_pose_fault - + "; do not retry automatically; cancel or stop " - "the task before another motion command"); - } - if (const std::string unavailable = - fault_monitoring_unavailable(); - !unavailable.empty()) { - return fault_monitoring_unavailable_result(unavailable); - } - if (target_reached) { - return AgvResult::success(); - } - return AgvResult::failure( - AgvErrorCode::TaskRejected, - "SRC1100 free-navigation task completed before a stable running " - "state, but the requested target was not reached: " - + last_status + ", " + pose_detail - + "; check the freeGo payload and controller alarms before retrying"); - } - if (task_status.state == 5 || task_status.state == 6) { - std::string detail = last_status; - std::uint64_t fault_control_attempt = 0; - const std::string fault = cachedControllerFaultDetail_( - context.controller_fault_sequence_at_start, - controllerFaultCaptureGraceMs_(), - &fault_control_attempt); - if (superseded()) { - return superseded_result(); - } - if (!fault.empty()) { - if (fault_attribution_is_ambiguous( - fault_control_attempt)) { - detail += - ", controller_fault_attribution=ambiguous because " - "the fault was observed after another control " - "command attempt had begun"; - } - detail += ", " + fault; - } - return AgvResult::failure( - task_status.state == 5 - ? AgvErrorCode::TaskFailed - : AgvErrorCode::TaskCanceled, - task_status.state == 5 - ? "SRC1100 free-navigation task failed: " + detail - : "SRC1100 free-navigation task was canceled: " + detail); - } - } else { - consecutive_running_samples = 0; - running_stability_window_active = false; - } - - const auto now = std::chrono::steady_clock::now(); - if (now >= hard_deadline - || (now >= deadline && !running_stability_window_active)) { - break; - } - std::this_thread::sleep_for(kPoseNavigationPollInterval); - } - - if (matching_task_observed - && last_poll_matched - && last_matching_state == 1) { - // A matching Waiting task has been accepted by the controller and may - // legitimately remain queued. Returning a rejection here would invite - // a duplicate command while the original task can still start later. - std::uint64_t fault_control_attempt = 0; - const std::string fault = cachedControllerFaultDetail_( - context.controller_fault_sequence_at_start, - controllerFaultCaptureGraceMs_(), - &fault_control_attempt); - if (superseded()) { - return superseded_result(); - } - if (const std::string unavailable = - fault_monitoring_unavailable(); - !unavailable.empty()) { - return fault_monitoring_unavailable_result(unavailable); - } - if (!fault.empty()) { - if (fault_attribution_is_ambiguous( - fault_control_attempt)) { - return ambiguous_fault_result(last_status, fault); - } - return AgvResult::failure( - AgvErrorCode::Fault, - "SRC1100 free-navigation task was accepted and remains active, " - "but the controller reported a new fault: " - + last_status + ", " + fault - + "; the task may still start later, so do not retry " - "automatically; cancel or stop it before another motion command"); - } - return AgvResult::success(); - } - - std::string detail = last_status; - std::uint64_t fault_control_attempt = 0; - const std::string fault = cachedControllerFaultDetail_( - context.controller_fault_sequence_at_start, - 0, - &fault_control_attempt); - if (superseded()) { - return superseded_result(); - } - if (!fault.empty()) { - if (fault_attribution_is_ambiguous( - fault_control_attempt)) { - detail += - ", controller_fault_attribution=ambiguous because the fault " - "was observed after another control command attempt had begun"; - } - detail += ", " + fault; - } - return AgvResult::failure( - AgvErrorCode::TaskRejected, - "SRC1100 accepted the free-navigation command, but no stable matching " - "pose task was established within " - + std::to_string(kPoseNavigationStartTimeout.count()) - + " ms; last " + detail - + "; do not retry automatically before checking or canceling navigation"); -} - -AgvResult Src1100Agv::sendControlledCommand_( - const int sock, - const std::uint16_t command, - const Json::Value& payload, - Json::Value* response, - std::uint64_t* accepted_navigation_generation, - std::uint64_t* controller_fault_sequence_at_attempt, - std::uint64_t* control_attempt_sequence, - PoseTaskContext* pose_context_to_publish, - const bool reject_if_active_controller_fault) const -{ - // Keep the permission acquisition and the following write ordered with - // respect to other control RPCs in this process. Channel I/O serialization - // is separate, so this must remain a distinct lock. - std::lock_guard sequence_lock(control_sequence_mutex_); - const auto attempt_sequence = - control_attempt_sequence_.fetch_add( - 1, - std::memory_order_relaxed) + 1; - if (control_attempt_sequence) { - *control_attempt_sequence = attempt_sequence; - } - if (pose_context_to_publish) { - pose_context_to_publish->control_attempt_sequence_at_start = - attempt_sequence; - } - - const auto authority = acquireControl_(); - if (!authority.ok()) { - const std::string detail = authority.message.empty() ? "unknown error" : authority.message; - return AgvResult::failure( - authority.code, - "SRC1100 acquire control authority failed: " + detail); - } - std::string controller_fault_gate_error; - if (controller_fault_sequence_at_attempt - || pose_context_to_publish - || reject_if_active_controller_fault) { - std::lock_guard lock(runtime_state_mutex_); - if (controller_fault_sequence_at_attempt) { - *controller_fault_sequence_at_attempt = - controller_fault_sequence_; - } - if (pose_context_to_publish) { - pose_context_to_publish->controller_fault_sequence_at_start = - controller_fault_sequence_; - pose_context_to_publish - ->controller_fault_channel_epoch_at_start = - controller_fault_channel_epoch_.load( - std::memory_order_relaxed); - } - if (reject_if_active_controller_fault) { - if (!state_push_enabled_) { - controller_fault_gate_error = - "controller fault state is unavailable because state push " - "is disabled"; - } else if (!active_controller_fault_detail_.empty()) { - controller_fault_gate_error = - "the controller reported a fault or invalid fault state: " - + active_controller_fault_detail_; - } else if (!controller_fault_state_observed_) { - controller_fault_gate_error = - "no state push containing fatals/errors has been observed"; - } else { - const auto fault_state_age = - std::chrono::duration_cast( - std::chrono::steady_clock::now() - - controller_fault_state_observed_at_) - .count(); - if (fault_state_age > controllerFaultStateMaxAgeMs_()) { - controller_fault_gate_error = - "the most recent fatals/errors state push is stale " - "(age_ms=" + std::to_string(fault_state_age) - + ", max_age_ms=" - + std::to_string(controllerFaultStateMaxAgeMs_()) - + ")"; - } - } - } - } - if (!controller_fault_gate_error.empty()) { - return AgvResult::failure( - AgvErrorCode::Fault, - "SRC1100 free-navigation command was not sent because " - + controller_fault_gate_error); - } - const auto publish_navigation_generation = - [this, - accepted_navigation_generation, - pose_context_to_publish]() { - const auto generation = - navigation_generation_.fetch_add( - 1, - std::memory_order_relaxed) + 1; - *accepted_navigation_generation = generation; - if (pose_context_to_publish) { - pose_context_to_publish->navigation_generation = generation; - rememberPoseTask_(*pose_context_to_publish); - } - }; - auto result = sendCommand_(sock, command, payload, response); - if (!result.ok()) { - if (accepted_navigation_generation) { - // Once the control write has been attempted, a timeout, disconnect, - // wrong response opcode, or malformed JSON cannot prove rejection: - // the controller may already have executed the command. - publish_navigation_generation(); - return withUnknownControllerOutcome(std::move(result)); - } - return result; - } - if (!accepted_navigation_generation) { - return result; - } - if (!response) { - publish_navigation_generation(); - return withUnknownControllerOutcome(AgvResult::failure( - AgvErrorCode::CommandFailed, - "SRC1100 cannot confirm navigation command without a response")); - } - if (!hasNumericControllerRetCode(*response)) { - publish_navigation_generation(); - return withUnknownControllerOutcome(resultFromResponse_(*response)); - } - result = resultFromResponse_(*response); - if (!result.ok()) { - return result; - } - - // Advance only after the controller accepted the command, and do it before - // releasing control_sequence_mutex_. This prevents a failed cancel/pause or - // failed authority acquisition from falsely reporting a pose task canceled, - // while preserving the controller's actual command order under concurrency. - publish_navigation_generation(); - return result; -} - -AgvResult Src1100Agv::sendCommand_( - const int sock, - const std::uint16_t command, - const Json::Value& payload, - Json::Value* response) const -{ - std::string response_payload; - auto result = sendCommandRaw_(sock, command, payload, &response_payload); - if (!result.ok()) { - return result; - } - if (!response) { - return AgvResult::success(); - } - - Json::Value parsed; - std::string error; - if (!parseJson_(response_payload, parsed, error)) { - const std::string json_text = extractJson_(response_payload); - if (json_text.empty() || !parseJson_(json_text, parsed, error)) { - return AgvResult::failure(AgvErrorCode::CommandFailed, error); - } - } - - *response = std::move(parsed); - return AgvResult::success(); -} - -AgvResult Src1100Agv::sendCommandRaw_( - const int sock, - const std::uint16_t command, - const Json::Value& payload, - std::string* response_payload) const -{ - const auto exchange = [&]() { - const std::string payload_text = payload.empty() ? std::string{} : toJsonString_(payload); - const auto frame = buildFrame_(command, payload_text); - if (::send(sock, frame.data(), frame.size(), MSG_NOSIGNAL) - != static_cast(frame.size())) { - return AgvResult::failure( - AgvErrorCode::CommandFailed, - "SRC1100 send command failed: " + systemError()); - } - - std::uint16_t response_command = 0; - std::string payload_text_response; - const auto result = receiveFrame_(sock, response_command, payload_text_response); - if (!result.ok()) { - return result; - } - const auto expected_response_command = static_cast( - command + 10000U); - if (response_command != expected_response_command) { - return AgvResult::failure( - AgvErrorCode::CommandFailed, - "SRC1100 response command mismatch: expected=" - + std::to_string(expected_response_command) - + ", actual=" + std::to_string(response_command)); - } - if (response_payload) { - *response_payload = std::move(payload_text_response); - } - return AgvResult::success(); - }; - const auto close_matching_socket_locked = [this, sock]() { - if (sock == sock_status_) { - closeSocket_(sock_status_); - } else if (sock == sock_control_) { - closeSocket_(sock_control_); - } else if (sock == sock_navigation_) { - closeSocket_(sock_navigation_); - } else if (sock == sock_config_) { - closeSocket_(sock_config_); - } else if (sock == sock_other_) { - closeSocket_(sock_other_); - } - }; - const auto mark_channel_desynchronized = [](AgvResult result) { - std::string detail = result.message.empty() - ? "unknown transport or frame error" - : result.message; - detail += - "; SRC1100 channel closed because the response stream may be " - "desynchronized; reconnect before sending another command"; - return AgvResult::failure(result.code, detail); - }; - - bool is_status_socket = false; - { - std::lock_guard lock(mutex_); - if (sock < 0) { - return AgvResult::failure( - AgvErrorCode::NotConnected, - "SRC1100 socket not connected"); - } - is_status_socket = sock == sock_status_; - } - - if (is_status_socket) { - // A slow 1110 status response must never hold the lifecycle/global I/O - // mutex needed by cancelNavigation() or emergencyStop(). The dedicated - // status lock still serializes requests on port 19204. connect_() and - // disconnect_() take this lock before changing the descriptor. - std::lock_guard status_lock(status_io_mutex_); - { - std::lock_guard lock(mutex_); - if (sock < 0 || sock != sock_status_) { - return AgvResult::failure( - AgvErrorCode::NotConnected, - "SRC1100 status socket is no longer connected"); - } - } - auto result = exchange(); - if (!result.ok()) { - std::lock_guard lock(mutex_); - close_matching_socket_locked(); - return mark_channel_desynchronized(std::move(result)); - } - return result; - } - - std::lock_guard lock(mutex_); - if (sock < 0 - || (sock != sock_control_ - && sock != sock_navigation_ - && sock != sock_config_ - && sock != sock_other_)) { - return AgvResult::failure( - AgvErrorCode::NotConnected, - "SRC1100 socket is no longer connected"); - } - auto result = exchange(); - if (!result.ok()) { - close_matching_socket_locked(); - return mark_channel_desynchronized(std::move(result)); - } - return result; -} - -AgvResult Src1100Agv::sendCommandNoResponse_( - const int sock, - const std::uint16_t command, - const Json::Value& payload) const -{ - return sendCommand_(sock, command, payload, nullptr); -} - -AgvResult Src1100Agv::configurePush_() -{ - if (config_.state_push_included_fields_size() > 0 && config_.state_push_excluded_fields_size() > 0) { - return AgvResult::failure( - AgvErrorCode::InvalidArgument, - "SRC1100 push included_fields and excluded_fields cannot both be set"); - } - - Json::Value payload(Json::objectValue); - if (config_.state_push_interval_ms() > 0) { - jsonMember(payload, "interval") = config_.state_push_interval_ms(); - } - appendStringArray(payload, "included_fields", config_.state_push_included_fields()); - appendStringArray(payload, "excluded_fields", config_.state_push_excluded_fields()); - - if (payload.empty()) { - return AgvResult::success(); - } - - const std::string payload_text = toJsonString_(payload); - const auto frame = buildFrame_(kRobotPushConfigReq, payload_text); - - std::lock_guard lock(mutex_); - if (sock_push_ < 0) { - return AgvResult::failure(AgvErrorCode::NotConnected, "SRC1100 push socket not connected"); - } - if (::send(sock_push_, frame.data(), frame.size(), MSG_NOSIGNAL) != static_cast(frame.size())) { - return AgvResult::failure(AgvErrorCode::CommandFailed, "SRC1100 send push config failed: " + systemError()); - } - - while (true) { - std::uint16_t command = 0; - std::string response_payload; - const auto result = receiveFrame_(sock_push_, command, response_payload); - if (!result.ok()) { - return result; - } - - Json::Value response; - std::string error; - if (!response_payload.empty() && !parseJson_(response_payload, response, error)) { - return AgvResult::failure(AgvErrorCode::CommandFailed, error); - } - - if (command == kRobotPushConfigRes) { - return resultFromResponse_(response); - } - if (command == kRobotPush && response.isObject()) { - updateCachedRuntimeState_(response); - } - } -} - -void Src1100Agv::startPushThread_() -{ - if (!state_push_enabled_) { - return; - } - if (push_running_.exchange(true)) { - return; - } - if (sock_push_ < 0) { - push_running_ = false; - return; - } - push_thread_ = std::thread(&Src1100Agv::pushLoop_, this); -} - -void Src1100Agv::stopPushThread_() -{ - const bool was_running = push_running_.exchange(false); - if (was_running) { - int sock = -1; - { - std::lock_guard lock(mutex_); - sock = sock_push_; - } - if (sock >= 0) { - ::shutdown(sock, SHUT_RDWR); - } - } - if (push_thread_.joinable()) { - push_thread_.join(); - } - invalidateControllerFaultState_(); -} - -void Src1100Agv::pushLoop_() -{ - while (push_running_) { - int sock = -1; - { - std::lock_guard lock(mutex_); - sock = sock_push_; - } - if (sock < 0) { - std::this_thread::sleep_for(std::chrono::milliseconds(100)); - continue; - } - - std::uint16_t command = 0; - std::string payload; - const auto result = receiveFrame_(sock, command, payload); - if (!push_running_) { - break; - } - if (!result.ok()) { - if (result.code != AgvErrorCode::Timeout) { - invalidateControllerFaultState_(); - std::lock_guard lock(mutex_); - last_error_ = result.message; - closeSocket_(sock_push_); - } - continue; - } - if (command != kRobotPush || payload.empty()) { - continue; - } - - Json::Value parsed; - std::string error; - if (!parseJson_(payload, parsed, error)) { - invalidateControllerFaultState_(); - std::lock_guard lock(mutex_); - last_error_ = error; - continue; - } - updateCachedRuntimeState_(parsed); - } -} - -void Src1100Agv::invalidateControllerFaultState_() -{ - std::lock_guard lock(runtime_state_mutex_); - controller_fault_channel_epoch_.fetch_add( - 1, - std::memory_order_relaxed); - controller_fault_state_observed_ = false; - controller_fault_state_observed_at_ = {}; - active_controller_fault_detail_.clear(); - runtime_state_cv_.notify_all(); -} - -void Src1100Agv::updateCachedRuntimeState_(const Json::Value& payload) -{ - std::lock_guard lock(runtime_state_mutex_); - auto state = cached_runtime_state_valid_ ? cached_runtime_state_ : AgvRuntimeState{}; - state.timestamp = nowSeconds(); - state.connected = true; - - if (jsonHas(payload, "x")) state.pose.x = jsonGet(payload, "x", state.pose.x).asDouble(); - if (jsonHas(payload, "y")) state.pose.y = jsonGet(payload, "y", state.pose.y).asDouble(); - if (jsonHas(payload, "angle")) state.pose.theta = jsonGet(payload, "angle", state.pose.theta).asDouble(); - if (jsonHas(payload, "vx")) state.velocity.vx = jsonGet(payload, "vx", state.velocity.vx).asDouble(); - if (jsonHas(payload, "vy")) state.velocity.vy = jsonGet(payload, "vy", state.velocity.vy).asDouble(); - if (jsonHas(payload, "w")) state.velocity.wz = jsonGet(payload, "w", state.velocity.wz).asDouble(); - if (jsonHas(payload, "battery_level")) { - state.battery.percentage = jsonGet(payload, "battery_level", state.battery.percentage).asDouble(); - } - if (jsonHas(payload, "battery_temp")) { - state.battery.temperature = jsonGet(payload, "battery_temp", state.battery.temperature).asDouble(); - } - if (jsonHas(payload, "charging")) { - state.battery.charging = jsonGet(payload, "charging", state.battery.charging).asBool(); - } - if (jsonHas(payload, "voltage")) { - state.battery.voltage = jsonGet(payload, "voltage", state.battery.voltage).asDouble(); - } - if (jsonHas(payload, "current")) { - state.battery.current = jsonGet(payload, "current", state.battery.current).asDouble(); - } - if (jsonHas(payload, "current_map")) { - state.current_map = jsonGet(payload, "current_map", state.current_map).asString(); - } - if (jsonHas(payload, "current_station")) { - state.current_station = jsonGet(payload, "current_station", state.current_station).asString(); - } - if (jsonHas(payload, "confidence")) { - state.localized = jsonGet(payload, "confidence", 0.0).asDouble() > 0.0; - } - if (jsonHas(payload, "emergency")) { - state.emergency_stopped = jsonGet(payload, "emergency", state.emergency_stopped).asBool(); - } - - state.moving = std::hypot(state.velocity.vx, state.velocity.vy) > 1e-4 || std::abs(state.velocity.wz) > 1e-4; - const bool has_fatals = jsonHas(payload, "fatals"); - const bool has_errors = jsonHas(payload, "errors"); - const bool has_fault_fields = has_fatals || has_errors; - if (has_fault_fields) { - const auto* fatals = jsonFind(payload, "fatals"); - const auto* errors = jsonFind(payload, "errors"); - const bool valid_fatals = !has_fatals - || (fatals && fatals->isArray()); - const bool valid_errors = !has_errors - || (errors && errors->isArray()); - const bool complete_fault_state = - has_fatals && has_errors && valid_fatals && valid_errors; - if (complete_fault_state) { - controller_fault_state_observed_ = true; - controller_fault_state_observed_at_ = - std::chrono::steady_clock::now(); - } else { - controller_fault_state_observed_ = false; - controller_fault_state_observed_at_ = {}; - } - - const bool reported_fault = - hasFaultArray(payload, "fatals") - || hasFaultArray(payload, "errors"); - const bool invalid_or_incomplete_fault_state = - !complete_fault_state && !reported_fault; - state.fault = reported_fault - || invalid_or_incomplete_fault_state; - if (state.fault) { - std::ostringstream detail; - detail << (reported_fault - ? "SRC1100 controller fault" - : "SRC1100 controller fault state is incomplete or malformed"); - if (fatals - && (!fatals->isArray() - || !fatals->empty() - || !complete_fault_state)) { - detail << ": fatals=" << jsonValueToString(*fatals); - } - if (errors - && (!errors->isArray() - || !errors->empty() - || !complete_fault_state)) { - detail << ": errors=" << jsonValueToString(*errors); - } - state.last_error = detail.str(); - if (state.last_error != active_controller_fault_detail_) { - active_controller_fault_detail_ = state.last_error; - ++controller_fault_sequence_; - last_controller_fault_timestamp_ = state.timestamp; - last_controller_fault_detail_ = state.last_error; - last_controller_fault_control_attempt_ = - control_attempt_sequence_.load( - std::memory_order_acquire); - } - } else { - state.last_error.clear(); - active_controller_fault_detail_.clear(); - } - } - if (state.emergency_stopped) { - state.mode = AgvMode::EmergencyStop; - } else if (state.fault) { - state.mode = AgvMode::Fault; - } else if (state.battery.charging) { - state.mode = AgvMode::Charging; - } else if (state.moving) { - state.mode = AgvMode::Auto; - } else { - state.mode = AgvMode::Idle; - } - - cached_runtime_state_ = state; - cached_runtime_state_valid_ = true; - runtime_state_cv_.notify_all(); -} - -std::vector Src1100Agv::buildFrame_( - const std::uint16_t command, - const std::string& payload) -{ - std::vector frame(16 + payload.size(), 0); - frame[0] = 0x5A; - frame[1] = 0x01; - frame[2] = 0x00; - frame[3] = 0x01; - const auto length = static_cast(payload.size()); - frame[4] = static_cast((length >> 24U) & 0xFFU); - frame[5] = static_cast((length >> 16U) & 0xFFU); - frame[6] = static_cast((length >> 8U) & 0xFFU); - frame[7] = static_cast(length & 0xFFU); - frame[8] = static_cast((command >> 8U) & 0xFFU); - frame[9] = static_cast(command & 0xFFU); - std::copy(payload.begin(), payload.end(), frame.begin() + 16); - return frame; -} - -std::string Src1100Agv::toJsonString_(const Json::Value& value) -{ - Json::StreamWriterBuilder builder; - builder["indentation"] = ""; - return Json::writeString(builder, value); -} - -bool Src1100Agv::parseJson_(const std::string& input, Json::Value& output, std::string& error) -{ - Json::CharReaderBuilder builder; - std::unique_ptr reader(builder.newCharReader()); - return reader->parse(input.data(), input.data() + input.size(), &output, &error); -} - -std::string Src1100Agv::extractJson_(const std::string& raw) -{ - const auto begin = raw.find('{'); - const auto end = raw.rfind('}'); - if (begin == std::string::npos || end == std::string::npos || end < begin) { - return {}; - } - return raw.substr(begin, end - begin + 1); -} - -AgvResult Src1100Agv::receiveFrame_(const int sock, std::uint16_t& command, std::string& payload) -{ - const auto recv_exact = [](const int fd, std::uint8_t* data, const std::size_t size) -> AgvResult { - std::size_t offset = 0; - while (offset < size) { - const ssize_t count = ::recv(fd, data + offset, size - offset, 0); - if (count > 0) { - offset += static_cast(count); - continue; - } - if (count == 0) { - return AgvResult::failure(AgvErrorCode::NotConnected, "SRC1100 socket closed"); - } - if (errno == EINTR) { - continue; - } - if (errno == EAGAIN || errno == EWOULDBLOCK) { - return AgvResult::failure(AgvErrorCode::Timeout, "SRC1100 receive timeout"); - } - return AgvResult::failure(AgvErrorCode::CommandFailed, "SRC1100 receive failed: " + systemError()); - } - return AgvResult::success(); - }; - - std::uint8_t header[16]{}; - auto result = recv_exact(sock, header, sizeof(header)); - if (!result.ok()) { - return result; - } - if (header[0] != 0x5A) { - return AgvResult::failure(AgvErrorCode::CommandFailed, "SRC1100 frame header is invalid"); - } - - const auto length = (static_cast(header[4]) << 24U) - | (static_cast(header[5]) << 16U) - | (static_cast(header[6]) << 8U) - | static_cast(header[7]); - command = static_cast((static_cast(header[8]) << 8U) | header[9]); - payload.clear(); - if (length == 0) { - return AgvResult::success(); - } - if (length > kMaxFramePayloadBytes) { - return AgvResult::failure(AgvErrorCode::CommandFailed, "SRC1100 frame payload is too large"); - } - - std::vector buffer(length); - result = recv_exact(sock, buffer.data(), buffer.size()); - if (!result.ok()) { - return result; - } - payload.assign(reinterpret_cast(buffer.data()), buffer.size()); - return AgvResult::success(); -} - -void Src1100Agv::applyMotionOptions_( - Json::Value& payload, - const AgvMotionOptions& options, - const bool include_reach_options) -{ - if (options.max_speed > 0.0) jsonMember(payload, "max_speed") = options.max_speed; - if (options.max_angular_speed > 0.0) jsonMember(payload, "max_wspeed") = options.max_angular_speed; - if (options.max_acceleration > 0.0) jsonMember(payload, "max_acc") = options.max_acceleration; - if (options.max_angular_acceleration > 0.0) jsonMember(payload, "max_wacc") = options.max_angular_acceleration; - if (include_reach_options) { - if (options.reach_distance > 0.0) jsonMember(payload, "reach_dist") = options.reach_distance; - if (options.reach_angle > 0.0) jsonMember(payload, "reach_angle") = options.reach_angle; - } -} - -void Src1100Agv::applyAdapterParams_(Json::Value& payload, const AgvAdapterParams& params) -{ - for (const auto& [key, value] : params.values) { - if (key.rfind("port_", 0) == 0 - || key == "target_id" - || key == "id" - || key == "x" - || key == "y" - || key == "angle" - || key == "freeGo" - || key == "max_speed" - || key == "max_wspeed" - || key == "max_acc" - || key == "max_wacc" - || key == "reach_dist" - || key == "reach_angle" - || key == "jack_height") { - continue; - } - jsonMember(payload, key) = value; - } - if (const auto jack_height = params.getDouble("jack_height")) { - jsonMember(payload, "jack_height") = *jack_height; - } -} - -AgvResult Src1100Agv::resultFromResponse_(const Json::Value& response) -{ - if (!hasNumericControllerRetCode(response)) { - return AgvResult::failure( - AgvErrorCode::CommandFailed, - "SRC1100 controller response is missing a numeric ret_code"); - } - const auto* ret_code_value = jsonFind(response, "ret_code"); - const bool success = ret_code_value->isUInt() || ret_code_value->isUInt64() - ? ret_code_value->asUInt64() == 0 - : ret_code_value->asInt64() == 0; - const std::string ret_code = jsonValueToString(*ret_code_value); - const std::string message = jsonGet(response, "err_msg", "").asString(); - if (success) { - return AgvResult::success(); - } - std::string detail = "SRC1100 command failed: ret_code=" + ret_code; - if (!message.empty()) { - detail += ", err_msg=" + message; - } - return AgvResult::failure(AgvErrorCode::CommandFailed, detail); -} - -} // namespace cmvr::device diff --git a/cmvr-es/devices/agv/src1100/tests/src1100_control_authority_test.cpp b/cmvr-es/devices/agv/src1100/tests/src1100_control_authority_test.cpp deleted file mode 100644 index da0034ea..00000000 --- a/cmvr-es/devices/agv/src1100/tests/src1100_control_authority_test.cpp +++ /dev/null @@ -1,2743 +0,0 @@ -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include - -#include -#include - -#include "devices/agv/src1100/include/src1100_agv.h" - -namespace cmvr::device { - -class Src1100AgvTestPeer { -public: - static void installSockets( - Src1100Agv& agv, - const int status, - const int control, - const int navigation, - const int config, - const int other) - { - agv.sock_status_ = status; - agv.sock_control_ = control; - agv.sock_navigation_ = navigation; - agv.sock_config_ = config; - agv.sock_other_ = other; - } - - static void cacheRuntimeState(Src1100Agv& agv, const Json::Value& payload) - { - agv.updateCachedRuntimeState_(payload); - } - - static void setAdapterError(Src1100Agv& agv, std::string error) - { - std::lock_guard lock(agv.mutex_); - agv.last_error_ = std::move(error); - } - - static void setFaultStateUnknown(Src1100Agv& agv) - { - agv.invalidateControllerFaultState_(); - } - - static void setFaultStateAge( - Src1100Agv& agv, - const std::chrono::milliseconds age) - { - std::lock_guard lock(agv.runtime_state_mutex_); - agv.controller_fault_state_observed_ = true; - agv.controller_fault_state_observed_at_ = - std::chrono::steady_clock::now() - age; - agv.active_controller_fault_detail_.clear(); - } - - static bool hasTrackedPoseTask(const Src1100Agv& agv) - { - Src1100Agv::PoseTaskContext context; - return agv.currentPoseTask_(context); - } - - static AgvResult disconnect(Src1100Agv& agv) - { - return agv.disconnect_(); - } - - static void setNavigationReceiveTimeout( - Src1100Agv& agv, - const std::chrono::milliseconds timeout) - { - timeval value{}; - value.tv_sec = static_cast(timeout.count() / 1000); - value.tv_usec = static_cast( - (timeout.count() % 1000) * 1000); - ASSERT_EQ( - ::setsockopt( - agv.sock_navigation_, - SOL_SOCKET, - SO_RCVTIMEO, - &value, - sizeof(value)), - 0); - } -}; - -namespace { - -constexpr std::uint16_t kRobotStatusTask = 1020; -constexpr std::uint16_t kRobotStatusLoc = 1004; -constexpr std::uint16_t kRobotStatusTaskPackage = 1110; -constexpr std::uint16_t kRobotControlStop = 2000; -constexpr std::uint16_t kRobotControlMotion = 2010; -constexpr std::uint16_t kRobotControlLoadMap = 2022; -constexpr std::uint16_t kRobotTaskPause = 3001; -constexpr std::uint16_t kRobotTaskResume = 3002; -constexpr std::uint16_t kRobotTaskCancel = 3003; -constexpr std::uint16_t kRobotTaskGoTarget = 3051; -constexpr std::uint16_t kRobotTaskGoTargetList = 3066; -constexpr std::uint16_t kRobotConfigLock = 4005; -constexpr std::uint16_t kRobotConfigUploadMap = 4010; -constexpr std::uint16_t kRobotConfigDownloadMap = 4011; -constexpr std::uint16_t kRobotOtherStartMapping = 6100; -constexpr std::uint16_t kRobotOtherStopMapping = 6101; - -enum class Channel : std::size_t { - Status = 0, - Control, - Navigation, - Config, - Other, - Count -}; - -struct CommandRecord { - std::uint16_t command{0}; - std::string payload; -}; - -Json::Value parsePayload(const CommandRecord& record) -{ - Json::Value payload; - Json::CharReaderBuilder builder; - std::string error; - std::unique_ptr reader(builder.newCharReader()); - if (!reader->parse( - record.payload.data(), - record.payload.data() + record.payload.size(), - &payload, - &error)) { - ADD_FAILURE() << "Failed to parse command " << record.command - << " payload: " << error; - } - return payload; -} - -const Json::Value& payloadValue(const Json::Value& payload, const char* key) -{ - const auto* value = payload.find(key, key + std::strlen(key)); - if (!value) { - ADD_FAILURE() << "Missing JSON field: " << key; - static const Json::Value null_value; - return null_value; - } - return *value; -} - -bool payloadHas(const Json::Value& payload, const char* key) -{ - return payload.find(key, key + std::strlen(key)) != nullptr; -} - -bool receiveExact(const int fd, void* output, const std::size_t size) -{ - auto* bytes = static_cast(output); - std::size_t offset = 0; - while (offset < size) { - const auto count = ::recv(fd, bytes + offset, size - offset, 0); - if (count > 0) { - offset += static_cast(count); - continue; - } - if (count < 0 && errno == EINTR) { - continue; - } - return false; - } - return true; -} - -bool sendAll(const int fd, const std::vector& data) -{ - std::size_t offset = 0; - while (offset < data.size()) { - const auto count = ::send( - fd, - data.data() + offset, - data.size() - offset, - MSG_NOSIGNAL); - if (count > 0) { - offset += static_cast(count); - continue; - } - if (count < 0 && errno == EINTR) { - continue; - } - return false; - } - return true; -} - -std::vector responseFrame( - const std::uint16_t response_command, - const std::string& payload) -{ - std::vector frame(16 + payload.size(), 0); - frame[0] = 0x5A; - frame[1] = 0x01; - frame[3] = 0x01; - const auto length = static_cast(payload.size()); - frame[4] = static_cast((length >> 24U) & 0xFFU); - frame[5] = static_cast((length >> 16U) & 0xFFU); - frame[6] = static_cast((length >> 8U) & 0xFFU); - frame[7] = static_cast(length & 0xFFU); - frame[8] = static_cast((response_command >> 8U) & 0xFFU); - frame[9] = static_cast(response_command & 0xFFU); - std::copy(payload.begin(), payload.end(), frame.begin() + 16); - return frame; -} - -std::string injectRequestedTaskId( - std::string response_payload, - const std::string& request_payload) -{ - constexpr char kTaskIdToken[] = "${TASK_ID}"; - const auto token_position = response_payload.find(kTaskIdToken); - if (token_position == std::string::npos) { - return response_payload; - } - - Json::Value request; - Json::CharReaderBuilder builder; - std::string error; - std::unique_ptr reader(builder.newCharReader()); - if (!reader->parse( - request_payload.data(), - request_payload.data() + request_payload.size(), - &request, - &error)) { - return response_payload; - } - const auto* task_ids = request.find("task_ids", "task_ids" + std::strlen("task_ids")); - if (!task_ids || !task_ids->isArray() || task_ids->empty()) { - return response_payload; - } - - response_payload.replace( - token_position, - std::strlen(kTaskIdToken), - (*task_ids)[0].asString()); - return response_payload; -} - -class FakeSrc1100Controller { -public: - FakeSrc1100Controller() - { - for (auto& endpoint : endpoints_) { - int pair[2]{-1, -1}; - if (::socketpair(AF_UNIX, SOCK_STREAM, 0, pair) != 0) { - throw std::runtime_error("socketpair failed"); - } - endpoint.client = pair[0]; - endpoint.server = pair[1]; - } - for (std::size_t index = 0; index < endpoints_.size(); ++index) { - endpoints_[index].worker = std::thread( - &FakeSrc1100Controller::serve, - this, - index); - } - } - - ~FakeSrc1100Controller() - { - for (auto& endpoint : endpoints_) { - if (endpoint.client >= 0) { - ::shutdown(endpoint.client, SHUT_RDWR); - ::close(endpoint.client); - endpoint.client = -1; - } - if (endpoint.server >= 0) { - ::shutdown(endpoint.server, SHUT_RDWR); - } - } - for (auto& endpoint : endpoints_) { - if (endpoint.worker.joinable()) { - endpoint.worker.join(); - } - if (endpoint.server >= 0) { - ::close(endpoint.server); - endpoint.server = -1; - } - } - } - - int takeClient(const Channel channel) - { - auto& endpoint = endpoints_[static_cast(channel)]; - const int client = endpoint.client; - endpoint.client = -1; - return client; - } - - void setResponseCode(const std::uint16_t command, const int ret_code) - { - std::lock_guard lock(response_codes_mutex_); - response_codes_[command] = ret_code; - response_payloads_.erase(command); - } - - void setResponsePayload(const std::uint16_t command, std::string payload) - { - std::lock_guard lock(response_codes_mutex_); - response_codes_.erase(command); - response_payloads_[command] = {std::move(payload)}; - } - - void queueResponsePayload(const std::uint16_t command, std::string payload) - { - std::lock_guard lock(response_codes_mutex_); - response_codes_.erase(command); - response_payloads_[command].push_back(std::move(payload)); - } - - void setResponseDelay( - const std::uint16_t command, - const std::chrono::milliseconds delay) - { - std::lock_guard lock(response_codes_mutex_); - response_delays_[command] = delay; - } - - void setResponseCommand( - const std::uint16_t request_command, - const std::uint16_t response_command) - { - std::lock_guard lock(response_codes_mutex_); - response_commands_[request_command] = response_command; - } - - void clearRecords() - { - std::lock_guard lock(records_mutex_); - records_.clear(); - } - - std::vector records() const - { - std::lock_guard lock(records_mutex_); - return records_; - } - -private: - struct Endpoint { - int client{-1}; - int server{-1}; - std::thread worker; - }; - - void serve(const std::size_t index) - { - const int fd = endpoints_[index].server; - while (true) { - std::array header{}; - if (!receiveExact(fd, header.data(), header.size())) { - return; - } - const auto length = (static_cast(header[4]) << 24U) - | (static_cast(header[5]) << 16U) - | (static_cast(header[6]) << 8U) - | static_cast(header[7]); - const auto command = static_cast( - (static_cast(header[8]) << 8U) | header[9]); - std::string payload(length, '\0'); - if (length > 0 && !receiveExact(fd, payload.data(), payload.size())) { - return; - } - { - std::lock_guard lock(records_mutex_); - records_.push_back({command, payload}); - } - int ret_code = 0; - std::string response_payload; - std::chrono::milliseconds response_delay{0}; - std::uint16_t response_command = static_cast( - command + 10000U); - { - std::lock_guard lock(response_codes_mutex_); - const auto payloads = response_payloads_.find(command); - if (payloads != response_payloads_.end() && !payloads->second.empty()) { - response_payload = payloads->second.front(); - if (payloads->second.size() > 1U) { - payloads->second.pop_front(); - } - } - const auto response_code = response_codes_.find(command); - if (response_code != response_codes_.end()) { - ret_code = response_code->second; - } - const auto delay = response_delays_.find(command); - if (delay != response_delays_.end()) { - response_delay = delay->second; - } - const auto response_command_override = response_commands_.find(command); - if (response_command_override != response_commands_.end()) { - response_command = response_command_override->second; - } - } - if (response_delay.count() > 0) { - std::this_thread::sleep_for(response_delay); - } - if (response_payload.empty()) { - response_payload = ret_code == 0 - ? R"({"ret_code":0,"err_msg":""})" - : "{\"ret_code\":" + std::to_string(ret_code) - + R"(,"err_msg":"simulated command failure"})"; - } - response_payload = injectRequestedTaskId( - std::move(response_payload), - payload); - if (!sendAll(fd, responseFrame(response_command, response_payload))) { - return; - } - } - } - - std::array(Channel::Count)> endpoints_; - mutable std::mutex records_mutex_; - std::vector records_; - std::mutex response_codes_mutex_; - std::unordered_map response_codes_; - std::unordered_map> response_payloads_; - std::unordered_map response_delays_; - std::unordered_map response_commands_; -}; - -class Src1100ControlAuthorityTest : public ::testing::Test { -protected: - void SetUp() override - { - config::Src1100AgvConfig cfg; - cfg.set_id("src1100"); - cfg.set_ip("invalid-ip"); - cfg.set_recv_timeout_ms(100); - cfg.set_control_nick_name("cmvr-test"); - cfg.set_enable_state_push(true); - cfg.set_state_push_interval_ms(200); - controller_.setResponsePayload( - kRobotStatusTask, - R"({"ret_code":0,"err_msg":"","task_status":2,"task_type":1,"target_point":[1.0,2.0,0.5]})"); - controller_.setResponsePayload( - kRobotStatusTaskPackage, - R"({"ret_code":0,"err_msg":"","task_status_package":{"task_status_list":[{"task_id":"${TASK_ID}","status":2,"type":1}]}})"); - agv_ = std::make_unique(cfg); - const int status_socket = controller_.takeClient(Channel::Status); - const int control_socket = controller_.takeClient(Channel::Control); - const int navigation_socket = controller_.takeClient(Channel::Navigation); - const int config_socket = controller_.takeClient(Channel::Config); - const int other_socket = controller_.takeClient(Channel::Other); - Src1100AgvTestPeer::installSockets( - *agv_, - status_socket, - control_socket, - navigation_socket, - config_socket, - other_socket); - Json::Value fault_state_push(Json::objectValue); - *fault_state_push.demand( - "errors", - "errors" + std::strlen("errors")) = - Json::Value(Json::arrayValue); - *fault_state_push.demand( - "fatals", - "fatals" + std::strlen("fatals")) = - Json::Value(Json::arrayValue); - Src1100AgvTestPeer::cacheRuntimeState(*agv_, fault_state_push); - } - - void TearDown() override - { - agv_.reset(); - } - - void expectControlledSequence( - const std::vector& commands, - const std::function& invoke) - { - controller_.clearRecords(); - const auto result = invoke(); - ASSERT_TRUE(result.ok()) << result.message; - - const auto records = controller_.records(); - ASSERT_EQ(records.size(), commands.size() + 1U); - EXPECT_EQ(records[0].command, kRobotConfigLock); - for (std::size_t index = 0; index < commands.size(); ++index) { - EXPECT_EQ(records[index + 1U].command, commands[index]); - } - - Json::Value lock_payload; - Json::CharReaderBuilder builder; - std::string error; - std::unique_ptr reader(builder.newCharReader()); - ASSERT_TRUE(reader->parse( - records[0].payload.data(), - records[0].payload.data() + records[0].payload.size(), - &lock_payload, - &error)) << error; - constexpr char kNickName[] = "nick_name"; - const auto* nick_name = lock_payload.find( - kNickName, - kNickName + std::strlen(kNickName)); - ASSERT_NE(nick_name, nullptr); - EXPECT_EQ(nick_name->asString(), "cmvr-test"); - } - - void expectControlled( - const std::uint16_t command, - const std::function& invoke) - { - expectControlledSequence({command}, invoke); - } - - FakeSrc1100Controller controller_; - std::unique_ptr agv_; -}; - -TEST_F(Src1100ControlAuthorityTest, EveryImplementedMutatingOperationAcquiresAuthorityFirst) -{ - controller_.clearRecords(); - const auto pose_result = - agv_->navigateToPose(math::Pose2d{1.0, 2.0, 0.5}); - ASSERT_TRUE(pose_result.ok()) << pose_result.message; - const auto pose_records = controller_.records(); - ASSERT_GE(pose_records.size(), 4U); - EXPECT_EQ(pose_records[0].command, kRobotConfigLock); - EXPECT_EQ(pose_records[1].command, kRobotTaskGoTarget); - for (std::size_t index = 2; index < pose_records.size(); ++index) { - EXPECT_EQ(pose_records[index].command, kRobotStatusTaskPackage); - } - expectControlled(kRobotTaskGoTarget, [this]() { - return agv_->navigateToStation("station-1"); - }); - expectControlled(kRobotTaskGoTargetList, [this]() { - return agv_->followPath({AgvPathSegment{"station-1", "station-2"}}); - }); - expectControlled(kRobotTaskPause, [this]() { - return agv_->pauseNavigation(); - }); - expectControlled(kRobotTaskResume, [this]() { - return agv_->resumeNavigation(); - }); - expectControlled(kRobotTaskCancel, [this]() { - return agv_->cancelNavigation(); - }); - expectControlledSequence({kRobotControlStop, kRobotTaskCancel}, [this]() { - return agv_->emergencyStop(); - }); - expectControlled(kRobotControlMotion, [this]() { - return agv_->setVelocity(AgvVelocity{0.1, 0.0, 0.2}); - }); - expectControlled(kRobotControlMotion, [this]() { - return agv_->stopVelocityControl(); - }); - expectControlled(kRobotControlLoadMap, [this]() { - return agv_->switchMap("map-1"); - }); - expectControlled(kRobotConfigUploadMap, [this]() { - return agv_->uploadMap("map-1", "{}"); - }); - expectControlled(kRobotOtherStartMapping, [this]() { - return agv_->startMapping(); - }); - expectControlled(kRobotOtherStopMapping, [this]() { - return agv_->stopMapping(); - }); -} - -TEST_F(Src1100ControlAuthorityTest, AcquisitionFailureDoesNotSendControlCommand) -{ - controller_.setResponseCode(kRobotConfigLock, 40020); - controller_.clearRecords(); - - const auto result = agv_->setVelocity(AgvVelocity{0.1, 0.0, 0.0}); - - EXPECT_FALSE(result.ok()); - EXPECT_EQ(result.code, AgvErrorCode::CommandFailed); - EXPECT_NE(result.message.find("acquire control authority failed"), std::string::npos); - EXPECT_NE(result.message.find("ret_code=40020"), std::string::npos); - const auto records = controller_.records(); - ASSERT_EQ(records.size(), 1U); - EXPECT_EQ(records[0].command, kRobotConfigLock); -} - -TEST_F(Src1100ControlAuthorityTest, EmergencyStopAcquisitionFailureDoesNotSendStopCommands) -{ - controller_.setResponseCode(kRobotConfigLock, 40020); - controller_.clearRecords(); - - const auto result = agv_->emergencyStop(); - - EXPECT_FALSE(result.ok()); - EXPECT_EQ(result.code, AgvErrorCode::CommandFailed); - EXPECT_NE(result.message.find("acquire control authority failed"), std::string::npos); - EXPECT_NE(result.message.find("ret_code=40020"), std::string::npos); - const auto records = controller_.records(); - ASSERT_EQ(records.size(), 1U); - EXPECT_EQ(records[0].command, kRobotConfigLock); -} - -TEST_F(Src1100ControlAuthorityTest, EmergencyStopAttemptsBothStopsAndAggregatesFailures) -{ - controller_.setResponseCode(kRobotControlStop, 50001); - controller_.setResponseCode(kRobotTaskCancel, 50002); - controller_.clearRecords(); - - const auto result = agv_->emergencyStop(); - - EXPECT_FALSE(result.ok()); - EXPECT_NE(result.message.find("control stop"), std::string::npos); - EXPECT_NE(result.message.find("cancel navigation"), std::string::npos); - EXPECT_NE(result.message.find("ret_code=50001"), std::string::npos); - EXPECT_NE(result.message.find("ret_code=50002"), std::string::npos); - const auto records = controller_.records(); - ASSERT_EQ(records.size(), 3U); - EXPECT_EQ(records[0].command, kRobotConfigLock); - EXPECT_EQ(records[1].command, kRobotControlStop); - EXPECT_EQ(records[2].command, kRobotTaskCancel); -} - -TEST_F(Src1100ControlAuthorityTest, UnsupportedClearFaultDoesNotAcquireAuthority) -{ - controller_.clearRecords(); - - const auto result = agv_->clearFault(); - - EXPECT_FALSE(result.ok()); - EXPECT_EQ(result.code, AgvErrorCode::UnsupportedCommand); - EXPECT_TRUE(controller_.records().empty()); -} - -TEST_F(Src1100ControlAuthorityTest, ReadOnlyMapDownloadDoesNotAcquireAuthority) -{ - controller_.clearRecords(); - std::string content; - - const auto result = agv_->downloadMap("map-1", content); - - ASSERT_TRUE(result.ok()) << result.message; - const auto records = controller_.records(); - ASSERT_EQ(records.size(), 1U); - EXPECT_EQ(records[0].command, kRobotConfigDownloadMap); -} - -TEST_F( - Src1100ControlAuthorityTest, - NavigateToPoseUsesLegacyCompatibleFreeGoPayloadWithTypedMotionLimits) -{ - AgvMotionOptions options; - options.max_speed = 0.6; - options.max_angular_speed = 0.7; - options.max_acceleration = 0.8; - options.max_angular_acceleration = 0.9; - options.reach_distance = 0.1; - options.reach_angle = 0.2; - controller_.clearRecords(); - - const auto result = agv_->navigateToPose( - math::Pose2d{1.0, 2.0, 0.5}, - options); - - ASSERT_TRUE(result.ok()) << result.message; - const auto records = controller_.records(); - ASSERT_GE(records.size(), 4U); - EXPECT_EQ(records[0].command, kRobotConfigLock); - EXPECT_EQ(records[1].command, kRobotTaskGoTarget); - for (std::size_t index = 2; index < records.size(); ++index) { - EXPECT_EQ(records[index].command, kRobotStatusTaskPackage); - } - - const auto payload = parsePayload(records[1]); - EXPECT_EQ(payloadValue(payload, "source_id").asString(), "SELF_POSITION"); - EXPECT_EQ(payloadValue(payload, "id").asString(), ""); - EXPECT_FALSE(payloadValue(payload, "task_id").asString().empty()); - const auto& free_go = payloadValue(payload, "freeGo"); - EXPECT_TRUE(payloadValue(free_go, "x").isNumeric()); - EXPECT_TRUE(payloadValue(free_go, "y").isNumeric()); - EXPECT_TRUE(payloadValue(free_go, "theta").isNumeric()); - EXPECT_DOUBLE_EQ(payloadValue(free_go, "x").asDouble(), 1.0); - EXPECT_DOUBLE_EQ(payloadValue(free_go, "y").asDouble(), 2.0); - EXPECT_DOUBLE_EQ(payloadValue(free_go, "theta").asDouble(), 0.5); - EXPECT_DOUBLE_EQ(payloadValue(payload, "max_speed").asDouble(), 0.6); - EXPECT_DOUBLE_EQ(payloadValue(payload, "max_wspeed").asDouble(), 0.7); - EXPECT_DOUBLE_EQ(payloadValue(payload, "max_acc").asDouble(), 0.8); - EXPECT_DOUBLE_EQ(payloadValue(payload, "max_wacc").asDouble(), 0.9); - EXPECT_DOUBLE_EQ(payloadValue(payload, "reach_dist").asDouble(), 0.1); - EXPECT_DOUBLE_EQ(payloadValue(payload, "reach_angle").asDouble(), 0.2); - EXPECT_EQ( - payloadValue(payload, "skill_name").asString(), - "GotoSpecifiedPose"); - EXPECT_FALSE(payloadHas(payload, "x")); - EXPECT_FALSE(payloadHas(payload, "y")); - EXPECT_FALSE(payloadHas(payload, "angle")); - EXPECT_FALSE(payloadHas(payload, "jack_height")); - - const auto status_payload = parsePayload(records[2]); - const auto& requested_task_ids = payloadValue(status_payload, "task_ids"); - ASSERT_TRUE(requested_task_ids.isArray()); - ASSERT_EQ(requested_task_ids.size(), 1U); - ASSERT_TRUE(requested_task_ids[0].isString()); - EXPECT_EQ( - requested_task_ids[0].asString(), - payloadValue(payload, "task_id").asString()); - const auto second_status_payload = parsePayload(records.back()); - const auto& second_requested_task_ids = - payloadValue(second_status_payload, "task_ids"); - ASSERT_TRUE(second_requested_task_ids.isArray()); - ASSERT_EQ(second_requested_task_ids.size(), 1U); - EXPECT_EQ( - second_requested_task_ids[0].asString(), - payloadValue(payload, "task_id").asString()); -} - -TEST_F( - Src1100ControlAuthorityTest, - NavigateToPoseUsesExplicitTaskIdAsUniquePrefixAndWhitelistsAdapterFields) -{ - controller_.setResponsePayload( - kRobotStatusTaskPackage, - R"({"ret_code":0,"task_status_package":{"task_status_list":[{"task_id":"${TASK_ID}","status":2,"type":1}]}})"); - AgvAdapterParams adapter_params; - adapter_params.values.emplace("source_id", "SELF_POSITION"); - adapter_params.values.emplace("target_id", "SELF_POSITION"); - adapter_params.values.emplace("task_id", "pose-task"); - adapter_params.values.emplace("skill_name", "GotoSpecifiedPose"); - adapter_params.values.emplace("operation", "JackHeight"); - adapter_params.values.emplace("jack_height", "0.5"); - adapter_params.values.emplace("script_name", "unsafe-script"); - adapter_params.values.emplace("unknown_field", "unsafe-value"); - controller_.clearRecords(); - - const auto result = agv_->navigateToPose( - math::Pose2d{0.0, 0.0, 0.5}, - {}, - adapter_params); - - ASSERT_TRUE(result.ok()) << result.message; - const auto records = controller_.records(); - ASSERT_GE(records.size(), 4U); - EXPECT_EQ(records[0].command, kRobotConfigLock); - EXPECT_EQ(records[1].command, kRobotTaskGoTarget); - for (std::size_t index = 2; index < records.size(); ++index) { - EXPECT_EQ(records[index].command, kRobotStatusTaskPackage); - } - - const auto payload = parsePayload(records[1]); - const std::string first_task_id = - payloadValue(payload, "task_id").asString(); - EXPECT_EQ(payloadValue(payload, "source_id").asString(), "SELF_POSITION"); - EXPECT_EQ(payloadValue(payload, "id").asString(), ""); - EXPECT_EQ( - first_task_id.find("pose-task_pose_"), - 0U); - EXPECT_EQ(payloadValue(payload, "skill_name").asString(), "GotoSpecifiedPose"); - const auto& free_go = payloadValue(payload, "freeGo"); - EXPECT_DOUBLE_EQ(payloadValue(free_go, "x").asDouble(), 0.0); - EXPECT_DOUBLE_EQ(payloadValue(free_go, "y").asDouble(), 0.0); - EXPECT_DOUBLE_EQ(payloadValue(free_go, "theta").asDouble(), 0.5); - EXPECT_FALSE(payloadHas(payload, "operation")); - EXPECT_FALSE(payloadHas(payload, "jack_height")); - EXPECT_FALSE(payloadHas(payload, "script_name")); - EXPECT_FALSE(payloadHas(payload, "unknown_field")); - - controller_.clearRecords(); - const auto second_result = agv_->navigateToPose( - math::Pose2d{0.0, 0.0, 0.5}, - {}, - adapter_params); - ASSERT_TRUE(second_result.ok()) << second_result.message; - const auto second_records = controller_.records(); - ASSERT_GE(second_records.size(), 2U); - const auto second_payload = parsePayload(second_records[1]); - const std::string second_task_id = - payloadValue(second_payload, "task_id").asString(); - EXPECT_EQ(second_task_id.find("pose-task_pose_"), 0U); - EXPECT_NE(first_task_id, second_task_id); -} - -TEST_F(Src1100ControlAuthorityTest, NavigateToPoseRejectsUnsafeFallbackStationAndSkill) -{ - AgvAdapterParams adapter_params; - adapter_params.values.emplace("source_id", "station-0"); - controller_.clearRecords(); - - auto result = agv_->navigateToPose( - math::Pose2d{1.0, 2.0, 0.5}, - {}, - adapter_params); - - EXPECT_FALSE(result.ok()); - EXPECT_EQ(result.code, AgvErrorCode::InvalidArgument); - EXPECT_NE(result.message.find("source_id must be SELF_POSITION"), std::string::npos); - EXPECT_TRUE(controller_.records().empty()); - - adapter_params.values.clear(); - adapter_params.values.emplace("target_id", "station-1"); - controller_.clearRecords(); - - result = agv_->navigateToPose( - math::Pose2d{1.0, 2.0, 0.5}, - {}, - adapter_params); - - EXPECT_FALSE(result.ok()); - EXPECT_EQ(result.code, AgvErrorCode::InvalidArgument); - EXPECT_NE(result.message.find("target_id must be empty"), std::string::npos); - EXPECT_TRUE(controller_.records().empty()); - - adapter_params.values.clear(); - adapter_params.values.emplace("skill_name", "unsafe-custom-skill"); - controller_.clearRecords(); - - result = agv_->navigateToPose( - math::Pose2d{1.0, 2.0, 0.5}, - {}, - adapter_params); - - EXPECT_FALSE(result.ok()); - EXPECT_EQ(result.code, AgvErrorCode::InvalidArgument); - EXPECT_NE(result.message.find("skill_name must be GotoSpecifiedPose"), std::string::npos); - EXPECT_TRUE(controller_.records().empty()); -} - -TEST_F( - Src1100ControlAuthorityTest, - MotionCommandsRejectNonFiniteNumericInputsBeforeAcquiringAuthority) -{ - const double nan = std::numeric_limits::quiet_NaN(); - const double infinity = std::numeric_limits::infinity(); - - controller_.clearRecords(); - auto result = agv_->navigateToPose(math::Pose2d{nan, 0.0, 0.0}); - EXPECT_FALSE(result.ok()); - EXPECT_EQ(result.code, AgvErrorCode::InvalidArgument); - EXPECT_NE(result.message.find("pose"), std::string::npos); - EXPECT_TRUE(controller_.records().empty()); - - AgvMotionOptions pose_options; - pose_options.reach_distance = infinity; - controller_.clearRecords(); - result = agv_->navigateToPose( - math::Pose2d{0.0, 0.0, 0.0}, - pose_options); - EXPECT_FALSE(result.ok()); - EXPECT_EQ(result.code, AgvErrorCode::InvalidArgument); - EXPECT_NE(result.message.find("reach_distance"), std::string::npos); - EXPECT_TRUE(controller_.records().empty()); - - AgvMotionOptions station_options; - station_options.max_acceleration = nan; - controller_.clearRecords(); - result = agv_->navigateToStation("station-1", station_options); - EXPECT_FALSE(result.ok()); - EXPECT_EQ(result.code, AgvErrorCode::InvalidArgument); - EXPECT_NE(result.message.find("max_acceleration"), std::string::npos); - EXPECT_TRUE(controller_.records().empty()); - - AgvMotionOptions negative_options; - negative_options.max_speed = -0.1; - controller_.clearRecords(); - result = agv_->navigateToPose( - math::Pose2d{0.0, 0.0, 0.0}, - negative_options); - EXPECT_FALSE(result.ok()); - EXPECT_EQ(result.code, AgvErrorCode::InvalidArgument); - EXPECT_NE(result.message.find("non-negative"), std::string::npos); - EXPECT_TRUE(controller_.records().empty()); - - AgvAdapterParams invalid_adapter; - invalid_adapter.values.emplace("jack_height", "inf"); - controller_.clearRecords(); - result = agv_->navigateToStation( - "station-1", - {}, - invalid_adapter); - EXPECT_FALSE(result.ok()); - EXPECT_EQ(result.code, AgvErrorCode::InvalidArgument); - EXPECT_NE(result.message.find("jack_height"), std::string::npos); - EXPECT_TRUE(controller_.records().empty()); - - controller_.clearRecords(); - result = agv_->setVelocity(AgvVelocity{0.0, infinity, 0.0}); - EXPECT_FALSE(result.ok()); - EXPECT_EQ(result.code, AgvErrorCode::InvalidArgument); - EXPECT_NE(result.message.find("velocity"), std::string::npos); - EXPECT_TRUE(controller_.records().empty()); -} - -TEST_F(Src1100ControlAuthorityTest, NavigateToPoseIgnoresUncorrelatedStatusUntilPoseTaskAppears) -{ - controller_.setResponsePayload( - kRobotStatusTaskPackage, - R"({"ret_code":0,"task_status_package":{"task_status_list":[{"task_id":"old-pose-task","status":2,"type":1}]}})"); - controller_.queueResponsePayload( - kRobotStatusTaskPackage, - R"({"ret_code":0,"task_status_package":{"task_status_list":[{"task_id":"${TASK_ID}","status":2,"type":1}]}})"); - controller_.clearRecords(); - - const auto result = agv_->navigateToPose(math::Pose2d{1.0, 2.0, 0.5}); - - ASSERT_TRUE(result.ok()) << result.message; - const auto records = controller_.records(); - ASSERT_GE(records.size(), 5U); - EXPECT_EQ(records[0].command, kRobotConfigLock); - EXPECT_EQ(records[1].command, kRobotTaskGoTarget); - for (std::size_t index = 2; index < records.size(); ++index) { - EXPECT_EQ(records[index].command, kRobotStatusTaskPackage); - } -} - -TEST_F(Src1100ControlAuthorityTest, NavigateToPoseWaitsForStableRunningAfterWaiting) -{ - controller_.setResponsePayload( - kRobotStatusTaskPackage, - R"({"ret_code":0,"task_status_package":{"task_status_list":[{"task_id":"${TASK_ID}","status":1,"type":1}]}})"); - controller_.queueResponsePayload( - kRobotStatusTaskPackage, - R"({"ret_code":0,"task_status_package":{"task_status_list":[{"task_id":"${TASK_ID}","status":2,"type":1}]}})"); - controller_.clearRecords(); - - const auto result = agv_->navigateToPose(math::Pose2d{1.0, 2.0, 0.5}); - - ASSERT_TRUE(result.ok()) << result.message; - const auto records = controller_.records(); - ASSERT_GE(records.size(), 5U); - EXPECT_EQ(records[0].command, kRobotConfigLock); - EXPECT_EQ(records[1].command, kRobotTaskGoTarget); - for (std::size_t index = 2; index < records.size(); ++index) { - EXPECT_EQ(records[index].command, kRobotStatusTaskPackage); - } -} - -TEST_F( - Src1100ControlAuthorityTest, - NavigateToPoseDoesNotReturnSuccessBeforeLateRunningFaultPush) -{ - controller_.clearRecords(); - AgvResult result = AgvResult::success(); - - std::thread pose_thread([this, &result]() { - result = agv_->navigateToPose(math::Pose2d{1.0, 2.0, 0.5}); - }); - - bool status_query_observed = false; - for (int attempt = 0; attempt < 200; ++attempt) { - const auto records = controller_.records(); - status_query_observed = std::any_of( - records.begin(), - records.end(), - [](const CommandRecord& record) { - return record.command == kRobotStatusTaskPackage; - }); - if (status_query_observed) { - break; - } - std::this_thread::sleep_for(std::chrono::milliseconds(1)); - } - EXPECT_TRUE(status_query_observed); - - Json::Value fault_push(Json::objectValue); - Json::Value errors(Json::arrayValue); - errors.append("E_RUNNING_31: safety controller rejected free navigation"); - *fault_push.demand("errors", "errors" + std::strlen("errors")) = errors; - Src1100AgvTestPeer::cacheRuntimeState(*agv_, fault_push); - pose_thread.join(); - - EXPECT_FALSE(result.ok()); - EXPECT_EQ(result.code, AgvErrorCode::Fault); - EXPECT_NE(result.message.find("E_RUNNING_31"), std::string::npos); - EXPECT_NE(result.message.find("Running state"), std::string::npos); - EXPECT_NE( - result.message.find("do not retry automatically"), - std::string::npos); -} - -TEST_F( - Src1100ControlAuthorityTest, - NavigateToPoseFailsIfFaultPushChannelInvalidatesDuringStartConfirmation) -{ - controller_.clearRecords(); - AgvResult result = AgvResult::success(); - - std::thread pose_thread([this, &result]() { - result = agv_->navigateToPose(math::Pose2d{1.0, 2.0, 0.5}); - }); - - bool status_query_observed = false; - for (int attempt = 0; attempt < 200; ++attempt) { - const auto records = controller_.records(); - status_query_observed = std::any_of( - records.begin(), - records.end(), - [](const CommandRecord& record) { - return record.command == kRobotStatusTaskPackage; - }); - if (status_query_observed) { - break; - } - std::this_thread::sleep_for(std::chrono::milliseconds(1)); - } - EXPECT_TRUE(status_query_observed); - - Src1100AgvTestPeer::setFaultStateUnknown(*agv_); - pose_thread.join(); - - EXPECT_FALSE(result.ok()); - EXPECT_EQ(result.code, AgvErrorCode::Fault); - EXPECT_NE( - result.message.find("fault monitoring became unavailable"), - std::string::npos); - EXPECT_NE( - result.message.find("push channel changed or was invalidated"), - std::string::npos); -} - -TEST_F( - Src1100ControlAuthorityTest, - NavigateToPoseRejectsCompletedTargetWhenLateControllerFaultArrives) -{ - controller_.setResponsePayload( - kRobotStatusTaskPackage, - R"({"ret_code":0,"task_status_package":{"task_status_list":[{"task_id":"${TASK_ID}","status":4,"type":1}]}})"); - controller_.setResponsePayload( - kRobotStatusLoc, - R"({"ret_code":0,"x":1.0,"y":2.0,"angle":0.5})"); - controller_.clearRecords(); - AgvResult result = AgvResult::success(); - - std::thread pose_thread([this, &result]() { - result = agv_->navigateToPose(math::Pose2d{1.0, 2.0, 0.5}); - }); - - bool status_query_observed = false; - for (int attempt = 0; attempt < 200; ++attempt) { - const auto records = controller_.records(); - status_query_observed = std::any_of( - records.begin(), - records.end(), - [](const CommandRecord& record) { - return record.command == kRobotStatusTaskPackage; - }); - if (status_query_observed) { - break; - } - std::this_thread::sleep_for(std::chrono::milliseconds(1)); - } - EXPECT_TRUE(status_query_observed); - - Json::Value fault_push(Json::objectValue); - Json::Value errors(Json::arrayValue); - errors.append("E_COMPLETED_45: controller rejected completed pose"); - *fault_push.demand("errors", "errors" + std::strlen("errors")) = errors; - *fault_push.demand("fatals", "fatals" + std::strlen("fatals")) = - Json::Value(Json::arrayValue); - Src1100AgvTestPeer::cacheRuntimeState(*agv_, fault_push); - pose_thread.join(); - - EXPECT_FALSE(result.ok()); - EXPECT_EQ(result.code, AgvErrorCode::Fault); - EXPECT_NE(result.message.find("reported Completed"), std::string::npos); - EXPECT_NE(result.message.find("E_COMPLETED_45"), std::string::npos); - const auto records = controller_.records(); - EXPECT_FALSE(std::any_of( - records.begin(), - records.end(), - [](const CommandRecord& record) { - return record.command == kRobotStatusLoc; - })); -} - -TEST_F( - Src1100ControlAuthorityTest, - NavigateToPoseRejectsFaultArrivingDuringCompletedPoseVerification) -{ - controller_.setResponsePayload( - kRobotStatusTaskPackage, - R"({"ret_code":0,"task_status_package":{"task_status_list":[{"task_id":"${TASK_ID}","status":4,"type":1}]}})"); - controller_.setResponsePayload( - kRobotStatusLoc, - R"({"ret_code":0,"x":1.0,"y":2.0,"angle":0.5})"); - controller_.setResponseDelay( - kRobotStatusLoc, - std::chrono::milliseconds(300)); - controller_.clearRecords(); - AgvResult result = AgvResult::success(); - - std::thread pose_thread([this, &result]() { - result = agv_->navigateToPose(math::Pose2d{1.0, 2.0, 0.5}); - }); - - bool location_query_observed = false; - for (int attempt = 0; attempt < 700; ++attempt) { - const auto records = controller_.records(); - location_query_observed = std::any_of( - records.begin(), - records.end(), - [](const CommandRecord& record) { - return record.command == kRobotStatusLoc; - }); - if (location_query_observed) { - break; - } - std::this_thread::sleep_for(std::chrono::milliseconds(1)); - } - EXPECT_TRUE(location_query_observed); - - Json::Value fault_push(Json::objectValue); - Json::Value errors(Json::arrayValue); - errors.append("E_POSE_VERIFY_46: fault during target verification"); - *fault_push.demand("errors", "errors" + std::strlen("errors")) = errors; - *fault_push.demand("fatals", "fatals" + std::strlen("fatals")) = - Json::Value(Json::arrayValue); - Src1100AgvTestPeer::cacheRuntimeState(*agv_, fault_push); - pose_thread.join(); - - EXPECT_FALSE(result.ok()); - EXPECT_EQ(result.code, AgvErrorCode::Fault); - EXPECT_NE(result.message.find("target verification"), std::string::npos); - EXPECT_NE(result.message.find("E_POSE_VERIFY_46"), std::string::npos); -} - -TEST_F( - Src1100ControlAuthorityTest, - NavigateToPoseDoesNotMisattributeFaultFromCommandAwaitingAck) -{ - controller_.clearRecords(); - AgvResult pose_result = AgvResult::success(); - AgvResult station_result = AgvResult::success(); - - std::thread pose_thread([this, &pose_result]() { - pose_result = - agv_->navigateToPose(math::Pose2d{1.0, 2.0, 0.5}); - }); - - bool status_query_observed = false; - for (int attempt = 0; attempt < 200; ++attempt) { - const auto records = controller_.records(); - status_query_observed = std::any_of( - records.begin(), - records.end(), - [](const CommandRecord& record) { - return record.command == kRobotStatusTaskPackage; - }); - if (status_query_observed) { - break; - } - std::this_thread::sleep_for(std::chrono::milliseconds(1)); - } - EXPECT_TRUE(status_query_observed); - - controller_.setResponseDelay( - kRobotTaskGoTarget, - std::chrono::milliseconds(300)); - std::thread station_thread([this, &station_result]() { - station_result = agv_->navigateToStation("station-1"); - }); - - bool station_command_observed = false; - for (int attempt = 0; attempt < 500; ++attempt) { - const auto records = controller_.records(); - const auto go_target_count = std::count_if( - records.begin(), - records.end(), - [](const CommandRecord& record) { - return record.command == kRobotTaskGoTarget; - }); - station_command_observed = go_target_count >= 2; - if (station_command_observed) { - break; - } - std::this_thread::sleep_for(std::chrono::milliseconds(1)); - } - EXPECT_TRUE(station_command_observed); - - Json::Value fault_push(Json::objectValue); - Json::Value errors(Json::arrayValue); - errors.append("E_STATION_ACK_18: fault from concurrent station command"); - *fault_push.demand("errors", "errors" + std::strlen("errors")) = errors; - Src1100AgvTestPeer::cacheRuntimeState(*agv_, fault_push); - - pose_thread.join(); - station_thread.join(); - - EXPECT_FALSE(pose_result.ok()); - EXPECT_EQ(pose_result.code, AgvErrorCode::Fault); - EXPECT_NE( - pose_result.message.find("another control command attempt"), - std::string::npos); - EXPECT_NE( - pose_result.message.find("cannot be attributed"), - std::string::npos); - EXPECT_NE( - pose_result.message.find("E_STATION_ACK_18"), - std::string::npos); - ASSERT_TRUE(station_result.ok()) << station_result.message; -} - -TEST_F( - Src1100ControlAuthorityTest, - NavigateToPoseDoesNotMisattributeFaultObservedAfterAnotherCommandAck) -{ - controller_.clearRecords(); - AgvResult pose_result = AgvResult::success(); - - std::thread pose_thread([this, &pose_result]() { - pose_result = - agv_->navigateToPose(math::Pose2d{1.0, 2.0, 0.5}); - }); - - bool status_query_observed = false; - for (int attempt = 0; attempt < 200; ++attempt) { - const auto records = controller_.records(); - status_query_observed = std::any_of( - records.begin(), - records.end(), - [](const CommandRecord& record) { - return record.command == kRobotStatusTaskPackage; - }); - if (status_query_observed) { - break; - } - std::this_thread::sleep_for(std::chrono::milliseconds(1)); - } - EXPECT_TRUE(status_query_observed); - - controller_.setResponseCode(kRobotTaskGoTarget, 4188); - const auto station_result = agv_->navigateToStation("station-1"); - EXPECT_FALSE(station_result.ok()); - EXPECT_NE(station_result.message.find("ret_code=4188"), std::string::npos); - - Json::Value fault_push(Json::objectValue); - Json::Value errors(Json::arrayValue); - errors.append("E_AFTER_ACK_19: delayed station command alarm"); - *fault_push.demand("errors", "errors" + std::strlen("errors")) = errors; - *fault_push.demand("fatals", "fatals" + std::strlen("fatals")) = - Json::Value(Json::arrayValue); - Src1100AgvTestPeer::cacheRuntimeState(*agv_, fault_push); - pose_thread.join(); - - EXPECT_FALSE(pose_result.ok()); - EXPECT_EQ(pose_result.code, AgvErrorCode::Fault); - EXPECT_NE( - pose_result.message.find("another control command attempt"), - std::string::npos); - EXPECT_NE( - pose_result.message.find("cannot be attributed"), - std::string::npos); - EXPECT_NE(pose_result.message.find("E_AFTER_ACK_19"), std::string::npos); -} - -TEST_F( - Src1100ControlAuthorityTest, - PauseDuringPoseCommandAckPreservesPublishedTaskContext) -{ - controller_.setResponseDelay( - kRobotTaskGoTarget, - std::chrono::milliseconds(300)); - controller_.clearRecords(); - AgvResult pose_result = AgvResult::success(); - AgvResult pause_result = AgvResult::failure( - AgvErrorCode::CommandFailed, - "pause not called"); - - std::thread pose_thread([this, &pose_result]() { - pose_result = - agv_->navigateToPose(math::Pose2d{1.0, 2.0, 0.5}); - }); - - bool pose_command_observed = false; - for (int attempt = 0; attempt < 200; ++attempt) { - const auto records = controller_.records(); - pose_command_observed = std::any_of( - records.begin(), - records.end(), - [](const CommandRecord& record) { - return record.command == kRobotTaskGoTarget; - }); - if (pose_command_observed) { - break; - } - std::this_thread::sleep_for(std::chrono::milliseconds(1)); - } - EXPECT_TRUE(pose_command_observed); - - std::thread pause_thread([this, &pause_result]() { - pause_result = agv_->pauseNavigation(); - }); - pause_thread.join(); - pose_thread.join(); - - ASSERT_TRUE(pause_result.ok()) << pause_result.message; - EXPECT_FALSE(pose_result.ok()); - EXPECT_NE(pose_result.message.find("superseded"), std::string::npos); - EXPECT_TRUE(Src1100AgvTestPeer::hasTrackedPoseTask(*agv_)); -} - -TEST_F( - Src1100ControlAuthorityTest, - NavigateToPoseReturnsAcceptedWhenMatchingTaskRemainsQueued) -{ - controller_.setResponsePayload( - kRobotStatusTaskPackage, - R"({"ret_code":0,"task_status_package":{"info":"queued","task_status_list":[{"task_id":"${TASK_ID}","status":1,"type":1}]}})"); - controller_.clearRecords(); - - const auto result = - agv_->navigateToPose(math::Pose2d{1.0, 2.0, 0.5}); - - ASSERT_TRUE(result.ok()) << result.message; - const auto records = controller_.records(); - const auto status_query_count = std::count_if( - records.begin(), - records.end(), - [](const CommandRecord& record) { - return record.command == kRobotStatusTaskPackage; - }); - EXPECT_GT(status_query_count, 2); -} - -TEST_F( - Src1100ControlAuthorityTest, - NavigateToPoseReturnsControllerFaultWhenQueuedTaskRaisesAlarm) -{ - controller_.setResponsePayload( - kRobotStatusTaskPackage, - R"({"ret_code":0,"task_status_package":{"info":"queued","task_status_list":[{"task_id":"${TASK_ID}","status":1,"type":1}]}})"); - controller_.clearRecords(); - AgvResult result = AgvResult::success(); - - std::thread pose_thread([this, &result]() { - result = agv_->navigateToPose(math::Pose2d{1.0, 2.0, 0.5}); - }); - - bool status_query_observed = false; - for (int attempt = 0; attempt < 200; ++attempt) { - const auto records = controller_.records(); - status_query_observed = std::any_of( - records.begin(), - records.end(), - [](const CommandRecord& record) { - return record.command == kRobotStatusTaskPackage; - }); - if (status_query_observed) { - break; - } - std::this_thread::sleep_for(std::chrono::milliseconds(1)); - } - EXPECT_TRUE(status_query_observed); - - Json::Value fault_push(Json::objectValue); - Json::Value errors(Json::arrayValue); - errors.append("E_WAIT_19: safety interlock"); - *fault_push.demand("errors", "errors" + std::strlen("errors")) = errors; - Src1100AgvTestPeer::cacheRuntimeState(*agv_, fault_push); - pose_thread.join(); - - EXPECT_FALSE(result.ok()); - EXPECT_EQ(result.code, AgvErrorCode::Fault); - EXPECT_NE(result.message.find("remains active"), std::string::npos); - EXPECT_NE(result.message.find("E_WAIT_19"), std::string::npos); - EXPECT_NE(result.message.find("do not retry automatically"), std::string::npos); -} - -TEST_F( - Src1100ControlAuthorityTest, - NavigateToPoseDoesNotReportAcceptedAfterMatchingTaskDisappears) -{ - controller_.setResponsePayload( - kRobotStatusTaskPackage, - R"({"ret_code":0,"task_status_package":{"task_status_list":[{"task_id":"${TASK_ID}","status":1,"type":1}]}})"); - controller_.queueResponsePayload( - kRobotStatusTaskPackage, - R"({"ret_code":0,"task_status_package":{"task_status_list":[]}})"); - controller_.clearRecords(); - - const auto result = - agv_->navigateToPose(math::Pose2d{1.0, 2.0, 0.5}); - - EXPECT_FALSE(result.ok()); - EXPECT_EQ(result.code, AgvErrorCode::TaskRejected); - EXPECT_NE( - result.message.find("not present in task_status_package"), - std::string::npos); -} - -TEST_F( - Src1100ControlAuthorityTest, - NavigateToPoseDoesNotHangWhenRunningTaskDisappears) -{ - controller_.setResponsePayload( - kRobotStatusTaskPackage, - R"({"ret_code":0,"task_status_package":{"task_status_list":[{"task_id":"${TASK_ID}","status":2,"type":1}]}})"); - controller_.queueResponsePayload( - kRobotStatusTaskPackage, - R"({"ret_code":0,"task_status_package":{"task_status_list":[]}})"); - controller_.clearRecords(); - - const auto started_at = std::chrono::steady_clock::now(); - const auto result = - agv_->navigateToPose(math::Pose2d{1.0, 2.0, 0.5}); - const auto elapsed = std::chrono::duration_cast( - std::chrono::steady_clock::now() - started_at); - - EXPECT_FALSE(result.ok()); - EXPECT_EQ(result.code, AgvErrorCode::TaskRejected); - EXPECT_NE( - result.message.find("not present in task_status_package"), - std::string::npos); - EXPECT_LT(elapsed, std::chrono::seconds(3)); -} - -TEST_F( - Src1100ControlAuthorityTest, - NavigateToPoseReturnsFailureAfterWaitingTransitionsToFailed) -{ - controller_.setResponsePayload( - kRobotStatusTaskPackage, - R"({"ret_code":0,"task_status_package":{"task_status_list":[{"task_id":"${TASK_ID}","status":1,"type":1}]}})"); - controller_.queueResponsePayload( - kRobotStatusTaskPackage, - R"({"ret_code":0,"create_on":"2026-07-31T10:00:00Z","err_msg":"controller task failed","task_status_package":{"closest_target":"goal-7","source_name":"SELF_POSITION","target_name":"free-goal","percentage":0.0,"distance":1.4,"info":"planner rejected pose","task_status_list":[{"task_id":"${TASK_ID}","status":5,"type":1}]}})"); - controller_.clearRecords(); - - const auto result = agv_->navigateToPose(math::Pose2d{1.0, 2.0, 0.5}); - - EXPECT_FALSE(result.ok()); - EXPECT_EQ(result.code, AgvErrorCode::TaskFailed); - EXPECT_NE(result.message.find("planner rejected pose"), std::string::npos); - EXPECT_NE(result.message.find("task_status=5"), std::string::npos); - EXPECT_NE(result.message.find("status_query_ret_code=0"), std::string::npos); - EXPECT_NE(result.message.find("controller task failed"), std::string::npos); - EXPECT_NE(result.message.find("closest_target=goal-7"), std::string::npos); - EXPECT_NE(result.message.find("distance=1.400000"), std::string::npos); - const auto records = controller_.records(); - ASSERT_EQ(records.size(), 4U); - EXPECT_EQ(records[2].command, kRobotStatusTaskPackage); - EXPECT_EQ(records[3].command, kRobotStatusTaskPackage); -} - -TEST_F( - Src1100ControlAuthorityTest, - NavigateToPoseRejectsCompletedTaskWhenRequestedTargetWasNotReached) -{ - controller_.setResponsePayload( - kRobotStatusTaskPackage, - R"({"ret_code":0,"task_status_package":{"task_status_list":[{"task_id":"${TASK_ID}","status":4,"type":1}]}})"); - controller_.setResponsePayload( - kRobotStatusLoc, - R"({"ret_code":0,"x":0.0,"y":0.0,"angle":0.0})"); - controller_.clearRecords(); - - const auto result = agv_->navigateToPose(math::Pose2d{1.0, 0.0, 0.0}); - - EXPECT_FALSE(result.ok()); - EXPECT_EQ(result.code, AgvErrorCode::TaskRejected); - EXPECT_NE(result.message.find("target was not reached"), std::string::npos); - EXPECT_NE(result.message.find("distance_error=1"), std::string::npos); - const auto records = controller_.records(); - ASSERT_EQ(records.size(), 4U); - EXPECT_EQ(records[2].command, kRobotStatusTaskPackage); - EXPECT_EQ(records[3].command, kRobotStatusLoc); -} - -TEST_F( - Src1100ControlAuthorityTest, - NavigateToPoseAcceptsCompletedTaskOnlyWhenRequestedTargetWasReached) -{ - controller_.setResponsePayload( - kRobotStatusTaskPackage, - R"({"ret_code":0,"task_status_package":{"task_status_list":[{"task_id":"${TASK_ID}","status":4,"type":1}]}})"); - controller_.setResponsePayload( - kRobotStatusLoc, - R"({"ret_code":0,"x":1.0,"y":2.0,"angle":0.5})"); - controller_.clearRecords(); - - const auto result = agv_->navigateToPose(math::Pose2d{1.0, 2.0, 0.5}); - - ASSERT_TRUE(result.ok()) << result.message; - const auto records = controller_.records(); - ASSERT_EQ(records.size(), 4U); - EXPECT_EQ(records[2].command, kRobotStatusTaskPackage); - EXPECT_EQ(records[3].command, kRobotStatusLoc); -} - -TEST_F( - Src1100ControlAuthorityTest, - NavigateToPoseDoesNotUseFaultOnlyPushAsAValidCompletedPose) -{ - Json::Value push_payload(Json::objectValue); - Json::Value errors(Json::arrayValue); - errors.append("controller fault without pose fields"); - *push_payload.demand("errors", "errors" + std::strlen("errors")) = errors; - Src1100AgvTestPeer::cacheRuntimeState(*agv_, push_payload); - Json::Value cleared_push(Json::objectValue); - *cleared_push.demand("errors", "errors" + std::strlen("errors")) = - Json::Value(Json::arrayValue); - *cleared_push.demand("fatals", "fatals" + std::strlen("fatals")) = - Json::Value(Json::arrayValue); - Src1100AgvTestPeer::cacheRuntimeState(*agv_, cleared_push); - controller_.setResponsePayload( - kRobotStatusTaskPackage, - R"({"ret_code":0,"task_status_package":{"task_status_list":[{"task_id":"${TASK_ID}","status":4,"type":1}]}})"); - controller_.setResponsePayload( - kRobotStatusLoc, - R"({"ret_code":0,"err_msg":""})"); - controller_.clearRecords(); - - const auto result = agv_->navigateToPose(math::Pose2d{0.0, 0.0, 0.0}); - - EXPECT_FALSE(result.ok()); - EXPECT_EQ(result.code, AgvErrorCode::TaskRejected); - EXPECT_NE( - result.message.find("target pose could not be verified"), - std::string::npos); - EXPECT_NE( - result.message.find("did not contain numeric x/y/angle"), - std::string::npos); -} - -TEST_F( - Src1100ControlAuthorityTest, - NavigateToPoseRejectsCompletedTaskWithNonNumericControllerPose) -{ - controller_.setResponsePayload( - kRobotStatusTaskPackage, - R"({"ret_code":0,"task_status_package":{"task_status_list":[{"task_id":"${TASK_ID}","status":4,"type":1}]}})"); - controller_.setResponsePayload( - kRobotStatusLoc, - R"({"ret_code":0,"x":null,"y":false,"angle":"0.0"})"); - controller_.clearRecords(); - - const auto result = agv_->navigateToPose(math::Pose2d{0.0, 0.0, 0.0}); - - EXPECT_FALSE(result.ok()); - EXPECT_EQ(result.code, AgvErrorCode::TaskRejected); - EXPECT_NE( - result.message.find("did not contain numeric x/y/angle"), - std::string::npos); -} - -TEST_F(Src1100ControlAuthorityTest, NavigateToPoseReturnsAsynchronousControllerFailure) -{ - controller_.setResponsePayload( - kRobotStatusTaskPackage, - R"({"ret_code":0,"task_status_package":{"info":"planner rejected pose","task_status_list":[{"task_id":"${TASK_ID}","status":5,"type":1}]}})"); - controller_.clearRecords(); - - const auto result = agv_->navigateToPose(math::Pose2d{1.0, 2.0, 0.5}); - - EXPECT_FALSE(result.ok()); - EXPECT_EQ(result.code, AgvErrorCode::TaskFailed); - EXPECT_NE(result.message.find("planner rejected pose"), std::string::npos); - EXPECT_NE(result.message.find("task_status=5"), std::string::npos); - EXPECT_NE(result.message.find("task_type=1"), std::string::npos); -} - -TEST_F(Src1100ControlAuthorityTest, NavigateToPosePreservesSynchronousControllerCode) -{ - controller_.setResponseCode(kRobotTaskGoTarget, 43051); - controller_.clearRecords(); - - const auto result = agv_->navigateToPose(math::Pose2d{1.0, 2.0, 0.5}); - - EXPECT_FALSE(result.ok()); - EXPECT_EQ(result.code, AgvErrorCode::CommandFailed); - EXPECT_NE(result.message.find("ret_code=43051"), std::string::npos); - EXPECT_NE(result.message.find("err_msg=simulated command failure"), std::string::npos); - const auto records = controller_.records(); - ASSERT_EQ(records.size(), 2U); - EXPECT_EQ(records[0].command, kRobotConfigLock); - EXPECT_EQ(records[1].command, kRobotTaskGoTarget); -} - -TEST_F(Src1100ControlAuthorityTest, NavigateToPosePreservesStatusQueryControllerCode) -{ - controller_.setResponseCode(kRobotStatusTaskPackage, 41110); - controller_.clearRecords(); - - const auto result = agv_->navigateToPose(math::Pose2d{1.0, 2.0, 0.5}); - - EXPECT_FALSE(result.ok()); - EXPECT_EQ(result.code, AgvErrorCode::CommandFailed); - EXPECT_NE(result.message.find("ret_code=41110"), std::string::npos); - EXPECT_NE(result.message.find("err_msg=simulated command failure"), std::string::npos); -} - -TEST_F(Src1100ControlAuthorityTest, NavigateToPoseRejectsWrongResponseCommand) -{ - controller_.setResponseCommand(kRobotTaskGoTarget, 13052); - controller_.clearRecords(); - - const auto result = agv_->navigateToPose(math::Pose2d{1.0, 2.0, 0.5}); - - EXPECT_FALSE(result.ok()); - EXPECT_EQ(result.code, AgvErrorCode::CommandFailed); - EXPECT_NE(result.message.find("expected=13051"), std::string::npos); - EXPECT_NE(result.message.find("actual=13052"), std::string::npos); - EXPECT_NE(result.message.find("channel closed"), std::string::npos); - EXPECT_NE(result.message.find("controller outcome is unknown"), std::string::npos); - const auto records = controller_.records(); - ASSERT_EQ(records.size(), 2U); - EXPECT_EQ(records[1].command, kRobotTaskGoTarget); -} - -TEST_F(Src1100ControlAuthorityTest, NavigateToPoseRejectsMissingControllerCode) -{ - controller_.setResponsePayload( - kRobotTaskGoTarget, - R"({"err_msg":"missing acknowledgment code"})"); - controller_.clearRecords(); - - const auto result = agv_->navigateToPose(math::Pose2d{1.0, 2.0, 0.5}); - - EXPECT_FALSE(result.ok()); - EXPECT_EQ(result.code, AgvErrorCode::CommandFailed); - EXPECT_NE(result.message.find("missing a numeric ret_code"), std::string::npos); - EXPECT_NE(result.message.find("controller outcome is unknown"), std::string::npos); - const auto records = controller_.records(); - ASSERT_EQ(records.size(), 2U); - EXPECT_EQ(records[1].command, kRobotTaskGoTarget); -} - -TEST_F(Src1100ControlAuthorityTest, NavigateToPoseReportsPausedTaskExplicitly) -{ - controller_.setResponsePayload( - kRobotStatusTaskPackage, - R"({"ret_code":0,"task_status_package":{"info":"safety pause","task_status_list":[{"task_id":"${TASK_ID}","status":3,"type":1}]}})"); - controller_.clearRecords(); - - const auto result = agv_->navigateToPose(math::Pose2d{1.0, 2.0, 0.5}); - - EXPECT_FALSE(result.ok()); - EXPECT_EQ(result.code, AgvErrorCode::TaskRejected); - EXPECT_NE(result.message.find("established but is paused"), std::string::npos); - EXPECT_NE(result.message.find("safety pause"), std::string::npos); - EXPECT_NE(result.message.find("do not retry automatically"), std::string::npos); -} - -TEST_F( - Src1100ControlAuthorityTest, - NavigateToPosePreservesControllerFaultWhenTaskImmediatelyPauses) -{ - controller_.setResponsePayload( - kRobotStatusTaskPackage, - R"({"ret_code":0,"task_status_package":{"info":"safety pause","task_status_list":[{"task_id":"${TASK_ID}","status":3,"type":1}]}})"); - controller_.clearRecords(); - AgvResult result = AgvResult::success(); - - std::thread pose_thread([this, &result]() { - result = agv_->navigateToPose(math::Pose2d{1.0, 2.0, 0.5}); - }); - - bool status_query_observed = false; - for (int attempt = 0; attempt < 200; ++attempt) { - const auto records = controller_.records(); - status_query_observed = std::any_of( - records.begin(), - records.end(), - [](const CommandRecord& record) { - return record.command == kRobotStatusTaskPackage; - }); - if (status_query_observed) { - break; - } - std::this_thread::sleep_for(std::chrono::milliseconds(1)); - } - EXPECT_TRUE(status_query_observed); - - Json::Value fault_push(Json::objectValue); - Json::Value errors(Json::arrayValue); - errors.append("E_PAUSED_55: safety controller paused failed task"); - *fault_push.demand("errors", "errors" + std::strlen("errors")) = errors; - *fault_push.demand("fatals", "fatals" + std::strlen("fatals")) = - Json::Value(Json::arrayValue); - Src1100AgvTestPeer::cacheRuntimeState(*agv_, fault_push); - pose_thread.join(); - - EXPECT_FALSE(result.ok()); - EXPECT_EQ(result.code, AgvErrorCode::Fault); - EXPECT_NE(result.message.find("paused"), std::string::npos); - EXPECT_NE(result.message.find("E_PAUSED_55"), std::string::npos); -} - -TEST_F( - Src1100ControlAuthorityTest, - NavigateToPoseIncludesTaskCorrelatedRawControllerFaultDetail) -{ - controller_.setResponsePayload( - kRobotStatusTaskPackage, - R"({"ret_code":0,"task_status_package":{"task_status_list":[]}})"); - controller_.clearRecords(); - AgvResult result = AgvResult::success(); - - std::thread pose_thread([this, &result]() { - result = agv_->navigateToPose(math::Pose2d{1.0, 2.0, 0.5}); - }); - - bool status_query_observed = false; - for (int attempt = 0; attempt < 200; ++attempt) { - const auto records = controller_.records(); - status_query_observed = std::any_of( - records.begin(), - records.end(), - [](const CommandRecord& record) { - return record.command == kRobotStatusTaskPackage; - }); - if (status_query_observed) { - break; - } - std::this_thread::sleep_for(std::chrono::milliseconds(1)); - } - EXPECT_TRUE(status_query_observed); - - Json::Value fault_push(Json::objectValue); - Json::Value errors(Json::arrayValue); - errors.append("E_NAV_42: planner alarm"); - *fault_push.demand("errors", "errors" + std::strlen("errors")) = errors; - Src1100AgvTestPeer::cacheRuntimeState(*agv_, fault_push); - - Json::Value cleared_push(Json::objectValue); - *cleared_push.demand("errors", "errors" + std::strlen("errors")) = - Json::Value(Json::arrayValue); - *cleared_push.demand("fatals", "fatals" + std::strlen("fatals")) = - Json::Value(Json::arrayValue); - Src1100AgvTestPeer::cacheRuntimeState(*agv_, cleared_push); - controller_.setResponsePayload( - kRobotStatusTaskPackage, - R"({"ret_code":0,"task_status_package":{"info":"navigation failed","task_status_list":[{"task_id":"${TASK_ID}","status":5,"type":1}]}})"); - pose_thread.join(); - - EXPECT_FALSE(result.ok()); - EXPECT_EQ(result.code, AgvErrorCode::TaskFailed); - EXPECT_NE(result.message.find("E_NAV_42"), std::string::npos); - EXPECT_NE(result.message.find("planner alarm"), std::string::npos); -} - -TEST_F( - Src1100ControlAuthorityTest, - NavigateToPoseRejectsPreexistingControllerFaultWithoutSendingTask) -{ - Json::Value push_payload(Json::objectValue); - Json::Value errors(Json::arrayValue); - errors.append("OLD_FAULT_FROM_PREVIOUS_TASK"); - *push_payload.demand("errors", "errors" + std::strlen("errors")) = errors; - Src1100AgvTestPeer::cacheRuntimeState(*agv_, push_payload); - controller_.clearRecords(); - const auto result = - agv_->navigateToPose(math::Pose2d{1.0, 2.0, 0.5}); - - EXPECT_FALSE(result.ok()); - EXPECT_EQ(result.code, AgvErrorCode::Fault); - EXPECT_NE( - result.message.find("OLD_FAULT_FROM_PREVIOUS_TASK"), - std::string::npos); - EXPECT_NE(result.message.find("was not sent"), std::string::npos); - const auto records = controller_.records(); - ASSERT_EQ(records.size(), 1U); - EXPECT_EQ(records[0].command, kRobotConfigLock); -} - -TEST_F( - Src1100ControlAuthorityTest, - NavigateToPoseRejectsUnknownOrStaleFaultStateWithoutSendingTask) -{ - Src1100AgvTestPeer::setFaultStateUnknown(*agv_); - controller_.clearRecords(); - - auto result = - agv_->navigateToPose(math::Pose2d{1.0, 2.0, 0.5}); - - EXPECT_FALSE(result.ok()); - EXPECT_EQ(result.code, AgvErrorCode::Fault); - EXPECT_NE( - result.message.find("no state push containing fatals/errors"), - std::string::npos); - auto records = controller_.records(); - ASSERT_EQ(records.size(), 1U); - EXPECT_EQ(records[0].command, kRobotConfigLock); - - Src1100AgvTestPeer::setFaultStateAge( - *agv_, - std::chrono::seconds(3)); - controller_.clearRecords(); - - result = agv_->navigateToPose(math::Pose2d{1.0, 2.0, 0.5}); - - EXPECT_FALSE(result.ok()); - EXPECT_EQ(result.code, AgvErrorCode::Fault); - EXPECT_NE(result.message.find("state push is stale"), std::string::npos); - records = controller_.records(); - ASSERT_EQ(records.size(), 1U); - EXPECT_EQ(records[0].command, kRobotConfigLock); -} - -TEST_F( - Src1100ControlAuthorityTest, - NavigateToPoseRejectsMalformedFaultStateWithoutSendingTask) -{ - Json::Value malformed_push(Json::objectValue); - *malformed_push.demand("errors", "errors" + std::strlen("errors")) = - Json::Value(); - *malformed_push.demand("fatals", "fatals" + std::strlen("fatals")) = - Json::Value(Json::arrayValue); - Src1100AgvTestPeer::cacheRuntimeState(*agv_, malformed_push); - controller_.clearRecords(); - - const auto result = - agv_->navigateToPose(math::Pose2d{1.0, 2.0, 0.5}); - - EXPECT_FALSE(result.ok()); - EXPECT_EQ(result.code, AgvErrorCode::Fault); - EXPECT_NE( - result.message.find("incomplete or malformed"), - std::string::npos); - EXPECT_NE(result.message.find("errors=null"), std::string::npos); - const auto records = controller_.records(); - ASSERT_EQ(records.size(), 1U); - EXPECT_EQ(records[0].command, kRobotConfigLock); -} - -TEST_F( - Src1100ControlAuthorityTest, - NavigateToPoseDoesNotAttributeClearedFaultHistoryToNewTask) -{ - Json::Value fault_push(Json::objectValue); - Json::Value errors(Json::arrayValue); - errors.append("OLD_CLEARED_FAULT"); - *fault_push.demand("errors", "errors" + std::strlen("errors")) = errors; - Src1100AgvTestPeer::cacheRuntimeState(*agv_, fault_push); - - Json::Value cleared_push(Json::objectValue); - *cleared_push.demand("errors", "errors" + std::strlen("errors")) = - Json::Value(Json::arrayValue); - *cleared_push.demand("fatals", "fatals" + std::strlen("fatals")) = - Json::Value(Json::arrayValue); - Src1100AgvTestPeer::cacheRuntimeState(*agv_, cleared_push); - controller_.setResponsePayload( - kRobotStatusTaskPackage, - R"({"ret_code":0,"task_status_package":{"info":"new task failed","task_status_list":[{"task_id":"${TASK_ID}","status":5,"type":1}]}})"); - controller_.clearRecords(); - - const auto result = - agv_->navigateToPose(math::Pose2d{1.0, 2.0, 0.5}); - - EXPECT_FALSE(result.ok()); - EXPECT_EQ(result.code, AgvErrorCode::TaskFailed); - EXPECT_NE(result.message.find("new task failed"), std::string::npos); - EXPECT_EQ(result.message.find("OLD_CLEARED_FAULT"), std::string::npos); -} - -TEST_F( - Src1100ControlAuthorityTest, - CancelSupersedesCompletedPoseWhileLocationVerificationIsInFlight) -{ - controller_.setResponsePayload( - kRobotStatusTaskPackage, - R"({"ret_code":0,"task_status_package":{"task_status_list":[{"task_id":"${TASK_ID}","status":4,"type":1}]}})"); - controller_.setResponsePayload( - kRobotStatusLoc, - R"({"ret_code":0,"x":1.0,"y":2.0,"angle":0.5})"); - controller_.setResponseDelay( - kRobotStatusLoc, - std::chrono::milliseconds(300)); - controller_.clearRecords(); - AgvResult pose_result = AgvResult::success(); - - std::thread pose_thread([this, &pose_result]() { - pose_result = agv_->navigateToPose(math::Pose2d{1.0, 2.0, 0.5}); - }); - - bool location_query_observed = false; - for (int attempt = 0; attempt < 500; ++attempt) { - const auto records = controller_.records(); - location_query_observed = std::any_of( - records.begin(), - records.end(), - [](const CommandRecord& record) { - return record.command == kRobotStatusLoc; - }); - if (location_query_observed) { - break; - } - std::this_thread::sleep_for(std::chrono::milliseconds(1)); - } - EXPECT_TRUE(location_query_observed); - - const auto cancel_result = agv_->cancelNavigation(); - pose_thread.join(); - - ASSERT_TRUE(cancel_result.ok()) << cancel_result.message; - EXPECT_FALSE(pose_result.ok()); - EXPECT_EQ(pose_result.code, AgvErrorCode::CommandFailed); - EXPECT_NE(pose_result.message.find("superseded"), std::string::npos); -} - -TEST_F(Src1100ControlAuthorityTest, CancelSupersedesPoseStartConfirmation) -{ - controller_.setResponsePayload( - kRobotStatusTaskPackage, - R"({"ret_code":0,"task_status_package":{"task_status_list":[]}})"); - controller_.clearRecords(); - AgvResult pose_result = AgvResult::success(); - - std::thread pose_thread([this, &pose_result]() { - pose_result = agv_->navigateToPose(math::Pose2d{1.0, 2.0, 0.5}); - }); - - bool status_query_observed = false; - for (int attempt = 0; attempt < 200; ++attempt) { - const auto records = controller_.records(); - status_query_observed = std::any_of( - records.begin(), - records.end(), - [](const CommandRecord& record) { - return record.command == kRobotStatusTaskPackage; - }); - if (status_query_observed) { - break; - } - std::this_thread::sleep_for(std::chrono::milliseconds(1)); - } - - const auto cancel_result = agv_->cancelNavigation(); - pose_thread.join(); - - EXPECT_TRUE(status_query_observed); - ASSERT_TRUE(cancel_result.ok()) << cancel_result.message; - EXPECT_FALSE(pose_result.ok()); - EXPECT_EQ(pose_result.code, AgvErrorCode::CommandFailed); - EXPECT_NE(pose_result.message.find("superseded"), std::string::npos); - EXPECT_NE(pose_result.message.find("state is unknown"), std::string::npos); -} - -TEST_F(Src1100ControlAuthorityTest, FailedCancelDoesNotSupersedePoseStartConfirmation) -{ - controller_.setResponsePayload( - kRobotStatusTaskPackage, - R"({"ret_code":0,"task_status_package":{"task_status_list":[]}})"); - controller_.clearRecords(); - AgvResult pose_result = AgvResult::success(); - std::atomic_bool pose_finished{false}; - - std::thread pose_thread([this, &pose_result, &pose_finished]() { - pose_result = agv_->navigateToPose(math::Pose2d{1.0, 2.0, 0.5}); - pose_finished.store(true, std::memory_order_release); - }); - - bool status_query_observed = false; - for (int attempt = 0; attempt < 200; ++attempt) { - const auto records = controller_.records(); - status_query_observed = std::any_of( - records.begin(), - records.end(), - [](const CommandRecord& record) { - return record.command == kRobotStatusTaskPackage; - }); - if (status_query_observed) { - break; - } - std::this_thread::sleep_for(std::chrono::milliseconds(1)); - } - EXPECT_TRUE(status_query_observed); - - controller_.setResponseCode(kRobotConfigLock, 17); - const auto authority_failure = agv_->cancelNavigation(); - EXPECT_FALSE(authority_failure.ok()); - std::this_thread::sleep_for(std::chrono::milliseconds(75)); - EXPECT_FALSE(pose_finished.load(std::memory_order_acquire)); - - controller_.setResponseCode(kRobotConfigLock, 0); - controller_.setResponseCode(kRobotTaskCancel, 23); - const auto command_failure = agv_->cancelNavigation(); - EXPECT_FALSE(command_failure.ok()); - std::this_thread::sleep_for(std::chrono::milliseconds(75)); - EXPECT_FALSE(pose_finished.load(std::memory_order_acquire)); - - controller_.setResponseCode(kRobotTaskCancel, 0); - const auto successful_cancel = agv_->cancelNavigation(); - pose_thread.join(); - - ASSERT_TRUE(successful_cancel.ok()) << successful_cancel.message; - EXPECT_TRUE(pose_finished.load(std::memory_order_acquire)); - EXPECT_FALSE(pose_result.ok()); - EXPECT_EQ(pose_result.code, AgvErrorCode::CommandFailed); - EXPECT_NE(pose_result.message.find("superseded"), std::string::npos); - EXPECT_NE(pose_result.message.find("state is unknown"), std::string::npos); -} - -TEST_F(Src1100ControlAuthorityTest, IndeterminateCancelSupersedesPoseStartConfirmation) -{ - controller_.setResponsePayload( - kRobotStatusTaskPackage, - R"({"ret_code":0,"task_status_package":{"task_status_list":[]}})"); - controller_.clearRecords(); - AgvResult pose_result = AgvResult::success(); - - std::thread pose_thread([this, &pose_result]() { - pose_result = agv_->navigateToPose(math::Pose2d{1.0, 2.0, 0.5}); - }); - - bool status_query_observed = false; - for (int attempt = 0; attempt < 200; ++attempt) { - const auto records = controller_.records(); - status_query_observed = std::any_of( - records.begin(), - records.end(), - [](const CommandRecord& record) { - return record.command == kRobotStatusTaskPackage; - }); - if (status_query_observed) { - break; - } - std::this_thread::sleep_for(std::chrono::milliseconds(1)); - } - ASSERT_TRUE(status_query_observed); - - controller_.setResponsePayload( - kRobotTaskCancel, - R"({"err_msg":"acknowledgment lost"})"); - const auto cancel_result = agv_->cancelNavigation(); - pose_thread.join(); - - EXPECT_FALSE(cancel_result.ok()); - EXPECT_NE(cancel_result.message.find("missing a numeric ret_code"), std::string::npos); - EXPECT_NE(cancel_result.message.find("controller outcome is unknown"), std::string::npos); - EXPECT_NE( - cancel_result.message.find("do not issue another motion command automatically"), - std::string::npos); - EXPECT_FALSE(pose_result.ok()); - EXPECT_EQ(pose_result.code, AgvErrorCode::CommandFailed); - EXPECT_NE(pose_result.message.find("state is unknown"), std::string::npos); -} - -TEST_F(Src1100ControlAuthorityTest, TimedOutChannelIsClosedBeforeSameCommandCanRetry) -{ - Src1100AgvTestPeer::setNavigationReceiveTimeout( - *agv_, - std::chrono::milliseconds(50)); - controller_.setResponseDelay( - kRobotTaskCancel, - std::chrono::milliseconds(200)); - controller_.clearRecords(); - - const auto first_result = agv_->cancelNavigation(); - const auto second_result = agv_->cancelNavigation(); - - EXPECT_FALSE(first_result.ok()); - EXPECT_EQ(first_result.code, AgvErrorCode::Timeout); - EXPECT_NE(first_result.message.find("channel closed"), std::string::npos); - EXPECT_NE( - first_result.message.find("controller outcome is unknown"), - std::string::npos); - EXPECT_FALSE(second_result.ok()); - EXPECT_EQ(second_result.code, AgvErrorCode::NotConnected); - EXPECT_NE( - second_result.message.find("controller outcome is unknown"), - std::string::npos); - - const auto records = controller_.records(); - const auto cancel_count = std::count_if( - records.begin(), - records.end(), - [](const CommandRecord& record) { - return record.command == kRobotTaskCancel; - }); - EXPECT_EQ(cancel_count, 1); -} - -TEST_F(Src1100ControlAuthorityTest, SlowTaskStatusDoesNotBlockEmergencyStop) -{ - controller_.setResponsePayload( - kRobotStatusTaskPackage, - R"({"ret_code":0,"task_status_package":{"task_status_list":[]}})"); - controller_.setResponseDelay( - kRobotStatusTaskPackage, - std::chrono::milliseconds(300)); - controller_.clearRecords(); - AgvResult pose_result = AgvResult::success(); - - std::thread pose_thread([this, &pose_result]() { - pose_result = agv_->navigateToPose(math::Pose2d{1.0, 2.0, 0.5}); - }); - - bool status_query_observed = false; - for (int attempt = 0; attempt < 200; ++attempt) { - const auto records = controller_.records(); - status_query_observed = std::any_of( - records.begin(), - records.end(), - [](const CommandRecord& record) { - return record.command == kRobotStatusTaskPackage; - }); - if (status_query_observed) { - break; - } - std::this_thread::sleep_for(std::chrono::milliseconds(1)); - } - ASSERT_TRUE(status_query_observed); - - const auto start = std::chrono::steady_clock::now(); - const auto stop_result = agv_->emergencyStop(); - const auto elapsed = std::chrono::duration_cast( - std::chrono::steady_clock::now() - start); - pose_thread.join(); - - ASSERT_TRUE(stop_result.ok()) << stop_result.message; - EXPECT_LT(elapsed.count(), 150); - EXPECT_FALSE(pose_result.ok()); - EXPECT_EQ(pose_result.code, AgvErrorCode::CommandFailed); - EXPECT_NE(pose_result.message.find("state is unknown"), std::string::npos); - - const auto records = controller_.records(); - const auto control_stop = std::find_if( - records.begin(), - records.end(), - [](const CommandRecord& record) { - return record.command == kRobotControlStop; - }); - const auto navigation_cancel = std::find_if( - records.begin(), - records.end(), - [](const CommandRecord& record) { - return record.command == kRobotTaskCancel; - }); - ASSERT_NE(control_stop, records.end()); - ASSERT_NE(navigation_cancel, records.end()); - EXPECT_LT(control_stop, navigation_cancel); -} - -TEST_F(Src1100ControlAuthorityTest, EmergencyStopInvalidatesPoseAfterFirstAcceptedStop) -{ - controller_.setResponseDelay( - kRobotStatusTaskPackage, - std::chrono::milliseconds(200)); - controller_.setResponseDelay( - kRobotTaskCancel, - std::chrono::milliseconds(400)); - controller_.clearRecords(); - AgvResult pose_result = AgvResult::success(); - AgvResult stop_result = AgvResult::success(); - std::atomic_bool pose_finished{false}; - std::atomic_bool stop_finished{false}; - - std::thread pose_thread([this, &pose_result, &pose_finished]() { - pose_result = agv_->navigateToPose(math::Pose2d{1.0, 2.0, 0.5}); - pose_finished.store(true, std::memory_order_release); - }); - - bool status_query_observed = false; - for (int attempt = 0; attempt < 200; ++attempt) { - const auto records = controller_.records(); - status_query_observed = std::any_of( - records.begin(), - records.end(), - [](const CommandRecord& record) { - return record.command == kRobotStatusTaskPackage; - }); - if (status_query_observed) { - break; - } - std::this_thread::sleep_for(std::chrono::milliseconds(1)); - } - ASSERT_TRUE(status_query_observed); - - std::thread stop_thread([this, &stop_result, &stop_finished]() { - stop_result = agv_->emergencyStop(); - stop_finished.store(true, std::memory_order_release); - }); - - bool control_stop_observed = false; - for (int attempt = 0; attempt < 200; ++attempt) { - const auto records = controller_.records(); - control_stop_observed = std::any_of( - records.begin(), - records.end(), - [](const CommandRecord& record) { - return record.command == kRobotControlStop; - }); - if (control_stop_observed) { - break; - } - std::this_thread::sleep_for(std::chrono::milliseconds(1)); - } - ASSERT_TRUE(control_stop_observed); - - for (int attempt = 0; attempt < 350; ++attempt) { - if (pose_finished.load(std::memory_order_acquire)) { - break; - } - std::this_thread::sleep_for(std::chrono::milliseconds(1)); - } - ASSERT_TRUE(pose_finished.load(std::memory_order_acquire)); - EXPECT_FALSE(stop_finished.load(std::memory_order_acquire)); - EXPECT_FALSE(pose_result.ok()); - EXPECT_EQ(pose_result.code, AgvErrorCode::CommandFailed); - EXPECT_NE(pose_result.message.find("state is unknown"), std::string::npos); - - stop_thread.join(); - pose_thread.join(); - ASSERT_TRUE(stop_result.ok()) << stop_result.message; -} - -TEST_F(Src1100ControlAuthorityTest, NavigateToStationForwardsTypedMotionOptions) -{ - AgvMotionOptions options; - options.max_speed = 0.4; - options.max_angular_speed = 0.5; - options.max_acceleration = 0.6; - options.max_angular_acceleration = 0.7; - AgvAdapterParams adapter_params; - adapter_params.values.emplace("id", "wrong-station"); - adapter_params.values.emplace("x", "99.0"); - adapter_params.values.emplace("freeGo", "invalid"); - adapter_params.values.emplace("max_speed", "not-a-number"); - adapter_params.values.emplace("reach_dist", "not-a-number"); - controller_.clearRecords(); - - const auto result = agv_->navigateToStation( - "station-1", - options, - adapter_params); - - ASSERT_TRUE(result.ok()) << result.message; - const auto records = controller_.records(); - ASSERT_EQ(records.size(), 2U); - EXPECT_EQ(records[0].command, kRobotConfigLock); - EXPECT_EQ(records[1].command, kRobotTaskGoTarget); - - const auto payload = parsePayload(records[1]); - EXPECT_EQ(payloadValue(payload, "source_id").asString(), "SELF_POSITION"); - EXPECT_EQ(payloadValue(payload, "id").asString(), "station-1"); - EXPECT_TRUE(payloadValue(payload, "max_speed").isNumeric()); - EXPECT_TRUE(payloadValue(payload, "max_wspeed").isNumeric()); - EXPECT_TRUE(payloadValue(payload, "max_acc").isNumeric()); - EXPECT_TRUE(payloadValue(payload, "max_wacc").isNumeric()); - EXPECT_DOUBLE_EQ(payloadValue(payload, "max_speed").asDouble(), 0.4); - EXPECT_DOUBLE_EQ(payloadValue(payload, "max_wspeed").asDouble(), 0.5); - EXPECT_DOUBLE_EQ(payloadValue(payload, "max_acc").asDouble(), 0.6); - EXPECT_DOUBLE_EQ(payloadValue(payload, "max_wacc").asDouble(), 0.7); - EXPECT_FALSE(payloadHas(payload, "x")); - EXPECT_FALSE(payloadHas(payload, "freeGo")); - EXPECT_FALSE(payloadHas(payload, "reach_dist")); - EXPECT_FALSE(payloadHas(payload, "jack_height")); -} - -TEST_F(Src1100ControlAuthorityTest, SetVelocityUsesOnlyDocumentedNumericFields) -{ - controller_.clearRecords(); - - const auto result = agv_->setVelocity(AgvVelocity{0.1, -0.2, 0.3}); - - ASSERT_TRUE(result.ok()) << result.message; - auto records = controller_.records(); - ASSERT_EQ(records.size(), 2U); - EXPECT_EQ(records[0].command, kRobotConfigLock); - EXPECT_EQ(records[1].command, kRobotControlMotion); - - auto payload = parsePayload(records[1]); - EXPECT_TRUE(payloadValue(payload, "vx").isNumeric()); - EXPECT_TRUE(payloadValue(payload, "vy").isNumeric()); - EXPECT_TRUE(payloadValue(payload, "w").isNumeric()); - EXPECT_DOUBLE_EQ(payloadValue(payload, "vx").asDouble(), 0.1); - EXPECT_DOUBLE_EQ(payloadValue(payload, "vy").asDouble(), -0.2); - EXPECT_DOUBLE_EQ(payloadValue(payload, "w").asDouble(), 0.3); - EXPECT_FALSE(payloadHas(payload, "duration")); - - controller_.clearRecords(); - const auto stop_result = agv_->stopVelocityControl(); - - ASSERT_TRUE(stop_result.ok()) << stop_result.message; - records = controller_.records(); - ASSERT_EQ(records.size(), 2U); - EXPECT_EQ(records[1].command, kRobotControlMotion); - payload = parsePayload(records[1]); - EXPECT_TRUE(payloadValue(payload, "vx").isNumeric()); - EXPECT_TRUE(payloadValue(payload, "vy").isNumeric()); - EXPECT_TRUE(payloadValue(payload, "w").isNumeric()); - EXPECT_DOUBLE_EQ(payloadValue(payload, "vx").asDouble(), 0.0); - EXPECT_DOUBLE_EQ(payloadValue(payload, "vy").asDouble(), 0.0); - EXPECT_DOUBLE_EQ(payloadValue(payload, "w").asDouble(), 0.0); - EXPECT_FALSE(payloadHas(payload, "duration")); -} - -TEST_F(Src1100ControlAuthorityTest, ControllerErrorCodeIsPreservedInResultMessage) -{ - controller_.setResponseCode(kRobotControlMotion, 41200); - controller_.clearRecords(); - - const auto result = agv_->setVelocity(AgvVelocity{0.1, 0.0, 0.0}); - - EXPECT_FALSE(result.ok()); - EXPECT_EQ(result.code, AgvErrorCode::CommandFailed); - EXPECT_NE(result.message.find("ret_code=41200"), std::string::npos); - EXPECT_NE(result.message.find("err_msg=simulated command failure"), std::string::npos); -} - -TEST_F( - Src1100ControlAuthorityTest, - MapModeCommandsSupersedeTrackedFreeNavigation) -{ - auto result = - agv_->navigateToPose(math::Pose2d{1.0, 2.0, 0.5}); - ASSERT_TRUE(result.ok()) << result.message; - ASSERT_TRUE(Src1100AgvTestPeer::hasTrackedPoseTask(*agv_)); - - result = agv_->switchMap("map-1"); - ASSERT_TRUE(result.ok()) << result.message; - EXPECT_FALSE(Src1100AgvTestPeer::hasTrackedPoseTask(*agv_)); - - result = agv_->navigateToPose(math::Pose2d{1.0, 2.0, 0.5}); - ASSERT_TRUE(result.ok()) << result.message; - ASSERT_TRUE(Src1100AgvTestPeer::hasTrackedPoseTask(*agv_)); - - result = agv_->startMapping(); - ASSERT_TRUE(result.ok()) << result.message; - EXPECT_FALSE(Src1100AgvTestPeer::hasTrackedPoseTask(*agv_)); - - result = agv_->navigateToPose(math::Pose2d{1.0, 2.0, 0.5}); - ASSERT_TRUE(result.ok()) << result.message; - ASSERT_TRUE(Src1100AgvTestPeer::hasTrackedPoseTask(*agv_)); - - result = agv_->stopMapping(); - ASSERT_TRUE(result.ok()) << result.message; - EXPECT_FALSE(Src1100AgvTestPeer::hasTrackedPoseTask(*agv_)); -} - -TEST_F( - Src1100ControlAuthorityTest, - PauseResumeAndStopVelocityPreserveTrackedFreeNavigation) -{ - auto result = - agv_->navigateToPose(math::Pose2d{1.0, 2.0, 0.5}); - ASSERT_TRUE(result.ok()) << result.message; - ASSERT_TRUE(Src1100AgvTestPeer::hasTrackedPoseTask(*agv_)); - - result = agv_->pauseNavigation(); - ASSERT_TRUE(result.ok()) << result.message; - EXPECT_TRUE(Src1100AgvTestPeer::hasTrackedPoseTask(*agv_)); - - result = agv_->resumeNavigation(); - ASSERT_TRUE(result.ok()) << result.message; - EXPECT_TRUE(Src1100AgvTestPeer::hasTrackedPoseTask(*agv_)); - - result = agv_->stopVelocityControl(); - ASSERT_TRUE(result.ok()) << result.message; - EXPECT_TRUE(Src1100AgvTestPeer::hasTrackedPoseTask(*agv_)); - - controller_.clearRecords(); - const auto status = agv_->navigationStatus(); - EXPECT_EQ(status.state, AgvTaskState::Running); - EXPECT_EQ(status.type, AgvTaskType::NavigateToPose); - const auto records = controller_.records(); - ASSERT_EQ(records.size(), 1U); - EXPECT_EQ(records[0].command, kRobotStatusTaskPackage); -} - -TEST_F(Src1100ControlAuthorityTest, NavigationStatusQueriesTrackedPoseTaskPackage) -{ - AgvAdapterParams adapter_params; - adapter_params.values.emplace("task_id", "pose-task-current"); - const auto navigate_result = agv_->navigateToPose( - math::Pose2d{1.0, 2.0, 0.5}, - {}, - adapter_params); - ASSERT_TRUE(navigate_result.ok()) << navigate_result.message; - const auto navigate_records = controller_.records(); - ASSERT_GE(navigate_records.size(), 2U); - const auto navigate_payload = parsePayload(navigate_records[1]); - const std::string generated_task_id = - payloadValue(navigate_payload, "task_id").asString(); - ASSERT_FALSE(generated_task_id.empty()); - - controller_.setResponsePayload( - kRobotStatusTaskPackage, - R"({"ret_code":0,"create_on":"2026-07-31T10:00:01Z","err_msg":"","task_status_package":{"percentage":42.5,"distance":0.7,"info":"operator pause","task_status_list":[{"task_id":"${TASK_ID}","status":3,"type":1}]}})"); - controller_.clearRecords(); - - const auto status = agv_->navigationStatus(); - - EXPECT_EQ(status.state, AgvTaskState::Paused); - EXPECT_EQ(status.type, AgvTaskType::NavigateToPose); - EXPECT_DOUBLE_EQ(status.progress, 42.5); - EXPECT_NE(status.message.find("task_id=" + generated_task_id), std::string::npos); - EXPECT_NE(status.message.find("operator pause"), std::string::npos); - EXPECT_NE(status.message.find("create_on=2026-07-31T10:00:01Z"), std::string::npos); - const auto records = controller_.records(); - ASSERT_EQ(records.size(), 1U); - EXPECT_EQ(records[0].command, kRobotStatusTaskPackage); - const auto payload = parsePayload(records[0]); - const auto& task_ids = payloadValue(payload, "task_ids"); - ASSERT_TRUE(task_ids.isArray()); - ASSERT_EQ(task_ids.size(), 1U); - EXPECT_EQ(task_ids[0].asString(), generated_task_id); -} - -TEST_F( - Src1100ControlAuthorityTest, - NavigationStatusReturnsControllerFaultWhileTaskStillReportsRunning) -{ - const auto navigate_result = - agv_->navigateToPose(math::Pose2d{1.0, 2.0, 0.5}); - ASSERT_TRUE(navigate_result.ok()) << navigate_result.message; - - Json::Value fault_push(Json::objectValue); - Json::Value errors(Json::arrayValue); - errors.append("E_RUNNING_STATUS_52: collision input active"); - *fault_push.demand("errors", "errors" + std::strlen("errors")) = errors; - *fault_push.demand("fatals", "fatals" + std::strlen("fatals")) = - Json::Value(Json::arrayValue); - Src1100AgvTestPeer::cacheRuntimeState(*agv_, fault_push); - controller_.clearRecords(); - - const auto status = agv_->navigationStatus(); - - EXPECT_EQ(status.state, AgvTaskState::Failed); - EXPECT_EQ(status.type, AgvTaskType::NavigateToPose); - EXPECT_NE(status.message.find("controller_task_state=2"), std::string::npos); - EXPECT_NE(status.message.find("E_RUNNING_STATUS_52"), std::string::npos); - EXPECT_TRUE(Src1100AgvTestPeer::hasTrackedPoseTask(*agv_)); - const auto records = controller_.records(); - ASSERT_EQ(records.size(), 1U); - EXPECT_EQ(records[0].command, kRobotStatusTaskPackage); -} - -TEST_F( - Src1100ControlAuthorityTest, - NavigationStatusPreservesFaultWhenTrackedTaskDisappears) -{ - const auto navigate_result = - agv_->navigateToPose(math::Pose2d{1.0, 2.0, 0.5}); - ASSERT_TRUE(navigate_result.ok()) << navigate_result.message; - controller_.setResponsePayload( - kRobotStatusTaskPackage, - R"({"ret_code":0,"task_status_package":{"info":"task vanished","task_status_list":[]}})"); - controller_.clearRecords(); - AgvNavigationStatus status; - - std::thread status_thread([this, &status]() { - status = agv_->navigationStatus(); - }); - - bool status_query_observed = false; - for (int attempt = 0; attempt < 200; ++attempt) { - const auto records = controller_.records(); - status_query_observed = std::any_of( - records.begin(), - records.end(), - [](const CommandRecord& record) { - return record.command == kRobotStatusTaskPackage; - }); - if (status_query_observed) { - break; - } - std::this_thread::sleep_for(std::chrono::milliseconds(1)); - } - EXPECT_TRUE(status_query_observed); - - Json::Value fault_push(Json::objectValue); - Json::Value errors(Json::arrayValue); - errors.append("E_TASK_GONE_54: controller removed failed task"); - *fault_push.demand("errors", "errors" + std::strlen("errors")) = errors; - *fault_push.demand("fatals", "fatals" + std::strlen("fatals")) = - Json::Value(Json::arrayValue); - Src1100AgvTestPeer::cacheRuntimeState(*agv_, fault_push); - status_thread.join(); - - EXPECT_EQ(status.state, AgvTaskState::Failed); - EXPECT_EQ(status.type, AgvTaskType::NavigateToPose); - EXPECT_NE(status.message.find("task disappeared"), std::string::npos); - EXPECT_NE(status.message.find("E_TASK_GONE_54"), std::string::npos); - EXPECT_FALSE(Src1100AgvTestPeer::hasTrackedPoseTask(*agv_)); - const auto records = controller_.records(); - EXPECT_FALSE(std::any_of( - records.begin(), - records.end(), - [](const CommandRecord& record) { - return record.command == kRobotStatusTask; - })); -} - -TEST_F( - Src1100ControlAuthorityTest, - NavigationStatusRejectsFaultArrivingDuringCompletedPoseVerification) -{ - const auto navigate_result = - agv_->navigateToPose(math::Pose2d{1.0, 2.0, 0.5}); - ASSERT_TRUE(navigate_result.ok()) << navigate_result.message; - controller_.setResponsePayload( - kRobotStatusTaskPackage, - R"({"ret_code":0,"task_status_package":{"info":"controller says complete","task_status_list":[{"task_id":"${TASK_ID}","status":4,"type":1}]}})"); - controller_.setResponsePayload( - kRobotStatusLoc, - R"({"ret_code":0,"x":1.0,"y":2.0,"angle":0.5})"); - controller_.setResponseDelay( - kRobotStatusLoc, - std::chrono::milliseconds(300)); - controller_.clearRecords(); - AgvNavigationStatus status; - - std::thread status_thread([this, &status]() { - status = agv_->navigationStatus(); - }); - - bool location_query_observed = false; - for (int attempt = 0; attempt < 700; ++attempt) { - const auto records = controller_.records(); - location_query_observed = std::any_of( - records.begin(), - records.end(), - [](const CommandRecord& record) { - return record.command == kRobotStatusLoc; - }); - if (location_query_observed) { - break; - } - std::this_thread::sleep_for(std::chrono::milliseconds(1)); - } - EXPECT_TRUE(location_query_observed); - - Json::Value fault_push(Json::objectValue); - Json::Value errors(Json::arrayValue); - errors.append("E_STATUS_VERIFY_53: fault during completed pose check"); - *fault_push.demand("errors", "errors" + std::strlen("errors")) = errors; - *fault_push.demand("fatals", "fatals" + std::strlen("fatals")) = - Json::Value(Json::arrayValue); - Src1100AgvTestPeer::cacheRuntimeState(*agv_, fault_push); - status_thread.join(); - - EXPECT_EQ(status.state, AgvTaskState::Failed); - EXPECT_EQ(status.type, AgvTaskType::NavigateToPose); - EXPECT_NE(status.message.find("target verification"), std::string::npos); - EXPECT_NE(status.message.find("E_STATUS_VERIFY_53"), std::string::npos); - EXPECT_FALSE(Src1100AgvTestPeer::hasTrackedPoseTask(*agv_)); -} - -TEST_F( - Src1100ControlAuthorityTest, - NavigationStatusRejectsCompletedPoseWhenTargetWasNotReached) -{ - const auto navigate_result = - agv_->navigateToPose(math::Pose2d{1.0, 2.0, 0.5}); - ASSERT_TRUE(navigate_result.ok()) << navigate_result.message; - controller_.setResponsePayload( - kRobotStatusTaskPackage, - R"({"ret_code":0,"task_status_package":{"info":"controller says complete","task_status_list":[{"task_id":"${TASK_ID}","status":4,"type":1}]}})"); - controller_.setResponsePayload( - kRobotStatusLoc, - R"({"ret_code":0,"x":0.0,"y":0.0,"angle":0.0})"); - controller_.clearRecords(); - - const auto status = agv_->navigationStatus(); - - EXPECT_EQ(status.state, AgvTaskState::Failed); - EXPECT_EQ(status.type, AgvTaskType::NavigateToPose); - EXPECT_NE( - status.message.find("requested target was not reached"), - std::string::npos); - EXPECT_NE(status.message.find("distance_error"), std::string::npos); - EXPECT_FALSE(Src1100AgvTestPeer::hasTrackedPoseTask(*agv_)); - const auto records = controller_.records(); - ASSERT_EQ(records.size(), 2U); - EXPECT_EQ(records[0].command, kRobotStatusTaskPackage); - EXPECT_EQ(records[1].command, kRobotStatusLoc); -} - -TEST_F( - Src1100ControlAuthorityTest, - NavigationStatusWaitsBrieflyForLateControllerFaultDetail) -{ - const auto navigate_result = - agv_->navigateToPose(math::Pose2d{1.0, 2.0, 0.5}); - ASSERT_TRUE(navigate_result.ok()) << navigate_result.message; - controller_.setResponsePayload( - kRobotStatusTaskPackage, - R"({"ret_code":0,"task_status_package":{"info":"planner failed","task_status_list":[{"task_id":"${TASK_ID}","status":5,"type":1}]}})"); - controller_.clearRecords(); - AgvNavigationStatus status; - - std::thread status_thread([this, &status]() { - status = agv_->navigationStatus(); - }); - - bool status_query_observed = false; - for (int attempt = 0; attempt < 200; ++attempt) { - const auto records = controller_.records(); - status_query_observed = std::any_of( - records.begin(), - records.end(), - [](const CommandRecord& record) { - return record.command == kRobotStatusTaskPackage; - }); - if (status_query_observed) { - break; - } - std::this_thread::sleep_for(std::chrono::milliseconds(1)); - } - EXPECT_TRUE(status_query_observed); - - Json::Value fault_push(Json::objectValue); - Json::Value fatals(Json::arrayValue); - fatals.append("E_LATE_77: localization alarm"); - *fault_push.demand("fatals", "fatals" + std::strlen("fatals")) = fatals; - Src1100AgvTestPeer::cacheRuntimeState(*agv_, fault_push); - status_thread.join(); - - EXPECT_EQ(status.state, AgvTaskState::Failed); - EXPECT_EQ(status.type, AgvTaskType::NavigateToPose); - EXPECT_NE(status.message.find("planner failed"), std::string::npos); - EXPECT_NE(status.message.find("E_LATE_77"), std::string::npos); - EXPECT_NE(status.message.find("localization alarm"), std::string::npos); - EXPECT_FALSE(Src1100AgvTestPeer::hasTrackedPoseTask(*agv_)); -} - -TEST_F( - Src1100ControlAuthorityTest, - NavigationStatusDoesNotReturnOldPoseTaskAfterStationSupersedesIt) -{ - const auto navigate_result = - agv_->navigateToPose(math::Pose2d{1.0, 2.0, 0.5}); - ASSERT_TRUE(navigate_result.ok()) << navigate_result.message; - - controller_.setResponsePayload( - kRobotStatusTaskPackage, - R"({"ret_code":0,"task_status_package":{"info":"old pose paused","task_status_list":[{"task_id":"${TASK_ID}","status":3,"type":1}]}})"); - controller_.setResponseDelay( - kRobotStatusTaskPackage, - std::chrono::milliseconds(300)); - controller_.setResponsePayload( - kRobotStatusTask, - R"({"ret_code":0,"err_msg":"","task_status":2,"task_type":2,"move_status_info":"station task running"})"); - controller_.clearRecords(); - AgvNavigationStatus status; - - std::thread status_thread([this, &status]() { - status = agv_->navigationStatus(); - }); - - bool status_query_observed = false; - for (int attempt = 0; attempt < 500; ++attempt) { - const auto records = controller_.records(); - status_query_observed = std::any_of( - records.begin(), - records.end(), - [](const CommandRecord& record) { - return record.command == kRobotStatusTaskPackage; - }); - if (status_query_observed) { - break; - } - std::this_thread::sleep_for(std::chrono::milliseconds(1)); - } - EXPECT_TRUE(status_query_observed); - - const auto station_result = agv_->navigateToStation("station-1"); - status_thread.join(); - - ASSERT_TRUE(station_result.ok()) << station_result.message; - EXPECT_EQ(status.state, AgvTaskState::Running); - EXPECT_EQ(status.type, AgvTaskType::NavigateToStation); - EXPECT_EQ(status.message, "station task running"); - const auto records = controller_.records(); - EXPECT_TRUE(std::any_of( - records.begin(), - records.end(), - [](const CommandRecord& record) { - return record.command == kRobotStatusTask; - })); -} - -TEST_F(Src1100ControlAuthorityTest, DisconnectClearsTrackedPoseTask) -{ - const auto navigate_result = - agv_->navigateToPose(math::Pose2d{1.0, 2.0, 0.5}); - ASSERT_TRUE(navigate_result.ok()) << navigate_result.message; - ASSERT_TRUE(Src1100AgvTestPeer::hasTrackedPoseTask(*agv_)); - - const auto disconnect_result = Src1100AgvTestPeer::disconnect(*agv_); - - ASSERT_TRUE(disconnect_result.ok()) << disconnect_result.message; - EXPECT_FALSE(Src1100AgvTestPeer::hasTrackedPoseTask(*agv_)); -} - -TEST_F(Src1100ControlAuthorityTest, RuntimeStatePreservesCachedControllerFaultDetail) -{ - Json::Value push_payload(Json::objectValue); - Json::Value errors(Json::arrayValue); - Json::Value error(Json::objectValue); - *error.demand("code", "code" + std::strlen("code")) = "E_NAV_42"; - *error.demand("message", "message" + std::strlen("message")) = "planner alarm"; - errors.append(error); - *push_payload.demand("errors", "errors" + std::strlen("errors")) = errors; - Src1100AgvTestPeer::cacheRuntimeState(*agv_, push_payload); - Src1100AgvTestPeer::setAdapterError( - *agv_, - "SRC1100 map file is empty after stripping the transport header"); - controller_.clearRecords(); - - const auto state = agv_->runtimeState(); - - EXPECT_TRUE(state.connected); - EXPECT_TRUE(state.fault); - EXPECT_EQ(state.mode, AgvMode::Fault); - EXPECT_NE(state.last_error.find("E_NAV_42"), std::string::npos); - EXPECT_NE(state.last_error.find("planner alarm"), std::string::npos); - EXPECT_NE(state.last_error.find("adapter_error="), std::string::npos); - EXPECT_NE(state.last_error.find("map file is empty"), std::string::npos); - EXPECT_TRUE(controller_.records().empty()); -} - -TEST_F(Src1100ControlAuthorityTest, NavigationStatusPreservesControllerErrorCode) -{ - controller_.setResponseCode(kRobotStatusTask, 51020); - controller_.clearRecords(); - - const auto status = agv_->navigationStatus(); - - EXPECT_EQ(status.state, AgvTaskState::Failed); - EXPECT_NE(status.message.find("ret_code=51020"), std::string::npos); - EXPECT_NE(status.message.find("err_msg=simulated command failure"), std::string::npos); - const auto records = controller_.records(); - ASSERT_EQ(records.size(), 1U); - EXPECT_EQ(records[0].command, kRobotStatusTask); -} - -} // namespace -} // namespace cmvr::device diff --git a/cmvr-es/service/grpc/src/grpc_agv_service.cpp b/cmvr-es/service/grpc/src/grpc_agv_service.cpp index 1f8d160f..f289a587 100644 --- a/cmvr-es/service/grpc/src/grpc_agv_service.cpp +++ b/cmvr-es/service/grpc/src/grpc_agv_service.cpp @@ -27,7 +27,24 @@ grpc::Status resultToStatus(const device::AgvResult& result) if (result.ok()) { return grpc::Status::OK; } - return grpc::Status(grpc::StatusCode::INTERNAL, result.message); + switch (result.code) { + case device::AgvErrorCode::InvalidArgument: + return grpc::Status( + grpc::StatusCode::INVALID_ARGUMENT, + result.message); + case device::AgvErrorCode::TaskCanceled: + return grpc::Status( + grpc::StatusCode::CANCELLED, + result.message); + case device::AgvErrorCode::Timeout: + return grpc::Status( + grpc::StatusCode::DEADLINE_EXCEEDED, + result.message); + default: + return grpc::Status( + grpc::StatusCode::INTERNAL, + result.message); + } } template @@ -58,6 +75,15 @@ grpc::Status setDeviceNotFound(api::CommandHeader_Feedback* response, const std: return grpc::Status(grpc::StatusCode::NOT_FOUND, message); } +template +grpc::Status setNavigationRequestCanceled(Response* response) +{ + constexpr char message[] = + "AGV navigation request was canceled before command dispatch"; + fillFeedback(response->mutable_header(), false, message); + return grpc::Status(grpc::StatusCode::CANCELLED, message); +} + device::AgvAdapterParams toAdapterParams(const msgs::AgvAdapterParams& src) { device::AgvAdapterParams dst; @@ -67,7 +93,9 @@ device::AgvAdapterParams toAdapterParams(const msgs::AgvAdapterParams& src) return dst; } -device::AgvMotionOptions toMotionOptions(const msgs::AgvMotionOptions& src) +device::AgvMotionOptions toMotionOptions( + const msgs::AgvMotionOptions& src, + grpc::ServerContext* context = nullptr) { device::AgvMotionOptions dst; dst.max_speed = src.max_speed(); @@ -78,6 +106,13 @@ device::AgvMotionOptions toMotionOptions(const msgs::AgvMotionOptions& src) dst.reach_angle = src.reach_angle(); dst.speed_ratio = src.speed_ratio() > 0.0 ? src.speed_ratio() : 1.0; dst.asynchronous = src.asynchronous(); + dst.wait_timeout_ms = src.wait_timeout_ms(); + dst.poll_interval_ms = src.poll_interval_ms(); + if (context) { + dst.cancellation_requested = [context]() { + return context->IsCancelled(); + }; + } return dst; } @@ -391,11 +426,14 @@ grpc::Status gRPCAgvServiceImpl::clearFault(grpc::ServerContext*, } } -grpc::Status gRPCAgvServiceImpl::navigateToPose(grpc::ServerContext*, +grpc::Status gRPCAgvServiceImpl::navigateToPose(grpc::ServerContext* context, const api::AgvNavigateToPoseCommand_Request* request, api::AgvNavigateToPoseCommand_Feedback* response) { try { + if (context && context->IsCancelled()) { + return setNavigationRequestCanceled(response); + } const std::string device_id = request->header().device_id(); auto agv = dmgr_.getDevice(device_id); if (!agv) { @@ -403,7 +441,7 @@ grpc::Status gRPCAgvServiceImpl::navigateToPose(grpc::ServerContext*, } return setResponseResult(response, agv->navigateToPose( toPose2d(request->pose()), - toMotionOptions(request->options()), + toMotionOptions(request->options(), context), toAdapterParams(request->adapter_params()))); } catch (const std::exception& e) { fillFeedback(response->mutable_header(), false, e.what()); @@ -411,11 +449,14 @@ grpc::Status gRPCAgvServiceImpl::navigateToPose(grpc::ServerContext*, } } -grpc::Status gRPCAgvServiceImpl::navigateToStation(grpc::ServerContext*, +grpc::Status gRPCAgvServiceImpl::navigateToStation(grpc::ServerContext* context, const api::AgvNavigateToStationCommand_Request* request, api::AgvNavigateToStationCommand_Feedback* response) { try { + if (context && context->IsCancelled()) { + return setNavigationRequestCanceled(response); + } const std::string device_id = request->header().device_id(); auto agv = dmgr_.getDevice(device_id); if (!agv) { @@ -423,7 +464,7 @@ grpc::Status gRPCAgvServiceImpl::navigateToStation(grpc::ServerContext*, } return setResponseResult(response, agv->navigateToStation( request->station_id(), - toMotionOptions(request->options()), + toMotionOptions(request->options(), context), toAdapterParams(request->adapter_params()))); } catch (const std::exception& e) { fillFeedback(response->mutable_header(), false, e.what()); @@ -431,11 +472,14 @@ grpc::Status gRPCAgvServiceImpl::navigateToStation(grpc::ServerContext*, } } -grpc::Status gRPCAgvServiceImpl::followPath(grpc::ServerContext*, +grpc::Status gRPCAgvServiceImpl::followPath(grpc::ServerContext* context, const api::AgvFollowPathCommand_Request* request, api::AgvFollowPathCommand_Feedback* response) { try { + if (context && context->IsCancelled()) { + return setNavigationRequestCanceled(response); + } const std::string device_id = request->header().device_id(); auto agv = dmgr_.getDevice(device_id); if (!agv) { @@ -446,7 +490,11 @@ grpc::Status gRPCAgvServiceImpl::followPath(grpc::ServerContext*, for (const auto& segment : request->path()) { path.push_back(toPathSegment(segment)); } - return setResponseResult(response, agv->followPath(path)); + return setResponseResult( + response, + agv->followPath( + path, + toMotionOptions(request->options(), context))); } catch (const std::exception& e) { fillFeedback(response->mutable_header(), false, e.what()); return grpc::Status(grpc::StatusCode::INTERNAL, e.what()); diff --git a/cmvr-es/service/grpc/tests/grpc_agv_service_test.cpp b/cmvr-es/service/grpc/tests/grpc_agv_service_test.cpp index da988fdd..ff144764 100644 --- a/cmvr-es/service/grpc/tests/grpc_agv_service_test.cpp +++ b/cmvr-es/service/grpc/tests/grpc_agv_service_test.cpp @@ -2,6 +2,7 @@ #include #include +#include #include #include @@ -13,9 +14,9 @@ namespace cmvr::service { namespace { constexpr char kNativeErrorMessage[] = - "SRC1100 command failed: ret_code=41200, err_msg=speed_illegal"; + "SEER Robokit command failed: ret_code=41200, err_msg=speed_illegal"; constexpr char kNativeNavigationErrorMessage[] = - "SRC1100 command failed: ret_code=43051, err_msg=planner_rejected_pose"; + "SEER Robokit command failed: ret_code=43051, err_msg=planner_rejected_pose"; class FakeAgv final : public device::AbstractAGV { public: @@ -33,16 +34,41 @@ public: { pose_ = pose; pose_options_ = options; + pose_cancellation_bound_ = + static_cast(options.cancellation_requested); + pose_cancellation_requested_during_call_ = + pose_cancellation_bound_ && options.cancellation_requested(); + pose_options_.cancellation_requested = {}; return pose_result_; } device::AgvResult navigateToStation( const std::string& station_id, const device::AgvMotionOptions& options, - const device::AgvAdapterParams&) override + const device::AgvAdapterParams& adapter_params) override { station_id_ = station_id; station_options_ = options; + station_adapter_params_ = adapter_params; + station_cancellation_bound_ = + static_cast(options.cancellation_requested); + station_cancellation_requested_during_call_ = + station_cancellation_bound_ && options.cancellation_requested(); + station_options_.cancellation_requested = {}; + return device::AgvResult::success(); + } + + device::AgvResult followPath( + const std::vector& path, + const device::AgvMotionOptions& options) override + { + path_ = path; + path_options_ = options; + path_cancellation_bound_ = + static_cast(options.cancellation_requested); + path_cancellation_requested_during_call_ = + path_cancellation_bound_ && options.cancellation_requested(); + path_options_.cancellation_requested = {}; return device::AgvResult::success(); } @@ -58,6 +84,29 @@ public: device::AgvResult pose_result_{device::AgvResult::success()}; std::string station_id_; device::AgvMotionOptions station_options_; + device::AgvAdapterParams station_adapter_params_; + std::vector path_; + device::AgvMotionOptions path_options_; + bool pose_cancellation_bound_{false}; + bool pose_cancellation_requested_during_call_{false}; + bool station_cancellation_bound_{false}; + bool station_cancellation_requested_during_call_{false}; + bool path_cancellation_bound_{false}; + bool path_cancellation_requested_during_call_{false}; +}; + +class LegacyFollowPathAgv final : public device::AbstractAGV { +public: + std::string typeName() const override { return "LegacyFollowPathAgv"; } + + device::AgvResult followPath( + const std::vector& path) override + { + path_ = path; + return device::AgvResult::success(); + } + + std::vector path_; }; class GrpcAgvServiceTest : public ::testing::Test { @@ -90,6 +139,8 @@ void setMotionOptions(msgs::AgvMotionOptions* options) options->set_max_angular_acceleration(0.7); options->set_reach_distance(0.08); options->set_reach_angle(0.09); + options->set_wait_timeout_ms(1234); + options->set_poll_interval_ms(55); } void expectMotionOptions(const device::AgvMotionOptions& options) @@ -100,6 +151,9 @@ void expectMotionOptions(const device::AgvMotionOptions& options) EXPECT_DOUBLE_EQ(options.max_angular_acceleration, 0.7); EXPECT_DOUBLE_EQ(options.reach_distance, 0.08); EXPECT_DOUBLE_EQ(options.reach_angle, 0.09); + EXPECT_EQ(options.wait_timeout_ms, 1234); + EXPECT_EQ(options.poll_interval_ms, 55); + EXPECT_FALSE(options.asynchronous); } TEST_F(GrpcAgvServiceTest, NavigationRpcsForwardSpeedAndAccelerationOptions) @@ -124,6 +178,8 @@ TEST_F(GrpcAgvServiceTest, NavigationRpcsForwardSpeedAndAccelerationOptions) EXPECT_DOUBLE_EQ(agv_->pose_.y, 2.0); EXPECT_DOUBLE_EQ(agv_->pose_.theta, 0.5); expectMotionOptions(agv_->pose_options_); + EXPECT_TRUE(agv_->pose_cancellation_bound_); + EXPECT_FALSE(agv_->pose_cancellation_requested_during_call_); api::AgvNavigateToStationCommand_Request station_request; station_request.mutable_header()->set_device_id("test-agv"); @@ -141,6 +197,31 @@ TEST_F(GrpcAgvServiceTest, NavigationRpcsForwardSpeedAndAccelerationOptions) EXPECT_TRUE(station_response.header().success()); EXPECT_EQ(agv_->station_id_, "station-1"); expectMotionOptions(agv_->station_options_); + EXPECT_TRUE(agv_->station_cancellation_bound_); + EXPECT_FALSE(agv_->station_cancellation_requested_during_call_); + + api::AgvFollowPathCommand_Request path_request; + path_request.mutable_header()->set_device_id("test-agv"); + auto* segment = path_request.add_path(); + segment->set_source_station("station-1"); + segment->set_target_station("station-2"); + setMotionOptions(path_request.mutable_options()); + api::AgvFollowPathCommand_Feedback path_response; + grpc::ServerContext path_context; + + const auto path_status = service_->followPath( + &path_context, + &path_request, + &path_response); + + ASSERT_TRUE(path_status.ok()) << path_status.error_message(); + EXPECT_TRUE(path_response.header().success()); + ASSERT_EQ(agv_->path_.size(), 1U); + EXPECT_EQ(agv_->path_[0].source_station, "station-1"); + EXPECT_EQ(agv_->path_[0].target_station, "station-2"); + expectMotionOptions(agv_->path_options_); + EXPECT_TRUE(agv_->path_cancellation_bound_); + EXPECT_FALSE(agv_->path_cancellation_requested_during_call_); } TEST_F(GrpcAgvServiceTest, NativeControllerCodeIsReturnedInGrpcMessage) @@ -185,6 +266,122 @@ TEST_F(GrpcAgvServiceTest, NativeNavigationCodeIsReturnedInGrpcMessage) EXPECT_EQ( response.header().error_message(), kNativeNavigationErrorMessage); + EXPECT_FALSE(agv_->pose_options_.asynchronous); + EXPECT_EQ(agv_->pose_options_.wait_timeout_ms, 0); + EXPECT_EQ(agv_->pose_options_.poll_interval_ms, 0); + EXPECT_TRUE(agv_->pose_cancellation_bound_); + EXPECT_FALSE(agv_->pose_cancellation_requested_during_call_); +} + +TEST_F(GrpcAgvServiceTest, ExplicitAsynchronousNavigationIsForwarded) +{ + api::AgvNavigateToStationCommand_Request request; + request.mutable_header()->set_device_id("test-agv"); + request.set_station_id("station-async"); + request.mutable_options()->set_asynchronous(true); + api::AgvNavigateToStationCommand_Feedback response; + grpc::ServerContext context; + + const auto status = service_->navigateToStation( + &context, + &request, + &response); + + ASSERT_TRUE(status.ok()) << status.error_message(); + EXPECT_TRUE(agv_->station_options_.asynchronous); + EXPECT_TRUE(agv_->station_cancellation_bound_); + EXPECT_FALSE(agv_->station_cancellation_requested_during_call_); +} + +TEST_F(GrpcAgvServiceTest, NavigateToStationForwardsPgvAdapterParams) +{ + api::AgvNavigateToStationCommand_Request request; + request.mutable_header()->set_device_id("test-agv"); + request.set_station_id("AP1"); + auto* values = request.mutable_adapter_params()->mutable_values(); + (*values)["use_pgv"] = "true"; + (*values)["pgv_adjust_dist"] = "0.3"; + (*values)["pgv_adjust_cx"] = "-0.3"; + (*values)["pgv_adjust_cy"] = "0"; + api::AgvNavigateToStationCommand_Feedback response; + grpc::ServerContext context; + + const auto status = service_->navigateToStation( + &context, + &request, + &response); + + ASSERT_TRUE(status.ok()) << status.error_message(); + EXPECT_TRUE(response.header().success()); + EXPECT_EQ(agv_->station_id_, "AP1"); + EXPECT_EQ( + agv_->station_adapter_params_.getString("use_pgv").value_or(""), + "true"); + EXPECT_EQ( + agv_->station_adapter_params_.getString("pgv_adjust_dist").value_or(""), + "0.3"); + EXPECT_EQ( + agv_->station_adapter_params_.getString("pgv_adjust_cx").value_or(""), + "-0.3"); + EXPECT_EQ( + agv_->station_adapter_params_.getString("pgv_adjust_cy").value_or(""), + "0"); +} + +TEST_F(GrpcAgvServiceTest, NavigationErrorsMapToGrpcCodesAndPreserveDetails) +{ + struct ErrorCase { + device::AgvErrorCode device_code; + grpc::StatusCode grpc_code; + }; + const ErrorCase cases[] = { + {device::AgvErrorCode::InvalidArgument, + grpc::StatusCode::INVALID_ARGUMENT}, + {device::AgvErrorCode::TaskCanceled, + grpc::StatusCode::CANCELLED}, + {device::AgvErrorCode::Timeout, + grpc::StatusCode::DEADLINE_EXCEEDED}, + }; + + for (const auto& test_case : cases) { + const std::string detail = + "SEER Robokit navigation detail for code=" + + std::to_string(static_cast(test_case.device_code)); + agv_->pose_result_ = device::AgvResult::failure( + test_case.device_code, + detail); + api::AgvNavigateToPoseCommand_Request request; + request.mutable_header()->set_device_id("test-agv"); + api::AgvNavigateToPoseCommand_Feedback response; + grpc::ServerContext context; + + const auto status = service_->navigateToPose( + &context, + &request, + &response); + + EXPECT_EQ(status.error_code(), test_case.grpc_code); + EXPECT_EQ(status.error_message(), detail); + EXPECT_FALSE(response.header().success()); + EXPECT_EQ(response.header().error_message(), detail); + } +} + +TEST(AbstractAgvCompatibilityTest, FollowPathOptionsDelegateToLegacyOverride) +{ + LegacyFollowPathAgv legacy; + device::AbstractAGV* abstract = &legacy; + const std::vector path = { + {"station-1", "station-2"}, + }; + device::AgvMotionOptions options; + + const auto result = abstract->followPath(path, options); + + ASSERT_TRUE(result.ok()) << result.message; + ASSERT_EQ(legacy.path_.size(), 1U); + EXPECT_EQ(legacy.path_[0].source_station, "station-1"); + EXPECT_EQ(legacy.path_[0].target_station, "station-2"); } } // namespace diff --git a/cmvr-es/service/quic_edge/tests/quic_edge_protocol_test.cpp b/cmvr-es/service/quic_edge/tests/quic_edge_protocol_test.cpp index a6d9b6a8..845191d8 100644 --- a/cmvr-es/service/quic_edge/tests/quic_edge_protocol_test.cpp +++ b/cmvr-es/service/quic_edge/tests/quic_edge_protocol_test.cpp @@ -630,7 +630,7 @@ bool testDeviceManagerSnapshotInHeartbeat() device::ManagedDeviceSnapshot running; running.id = "src1100"; running.kind = device::DeviceKind::AGV; - running.type_name = "Src1100Agv"; + running.type_name = "SeerRobokitAgv"; running.enabled = true; running.state = device::ManagedDeviceState::Running; running.health.state = device::DeviceHealthState::Healthy; diff --git a/protos/cmvr/api/agv_command.proto b/protos/cmvr/api/agv_command.proto index 267c88f3..da4414ab 100644 --- a/protos/cmvr/api/agv_command.proto +++ b/protos/cmvr/api/agv_command.proto @@ -45,7 +45,7 @@ message AgvNavigateToPoseCommand { CommandHeader.Request header = 1; // 目标位姿。x/y 单位：米，theta 单位：弧度。 cmvr.msgs.AgvPose2d pose = 2; - // 通用运动约束和执行选项。 + // 通用运动约束和执行选项；默认同步阻塞至任务终态并确认停车。 cmvr.msgs.AgvMotionOptions options = 3; // AGV 适配器扩展参数，用于传递厂商特有选项。 cmvr.msgs.AgvAdapterParams adapter_params = 4; @@ -65,7 +65,7 @@ message AgvNavigateToStationCommand { CommandHeader.Request header = 1; // 目标站点 id。 string station_id = 2; - // 通用运动约束和执行选项。 + // 通用运动约束和执行选项；默认同步阻塞至任务终态并确认停车。 cmvr.msgs.AgvMotionOptions options = 3; // AGV 适配器扩展参数，用于传递厂商特有选项。 cmvr.msgs.AgvAdapterParams adapter_params = 4; @@ -85,6 +85,8 @@ message AgvFollowPathCommand { CommandHeader.Request header = 1; // 路径段列表。每段包含起点站点 id 和终点站点 id。 repeated cmvr.msgs.AgvPathSegment path = 2; + // 通用执行选项；默认同步阻塞至整条路径终态并确认停车。 + cmvr.msgs.AgvMotionOptions options = 3; } // 反馈体。 message Feedback { diff --git a/protos/cmvr/config/agv_config/agv_config.proto b/protos/cmvr/config/agv_config/agv_config.proto index 10dae43b..4e579b7f 100644 --- a/protos/cmvr/config/agv_config/agv_config.proto +++ b/protos/cmvr/config/agv_config/agv_config.proto @@ -11,11 +11,11 @@ message MyAgvConfig { int32 port = 3; } -// 仙工 SRC1100 AGV 后端配置。 -message Src1100AgvConfig { +// 仙工 SEER Robokit AGV 后端配置。 +message SeerRobokitAgvConfig { // 设备 id。为空时通常由外层 AGVDeviceConfig.id 补齐。 string id = 1; - // SRC1100 控制器 IP 地址。 + // SEER Robokit 控制器 IP 地址。 string ip = 2; // 是否启用该后端配置。当前设备是否创建仍以设备管理器配置为准。 bool enable = 3; @@ -49,7 +49,7 @@ message Src1100AgvConfig { int32 map_update_interval_ms = 17; // 统一地图更新缓存条数。0 表示使用适配器默认值；缓存满后会丢弃最旧更新。 uint32 map_update_history_size = 18; - // 抢占 SRC1100 控制权时上报的稳定昵称。为空时适配器使用 "cmvr-es:"。 + // 抢占 SEER Robokit 控制权时上报的稳定昵称。为空时适配器使用 "cmvr-es:"。 string control_nick_name = 19; } @@ -62,8 +62,8 @@ message AGVDeviceConfig { oneof backend { // 示例/测试 AGV 后端。 MyAgvConfig my_agv = 10; - // 仙工 SRC1100 AGV 后端。 - Src1100AgvConfig src1100_agv = 11; + // 仙工 SEER Robokit AGV 后端。 + SeerRobokitAgvConfig seer_robokit_agv = 11; } } diff --git a/protos/cmvr/msgs/agv.proto b/protos/cmvr/msgs/agv.proto index 54439eeb..ac1583c9 100644 --- a/protos/cmvr/msgs/agv.proto +++ b/protos/cmvr/msgs/agv.proto @@ -52,8 +52,14 @@ message AgvMotionOptions { double reach_angle = 6; // 速度比例，范围通常为 [0, 1]；1 表示不降速。 double speed_ratio = 7; - // 是否异步执行；true 表示下发任务后立即返回。 + // 是否异步执行；false（默认）表示到达、失败、取消或遇障停止后才返回， + // true 表示任务被控制器接受后立即返回。 bool asynchronous = 8; + // 同步导航的最大等待时间，单位：毫秒；0 表示使用适配器默认值。 + // gRPC deadline 应大于该值或预计行程时间，否则服务端会安全取消导航。 + int32 wait_timeout_ms = 9; + // 同步导航的状态轮询周期，单位：毫秒；0 表示使用适配器默认值。 + int32 poll_interval_ms = 10; } // AGV 适配器扩展参数。用于传递厂商或控制器特有的参数。 diff --git a/protos/rbk/protocol/src1100_map3d.proto b/protos/rbk/protocol/seer_robokit_map3d.proto similarity index 97% rename from protos/rbk/protocol/src1100_map3d.proto rename to protos/rbk/protocol/seer_robokit_map3d.proto index b33474f1..4936722f 100644 --- a/protos/rbk/protocol/src1100_map3d.proto +++ b/protos/rbk/protocol/seer_robokit_map3d.proto @@ -2,7 +2,7 @@ syntax = "proto3"; package rbk.protocol; -// 仙工 SRC1100 3D 地图文件 0.3dsmap 的最小解析结构。 +// 仙工 SEER Robokit 3D 地图文件 0.3dsmap 的最小解析结构。 // 这里只保留转换统一地图所需字段，未声明字段由 protobuf 作为未知字段跳过。 // 地图坐标系下的三维位置，单位：米。