From 28f1dd1bf8560f76f6f1eee0a4ab4a501ac15c34 Mon Sep 17 00:00:00 2001 From: xtkuang <87661715@qq.com> Date: Thu, 30 Jul 2026 15:09:07 +0800 Subject: [PATCH 01/20] feat: add gRPC motor control over Modbus TCP Add synchronous and streaming MotorService APIs backed by the PLC Modbus TCP runtime and protocol driver. Extend AUBO JSON commands and isolate vendor libstdc++ paths while keeping build-tree tests runnable. --- CMakeLists.txt | 28 +- README.md | 117 + cmake/FindExternalLib.cmake | 13 +- .../config/devices/motor/plc_motors.pb.txt | 56 + cmvr-es/config/manager/device_manager.pb.txt | 8 + cmvr-es/devices/arm/aubo_arm/CMakeLists.txt | 47 + cmvr-es/devices/arm/aubo_arm/aubo_arm.cpp | 238 ++ cmvr-es/devices/arm/aubo_arm/aubo_arm.h | 2 + .../tests/aubo_arm_json_command_test.cpp | 84 + cmvr-es/devices/motor/CMakeLists.txt | 48 + .../devices/motor/bus_runtime/CMakeLists.txt | 12 +- .../include/cmvr_plc_register_map.h | 209 ++ .../modbus_tcp/include/modbus_tcp_client.h | 47 + .../include/modbus_tcp_motor_bus_runtime.h | 173 ++ .../modbus_tcp/src/modbus_tcp_client.cpp | 188 ++ .../src/modbus_tcp_motor_bus_runtime.cpp | 1013 ++++++++ .../modbus_tcp_motor_bus_runtime_test.cpp | 1586 +++++++++++++ .../drivers/modbus_plc_motor/CMakeLists.txt | 21 + .../include/cmvr_plc_motor_protocol.h | 89 + .../include/modbus_plc_motor.h | 21 + .../src/cmvr_plc_motor_protocol.cpp | 582 +++++ .../modbus_plc_motor/src/modbus_plc_motor.cpp | 55 + cmvr-es/devices/motor/manager/CMakeLists.txt | 1 + .../motor/manager/include/motor_manager.h | 4 + .../motor/manager/src/motor_manager.cpp | 61 + cmvr-es/service/CMakeLists.txt | 71 + .../service/grpc/include/grpc_motor_service.h | 197 ++ .../service/grpc/src/grpc_motor_service.cpp | 2051 +++++++++++++++++ .../grpc_motor_service_modbus_e2e_test.cpp | 907 ++++++++ .../grpc/tests/grpc_motor_service_test.cpp | 1586 +++++++++++++ .../include/grpc_server_task.h | 1 + .../grpc_server_task/src/grpc_server_task.cpp | 4 + docs/motor_service_modbus_tcp.md | 1022 ++++++++ protos/cmvr/api/motor_command.proto | 151 ++ protos/cmvr/api/motor_service.proto | 21 + .../config/motor_config/motor_config.proto | 27 + 36 files changed, 10736 insertions(+), 5 deletions(-) create mode 100644 cmvr-es/config/devices/motor/plc_motors.pb.txt create mode 100644 cmvr-es/devices/arm/aubo_arm/tests/aubo_arm_json_command_test.cpp create mode 100644 cmvr-es/devices/motor/bus_runtime/modbus_tcp/include/cmvr_plc_register_map.h create mode 100644 cmvr-es/devices/motor/bus_runtime/modbus_tcp/include/modbus_tcp_client.h create mode 100644 cmvr-es/devices/motor/bus_runtime/modbus_tcp/include/modbus_tcp_motor_bus_runtime.h create mode 100644 cmvr-es/devices/motor/bus_runtime/modbus_tcp/src/modbus_tcp_client.cpp create mode 100644 cmvr-es/devices/motor/bus_runtime/modbus_tcp/src/modbus_tcp_motor_bus_runtime.cpp create mode 100644 cmvr-es/devices/motor/bus_runtime/modbus_tcp/tests/modbus_tcp_motor_bus_runtime_test.cpp create mode 100644 cmvr-es/devices/motor/drivers/modbus_plc_motor/CMakeLists.txt create mode 100644 cmvr-es/devices/motor/drivers/modbus_plc_motor/include/cmvr_plc_motor_protocol.h create mode 100644 cmvr-es/devices/motor/drivers/modbus_plc_motor/include/modbus_plc_motor.h create mode 100644 cmvr-es/devices/motor/drivers/modbus_plc_motor/src/cmvr_plc_motor_protocol.cpp create mode 100644 cmvr-es/devices/motor/drivers/modbus_plc_motor/src/modbus_plc_motor.cpp create mode 100644 cmvr-es/service/grpc/include/grpc_motor_service.h create mode 100644 cmvr-es/service/grpc/src/grpc_motor_service.cpp create mode 100644 cmvr-es/service/grpc/tests/grpc_motor_service_modbus_e2e_test.cpp create mode 100644 cmvr-es/service/grpc/tests/grpc_motor_service_test.cpp create mode 100644 docs/motor_service_modbus_tcp.md create mode 100644 protos/cmvr/api/motor_command.proto create mode 100644 protos/cmvr/api/motor_service.proto diff --git a/CMakeLists.txt b/CMakeLists.txt index c75b3ed7..bb267040 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -9,7 +9,29 @@ set(CMAKE_CXX_STANDARD_REQUIRED ON) #set(CMAKE_CXX_STANDARD_REQUIRED True) set(CMAKE_POSITION_INDEPENDENT_CODE ON) +# Preserve the project's production-build behavior: tests are opt-in via +# -DBUILD_TESTING=ON, while still registering them with CTest when requested. +option(BUILD_TESTING "Build the test targets" OFF) +include(CTest) +if(BUILD_TESTING AND UNIX AND NOT APPLE) + # Test executables can still inherit the AUBO imported target's build-tree + # RUNPATH. Keep the active toolchain runtime ahead of that vendor path. + execute_process( + COMMAND ${CMAKE_CXX_COMPILER} -print-file-name=libstdc++.so.6 + OUTPUT_VARIABLE CMVR_TEST_SYSTEM_LIBSTDCXX + OUTPUT_STRIP_TRAILING_WHITESPACE + ) + if(EXISTS "${CMVR_TEST_SYSTEM_LIBSTDCXX}") + get_filename_component( + CMVR_TEST_SYSTEM_LIBSTDCXX + "${CMVR_TEST_SYSTEM_LIBSTDCXX}" + REALPATH + ) + else() + unset(CMVR_TEST_SYSTEM_LIBSTDCXX) + endif() +endif() # Install to /output set(CMAKE_INSTALL_PREFIX "${CMAKE_SOURCE_DIR}/output" CACHE PATH "" FORCE) @@ -18,9 +40,6 @@ set(CMAKE_INSTALL_PREFIX "${CMAKE_SOURCE_DIR}/output" CACHE PATH "" FORCE) set(CMAKE_BUILD_RPATH "\$ORIGIN:\$ORIGIN/../lib") set(CMAKE_INSTALL_RPATH "\$ORIGIN:\$ORIGIN/../lib") -# Use transitive RPATH so CLion can run build-tree test executables without -# manually setting LD_LIBRARY_PATH for indirect third-party dependencies. -add_link_options(-Wl,--disable-new-dtags) set(CMAKE_BUILD_WITH_INSTALL_RPATH OFF) set(CMAKE_INSTALL_RPATH_USE_LINK_PATH TRUE) @@ -31,6 +50,9 @@ list(APPEND CMAKE_MODULE_PATH "${CMAKE_SOURCE_DIR}/cmake") include(FindExternalLib) set(ARCH "x86") setup_external_libs(${ARCH}) +if(BUILD_TESTING AND CMVR_EXTERNAL_LIBRARY_DIRS) + list(JOIN CMVR_EXTERNAL_LIBRARY_DIRS ":" CMVR_TEST_EXTERNAL_LIBRARY_PATH) +endif() # 在调用 setup_external_libs 之后 message(STATUS "CMAKE_EXE_LINKER_FLAGS: ${CMAKE_EXE_LINKER_FLAGS}") message(STATUS "CMAKE_SHARED_LINKER_FLAGS: ${CMAKE_SHARED_LINKER_FLAGS}") diff --git a/README.md b/README.md index 1bcb8565..5d0468e2 100644 --- a/README.md +++ b/README.md @@ -138,3 +138,120 @@ $IGH_ETHERCAT_ROOT/bin/ethercat pdos sudo script/ethercat/stop_ethercat.sh eno1 sudo script/ethercat/stop_ethercat.sh eno1 --restore-network ``` + +## MotorService 与 Modbus TCP PLC + +工程包含从 gRPC `MotorService`、`MotorManager`、`AbstractMotor` 到 +`ModbusTcpMotorBusRuntime` 的 CMVR PLC v1 电机控制链，x86-64 的 libmodbus +3.1.11 已放在 `dependency/x86/third_party/modbus/3.1.11`。 + +PLC 对接时特别注意： + +- `host` 必须配置为 IPv4 字面量，PLC boot ID 必须非零且每次重启变化； +- owner 决策、命令 ACK 都必须回显对应 session，重连不得执行旧 mailbox； +- PLC 在进程启动时可以离线；连接 supervisor 会继续退避重试，离线期间状态 + 返回不可用且运动命令不会写 mailbox； +- 状态区按 odd/even seqlock 发布，上位机使用 + sequence-before → 64-word block → sequence-after 三段读取验证； +- 每次 `OpenCyclicPosition/Velocity` 创建新 stream epoch，PLC 必须原子清零 + `last_applied_cyclic_sequence`、旧样本去重状态和 cyclic watchdog，确认 + `StreamActive` 与正确 mode 后才 ACK；重开后的首样本序列从 `1` 开始并必须 + 重新应用； +- 活动 cyclic 流跨 `connection_epoch` 后不会自动重开；旧流的当前和后续 + setpoint 均被拒绝并在 Quick Stop 后终止，客户端必须新建 gRPC 流。断链前 + 或断链期间 pending 的 setpoint 不会应用到新 session； +- 任何清理 Quick Stop 未确认时，MotorService 都会 fail-closed 锁存，并在 + 成功执行 `setEnabled(true)` 前拒绝新的运动命令； +- Modbus Quick Stop 只是功能性停止，不能替代硬接线急停或驱动器 STO。 +- 当前 Modbus 后端只提供 x86-64 的 libmodbus 3.1.11；`dependency/arm` + 尚无对应库，因此 ARM 构建暂不支持该后端。 + +完整 gRPC 语义、配置样例、寄存器表、TIA Portal 要求、构建测试和安全边界见 +[`docs/motor_service_modbus_tcp.md`](docs/motor_service_modbus_tcp.md)。 + +## AUBO 控制柜 IO + +`AuboArm` 通过通用的 `executeJsonCommand` 接口提供控制柜 Standard 数字 IO +读写。第一版支持以下命令： + +| `operation` | 说明 | 必填字段 | +| --- | --- | --- | +| `get_di` | 读取控制柜数字输入 | `index` | +| `get_do` | 读取控制柜数字输出及其 runstate | `index` | +| `set_do` | 设置控制柜数字输出 | `index`、`value` | + +JSON 命令示例： + +```json +{"command":"cabinet_io","operation":"get_di","index":0} +{"command":"cabinet_io","operation":"get_do","index":0} +{"command":"cabinet_io","operation":"set_do","index":0,"value":true} +``` + +其中 `index` 从 `0` 开始，运行时会根据控制器返回的 IO 数量检查范围。 +`set_do.value` 必须是 JSON 布尔值 `true` 或 `false`，不接受 `0/1` 或字符串。 +`set_do` 成功响应中的 `requested_value` 表示 SDK 已接受的请求值；需要确认控制器 +当前输出状态时，再调用一次 `get_do` 读取实际值。 + +成功响应示例： + +```json +{ + "success": true, + "command": "cabinet_io", + "operation": "get_di", + "index": 0, + "count": 16, + "value": false +} +``` + +### 通过 gRPC 调用 + +该功能复用 `cmvr.api.SystemService/ExecuteJsonCommand`。默认配置中的 gRPC +端口是 `50052`，读取 DI0： + +```shell +grpcurl -plaintext \ + -d '{ + "header":{"deviceId":"aubo_arm"}, + "requestJson":"{\"command\":\"cabinet_io\",\"operation\":\"get_di\",\"index\":0}" + }' \ + 127.0.0.1:50052 \ + cmvr.api.SystemService/ExecuteJsonCommand +``` + +设置 DO0 为高电平： + +```shell +grpcurl -plaintext \ + -d '{ + "header":{"deviceId":"aubo_arm"}, + "requestJson":"{\"command\":\"cabinet_io\",\"operation\":\"set_do\",\"index\":0,\"value\":true}" + }' \ + 127.0.0.1:50052 \ + cmvr.api.SystemService/ExecuteJsonCommand +``` + +使用源码树默认配置时，先在 `cmvr-es/config/manager/device_manager.pb.txt` 中把 +`aubo_arm` 的 `enable` 改为 `true`，并在 +`cmvr-es/config/devices/arm/aubo_arm.pb.txt` 中配置正确的控制器地址和登录信息， +然后重新安装配置并启动安装产物： + +```shell +cmake --install build +./output/bin/cmvr_es +``` + +`output/bin/cmvr_es` 读取的是 `output/bin/config/`；如果进程使用显式配置路径， +应修改该配置根下的对应文件。 +设备未启用或初始化失败时，gRPC 会返回 `Device not found: aubo_arm`。 + +### 安全约束 + +- 该接口只访问控制柜 Standard 数字 IO，不操作工具端 IO、可配置 IO 或安全 IO。 +- `set_do` 不会修改控制器的输出 runstate。只有目标通道的 runstate 为 + `StandardOutputRunState::None` 时才允许写入，否则返回 + `output_managed_by_runstate`。 +- 接口不会调用会重置全部输出配置的 `setDigitalOutputRunstateDefault()`。 +- 模拟量 IO 涉及 domain、单位和量程，第一版暂不通过该 JSON 接口开放。 diff --git a/cmake/FindExternalLib.cmake b/cmake/FindExternalLib.cmake index 31ee4f6c..3e2a0b2c 100644 --- a/cmake/FindExternalLib.cmake +++ b/cmake/FindExternalLib.cmake @@ -55,7 +55,17 @@ function(setup_external_libs ARCH) # ---- library dirs ---- if(EXISTS "${FULL_PATH}/lib") - list(APPEND LIBRARY_DIRS "${FULL_PATH}/lib") + file(GLOB _BUNDLED_LIBSTDCXX_FILES + "${FULL_PATH}/lib/libstdc++.so" + "${FULL_PATH}/lib/libstdc++.so.*" + ) + if(_BUNDLED_LIBSTDCXX_FILES) + message(STATUS + "${LIB_NAME}: excluding vendor lib directory from global " + "link paths because it contains a private libstdc++") + else() + list(APPEND LIBRARY_DIRS "${FULL_PATH}/lib") + endif() set(HAS_LIB TRUE) # Collect shared libs for install: *.so and *.so.* @@ -129,6 +139,7 @@ function(setup_external_libs ARCH) list(REMOVE_DUPLICATES LIBRARY_DIRS) link_directories(${LIBRARY_DIRS}) endif() + set(CMVR_EXTERNAL_LIBRARY_DIRS "${LIBRARY_DIRS}" PARENT_SCOPE) # ---- install third-party shared libs into /lib ---- if(INSTALL_SO_FILES) diff --git a/cmvr-es/config/devices/motor/plc_motors.pb.txt b/cmvr-es/config/devices/motor/plc_motors.pb.txt new file mode 100644 index 00000000..0a1581c2 --- /dev/null +++ b/cmvr-es/config/devices/motor/plc_motors.pb.txt @@ -0,0 +1,56 @@ +motor { + id: "plc_motors" + + motor_groups { + id: "plc_axis_group" + bus_type: MOTOR_BUS_MODBUS_TCP + vendor: MOTOR_VENDOR_PLC_GENERIC + protocol: MOTOR_PROTOCOL_CMVR_PLC_V1 + + modbus_tcp { + host: "192.168.0.10" + port: 502 + unit_id: 1 + + connect_timeout_ms: 500 + io_timeout_ms: 100 + heartbeat_period_ms: 100 + communication_watchdog_ms: 1000 + cyclic_watchdog_ms: 500 + status_poll_period_ms: 20 + reconnect_min_ms: 100 + reconnect_max_ms: 2000 + command_ack_timeout_ms: 500 + + protocol_major: 1 + protocol_minor: 0 + + axes { motor_id: 1 axis_index: 0 } + axes { motor_id: 2 axis_index: 1 } + } + + joint_limits { + enable: true + source: JOINT_LIMIT_SOURCE_CUSTOM + joints { + joint_name: "PLC_AXIS_1" + q_lb: -3.141592653589793 + q_ub: 3.141592653589793 + qd: 1.0 + qdd: 2.0 + } + joints { + joint_name: "PLC_AXIS_2" + q_lb: -1.5707963267948966 + q_ub: 1.5707963267948966 + qd: 0.5 + qdd: 1.0 + } + } + + motors { + motors { id: 1 joint_name: "PLC_AXIS_1" } + motors { id: 2 joint_name: "PLC_AXIS_2" } + } + } +} diff --git a/cmvr-es/config/manager/device_manager.pb.txt b/cmvr-es/config/manager/device_manager.pb.txt index 3506a85a..5b85ffe4 100644 --- a/cmvr-es/config/manager/device_manager.pb.txt +++ b/cmvr-es/config/manager/device_manager.pb.txt @@ -82,6 +82,14 @@ device_manager { enable: false } + devices { + id: "plc_motors" + type: DEVICE_TYPE_MOTOR_SYSTEM + config_file: "devices/motor/plc_motors.pb.txt" + # Configure the PLC endpoint and complete the safety checkout before enabling. + enable: false + } + devices { id: "right_arm" type: DEVICE_TYPE_ROBOT_ARM diff --git a/cmvr-es/devices/arm/aubo_arm/CMakeLists.txt b/cmvr-es/devices/arm/aubo_arm/CMakeLists.txt index b8947772..cd0e47ed 100644 --- a/cmvr-es/devices/arm/aubo_arm/CMakeLists.txt +++ b/cmvr-es/devices/arm/aubo_arm/CMakeLists.txt @@ -46,7 +46,54 @@ target_link_libraries(aubo_arm cmvr_es::proto PRIVATE glog + jsoncpp ) add_library(cmvr_es::device::aubo_arm ALIAS aubo_arm) install(TARGETS aubo_arm LIBRARY DESTINATION lib) + +if(BUILD_TESTING) + enable_testing() + add_executable(aubo_arm_json_command_test + tests/aubo_arm_json_command_test.cpp + ) + target_link_libraries(aubo_arm_json_command_test + PRIVATE + cmvr_es::device::aubo_arm + cmvr_es::proto + ) + add_test( + NAME aubo_arm_json_command_test + COMMAND aubo_arm_json_command_test + ) + set_tests_properties(aubo_arm_json_command_test PROPERTIES + TIMEOUT 10 + ENVIRONMENT "LD_LIBRARY_PATH=${CMVR_TEST_EXTERNAL_LIBRARY_PATH}" + ) + if(UNIX AND NOT APPLE) + # The imported AUBO target still contributes its vendor directory to + # direct consumers' build-tree RUNPATH. Put the system runtime first + # for this test; installed artifacts exclude the vendor libstdc++. + execute_process( + COMMAND ${CMAKE_CXX_COMPILER} -print-file-name=libstdc++.so.6 + OUTPUT_VARIABLE AUBO_TEST_SYSTEM_LIBSTDCXX + OUTPUT_STRIP_TRAILING_WHITESPACE + ) + if(EXISTS "${AUBO_TEST_SYSTEM_LIBSTDCXX}") + get_filename_component( + AUBO_TEST_SYSTEM_LIBSTDCXX_REAL + "${AUBO_TEST_SYSTEM_LIBSTDCXX}" + REALPATH + ) + get_filename_component( + AUBO_TEST_SYSTEM_LIBSTDCXX_DIR + "${AUBO_TEST_SYSTEM_LIBSTDCXX_REAL}" + DIRECTORY + ) + set_property( + TARGET aubo_arm_json_command_test + PROPERTY BUILD_RPATH "${AUBO_TEST_SYSTEM_LIBSTDCXX_DIR}" + ) + endif() + endif() +endif() diff --git a/cmvr-es/devices/arm/aubo_arm/aubo_arm.cpp b/cmvr-es/devices/arm/aubo_arm/aubo_arm.cpp index 41dedc9f..9a3e2b19 100644 --- a/cmvr-es/devices/arm/aubo_arm/aubo_arm.cpp +++ b/cmvr-es/devices/arm/aubo_arm/aubo_arm.cpp @@ -1,12 +1,15 @@ #include "devices/arm/aubo_arm/aubo_arm.h" #include +#include #include +#include #include #include #include #include "common/base/logging/logger.h" +#include "json/json.h" #include "aubo_sdk/rpc.h" @@ -44,6 +47,103 @@ using arcs::aubo_sdk::RobotInterfacePtr; constexpr int kAuboServoMode = 3; +enum class CabinetIoOperation { + GetDigitalInput, + GetDigitalOutput, + SetDigitalOutput, +}; + +std::string lowerString(std::string value) +{ + std::transform(value.begin(), value.end(), value.begin(), [](const unsigned char c) { + return static_cast(std::tolower(c)); + }); + return value; +} + +bool parseJsonCommand(const std::string& request_json, + Json::Value& root, + std::string& error) +{ + Json::CharReaderBuilder builder; + std::unique_ptr reader(builder.newCharReader()); + return reader->parse( + request_json.data(), + request_json.data() + request_json.size(), + &root, + &error); +} + +std::string compactJson(const Json::Value& value) +{ + Json::StreamWriterBuilder builder; + builder[std::string("indentation")] = ""; + return Json::writeString(builder, value); +} + +Json::Value& jsonMember(Json::Value& root, const char* name) +{ + return *root.demand(name, name + std::strlen(name)); +} + +const Json::Value* findJsonMember(const Json::Value& root, const char* name) +{ + return root.find(name, name + std::strlen(name)); +} + +bool requiredJsonString(const Json::Value& root, + const char* name, + std::string& value) +{ + const Json::Value* member = findJsonMember(root, name); + if (!member || !member->isString() || member->asString().empty()) { + return false; + } + value = member->asString(); + return true; +} + +bool requiredJsonInt(const Json::Value& root, const char* name, int& value) +{ + const Json::Value* member = findJsonMember(root, name); + if (!member || !member->isInt()) { + return false; + } + value = member->asInt(); + return true; +} + +bool requiredJsonBool(const Json::Value& root, const char* name, bool& value) +{ + const Json::Value* member = findJsonMember(root, name); + if (!member || !member->isBool()) { + return false; + } + value = member->asBool(); + return true; +} + +bool parseCabinetIoOperation(const std::string& name, CabinetIoOperation& operation) +{ + const std::string normalized = lowerString(name); + if (normalized == "get_di") { + operation = CabinetIoOperation::GetDigitalInput; + } else if (normalized == "get_do") { + operation = CabinetIoOperation::GetDigitalOutput; + } else if (normalized == "set_do") { + operation = CabinetIoOperation::SetDigitalOutput; + } else { + return false; + } + return true; +} + +std::string standardOutputRunstateName( + const arcs::common_interface::StandardOutputRunState runstate) +{ + return arcs::common_interface::toString(runstate); +} + RobotInterfacePtr getPrimaryRobotInterface(const std::shared_ptr& rpc_client, const std::string& context, Result& result) @@ -179,6 +279,142 @@ bool AuboArm::stop() return stopMotion().ok(); } +bool AuboArm::executeJsonCommand(const std::string& request_json, + std::string& response_json) +{ + Json::Value response(Json::objectValue); + jsonMember(response, "success") = false; + const auto fail = [&](const std::string& error_code, + const std::string& error_message) { + jsonMember(response, "success") = false; + jsonMember(response, "error_code") = error_code; + jsonMember(response, "error_message") = error_message; + response_json = compactJson(response); + return false; + }; + + Json::Value root; + std::string parse_error; + if (!parseJsonCommand(request_json, root, parse_error)) { + return fail("invalid_json", "invalid json: " + parse_error); + } + if (!root.isObject()) { + return fail("invalid_json", "invalid json: root must be an object"); + } + + std::string command; + if (!requiredJsonString(root, "command", command)) { + return fail("invalid_argument", + "field 'command' is required and must be a non-empty string"); + } + command = lowerString(command); + if (command != "cabinet_io") { + return fail("unsupported_command", "unsupported json command: " + command); + } + jsonMember(response, "command") = command; + + std::string operation_name; + if (!requiredJsonString(root, "operation", operation_name)) { + return fail("invalid_argument", + "field 'operation' is required and must be a non-empty string"); + } + operation_name = lowerString(operation_name); + CabinetIoOperation operation{}; + if (!parseCabinetIoOperation(operation_name, operation)) { + return fail("invalid_operation", + "unsupported cabinet_io operation: " + operation_name); + } + jsonMember(response, "operation") = operation_name; + + int index = -1; + if (!requiredJsonInt(root, "index", index) || index < 0) { + return fail("invalid_argument", + "field 'index' is required and must be a non-negative JSON integer"); + } + jsonMember(response, "index") = index; + + bool output_value = false; + if (operation == CabinetIoOperation::SetDigitalOutput && + !requiredJsonBool(root, "value", output_value)) { + return fail("invalid_argument", + "field 'value' is required for set_do and must be a JSON boolean"); + } + + std::lock_guard lock(mutex_); + const auto ready = ensureConnected_("cabinet_io"); + if (!ready.ok()) { + return fail("not_connected", ready.message); + } + + try { + Result interface_result; + auto robot_interface = + getPrimaryRobotInterface(sdk_->rpc_client, "cabinet_io", interface_result); + if (!interface_result.ok() || !robot_interface) { + return fail("robot_interface_unavailable", interface_result.message); + } + + auto io = robot_interface->getIoControl(); + if (!io) { + return fail("io_interface_unavailable", + "[AuboArm] cabinet_io failed: IO interface is null"); + } + + const bool is_input = operation == CabinetIoOperation::GetDigitalInput; + const int count = is_input + ? io->getStandardDigitalInputNum() + : io->getStandardDigitalOutputNum(); + jsonMember(response, "count") = count; + if (index >= count) { + return fail( + "index_out_of_range", + "[AuboArm] cabinet_io index out of range: index=" + + std::to_string(index) + ", count=" + std::to_string(count)); + } + + if (operation == CabinetIoOperation::GetDigitalInput) { + jsonMember(response, "value") = io->getStandardDigitalInput(index); + } else { + const auto runstate = io->getStandardDigitalOutputRunstate(index); + jsonMember(response, "runstate") = standardOutputRunstateName(runstate); + jsonMember(response, "runstate_code") = static_cast(runstate); + + if (operation == CabinetIoOperation::GetDigitalOutput) { + jsonMember(response, "value") = + io->getStandardDigitalOutput(index); + } else { + if (runstate != arcs::common_interface::StandardOutputRunState::None) { + return fail( + "output_managed_by_runstate", + "[AuboArm] cabinet_io set_do rejected: output is managed by " + "controller runstate; configure this channel as None before writing"); + } + + const int ret = io->setStandardDigitalOutput(index, output_value); + jsonMember(response, "sdk_return_code") = ret; + if (ret != 0) { + return fail( + "sdk_command_failed", + "[AuboArm] cabinet_io set_do failed: sdk ret=" + + std::to_string(ret)); + } + jsonMember(response, "requested_value") = output_value; + } + } + + jsonMember(response, "success") = true; + response_json = compactJson(response); + return true; + } catch (const arcs::common_interface::AuboException& e) { + jsonMember(response, "sdk_return_code") = e.code(); + return fail("sdk_exception", + std::string("[AuboArm] cabinet_io failed: ") + e.what()); + } catch (const std::exception& e) { + return fail("sdk_exception", + std::string("[AuboArm] cabinet_io failed: ") + e.what()); + } +} + ArmState AuboArm::getRobotState() const { ArmState state; @@ -788,6 +1024,7 @@ Result AuboArm::stopServoMode() Result AuboArm::connect(const std::string& ip, const int port) { + std::lock_guard lock(mutex_); if (connected_.load()) { return Result::success(); } @@ -855,6 +1092,7 @@ Result AuboArm::connect(const std::string& ip, const int port) Result AuboArm::disconnect() { + std::lock_guard lock(mutex_); try { if (sdk_ && sdk_->rpc_client) { if (sdk_->rpc_client->hasLogined()) { diff --git a/cmvr-es/devices/arm/aubo_arm/aubo_arm.h b/cmvr-es/devices/arm/aubo_arm/aubo_arm.h index 20049b6f..9c8a0725 100644 --- a/cmvr-es/devices/arm/aubo_arm/aubo_arm.h +++ b/cmvr-es/devices/arm/aubo_arm/aubo_arm.h @@ -21,6 +21,8 @@ public: std::string typeName() const override { return "AuboARM"; } bool init() override; bool stop() override; + bool executeJsonCommand(const std::string& request_json, + std::string& response_json) override; RobotModel getRobotModel() const override { return model_; } std::size_t getDof() const override { return model_.dof; } diff --git a/cmvr-es/devices/arm/aubo_arm/tests/aubo_arm_json_command_test.cpp b/cmvr-es/devices/arm/aubo_arm/tests/aubo_arm_json_command_test.cpp new file mode 100644 index 00000000..d31683f6 --- /dev/null +++ b/cmvr-es/devices/arm/aubo_arm/tests/aubo_arm_json_command_test.cpp @@ -0,0 +1,84 @@ +#include "devices/arm/aubo_arm/aubo_arm.h" + +#include +#include + +namespace { + +#define CHECK_TRUE(condition) \ + do { \ + if (!(condition)) { \ + std::cerr << "CHECK_TRUE failed at line " << __LINE__ << ": " \ + << #condition << std::endl; \ + return 1; \ + } \ + } while (false) + +bool contains(const std::string& value, const std::string& expected) +{ + return value.find(expected) != std::string::npos; +} + +cmvr::config::RobotArmConfig makeConfig() +{ + cmvr::config::RobotArmConfig config; + config.set_id("aubo_arm_json_test"); + auto* vendor = config.mutable_vendor(); + vendor->set_brand(cmvr::config::VENDOR_ROBOT_ARM_BRAND_AUBO_ARM); + vendor->set_model("AuboTest"); + vendor->set_dof(6); + return config; +} + +} // namespace + +int main() +{ + cmvr::device::AuboArm arm(makeConfig()); + cmvr::device::AbstractDevice* device = &arm; + std::string response; + + CHECK_TRUE(!device->executeJsonCommand("{", response)); + CHECK_TRUE(contains(response, R"("error_code":"invalid_json")")); + + CHECK_TRUE(!device->executeJsonCommand("[]", response)); + CHECK_TRUE(contains(response, R"("error_code":"invalid_json")")); + + CHECK_TRUE(!device->executeJsonCommand( + R"({"command":"ptz","operation":"get_di","index":0})", response)); + CHECK_TRUE(contains(response, R"("error_code":"unsupported_command")")); + + CHECK_TRUE(!device->executeJsonCommand( + R"({"command":"cabinet_io","operation":"get_ai","index":0})", response)); + CHECK_TRUE(contains(response, R"("error_code":"invalid_operation")")); + + CHECK_TRUE(!device->executeJsonCommand( + R"({"command":"cabinet_io","operation":"get_di","index":-1})", response)); + CHECK_TRUE(contains(response, R"("error_code":"invalid_argument")")); + + CHECK_TRUE(!device->executeJsonCommand( + R"({"command":"cabinet_io","operation":"set_do","index":0,"value":1})", + response)); + CHECK_TRUE(contains(response, R"("error_code":"invalid_argument")")); + + CHECK_TRUE(!device->executeJsonCommand( + R"({"command":"cabinet_io","operation":"set_do","index":0})", response)); + CHECK_TRUE(contains(response, R"("error_code":"invalid_argument")")); + + CHECK_TRUE(!device->executeJsonCommand( + R"({"command":"cabinet_io","operation":"get_di","index":0})", response)); + CHECK_TRUE(contains(response, R"("error_code":"not_connected")")); + + CHECK_TRUE(!device->executeJsonCommand( + R"({"command":"cabinet_io","operation":"get_do","index":0})", response)); + CHECK_TRUE(contains(response, R"("operation":"get_do")")); + CHECK_TRUE(contains(response, R"("error_code":"not_connected")")); + + CHECK_TRUE(!device->executeJsonCommand( + R"({"command":"cabinet_io","operation":"set_do","index":0,"value":true})", + response)); + CHECK_TRUE(contains(response, R"("operation":"set_do")")); + CHECK_TRUE(contains(response, R"("error_code":"not_connected")")); + + return 0; +} diff --git a/cmvr-es/devices/motor/CMakeLists.txt b/cmvr-es/devices/motor/CMakeLists.txt index c3081330..08fa5f13 100644 --- a/cmvr-es/devices/motor/CMakeLists.txt +++ b/cmvr-es/devices/motor/CMakeLists.txt @@ -17,4 +17,52 @@ add_subdirectory(drivers/ti5_canopen) add_subdirectory(drivers/mujoco) add_subdirectory(bus_runtime) add_subdirectory(drivers/ethercat_motor) +add_subdirectory(drivers/modbus_plc_motor) + +if(BUILD_TESTING) + enable_testing() + set(CMVR_MOTOR_LIBMODBUS_ROOT + ${CMAKE_SOURCE_DIR}/dependency/${ARCH}/third_party/modbus/3.1.11) + add_executable(modbus_tcp_motor_bus_runtime_test + bus_runtime/modbus_tcp/tests/modbus_tcp_motor_bus_runtime_test.cpp + ) + target_include_directories(modbus_tcp_motor_bus_runtime_test + PRIVATE + ${CMAKE_SOURCE_DIR}/cmvr-es + ${CMVR_MOTOR_LIBMODBUS_ROOT}/include + ) + target_link_directories(modbus_tcp_motor_bus_runtime_test + PRIVATE + ${CMVR_MOTOR_LIBMODBUS_ROOT}/lib + ) + target_link_libraries(modbus_tcp_motor_bus_runtime_test + PRIVATE + cmvr_es::device::motor_bus_runtime + cmvr_es::device::modbus_plc_motor_driver + cmvr_es::proto + modbus + gtest + gtest_main + pthread + glog + ) + target_compile_definitions(modbus_tcp_motor_bus_runtime_test + PRIVATE + CMVR_PLC_MOTOR_SAMPLE_CONFIG_PATH="${CMAKE_SOURCE_DIR}/cmvr-es/config/devices/motor/plc_motors.pb.txt" + ) + add_test(NAME modbus_tcp_motor_bus_runtime_test + COMMAND modbus_tcp_motor_bus_runtime_test) + set(_modbus_motor_test_environment + "LD_LIBRARY_PATH=${CMVR_TEST_EXTERNAL_LIBRARY_PATH}" + ) + if(CMVR_TEST_SYSTEM_LIBSTDCXX) + list(APPEND _modbus_motor_test_environment + "LD_PRELOAD=${CMVR_TEST_SYSTEM_LIBSTDCXX}") + endif() + set_tests_properties(modbus_tcp_motor_bus_runtime_test PROPERTIES + TIMEOUT 15 + ENVIRONMENT "${_modbus_motor_test_environment}" + ) +endif() + add_subdirectory(manager) diff --git a/cmvr-es/devices/motor/bus_runtime/CMakeLists.txt b/cmvr-es/devices/motor/bus_runtime/CMakeLists.txt index ce09edab..e2a9c6f2 100644 --- a/cmvr-es/devices/motor/bus_runtime/CMakeLists.txt +++ b/cmvr-es/devices/motor/bus_runtime/CMakeLists.txt @@ -2,20 +2,29 @@ add_library(motor_bus_runtime SHARED can/src/can_motor_bus_runtime.cpp mujoco/src/mujoco_motor_bus_runtime.cpp ethercat/src/ethercat_motor_bus_runtime.cpp + modbus_tcp/src/modbus_tcp_client.cpp + modbus_tcp/src/modbus_tcp_motor_bus_runtime.cpp ) set(IGH_ETHERCAT_ROOT ${CMAKE_SOURCE_DIR}/dependency/x86/third_party/ethercat/v1.7.0 ) +set(CMVR_LIBMODBUS_ROOT + ${CMAKE_SOURCE_DIR}/dependency/${ARCH}/third_party/modbus/3.1.11 +) target_include_directories(motor_bus_runtime PUBLIC ${CMAKE_CURRENT_SOURCE_DIR} PRIVATE ${IGH_ETHERCAT_ROOT}/include + ${CMVR_LIBMODBUS_ROOT}/include ) -target_link_directories(motor_bus_runtime PRIVATE ${IGH_ETHERCAT_ROOT}/lib) +target_link_directories(motor_bus_runtime PRIVATE + ${IGH_ETHERCAT_ROOT}/lib + ${CMVR_LIBMODBUS_ROOT}/lib +) target_link_libraries(motor_bus_runtime PUBLIC @@ -24,6 +33,7 @@ target_link_libraries(motor_bus_runtime cmvr_es::mujoco_world PRIVATE ethercat + modbus cmvr_es::device::canbus glog ) diff --git a/cmvr-es/devices/motor/bus_runtime/modbus_tcp/include/cmvr_plc_register_map.h b/cmvr-es/devices/motor/bus_runtime/modbus_tcp/include/cmvr_plc_register_map.h new file mode 100644 index 00000000..4c09afa9 --- /dev/null +++ b/cmvr-es/devices/motor/bus_runtime/modbus_tcp/include/cmvr_plc_register_map.h @@ -0,0 +1,209 @@ +#ifndef CMVR_ES_CMVR_PLC_REGISTER_MAP_H +#define CMVR_ES_CMVR_PLC_REGISTER_MAP_H + +#include +#include +#include + +namespace cmvr::device::cmvr_plc { + +constexpr std::uint16_t kMagicCm = 0x434d; +constexpr std::uint16_t kMagicVr = 0x5652; +constexpr std::uint16_t kProtocolMajor = 1; +constexpr std::uint16_t kProtocolMinor = 0; +constexpr double kPositionScale = 1000000.0; +constexpr double kVelocityScale = 1000000.0; +constexpr double kAccelerationScale = 1000000.0; +constexpr double kTorqueScale = 1000.0; + +constexpr int kGlobalRegisterCount = 32; +constexpr int kMagicCmOffset = 0; +constexpr int kMagicVrOffset = 1; +constexpr int kProtocolMajorOffset = 2; +constexpr int kProtocolMinorOffset = 3; +constexpr int kAxisCountOffset = 4; +constexpr int kPlcGlobalStateOffset = 5; +constexpr int kPlcBootIdOffset = 6; +constexpr int kCmvrSessionIdOffset = 8; +constexpr int kCmvrHeartbeatOffset = 10; +constexpr int kPlcHeartbeatOffset = 12; +constexpr int kCommunicationWatchdogOffset = 14; +constexpr int kGlobalErrorOffset = 16; +constexpr int kOwnerStateOffset = 17; +constexpr int kOwnerSessionIdOffset = 18; + +constexpr int kAxisFirstOffset = 100; +constexpr int kAxisRegisterStride = 128; +constexpr int kAxisControlRegisterCount = 64; +constexpr int kAxisStatusRelativeOffset = 64; +constexpr int kAxisStatusRegisterCount = 64; + +constexpr int axisBase(const std::uint32_t axis_index) +{ + return kAxisFirstOffset + static_cast(axis_index) * kAxisRegisterStride; +} + +enum class CommandCode : std::uint16_t { + Nop = 0, + SetZero = 1, + MoveToZero = 2, + ProfilePosition = 3, + ProfileVelocity = 4, + OpenCyclicPosition = 5, + CyclicPositionSample = 6, + OpenCyclicVelocity = 7, + CyclicVelocitySample = 8, + CloseCyclicStream = 9, + QuickStop = 10, + Enable = 11, + Disable = 12, +}; + +enum class CommandState : std::uint16_t { + Idle = 0, + Received = 1, + Validating = 2, + Accepted = 3, + Running = 4, + TargetReached = 5, + Completed = 6, + Rejected = 7, + Failed = 8, + TimedOut = 9, + QuickStopped = 10, + CommunicationLost = 11, +}; + +enum class ResultCode : std::uint16_t { + Ok = 0, + InvalidCommand = 1, + InvalidParameter = 2, + AxisNotReady = 3, + AxisBusy = 4, + NotEnabled = 5, + PositionLimit = 6, + VelocityLimit = 7, + AccelerationLimit = 8, + ZeroNotValid = 9, + DriveFault = 10, + CommandTimeout = 11, + SequenceError = 12, + SessionMismatch = 13, + CommunicationWatchdog = 14, + CyclicWatchdog = 15, + Unsupported = 16, + InternalError = 17, +}; + +enum StatusFlag : std::uint16_t { + Enabled = 1U << 0U, + Moving = 1U << 1U, + TargetReached = 1U << 2U, + Fault = 1U << 3U, + QuickStopActive = 1U << 4U, + CommunicationWatchdogExpired = 1U << 5U, + CyclicWatchdogExpired = 1U << 6U, + ZeroValid = 1U << 7U, + StreamActive = 1U << 8U, + CommandBusy = 1U << 9U, +}; + +constexpr std::size_t kCommandPayloadRegisterCount = 62; +constexpr int kCommitSequenceRelativeOffset = 62; + +constexpr std::size_t kPayloadSequence = 0; +constexpr std::size_t kCommandCode = 2; +constexpr std::size_t kCommandFlags = 3; +constexpr std::size_t kTargetPosition = 4; +constexpr std::size_t kTargetVelocity = 6; +constexpr std::size_t kAcceleration = 8; +constexpr std::size_t kTargetTorque = 10; +constexpr std::size_t kPositionTolerance = 12; +constexpr std::size_t kVelocityTolerance = 14; +constexpr std::size_t kCommandTimeout = 16; +constexpr std::size_t kStreamWatchdog = 18; +constexpr std::size_t kCyclicSampleSequence = 20; +constexpr std::size_t kClientMonotonicTime = 22; +constexpr std::size_t kExpectedZeroEpoch = 24; +constexpr std::size_t kDisconnectAction = 26; +constexpr std::size_t kCommandSessionId = 27; +constexpr std::size_t kPayloadSequenceMirror = 60; + +constexpr std::size_t kAckSequence = 0; +constexpr std::size_t kActiveSequence = 2; +constexpr std::size_t kCommandState = 4; +constexpr std::size_t kResultCode = 5; +constexpr std::size_t kAxisState = 6; +constexpr std::size_t kCurrentMode = 7; +constexpr std::size_t kActualPosition = 8; +constexpr std::size_t kActualVelocity = 10; +constexpr std::size_t kActualTorque = 12; +constexpr std::size_t kTargetPositionStatus = 14; +constexpr std::size_t kTargetVelocityStatus = 16; +constexpr std::size_t kStatusFlags = 18; +constexpr std::size_t kDriveStatusword = 19; +constexpr std::size_t kFaultCode = 20; +constexpr std::size_t kZeroEpoch = 22; +constexpr std::size_t kLastAppliedCyclicSequence = 24; +constexpr std::size_t kStateSequence = 26; +constexpr std::size_t kPlcMonotonicTime = 28; +constexpr std::size_t kHeartbeatAge = 30; +constexpr std::size_t kAckSessionId = 32; +constexpr std::size_t kStateSequenceMirror = 62; + +enum class OwnerState : std::uint16_t { + None = 0, + Accepting = 1, + Accepted = 2, + Rejected = 3, +}; + +inline void encodeUint32(std::uint16_t* registers, + const std::size_t offset, + const std::uint32_t value) +{ + registers[offset] = static_cast(value >> 16U); + registers[offset + 1] = static_cast(value & 0xffffU); +} + +inline void encodeInt32(std::uint16_t* registers, + const std::size_t offset, + const std::int32_t value) +{ + encodeUint32(registers, offset, static_cast(value)); +} + +inline std::uint32_t decodeUint32(const std::uint16_t* registers, + const std::size_t offset) +{ + return (static_cast(registers[offset]) << 16U) | + static_cast(registers[offset + 1]); +} + +inline std::int32_t decodeInt32(const std::uint16_t* registers, + const std::size_t offset) +{ + return static_cast(decodeUint32(registers, offset)); +} + +inline bool isTerminal(const CommandState state) +{ + return state == CommandState::Completed || + state == CommandState::Rejected || + state == CommandState::Failed || + state == CommandState::TimedOut || + state == CommandState::QuickStopped || + state == CommandState::CommunicationLost; +} + +inline bool isFailure(const CommandState state) +{ + return state == CommandState::Rejected || + state == CommandState::Failed || + state == CommandState::TimedOut || + state == CommandState::CommunicationLost; +} + +} // namespace cmvr::device::cmvr_plc + +#endif // CMVR_ES_CMVR_PLC_REGISTER_MAP_H diff --git a/cmvr-es/devices/motor/bus_runtime/modbus_tcp/include/modbus_tcp_client.h b/cmvr-es/devices/motor/bus_runtime/modbus_tcp/include/modbus_tcp_client.h new file mode 100644 index 00000000..2a8015bc --- /dev/null +++ b/cmvr-es/devices/motor/bus_runtime/modbus_tcp/include/modbus_tcp_client.h @@ -0,0 +1,47 @@ +#ifndef CMVR_ES_MODBUS_TCP_CLIENT_H +#define CMVR_ES_MODBUS_TCP_CLIENT_H + +#include +#include +#include +#include + +struct _modbus; +using modbus_t = struct _modbus; + +namespace cmvr::device { + +class ModbusTcpClient { +public: + ModbusTcpClient() = default; + ~ModbusTcpClient(); + + ModbusTcpClient(const ModbusTcpClient&) = delete; + ModbusTcpClient& operator=(const ModbusTcpClient&) = delete; + + bool open(const std::string& host, + std::uint16_t port, + std::uint8_t unit_id, + std::uint32_t connect_timeout_ms, + std::uint32_t response_timeout_ms); + void close(); + void shutdown(); + bool connected() const { return context_ != nullptr; } + + bool readHoldingRegisters(int address, int count, std::vector& values); + bool writeHoldingRegisters(int address, const std::uint16_t* values, int count); + bool writeHoldingRegisters(int address, const std::vector& values); + + const std::string& lastError() const { return last_error_; } + +private: + void setLastErrnoError_(const char* operation); + + modbus_t* context_{nullptr}; + std::atomic socket_fd_{-1}; + std::string last_error_; +}; + +} // namespace cmvr::device + +#endif // CMVR_ES_MODBUS_TCP_CLIENT_H diff --git a/cmvr-es/devices/motor/bus_runtime/modbus_tcp/include/modbus_tcp_motor_bus_runtime.h b/cmvr-es/devices/motor/bus_runtime/modbus_tcp/include/modbus_tcp_motor_bus_runtime.h new file mode 100644 index 00000000..110dbd3f --- /dev/null +++ b/cmvr-es/devices/motor/bus_runtime/modbus_tcp/include/modbus_tcp_motor_bus_runtime.h @@ -0,0 +1,173 @@ +#ifndef CMVR_ES_MODBUS_TCP_MOTOR_BUS_RUNTIME_H +#define CMVR_ES_MODBUS_TCP_MOTOR_BUS_RUNTIME_H + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "devices/motor/bus_runtime/abstract_motor_bus_runtime.h" +#include "devices/motor/bus_runtime/modbus_tcp/include/cmvr_plc_register_map.h" +#include "devices/motor/bus_runtime/modbus_tcp/include/modbus_tcp_client.h" + +namespace cmvr::device { + +struct ModbusTcpMotorBusRuntimeTestAccess; + +struct CmvrPlcAxisCommand { + cmvr_plc::CommandCode code{cmvr_plc::CommandCode::Nop}; + std::uint16_t flags{0}; + std::int32_t target_position{0}; + std::int32_t target_velocity{0}; + std::int32_t acceleration{0}; + std::int32_t target_torque{0}; + std::int32_t position_tolerance{0}; + std::int32_t velocity_tolerance{0}; + std::uint32_t command_timeout_ms{0}; + std::uint32_t stream_watchdog_ms{0}; + std::uint32_t cyclic_sample_sequence{0}; + std::uint32_t client_monotonic_time_ms{0}; + std::uint32_t expected_zero_epoch{0}; + std::uint16_t disconnect_action{0}; +}; + +struct CmvrPlcAxisStatus { + std::uint32_t ack_sequence{0}; + std::uint32_t active_sequence{0}; + cmvr_plc::CommandState command_state{cmvr_plc::CommandState::Idle}; + cmvr_plc::ResultCode result_code{cmvr_plc::ResultCode::Ok}; + std::uint16_t axis_state{0}; + std::uint16_t current_mode{0}; + std::int32_t actual_position{0}; + std::int32_t actual_velocity{0}; + std::int32_t actual_torque{0}; + std::int32_t target_position{0}; + std::int32_t target_velocity{0}; + std::uint16_t status_flags{0}; + std::uint16_t drive_statusword{0}; + std::uint32_t fault_code{0}; + std::uint32_t zero_epoch{0}; + std::uint32_t last_applied_cyclic_sequence{0}; + std::uint32_t state_sequence{0}; + std::uint32_t plc_monotonic_time_ms{0}; + std::uint32_t heartbeat_age_ms{0}; + std::uint32_t ack_session_id{0}; +}; + +class ModbusTcpMotorBusRuntime final : public AbstractMotorBusRuntime { +public: + ModbusTcpMotorBusRuntime(); + ~ModbusTcpMotorBusRuntime() override; + + bool init(const config::MotorGroupConfig& group_cfg) override; + bool start() override; + void stop() override; + config::MotorBusType busType() const override { return config::MOTOR_BUS_MODBUS_TCP; } + + bool hasMotor(std::uint8_t motor_id) const; + bool axisForMotor(std::uint8_t motor_id, std::uint32_t& axis_index) const; + bool connected() const { return connected_.load(); } + std::uint32_t sessionId() const { return session_id_.load(); } + std::uint32_t plcBootId() const { return plc_boot_id_.load(); } + std::uint64_t connectionEpoch() const { return connection_epoch_.load(); } + std::uint32_t streamWatchdogMs() const { return stream_watchdog_ms_; } + std::uint32_t commandAckTimeoutMs() const { return command_ack_timeout_ms_; } + const std::string& id() const { return id_; } + std::string lastError() const; + + bool readAxisStatus(std::uint8_t motor_id, CmvrPlcAxisStatus& status); + bool submitAxisCommand(std::uint8_t motor_id, + const CmvrPlcAxisCommand& command, + bool wait_for_terminal_state = false, + std::optional + expected_connection_epoch = std::nullopt); + bool submitAxisSafetyCommand(std::uint8_t motor_id, + const CmvrPlcAxisCommand& command); + +private: + friend struct ModbusTcpMotorBusRuntimeTestAccess; + + bool connectAndHandshakeLocked_(); + void markDisconnectedLocked_(const std::string& error); + bool readRegistersLocked_(int address, int count, std::vector& values); + bool writeRegistersLocked_(int address, const std::uint16_t* values, int count); + bool writeHeartbeatLocked_(); + bool readAxisStatusByIndex_(std::uint32_t axis_index, CmvrPlcAxisStatus& status); + bool waitForCommand_(std::uint32_t axis_index, + std::uint32_t command_sequence, + std::uint32_t command_session_id, + std::uint64_t connection_epoch, + std::uint64_t cancel_generation, + bool cancel_on_safety_preemption, + const CmvrPlcAxisCommand& command, + bool wait_for_terminal_state, + const CmvrPlcAxisStatus& initial_status); + bool submitAxisCommandImpl_(std::uint8_t motor_id, + const CmvrPlcAxisCommand& command, + bool wait_for_terminal_state, + bool safety_priority, + std::optional + expected_connection_epoch); + bool writeClientHeartbeatLocked_(); + std::shared_ptr axisMutex_(std::uint8_t motor_id) const; + std::shared_ptr safetyMutex_(std::uint8_t motor_id) const; + void workerLoop_(); + static std::uint32_t randomNonZeroSessionId_(); + static std::uint32_t monotonicMilliseconds_(); + + std::string id_; + config::ModbusTcpConfig config_; + std::unordered_map motor_axes_; + mutable std::unordered_map> axis_mutexes_; + mutable std::unordered_map> safety_mutexes_; + std::unordered_map command_sequences_; + std::unordered_map cancel_generations_; + // Monotonic admission counter protected by state_mutex_. Besides being + // useful when diagnosing queueing, it gives concurrency tests an exact + // synchronization point after an invocation has captured its safety + // generation and connection epoch, but before it waits on a command + // serialization mutex. + std::uint64_t command_admission_count_{0}; + + mutable std::mutex io_mutex_; + mutable std::mutex state_mutex_; + mutable std::mutex lifecycle_mutex_; + ModbusTcpClient client_; + std::string last_error_; + std::thread worker_; + std::mutex worker_wait_mutex_; + std::condition_variable worker_wait_cv_; + std::atomic running_{false}; + std::atomic connected_{false}; + std::atomic plc_boot_id_{0}; + std::atomic connection_epoch_{0}; + std::atomic session_id_{0}; + std::uint32_t last_session_id_{0}; + std::uint32_t heartbeat_counter_{0}; + std::chrono::steady_clock::time_point last_client_heartbeat_write_at_{}; + std::uint32_t plc_heartbeat_counter_{0}; + std::chrono::steady_clock::time_point plc_heartbeat_changed_at_{}; + std::uint32_t connect_timeout_ms_{500}; + std::uint32_t io_timeout_ms_{100}; + std::uint32_t heartbeat_period_ms_{100}; + std::uint32_t communication_watchdog_ms_{500}; + std::uint32_t status_poll_period_ms_{20}; + std::uint32_t reconnect_min_ms_{100}; + std::uint32_t reconnect_max_ms_{2000}; + std::uint32_t command_ack_timeout_ms_{500}; + std::uint32_t stream_watchdog_ms_{500}; + std::uint16_t protocol_major_{cmvr_plc::kProtocolMajor}; + std::uint16_t protocol_minor_{cmvr_plc::kProtocolMinor}; +}; + +} // namespace cmvr::device + +#endif // CMVR_ES_MODBUS_TCP_MOTOR_BUS_RUNTIME_H diff --git a/cmvr-es/devices/motor/bus_runtime/modbus_tcp/src/modbus_tcp_client.cpp b/cmvr-es/devices/motor/bus_runtime/modbus_tcp/src/modbus_tcp_client.cpp new file mode 100644 index 00000000..c2905f5e --- /dev/null +++ b/cmvr-es/devices/motor/bus_runtime/modbus_tcp/src/modbus_tcp_client.cpp @@ -0,0 +1,188 @@ +#include "devices/motor/bus_runtime/modbus_tcp/include/modbus_tcp_client.h" + +#include +#include +#include +#include +#include +#include +#include + +#include + +namespace cmvr::device { + +ModbusTcpClient::~ModbusTcpClient() +{ + close(); +} + +bool ModbusTcpClient::open(const std::string& host, + const std::uint16_t port, + const std::uint8_t unit_id, + const std::uint32_t connect_timeout_ms, + const std::uint32_t response_timeout_ms) +{ + close(); + context_ = modbus_new_tcp(host.c_str(), static_cast(port)); + if (!context_) { + last_error_ = "modbus_new_tcp failed"; + return false; + } + if (modbus_set_slave(context_, static_cast(unit_id)) == -1) { + setLastErrnoError_("modbus_set_slave"); + close(); + return false; + } + + const auto timeout = response_timeout_ms == 0 ? 200U : response_timeout_ms; + if (modbus_set_response_timeout(context_, timeout / 1000U, + (timeout % 1000U) * 1000U) == -1 || + modbus_set_byte_timeout(context_, timeout / 1000U, + (timeout % 1000U) * 1000U) == -1) { + setLastErrnoError_("modbus_set_timeout"); + close(); + return false; + } + addrinfo hints{}; + hints.ai_family = AF_INET; + hints.ai_socktype = SOCK_STREAM; + hints.ai_flags = AI_NUMERICHOST; + addrinfo* addresses = nullptr; + const auto service = std::to_string(port); + const auto resolve_result = + getaddrinfo(host.c_str(), service.c_str(), &hints, &addresses); + if (resolve_result != 0) { + last_error_ = std::string("host must be a numeric IPv4 address: ") + + gai_strerror(resolve_result); + close(); + return false; + } + int connected_fd = -1; + for (auto* address = addresses; address; address = address->ai_next) { + const int fd = ::socket(address->ai_family, address->ai_socktype, address->ai_protocol); + if (fd < 0) { + continue; + } + const int old_flags = fcntl(fd, F_GETFL, 0); + if (old_flags < 0 || fcntl(fd, F_SETFL, old_flags | O_NONBLOCK) < 0) { + ::close(fd); + continue; + } + socket_fd_.store(fd); + const int rc = ::connect(fd, address->ai_addr, address->ai_addrlen); + if (rc == 0 || errno == EINPROGRESS) { + pollfd descriptor{fd, POLLOUT, 0}; + const auto timeout = static_cast( + connect_timeout_ms == 0 ? 1000U : connect_timeout_ms); + if (rc == 0 || poll(&descriptor, 1, timeout) > 0) { + int socket_error = 0; + socklen_t error_size = sizeof(socket_error); + if (getsockopt(fd, SOL_SOCKET, SO_ERROR, &socket_error, &error_size) == 0 && + socket_error == 0) { + if (fcntl(fd, F_SETFL, old_flags) == 0) { + connected_fd = fd; + break; + } + } + } + } + socket_fd_.store(-1); + ::close(fd); + } + freeaddrinfo(addresses); + if (connected_fd < 0 || modbus_set_socket(context_, connected_fd) == -1) { + if (connected_fd >= 0) { + ::close(connected_fd); + } + last_error_ = "Modbus TCP connect timed out or failed"; + close(); + return false; + } + socket_fd_.store(connected_fd); + last_error_.clear(); + return true; +} + +void ModbusTcpClient::close() +{ + socket_fd_.store(-1); + if (context_) { + modbus_close(context_); + modbus_free(context_); + context_ = nullptr; + } +} + +void ModbusTcpClient::shutdown() +{ + const auto fd = socket_fd_.load(); + if (fd >= 0) { + ::shutdown(fd, SHUT_RDWR); + } +} + +bool ModbusTcpClient::readHoldingRegisters( + const int address, + const int count, + std::vector& values) +{ + if (!context_) { + last_error_ = "Modbus TCP connection is closed"; + return false; + } + if (address < 0 || count <= 0 || count > MODBUS_MAX_READ_REGISTERS) { + last_error_ = "invalid holding-register read range"; + return false; + } + + values.assign(static_cast(count), 0); + const auto rc = modbus_read_registers(context_, address, count, values.data()); + if (rc != count) { + setLastErrnoError_("modbus_read_registers"); + return false; + } + last_error_.clear(); + return true; +} + +bool ModbusTcpClient::writeHoldingRegisters( + const int address, + const std::uint16_t* values, + const int count) +{ + if (!context_) { + last_error_ = "Modbus TCP connection is closed"; + return false; + } + if (address < 0 || !values || count <= 0 || count > MODBUS_MAX_WRITE_REGISTERS) { + last_error_ = "invalid holding-register write range"; + return false; + } + + const auto rc = modbus_write_registers(context_, address, count, values); + if (rc != count) { + setLastErrnoError_("modbus_write_registers"); + return false; + } + last_error_.clear(); + return true; +} + +bool ModbusTcpClient::writeHoldingRegisters( + const int address, + const std::vector& values) +{ + if (values.size() > static_cast(std::numeric_limits::max())) { + last_error_ = "holding-register write is too large"; + return false; + } + return writeHoldingRegisters(address, values.data(), static_cast(values.size())); +} + +void ModbusTcpClient::setLastErrnoError_(const char* operation) +{ + last_error_ = std::string(operation) + ": " + modbus_strerror(errno); +} + +} // namespace cmvr::device diff --git a/cmvr-es/devices/motor/bus_runtime/modbus_tcp/src/modbus_tcp_motor_bus_runtime.cpp b/cmvr-es/devices/motor/bus_runtime/modbus_tcp/src/modbus_tcp_motor_bus_runtime.cpp new file mode 100644 index 00000000..849a6358 --- /dev/null +++ b/cmvr-es/devices/motor/bus_runtime/modbus_tcp/src/modbus_tcp_motor_bus_runtime.cpp @@ -0,0 +1,1013 @@ +#include "devices/motor/bus_runtime/modbus_tcp/include/modbus_tcp_motor_bus_runtime.h" + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "cmvr/msgs/motor.pb.h" +#include "common/base/logging/logger.h" + +namespace cmvr::device { +namespace { + +std::uint32_t valueOrDefault(const std::uint32_t value, const std::uint32_t fallback) +{ + return value == 0 ? fallback : value; +} + +bool validCommandState(const cmvr_plc::CommandState state) +{ + return static_cast(state) <= + static_cast(cmvr_plc::CommandState::CommunicationLost); +} + +} // namespace + +ModbusTcpMotorBusRuntime::ModbusTcpMotorBusRuntime() = default; + +ModbusTcpMotorBusRuntime::~ModbusTcpMotorBusRuntime() +{ + stop(); +} + +bool ModbusTcpMotorBusRuntime::init(const config::MotorGroupConfig& group_cfg) +{ + stop(); + id_ = group_cfg.id(); + if (id_.empty() || group_cfg.bus_type() != config::MOTOR_BUS_MODBUS_TCP || + !group_cfg.has_modbus_tcp()) { + CMVR_LOG(ERROR) << "[ModbusTcpMotorBusRuntime] invalid group configuration"; + return false; + } + if (group_cfg.protocol() != config::MOTOR_PROTOCOL_CMVR_PLC_V1) { + CMVR_LOG(ERROR) << "[ModbusTcpMotorBusRuntime] unsupported protocol for group: " << id_; + return false; + } + + config_ = group_cfg.modbus_tcp(); + in_addr ipv4_address{}; + if (config_.host().empty() || + inet_pton(AF_INET, config_.host().c_str(), &ipv4_address) != 1 || + config_.port() > 65535U || config_.unit_id() > 247U) { + CMVR_LOG(ERROR) << "[ModbusTcpMotorBusRuntime] invalid endpoint for group: " << id_; + return false; + } + if (config_.axes().empty()) { + CMVR_LOG(ERROR) << "[ModbusTcpMotorBusRuntime] no PLC axes configured for group: " << id_; + return false; + } + + motor_axes_.clear(); + axis_mutexes_.clear(); + safety_mutexes_.clear(); + command_sequences_.clear(); + cancel_generations_.clear(); + command_admission_count_ = 0; + std::unordered_map axes_to_motors; + for (const auto& axis : config_.axes()) { + if (axis.motor_id() < 0 || axis.motor_id() > 255) { + CMVR_LOG(ERROR) << "[ModbusTcpMotorBusRuntime] motor id is outside uint8 range"; + return false; + } + const auto motor_id = static_cast(axis.motor_id()); + const auto last_register = + static_cast(cmvr_plc::kAxisFirstOffset) + + static_cast(axis.axis_index()) * + static_cast(cmvr_plc::kAxisRegisterStride) + + static_cast(cmvr_plc::kAxisRegisterStride - 1); + if (last_register > 65535U || + last_register > static_cast(std::numeric_limits::max())) { + CMVR_LOG(ERROR) << "[ModbusTcpMotorBusRuntime] axis_index maps outside the " + "Modbus address space: " + << axis.axis_index(); + return false; + } + if (!motor_axes_.emplace(motor_id, axis.axis_index()).second || + !axes_to_motors.emplace(axis.axis_index(), motor_id).second) { + CMVR_LOG(ERROR) << "[ModbusTcpMotorBusRuntime] duplicate motor or axis mapping in " + << id_; + return false; + } + axis_mutexes_.emplace(motor_id, std::make_shared()); + safety_mutexes_.emplace(motor_id, std::make_shared()); + command_sequences_.emplace(motor_id, 0U); + cancel_generations_.emplace(motor_id, 0U); + } + + connect_timeout_ms_ = valueOrDefault(config_.connect_timeout_ms(), 500U); + io_timeout_ms_ = valueOrDefault(config_.io_timeout_ms(), 100U); + heartbeat_period_ms_ = valueOrDefault(config_.heartbeat_period_ms(), 100U); + communication_watchdog_ms_ = + valueOrDefault(config_.communication_watchdog_ms(), 500U); + status_poll_period_ms_ = valueOrDefault(config_.status_poll_period_ms(), 20U); + reconnect_min_ms_ = valueOrDefault(config_.reconnect_min_ms(), 100U); + reconnect_max_ms_ = valueOrDefault(config_.reconnect_max_ms(), 2000U); + command_ack_timeout_ms_ = valueOrDefault(config_.command_ack_timeout_ms(), 500U); + protocol_major_ = static_cast( + valueOrDefault(config_.protocol_major(), cmvr_plc::kProtocolMajor)); + protocol_minor_ = static_cast(config_.protocol_minor()); + stream_watchdog_ms_ = valueOrDefault(config_.cyclic_watchdog_ms(), 500U); + if (config_.protocol_major() > std::numeric_limits::max() || + config_.protocol_minor() > std::numeric_limits::max() || + connect_timeout_ms_ < 10U || connect_timeout_ms_ > 10000U || + io_timeout_ms_ < 10U || io_timeout_ms_ > 2000U || + heartbeat_period_ms_ < 10U || heartbeat_period_ms_ > 60000U || + communication_watchdog_ms_ > 120000U || + static_cast(communication_watchdog_ms_) < + static_cast(heartbeat_period_ms_) * 2U || + status_poll_period_ms_ < 1U || status_poll_period_ms_ > 1000U || + reconnect_min_ms_ < 10U || reconnect_min_ms_ > 60000U || + reconnect_max_ms_ > 120000U || + command_ack_timeout_ms_ < io_timeout_ms_ * 3U + status_poll_period_ms_ || + command_ack_timeout_ms_ > 60000U || + stream_watchdog_ms_ < 20U || stream_watchdog_ms_ > 60000U || + communication_watchdog_ms_ < + io_timeout_ms_ * 3U + heartbeat_period_ms_ * 2U || + stream_watchdog_ms_ < io_timeout_ms_ * 3U + status_poll_period_ms_ || + reconnect_max_ms_ < reconnect_min_ms_) { + CMVR_LOG(ERROR) << "[ModbusTcpMotorBusRuntime] invalid watchdog/reconnect timing for " + << id_; + return false; + } + session_id_ = 0; + heartbeat_counter_ = 0; + return true; +} + +bool ModbusTcpMotorBusRuntime::start() +{ + std::lock_guard lifecycle_lock(lifecycle_mutex_); + if (running_.load()) { + return true; + } + if (id_.empty() || motor_axes_.empty()) { + CMVR_LOG(ERROR) << "[ModbusTcpMotorBusRuntime] runtime is not initialized"; + return false; + } + + running_.store(true); + { + std::lock_guard lock(io_mutex_); + if (!connectAndHandshakeLocked_()) { + CMVR_LOG(WARNING) + << "[ModbusTcpMotorBusRuntime] PLC is offline or incompatible at " + << "startup; supervisor will retry group=" << id_ + << ", error=" << lastError(); + } + } + if (running_.load()) { + worker_ = std::thread(&ModbusTcpMotorBusRuntime::workerLoop_, this); + } + return true; +} + +void ModbusTcpMotorBusRuntime::stop() +{ + running_.store(false); + connected_.store(false); + session_id_.store(0); + connection_epoch_.fetch_add(1); + { + std::lock_guard lock(state_mutex_); + for (auto& [motor, generation] : cancel_generations_) { + (void)motor; + ++generation; + } + } + worker_wait_cv_.notify_all(); + client_.shutdown(); + std::lock_guard lifecycle_lock(lifecycle_mutex_); + const bool restarted_during_stop = running_.exchange(false); + connected_.store(false); + session_id_.store(0); + if (restarted_during_stop) { + connection_epoch_.fetch_add(1); + std::lock_guard state_lock(state_mutex_); + for (auto& [motor, generation] : cancel_generations_) { + (void)motor; + ++generation; + } + } + worker_wait_cv_.notify_all(); + client_.shutdown(); + if (worker_.joinable()) { + worker_.join(); + } + std::lock_guard lock(io_mutex_); + client_.close(); + connected_.store(false); + session_id_.store(0); +} + +bool ModbusTcpMotorBusRuntime::hasMotor(const std::uint8_t motor_id) const +{ + return motor_axes_.count(motor_id) != 0; +} + +bool ModbusTcpMotorBusRuntime::axisForMotor( + const std::uint8_t motor_id, + std::uint32_t& axis_index) const +{ + const auto it = motor_axes_.find(motor_id); + if (it == motor_axes_.end()) { + return false; + } + axis_index = it->second; + return true; +} + +std::string ModbusTcpMotorBusRuntime::lastError() const +{ + std::lock_guard lock(state_mutex_); + return last_error_; +} + +bool ModbusTcpMotorBusRuntime::readAxisStatus( + const std::uint8_t motor_id, + CmvrPlcAxisStatus& status) +{ + std::uint32_t axis_index = 0; + if (!axisForMotor(motor_id, axis_index)) { + std::lock_guard lock(state_mutex_); + last_error_ = "motor id has no PLC axis mapping"; + return false; + } + return readAxisStatusByIndex_(axis_index, status); +} + +bool ModbusTcpMotorBusRuntime::submitAxisCommand( + const std::uint8_t motor_id, + const CmvrPlcAxisCommand& command, + const bool wait_for_terminal_state, + const std::optional expected_connection_epoch) +{ + return submitAxisCommandImpl_( + motor_id, command, wait_for_terminal_state, false, + expected_connection_epoch); +} + +bool ModbusTcpMotorBusRuntime::submitAxisSafetyCommand( + const std::uint8_t motor_id, + const CmvrPlcAxisCommand& command) +{ + return submitAxisCommandImpl_( + motor_id, command, true, true, std::nullopt); +} + +bool ModbusTcpMotorBusRuntime::submitAxisCommandImpl_( + const std::uint8_t motor_id, + const CmvrPlcAxisCommand& command, + const bool wait_for_terminal_state, + const bool safety_priority, + const std::optional expected_connection_epoch) +{ + // Snapshot the connection generation at invocation entry. In particular, + // do not let a caller that arrived while the runtime was offline, or one + // that later waits behind a queue/handshake, silently attach itself to a + // newer PLC ownership session. + const auto invocation_connection_epoch = + connection_epoch_.load(std::memory_order_acquire); + if (!running_.load(std::memory_order_acquire) || + !connected_.load(std::memory_order_acquire) || + connection_epoch_.load(std::memory_order_acquire) != + invocation_connection_epoch || + (expected_connection_epoch.has_value() && + invocation_connection_epoch != *expected_connection_epoch)) { + std::lock_guard lock(state_mutex_); + last_error_ = expected_connection_epoch.has_value() && + invocation_connection_epoch != + *expected_connection_epoch + ? "command expected a different connection epoch" + : "Modbus TCP connection is unavailable at command entry"; + return false; + } + + std::uint32_t axis_index = 0; + auto axis_mutex = axisMutex_(motor_id); + auto safety_mutex = safetyMutex_(motor_id); + if (!axis_mutex || !safety_mutex || !axisForMotor(motor_id, axis_index)) { + std::lock_guard lock(state_mutex_); + last_error_ = "motor id has no PLC axis mapping"; + return false; + } + std::unique_lock axis_lock; + std::unique_lock safety_lock; + std::uint64_t cancel_generation = 0; + { + std::lock_guard lock(state_mutex_); + if (safety_priority) { + ++cancel_generations_[motor_id]; + } + cancel_generation = cancel_generations_[motor_id]; + ++command_admission_count_; + } + if (safety_priority) { + // Cancel ordinary motion immediately, then serialize only with other + // safety commands. A later safety command must not cancel this one's + // result while it is awaiting its ACK. + safety_lock = std::unique_lock(*safety_mutex); + } else { + // Capture above before waiting for the per-axis queue. A safety + // command that arrives while this normal command is queued must make + // the queued command stale rather than becoming its new baseline. + axis_lock = std::unique_lock(*axis_mutex); + std::lock_guard lock(state_mutex_); + if (cancel_generations_[motor_id] != cancel_generation) { + last_error_ = + "PLC command was preempted while waiting for the axis queue"; + return false; + } + } + + if (!running_.load(std::memory_order_acquire) || + !connected_.load(std::memory_order_acquire) || + connection_epoch_.load(std::memory_order_acquire) != + invocation_connection_epoch || + (expected_connection_epoch.has_value() && + invocation_connection_epoch != *expected_connection_epoch)) { + std::lock_guard lock(state_mutex_); + last_error_ = "connection epoch changed while waiting for command queue"; + return false; + } + + CmvrPlcAxisStatus initial_status; + // Only SetZero needs a fresh pre-command zero_epoch. Other commands avoid + // a three-FC3 baseline read; QuickStop/Disable therefore spend the first + // available transaction on the safety mailbox itself. + if (!safety_priority && + command.code == cmvr_plc::CommandCode::SetZero && + !readAxisStatusByIndex_(axis_index, initial_status)) { + return false; + } + + std::uint32_t command_sequence = 0; + std::uint32_t command_session_id = 0; + std::uint64_t command_connection_epoch = 0; + std::array payload{}; + payload[cmvr_plc::kCommandCode] = static_cast(command.code); + payload[cmvr_plc::kCommandFlags] = command.flags; + cmvr_plc::encodeInt32(payload.data(), cmvr_plc::kTargetPosition, command.target_position); + cmvr_plc::encodeInt32(payload.data(), cmvr_plc::kTargetVelocity, command.target_velocity); + cmvr_plc::encodeInt32(payload.data(), cmvr_plc::kAcceleration, command.acceleration); + cmvr_plc::encodeInt32(payload.data(), cmvr_plc::kTargetTorque, command.target_torque); + cmvr_plc::encodeInt32(payload.data(), cmvr_plc::kPositionTolerance, + command.position_tolerance); + cmvr_plc::encodeInt32(payload.data(), cmvr_plc::kVelocityTolerance, + command.velocity_tolerance); + cmvr_plc::encodeUint32(payload.data(), cmvr_plc::kCommandTimeout, + command.command_timeout_ms); + cmvr_plc::encodeUint32(payload.data(), cmvr_plc::kStreamWatchdog, + command.stream_watchdog_ms); + cmvr_plc::encodeUint32(payload.data(), cmvr_plc::kCyclicSampleSequence, + command.cyclic_sample_sequence); + cmvr_plc::encodeUint32( + payload.data(), cmvr_plc::kClientMonotonicTime, + command.client_monotonic_time_ms == 0 ? monotonicMilliseconds_() + : command.client_monotonic_time_ms); + const auto expected_zero_epoch = + command.code == cmvr_plc::CommandCode::SetZero + ? initial_status.zero_epoch + : command.expected_zero_epoch; + cmvr_plc::encodeUint32(payload.data(), cmvr_plc::kExpectedZeroEpoch, + expected_zero_epoch); + payload[cmvr_plc::kDisconnectAction] = command.disconnect_action; + + const auto base = cmvr_plc::axisBase(axis_index); + { + std::lock_guard lock(io_mutex_); + if (!running_.load(std::memory_order_acquire) || + !connected_.load(std::memory_order_acquire) || + connection_epoch_.load(std::memory_order_acquire) != + invocation_connection_epoch || + (expected_connection_epoch.has_value() && + invocation_connection_epoch != *expected_connection_epoch)) { + std::lock_guard state_lock(state_mutex_); + last_error_ = + "Modbus TCP connection changed before mailbox commit"; + return false; + } + { + std::lock_guard state_lock(state_mutex_); + if (!safety_priority && + cancel_generations_[motor_id] != cancel_generation) { + last_error_ = "PLC command was preempted before mailbox commit"; + return false; + } + command_sequence = ++command_sequences_[motor_id]; + if (command_sequence == 0) { + command_sequence = ++command_sequences_[motor_id]; + } + } + command_session_id = session_id_.load(); + command_connection_epoch = invocation_connection_epoch; + cmvr_plc::encodeUint32(payload.data(), cmvr_plc::kPayloadSequence, + command_sequence); + cmvr_plc::encodeUint32(payload.data(), cmvr_plc::kPayloadSequenceMirror, + command_sequence); + cmvr_plc::encodeUint32(payload.data(), cmvr_plc::kCommandSessionId, + command_session_id); + const auto heartbeat_due = + std::chrono::steady_clock::now() - + last_client_heartbeat_write_at_ >= + std::chrono::milliseconds(heartbeat_period_ms_); + if (!safety_priority && heartbeat_due && + !writeClientHeartbeatLocked_()) { + return false; + } + if (!writeRegistersLocked_(base, payload.data(), static_cast(payload.size()))) { + return false; + } + std::array commit{}; + cmvr_plc::encodeUint32(commit.data(), 0, command_sequence); + if (!writeRegistersLocked_(base + cmvr_plc::kCommitSequenceRelativeOffset, + commit.data(), static_cast(commit.size()))) { + return false; + } + } + return waitForCommand_(axis_index, command_sequence, command_session_id, + command_connection_epoch, cancel_generation, + !safety_priority, command, + wait_for_terminal_state, initial_status); +} + +bool ModbusTcpMotorBusRuntime::connectAndHandshakeLocked_() +{ + client_.close(); + connected_.store(false); + const auto port = static_cast( + config_.port() == 0 ? 502U : config_.port()); + const auto unit_id = static_cast( + config_.unit_id() == 0 ? 1U : config_.unit_id()); + if (!client_.open(config_.host(), port, unit_id, + connect_timeout_ms_, io_timeout_ms_)) { + std::lock_guard state_lock(state_mutex_); + last_error_ = client_.lastError(); + return false; + } + + std::vector global; + if (!readRegistersLocked_(0, cmvr_plc::kGlobalRegisterCount, global)) { + return false; + } + const auto initial_protocol_major = + global[cmvr_plc::kProtocolMajorOffset]; + const auto initial_protocol_minor = + global[cmvr_plc::kProtocolMinorOffset]; + const auto initial_axis_count = global[cmvr_plc::kAxisCountOffset]; + const auto initial_boot_id = cmvr_plc::decodeUint32( + global.data(), cmvr_plc::kPlcBootIdOffset); + if (global[cmvr_plc::kMagicCmOffset] != cmvr_plc::kMagicCm || + global[cmvr_plc::kMagicVrOffset] != cmvr_plc::kMagicVr) { + markDisconnectedLocked_("PLC register map magic does not match CMVR"); + return false; + } + if (initial_protocol_major != protocol_major_) { + markDisconnectedLocked_("PLC protocol major version mismatch"); + return false; + } + if (initial_protocol_minor < protocol_minor_) { + markDisconnectedLocked_("PLC protocol minor version is older than required"); + return false; + } + if (initial_boot_id == 0U) { + markDisconnectedLocked_("PLC boot_id must be non-zero"); + return false; + } + for (const auto& [motor_id, axis_index] : motor_axes_) { + (void)motor_id; + if (axis_index >= initial_axis_count) { + markDisconnectedLocked_("configured PLC axis is outside PLC axis_count"); + return false; + } + } + const auto validate_handshake_identity = + [&](const std::vector& snapshot) { + if (snapshot[cmvr_plc::kMagicCmOffset] != cmvr_plc::kMagicCm || + snapshot[cmvr_plc::kMagicVrOffset] != cmvr_plc::kMagicVr || + snapshot[cmvr_plc::kProtocolMajorOffset] != initial_protocol_major || + snapshot[cmvr_plc::kProtocolMinorOffset] != initial_protocol_minor || + snapshot[cmvr_plc::kAxisCountOffset] != initial_axis_count) { + markDisconnectedLocked_( + "PLC register-map identity changed during session handshake"); + return false; + } + const auto boot_id = cmvr_plc::decodeUint32( + snapshot.data(), cmvr_plc::kPlcBootIdOffset); + if (boot_id == 0U || boot_id != initial_boot_id) { + markDisconnectedLocked_( + "PLC boot_id changed or became zero during session handshake"); + return false; + } + return true; + }; + + std::uint32_t next_session_id = 0; + do { + next_session_id = randomNonZeroSessionId_(); + } while (next_session_id == last_session_id_); + // A reconnect must not immediately reuse its preceding command namespace. + // This does not claim global lifetime uniqueness, only adjacent-session + // separation, which is what stale mailbox/ACK rejection requires. + session_id_ = next_session_id; + last_session_id_ = next_session_id; + heartbeat_counter_ = 0; + { + std::lock_guard state_lock(state_mutex_); + for (auto& [motor_id, sequence] : command_sequences_) { + (void)motor_id; + sequence = 0; + } + } + + std::array watchdog{}; + cmvr_plc::encodeUint32(watchdog.data(), 0, communication_watchdog_ms_); + if (!writeRegistersLocked_(cmvr_plc::kCommunicationWatchdogOffset, + watchdog.data(), static_cast(watchdog.size()))) { + return false; + } + // Publish the new owner candidate only after all parameters it depends on + // are visible to the PLC. + std::array session_and_heartbeat{}; + cmvr_plc::encodeUint32(session_and_heartbeat.data(), 0, session_id_.load()); + cmvr_plc::encodeUint32(session_and_heartbeat.data(), 2, ++heartbeat_counter_); + if (!writeRegistersLocked_(cmvr_plc::kCmvrSessionIdOffset, + session_and_heartbeat.data(), + static_cast(session_and_heartbeat.size()))) { + return false; + } + last_client_heartbeat_write_at_ = std::chrono::steady_clock::now(); + + const auto handshake_deadline = std::chrono::steady_clock::now() + + std::chrono::milliseconds(command_ack_timeout_ms_); + do { + if (!readRegistersLocked_(0, cmvr_plc::kGlobalRegisterCount, global)) { + return false; + } + if (!validate_handshake_identity(global)) { + return false; + } + const auto owner_session = cmvr_plc::decodeUint32( + global.data(), cmvr_plc::kOwnerSessionIdOffset); + const auto owner_state = + static_cast(global[cmvr_plc::kOwnerStateOffset]); + if (owner_session == session_id_.load() && + owner_state == cmvr_plc::OwnerState::Accepted) { + break; + } + if (owner_session == session_id_.load() && + owner_state == cmvr_plc::OwnerState::Rejected) { + markDisconnectedLocked_("PLC rejected CMVR session ownership"); + return false; + } + std::this_thread::sleep_for(std::chrono::milliseconds(status_poll_period_ms_)); + } while (std::chrono::steady_clock::now() < handshake_deadline); + if (cmvr_plc::decodeUint32(global.data(), cmvr_plc::kOwnerSessionIdOffset) != + session_id_.load() || + static_cast(global[cmvr_plc::kOwnerStateOffset]) != + cmvr_plc::OwnerState::Accepted) { + markDisconnectedLocked_("PLC session ownership handshake timed out"); + return false; + } + + // Confirm one complete immutable snapshot after owner acceptance. This + // closes the race where the PLC restarts or swaps register-map versions + // between the accepted-session observation and connected_=true. + if (!readRegistersLocked_(0, cmvr_plc::kGlobalRegisterCount, global) || + !validate_handshake_identity(global)) { + return false; + } + if (cmvr_plc::decodeUint32(global.data(), + cmvr_plc::kOwnerSessionIdOffset) != + session_id_.load() || + static_cast( + global[cmvr_plc::kOwnerStateOffset]) != + cmvr_plc::OwnerState::Accepted) { + markDisconnectedLocked_( + "PLC session ownership changed during final handshake confirmation"); + return false; + } + + if (!running_.load()) { + markDisconnectedLocked_( + "runtime stopped during PLC session handshake"); + return false; + } + plc_boot_id_.store(initial_boot_id); + plc_heartbeat_counter_ = cmvr_plc::decodeUint32( + global.data(), cmvr_plc::kPlcHeartbeatOffset); + plc_heartbeat_changed_at_ = std::chrono::steady_clock::now(); + connected_.store(true); + connection_epoch_.fetch_add(1); + { + std::lock_guard state_lock(state_mutex_); + last_error_.clear(); + } + CMVR_LOG(INFO) << "[ModbusTcpMotorBusRuntime] connected group=" << id_ + << ", endpoint=" << config_.host() << ":" << port + << ", boot_id=" << plc_boot_id_.load() + << ", protocol=" << global[cmvr_plc::kProtocolMajorOffset] + << "." << global[cmvr_plc::kProtocolMinorOffset]; + return true; +} + +void ModbusTcpMotorBusRuntime::markDisconnectedLocked_(const std::string& error) +{ + connected_.store(false); + client_.close(); + std::lock_guard state_lock(state_mutex_); + last_error_ = error; +} + +bool ModbusTcpMotorBusRuntime::readRegistersLocked_( + const int address, + const int count, + std::vector& values) +{ + if (!client_.readHoldingRegisters(address, count, values)) { + markDisconnectedLocked_(client_.lastError()); + return false; + } + return true; +} + +bool ModbusTcpMotorBusRuntime::writeRegistersLocked_( + const int address, + const std::uint16_t* values, + const int count) +{ + if (!client_.writeHoldingRegisters(address, values, count)) { + markDisconnectedLocked_(client_.lastError()); + return false; + } + return true; +} + +bool ModbusTcpMotorBusRuntime::writeClientHeartbeatLocked_() +{ + std::array values{}; + cmvr_plc::encodeUint32(values.data(), 0, session_id_.load()); + cmvr_plc::encodeUint32(values.data(), 2, ++heartbeat_counter_); + if (!writeRegistersLocked_(cmvr_plc::kCmvrSessionIdOffset, + values.data(), static_cast(values.size()))) { + return false; + } + last_client_heartbeat_write_at_ = std::chrono::steady_clock::now(); + return true; +} + +bool ModbusTcpMotorBusRuntime::writeHeartbeatLocked_() +{ + if (!writeClientHeartbeatLocked_()) { + return false; + } + std::vector global; + if (!readRegistersLocked_(0, cmvr_plc::kGlobalRegisterCount, global)) { + return false; + } + const auto boot_id = + cmvr_plc::decodeUint32(global.data(), cmvr_plc::kPlcBootIdOffset); + if (boot_id != plc_boot_id_.load()) { + markDisconnectedLocked_("PLC boot_id changed"); + return false; + } + const auto owner_session = cmvr_plc::decodeUint32( + global.data(), cmvr_plc::kOwnerSessionIdOffset); + if (owner_session != session_id_.load() || + static_cast(global[cmvr_plc::kOwnerStateOffset]) != + cmvr_plc::OwnerState::Accepted) { + markDisconnectedLocked_("PLC session ownership was lost"); + return false; + } + const auto plc_heartbeat = cmvr_plc::decodeUint32( + global.data(), cmvr_plc::kPlcHeartbeatOffset); + const auto now = std::chrono::steady_clock::now(); + if (plc_heartbeat != plc_heartbeat_counter_) { + plc_heartbeat_counter_ = plc_heartbeat; + plc_heartbeat_changed_at_ = now; + } else if (now - plc_heartbeat_changed_at_ >= + std::chrono::milliseconds(communication_watchdog_ms_)) { + markDisconnectedLocked_("PLC heartbeat stopped advancing"); + return false; + } + return true; +} + +bool ModbusTcpMotorBusRuntime::readAxisStatusByIndex_( + const std::uint32_t axis_index, + CmvrPlcAxisStatus& status) +{ + std::vector values; + { + std::lock_guard lock(io_mutex_); + if (!connected_.load()) { + std::lock_guard state_lock(state_mutex_); + last_error_ = "Modbus TCP connection is unavailable"; + return false; + } + const auto status_base = cmvr_plc::axisBase(axis_index) + + cmvr_plc::kAxisStatusRelativeOffset; + bool stable_snapshot = false; + for (int attempt = 0; attempt < 3; ++attempt) { + std::vector before; + std::vector candidate; + std::vector after; + if (!readRegistersLocked_( + status_base + static_cast(cmvr_plc::kStateSequence), + 2, before) || + !readRegistersLocked_( + status_base, cmvr_plc::kAxisStatusRegisterCount, + candidate) || + !readRegistersLocked_( + status_base + static_cast(cmvr_plc::kStateSequence), + 2, after)) { + return false; + } + const auto before_sequence = + cmvr_plc::decodeUint32(before.data(), 0); + const auto after_sequence = + cmvr_plc::decodeUint32(after.data(), 0); + const auto block_sequence = cmvr_plc::decodeUint32( + candidate.data(), cmvr_plc::kStateSequence); + const auto block_mirror = cmvr_plc::decodeUint32( + candidate.data(), cmvr_plc::kStateSequenceMirror); + if (before_sequence != 0U && + (before_sequence & 1U) == 0U && + before_sequence == after_sequence && + before_sequence == block_sequence && + before_sequence == block_mirror) { + values = std::move(candidate); + stable_snapshot = true; + break; + } + } + if (!stable_snapshot) { + std::lock_guard state_lock(state_mutex_); + last_error_ = + "PLC axis status changed during bounded seqlock read"; + return false; + } + } + + status.ack_sequence = cmvr_plc::decodeUint32(values.data(), cmvr_plc::kAckSequence); + status.active_sequence = + cmvr_plc::decodeUint32(values.data(), cmvr_plc::kActiveSequence); + status.command_state = + static_cast(values[cmvr_plc::kCommandState]); + status.result_code = + static_cast(values[cmvr_plc::kResultCode]); + status.axis_state = values[cmvr_plc::kAxisState]; + status.current_mode = values[cmvr_plc::kCurrentMode]; + status.actual_position = + cmvr_plc::decodeInt32(values.data(), cmvr_plc::kActualPosition); + status.actual_velocity = + cmvr_plc::decodeInt32(values.data(), cmvr_plc::kActualVelocity); + status.actual_torque = + cmvr_plc::decodeInt32(values.data(), cmvr_plc::kActualTorque); + status.target_position = + cmvr_plc::decodeInt32(values.data(), cmvr_plc::kTargetPositionStatus); + status.target_velocity = + cmvr_plc::decodeInt32(values.data(), cmvr_plc::kTargetVelocityStatus); + status.status_flags = values[cmvr_plc::kStatusFlags]; + status.drive_statusword = values[cmvr_plc::kDriveStatusword]; + status.fault_code = cmvr_plc::decodeUint32(values.data(), cmvr_plc::kFaultCode); + status.zero_epoch = cmvr_plc::decodeUint32(values.data(), cmvr_plc::kZeroEpoch); + status.last_applied_cyclic_sequence = + cmvr_plc::decodeUint32(values.data(), cmvr_plc::kLastAppliedCyclicSequence); + status.state_sequence = + cmvr_plc::decodeUint32(values.data(), cmvr_plc::kStateSequence); + status.plc_monotonic_time_ms = + cmvr_plc::decodeUint32(values.data(), cmvr_plc::kPlcMonotonicTime); + status.heartbeat_age_ms = + cmvr_plc::decodeUint32(values.data(), cmvr_plc::kHeartbeatAge); + status.ack_session_id = + cmvr_plc::decodeUint32(values.data(), cmvr_plc::kAckSessionId); + if (status.heartbeat_age_ms > communication_watchdog_ms_) { + std::lock_guard state_lock(state_mutex_); + last_error_ = "PLC axis status is stale"; + return false; + } + return true; +} + +bool ModbusTcpMotorBusRuntime::waitForCommand_( + const std::uint32_t axis_index, + const std::uint32_t command_sequence, + const std::uint32_t command_session_id, + const std::uint64_t connection_epoch, + const std::uint64_t cancel_generation, + const bool cancel_on_safety_preemption, + const CmvrPlcAxisCommand& command, + const bool wait_for_terminal_state, + const CmvrPlcAxisStatus& initial_status) +{ + const auto timeout_ms = wait_for_terminal_state + ? valueOrDefault(command.command_timeout_ms, command_ack_timeout_ms_) + : command_ack_timeout_ms_; + const auto deadline = std::chrono::steady_clock::now() + + std::chrono::milliseconds(timeout_ms); + do { + if (cancel_on_safety_preemption) { + std::lock_guard state_lock(state_mutex_); + for (const auto& [motor_id, axis] : motor_axes_) { + if (axis == axis_index && + cancel_generations_[motor_id] != cancel_generation) { + last_error_ = "PLC command was preempted by a safety command"; + return false; + } + } + } + if (connection_epoch_.load() != connection_epoch) { + std::lock_guard state_lock(state_mutex_); + last_error_ = "connection session changed while awaiting PLC command"; + return false; + } + CmvrPlcAxisStatus status; + if (!readAxisStatusByIndex_(axis_index, status)) { + return false; + } + if (std::chrono::steady_clock::now() >= deadline) { + break; + } + if (status.ack_session_id == command_session_id && + status.ack_sequence == command_sequence) { + if (!validCommandState(status.command_state)) { + std::lock_guard state_lock(state_mutex_); + last_error_ = "PLC returned an unknown command state"; + return false; + } + if (cmvr_plc::isFailure(status.command_state) || + status.result_code != cmvr_plc::ResultCode::Ok) { + std::lock_guard state_lock(state_mutex_); + last_error_ = "PLC rejected or failed command, result=" + + std::to_string(static_cast(status.result_code)); + return false; + } + if (!wait_for_terminal_state) { + const bool accepted_state = + status.command_state == cmvr_plc::CommandState::Accepted || + status.command_state == cmvr_plc::CommandState::Running || + status.command_state == cmvr_plc::CommandState::TargetReached || + status.command_state == cmvr_plc::CommandState::Completed; + if (!accepted_state) { + std::lock_guard state_lock(state_mutex_); + last_error_ = "PLC ACK did not report an accepted command state"; + return false; + } + if (command.code == cmvr_plc::CommandCode::CyclicPositionSample || + command.code == cmvr_plc::CommandCode::CyclicVelocitySample) { + if (status.last_applied_cyclic_sequence == + command.cyclic_sample_sequence) { + std::lock_guard state_lock(state_mutex_); + last_error_.clear(); + return true; + } + } else if ( + command.code == cmvr_plc::CommandCode::OpenCyclicPosition || + command.code == cmvr_plc::CommandCode::OpenCyclicVelocity) { + const auto expected_mode = + command.code == + cmvr_plc::CommandCode::OpenCyclicPosition + ? msgs::RUN_MODE_CYCLIC_SYNC_POSITION + : msgs::RUN_MODE_CYCLIC_SYNC_VELOCITY; + if (status.last_applied_cyclic_sequence != 0U || + (status.status_flags & + cmvr_plc::StatusFlag::StreamActive) == 0U || + status.current_mode != + static_cast(expected_mode)) { + std::lock_guard state_lock(state_mutex_); + last_error_ = + "PLC OpenCyclic ACK did not establish a fresh active stream"; + return false; + } + std::lock_guard state_lock(state_mutex_); + last_error_.clear(); + return true; + } else { + std::lock_guard state_lock(state_mutex_); + last_error_.clear(); + return true; + } + } else { + bool completed = false; + switch (command.code) { + case cmvr_plc::CommandCode::SetZero: + completed = + status.command_state == cmvr_plc::CommandState::Completed && + (status.status_flags & cmvr_plc::StatusFlag::ZeroValid) != 0U && + status.zero_epoch != initial_status.zero_epoch; + break; + case cmvr_plc::CommandCode::Enable: + completed = + status.command_state == cmvr_plc::CommandState::Completed && + (status.status_flags & cmvr_plc::StatusFlag::Enabled) != 0U; + break; + case cmvr_plc::CommandCode::Disable: + completed = + status.command_state == cmvr_plc::CommandState::Completed && + (status.status_flags & cmvr_plc::StatusFlag::Enabled) == 0U; + break; + case cmvr_plc::CommandCode::QuickStop: + completed = + (status.command_state == cmvr_plc::CommandState::QuickStopped || + status.command_state == cmvr_plc::CommandState::Completed) && + std::abs(static_cast(status.actual_velocity)) <= 10000; + break; + default: + completed = + status.command_state == cmvr_plc::CommandState::Completed || + status.command_state == cmvr_plc::CommandState::TargetReached; + break; + } + if (completed) { + std::lock_guard state_lock(state_mutex_); + last_error_.clear(); + return true; + } + } + } + std::this_thread::sleep_for(std::chrono::milliseconds(status_poll_period_ms_)); + } while (std::chrono::steady_clock::now() < deadline && connected_.load()); + + std::lock_guard state_lock(state_mutex_); + last_error_ = wait_for_terminal_state ? "PLC command completion timeout" + : "PLC command acknowledgement timeout"; + return false; +} + +std::shared_ptr ModbusTcpMotorBusRuntime::axisMutex_( + const std::uint8_t motor_id) const +{ + const auto it = axis_mutexes_.find(motor_id); + return it == axis_mutexes_.end() ? nullptr : it->second; +} + +std::shared_ptr ModbusTcpMotorBusRuntime::safetyMutex_( + const std::uint8_t motor_id) const +{ + const auto it = safety_mutexes_.find(motor_id); + return it == safety_mutexes_.end() ? nullptr : it->second; +} + +void ModbusTcpMotorBusRuntime::workerLoop_() +{ + auto reconnect_delay = reconnect_min_ms_; + const auto interrupted = [this](const std::uint32_t wait_ms) { + std::unique_lock lock(worker_wait_mutex_); + return worker_wait_cv_.wait_for( + lock, std::chrono::milliseconds(wait_ms), + [this] { return !running_.load(); }); + }; + while (running_.load()) { + if (connected_.load()) { + if (interrupted(heartbeat_period_ms_)) { + break; + } + { + std::lock_guard lock(io_mutex_); + if (connected_.load()) { + writeHeartbeatLocked_(); + } + } + continue; + } + + if (interrupted(reconnect_delay)) { + break; + } + if (!running_.load()) { + break; + } + { + std::lock_guard lock(io_mutex_); + if (connectAndHandshakeLocked_()) { + reconnect_delay = reconnect_min_ms_; + continue; + } + } + reconnect_delay = std::min(reconnect_max_ms_, reconnect_delay * 2U); + } +} + +std::uint32_t ModbusTcpMotorBusRuntime::randomNonZeroSessionId_() +{ + std::random_device random_device; + std::mt19937 generator(random_device()); + std::uniform_int_distribution distribution( + 1U, std::numeric_limits::max()); + return distribution(generator); +} + +std::uint32_t ModbusTcpMotorBusRuntime::monotonicMilliseconds_() +{ + return static_cast( + std::chrono::duration_cast( + std::chrono::steady_clock::now().time_since_epoch()) + .count()); +} + +} // namespace cmvr::device diff --git a/cmvr-es/devices/motor/bus_runtime/modbus_tcp/tests/modbus_tcp_motor_bus_runtime_test.cpp b/cmvr-es/devices/motor/bus_runtime/modbus_tcp/tests/modbus_tcp_motor_bus_runtime_test.cpp new file mode 100644 index 00000000..a18fb2c9 --- /dev/null +++ b/cmvr-es/devices/motor/bus_runtime/modbus_tcp/tests/modbus_tcp_motor_bus_runtime_test.cpp @@ -0,0 +1,1586 @@ +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include + +#include "devices/motor/bus_runtime/modbus_tcp/include/cmvr_plc_register_map.h" +#include "devices/motor/bus_runtime/modbus_tcp/include/modbus_tcp_motor_bus_runtime.h" +#include "devices/motor/drivers/modbus_plc_motor/include/cmvr_plc_motor_protocol.h" + +namespace cmvr::device { + +struct ModbusTcpMotorBusRuntimeTestAccess { + static std::shared_ptr axisMutex( + const ModbusTcpMotorBusRuntime& runtime, + const std::uint8_t motor_id) + { + return runtime.axisMutex_(motor_id); + } + + static std::shared_ptr safetyMutex( + const ModbusTcpMotorBusRuntime& runtime, + const std::uint8_t motor_id) + { + return runtime.safetyMutex_(motor_id); + } + + static std::uint64_t commandAdmissionCount( + const ModbusTcpMotorBusRuntime& runtime) + { + std::lock_guard lock(runtime.state_mutex_); + return runtime.command_admission_count_; + } +}; + +namespace { + +using namespace std::chrono_literals; + +std::uint16_t reserveLoopbackPort() +{ + const int socket_fd = ::socket(AF_INET, SOCK_STREAM, 0); + if (socket_fd < 0) { + return 0; + } + sockaddr_in address{}; + address.sin_family = AF_INET; + address.sin_addr.s_addr = htonl(INADDR_LOOPBACK); + address.sin_port = 0; + if (::bind(socket_fd, reinterpret_cast(&address), sizeof(address)) != 0) { + ::close(socket_fd); + return 0; + } + socklen_t length = sizeof(address); + if (::getsockname(socket_fd, reinterpret_cast(&address), &length) != 0) { + ::close(socket_fd); + return 0; + } + const auto port = ntohs(address.sin_port); + ::close(socket_fd); + return port; +} + +class FakeCmvrPlc { +public: + enum class HandshakeMutation { + None, + BootId, + ProtocolMajor, + }; + + ~FakeCmvrPlc() { stop(); } + + bool start(const std::uint16_t requested_port = 0) + { + port_ = requested_port == 0 ? reserveLoopbackPort() : requested_port; + if (port_ == 0) { + std::fprintf(stderr, "reserveLoopbackPort failed: %s\n", std::strerror(errno)); + return false; + } + context_ = modbus_new_tcp("127.0.0.1", port_); + mapping_ = modbus_mapping_new(1, 1, + cmvr_plc::kAxisFirstOffset + + cmvr_plc::kAxisRegisterStride, + 1); + if (!context_ || !mapping_) { + std::fprintf(stderr, "fake PLC allocation failed: %s\n", modbus_strerror(errno)); + stop(); + return false; + } + auto* registers = mapping_->tab_registers; + registers[cmvr_plc::kMagicCmOffset] = cmvr_plc::kMagicCm; + registers[cmvr_plc::kMagicVrOffset] = cmvr_plc::kMagicVr; + registers[cmvr_plc::kProtocolMajorOffset] = cmvr_plc::kProtocolMajor; + registers[cmvr_plc::kProtocolMinorOffset] = cmvr_plc::kProtocolMinor; + registers[cmvr_plc::kAxisCountOffset] = 1; + registers[cmvr_plc::kOwnerStateOffset] = + static_cast( + preserve_stale_rejected_owner_ + ? cmvr_plc::OwnerState::Rejected + : cmvr_plc::OwnerState::None); + if (preserve_stale_rejected_owner_) { + cmvr_plc::encodeUint32( + registers, cmvr_plc::kOwnerSessionIdOffset, 0x55667788U); + } + cmvr_plc::encodeUint32(registers, cmvr_plc::kPlcBootIdOffset, + initial_boot_id_); + auto* status = registers + cmvr_plc::axisBase(0) + + cmvr_plc::kAxisStatusRelativeOffset; + cmvr_plc::encodeUint32(status, cmvr_plc::kStateSequence, + state_sequence_); + cmvr_plc::encodeUint32(status, cmvr_plc::kStateSequenceMirror, + state_sequence_); + registers[cmvr_plc::axisBase(0) + + cmvr_plc::kAxisStatusRelativeOffset + + cmvr_plc::kCommandState] = + static_cast(cmvr_plc::CommandState::Idle); + registers[cmvr_plc::axisBase(0) + + cmvr_plc::kAxisStatusRelativeOffset + + cmvr_plc::kResultCode] = + static_cast(cmvr_plc::ResultCode::Ok); + + listen_socket_.store(modbus_tcp_listen(context_, 2)); + if (listen_socket_.load() < 0) { + std::fprintf(stderr, "modbus_tcp_listen failed on %u: %s\n", + port_, modbus_strerror(errno)); + stop(); + return false; + } + running_.store(true); + worker_ = std::thread(&FakeCmvrPlc::loop_, this); + return true; + } + + void stop() + { + running_.store(false); + const auto client = client_socket_.exchange(-1); + if (client >= 0) { + ::shutdown(client, SHUT_RDWR); + ::close(client); + } + const auto listener = listen_socket_.exchange(-1); + if (listener >= 0) { + ::shutdown(listener, SHUT_RDWR); + ::close(listener); + } + if (worker_.joinable()) { + worker_.join(); + } + if (mapping_) { + modbus_mapping_free(mapping_); + mapping_ = nullptr; + } + if (context_) { + modbus_free(context_); + context_ = nullptr; + } + } + + void disconnectClient() + { + const auto client = client_socket_.load(); + if (client >= 0) { + ::shutdown(client, SHUT_RDWR); + } + } + + std::uint16_t port() const { return port_; } + std::uint32_t commandCount() const { return command_count_.load(); } + std::uint32_t lastExpectedZeroEpoch() const + { + return last_expected_zero_epoch_.load(); + } + std::uint32_t lastProfileCommandTimeoutMs() const + { + return last_profile_command_timeout_ms_.load(); + } + std::uint32_t cyclicOpenCount() const + { + return cyclic_open_count_.load(); + } + std::uint32_t cyclicApplyCount() const + { + return cyclic_apply_count_.load(); + } + std::uint32_t currentStreamFirstSampleSequence() const + { + return current_stream_first_sample_sequence_.load(); + } + std::uint32_t clientHeartbeatWriteCount() const + { + return client_heartbeat_write_count_.load(); + } + std::uint32_t fullStatusReadCount() const + { + return full_status_read_count_.load(); + } + std::uint32_t observedProfileMailboxCount() const + { + return observed_profile_mailbox_count_.load(); + } + std::uint32_t observedMailboxCommitCount() const + { + return observed_mailbox_commit_count_.load(); + } + void setInitialBootId(const std::uint32_t boot_id) + { + initial_boot_id_ = boot_id; + } + void setHandshakeMutation(const HandshakeMutation mutation) + { + handshake_mutation_ = mutation; + } + void preserveStaleRejectedOwnerDuringHandshake(const bool preserve) + { + preserve_stale_rejected_owner_ = preserve; + } + void setHandshakeAcceptanceDelay(const std::uint32_t delay_ms) + { + handshake_acceptance_delay_ms_ = delay_ms; + } + void holdStatusSnapshotInProgress(const bool hold) + { + hold_status_snapshot_in_progress_.store(hold); + } + void publishOddEqualStatusSnapshot(const bool publish) + { + publish_odd_equal_status_snapshot_.store(publish); + } + void setQuickStopProcessingDelay(const std::uint32_t delay_ms) + { + quick_stop_processing_delay_ms_.store(delay_ms); + } + void injectMixedStablePositionOnce(const std::int32_t mixed_position, + const std::int32_t corrected_position) + { + std::lock_guard lock(mapping_mutex_); + auto* status = mapping_->tab_registers + cmvr_plc::axisBase(0) + + cmvr_plc::kAxisStatusRelativeOffset; + // Deliberately emulate a non-atomic FC3 copy whose markers still show + // the old stable version while one field comes from the next scan. + cmvr_plc::encodeInt32(status, cmvr_plc::kActualPosition, + mixed_position); + corrected_mixed_position_.store(corrected_position); + correct_mixed_snapshot_after_next_read_.store(true); + } + void publishPosition(const std::int32_t position) + { + std::lock_guard lock(mapping_mutex_); + auto* status = mapping_->tab_registers + cmvr_plc::axisBase(0) + + cmvr_plc::kAxisStatusRelativeOffset; + beginSnapshotUpdate_(status); + cmvr_plc::encodeInt32(status, cmvr_plc::kActualPosition, position); + publishSnapshot_(status); + } + void publishFatalFlags() + { + std::lock_guard lock(mapping_mutex_); + auto* status = mapping_->tab_registers + cmvr_plc::axisBase(0) + + cmvr_plc::kAxisStatusRelativeOffset; + beginSnapshotUpdate_(status); + status[cmvr_plc::kStatusFlags] |= + cmvr_plc::StatusFlag::Fault | + cmvr_plc::StatusFlag::CommunicationWatchdogExpired | + cmvr_plc::StatusFlag::CyclicWatchdogExpired; + publishSnapshot_(status); + } + void delayProfileAcks(const bool delay) { delay_profile_acks_.store(delay); } + +private: + void loop_() + { + std::array request{}; + while (running_.load()) { + int listener = listen_socket_.load(); + const int accepted = + listener < 0 ? -1 : modbus_tcp_accept(context_, &listener); + if (accepted < 0) { + if (running_.load()) { + std::this_thread::sleep_for(5ms); + } + continue; + } + client_socket_.store(accepted); + while (running_.load()) { + const auto request_length = modbus_receive(context_, request.data()); + if (request_length <= 0) { + break; + } + { + std::lock_guard mapping_lock(mapping_mutex_); + if (modbus_reply(context_, request.data(), request_length, + mapping_) < 0) { + break; + } + processMailbox_(request.data(), request_length); + } + } + const auto client = client_socket_.exchange(-1); + if (client >= 0) { + ::close(client); + } + } + } + + void processMailbox_(const std::uint8_t* request, + const int request_length) + { + auto* registers = mapping_->tab_registers; + const auto base = cmvr_plc::axisBase(0); + const auto status_base = base + cmvr_plc::kAxisStatusRelativeOffset; + int request_address = -1; + int request_count = 0; + const auto function = + request && request_length >= 8 ? request[7] : 0U; + if (request && request_length >= 12 && + (function == 0x03U || function == 0x10U)) { + request_address = + (static_cast(request[8]) << 8) | + static_cast(request[9]); + request_count = + (static_cast(request[10]) << 8) | + static_cast(request[11]); + } + if (function == 0x10U && + request_address == cmvr_plc::kCmvrSessionIdOffset && + request_count == 4) { + client_heartbeat_write_count_.fetch_add(1); + } + const bool status_read = + function == 0x03U && + request_address >= status_base && + request_address < status_base + + cmvr_plc::kAxisStatusRegisterCount; + const bool full_status_read = + function == 0x03U && + request_address == status_base && + request_count == cmvr_plc::kAxisStatusRegisterCount; + if (full_status_read) { + full_status_read_count_.fetch_add(1); + } + cmvr_plc::encodeUint32( + registers, cmvr_plc::kPlcHeartbeatOffset, ++plc_heartbeat_); + const auto session = + cmvr_plc::decodeUint32(registers, cmvr_plc::kCmvrSessionIdOffset); + const auto now = std::chrono::steady_clock::now(); + if (session != observed_session_) { + observed_session_ = session; + session_observed_at_ = now; + if (!handshake_identity_mutated_) { + if (handshake_mutation_ == HandshakeMutation::BootId) { + cmvr_plc::encodeUint32( + registers, cmvr_plc::kPlcBootIdOffset, + initial_boot_id_ + 1U); + } else if (handshake_mutation_ == + HandshakeMutation::ProtocolMajor) { + registers[cmvr_plc::kProtocolMajorOffset] = + cmvr_plc::kProtocolMajor + 1U; + } + handshake_identity_mutated_ = + handshake_mutation_ != HandshakeMutation::None; + } + if (!preserve_stale_rejected_owner_) { + registers[cmvr_plc::kOwnerStateOffset] = + static_cast( + cmvr_plc::OwnerState::Accepting); + } + last_sequence_ = 0; + last_observed_mailbox_sequence_ = 0; + last_observed_profile_sequence_ = 0; + cmvr_plc::encodeUint32(registers + base, + cmvr_plc::kPayloadSequence, 0); + cmvr_plc::encodeUint32(registers + base, + cmvr_plc::kPayloadSequenceMirror, 0); + cmvr_plc::encodeUint32(registers + base, + cmvr_plc::kCommitSequenceRelativeOffset, 0); + beginSnapshotUpdate_(registers + status_base); + cyclic_stream_open_ = false; + last_cyclic_sample_sequence_ = 0; + current_stream_first_sample_sequence_.store(0); + registers[status_base + cmvr_plc::kStatusFlags] &= + static_cast( + ~cmvr_plc::StatusFlag::StreamActive); + cmvr_plc::encodeUint32( + registers + status_base, + cmvr_plc::kLastAppliedCyclicSequence, 0); + publishSnapshot_(registers + status_base); + } + if (active_session_ != observed_session_ && + now - session_observed_at_ >= + std::chrono::milliseconds(handshake_acceptance_delay_ms_)) { + active_session_ = observed_session_; + cmvr_plc::encodeUint32( + registers, cmvr_plc::kOwnerSessionIdOffset, active_session_); + registers[cmvr_plc::kOwnerStateOffset] = + static_cast(cmvr_plc::OwnerState::Accepted); + } + if (active_session_ == 0 || active_session_ != session) { + processStatusInjection_(registers + status_base, status_read, + full_status_read); + return; + } + const auto sequence = + cmvr_plc::decodeUint32(registers + base, cmvr_plc::kPayloadSequence); + const auto mirror = + cmvr_plc::decodeUint32(registers + base, + cmvr_plc::kPayloadSequenceMirror); + const auto commit = + cmvr_plc::decodeUint32(registers + base, + cmvr_plc::kCommitSequenceRelativeOffset); + if (sequence == 0 || sequence == last_sequence_ || + sequence != mirror || sequence != commit || + cmvr_plc::decodeUint32(registers + base, + cmvr_plc::kCommandSessionId) != active_session_) { + processStatusInjection_(registers + status_base, status_read, + full_status_read); + return; + } + const auto code = static_cast( + registers[base + cmvr_plc::kCommandCode]); + if (sequence != last_observed_mailbox_sequence_) { + last_observed_mailbox_sequence_ = sequence; + observed_mailbox_commit_count_.fetch_add(1); + } + const bool profile_command = + code == cmvr_plc::CommandCode::ProfilePosition || + code == cmvr_plc::CommandCode::ProfileVelocity; + if (profile_command) { + if (sequence != last_observed_profile_sequence_) { + last_observed_profile_sequence_ = sequence; + observed_profile_mailbox_count_.fetch_add(1); + } + last_profile_command_timeout_ms_.store( + cmvr_plc::decodeUint32(registers + base, + cmvr_plc::kCommandTimeout)); + } + if (delay_profile_acks_.load() && profile_command) { + // Leave the last published status snapshot unchanged. Rewriting + // its seqlock marker on every FC3 poll would make the fake report + // a torn snapshot and release the caller's axis mutex instead of + // deterministically keeping the command pending. + return; + } + beginSnapshotUpdate_(registers + status_base); + if (code == cmvr_plc::CommandCode::QuickStop && + quick_stop_processing_delay_ms_.load() > 0U) { + std::this_thread::sleep_for(std::chrono::milliseconds( + quick_stop_processing_delay_ms_.load())); + } + const bool cyclic_sample = + code == cmvr_plc::CommandCode::CyclicPositionSample || + code == cmvr_plc::CommandCode::CyclicVelocitySample; + const auto cyclic_sample_sequence = cmvr_plc::decodeUint32( + registers + base, cmvr_plc::kCyclicSampleSequence); + if (cyclic_sample && + (!cyclic_stream_open_ || cyclic_sample_sequence == 0U || + cyclic_sample_sequence == last_cyclic_sample_sequence_)) { + last_sequence_ = sequence; + cmvr_plc::encodeUint32( + registers + status_base, cmvr_plc::kAckSequence, sequence); + cmvr_plc::encodeUint32( + registers + status_base, cmvr_plc::kAckSessionId, + active_session_); + registers[status_base + cmvr_plc::kCommandState] = + static_cast( + cmvr_plc::CommandState::Rejected); + registers[status_base + cmvr_plc::kResultCode] = + static_cast( + cmvr_plc::ResultCode::SequenceError); + publishSnapshot_(registers + status_base); + return; + } + if (code == cmvr_plc::CommandCode::SetZero) { + last_expected_zero_epoch_.store( + cmvr_plc::decodeUint32(registers + base, + cmvr_plc::kExpectedZeroEpoch)); + } + if (code == cmvr_plc::CommandCode::SetZero && + cmvr_plc::decodeUint32(registers + base, + cmvr_plc::kExpectedZeroEpoch) != + cmvr_plc::decodeUint32(registers + status_base, + cmvr_plc::kZeroEpoch)) { + last_sequence_ = sequence; + cmvr_plc::encodeUint32(registers + status_base, + cmvr_plc::kAckSequence, sequence); + cmvr_plc::encodeUint32(registers + status_base, + cmvr_plc::kAckSessionId, active_session_); + registers[status_base + cmvr_plc::kCommandState] = + static_cast(cmvr_plc::CommandState::Rejected); + registers[status_base + cmvr_plc::kResultCode] = + static_cast(cmvr_plc::ResultCode::SequenceError); + publishSnapshot_(registers + status_base); + return; + } + last_sequence_ = sequence; + command_count_.fetch_add(1); + cmvr_plc::encodeUint32(registers + status_base, cmvr_plc::kAckSequence, + sequence); + cmvr_plc::encodeUint32(registers + status_base, cmvr_plc::kActiveSequence, + sequence); + cmvr_plc::encodeUint32(registers + status_base, cmvr_plc::kAckSessionId, + active_session_); + registers[status_base + cmvr_plc::kResultCode] = + static_cast(cmvr_plc::ResultCode::Ok); + + const auto target_position = + cmvr_plc::decodeInt32(registers + base, cmvr_plc::kTargetPosition); + const auto target_velocity = + cmvr_plc::decodeInt32(registers + base, cmvr_plc::kTargetVelocity); + auto& flags = registers[status_base + cmvr_plc::kStatusFlags]; + auto state = cmvr_plc::CommandState::Completed; + switch (code) { + case cmvr_plc::CommandCode::SetZero: + cmvr_plc::encodeInt32(registers + status_base, + cmvr_plc::kActualPosition, 0); + flags |= cmvr_plc::StatusFlag::ZeroValid; + cmvr_plc::encodeUint32( + registers + status_base, cmvr_plc::kZeroEpoch, + cmvr_plc::decodeUint32(registers + status_base, + cmvr_plc::kZeroEpoch) + + 1U); + break; + case cmvr_plc::CommandCode::ProfilePosition: + registers[status_base + cmvr_plc::kCurrentMode] = + msgs::RUN_MODE_PROFILE_POSITION; + cmvr_plc::encodeInt32(registers + status_base, + cmvr_plc::kActualPosition, target_position); + cmvr_plc::encodeInt32(registers + status_base, + cmvr_plc::kTargetPositionStatus, target_position); + flags |= cmvr_plc::StatusFlag::TargetReached; + state = cmvr_plc::CommandState::TargetReached; + break; + case cmvr_plc::CommandCode::ProfileVelocity: + registers[status_base + cmvr_plc::kCurrentMode] = + msgs::RUN_MODE_PROFILE_VELOCITY; + cmvr_plc::encodeInt32(registers + status_base, + cmvr_plc::kActualVelocity, target_velocity); + cmvr_plc::encodeInt32(registers + status_base, + cmvr_plc::kTargetVelocityStatus, target_velocity); + flags |= cmvr_plc::StatusFlag::TargetReached; + state = cmvr_plc::CommandState::TargetReached; + break; + case cmvr_plc::CommandCode::OpenCyclicPosition: + registers[status_base + cmvr_plc::kCurrentMode] = + msgs::RUN_MODE_CYCLIC_SYNC_POSITION; + flags |= cmvr_plc::StatusFlag::StreamActive; + cyclic_stream_open_ = true; + last_cyclic_sample_sequence_ = 0; + current_stream_first_sample_sequence_.store(0); + cyclic_open_count_.fetch_add(1); + cmvr_plc::encodeUint32( + registers + status_base, + cmvr_plc::kLastAppliedCyclicSequence, 0); + state = cmvr_plc::CommandState::Accepted; + break; + case cmvr_plc::CommandCode::OpenCyclicVelocity: + registers[status_base + cmvr_plc::kCurrentMode] = + msgs::RUN_MODE_CYCLIC_SYNC_VELOCITY; + flags |= cmvr_plc::StatusFlag::StreamActive; + cyclic_stream_open_ = true; + last_cyclic_sample_sequence_ = 0; + current_stream_first_sample_sequence_.store(0); + cyclic_open_count_.fetch_add(1); + cmvr_plc::encodeUint32( + registers + status_base, + cmvr_plc::kLastAppliedCyclicSequence, 0); + state = cmvr_plc::CommandState::Accepted; + break; + case cmvr_plc::CommandCode::CyclicPositionSample: + cmvr_plc::encodeInt32(registers + status_base, + cmvr_plc::kActualPosition, target_position); + cmvr_plc::encodeInt32(registers + status_base, + cmvr_plc::kActualVelocity, target_velocity); + cmvr_plc::encodeUint32( + registers + status_base, + cmvr_plc::kLastAppliedCyclicSequence, + cyclic_sample_sequence); + last_cyclic_sample_sequence_ = cyclic_sample_sequence; + if (current_stream_first_sample_sequence_.load() == 0U) { + current_stream_first_sample_sequence_.store( + cyclic_sample_sequence); + } + cyclic_apply_count_.fetch_add(1); + state = cmvr_plc::CommandState::Accepted; + break; + case cmvr_plc::CommandCode::CyclicVelocitySample: + cmvr_plc::encodeInt32(registers + status_base, + cmvr_plc::kActualVelocity, target_velocity); + cmvr_plc::encodeUint32( + registers + status_base, + cmvr_plc::kLastAppliedCyclicSequence, + cyclic_sample_sequence); + last_cyclic_sample_sequence_ = cyclic_sample_sequence; + if (current_stream_first_sample_sequence_.load() == 0U) { + current_stream_first_sample_sequence_.store( + cyclic_sample_sequence); + } + cyclic_apply_count_.fetch_add(1); + state = cmvr_plc::CommandState::Accepted; + break; + case cmvr_plc::CommandCode::QuickStop: + cmvr_plc::encodeInt32(registers + status_base, + cmvr_plc::kActualVelocity, 0); + flags &= static_cast(~cmvr_plc::StatusFlag::StreamActive); + cyclic_stream_open_ = false; + state = cmvr_plc::CommandState::QuickStopped; + break; + case cmvr_plc::CommandCode::Enable: + flags |= cmvr_plc::StatusFlag::Enabled; + break; + case cmvr_plc::CommandCode::Disable: + flags &= static_cast(~cmvr_plc::StatusFlag::Enabled); + flags &= static_cast(~cmvr_plc::StatusFlag::StreamActive); + cyclic_stream_open_ = false; + break; + default: + break; + } + registers[status_base + cmvr_plc::kCommandState] = + static_cast(state); + publishSnapshot_(registers + status_base); + } + + void processStatusInjection_(std::uint16_t* status, + const bool status_read, + const bool full_status_read) + { + if (full_status_read && + correct_mixed_snapshot_after_next_read_.exchange(false)) { + beginSnapshotUpdate_(status); + cmvr_plc::encodeInt32( + status, cmvr_plc::kActualPosition, + corrected_mixed_position_.load()); + publishSnapshot_(status); + return; + } + if (status_read && + (hold_status_snapshot_in_progress_.load() || + publish_odd_equal_status_snapshot_.load())) { + beginSnapshotUpdate_(status); + publishSnapshot_(status); + } + } + + void publishSnapshot_(std::uint16_t* status) + { + cmvr_plc::encodeUint32(status, cmvr_plc::kHeartbeatAge, 0); + const auto in_progress_sequence = state_sequence_ + 1U; + if (publish_odd_equal_status_snapshot_.load()) { + cmvr_plc::encodeUint32(status, cmvr_plc::kStateSequenceMirror, + in_progress_sequence); + return; + } + if (hold_status_snapshot_in_progress_.load()) { + return; + } + auto next_stable_sequence = state_sequence_ + 2U; + if (next_stable_sequence == 0U) { + next_stable_sequence = 2U; + } + // Seqlock publication order: mirror first, sequence last. + cmvr_plc::encodeUint32(status, cmvr_plc::kStateSequenceMirror, + next_stable_sequence); + cmvr_plc::encodeUint32(status, cmvr_plc::kStateSequence, + next_stable_sequence); + state_sequence_ = next_stable_sequence; + } + + void beginSnapshotUpdate_(std::uint16_t* status) + { + // Mark the snapshot in progress before changing any status field. + // The mirror intentionally remains at the previous stable even value. + cmvr_plc::encodeUint32(status, cmvr_plc::kStateSequence, + state_sequence_ + 1U); + } + + modbus_t* context_{nullptr}; + modbus_mapping_t* mapping_{nullptr}; + std::mutex mapping_mutex_; + std::thread worker_; + std::atomic running_{false}; + std::atomic listen_socket_{-1}; + std::atomic client_socket_{-1}; + std::atomic command_count_{0}; + std::atomic last_expected_zero_epoch_{0}; + std::atomic last_profile_command_timeout_ms_{0}; + std::atomic cyclic_open_count_{0}; + std::atomic cyclic_apply_count_{0}; + std::atomic current_stream_first_sample_sequence_{0}; + std::atomic client_heartbeat_write_count_{0}; + std::atomic full_status_read_count_{0}; + std::atomic observed_profile_mailbox_count_{0}; + std::atomic observed_mailbox_commit_count_{0}; + std::atomic quick_stop_processing_delay_ms_{0}; + std::atomic corrected_mixed_position_{0}; + std::atomic correct_mixed_snapshot_after_next_read_{false}; + std::atomic delay_profile_acks_{false}; + std::atomic hold_status_snapshot_in_progress_{false}; + std::atomic publish_odd_equal_status_snapshot_{false}; + std::uint32_t active_session_{0}; + std::uint32_t observed_session_{0}; + std::uint32_t plc_heartbeat_{0}; + std::uint32_t state_sequence_{2}; + std::chrono::steady_clock::time_point session_observed_at_{}; + std::uint32_t last_sequence_{0}; + std::uint32_t last_observed_mailbox_sequence_{0}; + std::uint32_t last_observed_profile_sequence_{0}; + std::uint32_t initial_boot_id_{0x10203040U}; + HandshakeMutation handshake_mutation_{HandshakeMutation::None}; + bool handshake_identity_mutated_{false}; + bool preserve_stale_rejected_owner_{false}; + std::uint32_t handshake_acceptance_delay_ms_{20}; + bool cyclic_stream_open_{false}; + std::uint32_t last_cyclic_sample_sequence_{0}; + std::uint16_t port_{0}; +}; + +config::MotorGroupConfig makeConfig(const std::uint16_t port) +{ + config::MotorGroupConfig group; + group.set_id("test_plc"); + group.set_bus_type(config::MOTOR_BUS_MODBUS_TCP); + group.set_vendor(config::MOTOR_VENDOR_PLC_GENERIC); + group.set_protocol(config::MOTOR_PROTOCOL_CMVR_PLC_V1); + auto* modbus = group.mutable_modbus_tcp(); + modbus->set_host("127.0.0.1"); + modbus->set_port(port); + modbus->set_unit_id(1); + modbus->set_connect_timeout_ms(200); + modbus->set_io_timeout_ms(100); + modbus->set_heartbeat_period_ms(20); + modbus->set_communication_watchdog_ms(500); + modbus->set_status_poll_period_ms(2); + modbus->set_reconnect_min_ms(10); + modbus->set_reconnect_max_ms(50); + modbus->set_command_ack_timeout_ms(500); + modbus->set_cyclic_watchdog_ms(500); + auto* axis = modbus->add_axes(); + axis->set_motor_id(1); + axis->set_axis_index(0); + return group; +} + +template +bool waitUntil(Predicate&& predicate, const std::chrono::milliseconds timeout) +{ + const auto deadline = std::chrono::steady_clock::now() + timeout; + do { + if (predicate()) { + return true; + } + std::this_thread::sleep_for(5ms); + } while (std::chrono::steady_clock::now() < deadline); + return predicate(); +} + +TEST(ModbusTcpMotorBusRuntimeTest, ExecutesSupportedCmvrPlcV1Commands) +{ + FakeCmvrPlc plc; + ASSERT_TRUE(plc.start()); + auto runtime = std::make_shared(); + ASSERT_TRUE(runtime->init(makeConfig(plc.port()))); + ASSERT_TRUE(runtime->start()) << runtime->lastError(); + ASSERT_NE(runtime->sessionId(), 0U); + + CmvrPlcMotorProtocol protocol(runtime); + ASSERT_TRUE(protocol.initNode(1)); + protocol.setLimitQ(1, 2.0, -2.0); + protocol.setLimitQd(1, 2.0); + protocol.setLimitQdd(1, 5.0, -5.0); + + EXPECT_TRUE(protocol.torqueOn(1)); + EXPECT_TRUE(protocol.commandProfilePosition(1, 1.25, 0.5, 1.0)); + EXPECT_EQ(plc.lastProfileCommandTimeoutMs(), 600000U); + EXPECT_TRUE(protocol.reachedTargetQ(1)); + EXPECT_NEAR(protocol.getQ(1), 1.25, 1e-6); + EXPECT_TRUE(protocol.commandProfileVelocity(1, -0.4, 1.0)); + EXPECT_EQ(plc.lastProfileCommandTimeoutMs(), 600000U); + EXPECT_NEAR(protocol.getQd(1), -0.4, 1e-6); + EXPECT_TRUE(protocol.commandCyclicPosition(1, 0.75, 0.1)); + EXPECT_EQ(plc.cyclicOpenCount(), 1U); + EXPECT_EQ(plc.cyclicApplyCount(), 1U); + EXPECT_EQ(plc.currentStreamFirstSampleSequence(), 1U); + EXPECT_NEAR(protocol.getQ(1), 0.75, 1e-6); + EXPECT_TRUE(protocol.quickStop(1)); + EXPECT_TRUE(protocol.commandCyclicPosition(1, 0.5, 0.1)); + EXPECT_EQ(plc.cyclicOpenCount(), 2U); + EXPECT_EQ(plc.cyclicApplyCount(), 2U); + EXPECT_EQ(plc.currentStreamFirstSampleSequence(), 1U); + EXPECT_NEAR(protocol.getQ(1), 0.5, 1e-6); + EXPECT_TRUE(protocol.quickStop(1)); + EXPECT_TRUE(protocol.commandCyclicVelocity(1, -0.2)); + EXPECT_EQ(plc.cyclicOpenCount(), 3U); + EXPECT_EQ(plc.cyclicApplyCount(), 3U); + EXPECT_EQ(plc.currentStreamFirstSampleSequence(), 1U); + CmvrPlcAxisStatus cyclic_velocity_status; + ASSERT_TRUE(runtime->readAxisStatus(1, cyclic_velocity_status)); + EXPECT_EQ(cyclic_velocity_status.current_mode, + msgs::RUN_MODE_CYCLIC_SYNC_VELOCITY); + EXPECT_NE(cyclic_velocity_status.status_flags & + cmvr_plc::StatusFlag::StreamActive, + 0U); + EXPECT_EQ(cyclic_velocity_status.last_applied_cyclic_sequence, 1U); + EXPECT_EQ(cyclic_velocity_status.actual_velocity, -200000); + EXPECT_TRUE(protocol.quickStop(1)); + EXPECT_TRUE(protocol.calibrateZeroQ(1)); + EXPECT_NEAR(protocol.getQ(1), 0.0, 1e-6); + EXPECT_TRUE(protocol.calibrateZeroQ(1)); + EXPECT_EQ(plc.lastExpectedZeroEpoch(), 1U); + EXPECT_TRUE(protocol.quickStop(1)); + EXPECT_TRUE(protocol.torqueOff(1)); + EXPECT_FALSE(protocol.commandCyclicTorque(1, 0.1)); + runtime->stop(); +} + +TEST(ModbusTcpMotorBusRuntimeTest, AvoidsRedundantBaselineAndHeartbeatWrites) +{ + FakeCmvrPlc plc; + ASSERT_TRUE(plc.start()); + auto group = makeConfig(plc.port()); + group.mutable_modbus_tcp()->set_heartbeat_period_ms(1000); + group.mutable_modbus_tcp()->set_communication_watchdog_ms(3000); + auto runtime = std::make_shared(); + ASSERT_TRUE(runtime->init(group)); + ASSERT_TRUE(runtime->start()); + CmvrPlcMotorProtocol protocol(runtime); + ASSERT_TRUE(protocol.initNode(1)); + protocol.setLimitQ(1, 2.0, -2.0); + protocol.setLimitQd(1, 2.0); + protocol.setLimitQdd(1, 5.0, -5.0); + + const auto heartbeat_writes = plc.clientHeartbeatWriteCount(); + const auto reads_before_profile = plc.fullStatusReadCount(); + ASSERT_TRUE(protocol.commandProfilePosition(1, 0.5, 0.5, 1.0)); + EXPECT_EQ(plc.fullStatusReadCount() - reads_before_profile, 1U); + EXPECT_EQ(plc.clientHeartbeatWriteCount(), heartbeat_writes); + + const auto reads_before_zero = plc.fullStatusReadCount(); + ASSERT_TRUE(protocol.calibrateZeroQ(1)); + EXPECT_EQ(plc.fullStatusReadCount() - reads_before_zero, 2U); + EXPECT_EQ(plc.clientHeartbeatWriteCount(), heartbeat_writes); + runtime->stop(); +} + +TEST(ModbusTcpMotorBusRuntimeTest, ReconnectCreatesNewSessionWithoutReplayingCommands) +{ + FakeCmvrPlc plc; + ASSERT_TRUE(plc.start()); + auto runtime = std::make_shared(); + ASSERT_TRUE(runtime->init(makeConfig(plc.port()))); + ASSERT_TRUE(runtime->start()); + + CmvrPlcMotorProtocol protocol(runtime); + ASSERT_TRUE(protocol.initNode(1)); + ASSERT_TRUE(protocol.commandProfileVelocity(1, 0.2, 0.1)); + const auto old_session = runtime->sessionId(); + const auto old_epoch = runtime->connectionEpoch(); + const auto command_count = plc.commandCount(); + plc.disconnectClient(); + + ASSERT_TRUE(waitUntil( + [&] { return runtime->connected() && runtime->connectionEpoch() > old_epoch; }, + 1500ms)) << runtime->lastError(); + EXPECT_NE(runtime->sessionId(), old_session); + std::this_thread::sleep_for(100ms); + EXPECT_EQ(plc.commandCount(), command_count); + ASSERT_TRUE(protocol.commandProfileVelocity(1, -0.1, 0.1)); + EXPECT_EQ(plc.commandCount(), command_count + 1U); + const auto session_before_stop = runtime->sessionId(); + runtime->stop(); + EXPECT_EQ(runtime->sessionId(), 0U); + ASSERT_TRUE(runtime->start()) << runtime->lastError(); + EXPECT_NE(runtime->sessionId(), session_before_stop); + runtime->stop(); +} + +TEST(ModbusTcpMotorBusRuntimeTest, + ActiveCyclicGenerationRequiresExplicitRestartAfterReconnect) +{ + FakeCmvrPlc plc; + ASSERT_TRUE(plc.start()); + auto runtime = std::make_shared(); + ASSERT_TRUE(runtime->init(makeConfig(plc.port()))); + ASSERT_TRUE(runtime->start()); + + CmvrPlcMotorProtocol protocol(runtime); + ASSERT_TRUE(protocol.initNode(1)); + protocol.setMode(1, msgs::RUN_MODE_CYCLIC_SYNC_POSITION); + ASSERT_TRUE(protocol.commandCyclicPosition(1, 0.1, 0.0)); + ASSERT_EQ(plc.cyclicOpenCount(), 1U); + ASSERT_EQ(plc.cyclicApplyCount(), 1U); + + const auto old_epoch = runtime->connectionEpoch(); + plc.disconnectClient(); + ASSERT_TRUE(waitUntil( + [&] { + return runtime->connected() && + runtime->connectionEpoch() > old_epoch; + }, + 1500ms)) << runtime->lastError(); + + const auto commands_after_reconnect = plc.commandCount(); + const auto opens_after_reconnect = plc.cyclicOpenCount(); + const auto samples_after_reconnect = plc.cyclicApplyCount(); + // The first post-reconnect sample latches this upper-layer generation. + // Further calls from the same generation remain fail-closed and neither + // Open nor sample is committed into the new PLC session. + EXPECT_FALSE(protocol.commandCyclicPosition(1, 0.2, 0.0)); + EXPECT_FALSE(protocol.commandCyclicPosition(1, 0.3, 0.0)); + EXPECT_EQ(plc.commandCount(), commands_after_reconnect); + EXPECT_EQ(plc.cyclicOpenCount(), opens_after_reconnect); + EXPECT_EQ(plc.cyclicApplyCount(), samples_after_reconnect); + + // A new gRPC stream calls setMode even when its mode is unchanged. That + // explicit boundary establishes a new generation and clears the latch. + protocol.setMode(1, msgs::RUN_MODE_CYCLIC_SYNC_POSITION); + EXPECT_TRUE(protocol.commandCyclicPosition(1, 0.4, 0.0)); + EXPECT_EQ(plc.cyclicOpenCount(), opens_after_reconnect + 1U); + EXPECT_EQ(plc.cyclicApplyCount(), samples_after_reconnect + 1U); + EXPECT_EQ(plc.currentStreamFirstSampleSequence(), 1U); + runtime->stop(); +} + +TEST(ModbusTcpMotorBusRuntimeTest, + CyclicOpenQueuedAcrossReconnectCannotEnterNewSession) +{ + FakeCmvrPlc plc; + ASSERT_TRUE(plc.start()); + auto runtime = std::make_shared(); + ASSERT_TRUE(runtime->init(makeConfig(plc.port()))); + ASSERT_TRUE(runtime->start()); + + CmvrPlcMotorProtocol protocol(runtime); + ASSERT_TRUE(protocol.initNode(1)); + protocol.setMode(1, msgs::RUN_MODE_CYCLIC_SYNC_POSITION); + + auto axis_mutex = + ModbusTcpMotorBusRuntimeTestAccess::axisMutex(*runtime, 1); + ASSERT_NE(axis_mutex, nullptr); + std::unique_lock queue_hold(*axis_mutex); + const auto old_epoch = runtime->connectionEpoch(); + const auto mailbox_commits_before = plc.observedMailboxCommitCount(); + const auto opens_before = plc.cyclicOpenCount(); + const auto samples_before = plc.cyclicApplyCount(); + const auto admissions_before = + ModbusTcpMotorBusRuntimeTestAccess::commandAdmissionCount(*runtime); + + std::atomic result{true}; + std::thread queued([&] { + result.store(protocol.commandCyclicPosition(1, 0.1, 0.0)); + }); + const bool admitted = waitUntil( + [&] { + return ModbusTcpMotorBusRuntimeTestAccess::commandAdmissionCount( + *runtime) > + admissions_before; + }, + 500ms); + + // The protocol already bound OpenCyclicPosition to old_epoch, but the + // runtime invocation is still queued. Reconnect before releasing the + // queue and prove that expected_connection_epoch prevents both Open and + // its sample from entering the new PLC ownership session. + plc.disconnectClient(); + const bool reconnected = waitUntil( + [&] { + return runtime->connected() && + runtime->connectionEpoch() > old_epoch; + }, + 1500ms); + queue_hold.unlock(); + queued.join(); + + EXPECT_TRUE(admitted); + EXPECT_TRUE(reconnected) << runtime->lastError(); + EXPECT_FALSE(result.load()); + EXPECT_EQ(plc.observedMailboxCommitCount(), mailbox_commits_before); + EXPECT_EQ(plc.cyclicOpenCount(), opens_before); + EXPECT_EQ(plc.cyclicApplyCount(), samples_before); + + // Failure while crossing epochs permanently latches this generation. + EXPECT_FALSE(protocol.commandCyclicPosition(1, 0.2, 0.0)); + EXPECT_EQ(plc.observedMailboxCommitCount(), mailbox_commits_before); + EXPECT_EQ(plc.cyclicOpenCount(), opens_before); + EXPECT_EQ(plc.cyclicApplyCount(), samples_before); + + // Only the explicit boundary of a new upper-layer stream may recover. + protocol.setMode(1, msgs::RUN_MODE_CYCLIC_SYNC_POSITION); + EXPECT_TRUE(protocol.commandCyclicPosition(1, 0.3, 0.0)); + EXPECT_EQ(plc.cyclicOpenCount(), opens_before + 1U); + EXPECT_EQ(plc.cyclicApplyCount(), samples_before + 1U); + EXPECT_EQ(plc.currentStreamFirstSampleSequence(), 1U); + runtime->stop(); +} + +TEST(ModbusTcpMotorBusRuntimeTest, ProfileFeedbackIsBoundToConnectionEpoch) +{ + FakeCmvrPlc plc; + ASSERT_TRUE(plc.start()); + auto runtime = std::make_shared(); + ASSERT_TRUE(runtime->init(makeConfig(plc.port()))); + ASSERT_TRUE(runtime->start()); + + CmvrPlcMotorProtocol protocol(runtime); + ASSERT_TRUE(protocol.initNode(1)); + // Zero is deliberately chosen: the new PLC session's safe stopped + // velocity is also zero, so value equality cannot prove command identity. + ASSERT_TRUE(protocol.commandProfileVelocity(1, 0.0, 0.1)); + ASSERT_TRUE(std::isfinite(protocol.getQd(1))); + ASSERT_TRUE(protocol.reachedTargetQ(1)); + + auto old_epoch = runtime->connectionEpoch(); + plc.disconnectClient(); + ASSERT_TRUE(waitUntil( + [&] { + return runtime->connected() && + runtime->connectionEpoch() > old_epoch; + }, + 1500ms)) << runtime->lastError(); + + EXPECT_TRUE(std::isnan(protocol.getQ(1))); + EXPECT_TRUE(std::isnan(protocol.getQd(1))); + EXPECT_FALSE(protocol.reachedTargetQ(1)); + + // A successfully ACKed profile in the current session rebinds feedback. + ASSERT_TRUE(protocol.commandProfileVelocity(1, 0.0, 0.1)); + EXPECT_TRUE(std::isfinite(protocol.getQ(1))); + EXPECT_NEAR(protocol.getQd(1), 0.0, 1e-9); + + old_epoch = runtime->connectionEpoch(); + plc.disconnectClient(); + ASSERT_TRUE(waitUntil( + [&] { + return runtime->connected() && + runtime->connectionEpoch() > old_epoch; + }, + 1500ms)) << runtime->lastError(); + EXPECT_TRUE(std::isnan(protocol.getQd(1))); + + // Successful explicit safety recovery also clears the stale binding. + ASSERT_TRUE(protocol.quickStop(1)); + EXPECT_TRUE(std::isfinite(protocol.getQ(1))); + EXPECT_NEAR(protocol.getQd(1), 0.0, 1e-9); + runtime->stop(); +} + +TEST(ModbusTcpMotorBusRuntimeTest, SupervisorConnectsWhenPlcComesOnlineAfterStart) +{ + const auto port = reserveLoopbackPort(); + ASSERT_NE(port, 0U); + auto runtime = std::make_shared(); + ASSERT_TRUE(runtime->init(makeConfig(port))); + EXPECT_TRUE(runtime->start()); + EXPECT_FALSE(runtime->connected()); + CmvrPlcAxisStatus status; + EXPECT_FALSE(runtime->readAxisStatus(1, status)); + + FakeCmvrPlc plc; + ASSERT_TRUE(plc.start(port)); + ASSERT_TRUE(waitUntil([&] { return runtime->connected(); }, 1500ms)) + << runtime->lastError(); + runtime->stop(); +} + +TEST(ModbusTcpMotorBusRuntimeTest, StopDuringHandshakeLeavesRuntimeDisconnected) +{ + FakeCmvrPlc plc; + plc.setHandshakeAcceptanceDelay(500); + ASSERT_TRUE(plc.start()); + ModbusTcpMotorBusRuntime runtime; + ASSERT_TRUE(runtime.init(makeConfig(plc.port()))); + std::atomic start_result{false}; + std::thread starter([&] { start_result.store(runtime.start()); }); + std::this_thread::sleep_for(50ms); + runtime.stop(); + starter.join(); + EXPECT_TRUE(start_result.load()); + EXPECT_FALSE(runtime.connected()); + EXPECT_EQ(runtime.sessionId(), 0U); +} + +TEST(ModbusTcpMotorBusRuntimeTest, ConcurrentStartStopCannotResurrectWorker) +{ + const auto port = reserveLoopbackPort(); + ASSERT_NE(port, 0U); + ModbusTcpMotorBusRuntime runtime; + ASSERT_TRUE(runtime.init(makeConfig(port))); + for (int iteration = 0; iteration < 20; ++iteration) { + std::atomic ready{0}; + auto synchronize = [&] { + ready.fetch_add(1); + while (ready.load() < 2) { + std::this_thread::yield(); + } + }; + std::thread starter([&] { + synchronize(); + runtime.start(); + }); + std::thread stopper([&] { + synchronize(); + runtime.stop(); + }); + starter.join(); + stopper.join(); + // If start linearized after stop, this final stop owns that later + // lifecycle. It must always terminate and leave no resurrected worker. + runtime.stop(); + EXPECT_FALSE(runtime.connected()); + EXPECT_EQ(runtime.sessionId(), 0U); + } +} + +TEST(ModbusTcpMotorBusRuntimeTest, RejectsZeroPlcBootId) +{ + FakeCmvrPlc plc; + plc.setInitialBootId(0); + ASSERT_TRUE(plc.start()); + ModbusTcpMotorBusRuntime runtime; + ASSERT_TRUE(runtime.init(makeConfig(plc.port()))); + EXPECT_TRUE(runtime.start()); + EXPECT_FALSE(runtime.connected()); + EXPECT_NE(runtime.lastError().find("boot_id must be non-zero"), + std::string::npos); + runtime.stop(); +} + +TEST(ModbusTcpMotorBusRuntimeTest, RejectsBootIdChangeDuringHandshake) +{ + FakeCmvrPlc plc; + plc.setHandshakeMutation(FakeCmvrPlc::HandshakeMutation::BootId); + ASSERT_TRUE(plc.start()); + ModbusTcpMotorBusRuntime runtime; + ASSERT_TRUE(runtime.init(makeConfig(plc.port()))); + EXPECT_TRUE(runtime.start()); + EXPECT_FALSE(runtime.connected()); + EXPECT_NE(runtime.lastError().find("boot_id changed"), + std::string::npos); + runtime.stop(); +} + +TEST(ModbusTcpMotorBusRuntimeTest, RejectsProtocolVersionChangeDuringHandshake) +{ + FakeCmvrPlc plc; + plc.setHandshakeMutation( + FakeCmvrPlc::HandshakeMutation::ProtocolMajor); + ASSERT_TRUE(plc.start()); + ModbusTcpMotorBusRuntime runtime; + ASSERT_TRUE(runtime.init(makeConfig(plc.port()))); + EXPECT_TRUE(runtime.start()); + EXPECT_FALSE(runtime.connected()); + EXPECT_NE(runtime.lastError().find("identity changed"), + std::string::npos); + runtime.stop(); +} + +TEST(ModbusTcpMotorBusRuntimeTest, IgnoresRejectedDecisionForStaleSession) +{ + FakeCmvrPlc plc; + plc.preserveStaleRejectedOwnerDuringHandshake(true); + ASSERT_TRUE(plc.start()); + ModbusTcpMotorBusRuntime runtime; + ASSERT_TRUE(runtime.init(makeConfig(plc.port()))); + EXPECT_TRUE(runtime.start()) << runtime.lastError(); + EXPECT_TRUE(runtime.connected()); + runtime.stop(); +} + +TEST(ModbusTcpMotorBusRuntimeTest, RejectsInProgressAndOddStatusSnapshots) +{ + FakeCmvrPlc plc; + ASSERT_TRUE(plc.start()); + auto runtime = std::make_shared(); + ASSERT_TRUE(runtime->init(makeConfig(plc.port()))); + ASSERT_TRUE(runtime->start()); + + CmvrPlcAxisStatus status; + plc.holdStatusSnapshotInProgress(true); + EXPECT_FALSE(runtime->readAxisStatus(1, status)); + EXPECT_NE(runtime->lastError().find("bounded seqlock"), + std::string::npos); + + plc.holdStatusSnapshotInProgress(false); + plc.publishOddEqualStatusSnapshot(true); + EXPECT_FALSE(runtime->readAxisStatus(1, status)); + EXPECT_NE(runtime->lastError().find("bounded seqlock"), + std::string::npos); + runtime->stop(); +} + +TEST(ModbusTcpMotorBusRuntimeTest, RetriesTornReadAndAcceptsAdvancingStableVersions) +{ + FakeCmvrPlc plc; + ASSERT_TRUE(plc.start()); + auto runtime = std::make_shared(); + ASSERT_TRUE(runtime->init(makeConfig(plc.port()))); + ASSERT_TRUE(runtime->start()); + + CmvrPlcAxisStatus status; + for (std::int32_t position = 100; position <= 300; position += 100) { + plc.publishPosition(position); + ASSERT_TRUE(runtime->readAxisStatus(1, status)); + EXPECT_EQ(status.actual_position, position); + } + + plc.injectMixedStablePositionOnce(999999, 424242); + ASSERT_TRUE(runtime->readAxisStatus(1, status)) << runtime->lastError(); + EXPECT_EQ(status.actual_position, 424242); + runtime->stop(); +} + +TEST(ModbusTcpMotorBusRuntimeTest, QuickStopPreemptsPendingNormalCommand) +{ + FakeCmvrPlc plc; + ASSERT_TRUE(plc.start()); + auto runtime = std::make_shared(); + ASSERT_TRUE(runtime->init(makeConfig(plc.port()))); + ASSERT_TRUE(runtime->start()); + plc.delayProfileAcks(true); + + CmvrPlcAxisCommand normal; + normal.code = cmvr_plc::CommandCode::ProfileVelocity; + normal.target_velocity = 100000; + normal.command_timeout_ms = 5000; + std::atomic normal_result{true}; + std::thread pending([&] { + normal_result.store(runtime->submitAxisCommand(1, normal, false)); + }); + std::this_thread::sleep_for(50ms); + + CmvrPlcAxisCommand stop; + stop.code = cmvr_plc::CommandCode::QuickStop; + stop.command_timeout_ms = 1000; + const auto started = std::chrono::steady_clock::now(); + EXPECT_TRUE(runtime->submitAxisSafetyCommand(1, stop)); + const auto elapsed = std::chrono::duration_cast( + std::chrono::steady_clock::now() - started); + pending.join(); + EXPECT_FALSE(normal_result.load()); + EXPECT_LT(elapsed.count(), 500); + runtime->stop(); +} + +TEST(ModbusTcpMotorBusRuntimeTest, QueuedNormalCommandKeepsPreSafetyGeneration) +{ + FakeCmvrPlc plc; + ASSERT_TRUE(plc.start()); + auto runtime = std::make_shared(); + ASSERT_TRUE(runtime->init(makeConfig(plc.port()))); + ASSERT_TRUE(runtime->start()); + plc.delayProfileAcks(true); + + CmvrPlcAxisCommand normal_a; + normal_a.code = cmvr_plc::CommandCode::ProfileVelocity; + normal_a.target_velocity = 100000; + normal_a.command_timeout_ms = 5000; + std::atomic result_a{true}; + std::thread first([&] { + result_a.store(runtime->submitAxisCommand(1, normal_a, false)); + }); + EXPECT_TRUE(waitUntil( + [&] { return plc.observedProfileMailboxCount() == 1U; }, 500ms)); + + CmvrPlcAxisCommand normal_b = normal_a; + normal_b.target_velocity = 200000; + std::atomic result_b{true}; + const auto admissions_before_b = + ModbusTcpMotorBusRuntimeTestAccess::commandAdmissionCount(*runtime); + std::thread second([&] { + result_b.store(runtime->submitAxisCommand(1, normal_b, false)); + }); + // A is still holding axis_mutex. This counter advances in the exact + // critical section where B snapshots the pre-safety generation, before B + // starts waiting for the per-axis queue. + EXPECT_TRUE(waitUntil( + [&] { + return ModbusTcpMotorBusRuntimeTestAccess::commandAdmissionCount( + *runtime) > + admissions_before_b; + }, + 500ms)); + + CmvrPlcAxisCommand stop; + stop.code = cmvr_plc::CommandCode::QuickStop; + stop.command_timeout_ms = 1000; + EXPECT_TRUE(runtime->submitAxisSafetyCommand(1, stop)); + first.join(); + second.join(); + + EXPECT_FALSE(result_a.load()); + EXPECT_FALSE(result_b.load()); + EXPECT_EQ(plc.observedProfileMailboxCount(), 1U); + runtime->stop(); +} + +TEST(ModbusTcpMotorBusRuntimeTest, QueuedNormalCommandCannotCrossReconnectEpoch) +{ + FakeCmvrPlc plc; + ASSERT_TRUE(plc.start()); + auto runtime = std::make_shared(); + ASSERT_TRUE(runtime->init(makeConfig(plc.port()))); + ASSERT_TRUE(runtime->start()); + + auto axis_mutex = + ModbusTcpMotorBusRuntimeTestAccess::axisMutex(*runtime, 1); + ASSERT_NE(axis_mutex, nullptr); + std::unique_lock queue_hold(*axis_mutex); + const auto old_epoch = runtime->connectionEpoch(); + const auto mailbox_commits_before = plc.observedMailboxCommitCount(); + const auto admissions_before = + ModbusTcpMotorBusRuntimeTestAccess::commandAdmissionCount(*runtime); + + CmvrPlcAxisCommand command; + command.code = cmvr_plc::CommandCode::ProfileVelocity; + command.target_velocity = 200000; + command.command_timeout_ms = 1000; + std::atomic result{true}; + std::thread queued([&] { + result.store(runtime->submitAxisCommand(1, command, false)); + }); + const bool admitted = waitUntil( + [&] { + return ModbusTcpMotorBusRuntimeTestAccess::commandAdmissionCount( + *runtime) > + admissions_before; + }, + 500ms); + + // Keep the old invocation behind axis_mutex while the worker detects the + // broken socket and completes a new ownership handshake. + plc.disconnectClient(); + const bool reconnected = waitUntil( + [&] { + return runtime->connected() && + runtime->connectionEpoch() > old_epoch; + }, + 1500ms); + queue_hold.unlock(); + queued.join(); + + EXPECT_TRUE(admitted); + EXPECT_TRUE(reconnected) << runtime->lastError(); + EXPECT_FALSE(result.load()); + EXPECT_EQ(plc.observedMailboxCommitCount(), mailbox_commits_before); + runtime->stop(); +} + +TEST(ModbusTcpMotorBusRuntimeTest, + ProfileSubmitQueuedAcrossReconnectInvalidatesFeedback) +{ + FakeCmvrPlc plc; + ASSERT_TRUE(plc.start()); + auto runtime = std::make_shared(); + ASSERT_TRUE(runtime->init(makeConfig(plc.port()))); + ASSERT_TRUE(runtime->start()); + CmvrPlcMotorProtocol protocol(runtime); + ASSERT_TRUE(protocol.initNode(1)); + + auto axis_mutex = + ModbusTcpMotorBusRuntimeTestAccess::axisMutex(*runtime, 1); + ASSERT_NE(axis_mutex, nullptr); + std::unique_lock queue_hold(*axis_mutex); + const auto old_epoch = runtime->connectionEpoch(); + const auto mailbox_commits_before = plc.observedMailboxCommitCount(); + const auto admissions_before = + ModbusTcpMotorBusRuntimeTestAccess::commandAdmissionCount(*runtime); + + std::atomic result{true}; + std::thread queued([&] { + result.store(protocol.commandProfileVelocity(1, 0.0, 0.1)); + }); + const bool admitted = waitUntil( + [&] { + return ModbusTcpMotorBusRuntimeTestAccess::commandAdmissionCount( + *runtime) > + admissions_before; + }, + 500ms); + + plc.disconnectClient(); + const bool reconnected = waitUntil( + [&] { + return runtime->connected() && + runtime->connectionEpoch() > old_epoch; + }, + 1500ms); + queue_hold.unlock(); + queued.join(); + + EXPECT_TRUE(admitted); + EXPECT_TRUE(reconnected) << runtime->lastError(); + EXPECT_FALSE(result.load()); + EXPECT_EQ(plc.observedMailboxCommitCount(), mailbox_commits_before); + // Even though the safe S2 process image reports qdot=0, the ambiguous S1 + // invocation cannot claim that value as successful Profile feedback. + EXPECT_TRUE(std::isnan(protocol.getQd(1))); + ASSERT_TRUE(protocol.quickStop(1)); + EXPECT_NEAR(protocol.getQd(1), 0.0, 1e-9); + runtime->stop(); +} + +TEST(ModbusTcpMotorBusRuntimeTest, + RejectsCallerExpectedOldEpochWithoutMailboxCommit) +{ + FakeCmvrPlc plc; + ASSERT_TRUE(plc.start()); + auto runtime = std::make_shared(); + ASSERT_TRUE(runtime->init(makeConfig(plc.port()))); + ASSERT_TRUE(runtime->start()); + + const auto old_epoch = runtime->connectionEpoch(); + plc.disconnectClient(); + ASSERT_TRUE(waitUntil( + [&] { + return runtime->connected() && + runtime->connectionEpoch() > old_epoch; + }, + 1500ms)) << runtime->lastError(); + const auto mailbox_commits_before = plc.observedMailboxCommitCount(); + + CmvrPlcAxisCommand command; + command.code = cmvr_plc::CommandCode::ProfileVelocity; + command.target_velocity = 100000; + command.command_timeout_ms = 1000; + EXPECT_FALSE(runtime->submitAxisCommand( + 1, command, false, old_epoch)); + EXPECT_EQ(plc.observedMailboxCommitCount(), mailbox_commits_before); + runtime->stop(); +} + +TEST(ModbusTcpMotorBusRuntimeTest, QueuedSafetyCommandCannotCrossReconnectEpoch) +{ + FakeCmvrPlc plc; + ASSERT_TRUE(plc.start()); + auto runtime = std::make_shared(); + ASSERT_TRUE(runtime->init(makeConfig(plc.port()))); + ASSERT_TRUE(runtime->start()); + + auto safety_mutex = + ModbusTcpMotorBusRuntimeTestAccess::safetyMutex(*runtime, 1); + ASSERT_NE(safety_mutex, nullptr); + std::unique_lock queue_hold(*safety_mutex); + const auto old_epoch = runtime->connectionEpoch(); + const auto mailbox_commits_before = plc.observedMailboxCommitCount(); + const auto admissions_before = + ModbusTcpMotorBusRuntimeTestAccess::commandAdmissionCount(*runtime); + + CmvrPlcAxisCommand stop; + stop.code = cmvr_plc::CommandCode::QuickStop; + stop.command_timeout_ms = 1000; + std::atomic result{true}; + std::thread queued([&] { + result.store(runtime->submitAxisSafetyCommand(1, stop)); + }); + const bool admitted = waitUntil( + [&] { + return ModbusTcpMotorBusRuntimeTestAccess::commandAdmissionCount( + *runtime) > + admissions_before; + }, + 500ms); + + plc.disconnectClient(); + const bool reconnected = waitUntil( + [&] { + return runtime->connected() && + runtime->connectionEpoch() > old_epoch; + }, + 1500ms); + queue_hold.unlock(); + queued.join(); + + EXPECT_TRUE(admitted); + EXPECT_TRUE(reconnected) << runtime->lastError(); + EXPECT_FALSE(result.load()); + EXPECT_EQ(plc.observedMailboxCommitCount(), mailbox_commits_before); + runtime->stop(); +} + +TEST(ModbusTcpMotorBusRuntimeTest, ConcurrentSafetyCommandsAreSerialized) +{ + FakeCmvrPlc plc; + ASSERT_TRUE(plc.start()); + auto runtime = std::make_shared(); + ASSERT_TRUE(runtime->init(makeConfig(plc.port()))); + ASSERT_TRUE(runtime->start()); + plc.setQuickStopProcessingDelay(50); + + CmvrPlcAxisCommand stop; + stop.code = cmvr_plc::CommandCode::QuickStop; + stop.command_timeout_ms = 1000; + std::atomic first{false}; + std::atomic second{false}; + std::atomic ready{0}; + auto invoke = [&](std::atomic& result) { + ready.fetch_add(1); + while (ready.load() < 2) { + std::this_thread::yield(); + } + result.store(runtime->submitAxisSafetyCommand(1, stop)); + }; + std::thread first_thread(invoke, std::ref(first)); + std::thread second_thread(invoke, std::ref(second)); + first_thread.join(); + second_thread.join(); + EXPECT_TRUE(first.load()); + EXPECT_TRUE(second.load()); + runtime->stop(); +} + +TEST(ModbusTcpMotorBusRuntimeTest, FatalStatusFlagsInvalidateMotionFeedback) +{ + FakeCmvrPlc plc; + ASSERT_TRUE(plc.start()); + auto runtime = std::make_shared(); + ASSERT_TRUE(runtime->init(makeConfig(plc.port()))); + ASSERT_TRUE(runtime->start()); + CmvrPlcMotorProtocol protocol(runtime); + ASSERT_TRUE(protocol.initNode(1)); + + plc.publishPosition(123456); + ASSERT_TRUE(std::isfinite(protocol.getQ(1))); + plc.publishFatalFlags(); + EXPECT_TRUE(std::isnan(protocol.getQ(1))); + EXPECT_TRUE(std::isnan(protocol.getQd(1))); + EXPECT_FALSE(protocol.reachedTargetQ(1)); + runtime->stop(); +} + +TEST(ModbusTcpMotorBusRuntimeTest, RejectsOutOfRangeAxisAndReportsInvalidSamples) +{ + auto hostname_group = makeConfig(15020); + hostname_group.mutable_modbus_tcp()->set_host("localhost"); + ModbusTcpMotorBusRuntime hostname_runtime; + EXPECT_FALSE(hostname_runtime.init(hostname_group)); + + auto group = makeConfig(15020); + group.mutable_modbus_tcp()->mutable_axes(0)->set_axis_index(1000); + ModbusTcpMotorBusRuntime invalid_runtime; + EXPECT_FALSE(invalid_runtime.init(group)); + + auto runtime = std::make_shared(); + ASSERT_TRUE(runtime->init(makeConfig(15020))); + CmvrPlcMotorProtocol protocol(runtime); + ASSERT_TRUE(protocol.initNode(1)); + EXPECT_TRUE(std::isnan(protocol.getQ(1))); + EXPECT_TRUE(std::isnan(protocol.getQd(1))); + EXPECT_FALSE(protocol.commandProfilePosition( + 1, std::numeric_limits::quiet_NaN(), 0.1, 0.1)); +} + +TEST(ModbusTcpMotorBusRuntimeTest, RepositorySampleConfigParsesAndInitializesRuntime) +{ + std::ifstream input(CMVR_PLC_MOTOR_SAMPLE_CONFIG_PATH); + ASSERT_TRUE(input.is_open()) << CMVR_PLC_MOTOR_SAMPLE_CONFIG_PATH; + const std::string text((std::istreambuf_iterator(input)), + std::istreambuf_iterator()); + config::MotorRootConfig root; + ASSERT_TRUE(google::protobuf::TextFormat::ParseFromString(text, &root)); + ASSERT_TRUE(root.has_motor()); + ASSERT_EQ(root.motor().motor_groups_size(), 1); + + ModbusTcpMotorBusRuntime runtime; + EXPECT_TRUE(runtime.init(root.motor().motor_groups(0))) << runtime.lastError(); +} + +} // namespace +} // namespace cmvr::device diff --git a/cmvr-es/devices/motor/drivers/modbus_plc_motor/CMakeLists.txt b/cmvr-es/devices/motor/drivers/modbus_plc_motor/CMakeLists.txt new file mode 100644 index 00000000..7dbd3cdc --- /dev/null +++ b/cmvr-es/devices/motor/drivers/modbus_plc_motor/CMakeLists.txt @@ -0,0 +1,21 @@ +add_library(modbus_plc_motor_driver SHARED + src/cmvr_plc_motor_protocol.cpp + src/modbus_plc_motor.cpp +) + +target_include_directories(modbus_plc_motor_driver + PUBLIC + ${CMAKE_CURRENT_SOURCE_DIR}/include +) + +target_link_libraries(modbus_plc_motor_driver + PUBLIC + cmvr_es::device::motor_core + cmvr_es::device::motor_bus_runtime + PRIVATE + cmvr_es::proto + glog +) + +add_library(cmvr_es::device::modbus_plc_motor_driver ALIAS modbus_plc_motor_driver) +install(TARGETS modbus_plc_motor_driver LIBRARY DESTINATION lib) diff --git a/cmvr-es/devices/motor/drivers/modbus_plc_motor/include/cmvr_plc_motor_protocol.h b/cmvr-es/devices/motor/drivers/modbus_plc_motor/include/cmvr_plc_motor_protocol.h new file mode 100644 index 00000000..0bf534ba --- /dev/null +++ b/cmvr-es/devices/motor/drivers/modbus_plc_motor/include/cmvr_plc_motor_protocol.h @@ -0,0 +1,89 @@ +#ifndef CMVR_ES_CMVR_PLC_MOTOR_PROTOCOL_H +#define CMVR_ES_CMVR_PLC_MOTOR_PROTOCOL_H + +#include +#include +#include +#include +#include + +#include "devices/motor/bus_runtime/modbus_tcp/include/modbus_tcp_motor_bus_runtime.h" +#include "devices/motor/motor_protocol_interface.h" + +namespace cmvr::device { + +class CmvrPlcMotorProtocol final : public MotorProtocolInterface { +public: + explicit CmvrPlcMotorProtocol(std::shared_ptr bus_runtime); + ~CmvrPlcMotorProtocol() override = default; + + bool initNode(std::uint8_t node_id) override; + void setMode(std::uint8_t node_id, msgs::RunMode mode) override; + msgs::RunMode getMode(std::uint8_t node_id) override; + void setLimitQdd(std::uint8_t node_id, double u_qdd, double l_qdd) override; + void setLimitQd(std::uint8_t node_id, double qd) override; + void setLimitQ(std::uint8_t node_id, double ub, double lb) override; + bool calibrateZeroQ(std::uint8_t node_id) override; + bool reachedTargetQ(std::uint8_t node_id) override; + bool commandProfilePosition(std::uint8_t node_id, + double target_q, + double max_qd, + double max_qdd) override; + bool commandProfileVelocity(std::uint8_t node_id, + double target_qd, + double max_qdd) override; + bool commandCyclicPosition(std::uint8_t node_id, + double target_q, + double target_qd) override; + bool commandCyclicVelocity(std::uint8_t node_id, double target_qd) override; + bool commandCyclicTorque(std::uint8_t node_id, double target_tau) override; + void setMotorConversion(std::uint8_t node_id, + double encoder_counts_per_rev, + double gear_ratio) override; + bool torqueOn(std::uint8_t node_id) override; + bool torqueOff(std::uint8_t node_id) override; + bool brakeRelease(std::uint8_t node_id) override; + bool quickStop(std::uint8_t node_id) override; + double getQ(std::uint8_t node_id) override; + double getQd(std::uint8_t node_id) override; + +private: + struct NodeState { + msgs::RunMode requested_mode{msgs::RUN_MODE_UNSPECIFIED}; + double limit_q_lb{0.0}; + double limit_q_ub{0.0}; + double limit_qd{0.0}; + double limit_qdd{0.0}; + bool cyclic_stream_open{false}; + bool cyclic_reconnect_latched{false}; + std::uint32_t cyclic_sequence{0}; + std::uint64_t cyclic_generation{0}; + std::uint64_t cyclic_connection_epoch{0}; + bool profile_feedback_bound{false}; + std::uint64_t profile_connection_epoch{0}; + }; + + bool submitSimple_(std::uint8_t node_id, + cmvr_plc::CommandCode code, + bool wait_for_terminal, + std::uint32_t timeout_ms); + bool ensureCyclicOpen_(std::uint8_t node_id, + NodeState& state, + msgs::RunMode mode); + static std::optional toMicroUnits_(double value); + static double fromMicroUnits_(std::int32_t value); + static bool isSupportedMode_(msgs::RunMode mode); + bool validatePosition_(const NodeState& state, double position) const; + bool validateVelocity_(const NodeState& state, double velocity) const; + bool validateAcceleration_(const NodeState& state, double acceleration) const; + bool readMotionStatus_(std::uint8_t node_id, CmvrPlcAxisStatus& status); + NodeState& nodeStateLocked_(std::uint8_t node_id); + + std::shared_ptr bus_runtime_; + std::mutex nodes_mutex_; + std::unordered_map nodes_; +}; + +} // namespace cmvr::device + +#endif // CMVR_ES_CMVR_PLC_MOTOR_PROTOCOL_H diff --git a/cmvr-es/devices/motor/drivers/modbus_plc_motor/include/modbus_plc_motor.h b/cmvr-es/devices/motor/drivers/modbus_plc_motor/include/modbus_plc_motor.h new file mode 100644 index 00000000..f0a96831 --- /dev/null +++ b/cmvr-es/devices/motor/drivers/modbus_plc_motor/include/modbus_plc_motor.h @@ -0,0 +1,21 @@ +#ifndef CMVR_ES_MODBUS_PLC_MOTOR_H +#define CMVR_ES_MODBUS_PLC_MOTOR_H + +#include "cmvr/config/motor_config/motor_config.pb.h" +#include "devices/motor/abstract_motor.h" + +namespace cmvr::device { + +class ModbusPlcMotor final : public AbstractMotor { +public: + explicit ModbusPlcMotor(const config::MotorConfigItem& config); + + std::string typeName() const override { return "ModbusPlcMotor"; } + bool init() override; + bool torqueOff() override; + bool quickStop() override; +}; + +} // namespace cmvr::device + +#endif // CMVR_ES_MODBUS_PLC_MOTOR_H diff --git a/cmvr-es/devices/motor/drivers/modbus_plc_motor/src/cmvr_plc_motor_protocol.cpp b/cmvr-es/devices/motor/drivers/modbus_plc_motor/src/cmvr_plc_motor_protocol.cpp new file mode 100644 index 00000000..3867303c --- /dev/null +++ b/cmvr-es/devices/motor/drivers/modbus_plc_motor/src/cmvr_plc_motor_protocol.cpp @@ -0,0 +1,582 @@ +#include "devices/motor/drivers/modbus_plc_motor/include/cmvr_plc_motor_protocol.h" + +#include +#include +#include + +#include "common/base/logging/logger.h" + +namespace cmvr::device { +namespace { + +// MotorService accepts waits up to 10 minutes. The PLC timeout is a ceiling; +// shorter RPC timeouts still actively QuickStop from the service layer. +constexpr std::uint32_t kProfileCommandTimeoutCeilingMs = 600000; +constexpr std::uint32_t kSafetyCommandTimeoutMs = 5000; + +bool statusAllowsMotionFeedback(const CmvrPlcAxisStatus& status) +{ + constexpr std::uint16_t kFatalFlags = + cmvr_plc::StatusFlag::Fault | + cmvr_plc::StatusFlag::CommunicationWatchdogExpired | + cmvr_plc::StatusFlag::CyclicWatchdogExpired; + return (status.status_flags & kFatalFlags) == 0U && + status.result_code == cmvr_plc::ResultCode::Ok && + static_cast(status.command_state) <= + static_cast( + cmvr_plc::CommandState::CommunicationLost) && + !cmvr_plc::isFailure(status.command_state); +} + +} // namespace + +CmvrPlcMotorProtocol::CmvrPlcMotorProtocol( + std::shared_ptr bus_runtime) + : bus_runtime_(std::move(bus_runtime)) +{ + comm_proto = CommProto::CUSTOM; +} + +bool CmvrPlcMotorProtocol::initNode(const std::uint8_t node_id) +{ + if (!bus_runtime_ || !bus_runtime_->hasMotor(node_id)) { + CMVR_LOG(ERROR) << "[CmvrPlcMotorProtocol] missing PLC axis mapping for motor " + << static_cast(node_id); + return false; + } + std::lock_guard lock(nodes_mutex_); + auto& state = nodeStateLocked_(node_id); + state.cyclic_connection_epoch = bus_runtime_->connectionEpoch(); + state.cyclic_stream_open = false; + state.cyclic_reconnect_latched = false; + state.cyclic_sequence = 0; + state.cyclic_generation = 0; + state.profile_feedback_bound = false; + state.profile_connection_epoch = 0; + return true; +} + +void CmvrPlcMotorProtocol::setMode( + const std::uint8_t node_id, + const msgs::RunMode mode) +{ + if (!isSupportedMode_(mode)) { + CMVR_LOG(ERROR) << "[CmvrPlcMotorProtocol] unsupported mode " + << msgs::RunMode_Name(mode); + return; + } + std::lock_guard lock(nodes_mutex_); + auto& state = nodeStateLocked_(node_id); + const bool cyclic_mode = + mode == msgs::RUN_MODE_CYCLIC_SYNC_POSITION || + mode == msgs::RUN_MODE_CYCLIC_SYNC_VELOCITY; + if (cyclic_mode) { + // Every explicit cyclic setMode call is a new upper-layer stream + // generation, even when the mode value itself is unchanged. This is + // the only transition allowed to clear a reconnect latch. + state.requested_mode = mode; + state.cyclic_connection_epoch = + bus_runtime_ ? bus_runtime_->connectionEpoch() : 0U; + state.cyclic_stream_open = false; + state.cyclic_reconnect_latched = false; + state.cyclic_sequence = 0; + if (++state.cyclic_generation == 0U) { + ++state.cyclic_generation; + } + return; + } + if (state.requested_mode != mode) { + state.cyclic_stream_open = false; + } + state.requested_mode = mode; +} + +msgs::RunMode CmvrPlcMotorProtocol::getMode(const std::uint8_t node_id) +{ + CmvrPlcAxisStatus status; + if (bus_runtime_ && bus_runtime_->readAxisStatus(node_id, status)) { + const auto mode = static_cast(status.current_mode); + if (isSupportedMode_(mode)) { + return mode; + } + } + std::lock_guard lock(nodes_mutex_); + return nodeStateLocked_(node_id).requested_mode; +} + +void CmvrPlcMotorProtocol::setLimitQdd( + const std::uint8_t node_id, + const double u_qdd, + const double l_qdd) +{ + std::lock_guard lock(nodes_mutex_); + nodeStateLocked_(node_id).limit_qdd = std::max(std::abs(u_qdd), std::abs(l_qdd)); +} + +void CmvrPlcMotorProtocol::setLimitQd( + const std::uint8_t node_id, + const double qd) +{ + std::lock_guard lock(nodes_mutex_); + nodeStateLocked_(node_id).limit_qd = std::abs(qd); +} + +void CmvrPlcMotorProtocol::setLimitQ( + const std::uint8_t node_id, + const double ub, + const double lb) +{ + std::lock_guard lock(nodes_mutex_); + auto& state = nodeStateLocked_(node_id); + state.limit_q_ub = ub; + state.limit_q_lb = lb; +} + +bool CmvrPlcMotorProtocol::calibrateZeroQ(const std::uint8_t node_id) +{ + return submitSimple_(node_id, cmvr_plc::CommandCode::SetZero, true, + kSafetyCommandTimeoutMs); +} + +bool CmvrPlcMotorProtocol::reachedTargetQ(const std::uint8_t node_id) +{ + CmvrPlcAxisStatus status; + return readMotionStatus_(node_id, status) && + statusAllowsMotionFeedback(status) && + (status.status_flags & cmvr_plc::StatusFlag::TargetReached) != 0U; +} + +bool CmvrPlcMotorProtocol::commandProfilePosition( + const std::uint8_t node_id, + const double target_q, + const double max_qd, + const double max_qdd) +{ + NodeState state; + { + std::lock_guard lock(nodes_mutex_); + auto& stored = nodeStateLocked_(node_id); + stored.requested_mode = msgs::RUN_MODE_PROFILE_POSITION; + stored.cyclic_stream_open = false; + state = stored; + } + if (!validatePosition_(state, target_q) || + !validateVelocity_(state, max_qd) || + !validateAcceleration_(state, max_qdd)) { + CMVR_LOG(ERROR) << "[CmvrPlcMotorProtocol] invalid profile-position command"; + return false; + } + const auto position = toMicroUnits_(target_q); + const auto velocity = toMicroUnits_(std::abs(max_qd)); + const auto acceleration = toMicroUnits_(std::abs(max_qdd)); + if (!position || !velocity || !acceleration) { + return false; + } + + CmvrPlcAxisCommand command; + command.code = cmvr_plc::CommandCode::ProfilePosition; + command.target_position = *position; + command.target_velocity = *velocity; + command.acceleration = *acceleration; + command.command_timeout_ms = kProfileCommandTimeoutCeilingMs; + if (!bus_runtime_) { + return false; + } + const auto submission_epoch = bus_runtime_->connectionEpoch(); + const auto submitted = + bus_runtime_->submitAxisCommand( + node_id, command, false, submission_epoch); + const auto completed_epoch = bus_runtime_->connectionEpoch(); + if (!submitted && completed_epoch == submission_epoch) { + return false; + } + { + std::lock_guard lock(nodes_mutex_); + auto& stored = nodeStateLocked_(node_id); + // Bind on every cross-epoch outcome, including an ambiguous failed + // submit, so feedback remains invalid until safety cleanup. + stored.profile_feedback_bound = true; + stored.profile_connection_epoch = submission_epoch; + } + return submitted && completed_epoch == submission_epoch; +} + +bool CmvrPlcMotorProtocol::commandProfileVelocity( + const std::uint8_t node_id, + const double target_qd, + const double max_qdd) +{ + NodeState state; + { + std::lock_guard lock(nodes_mutex_); + auto& stored = nodeStateLocked_(node_id); + stored.requested_mode = msgs::RUN_MODE_PROFILE_VELOCITY; + stored.cyclic_stream_open = false; + state = stored; + } + if (!validateVelocity_(state, target_qd) || + !validateAcceleration_(state, max_qdd)) { + CMVR_LOG(ERROR) << "[CmvrPlcMotorProtocol] invalid profile-velocity command"; + return false; + } + const auto velocity = toMicroUnits_(target_qd); + const auto acceleration = toMicroUnits_(std::abs(max_qdd)); + if (!velocity || !acceleration) { + return false; + } + + CmvrPlcAxisCommand command; + command.code = cmvr_plc::CommandCode::ProfileVelocity; + command.target_velocity = *velocity; + command.acceleration = *acceleration; + command.command_timeout_ms = kProfileCommandTimeoutCeilingMs; + if (!bus_runtime_) { + return false; + } + const auto submission_epoch = bus_runtime_->connectionEpoch(); + const auto submitted = + bus_runtime_->submitAxisCommand( + node_id, command, false, submission_epoch); + const auto completed_epoch = bus_runtime_->connectionEpoch(); + if (!submitted && completed_epoch == submission_epoch) { + return false; + } + { + std::lock_guard lock(nodes_mutex_); + auto& stored = nodeStateLocked_(node_id); + stored.profile_feedback_bound = true; + stored.profile_connection_epoch = submission_epoch; + } + return submitted && completed_epoch == submission_epoch; +} + +bool CmvrPlcMotorProtocol::commandCyclicPosition( + const std::uint8_t node_id, + const double target_q, + const double target_qd) +{ + std::lock_guard lock(nodes_mutex_); + auto& state = nodeStateLocked_(node_id); + if (!validatePosition_(state, target_q) || + !validateVelocity_(state, target_qd) || + !ensureCyclicOpen_(node_id, state, msgs::RUN_MODE_CYCLIC_SYNC_POSITION)) { + return false; + } + const auto position = toMicroUnits_(target_q); + const auto velocity = toMicroUnits_(target_qd); + if (!position || !velocity) { + return false; + } + + CmvrPlcAxisCommand command; + command.code = cmvr_plc::CommandCode::CyclicPositionSample; + command.target_position = *position; + command.target_velocity = *velocity; + command.stream_watchdog_ms = bus_runtime_->streamWatchdogMs(); + if (++state.cyclic_sequence == 0) { + ++state.cyclic_sequence; + } + command.cyclic_sample_sequence = state.cyclic_sequence; + const auto submitted = + bus_runtime_->submitAxisCommand( + node_id, command, false, state.cyclic_connection_epoch); + if (submitted) { + state.profile_feedback_bound = false; + state.profile_connection_epoch = 0; + } else if ( + bus_runtime_->connectionEpoch() != state.cyclic_connection_epoch) { + state.cyclic_reconnect_latched = true; + state.cyclic_stream_open = false; + state.cyclic_sequence = 0; + state.cyclic_connection_epoch = bus_runtime_->connectionEpoch(); + } + return submitted; +} + +bool CmvrPlcMotorProtocol::commandCyclicVelocity( + const std::uint8_t node_id, + const double target_qd) +{ + std::lock_guard lock(nodes_mutex_); + auto& state = nodeStateLocked_(node_id); + if (!validateVelocity_(state, target_qd) || + !ensureCyclicOpen_(node_id, state, msgs::RUN_MODE_CYCLIC_SYNC_VELOCITY)) { + return false; + } + const auto velocity = toMicroUnits_(target_qd); + if (!velocity) { + return false; + } + + CmvrPlcAxisCommand command; + command.code = cmvr_plc::CommandCode::CyclicVelocitySample; + command.target_velocity = *velocity; + command.stream_watchdog_ms = bus_runtime_->streamWatchdogMs(); + if (++state.cyclic_sequence == 0) { + ++state.cyclic_sequence; + } + command.cyclic_sample_sequence = state.cyclic_sequence; + const auto submitted = + bus_runtime_->submitAxisCommand( + node_id, command, false, state.cyclic_connection_epoch); + if (submitted) { + state.profile_feedback_bound = false; + state.profile_connection_epoch = 0; + } else if ( + bus_runtime_->connectionEpoch() != state.cyclic_connection_epoch) { + state.cyclic_reconnect_latched = true; + state.cyclic_stream_open = false; + state.cyclic_sequence = 0; + state.cyclic_connection_epoch = bus_runtime_->connectionEpoch(); + } + return submitted; +} + +bool CmvrPlcMotorProtocol::commandCyclicTorque( + const std::uint8_t node_id, + const double target_tau) +{ + (void)target_tau; + CMVR_LOG(ERROR) << "[CmvrPlcMotorProtocol] cyclic torque is unsupported, motor=" + << static_cast(node_id); + return false; +} + +void CmvrPlcMotorProtocol::setMotorConversion( + const std::uint8_t node_id, + const double encoder_counts_per_rev, + const double gear_ratio) +{ + (void)node_id; + (void)encoder_counts_per_rev; + (void)gear_ratio; + // CMVR PLC v1 exchanges SI quantities in fixed-point micro-units. +} + +bool CmvrPlcMotorProtocol::torqueOn(const std::uint8_t node_id) +{ + return submitSimple_(node_id, cmvr_plc::CommandCode::Enable, true, + kSafetyCommandTimeoutMs); +} + +bool CmvrPlcMotorProtocol::torqueOff(const std::uint8_t node_id) +{ + CmvrPlcAxisCommand command; + command.code = cmvr_plc::CommandCode::Disable; + command.command_timeout_ms = kSafetyCommandTimeoutMs; + const auto result = + bus_runtime_ && bus_runtime_->submitAxisSafetyCommand(node_id, command); + std::lock_guard lock(nodes_mutex_); + auto& state = nodeStateLocked_(node_id); + state.cyclic_stream_open = false; + if (result) { + state.profile_feedback_bound = false; + state.profile_connection_epoch = 0; + } + return result; +} + +bool CmvrPlcMotorProtocol::brakeRelease(const std::uint8_t node_id) +{ + CMVR_LOG(ERROR) << "[CmvrPlcMotorProtocol] brake release is unsupported, motor=" + << static_cast(node_id); + return false; +} + +bool CmvrPlcMotorProtocol::quickStop(const std::uint8_t node_id) +{ + CmvrPlcAxisCommand command; + command.code = cmvr_plc::CommandCode::QuickStop; + command.command_timeout_ms = kSafetyCommandTimeoutMs; + const auto result = + bus_runtime_ && bus_runtime_->submitAxisSafetyCommand(node_id, command); + std::lock_guard lock(nodes_mutex_); + auto& state = nodeStateLocked_(node_id); + state.cyclic_stream_open = false; + if (result) { + state.profile_feedback_bound = false; + state.profile_connection_epoch = 0; + } + return result; +} + +double CmvrPlcMotorProtocol::getQ(const std::uint8_t node_id) +{ + CmvrPlcAxisStatus status; + if (!readMotionStatus_(node_id, status) || + !statusAllowsMotionFeedback(status)) { + return std::numeric_limits::quiet_NaN(); + } + return fromMicroUnits_(status.actual_position); +} + +double CmvrPlcMotorProtocol::getQd(const std::uint8_t node_id) +{ + CmvrPlcAxisStatus status; + if (!readMotionStatus_(node_id, status) || + !statusAllowsMotionFeedback(status)) { + return std::numeric_limits::quiet_NaN(); + } + return fromMicroUnits_(status.actual_velocity); +} + +bool CmvrPlcMotorProtocol::readMotionStatus_( + const std::uint8_t node_id, + CmvrPlcAxisStatus& status) +{ + if (!bus_runtime_) { + return false; + } + std::lock_guard lock(nodes_mutex_); + const auto& state = nodeStateLocked_(node_id); + const auto read_epoch = bus_runtime_->connectionEpoch(); + if (state.profile_feedback_bound && + state.profile_connection_epoch != read_epoch) { + return false; + } + if (!bus_runtime_->readAxisStatus(node_id, status)) { + return false; + } + // Reject a status transaction that straddled a disconnect/reconnect even + // when no profile binding was active at the first check. + if (bus_runtime_->connectionEpoch() != read_epoch) { + return false; + } + return !state.profile_feedback_bound || + state.profile_connection_epoch == read_epoch; +} + +bool CmvrPlcMotorProtocol::submitSimple_( + const std::uint8_t node_id, + const cmvr_plc::CommandCode code, + const bool wait_for_terminal, + const std::uint32_t timeout_ms) +{ + CmvrPlcAxisCommand command; + command.code = code; + command.command_timeout_ms = timeout_ms; + return bus_runtime_ && + bus_runtime_->submitAxisCommand(node_id, command, wait_for_terminal); +} + +bool CmvrPlcMotorProtocol::ensureCyclicOpen_( + const std::uint8_t node_id, + NodeState& state, + const msgs::RunMode mode) +{ + if (!bus_runtime_) { + return false; + } + const auto connection_epoch = bus_runtime_->connectionEpoch(); + if (state.cyclic_connection_epoch != connection_epoch) { + // A cyclic generation is bound to the PLC ownership epoch in which it + // was created. Never reinterpret a setpoint from that generation as + // the first setpoint of a freshly reconnected PLC stream. + if (state.cyclic_generation != 0U) { + state.cyclic_reconnect_latched = true; + } + state.cyclic_connection_epoch = connection_epoch; + state.cyclic_stream_open = false; + state.cyclic_sequence = 0; + } + if (state.cyclic_reconnect_latched) { + CMVR_LOG(WARNING) + << "[CmvrPlcMotorProtocol] cyclic stream crossed a PLC session; " + "explicit setMode from a new upper-layer stream is required, motor=" + << static_cast(node_id); + return false; + } + if (state.cyclic_stream_open && state.requested_mode == mode) { + return true; + } + + // Preserve direct protocol use that does not call setMode explicitly: + // its first successful Open still establishes a generation. Once that + // generation has observed a reconnect, only explicit setMode can recover. + if (state.cyclic_generation == 0U) { + state.cyclic_generation = 1U; + state.cyclic_connection_epoch = connection_epoch; + } + + CmvrPlcAxisCommand command; + command.code = mode == msgs::RUN_MODE_CYCLIC_SYNC_POSITION + ? cmvr_plc::CommandCode::OpenCyclicPosition + : cmvr_plc::CommandCode::OpenCyclicVelocity; + command.stream_watchdog_ms = bus_runtime_->streamWatchdogMs(); + command.command_timeout_ms = kSafetyCommandTimeoutMs; + if (!bus_runtime_->submitAxisCommand( + node_id, command, false, state.cyclic_connection_epoch)) { + if (bus_runtime_->connectionEpoch() != + state.cyclic_connection_epoch) { + state.cyclic_reconnect_latched = true; + state.cyclic_stream_open = false; + state.cyclic_sequence = 0; + state.cyclic_connection_epoch = + bus_runtime_->connectionEpoch(); + } + return false; + } + state.requested_mode = mode; + state.cyclic_stream_open = true; + state.cyclic_sequence = 0; + return true; +} + +std::optional CmvrPlcMotorProtocol::toMicroUnits_(const double value) +{ + if (!std::isfinite(value)) { + return std::nullopt; + } + const auto scaled = std::round(value * cmvr_plc::kPositionScale); + if (scaled < static_cast(std::numeric_limits::min()) || + scaled > static_cast(std::numeric_limits::max())) { + return std::nullopt; + } + return static_cast(scaled); +} + +double CmvrPlcMotorProtocol::fromMicroUnits_(const std::int32_t value) +{ + return static_cast(value) / cmvr_plc::kPositionScale; +} + +bool CmvrPlcMotorProtocol::isSupportedMode_(const msgs::RunMode mode) +{ + return mode == msgs::RUN_MODE_PROFILE_POSITION || + mode == msgs::RUN_MODE_PROFILE_VELOCITY || + mode == msgs::RUN_MODE_CYCLIC_SYNC_POSITION || + mode == msgs::RUN_MODE_CYCLIC_SYNC_VELOCITY; +} + +bool CmvrPlcMotorProtocol::validatePosition_( + const NodeState& state, + const double position) const +{ + return std::isfinite(position) && + (state.limit_q_ub <= state.limit_q_lb || + (position >= state.limit_q_lb && position <= state.limit_q_ub)); +} + +bool CmvrPlcMotorProtocol::validateVelocity_( + const NodeState& state, + const double velocity) const +{ + return std::isfinite(velocity) && + (state.limit_qd <= 0.0 || std::abs(velocity) <= state.limit_qd); +} + +bool CmvrPlcMotorProtocol::validateAcceleration_( + const NodeState& state, + const double acceleration) const +{ + return std::isfinite(acceleration) && acceleration >= 0.0 && + (state.limit_qdd <= 0.0 || acceleration <= state.limit_qdd); +} + +CmvrPlcMotorProtocol::NodeState& CmvrPlcMotorProtocol::nodeStateLocked_( + const std::uint8_t node_id) +{ + return nodes_[node_id]; +} + +} // namespace cmvr::device diff --git a/cmvr-es/devices/motor/drivers/modbus_plc_motor/src/modbus_plc_motor.cpp b/cmvr-es/devices/motor/drivers/modbus_plc_motor/src/modbus_plc_motor.cpp new file mode 100644 index 00000000..faa9952a --- /dev/null +++ b/cmvr-es/devices/motor/drivers/modbus_plc_motor/src/modbus_plc_motor.cpp @@ -0,0 +1,55 @@ +#include "devices/motor/drivers/modbus_plc_motor/include/modbus_plc_motor.h" + +#include "common/base/logging/logger.h" +#include "devices/motor/drivers/modbus_plc_motor/include/cmvr_plc_motor_protocol.h" +#include + +namespace cmvr::device { + +ModbusPlcMotor::ModbusPlcMotor(const config::MotorConfigItem& config) +{ + info_.id = config.id(); + info_.joint_name = config.joint_name(); + info_.limit_q_lb = config.limit_q_lb(); + info_.limit_q_ub = config.limit_q_ub(); + info_.limit_qd = config.limit_qd(); + info_.limit_qdd = config.limit_qdd(); + node_id_ = static_cast(config.id()); +} + +bool ModbusPlcMotor::init() +{ + if (!protocol_ || + !std::dynamic_pointer_cast(protocol_)) { + CMVR_LOG(ERROR) << "[ModbusPlcMotor] invalid CMVR PLC protocol for " + << info_.joint_name; + return false; + } + if (!std::isfinite(info_.limit_q_lb) || !std::isfinite(info_.limit_q_ub) || + !std::isfinite(info_.limit_qd) || !std::isfinite(info_.limit_qdd) || + info_.limit_q_ub <= info_.limit_q_lb || + info_.limit_qd <= 0.0 || info_.limit_qdd <= 0.0) { + CMVR_LOG(ERROR) << "[ModbusPlcMotor] finite position/velocity/acceleration " + "limits are required for " + << info_.joint_name; + return false; + } + setLimitQ(info_.limit_q_ub, info_.limit_q_lb); + setLimitQd(info_.limit_qd); + setLimitQdd(info_.limit_qdd, -info_.limit_qdd); + return true; +} + +bool ModbusPlcMotor::torqueOff() +{ + auto protocol = std::dynamic_pointer_cast(protocol_); + return protocol && protocol->torqueOff(node_id_); +} + +bool ModbusPlcMotor::quickStop() +{ + auto protocol = std::dynamic_pointer_cast(protocol_); + return protocol && protocol->quickStop(node_id_); +} + +} // namespace cmvr::device diff --git a/cmvr-es/devices/motor/manager/CMakeLists.txt b/cmvr-es/devices/motor/manager/CMakeLists.txt index 8ea5f495..46f66040 100644 --- a/cmvr-es/devices/motor/manager/CMakeLists.txt +++ b/cmvr-es/devices/motor/manager/CMakeLists.txt @@ -13,6 +13,7 @@ target_link_libraries(motor_manager cmvr_es::device::ti5_canopen_motor_driver cmvr_es::device::mujoco_motor_driver cmvr_es::device::ethercat_motor_driver + cmvr_es::device::modbus_plc_motor_driver cmvr_es::ik_solver glog ) diff --git a/cmvr-es/devices/motor/manager/include/motor_manager.h b/cmvr-es/devices/motor/manager/include/motor_manager.h index 3bbd2cab..e67a263e 100644 --- a/cmvr-es/devices/motor/manager/include/motor_manager.h +++ b/cmvr-es/devices/motor/manager/include/motor_manager.h @@ -78,6 +78,10 @@ private: const config::MotorGroupConfig& group_cfg, const std::vector& motor_cfgs, const std::shared_ptr& bus_runtime) const; + std::vector> createModbusTcpMotors_( + const config::MotorGroupConfig& group_cfg, + const std::vector& motor_cfgs, + const std::shared_ptr& bus_runtime) const; private: config::MotorConfig cfg_; diff --git a/cmvr-es/devices/motor/manager/src/motor_manager.cpp b/cmvr-es/devices/motor/manager/src/motor_manager.cpp index 5e83b043..ff2f37ff 100644 --- a/cmvr-es/devices/motor/manager/src/motor_manager.cpp +++ b/cmvr-es/devices/motor/manager/src/motor_manager.cpp @@ -15,11 +15,14 @@ #include "devices/motor/bus_runtime/can/include/can_motor_bus_runtime.h" #include "devices/motor/bus_runtime/ethercat/include/ethercat_motor_bus_runtime.h" #include "devices/motor/bus_runtime/mujoco/include/mujoco_motor_bus_runtime.h" +#include "devices/motor/bus_runtime/modbus_tcp/include/modbus_tcp_motor_bus_runtime.h" #include "devices/motor/drivers/ethercat_motor/include/cia402/cia402_protocol.h" #include "devices/motor/drivers/ethercat_motor/include/vendor/eyou/eyou_cia402_pdo_mapping.h" #include "devices/motor/drivers/ethercat_motor/include/vendor/eyou/eyou_motor.h" #include "devices/motor/drivers/ethercat_motor/include/vendor/eyou/eyou_motor_adapter.h" #include "devices/motor/drivers/mujoco/include/mujoco_motor.h" +#include "devices/motor/drivers/modbus_plc_motor/include/cmvr_plc_motor_protocol.h" +#include "devices/motor/drivers/modbus_plc_motor/include/modbus_plc_motor.h" #include "devices/motor/drivers/ti5_canopen/include/ti5_motor.h" #include "devices/motor/drivers/ti5_canopen/include/ti5_motor_canopen_protocol.h" @@ -415,6 +418,16 @@ std::shared_ptr MotorManager::createBusRuntime_( return std::make_shared(); case config::MOTOR_BUS_MUJOCO: return std::make_shared(); + case config::MOTOR_BUS_MODBUS_TCP: + if (group_cfg.vendor() == config::MOTOR_VENDOR_PLC_GENERIC && + group_cfg.protocol() == config::MOTOR_PROTOCOL_CMVR_PLC_V1) { + return std::make_shared(); + } + CMVR_LOG(ERROR) << "[MotorManager] unsupported Modbus TCP motor: vendor=" + << config::MotorVendor_Name(group_cfg.vendor()) + << ", protocol=" << config::MotorProtocol_Name(group_cfg.protocol()) + << ", group=" << group_cfg.id(); + return nullptr; case config::MOTOR_BUS_ETHERCAT: { auto runtime = std::make_shared(); if (group_cfg.vendor() == config::MOTOR_VENDOR_EYOU && @@ -454,6 +467,8 @@ std::vector> MotorManager::createMotors_( return createMujocoMotors_(group_cfg, motor_cfgs, bus_runtime); case config::MOTOR_BUS_ETHERCAT: return createEthercatMotors_(group_cfg, motor_cfgs, bus_runtime); + case config::MOTOR_BUS_MODBUS_TCP: + return createModbusTcpMotors_(group_cfg, motor_cfgs, bus_runtime); default: CMVR_LOG(ERROR) << "[MotorManager] unsupported motor bus type: " << config::MotorBusType_Name(group_cfg.bus_type()) @@ -596,4 +611,50 @@ std::vector> MotorManager::createEthercatMotors_( return motors; } +std::vector> MotorManager::createModbusTcpMotors_( + const config::MotorGroupConfig& group_cfg, + const std::vector& motor_cfgs, + const std::shared_ptr& bus_runtime) const +{ + auto modbus_runtime = + std::dynamic_pointer_cast(bus_runtime); + if (!modbus_runtime || !group_cfg.has_modbus_tcp()) { + CMVR_LOG(ERROR) << "[MotorManager] missing Modbus TCP runtime/config: " + << group_cfg.id(); + return {}; + } + if (group_cfg.vendor() != config::MOTOR_VENDOR_PLC_GENERIC || + group_cfg.protocol() != config::MOTOR_PROTOCOL_CMVR_PLC_V1) { + CMVR_LOG(ERROR) << "[MotorManager] unsupported Modbus TCP PLC motor: vendor=" + << config::MotorVendor_Name(group_cfg.vendor()) + << ", protocol=" << config::MotorProtocol_Name(group_cfg.protocol()); + return {}; + } + for (const auto& motor_cfg : motor_cfgs) { + if (motor_cfg.id() < 0 || motor_cfg.id() > 255 || + !modbus_runtime->hasMotor(static_cast(motor_cfg.id()))) { + CMVR_LOG(ERROR) << "[MotorManager] missing PLC axis mapping for motor id " + << motor_cfg.id() << " in group: " << group_cfg.id(); + return {}; + } + } + + auto protocol = std::make_shared(modbus_runtime); + std::vector> motors; + motors.reserve(motor_cfgs.size()); + for (const auto& cfg : motor_cfgs) { + auto motor = std::make_shared(cfg); + motor->setProtocol(protocol); + // initNode() and ModbusPlcMotor::init() only establish local mappings and + // limits. The shared TCP connection starts after all motors are created. + if (!motor->init()) { + CMVR_LOG(ERROR) << "[MotorManager] failed to init Modbus PLC motor: " + << cfg.joint_name(); + return {}; + } + motors.push_back(std::move(motor)); + } + return motors; +} + } // namespace cmvr::device diff --git a/cmvr-es/service/CMakeLists.txt b/cmvr-es/service/CMakeLists.txt index 24fad685..b46b443b 100644 --- a/cmvr-es/service/CMakeLists.txt +++ b/cmvr-es/service/CMakeLists.txt @@ -7,6 +7,7 @@ add_library(service grpc/src/grpc_head_service.cpp grpc/src/grpc_dexhand_service.cpp grpc/src/grpc_arm_service.cpp + grpc/src/grpc_motor_service.cpp grpc/src/grpc_agv_service.cpp grpc/src/grpc_hlc_service.cpp ../task/grpc_server_task/src/grpc_server_task.cpp @@ -42,6 +43,76 @@ if(BUILD_TESTING) COMMAND grpc_camera_stream_policy_test ) set_tests_properties(grpc_camera_stream_policy_test PROPERTIES TIMEOUT 10) + + add_executable(grpc_motor_service_test + grpc/tests/grpc_motor_service_test.cpp + ) + target_include_directories(grpc_motor_service_test + PRIVATE + ${CMAKE_SOURCE_DIR}/cmvr-es + ${CMAKE_SOURCE_DIR}/cmvr-es/manager/device_manager + ) + target_link_libraries(grpc_motor_service_test + PRIVATE + service + gtest + gtest_main + pthread + ) + add_test( + NAME grpc_motor_service_test + COMMAND grpc_motor_service_test + ) + set(_grpc_motor_test_environment + "LD_LIBRARY_PATH=${CMVR_TEST_EXTERNAL_LIBRARY_PATH}" + ) + if(CMVR_TEST_SYSTEM_LIBSTDCXX) + list(APPEND _grpc_motor_test_environment + "LD_PRELOAD=${CMVR_TEST_SYSTEM_LIBSTDCXX}") + endif() + set_tests_properties(grpc_motor_service_test PROPERTIES + TIMEOUT 10 + ENVIRONMENT "${_grpc_motor_test_environment}" + ) + + set(_grpc_motor_modbus_e2e_libmodbus_root + "${CMAKE_SOURCE_DIR}/dependency/${ARCH}/third_party/modbus/3.1.11") + add_executable(grpc_motor_service_modbus_e2e_test + grpc/tests/grpc_motor_service_modbus_e2e_test.cpp + ) + target_include_directories(grpc_motor_service_modbus_e2e_test + PRIVATE + ${CMAKE_SOURCE_DIR}/cmvr-es + ${CMAKE_SOURCE_DIR}/cmvr-es/manager/device_manager + ${_grpc_motor_modbus_e2e_libmodbus_root}/include + ) + target_link_directories(grpc_motor_service_modbus_e2e_test + PRIVATE + ${_grpc_motor_modbus_e2e_libmodbus_root}/lib + ) + target_link_libraries(grpc_motor_service_modbus_e2e_test + PRIVATE + service + modbus + gtest + gtest_main + pthread + ) + add_test( + NAME grpc_motor_service_modbus_e2e_test + COMMAND grpc_motor_service_modbus_e2e_test + ) + set(_grpc_motor_modbus_e2e_environment + "LD_LIBRARY_PATH=${CMVR_TEST_EXTERNAL_LIBRARY_PATH}" + ) + if(CMVR_TEST_SYSTEM_LIBSTDCXX) + list(APPEND _grpc_motor_modbus_e2e_environment + "LD_PRELOAD=${CMVR_TEST_SYSTEM_LIBSTDCXX}") + endif() + set_tests_properties(grpc_motor_service_modbus_e2e_test PROPERTIES + TIMEOUT 20 + ENVIRONMENT "${_grpc_motor_modbus_e2e_environment}" + ) endif() # -------------------------------------------------------- diff --git a/cmvr-es/service/grpc/include/grpc_motor_service.h b/cmvr-es/service/grpc/include/grpc_motor_service.h new file mode 100644 index 00000000..25137859 --- /dev/null +++ b/cmvr-es/service/grpc/include/grpc_motor_service.h @@ -0,0 +1,197 @@ +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +#include "cmvr/api/motor_service.grpc.pb.h" +#include "devices/motor/abstract_motor.h" + +namespace cmvr::device { +class DeviceManager; +} + +namespace cmvr::service { + +class gRPCMotorServiceImplTestAccess; + +// A deliberately thin synchronous gRPC facade over AbstractMotor. It does not +// schedule trajectories or retain asynchronous operations. The small amount of +// state below only prevents two RPCs from owning one motor at the same time and +// lets emergencyStop invalidate an already-running blocking RPC/stream. +class gRPCMotorServiceImpl final : public api::MotorService::Service { +public: + gRPCMotorServiceImpl(); + ~gRPCMotorServiceImpl() override = default; + + grpc::Status setZero(grpc::ServerContext* context, + const api::SetMotorZeroRequest* request, + api::MotorCommandResponse* response) override; + grpc::Status moveToZero(grpc::ServerContext* context, + const api::MoveMotorToZeroRequest* request, + api::MotorCommandResponse* response) override; + grpc::Status profilePosition(grpc::ServerContext* context, + const api::ProfilePositionRequest* request, + api::MotorCommandResponse* response) override; + grpc::Status profileVelocity(grpc::ServerContext* context, + const api::ProfileVelocityRequest* request, + api::MotorCommandResponse* response) override; + grpc::Status streamCyclicPosition( + grpc::ServerContext* context, + grpc::ServerReaderWriter* stream) override; + grpc::Status streamCyclicVelocity( + grpc::ServerContext* context, + grpc::ServerReaderWriter* stream) override; + grpc::Status emergencyStop(grpc::ServerContext* context, + const api::EmergencyStopRequest* request, + api::MotorCommandResponse* response) override; + grpc::Status getStatus(grpc::ServerContext* context, + const api::GetMotorStatusRequest* request, + api::GetMotorStatusResponse* response) override; + grpc::Status setEnabled(grpc::ServerContext* context, + const api::SetMotorEnabledRequest* request, + api::MotorCommandResponse* response) override; + +private: + friend class gRPCMotorServiceImplTestAccess; + + struct MotorControlState { + std::mutex mutex; + // Serializes all motion/enable writes with emergency quick-stop. The + // cancel-generation check and the corresponding motor write must occur + // while this mutex is held to prevent stale writes after an E-stop. + std::mutex command_mutex; + // Keeps the two-phase best-effort/final quick-stop sequence exclusive. + // Without this, one concurrent E-stop could clear the shared + // in-progress flag while another E-stop is still dispatching. + std::mutex emergency_mutex; + // Serializes exception cleanup from ownership inspection through the + // final release. A second stale cleanup must re-check ownership only + // after the first cleanup has fully completed. + std::mutex exception_cleanup_mutex; + bool busy{false}; + bool emergency_stopped{false}; + bool emergency_stop_in_progress{false}; + bool exception_cleanup_pending{false}; + std::uint64_t cancel_generation{0}; + api::MotorControlType active_control{api::MOTOR_CONTROL_NONE}; + std::string last_error; + }; + + struct ResolvedMotor { + std::shared_ptr motor; + std::shared_ptr control; + }; + + struct MotorControlEntry { + std::weak_ptr owner; + std::shared_ptr state; + }; + + class ControlLease { + public: + ControlLease(std::shared_ptr state, + std::uint64_t generation); + ~ControlLease(); + ControlLease(const ControlLease&) = delete; + ControlLease& operator=(const ControlLease&) = delete; + + std::uint64_t generation() const noexcept { return generation_; } + + private: + std::shared_ptr state_; + std::uint64_t generation_{0}; + int uncaught_on_entry_{0}; + }; + + grpc::Status resolveMotor(const api::MotorTarget& target, + ResolvedMotor& resolved) const; + std::shared_ptr stateFor( + const std::shared_ptr& motor) const; + std::unique_ptr acquireControl( + const ResolvedMotor& resolved, + api::MotorControlType control, + grpc::Status& failure, + bool allow_emergency_stopped = false) const; + + grpc::Status runProfilePosition(grpc::ServerContext* context, + const ResolvedMotor& resolved, + double target_position_rad, + double max_velocity_rad_s, + double acceleration_rad_s2, + const api::MotorWaitOptions& wait, + api::MotorCommandResponse* response); + grpc::Status waitForPosition(grpc::ServerContext* context, + const ResolvedMotor& resolved, + std::uint64_t generation, + double target_position_rad, + const api::MotorWaitOptions& wait, + api::MotorCommandResponse* response, + std::chrono::steady_clock::time_point started); + grpc::Status waitForVelocity(grpc::ServerContext* context, + const ResolvedMotor& resolved, + std::uint64_t generation, + double target_velocity_rad_s, + const api::MotorWaitOptions& wait, + api::MotorCommandResponse* response, + std::chrono::steady_clock::time_point started); + + grpc::Status setZeroImpl(grpc::ServerContext* context, + const api::SetMotorZeroRequest* request, + api::MotorCommandResponse* response); + grpc::Status moveToZeroImpl(grpc::ServerContext* context, + const api::MoveMotorToZeroRequest* request, + api::MotorCommandResponse* response); + grpc::Status profilePositionImpl( + grpc::ServerContext* context, + const api::ProfilePositionRequest* request, + api::MotorCommandResponse* response); + grpc::Status profileVelocityImpl( + grpc::ServerContext* context, + const api::ProfileVelocityRequest* request, + api::MotorCommandResponse* response); + grpc::Status emergencyStopImpl( + grpc::ServerContext* context, + const api::EmergencyStopRequest* request, + api::MotorCommandResponse* response); + grpc::Status getStatusImpl(grpc::ServerContext* context, + const api::GetMotorStatusRequest* request, + api::GetMotorStatusResponse* response); + grpc::Status setEnabledImpl( + grpc::ServerContext* context, + const api::SetMotorEnabledRequest* request, + api::MotorCommandResponse* response); + grpc::Status streamCyclicPositionImpl( + grpc::ServerContext* context, + grpc::ServerReaderWriter* stream, + std::optional& cleanup_target); + grpc::Status streamCyclicVelocityImpl( + grpc::ServerContext* context, + grpc::ServerReaderWriter* stream, + std::optional& cleanup_target); + + void bestEffortQuickStop(const api::MotorTarget& target, + const std::string& error) noexcept; + void latchUnsafeAfterFailedStop( + const std::shared_ptr& state, + const std::string& error) const; + void fillMotorStatus(const ResolvedMotor& resolved, + api::MotorStatus* status) const; + void setLastError(const std::shared_ptr& state, + const std::string& error) const; + + device::DeviceManager& dmgr_; + mutable std::mutex states_mutex_; + mutable std::unordered_map states_; +}; + +} // namespace cmvr::service diff --git a/cmvr-es/service/grpc/src/grpc_motor_service.cpp b/cmvr-es/service/grpc/src/grpc_motor_service.cpp new file mode 100644 index 00000000..1e2f01f4 --- /dev/null +++ b/cmvr-es/service/grpc/src/grpc_motor_service.cpp @@ -0,0 +1,2051 @@ +#include "service/grpc/include/grpc_motor_service.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include "common/base/logging/logger.h" +#include "devices/motor/manager/include/motor_manager.h" +#include "manager/device_manager/include/device_manager.h" + +namespace cmvr::service { + +namespace { + +using Clock = std::chrono::steady_clock; +using google::protobuf::util::TimeUtil; + +constexpr std::uint32_t kDefaultCommandTimeoutMs = 30000; +constexpr std::uint32_t kDefaultPollPeriodMs = 10; +constexpr std::uint32_t kDefaultSettleSamples = 3; +constexpr double kDefaultPositionToleranceRad = 1e-3; +constexpr double kDefaultVelocityToleranceRadS = 1e-2; +constexpr std::uint32_t kDefaultStreamWatchdogMs = 500; + +void fillFeedback(api::CommandHeader_Feedback* feedback, + const bool success, + const std::string& error = {}) +{ + feedback->set_success(success); + feedback->set_error_message(error); + *feedback->mutable_timestamp() = TimeUtil::GetCurrentTime(); +} + +std::uint64_t elapsedMs(const Clock::time_point started) +{ + return static_cast( + std::chrono::duration_cast(Clock::now() - started).count()); +} + +bool isFinite(const double value) +{ + return std::isfinite(value); +} + +std::uint32_t commandTimeoutMs(const api::MotorWaitOptions& options) +{ + const auto requested = options.timeout_ms(); + return requested == 0 ? kDefaultCommandTimeoutMs + : std::clamp(requested, 1, 600000); +} + +std::uint32_t pollPeriodMs(const api::MotorWaitOptions& options) +{ + const auto requested = options.poll_period_ms(); + return requested == 0 ? kDefaultPollPeriodMs + : std::clamp(requested, 1, 1000); +} + +std::uint32_t settleSamples(const api::MotorWaitOptions& options) +{ + const auto requested = options.settle_sample_count(); + return requested == 0 ? kDefaultSettleSamples + : std::clamp(requested, 1, 1000); +} + +double positionTolerance(const api::MotorWaitOptions& options) +{ + return options.position_tolerance_rad() > 0.0 + ? options.position_tolerance_rad() + : kDefaultPositionToleranceRad; +} + +double velocityTolerance(const api::MotorWaitOptions& options) +{ + return options.velocity_tolerance_rad_s() > 0.0 + ? options.velocity_tolerance_rad_s() + : kDefaultVelocityToleranceRadS; +} + +grpc::Status validateWaitOptions(const api::MotorWaitOptions& options) +{ + const double position_tolerance = options.position_tolerance_rad(); + if (position_tolerance != 0.0 && + (!isFinite(position_tolerance) || position_tolerance <= 0.0)) { + return grpc::Status( + grpc::StatusCode::INVALID_ARGUMENT, + "position_tolerance_rad must be finite and positive when specified"); + } + const double velocity_tolerance = options.velocity_tolerance_rad_s(); + if (velocity_tolerance != 0.0 && + (!isFinite(velocity_tolerance) || velocity_tolerance <= 0.0)) { + return grpc::Status( + grpc::StatusCode::INVALID_ARGUMENT, + "velocity_tolerance_rad_s must be finite and positive when specified"); + } + return grpc::Status::OK; +} + +grpc::Status cancelledStatus(grpc::ServerContext* context) +{ + if (std::chrono::system_clock::now() >= context->deadline()) { + return grpc::Status(grpc::StatusCode::DEADLINE_EXCEEDED, + "motor command gRPC deadline exceeded"); + } + return grpc::Status(grpc::StatusCode::CANCELLED, "motor command cancelled"); +} + +template +void sleepInterruptibly(grpc::ServerContext* context, + const Clock::time_point wake_time, + IsPreempted&& is_preempted) +{ + constexpr auto kCancellationSlice = std::chrono::milliseconds(10); + while (Clock::now() < wake_time) { + if (context->IsCancelled() || + std::chrono::system_clock::now() >= context->deadline() || + is_preempted()) { + return; + } + const auto remaining = wake_time - Clock::now(); + std::this_thread::sleep_for(std::min( + std::chrono::duration_cast(kCancellationSlice), + remaining)); + } +} + +class ScopeExit final { +public: + explicit ScopeExit(std::function callback) + : callback_(std::move(callback)) + { + } + + ~ScopeExit() noexcept + { + if (!callback_) { + return; + } + try { + callback_(); + } catch (...) { + } + } + + void release() noexcept { callback_ = {}; } + +private: + std::function callback_; +}; + +template +grpc::Status runUnaryGuarded(Response* response, + const char* rpc_name, + Body&& body, + Cleanup&& cleanup) +{ + try { + return body(); + } catch (const std::exception& e) { + const std::string error = + std::string(rpc_name) + " backend exception: " + e.what(); + try { + cleanup(error); + } catch (...) { + } + response->Clear(); + fillFeedback(response->mutable_header(), false, error); + return grpc::Status(grpc::StatusCode::INTERNAL, error); + } catch (...) { + const std::string error = + std::string(rpc_name) + " backend exception: unknown exception"; + try { + cleanup(error); + } catch (...) { + } + response->Clear(); + fillFeedback(response->mutable_header(), false, error); + return grpc::Status(grpc::StatusCode::INTERNAL, error); + } +} + +template +grpc::Status runStreamingGuarded(const char* rpc_name, + Body&& body, + Cleanup&& cleanup) +{ + try { + return body(); + } catch (const std::exception& e) { + const std::string error = + std::string(rpc_name) + " backend exception: " + e.what(); + try { + cleanup(error); + } catch (...) { + } + return grpc::Status(grpc::StatusCode::INTERNAL, error); + } catch (...) { + const std::string error = + std::string(rpc_name) + " backend exception: unknown exception"; + try { + cleanup(error); + } catch (...) { + } + return grpc::Status(grpc::StatusCode::INTERNAL, error); + } +} + +template +grpc::Status runCyclicLoop( + grpc::ServerContext* context, + grpc::ServerReaderWriter* stream, + const std::uint64_t generation, + std::uint32_t watchdog_timeout_ms, + Apply&& apply, + FillStatus&& fill_status, + IsPreempted&& is_preempted, + SetLastError&& set_last_error, + Stop&& stop) +{ + watchdog_timeout_ms = watchdog_timeout_ms == 0 + ? kDefaultStreamWatchdogMs + : std::clamp( + watchdog_timeout_ms, 20, 60000); + + struct InputSlot { + std::mutex mutex; + std::condition_variable cv; + std::optional pending; + bool ended{false}; + bool reader_failed{false}; + std::uint64_t dropped{0}; + Clock::time_point last_receive{Clock::now()}; + } input; + + api::CyclicControlResponse opened; + fillFeedback(opened.mutable_header(), true); + opened.set_phase(api::CYCLIC_STREAM_OPENED); + fill_status(opened.mutable_status()); + if (!stream->Write(opened)) { + try { + stop(); + } catch (...) { + } + return grpc::Status(grpc::StatusCode::CANCELLED, + "cyclic stream closed while opening"); + } + + std::thread reader; + bool reader_joined = false; + const auto safeStop = [&]() noexcept { + try { + return stop(); + } catch (...) { + return false; + } + }; + const auto joinReader = [&](const bool cancel_context) noexcept { + if (cancel_context) { + bool ended = false; + try { + std::unique_lock lock(input.mutex); + input.cv.wait_for( + lock, std::chrono::milliseconds(50), [&]() { + return input.ended; + }); + ended = input.ended; + } catch (...) { + } + if (!ended) { + context->TryCancel(); + } + } + input.cv.notify_all(); + if (reader.joinable()) { + try { + reader.join(); + } catch (...) { + } + } + reader_joined = true; + }; + ScopeExit reader_guard([&]() { + if (!reader_joined) { + safeStop(); + joinReader(true); + } + }); + try { + reader = std::thread([&]() { + try { + Request incoming; + while (stream->Read(&incoming)) { + { + std::lock_guard lock(input.mutex); + if (input.pending.has_value()) { + ++input.dropped; + } + input.pending = std::move(incoming); + input.last_receive = Clock::now(); + } + input.cv.notify_one(); + incoming.Clear(); + } + } catch (...) { + std::lock_guard lock(input.mutex); + input.reader_failed = true; + } + { + std::lock_guard lock(input.mutex); + input.ended = true; + } + input.cv.notify_one(); + }); + } catch (const std::exception& e) { + const std::string error = + std::string("failed to start cyclic stream reader: ") + e.what(); + set_last_error(error); + return grpc::Status(grpc::StatusCode::INTERNAL, error); + } catch (...) { + const std::string error = + "failed to start cyclic stream reader: unknown exception"; + set_last_error(error); + return grpc::Status(grpc::StatusCode::INTERNAL, error); + } + + std::uint64_t last_sequence = 0; + std::uint64_t last_dropped = 0; + const auto finishTerminal = [&](const api::CyclicStreamPhase requested_phase, + const bool success, + const std::string& requested_error, + const grpc::Status requested_status, + const bool cancel_reader) -> grpc::Status { + const bool stopped = safeStop(); + const std::string error = stopped + ? requested_error + : "failed to quick-stop motor while terminating cyclic stream"; + if (!error.empty()) { + set_last_error(error); + } + try { + api::CyclicControlResponse terminal; + fillFeedback(terminal.mutable_header(), success && stopped, error); + terminal.set_phase( + stopped ? requested_phase : api::CYCLIC_STREAM_FAILED); + terminal.set_sequence(last_sequence); + terminal.set_dropped_setpoints(last_dropped); + fill_status(terminal.mutable_status()); + stream->Write(terminal); + } catch (...) { + joinReader(true); + reader_guard.release(); + return grpc::Status( + grpc::StatusCode::INTERNAL, + "failed to publish cyclic stream terminal status"); + } + joinReader(cancel_reader); + reader_guard.release(); + if (!stopped) { + return grpc::Status(grpc::StatusCode::INTERNAL, error); + } + return requested_status; + }; + + try { + for (;;) { + if (context->IsCancelled()) { + const auto status = cancelledStatus(context); + set_last_error(status.error_message()); + const bool stopped = safeStop(); + joinReader(true); + reader_guard.release(); + return stopped + ? status + : grpc::Status( + grpc::StatusCode::INTERNAL, + "failed to quick-stop cancelled cyclic stream"); + } + if (is_preempted(generation)) { + const std::string error = + "cyclic stream preempted by emergency stop"; + set_last_error(error); + const bool stopped = safeStop(); + joinReader(true); + reader_guard.release(); + return stopped + ? grpc::Status(grpc::StatusCode::ABORTED, error) + : grpc::Status( + grpc::StatusCode::INTERNAL, + "failed to quick-stop preempted cyclic stream"); + } + + std::optional request; + bool ended = false; + bool reader_failed = false; + Clock::time_point last_receive; + { + std::unique_lock lock(input.mutex); + input.cv.wait_for(lock, std::chrono::milliseconds(10), [&]() { + return input.pending.has_value() || input.ended; + }); + if (input.pending.has_value()) { + request = std::move(input.pending); + input.pending.reset(); + } + ended = input.ended; + reader_failed = input.reader_failed; + last_dropped = input.dropped; + last_receive = input.last_receive; + } + + if (!request.has_value()) { + if (ended) { + if (reader_failed) { + const std::string error = + "cyclic stream reader terminated with an exception"; + return finishTerminal( + api::CYCLIC_STREAM_FAILED, false, error, + grpc::Status(grpc::StatusCode::INTERNAL, error), + false); + } + return finishTerminal( + api::CYCLIC_STREAM_STOPPED, true, {}, + grpc::Status::OK, false); + } + if (Clock::now() - last_receive >= + std::chrono::milliseconds(watchdog_timeout_ms)) { + const std::string error = + "cyclic stream watchdog expired"; + return finishTerminal( + api::CYCLIC_STREAM_WATCHDOG_EXPIRED, false, error, + grpc::Status(grpc::StatusCode::DEADLINE_EXCEEDED, error), + true); + } + continue; + } + + if (!request->has_setpoint()) { + const std::string error = + "only the first cyclic stream message may contain open"; + return finishTerminal( + api::CYCLIC_STREAM_FAILED, false, error, + grpc::Status(grpc::StatusCode::INVALID_ARGUMENT, error), + true); + } + + const auto& setpoint = request->setpoint(); + if (setpoint.sequence() == 0 || + setpoint.sequence() <= last_sequence) { + const std::string error = + "cyclic setpoint sequence must be strictly increasing and non-zero"; + return finishTerminal( + api::CYCLIC_STREAM_FAILED, false, error, + grpc::Status(grpc::StatusCode::INVALID_ARGUMENT, error), + true); + } + + const auto apply_status = apply(setpoint); + if (!apply_status.ok()) { + return finishTerminal( + api::CYCLIC_STREAM_FAILED, false, + apply_status.error_message(), apply_status, true); + } + if (is_preempted(generation)) { + const std::string error = + "cyclic setpoint preempted during backend dispatch"; + return finishTerminal( + api::CYCLIC_STREAM_FAILED, false, error, + grpc::Status(grpc::StatusCode::ABORTED, error), true); + } + + last_sequence = setpoint.sequence(); + api::CyclicControlResponse applied; + fillFeedback(applied.mutable_header(), true); + applied.set_phase(api::CYCLIC_STREAM_APPLIED); + applied.set_sequence(last_sequence); + applied.set_dropped_setpoints(last_dropped); + if (!stream->Write(applied)) { + const std::string error = + "cyclic stream client stopped reading"; + set_last_error(error); + const bool stopped = safeStop(); + joinReader(true); + reader_guard.release(); + return stopped + ? grpc::Status(grpc::StatusCode::CANCELLED, error) + : grpc::Status( + grpc::StatusCode::INTERNAL, + "failed to quick-stop closed cyclic stream"); + } + } + } catch (const std::exception& e) { + const std::string error = + std::string("cyclic stream internal exception: ") + e.what(); + set_last_error(error); + return finishTerminal( + api::CYCLIC_STREAM_FAILED, false, error, + grpc::Status(grpc::StatusCode::INTERNAL, error), true); + } catch (...) { + const std::string error = "cyclic stream unknown internal exception"; + set_last_error(error); + return finishTerminal( + api::CYCLIC_STREAM_FAILED, false, error, + grpc::Status(grpc::StatusCode::INTERNAL, error), true); + } +} + +} // namespace + +gRPCMotorServiceImpl::ControlLease::ControlLease( + std::shared_ptr state, + const std::uint64_t generation) + : state_(std::move(state)), + generation_(generation), + uncaught_on_entry_(std::uncaught_exceptions()) +{ +} + +gRPCMotorServiceImpl::ControlLease::~ControlLease() +{ + if (!state_) { + return; + } + std::lock_guard lock(state_->mutex); + if (std::uncaught_exceptions() > uncaught_on_entry_) { + // Keep ownership reserved until the public RPC exception barrier has + // completed its best-effort stop. This closes the window where a new + // RPC could acquire the motor between stack unwinding and cleanup. + state_->exception_cleanup_pending = true; + ++state_->cancel_generation; + return; + } + state_->busy = false; + state_->active_control = api::MOTOR_CONTROL_NONE; +} + +gRPCMotorServiceImpl::gRPCMotorServiceImpl() + : dmgr_(device::DeviceManager::getInstance()) +{ +} + +std::shared_ptr +gRPCMotorServiceImpl::stateFor( + const std::shared_ptr& motor) const +{ + std::lock_guard lock(states_mutex_); + for (auto it = states_.begin(); it != states_.end();) { + if (it->second.owner.expired()) { + it = states_.erase(it); + } else { + ++it; + } + } + + const auto existing = states_.find(motor.get()); + if (existing != states_.end()) { + const auto owner = existing->second.owner.lock(); + if (owner && owner == motor) { + return existing->second.state; + } + states_.erase(existing); + } + + auto state = std::make_shared(); + states_.emplace( + motor.get(), MotorControlEntry{std::weak_ptr(motor), + state}); + return state; +} + +grpc::Status gRPCMotorServiceImpl::resolveMotor( + const api::MotorTarget& target, + ResolvedMotor& resolved) const +{ + const std::string& manager_id = target.header().device_id(); + if (manager_id.empty()) { + return grpc::Status(grpc::StatusCode::INVALID_ARGUMENT, + "target.header.device_id is required"); + } + auto manager = dmgr_.getDevice(manager_id); + if (!manager) { + return grpc::Status(grpc::StatusCode::NOT_FOUND, + "MotorManager not found: " + manager_id); + } + + switch (target.selector_case()) { + case api::MotorTarget::kMotorId: + if (target.motor_id() > 255) { + return grpc::Status(grpc::StatusCode::INVALID_ARGUMENT, + "motor_id must fit in uint8"); + } + resolved.motor = manager->getMotor( + static_cast(target.motor_id())); + break; + case api::MotorTarget::kJointName: + if (target.joint_name().empty()) { + return grpc::Status(grpc::StatusCode::INVALID_ARGUMENT, + "joint_name cannot be empty"); + } + resolved.motor = manager->getMotor(target.joint_name()); + break; + case api::MotorTarget::SELECTOR_NOT_SET: + default: + return grpc::Status(grpc::StatusCode::INVALID_ARGUMENT, + "exactly one of motor_id or joint_name is required"); + } + + if (!resolved.motor) { + return grpc::Status(grpc::StatusCode::NOT_FOUND, + "motor not found in MotorManager: " + manager_id); + } + resolved.control = stateFor(resolved.motor); + return grpc::Status::OK; +} + +std::unique_ptr +gRPCMotorServiceImpl::acquireControl( + const ResolvedMotor& resolved, + const api::MotorControlType control, + grpc::Status& failure, + const bool allow_emergency_stopped) const +{ + std::lock_guard lock(resolved.control->mutex); + if (resolved.control->busy) { + failure = grpc::Status(grpc::StatusCode::RESOURCE_EXHAUSTED, + "motor is controlled by another RPC or stream"); + return nullptr; + } + if (resolved.control->emergency_stop_in_progress) { + failure = grpc::Status( + grpc::StatusCode::ABORTED, + "motor emergency stop is still in progress"); + return nullptr; + } + if (resolved.control->emergency_stopped && !allow_emergency_stopped) { + failure = grpc::Status( + grpc::StatusCode::FAILED_PRECONDITION, + "motor is emergency-stopped; enable it explicitly before commanding motion"); + return nullptr; + } + resolved.control->busy = true; + resolved.control->active_control = control; + resolved.control->last_error.clear(); + failure = grpc::Status::OK; + return std::make_unique( + resolved.control, resolved.control->cancel_generation); +} + +void gRPCMotorServiceImpl::setLastError( + const std::shared_ptr& state, + const std::string& error) const +{ + std::lock_guard lock(state->mutex); + state->last_error = error; +} + +void gRPCMotorServiceImpl::latchUnsafeAfterFailedStop( + const std::shared_ptr& state, + const std::string& error) const +{ + std::lock_guard lock(state->mutex); + state->emergency_stopped = true; + state->last_error = error; +} + +void gRPCMotorServiceImpl::bestEffortQuickStop( + const api::MotorTarget& target, + const std::string& error) noexcept +{ + try { + ResolvedMotor resolved; + if (!resolveMotor(target, resolved).ok()) { + return; + } + // Hold this across claim, quick-stop, and release. Otherwise two stale + // exception barriers can both observe one pending cleanup; the second + // may wake after a new RPC acquires the motor and stop that new owner. + std::lock_guard cleanup_lock( + resolved.control->exception_cleanup_mutex); + { + std::lock_guard state_lock(resolved.control->mutex); + if (!resolved.control->exception_cleanup_pending) { + if (resolved.control->busy) { + // A newer RPC already owns the motor. Stopping here would + // let an older exception cancel the new owner's command. + return; + } + resolved.control->busy = true; + resolved.control->exception_cleanup_pending = true; + ++resolved.control->cancel_generation; + } + resolved.control->last_error = error; + } + + bool stopped = false; + try { + std::lock_guard command_lock(resolved.control->command_mutex); + stopped = resolved.motor->quickStop(); + } catch (...) { + } + if (!stopped) { + latchUnsafeAfterFailedStop( + resolved.control, + error + "; best-effort quick-stop was not confirmed"); + } + { + std::lock_guard state_lock(resolved.control->mutex); + resolved.control->exception_cleanup_pending = false; + resolved.control->busy = false; + resolved.control->active_control = api::MOTOR_CONTROL_NONE; + } + } catch (...) { + } +} + +void gRPCMotorServiceImpl::fillMotorStatus( + const ResolvedMotor& resolved, + api::MotorStatus* status) const +{ + bool busy = false; + bool emergency_stopped = false; + api::MotorControlType active_control = api::MOTOR_CONTROL_NONE; + std::string last_error; + { + std::lock_guard lock(resolved.control->mutex); + busy = resolved.control->busy; + emergency_stopped = resolved.control->emergency_stopped; + active_control = resolved.control->active_control; + last_error = resolved.control->last_error; + } + + status->set_motor_id(resolved.motor->id()); + status->set_joint_name(resolved.motor->jointName()); + status->set_run_mode(resolved.motor->getMode()); + status->set_position_rad(resolved.motor->getQ()); + status->set_velocity_rad_s(resolved.motor->getQd()); + status->set_target_reached(resolved.motor->reachedTargetQ()); + status->set_service_busy(busy); + status->set_active_control(active_control); + status->set_emergency_stopped(emergency_stopped); + status->set_last_error(last_error); +} + +grpc::Status gRPCMotorServiceImpl::setZero( + grpc::ServerContext* context, + const api::SetMotorZeroRequest* request, + api::MotorCommandResponse* response) +{ + return runUnaryGuarded( + response, "setZero", + [&]() { return setZeroImpl(context, request, response); }, + [&](const std::string& error) { + bestEffortQuickStop(request->target(), error); + }); +} + +grpc::Status gRPCMotorServiceImpl::moveToZero( + grpc::ServerContext* context, + const api::MoveMotorToZeroRequest* request, + api::MotorCommandResponse* response) +{ + return runUnaryGuarded( + response, "moveToZero", + [&]() { return moveToZeroImpl(context, request, response); }, + [&](const std::string& error) { + bestEffortQuickStop(request->target(), error); + }); +} + +grpc::Status gRPCMotorServiceImpl::profilePosition( + grpc::ServerContext* context, + const api::ProfilePositionRequest* request, + api::MotorCommandResponse* response) +{ + return runUnaryGuarded( + response, "profilePosition", + [&]() { return profilePositionImpl(context, request, response); }, + [&](const std::string& error) { + bestEffortQuickStop(request->target(), error); + }); +} + +grpc::Status gRPCMotorServiceImpl::profileVelocity( + grpc::ServerContext* context, + const api::ProfileVelocityRequest* request, + api::MotorCommandResponse* response) +{ + return runUnaryGuarded( + response, "profileVelocity", + [&]() { return profileVelocityImpl(context, request, response); }, + [&](const std::string& error) { + bestEffortQuickStop(request->target(), error); + }); +} + +grpc::Status gRPCMotorServiceImpl::streamCyclicPosition( + grpc::ServerContext* context, + grpc::ServerReaderWriter* stream) +{ + std::optional cleanup_target; + return runStreamingGuarded( + "streamCyclicPosition", + [&]() { + return streamCyclicPositionImpl( + context, stream, cleanup_target); + }, + [&](const std::string& error) { + if (cleanup_target.has_value()) { + bestEffortQuickStop(*cleanup_target, error); + } + }); +} + +grpc::Status gRPCMotorServiceImpl::streamCyclicVelocity( + grpc::ServerContext* context, + grpc::ServerReaderWriter* stream) +{ + std::optional cleanup_target; + return runStreamingGuarded( + "streamCyclicVelocity", + [&]() { + return streamCyclicVelocityImpl( + context, stream, cleanup_target); + }, + [&](const std::string& error) { + if (cleanup_target.has_value()) { + bestEffortQuickStop(*cleanup_target, error); + } + }); +} + +grpc::Status gRPCMotorServiceImpl::emergencyStop( + grpc::ServerContext* context, + const api::EmergencyStopRequest* request, + api::MotorCommandResponse* response) +{ + return runUnaryGuarded( + response, "emergencyStop", + [&]() { return emergencyStopImpl(context, request, response); }, + [&](const std::string& error) { + bestEffortQuickStop(request->target(), error); + }); +} + +grpc::Status gRPCMotorServiceImpl::getStatus( + grpc::ServerContext* context, + const api::GetMotorStatusRequest* request, + api::GetMotorStatusResponse* response) +{ + return runUnaryGuarded( + response, "getStatus", + [&]() { return getStatusImpl(context, request, response); }, + [](const std::string&) {}); +} + +grpc::Status gRPCMotorServiceImpl::setEnabled( + grpc::ServerContext* context, + const api::SetMotorEnabledRequest* request, + api::MotorCommandResponse* response) +{ + return runUnaryGuarded( + response, "setEnabled", + [&]() { return setEnabledImpl(context, request, response); }, + [&](const std::string& error) { + bestEffortQuickStop(request->target(), error); + }); +} + +grpc::Status gRPCMotorServiceImpl::setZeroImpl( + grpc::ServerContext* context, + const api::SetMotorZeroRequest* request, + api::MotorCommandResponse* response) +{ + const auto started = Clock::now(); + ResolvedMotor resolved; + auto status = resolveMotor(request->target(), resolved); + if (!status.ok()) { + fillFeedback(response->mutable_header(), false, status.error_message()); + return status; + } + grpc::Status acquire_status; + auto lease = acquireControl( + resolved, api::MOTOR_CONTROL_SET_ZERO, acquire_status); + if (!lease) { + fillFeedback(response->mutable_header(), false, + acquire_status.error_message()); + fillMotorStatus(resolved, response->mutable_status()); + return acquire_status; + } + bool calibrated = false; + bool preempted_during_calibration = false; + bool cancelled_during_calibration = false; + grpc::Status post_calibration_cancel_status; + { + std::lock_guard command_lock(resolved.control->command_mutex); + if (context->IsCancelled() || + std::chrono::system_clock::now() >= context->deadline()) { + const auto cancelled = cancelledStatus(context); + lease.reset(); + fillFeedback(response->mutable_header(), false, + cancelled.error_message()); + fillMotorStatus(resolved, response->mutable_status()); + response->set_elapsed_ms(elapsedMs(started)); + return cancelled; + } + bool preempted = false; + { + std::lock_guard state_lock(resolved.control->mutex); + preempted = + resolved.control->cancel_generation != lease->generation(); + } + if (preempted) { + const std::string error = + "zero calibration preempted by emergency stop"; + lease.reset(); + fillFeedback(response->mutable_header(), false, error); + fillMotorStatus(resolved, response->mutable_status()); + response->set_elapsed_ms(elapsedMs(started)); + return grpc::Status(grpc::StatusCode::ABORTED, error); + } + calibrated = resolved.motor->calibrateZeroQ(); + { + std::lock_guard state_lock(resolved.control->mutex); + preempted_during_calibration = + resolved.control->cancel_generation != lease->generation() || + resolved.control->emergency_stop_in_progress; + } + if (!preempted_during_calibration && + (context->IsCancelled() || + std::chrono::system_clock::now() >= context->deadline())) { + cancelled_during_calibration = true; + post_calibration_cancel_status = cancelledStatus(context); + } + } + if (preempted_during_calibration) { + const std::string error = + "zero calibration preempted during backend dispatch"; + setLastError(resolved.control, error); + lease.reset(); + fillFeedback(response->mutable_header(), false, error); + fillMotorStatus(resolved, response->mutable_status()); + response->set_elapsed_ms(elapsedMs(started)); + return grpc::Status(grpc::StatusCode::ABORTED, error); + } + if (cancelled_during_calibration) { + const std::string error = + post_calibration_cancel_status.error_message() + + "; zero calibration outcome may already be committed"; + setLastError(resolved.control, error); + lease.reset(); + fillFeedback(response->mutable_header(), false, error); + fillMotorStatus(resolved, response->mutable_status()); + response->set_elapsed_ms(elapsedMs(started)); + return grpc::Status( + post_calibration_cancel_status.error_code(), error); + } + if (!calibrated) { + const std::string error = + "zero calibration outcome unknown; inspect PLC zero_epoch/session before retry"; + setLastError(resolved.control, error); + lease.reset(); + fillFeedback(response->mutable_header(), false, error); + fillMotorStatus(resolved, response->mutable_status()); + response->set_elapsed_ms(elapsedMs(started)); + return grpc::Status(grpc::StatusCode::FAILED_PRECONDITION, error); + } + lease.reset(); + fillFeedback(response->mutable_header(), true); + fillMotorStatus(resolved, response->mutable_status()); + response->set_elapsed_ms(elapsedMs(started)); + return grpc::Status::OK; +} + +grpc::Status gRPCMotorServiceImpl::runProfilePosition( + grpc::ServerContext* context, + const ResolvedMotor& resolved, + const double target_position_rad, + const double max_velocity_rad_s, + const double acceleration_rad_s2, + const api::MotorWaitOptions& wait, + api::MotorCommandResponse* response) +{ + const auto started = Clock::now(); + if (!isFinite(target_position_rad) || + !isFinite(max_velocity_rad_s) || max_velocity_rad_s <= 0.0 || + !isFinite(acceleration_rad_s2) || acceleration_rad_s2 <= 0.0) { + const std::string error = + "target position must be finite and velocity/acceleration must be positive"; + fillFeedback(response->mutable_header(), false, error); + return grpc::Status(grpc::StatusCode::INVALID_ARGUMENT, error); + } + const auto wait_status = validateWaitOptions(wait); + if (!wait_status.ok()) { + fillFeedback(response->mutable_header(), false, + wait_status.error_message()); + return wait_status; + } + + grpc::Status acquire_status; + auto lease = acquireControl( + resolved, api::MOTOR_CONTROL_PROFILE_POSITION, acquire_status); + if (!lease) { + fillFeedback(response->mutable_header(), false, + acquire_status.error_message()); + fillMotorStatus(resolved, response->mutable_status()); + return acquire_status; + } + + bool submitted = false; + bool preempted_during_dispatch = false; + { + std::lock_guard command_lock(resolved.control->command_mutex); + if (context->IsCancelled() || + std::chrono::system_clock::now() >= context->deadline()) { + const auto cancelled = cancelledStatus(context); + lease.reset(); + fillFeedback(response->mutable_header(), false, + cancelled.error_message()); + fillMotorStatus(resolved, response->mutable_status()); + response->set_elapsed_ms(elapsedMs(started)); + return cancelled; + } + bool preempted = false; + { + std::lock_guard state_lock(resolved.control->mutex); + preempted = + resolved.control->cancel_generation != lease->generation(); + } + if (preempted) { + const std::string error = + "profile position preempted by emergency stop"; + lease.reset(); + fillFeedback(response->mutable_header(), false, error); + fillMotorStatus(resolved, response->mutable_status()); + response->set_elapsed_ms(elapsedMs(started)); + return grpc::Status(grpc::StatusCode::ABORTED, error); + } + resolved.motor->setMode(msgs::RUN_MODE_PROFILE_POSITION); + submitted = resolved.motor->commandProfilePosition( + target_position_rad, max_velocity_rad_s, acceleration_rad_s2); + { + std::lock_guard state_lock(resolved.control->mutex); + preempted_during_dispatch = + resolved.control->cancel_generation != lease->generation() || + resolved.control->emergency_stop_in_progress; + } + } + if (preempted_during_dispatch) { + const std::string error = + "profile position preempted during backend dispatch"; + setLastError(resolved.control, error); + lease.reset(); + fillFeedback(response->mutable_header(), false, error); + fillMotorStatus(resolved, response->mutable_status()); + response->set_elapsed_ms(elapsedMs(started)); + return grpc::Status(grpc::StatusCode::ABORTED, error); + } + if (!submitted) { + bool stopped = false; + { + std::lock_guard command_lock(resolved.control->command_mutex); + { + std::lock_guard state_lock(resolved.control->mutex); + preempted_during_dispatch = + resolved.control->cancel_generation != lease->generation() || + resolved.control->emergency_stop_in_progress; + } + if (!preempted_during_dispatch) { + stopped = resolved.motor->quickStop(); + std::lock_guard state_lock(resolved.control->mutex); + preempted_during_dispatch = + resolved.control->cancel_generation != lease->generation() || + resolved.control->emergency_stop_in_progress; + } + } + if (preempted_during_dispatch) { + const std::string error = + "profile position rejected while being preempted by emergency stop"; + setLastError(resolved.control, error); + lease.reset(); + fillFeedback(response->mutable_header(), false, error); + fillMotorStatus(resolved, response->mutable_status()); + response->set_elapsed_ms(elapsedMs(started)); + return grpc::Status(grpc::StatusCode::ABORTED, error); + } + if (!stopped) { + const std::string error = + "failed to quick-stop after uncertain profile position dispatch"; + latchUnsafeAfterFailedStop(resolved.control, error); + lease.reset(); + fillFeedback(response->mutable_header(), false, error); + fillMotorStatus(resolved, response->mutable_status()); + response->set_elapsed_ms(elapsedMs(started)); + return grpc::Status(grpc::StatusCode::INTERNAL, error); + } + if (context->IsCancelled() || + std::chrono::system_clock::now() >= context->deadline()) { + const auto cancelled = cancelledStatus(context); + const std::string error = + cancelled.error_message() + + "; profile position dispatch outcome may have been committed"; + setLastError(resolved.control, error); + lease.reset(); + fillFeedback(response->mutable_header(), false, error); + fillMotorStatus(resolved, response->mutable_status()); + response->set_elapsed_ms(elapsedMs(started)); + return grpc::Status(cancelled.error_code(), error); + } + const std::string error = + "AbstractMotor rejected profile position command after safe stop"; + setLastError(resolved.control, error); + lease.reset(); + fillFeedback(response->mutable_header(), false, error); + fillMotorStatus(resolved, response->mutable_status()); + response->set_elapsed_ms(elapsedMs(started)); + return grpc::Status(grpc::StatusCode::FAILED_PRECONDITION, error); + } + + auto completion_status = waitForPosition( + context, resolved, lease->generation(), target_position_rad, + wait, response, started); + lease.reset(); + fillMotorStatus(resolved, response->mutable_status()); + return completion_status; +} + +grpc::Status gRPCMotorServiceImpl::waitForPosition( + grpc::ServerContext* context, + const ResolvedMotor& resolved, + const std::uint64_t generation, + const double target_position_rad, + const api::MotorWaitOptions& wait, + api::MotorCommandResponse* response, + const Clock::time_point started) +{ + const auto timeout = std::chrono::milliseconds(commandTimeoutMs(wait)); + const auto poll = std::chrono::milliseconds(pollPeriodMs(wait)); + const auto required_samples = settleSamples(wait); + const double q_tolerance = positionTolerance(wait); + const double qd_tolerance = velocityTolerance(wait); + std::uint32_t settled = 0; + + for (;;) { + bool preempted = false; + { + std::lock_guard lock(resolved.control->mutex); + preempted = resolved.control->cancel_generation != generation; + } + if (preempted) { + const std::string error = "profile position preempted by emergency stop"; + setLastError(resolved.control, error); + fillFeedback(response->mutable_header(), false, error); + response->set_elapsed_ms(elapsedMs(started)); + return grpc::Status(grpc::StatusCode::ABORTED, error); + } + if (context->IsCancelled() || + std::chrono::system_clock::now() >= context->deadline()) { + const auto cancelled = cancelledStatus(context); + setLastError(resolved.control, cancelled.error_message()); + bool stopped = false; + { + std::lock_guard command_lock( + resolved.control->command_mutex); + stopped = resolved.motor->quickStop(); + } + if (!stopped) { + const std::string error = + "failed to quick-stop cancelled profile position"; + latchUnsafeAfterFailedStop(resolved.control, error); + fillFeedback(response->mutable_header(), false, error); + response->set_elapsed_ms(elapsedMs(started)); + return grpc::Status(grpc::StatusCode::INTERNAL, error); + } + fillFeedback(response->mutable_header(), false, + cancelled.error_message()); + response->set_elapsed_ms(elapsedMs(started)); + return cancelled; + } + if (Clock::now() - started >= timeout) { + const std::string error = "profile position wait timed out"; + setLastError(resolved.control, error); + bool stopped = false; + { + std::lock_guard command_lock( + resolved.control->command_mutex); + stopped = resolved.motor->quickStop(); + } + if (!stopped) { + const std::string stop_error = + "failed to quick-stop timed-out profile position"; + latchUnsafeAfterFailedStop(resolved.control, stop_error); + fillFeedback(response->mutable_header(), false, stop_error); + response->set_elapsed_ms(elapsedMs(started)); + return grpc::Status(grpc::StatusCode::INTERNAL, stop_error); + } + fillFeedback(response->mutable_header(), false, error); + response->set_elapsed_ms(elapsedMs(started)); + return grpc::Status(grpc::StatusCode::DEADLINE_EXCEEDED, error); + } + + const double q = resolved.motor->getQ(); + const double qd = resolved.motor->getQd(); + const bool reached_target = resolved.motor->reachedTargetQ(); + { + std::lock_guard lock(resolved.control->mutex); + preempted = resolved.control->cancel_generation != generation; + } + if (preempted) { + const std::string error = + "profile position preempted during status sampling"; + setLastError(resolved.control, error); + fillFeedback(response->mutable_header(), false, error); + response->set_elapsed_ms(elapsedMs(started)); + return grpc::Status(grpc::StatusCode::ABORTED, error); + } + if (!isFinite(q) || !isFinite(qd)) { + bool stopped = false; + try { + std::lock_guard command_lock( + resolved.control->command_mutex); + stopped = resolved.motor->quickStop(); + } catch (...) { + stopped = false; + } + if (!stopped) { + const std::string error = + "failed to quick-stop profile position after non-finite feedback"; + latchUnsafeAfterFailedStop(resolved.control, error); + fillFeedback(response->mutable_header(), false, error); + response->set_elapsed_ms(elapsedMs(started)); + return grpc::Status(grpc::StatusCode::INTERNAL, error); + } + const std::string error = + "profile position feedback became non-finite"; + setLastError(resolved.control, error); + fillFeedback(response->mutable_header(), false, error); + response->set_elapsed_ms(elapsedMs(started)); + return grpc::Status(grpc::StatusCode::UNAVAILABLE, error); + } + const bool in_tolerance = + reached_target && + std::abs(q - target_position_rad) <= q_tolerance && + std::abs(qd) <= qd_tolerance; + settled = in_tolerance ? settled + 1 : 0; + if (settled >= required_samples) { + fillFeedback(response->mutable_header(), true); + response->set_elapsed_ms(elapsedMs(started)); + return grpc::Status::OK; + } + sleepInterruptibly( + context, Clock::now() + poll, [&]() { + std::lock_guard lock(resolved.control->mutex); + return resolved.control->cancel_generation != generation; + }); + } +} + +grpc::Status gRPCMotorServiceImpl::moveToZeroImpl( + grpc::ServerContext* context, + const api::MoveMotorToZeroRequest* request, + api::MotorCommandResponse* response) +{ + ResolvedMotor resolved; + auto status = resolveMotor(request->target(), resolved); + if (!status.ok()) { + fillFeedback(response->mutable_header(), false, status.error_message()); + return status; + } + return runProfilePosition( + context, resolved, 0.0, request->max_velocity_rad_s(), + request->acceleration_rad_s2(), request->wait(), response); +} + +grpc::Status gRPCMotorServiceImpl::profilePositionImpl( + grpc::ServerContext* context, + const api::ProfilePositionRequest* request, + api::MotorCommandResponse* response) +{ + ResolvedMotor resolved; + auto status = resolveMotor(request->target(), resolved); + if (!status.ok()) { + fillFeedback(response->mutable_header(), false, status.error_message()); + return status; + } + return runProfilePosition( + context, resolved, request->target_position_rad(), + request->max_velocity_rad_s(), request->acceleration_rad_s2(), + request->wait(), response); +} + +grpc::Status gRPCMotorServiceImpl::waitForVelocity( + grpc::ServerContext* context, + const ResolvedMotor& resolved, + const std::uint64_t generation, + const double target_velocity_rad_s, + const api::MotorWaitOptions& wait, + api::MotorCommandResponse* response, + const Clock::time_point started) +{ + const auto timeout = std::chrono::milliseconds(commandTimeoutMs(wait)); + const auto poll = std::chrono::milliseconds(pollPeriodMs(wait)); + const auto required_samples = settleSamples(wait); + const double tolerance = velocityTolerance(wait); + std::uint32_t settled = 0; + + for (;;) { + bool preempted = false; + { + std::lock_guard lock(resolved.control->mutex); + preempted = resolved.control->cancel_generation != generation; + } + if (preempted) { + const std::string error = "profile velocity preempted by emergency stop"; + setLastError(resolved.control, error); + fillFeedback(response->mutable_header(), false, error); + response->set_elapsed_ms(elapsedMs(started)); + return grpc::Status(grpc::StatusCode::ABORTED, error); + } + if (context->IsCancelled() || + std::chrono::system_clock::now() >= context->deadline()) { + const auto cancelled = cancelledStatus(context); + setLastError(resolved.control, cancelled.error_message()); + bool stopped = false; + { + std::lock_guard command_lock( + resolved.control->command_mutex); + stopped = resolved.motor->quickStop(); + } + if (!stopped) { + const std::string error = + "failed to quick-stop cancelled profile velocity"; + latchUnsafeAfterFailedStop(resolved.control, error); + fillFeedback(response->mutable_header(), false, error); + response->set_elapsed_ms(elapsedMs(started)); + return grpc::Status(grpc::StatusCode::INTERNAL, error); + } + fillFeedback(response->mutable_header(), false, + cancelled.error_message()); + response->set_elapsed_ms(elapsedMs(started)); + return cancelled; + } + if (Clock::now() - started >= timeout) { + const std::string error = "profile velocity wait timed out"; + setLastError(resolved.control, error); + bool stopped = false; + { + std::lock_guard command_lock( + resolved.control->command_mutex); + stopped = resolved.motor->quickStop(); + } + if (!stopped) { + const std::string stop_error = + "failed to quick-stop timed-out profile velocity"; + latchUnsafeAfterFailedStop(resolved.control, stop_error); + fillFeedback(response->mutable_header(), false, stop_error); + response->set_elapsed_ms(elapsedMs(started)); + return grpc::Status(grpc::StatusCode::INTERNAL, stop_error); + } + fillFeedback(response->mutable_header(), false, error); + response->set_elapsed_ms(elapsedMs(started)); + return grpc::Status(grpc::StatusCode::DEADLINE_EXCEEDED, error); + } + + const double qd = resolved.motor->getQd(); + { + std::lock_guard lock(resolved.control->mutex); + preempted = resolved.control->cancel_generation != generation; + } + if (preempted) { + const std::string error = + "profile velocity preempted during status sampling"; + setLastError(resolved.control, error); + fillFeedback(response->mutable_header(), false, error); + response->set_elapsed_ms(elapsedMs(started)); + return grpc::Status(grpc::StatusCode::ABORTED, error); + } + if (!isFinite(qd)) { + bool stopped = false; + try { + std::lock_guard command_lock( + resolved.control->command_mutex); + stopped = resolved.motor->quickStop(); + } catch (...) { + stopped = false; + } + if (!stopped) { + const std::string error = + "failed to quick-stop profile velocity after non-finite feedback"; + latchUnsafeAfterFailedStop(resolved.control, error); + fillFeedback(response->mutable_header(), false, error); + response->set_elapsed_ms(elapsedMs(started)); + return grpc::Status(grpc::StatusCode::INTERNAL, error); + } + const std::string error = + "profile velocity feedback became non-finite"; + setLastError(resolved.control, error); + fillFeedback(response->mutable_header(), false, error); + response->set_elapsed_ms(elapsedMs(started)); + return grpc::Status(grpc::StatusCode::UNAVAILABLE, error); + } + settled = std::abs(qd - target_velocity_rad_s) <= tolerance + ? settled + 1 + : 0; + if (settled >= required_samples) { + fillFeedback(response->mutable_header(), true); + response->set_elapsed_ms(elapsedMs(started)); + return grpc::Status::OK; + } + sleepInterruptibly( + context, Clock::now() + poll, [&]() { + std::lock_guard lock(resolved.control->mutex); + return resolved.control->cancel_generation != generation; + }); + } +} + +grpc::Status gRPCMotorServiceImpl::profileVelocityImpl( + grpc::ServerContext* context, + const api::ProfileVelocityRequest* request, + api::MotorCommandResponse* response) +{ + const auto started = Clock::now(); + ResolvedMotor resolved; + auto status = resolveMotor(request->target(), resolved); + if (!status.ok()) { + fillFeedback(response->mutable_header(), false, status.error_message()); + return status; + } + if (!isFinite(request->target_velocity_rad_s()) || + !isFinite(request->acceleration_rad_s2()) || + request->acceleration_rad_s2() <= 0.0) { + const std::string error = + "target velocity must be finite and acceleration must be positive"; + fillFeedback(response->mutable_header(), false, error); + return grpc::Status(grpc::StatusCode::INVALID_ARGUMENT, error); + } + const auto wait_options_status = validateWaitOptions(request->wait()); + if (!wait_options_status.ok()) { + fillFeedback(response->mutable_header(), false, + wait_options_status.error_message()); + return wait_options_status; + } + + grpc::Status acquire_status; + auto lease = acquireControl( + resolved, api::MOTOR_CONTROL_PROFILE_VELOCITY, acquire_status); + if (!lease) { + fillFeedback(response->mutable_header(), false, + acquire_status.error_message()); + fillMotorStatus(resolved, response->mutable_status()); + return acquire_status; + } + + bool submitted = false; + bool preempted_during_dispatch = false; + { + std::lock_guard command_lock(resolved.control->command_mutex); + if (context->IsCancelled() || + std::chrono::system_clock::now() >= context->deadline()) { + const auto cancelled = cancelledStatus(context); + lease.reset(); + fillFeedback(response->mutable_header(), false, + cancelled.error_message()); + fillMotorStatus(resolved, response->mutable_status()); + response->set_elapsed_ms(elapsedMs(started)); + return cancelled; + } + bool preempted = false; + { + std::lock_guard state_lock(resolved.control->mutex); + preempted = + resolved.control->cancel_generation != lease->generation(); + } + if (preempted) { + const std::string error = + "profile velocity preempted by emergency stop"; + lease.reset(); + fillFeedback(response->mutable_header(), false, error); + fillMotorStatus(resolved, response->mutable_status()); + response->set_elapsed_ms(elapsedMs(started)); + return grpc::Status(grpc::StatusCode::ABORTED, error); + } + resolved.motor->setMode(msgs::RUN_MODE_PROFILE_VELOCITY); + submitted = resolved.motor->commandProfileVelocity( + request->target_velocity_rad_s(), + request->acceleration_rad_s2()); + { + std::lock_guard state_lock(resolved.control->mutex); + preempted_during_dispatch = + resolved.control->cancel_generation != lease->generation() || + resolved.control->emergency_stop_in_progress; + } + } + if (preempted_during_dispatch) { + const std::string error = + "profile velocity preempted during backend dispatch"; + setLastError(resolved.control, error); + lease.reset(); + fillFeedback(response->mutable_header(), false, error); + fillMotorStatus(resolved, response->mutable_status()); + response->set_elapsed_ms(elapsedMs(started)); + return grpc::Status(grpc::StatusCode::ABORTED, error); + } + if (!submitted) { + bool stopped = false; + { + std::lock_guard command_lock(resolved.control->command_mutex); + { + std::lock_guard state_lock(resolved.control->mutex); + preempted_during_dispatch = + resolved.control->cancel_generation != lease->generation() || + resolved.control->emergency_stop_in_progress; + } + if (!preempted_during_dispatch) { + stopped = resolved.motor->quickStop(); + std::lock_guard state_lock(resolved.control->mutex); + preempted_during_dispatch = + resolved.control->cancel_generation != lease->generation() || + resolved.control->emergency_stop_in_progress; + } + } + if (preempted_during_dispatch) { + const std::string error = + "profile velocity rejected while being preempted by emergency stop"; + setLastError(resolved.control, error); + lease.reset(); + fillFeedback(response->mutable_header(), false, error); + fillMotorStatus(resolved, response->mutable_status()); + response->set_elapsed_ms(elapsedMs(started)); + return grpc::Status(grpc::StatusCode::ABORTED, error); + } + if (!stopped) { + const std::string error = + "failed to quick-stop after uncertain profile velocity dispatch"; + latchUnsafeAfterFailedStop(resolved.control, error); + lease.reset(); + fillFeedback(response->mutable_header(), false, error); + fillMotorStatus(resolved, response->mutable_status()); + response->set_elapsed_ms(elapsedMs(started)); + return grpc::Status(grpc::StatusCode::INTERNAL, error); + } + if (context->IsCancelled() || + std::chrono::system_clock::now() >= context->deadline()) { + const auto cancelled = cancelledStatus(context); + const std::string error = + cancelled.error_message() + + "; profile velocity dispatch outcome may have been committed"; + setLastError(resolved.control, error); + lease.reset(); + fillFeedback(response->mutable_header(), false, error); + fillMotorStatus(resolved, response->mutable_status()); + response->set_elapsed_ms(elapsedMs(started)); + return grpc::Status(cancelled.error_code(), error); + } + const std::string error = + "AbstractMotor rejected profile velocity command after safe stop"; + setLastError(resolved.control, error); + lease.reset(); + fillFeedback(response->mutable_header(), false, error); + fillMotorStatus(resolved, response->mutable_status()); + response->set_elapsed_ms(elapsedMs(started)); + return grpc::Status(grpc::StatusCode::FAILED_PRECONDITION, error); + } + + auto wait_status = waitForVelocity( + context, resolved, lease->generation(), + request->target_velocity_rad_s(), request->wait(), response, started); + lease.reset(); + fillMotorStatus(resolved, response->mutable_status()); + return wait_status; +} + +grpc::Status gRPCMotorServiceImpl::streamCyclicPositionImpl( + grpc::ServerContext* context, + grpc::ServerReaderWriter* stream, + std::optional& cleanup_target) +{ + api::CyclicPositionRequest first; + if (!stream->Read(&first) || !first.has_open()) { + return grpc::Status(grpc::StatusCode::INVALID_ARGUMENT, + "first cyclic position message must contain open"); + } + cleanup_target = first.open().target(); + ResolvedMotor resolved; + auto status = resolveMotor(first.open().target(), resolved); + if (!status.ok()) { + return status; + } + grpc::Status acquire_status; + auto lease = acquireControl( + resolved, api::MOTOR_CONTROL_CYCLIC_POSITION, acquire_status); + if (!lease) { + return acquire_status; + } + + const auto generation = lease->generation(); + { + std::lock_guard command_lock(resolved.control->command_mutex); + if (context->IsCancelled() || + std::chrono::system_clock::now() >= context->deadline()) { + return cancelledStatus(context); + } + bool preempted = false; + { + std::lock_guard state_lock(resolved.control->mutex); + preempted = + resolved.control->cancel_generation != generation; + } + if (preempted) { + return grpc::Status( + grpc::StatusCode::ABORTED, + "cyclic position open preempted by emergency stop"); + } + resolved.motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_POSITION); + std::lock_guard state_lock(resolved.control->mutex); + if (resolved.control->cancel_generation != generation) { + return grpc::Status( + grpc::StatusCode::ABORTED, + "cyclic position open preempted during backend dispatch"); + } + } + return runCyclicLoop( + context, stream, generation, + first.open().watchdog_timeout_ms(), + [&](const api::CyclicPositionSetpoint& setpoint) { + std::lock_guard command_lock( + resolved.control->command_mutex); + if (context->IsCancelled() || + std::chrono::system_clock::now() >= context->deadline()) { + return cancelledStatus(context); + } + { + std::lock_guard state_lock(resolved.control->mutex); + if (resolved.control->cancel_generation != generation) { + return grpc::Status( + grpc::StatusCode::ABORTED, + "cyclic position setpoint preempted by emergency stop"); + } + } + if (!isFinite(setpoint.target_position_rad()) || + (setpoint.has_target_velocity_rad_s() && + !isFinite(setpoint.target_velocity_rad_s()))) { + return grpc::Status( + grpc::StatusCode::INVALID_ARGUMENT, + "cyclic position setpoint contains a non-finite value"); + } + const bool submitted = resolved.motor->commandCyclicPosition( + setpoint.target_position_rad(), + setpoint.has_target_velocity_rad_s() + ? setpoint.target_velocity_rad_s() + : 0.0); + { + std::lock_guard state_lock(resolved.control->mutex); + if (resolved.control->cancel_generation != generation) { + return grpc::Status( + grpc::StatusCode::ABORTED, + "cyclic position setpoint preempted during backend dispatch"); + } + } + if (!submitted) { + return grpc::Status( + grpc::StatusCode::FAILED_PRECONDITION, + "AbstractMotor rejected cyclic position setpoint"); + } + return grpc::Status::OK; + }, + [&](api::MotorStatus* motor_status) { + fillMotorStatus(resolved, motor_status); + }, + [&](const std::uint64_t expected_generation) { + std::lock_guard lock(resolved.control->mutex); + return resolved.control->cancel_generation != expected_generation; + }, + [&](const std::string& error) { + setLastError(resolved.control, error); + }, + [&]() { + bool stopped = false; + try { + std::lock_guard command_lock( + resolved.control->command_mutex); + stopped = resolved.motor->quickStop(); + } catch (...) { + stopped = false; + } + if (!stopped) { + latchUnsafeAfterFailedStop( + resolved.control, + "failed to quick-stop cyclic position stream"); + } + return stopped; + }); +} + +grpc::Status gRPCMotorServiceImpl::streamCyclicVelocityImpl( + grpc::ServerContext* context, + grpc::ServerReaderWriter* stream, + std::optional& cleanup_target) +{ + api::CyclicVelocityRequest first; + if (!stream->Read(&first) || !first.has_open()) { + return grpc::Status(grpc::StatusCode::INVALID_ARGUMENT, + "first cyclic velocity message must contain open"); + } + cleanup_target = first.open().target(); + ResolvedMotor resolved; + auto status = resolveMotor(first.open().target(), resolved); + if (!status.ok()) { + return status; + } + grpc::Status acquire_status; + auto lease = acquireControl( + resolved, api::MOTOR_CONTROL_CYCLIC_VELOCITY, acquire_status); + if (!lease) { + return acquire_status; + } + + const auto generation = lease->generation(); + { + std::lock_guard command_lock(resolved.control->command_mutex); + if (context->IsCancelled() || + std::chrono::system_clock::now() >= context->deadline()) { + return cancelledStatus(context); + } + bool preempted = false; + { + std::lock_guard state_lock(resolved.control->mutex); + preempted = + resolved.control->cancel_generation != generation; + } + if (preempted) { + return grpc::Status( + grpc::StatusCode::ABORTED, + "cyclic velocity open preempted by emergency stop"); + } + resolved.motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_VELOCITY); + std::lock_guard state_lock(resolved.control->mutex); + if (resolved.control->cancel_generation != generation) { + return grpc::Status( + grpc::StatusCode::ABORTED, + "cyclic velocity open preempted during backend dispatch"); + } + } + return runCyclicLoop( + context, stream, generation, + first.open().watchdog_timeout_ms(), + [&](const api::CyclicVelocitySetpoint& setpoint) { + std::lock_guard command_lock( + resolved.control->command_mutex); + if (context->IsCancelled() || + std::chrono::system_clock::now() >= context->deadline()) { + return cancelledStatus(context); + } + { + std::lock_guard state_lock(resolved.control->mutex); + if (resolved.control->cancel_generation != generation) { + return grpc::Status( + grpc::StatusCode::ABORTED, + "cyclic velocity setpoint preempted by emergency stop"); + } + } + if (!isFinite(setpoint.target_velocity_rad_s())) { + return grpc::Status( + grpc::StatusCode::INVALID_ARGUMENT, + "cyclic velocity setpoint contains a non-finite value"); + } + const bool submitted = resolved.motor->commandCyclicVelocity( + setpoint.target_velocity_rad_s()); + { + std::lock_guard state_lock(resolved.control->mutex); + if (resolved.control->cancel_generation != generation) { + return grpc::Status( + grpc::StatusCode::ABORTED, + "cyclic velocity setpoint preempted during backend dispatch"); + } + } + if (!submitted) { + return grpc::Status( + grpc::StatusCode::FAILED_PRECONDITION, + "AbstractMotor rejected cyclic velocity setpoint"); + } + return grpc::Status::OK; + }, + [&](api::MotorStatus* motor_status) { + fillMotorStatus(resolved, motor_status); + }, + [&](const std::uint64_t expected_generation) { + std::lock_guard lock(resolved.control->mutex); + return resolved.control->cancel_generation != expected_generation; + }, + [&](const std::string& error) { + setLastError(resolved.control, error); + }, + [&]() { + bool stopped = false; + try { + std::lock_guard command_lock( + resolved.control->command_mutex); + stopped = resolved.motor->quickStop(); + } catch (...) { + stopped = false; + } + if (!stopped) { + latchUnsafeAfterFailedStop( + resolved.control, + "failed to quick-stop cyclic velocity stream"); + } + return stopped; + }); +} + +grpc::Status gRPCMotorServiceImpl::emergencyStopImpl( + grpc::ServerContext*, + const api::EmergencyStopRequest* request, + api::MotorCommandResponse* response) +{ + const auto started = Clock::now(); + ResolvedMotor resolved; + auto status = resolveMotor(request->target(), resolved); + if (!status.ok()) { + fillFeedback(response->mutable_header(), false, status.error_message()); + return status; + } + std::lock_guard emergency_lock(resolved.control->emergency_mutex); + bool stopped = false; + { + { + std::lock_guard state_lock(resolved.control->mutex); + ++resolved.control->cancel_generation; + resolved.control->emergency_stopped = true; + resolved.control->emergency_stop_in_progress = true; + resolved.control->last_error = "emergency stop requested"; + } + // First stop is deliberately issued before waiting for the service + // dispatch mutex. AbstractMotor serializes it with an in-flight driver + // call, so it takes effect at the earliest point the driver permits. + try { + resolved.motor->quickStop(); + } catch (...) { + } + // The confirmed second stop is ordered after every ordinary write that + // passed its generation check before this E-stop. No stale write can + // therefore occur after this final stop. + { + std::lock_guard command_lock(resolved.control->command_mutex); + try { + stopped = resolved.motor->quickStop(); + } catch (...) { + stopped = false; + } + } + std::lock_guard state_lock(resolved.control->mutex); + resolved.control->emergency_stop_in_progress = false; + } + if (!stopped) { + const std::string error = "AbstractMotor rejected emergency quick stop"; + setLastError(resolved.control, error); + fillFeedback(response->mutable_header(), false, error); + fillMotorStatus(resolved, response->mutable_status()); + response->set_elapsed_ms(elapsedMs(started)); + return grpc::Status(grpc::StatusCode::FAILED_PRECONDITION, error); + } + fillFeedback(response->mutable_header(), true); + fillMotorStatus(resolved, response->mutable_status()); + response->set_elapsed_ms(elapsedMs(started)); + return grpc::Status::OK; +} + +grpc::Status gRPCMotorServiceImpl::getStatusImpl( + grpc::ServerContext*, + const api::GetMotorStatusRequest* request, + api::GetMotorStatusResponse* response) +{ + ResolvedMotor resolved; + auto status = resolveMotor(request->target(), resolved); + if (!status.ok()) { + fillFeedback(response->mutable_header(), false, status.error_message()); + return status; + } + fillMotorStatus(resolved, response->mutable_status()); + if (!isFinite(response->status().position_rad()) || + !isFinite(response->status().velocity_rad_s())) { + const std::string error = + "motor status contains a non-finite position or velocity"; + fillFeedback(response->mutable_header(), false, error); + return grpc::Status(grpc::StatusCode::UNAVAILABLE, error); + } + fillFeedback(response->mutable_header(), true); + return grpc::Status::OK; +} + +grpc::Status gRPCMotorServiceImpl::setEnabledImpl( + grpc::ServerContext* context, + const api::SetMotorEnabledRequest* request, + api::MotorCommandResponse* response) +{ + const auto started = Clock::now(); + ResolvedMotor resolved; + auto status = resolveMotor(request->target(), resolved); + if (!status.ok()) { + fillFeedback(response->mutable_header(), false, status.error_message()); + return status; + } + grpc::Status acquire_status; + auto lease = acquireControl( + resolved, api::MOTOR_CONTROL_SET_ENABLED, acquire_status, true); + if (!lease) { + fillFeedback(response->mutable_header(), false, + acquire_status.error_message()); + fillMotorStatus(resolved, response->mutable_status()); + return acquire_status; + } + + bool success = false; + bool preempted_after_dispatch = false; + bool cancelled_after_dispatch = false; + grpc::Status post_dispatch_cancel_status; + bool cleanup_succeeded = true; + { + std::lock_guard command_lock(resolved.control->command_mutex); + if (context->IsCancelled() || + std::chrono::system_clock::now() >= context->deadline()) { + const auto cancelled = cancelledStatus(context); + lease.reset(); + fillFeedback(response->mutable_header(), false, + cancelled.error_message()); + fillMotorStatus(resolved, response->mutable_status()); + response->set_elapsed_ms(elapsedMs(started)); + return cancelled; + } + bool preempted = false; + { + std::lock_guard state_lock(resolved.control->mutex); + preempted = + resolved.control->cancel_generation != lease->generation() || + resolved.control->emergency_stop_in_progress; + } + if (preempted) { + const std::string error = + "enable/disable preempted by emergency stop"; + lease.reset(); + fillFeedback(response->mutable_header(), false, error); + fillMotorStatus(resolved, response->mutable_status()); + response->set_elapsed_ms(elapsedMs(started)); + return grpc::Status(grpc::StatusCode::ABORTED, error); + } + success = request->enabled() + ? resolved.motor->torqueOn() + : resolved.motor->torqueOff(); + { + std::lock_guard state_lock(resolved.control->mutex); + preempted_after_dispatch = + resolved.control->cancel_generation != lease->generation() || + resolved.control->emergency_stop_in_progress; + } + if (!preempted_after_dispatch && + (context->IsCancelled() || + std::chrono::system_clock::now() >= context->deadline())) { + cancelled_after_dispatch = true; + post_dispatch_cancel_status = cancelledStatus(context); + } + + if (!preempted_after_dispatch && !cancelled_after_dispatch && + success && request->enabled()) { + std::lock_guard state_lock(resolved.control->mutex); + if (resolved.control->cancel_generation == lease->generation() && + !resolved.control->emergency_stop_in_progress) { + resolved.control->emergency_stopped = false; + resolved.control->last_error.clear(); + } else { + preempted_after_dispatch = true; + } + } + + // A false acknowledgement may still mean the PLC committed the + // request. A successful enable also needs rollback if cancellation or + // E-stop won while torqueOn was in flight. + const bool cleanup_required = + (!success && !preempted_after_dispatch) || + (success && request->enabled() && + (preempted_after_dispatch || cancelled_after_dispatch)); + if (cleanup_required) { + const bool stopped = resolved.motor->quickStop(); + bool disabled = true; + if (request->enabled()) { + disabled = resolved.motor->torqueOff(); + } + cleanup_succeeded = stopped && disabled; + } + } + if (!cleanup_succeeded) { + const std::string error = + "failed to reach a safe state after uncertain enable/disable dispatch"; + latchUnsafeAfterFailedStop(resolved.control, error); + lease.reset(); + fillFeedback(response->mutable_header(), false, error); + fillMotorStatus(resolved, response->mutable_status()); + response->set_elapsed_ms(elapsedMs(started)); + return grpc::Status(grpc::StatusCode::INTERNAL, error); + } + if (preempted_after_dispatch) { + const std::string error = + "enable/disable preempted by emergency stop during dispatch"; + setLastError(resolved.control, error); + lease.reset(); + fillFeedback(response->mutable_header(), false, error); + fillMotorStatus(resolved, response->mutable_status()); + response->set_elapsed_ms(elapsedMs(started)); + return grpc::Status(grpc::StatusCode::ABORTED, error); + } + if (cancelled_after_dispatch) { + setLastError( + resolved.control, post_dispatch_cancel_status.error_message()); + lease.reset(); + fillFeedback(response->mutable_header(), false, + post_dispatch_cancel_status.error_message()); + fillMotorStatus(resolved, response->mutable_status()); + response->set_elapsed_ms(elapsedMs(started)); + return post_dispatch_cancel_status; + } + if (!success) { + const std::string error = request->enabled() + ? "AbstractMotor rejected enable after safe disable" + : "AbstractMotor rejected disable after safe stop"; + setLastError(resolved.control, error); + lease.reset(); + fillFeedback(response->mutable_header(), false, error); + fillMotorStatus(resolved, response->mutable_status()); + response->set_elapsed_ms(elapsedMs(started)); + return grpc::Status(grpc::StatusCode::FAILED_PRECONDITION, error); + } + lease.reset(); + fillFeedback(response->mutable_header(), true); + fillMotorStatus(resolved, response->mutable_status()); + response->set_elapsed_ms(elapsedMs(started)); + return grpc::Status::OK; +} + +} // namespace cmvr::service diff --git a/cmvr-es/service/grpc/tests/grpc_motor_service_modbus_e2e_test.cpp b/cmvr-es/service/grpc/tests/grpc_motor_service_modbus_e2e_test.cpp new file mode 100644 index 00000000..142608d6 --- /dev/null +++ b/cmvr-es/service/grpc/tests/grpc_motor_service_modbus_e2e_test.cpp @@ -0,0 +1,907 @@ +#include "service/grpc/include/grpc_motor_service.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#include "cmvr/config/device_manager_config/device_manager_config.pb.h" +#include "cmvr/config/motor_config/motor_config.pb.h" +#include "common/config/config_files.h" +#include "devices/motor/bus_runtime/modbus_tcp/include/cmvr_plc_register_map.h" +#include "devices/motor/manager/include/motor_manager.h" +#include "manager/device_manager/include/device_manager.h" + +namespace cmvr::service { +namespace { + +using namespace std::chrono_literals; +namespace plc = device::cmvr_plc; + +constexpr char kManagerId[] = "e2e_plc_motors"; +constexpr char kMotorGroupId[] = "e2e_plc_axis_group"; +constexpr char kJointName[] = "E2E_PLC_AXIS"; + +std::uint16_t reserveLoopbackPort() +{ + const int socket_fd = ::socket(AF_INET, SOCK_STREAM, 0); + if (socket_fd < 0) { + return 0; + } + + sockaddr_in address{}; + address.sin_family = AF_INET; + address.sin_addr.s_addr = htonl(INADDR_LOOPBACK); + address.sin_port = 0; + if (::bind(socket_fd, reinterpret_cast(&address), + sizeof(address)) != 0) { + ::close(socket_fd); + return 0; + } + + socklen_t length = sizeof(address); + if (::getsockname(socket_fd, reinterpret_cast(&address), + &length) != 0) { + ::close(socket_fd); + return 0; + } + const auto port = ntohs(address.sin_port); + ::close(socket_fd); + return port; +} + +template +bool waitUntil(Predicate&& predicate, const std::chrono::milliseconds timeout) +{ + const auto deadline = std::chrono::steady_clock::now() + timeout; + do { + if (predicate()) { + return true; + } + std::this_thread::sleep_for(5ms); + } while (std::chrono::steady_clock::now() < deadline); + return predicate(); +} + +class FakeCmvrPlc final { +public: + ~FakeCmvrPlc() { stop(); } + + bool start() + { + port_ = reserveLoopbackPort(); + if (port_ == 0) { + std::fprintf(stderr, "reserveLoopbackPort failed: %s\n", + std::strerror(errno)); + return false; + } + + context_ = modbus_new_tcp("127.0.0.1", port_); + mapping_ = modbus_mapping_new( + 1, 1, plc::kAxisFirstOffset + plc::kAxisRegisterStride, 1); + if (!context_ || !mapping_) { + std::fprintf(stderr, "fake PLC allocation failed: %s\n", + modbus_strerror(errno)); + stop(); + return false; + } + + auto* registers = mapping_->tab_registers; + registers[plc::kMagicCmOffset] = plc::kMagicCm; + registers[plc::kMagicVrOffset] = plc::kMagicVr; + registers[plc::kProtocolMajorOffset] = plc::kProtocolMajor; + registers[plc::kProtocolMinorOffset] = plc::kProtocolMinor; + registers[plc::kAxisCountOffset] = 1; + registers[plc::kOwnerStateOffset] = + static_cast(plc::OwnerState::None); + plc::encodeUint32(registers, plc::kPlcBootIdOffset, 0x45453245U); + plc::encodeUint32(registers, plc::kPlcHeartbeatOffset, + plc_heartbeat_); + + const auto status_base = + plc::axisBase(0) + plc::kAxisStatusRelativeOffset; + registers[status_base + plc::kCommandState] = + static_cast(plc::CommandState::Idle); + registers[status_base + plc::kResultCode] = + static_cast(plc::ResultCode::Ok); + publishSnapshot_(registers + status_base); + + listen_socket_.store(modbus_tcp_listen(context_, 2)); + if (listen_socket_.load() < 0) { + std::fprintf(stderr, "modbus_tcp_listen failed on %u: %s\n", + port_, modbus_strerror(errno)); + stop(); + return false; + } + + running_.store(true); + worker_ = std::thread(&FakeCmvrPlc::loop_, this); + return true; + } + + void stop() + { + running_.store(false); + const auto client = client_socket_.exchange(-1); + if (client >= 0) { + ::shutdown(client, SHUT_RDWR); + ::close(client); + } + const auto listener = listen_socket_.exchange(-1); + if (listener >= 0) { + ::shutdown(listener, SHUT_RDWR); + ::close(listener); + } + if (worker_.joinable()) { + worker_.join(); + } + if (mapping_) { + modbus_mapping_free(mapping_); + mapping_ = nullptr; + } + if (context_) { + modbus_free(context_); + context_ = nullptr; + } + } + + void disconnectClient() + { + const auto client = client_socket_.load(); + if (client >= 0) { + ::shutdown(client, SHUT_RDWR); + } + } + + std::uint16_t port() const { return port_; } + std::uint32_t commandCount() const { return command_count_.load(); } + std::uint32_t acceptedSessionCount() const + { + return accepted_session_count_.load(); + } + std::uint32_t enableCount() const { return enable_count_.load(); } + std::uint32_t profilePositionCount() const + { + return profile_position_count_.load(); + } + std::uint32_t profileVelocityCount() const + { + return profile_velocity_count_.load(); + } + std::uint32_t cyclicOpenCount() const + { + return cyclic_open_count_.load(); + } + std::uint32_t cyclicSampleCount() const + { + return cyclic_sample_count_.load(); + } + std::uint32_t quickStopCount() const + { + return quick_stop_count_.load(); + } + +private: + void loop_() + { + std::array request{}; + while (running_.load()) { + int listener = listen_socket_.load(); + const int accepted = + listener < 0 ? -1 : modbus_tcp_accept(context_, &listener); + if (accepted < 0) { + if (running_.load()) { + std::this_thread::sleep_for(5ms); + } + continue; + } + + client_socket_.store(accepted); + while (running_.load()) { + const auto request_length = + modbus_receive(context_, request.data()); + if (request_length <= 0) { + break; + } + if (modbus_reply(context_, request.data(), request_length, + mapping_) < 0) { + break; + } + // Treat the mapping as the PLC's published process image. + // Read-only FC3 requests must not themselves advance the + // seqlock; otherwise the runtime's guard/full/guard snapshot + // validation could never observe one stable scan. + if (request_length > 7 && request[7] == 0x10U) { + processMailbox_(); + } + } + + const auto client = client_socket_.exchange(-1); + if (client >= 0) { + ::close(client); + } + } + } + + void processMailbox_() + { + auto* registers = mapping_->tab_registers; + const auto base = plc::axisBase(0); + const auto status_base = base + plc::kAxisStatusRelativeOffset; + beginSnapshot_(registers + status_base); + + plc::encodeUint32(registers, plc::kPlcHeartbeatOffset, + ++plc_heartbeat_); + const auto session = + plc::decodeUint32(registers, plc::kCmvrSessionIdOffset); + if (session != observed_session_) { + // A new session is not observable as Accepted until the old + // stream/mailbox state has been made safe. + registers[plc::kOwnerStateOffset] = + static_cast(plc::OwnerState::Accepting); + observed_session_ = session; + active_session_ = session; + last_sequence_ = 0; + registers[status_base + plc::kStatusFlags] &= + static_cast(~plc::StatusFlag::StreamActive); + plc::encodeUint32(registers + base, plc::kPayloadSequence, 0); + plc::encodeUint32(registers + base, + plc::kPayloadSequenceMirror, 0); + plc::encodeUint32(registers + base, + plc::kCommitSequenceRelativeOffset, 0); + plc::encodeUint32(registers, plc::kOwnerSessionIdOffset, + active_session_); + registers[plc::kOwnerStateOffset] = + static_cast( + active_session_ == 0 ? plc::OwnerState::None + : plc::OwnerState::Accepted); + if (active_session_ != 0) { + accepted_session_count_.fetch_add(1); + } + } + + if (active_session_ == 0 || active_session_ != session) { + publishSnapshot_(registers + status_base); + return; + } + + const auto sequence = + plc::decodeUint32(registers + base, plc::kPayloadSequence); + const auto sequence_mirror = + plc::decodeUint32(registers + base, + plc::kPayloadSequenceMirror); + const auto commit = + plc::decodeUint32(registers + base, + plc::kCommitSequenceRelativeOffset); + const auto command_session = + plc::decodeUint32(registers + base, plc::kCommandSessionId); + if (sequence == 0 || sequence == last_sequence_ || + sequence != sequence_mirror || sequence != commit || + command_session != active_session_) { + publishSnapshot_(registers + status_base); + return; + } + + last_sequence_ = sequence; + command_count_.fetch_add(1); + plc::encodeUint32(registers + status_base, plc::kAckSequence, + sequence); + plc::encodeUint32(registers + status_base, plc::kActiveSequence, + sequence); + plc::encodeUint32(registers + status_base, plc::kAckSessionId, + active_session_); + registers[status_base + plc::kResultCode] = + static_cast(plc::ResultCode::Ok); + + const auto code = static_cast( + registers[base + plc::kCommandCode]); + const auto target_position = + plc::decodeInt32(registers + base, plc::kTargetPosition); + const auto target_velocity = + plc::decodeInt32(registers + base, plc::kTargetVelocity); + auto& flags = registers[status_base + plc::kStatusFlags]; + auto state = plc::CommandState::Completed; + + switch (code) { + case plc::CommandCode::ProfilePosition: + profile_position_count_.fetch_add(1); + registers[status_base + plc::kCurrentMode] = + msgs::RUN_MODE_PROFILE_POSITION; + plc::encodeInt32(registers + status_base, + plc::kActualPosition, target_position); + plc::encodeInt32(registers + status_base, + plc::kActualVelocity, 0); + plc::encodeInt32(registers + status_base, + plc::kTargetPositionStatus, target_position); + flags |= plc::StatusFlag::TargetReached; + flags &= static_cast( + ~plc::StatusFlag::QuickStopActive); + state = plc::CommandState::TargetReached; + break; + case plc::CommandCode::ProfileVelocity: + profile_velocity_count_.fetch_add(1); + registers[status_base + plc::kCurrentMode] = + msgs::RUN_MODE_PROFILE_VELOCITY; + plc::encodeInt32(registers + status_base, + plc::kActualVelocity, target_velocity); + plc::encodeInt32(registers + status_base, + plc::kTargetVelocityStatus, target_velocity); + flags &= static_cast( + ~plc::StatusFlag::QuickStopActive); + state = plc::CommandState::Accepted; + break; + case plc::CommandCode::OpenCyclicPosition: + cyclic_open_count_.fetch_add(1); + registers[status_base + plc::kCurrentMode] = + msgs::RUN_MODE_CYCLIC_SYNC_POSITION; + plc::encodeUint32( + registers + status_base, + plc::kLastAppliedCyclicSequence, 0); + flags |= plc::StatusFlag::StreamActive; + flags &= static_cast( + ~plc::StatusFlag::QuickStopActive); + state = plc::CommandState::Accepted; + break; + case plc::CommandCode::CyclicPositionSample: + cyclic_sample_count_.fetch_add(1); + plc::encodeInt32(registers + status_base, + plc::kActualPosition, target_position); + plc::encodeInt32(registers + status_base, + plc::kActualVelocity, target_velocity); + plc::encodeUint32( + registers + status_base, + plc::kLastAppliedCyclicSequence, + plc::decodeUint32(registers + base, + plc::kCyclicSampleSequence)); + state = plc::CommandState::Accepted; + break; + case plc::CommandCode::QuickStop: + quick_stop_count_.fetch_add(1); + plc::encodeInt32(registers + status_base, + plc::kActualVelocity, 0); + flags |= plc::StatusFlag::QuickStopActive; + flags &= static_cast( + ~plc::StatusFlag::StreamActive); + state = plc::CommandState::QuickStopped; + break; + case plc::CommandCode::Enable: + enable_count_.fetch_add(1); + flags |= plc::StatusFlag::Enabled; + flags &= static_cast( + ~plc::StatusFlag::QuickStopActive); + break; + case plc::CommandCode::Disable: + flags &= static_cast( + ~plc::StatusFlag::Enabled); + flags &= static_cast( + ~plc::StatusFlag::StreamActive); + break; + default: + registers[status_base + plc::kResultCode] = + static_cast( + plc::ResultCode::Unsupported); + state = plc::CommandState::Rejected; + break; + } + + registers[status_base + plc::kCommandState] = + static_cast(state); + publishSnapshot_(registers + status_base); + } + + void publishSnapshot_(std::uint16_t* status) + { + if (pending_state_sequence_ == 0U) { + beginSnapshot_(status); + } + plc::encodeUint32(status, plc::kHeartbeatAge, 0); + plc::encodeUint32(status, plc::kStateSequenceMirror, + pending_state_sequence_); + plc::encodeUint32(status, plc::kStateSequence, + pending_state_sequence_); + state_sequence_ = pending_state_sequence_; + pending_state_sequence_ = 0U; + } + + void beginSnapshot_(std::uint16_t* status) + { + auto next = state_sequence_ + 2U; + if (next == 0U) { + next = 2U; + } + pending_state_sequence_ = next; + plc::encodeUint32(status, plc::kStateSequence, next - 1U); + } + + modbus_t* context_{nullptr}; + modbus_mapping_t* mapping_{nullptr}; + std::thread worker_; + std::atomic running_{false}; + std::atomic listen_socket_{-1}; + std::atomic client_socket_{-1}; + std::atomic command_count_{0}; + std::atomic accepted_session_count_{0}; + std::atomic enable_count_{0}; + std::atomic profile_position_count_{0}; + std::atomic profile_velocity_count_{0}; + std::atomic cyclic_open_count_{0}; + std::atomic cyclic_sample_count_{0}; + std::atomic quick_stop_count_{0}; + std::uint32_t observed_session_{0}; + std::uint32_t active_session_{0}; + std::uint32_t plc_heartbeat_{1}; + std::uint32_t state_sequence_{0}; + std::uint32_t pending_state_sequence_{0}; + std::uint32_t last_sequence_{0}; + std::uint16_t port_{0}; +}; + +std::string writeTemporaryMotorConfig(const std::uint16_t port) +{ + char path[] = "/tmp/cmvr_motor_service_modbus_e2e_XXXXXX.pb.txt"; + const int descriptor = ::mkstemps(path, 7); + if (descriptor < 0) { + return {}; + } + ::close(descriptor); + + std::ofstream output(path, std::ios::out | std::ios::trunc); + if (!output.is_open()) { + std::remove(path); + return {}; + } + + output << "motor {\n" + << " id: \"" << kManagerId << "\"\n" + << " motor_groups {\n" + << " id: \"" << kMotorGroupId << "\"\n" + << " bus_type: MOTOR_BUS_MODBUS_TCP\n" + << " vendor: MOTOR_VENDOR_PLC_GENERIC\n" + << " protocol: MOTOR_PROTOCOL_CMVR_PLC_V1\n" + << " modbus_tcp {\n" + << " host: \"127.0.0.1\"\n" + << " port: " << port << "\n" + << " unit_id: 1\n" + << " connect_timeout_ms: 200\n" + << " io_timeout_ms: 50\n" + << " heartbeat_period_ms: 20\n" + << " communication_watchdog_ms: 300\n" + << " status_poll_period_ms: 2\n" + << " reconnect_min_ms: 10\n" + << " reconnect_max_ms: 50\n" + << " command_ack_timeout_ms: 300\n" + << " cyclic_watchdog_ms: 300\n" + << " protocol_major: 1\n" + << " protocol_minor: 0\n" + << " axes { motor_id: 1 axis_index: 0 }\n" + << " }\n" + << " joint_limits {\n" + << " enable: true\n" + << " source: JOINT_LIMIT_SOURCE_CUSTOM\n" + << " joints {\n" + << " joint_name: \"" << kJointName << "\"\n" + << " q_lb: -2.0\n" + << " q_ub: 2.0\n" + << " qd: 2.0\n" + << " qdd: 4.0\n" + << " }\n" + << " }\n" + << " motors {\n" + << " motors { id: 1 joint_name: \"" << kJointName << "\" }\n" + << " }\n" + << " }\n" + << "}\n"; + output.close(); + if (!output) { + std::remove(path); + return {}; + } + return path; +} + +class MotorServiceModbusE2eTest : public ::testing::Test { +protected: + void SetUp() override + { + device::DeviceManager::destroyInstance(); + ASSERT_TRUE(plc_.start()); + + config_path_ = writeTemporaryMotorConfig(plc_.port()); + ASSERT_FALSE(config_path_.empty()); + + config::MotorRootConfig parsed; + ASSERT_TRUE(ConfigHelper::loadConfigFileSilent(config_path_, parsed)); + ASSERT_EQ(parsed.motor().id(), kManagerId); + ASSERT_EQ(parsed.motor().motor_groups_size(), 1); + + config::DeviceManagerConfig device_config; + device_config.set_init_all_motors_when_no_active_joints(true); + auto* entry = device_config.add_devices(); + entry->set_id(kManagerId); + entry->set_type( + config::DeviceConfigEntry::DEVICE_TYPE_MOTOR_SYSTEM); + entry->set_config_file(config_path_); + entry->set_enable(true); + + auto& device_manager = + device::DeviceManager::getInstance(device_config); + manager_ = + device_manager.getDevice(kManagerId); + ASSERT_NE(manager_, nullptr); + ASSERT_NE(manager_->getMotor(1), nullptr); + ASSERT_EQ(manager_->getMotor(1)->jointName(), kJointName); + + service_ = std::make_unique(); + ASSERT_TRUE(startGrpcServer_()); + } + + void TearDown() override + { + if (grpc_server_) { + grpc_server_->Shutdown(); + grpc_server_->Wait(); + grpc_server_.reset(); + } + stub_.reset(); + service_.reset(); + + if (manager_) { + manager_->stop(); + manager_.reset(); + } + device::DeviceManager::destroyInstance(); + plc_.stop(); + + if (!grpc_socket_path_.empty()) { + std::remove(grpc_socket_path_.c_str()); + grpc_socket_path_.clear(); + } + if (!config_path_.empty()) { + std::remove(config_path_.c_str()); + config_path_.clear(); + } + } + + static api::MotorTarget makeTarget_() + { + api::MotorTarget target; + target.mutable_header()->set_device_id(kManagerId); + target.set_motor_id(1); + return target; + } + + bool startGrpcServer_() + { + grpc_socket_path_ = + "/tmp/cmvr_motor_service_modbus_e2e_" + + std::to_string(static_cast(::getpid())) + ".sock"; + std::remove(grpc_socket_path_.c_str()); + const std::string server_address = "unix:" + grpc_socket_path_; + + grpc::ServerBuilder builder; + builder.AddListeningPort( + server_address, grpc::InsecureServerCredentials()); + builder.RegisterService(service_.get()); + grpc_server_ = builder.BuildAndStart(); + if (!grpc_server_) { + return false; + } + stub_ = api::MotorService::NewStub( + grpc::CreateChannel( + server_address, + grpc::InsecureChannelCredentials())); + return stub_ != nullptr; + } + + FakeCmvrPlc plc_; + std::shared_ptr manager_; + std::unique_ptr service_; + std::unique_ptr grpc_server_; + std::unique_ptr stub_; + std::string config_path_; + std::string grpc_socket_path_; +}; + +TEST_F(MotorServiceModbusE2eTest, + StubTraversesManagerAndModbusRuntimeWithSafetyAndReconnect) +{ + api::SetMotorEnabledRequest enable_request; + *enable_request.mutable_target() = makeTarget_(); + enable_request.set_enabled(true); + grpc::ClientContext enable_context; + enable_context.set_deadline( + std::chrono::system_clock::now() + 3s); + api::MotorCommandResponse enable_response; + const auto enable_status = + stub_->setEnabled(&enable_context, enable_request, &enable_response); + ASSERT_TRUE(enable_status.ok()) << enable_status.error_message(); + ASSERT_TRUE(enable_response.header().success()) + << enable_response.header().error_message(); + EXPECT_EQ(plc_.enableCount(), 1U); + + api::ProfilePositionRequest position_request; + *position_request.mutable_target() = makeTarget_(); + position_request.set_target_position_rad(0.75); + position_request.set_max_velocity_rad_s(0.5); + position_request.set_acceleration_rad_s2(1.0); + position_request.mutable_wait()->set_timeout_ms(1000); + position_request.mutable_wait()->set_poll_period_ms(2); + position_request.mutable_wait()->set_settle_sample_count(1); + grpc::ClientContext position_context; + position_context.set_deadline( + std::chrono::system_clock::now() + 3s); + api::MotorCommandResponse position_response; + const auto position_status = stub_->profilePosition( + &position_context, position_request, &position_response); + ASSERT_TRUE(position_status.ok()) << position_status.error_message(); + ASSERT_TRUE(position_response.header().success()) + << position_response.header().error_message(); + EXPECT_NEAR(position_response.status().position_rad(), 0.75, 1e-6); + EXPECT_TRUE(position_response.status().target_reached()); + EXPECT_EQ(plc_.profilePositionCount(), 1U); + + api::GetMotorStatusRequest get_status_request; + *get_status_request.mutable_target() = makeTarget_(); + grpc::ClientContext get_status_context; + get_status_context.set_deadline( + std::chrono::system_clock::now() + 3s); + api::GetMotorStatusResponse get_status_response; + const auto get_status = stub_->getStatus( + &get_status_context, get_status_request, &get_status_response); + ASSERT_TRUE(get_status.ok()) << get_status.error_message(); + ASSERT_TRUE(get_status_response.header().success()) + << get_status_response.header().error_message(); + EXPECT_EQ(get_status_response.status().motor_id(), 1U); + EXPECT_EQ(get_status_response.status().joint_name(), kJointName); + EXPECT_EQ(get_status_response.status().run_mode(), + msgs::RUN_MODE_PROFILE_POSITION); + EXPECT_NEAR(get_status_response.status().position_rad(), 0.75, 1e-6); + + api::ProfileVelocityRequest velocity_request; + *velocity_request.mutable_target() = makeTarget_(); + velocity_request.set_target_velocity_rad_s(-0.2); + velocity_request.set_acceleration_rad_s2(0.5); + velocity_request.mutable_wait()->set_timeout_ms(1000); + velocity_request.mutable_wait()->set_poll_period_ms(2); + velocity_request.mutable_wait()->set_settle_sample_count(1); + grpc::ClientContext velocity_context; + velocity_context.set_deadline( + std::chrono::system_clock::now() + 3s); + api::MotorCommandResponse velocity_response; + const auto velocity_status = stub_->profileVelocity( + &velocity_context, velocity_request, &velocity_response); + ASSERT_TRUE(velocity_status.ok()) << velocity_status.error_message(); + ASSERT_TRUE(velocity_response.header().success()) + << velocity_response.header().error_message(); + EXPECT_NEAR(velocity_response.status().velocity_rad_s(), -0.2, 1e-6); + EXPECT_EQ(plc_.profileVelocityCount(), 1U); + + const auto quick_stops_before_stream = plc_.quickStopCount(); + grpc::ClientContext stream_context; + stream_context.set_deadline( + std::chrono::system_clock::now() + 3s); + auto stream = stub_->streamCyclicPosition(&stream_context); + ASSERT_NE(stream, nullptr); + + api::CyclicPositionRequest open_request; + *open_request.mutable_open()->mutable_target() = makeTarget_(); + open_request.mutable_open()->set_watchdog_timeout_ms(500); + ASSERT_TRUE(stream->Write(open_request)); + + api::CyclicControlResponse stream_response; + ASSERT_TRUE(stream->Read(&stream_response)); + ASSERT_TRUE(stream_response.header().success()) + << stream_response.header().error_message(); + EXPECT_EQ(stream_response.phase(), api::CYCLIC_STREAM_OPENED); + + api::CyclicPositionRequest setpoint_request; + auto* setpoint = setpoint_request.mutable_setpoint(); + setpoint->set_sequence(1); + setpoint->set_target_position_rad(0.25); + setpoint->set_target_velocity_rad_s(0.1); + ASSERT_TRUE(stream->Write(setpoint_request)); + + stream_response.Clear(); + ASSERT_TRUE(stream->Read(&stream_response)); + ASSERT_TRUE(stream_response.header().success()) + << stream_response.header().error_message(); + EXPECT_EQ(stream_response.phase(), api::CYCLIC_STREAM_APPLIED); + EXPECT_EQ(stream_response.sequence(), 1U); + EXPECT_FALSE(stream_response.has_status()); + EXPECT_EQ(plc_.cyclicOpenCount(), 1U); + EXPECT_EQ(plc_.cyclicSampleCount(), 1U); + + ASSERT_TRUE(stream->WritesDone()); + stream_response.Clear(); + ASSERT_TRUE(stream->Read(&stream_response)); + EXPECT_TRUE(stream_response.header().success()) + << stream_response.header().error_message(); + EXPECT_EQ(stream_response.phase(), api::CYCLIC_STREAM_STOPPED); + EXPECT_FALSE(stream->Read(&stream_response)); + const auto stream_finish = stream->Finish(); + ASSERT_TRUE(stream_finish.ok()) << stream_finish.error_message(); + EXPECT_GT(plc_.quickStopCount(), quick_stops_before_stream); + + const auto quick_stops_before_emergency = plc_.quickStopCount(); + api::EmergencyStopRequest emergency_request; + *emergency_request.mutable_target() = makeTarget_(); + grpc::ClientContext emergency_context; + emergency_context.set_deadline( + std::chrono::system_clock::now() + 3s); + api::MotorCommandResponse emergency_response; + const auto emergency_status = stub_->emergencyStop( + &emergency_context, emergency_request, &emergency_response); + ASSERT_TRUE(emergency_status.ok()) << emergency_status.error_message(); + ASSERT_TRUE(emergency_response.header().success()) + << emergency_response.header().error_message(); + EXPECT_TRUE(emergency_response.status().emergency_stopped()); + EXPECT_GT(plc_.quickStopCount(), quick_stops_before_emergency); + + const auto commands_before_rejected_motion = plc_.commandCount(); + api::ProfilePositionRequest rejected_request; + *rejected_request.mutable_target() = makeTarget_(); + rejected_request.set_target_position_rad(0.5); + rejected_request.set_max_velocity_rad_s(0.5); + rejected_request.set_acceleration_rad_s2(1.0); + grpc::ClientContext rejected_context; + rejected_context.set_deadline( + std::chrono::system_clock::now() + 3s); + api::MotorCommandResponse rejected_response; + const auto rejected_status = stub_->profilePosition( + &rejected_context, rejected_request, &rejected_response); + EXPECT_EQ(rejected_status.error_code(), + grpc::StatusCode::FAILED_PRECONDITION); + EXPECT_FALSE(rejected_response.header().success()); + EXPECT_EQ(plc_.commandCount(), commands_before_rejected_motion); + + const auto sessions_before_disconnect = + plc_.acceptedSessionCount(); + const auto commands_before_disconnect = plc_.commandCount(); + plc_.disconnectClient(); + ASSERT_TRUE(waitUntil( + [&] { + return plc_.acceptedSessionCount() > + sessions_before_disconnect; + }, + 3s)); + std::this_thread::sleep_for(100ms); + EXPECT_EQ(plc_.commandCount(), commands_before_disconnect); + + api::SetMotorEnabledRequest reenable_request; + *reenable_request.mutable_target() = makeTarget_(); + reenable_request.set_enabled(true); + grpc::ClientContext reenable_context; + reenable_context.set_deadline( + std::chrono::system_clock::now() + 3s); + api::MotorCommandResponse reenable_response; + const auto reenable_status = stub_->setEnabled( + &reenable_context, reenable_request, &reenable_response); + ASSERT_TRUE(reenable_status.ok()) << reenable_status.error_message(); + EXPECT_TRUE(reenable_response.header().success()) + << reenable_response.header().error_message(); + EXPECT_EQ(plc_.commandCount(), commands_before_disconnect + 1U); + EXPECT_FALSE(reenable_response.status().emergency_stopped()); +} + +TEST_F(MotorServiceModbusE2eTest, + ActiveCyclicStreamFailsClosedAcrossReconnectAndNewStreamRecovers) +{ + grpc::ClientContext old_context; + old_context.set_deadline(std::chrono::system_clock::now() + 8s); + auto old_stream = stub_->streamCyclicPosition(&old_context); + ASSERT_NE(old_stream, nullptr); + + api::CyclicPositionRequest open_request; + *open_request.mutable_open()->mutable_target() = makeTarget_(); + open_request.mutable_open()->set_watchdog_timeout_ms(1000); + ASSERT_TRUE(old_stream->Write(open_request)); + + api::CyclicControlResponse response; + ASSERT_TRUE(old_stream->Read(&response)); + ASSERT_TRUE(response.header().success()) + << response.header().error_message(); + ASSERT_EQ(response.phase(), api::CYCLIC_STREAM_OPENED); + + api::CyclicPositionRequest first_sample; + first_sample.mutable_setpoint()->set_sequence(1); + first_sample.mutable_setpoint()->set_target_position_rad(0.1); + first_sample.mutable_setpoint()->set_target_velocity_rad_s(0.0); + ASSERT_TRUE(old_stream->Write(first_sample)); + response.Clear(); + ASSERT_TRUE(old_stream->Read(&response)); + ASSERT_TRUE(response.header().success()) + << response.header().error_message(); + ASSERT_EQ(response.phase(), api::CYCLIC_STREAM_APPLIED); + ASSERT_EQ(plc_.cyclicOpenCount(), 1U); + ASSERT_EQ(plc_.cyclicSampleCount(), 1U); + + const auto accepted_sessions_before = plc_.acceptedSessionCount(); + plc_.disconnectClient(); + ASSERT_TRUE(waitUntil( + [&] { + return plc_.acceptedSessionCount() > + accepted_sessions_before; + }, + 3s)); + + const auto opens_after_reconnect = plc_.cyclicOpenCount(); + const auto samples_after_reconnect = plc_.cyclicSampleCount(); + const auto stops_after_reconnect = plc_.quickStopCount(); + api::CyclicPositionRequest stale_sample; + stale_sample.mutable_setpoint()->set_sequence(2); + stale_sample.mutable_setpoint()->set_target_position_rad(0.2); + stale_sample.mutable_setpoint()->set_target_velocity_rad_s(0.0); + ASSERT_TRUE(old_stream->Write(stale_sample)); + + response.Clear(); + ASSERT_TRUE(old_stream->Read(&response)); + EXPECT_FALSE(response.header().success()); + EXPECT_EQ(response.phase(), api::CYCLIC_STREAM_FAILED); + EXPECT_NE(response.header().error_message().find( + "rejected cyclic position setpoint"), + std::string::npos); + EXPECT_FALSE(old_stream->Read(&response)); + const auto old_finish = old_stream->Finish(); + EXPECT_TRUE( + old_finish.error_code() == grpc::StatusCode::FAILED_PRECONDITION || + old_finish.error_code() == grpc::StatusCode::CANCELLED) + << old_finish.error_message(); + // Stream cleanup may commit a safety QuickStop, but the stale generation + // must never commit an Open or sample into the new PLC session. + EXPECT_EQ(plc_.cyclicOpenCount(), opens_after_reconnect); + EXPECT_EQ(plc_.cyclicSampleCount(), samples_after_reconnect); + EXPECT_GT(plc_.quickStopCount(), stops_after_reconnect); + + grpc::ClientContext new_context; + new_context.set_deadline(std::chrono::system_clock::now() + 5s); + auto new_stream = stub_->streamCyclicPosition(&new_context); + ASSERT_NE(new_stream, nullptr); + ASSERT_TRUE(new_stream->Write(open_request)); + + response.Clear(); + ASSERT_TRUE(new_stream->Read(&response)); + ASSERT_TRUE(response.header().success()) + << response.header().error_message(); + ASSERT_EQ(response.phase(), api::CYCLIC_STREAM_OPENED); + + api::CyclicPositionRequest fresh_sample; + fresh_sample.mutable_setpoint()->set_sequence(1); + fresh_sample.mutable_setpoint()->set_target_position_rad(0.3); + fresh_sample.mutable_setpoint()->set_target_velocity_rad_s(0.0); + ASSERT_TRUE(new_stream->Write(fresh_sample)); + response.Clear(); + ASSERT_TRUE(new_stream->Read(&response)); + ASSERT_TRUE(response.header().success()) + << response.header().error_message(); + EXPECT_EQ(response.phase(), api::CYCLIC_STREAM_APPLIED); + EXPECT_EQ(plc_.cyclicOpenCount(), opens_after_reconnect + 1U); + EXPECT_EQ(plc_.cyclicSampleCount(), samples_after_reconnect + 1U); + + ASSERT_TRUE(new_stream->WritesDone()); + response.Clear(); + ASSERT_TRUE(new_stream->Read(&response)); + EXPECT_TRUE(response.header().success()) + << response.header().error_message(); + EXPECT_EQ(response.phase(), api::CYCLIC_STREAM_STOPPED); + EXPECT_FALSE(new_stream->Read(&response)); + const auto new_finish = new_stream->Finish(); + EXPECT_TRUE(new_finish.ok()) << new_finish.error_message(); +} + +} // namespace +} // namespace cmvr::service diff --git a/cmvr-es/service/grpc/tests/grpc_motor_service_test.cpp b/cmvr-es/service/grpc/tests/grpc_motor_service_test.cpp new file mode 100644 index 00000000..79ecf8f5 --- /dev/null +++ b/cmvr-es/service/grpc/tests/grpc_motor_service_test.cpp @@ -0,0 +1,1586 @@ +#include "service/grpc/include/grpc_motor_service.h" + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include "cmvr/config/device_manager_config/device_manager_config.pb.h" +#include "cmvr/config/motor_config/motor_config.pb.h" +#include "devices/motor/manager/include/motor_manager.h" +#include "devices/motor/motor_protocol_interface.h" +#include "manager/device_manager/include/device_manager.h" + +namespace cmvr::service { + +class gRPCMotorServiceImplTestAccess { +public: + static void bestEffortQuickStop(gRPCMotorServiceImpl& service, + const api::MotorTarget& target, + const std::string& error) + { + service.bestEffortQuickStop(target, error); + } + + static std::unique_lock holdExceptionCleanupMutex( + gRPCMotorServiceImpl& service, + const api::MotorTarget& target) + { + gRPCMotorServiceImpl::ResolvedMotor resolved; + const auto status = service.resolveMotor(target, resolved); + if (!status.ok()) { + throw std::runtime_error(status.error_message()); + } + return std::unique_lock( + resolved.control->exception_cleanup_mutex); + } +}; + +namespace { + +class FakeMotorProtocol final : public device::MotorProtocolInterface { +public: + bool initNode(std::uint8_t) override { return true; } + void setMode(std::uint8_t, msgs::RunMode mode) override + { + if (throw_set_mode_.load()) { + throw std::runtime_error("injected setMode exception"); + } + mode_ = mode; + } + msgs::RunMode getMode(std::uint8_t) override { return mode_.load(); } + void setLimitQdd(std::uint8_t, double, double) override {} + void setLimitQd(std::uint8_t, double) override {} + void setLimitQ(std::uint8_t, double, double) override {} + bool calibrateZeroQ(std::uint8_t) override + { + calibrate_started_ = true; + if (calibrate_delay_ms_.load() > 0) { + std::this_thread::sleep_for( + std::chrono::milliseconds(calibrate_delay_ms_.load())); + } + position_ = 0.0; + calibrate_finished_ = true; + return calibrate_success_.load(); + } + bool reachedTargetQ(std::uint8_t) override { return reached_.load(); } + bool commandProfilePosition(std::uint8_t, double target_q, + double, double) override + { + command_started_ = true; + if (throw_profile_position_.load()) { + throw std::runtime_error("injected profile position exception"); + } + profile_command_order_ = ++operation_counter_; + if (profile_delay_ms_.load() > 0) { + std::this_thread::sleep_for( + std::chrono::milliseconds(profile_delay_ms_.load())); + } + if (!hold_position_.load()) { + position_ = target_q; + velocity_ = 0.0; + reached_ = true; + } + return profile_position_success_.load(); + } + bool commandProfileVelocity(std::uint8_t, double target_qd, + double) override + { + profile_velocity_started_ = true; + if (profile_velocity_delay_ms_.load() > 0) { + std::this_thread::sleep_for( + std::chrono::milliseconds(profile_velocity_delay_ms_.load())); + } + velocity_ = target_qd; + return profile_velocity_success_.load(); + } + bool commandCyclicPosition(std::uint8_t, double target_q, + double target_qd) override + { + cyclic_position_started_ = true; + if (cyclic_position_delay_ms_.load() > 0) { + std::this_thread::sleep_for( + std::chrono::milliseconds(cyclic_position_delay_ms_.load())); + } + if (throw_cyclic_position_.load()) { + throw std::runtime_error("injected cyclic position exception"); + } + position_ = target_q; + velocity_ = target_qd; + return true; + } + bool commandCyclicVelocity(std::uint8_t, double target_qd) override + { + velocity_ = target_qd; + return true; + } + bool commandCyclicTorque(std::uint8_t, double) override { return false; } + void setMotorConversion(std::uint8_t, double, double) override {} + bool torqueOn(std::uint8_t) override + { + torque_on_started_ = true; + torque_on_order_ = ++operation_counter_; + if (torque_on_delay_ms_.load() > 0) { + std::this_thread::sleep_for( + std::chrono::milliseconds(torque_on_delay_ms_.load())); + } + return torque_on_success_.load(); + } + bool torqueOff(std::uint8_t) override + { + ++torque_off_count_; + return torque_off_success_.load(); + } + bool brakeRelease(std::uint8_t) override { return true; } + bool quickStop(std::uint8_t) override + { + last_quick_stop_order_ = ++operation_counter_; + ++quick_stop_count_; + if (quick_stop_delay_ms_.load() > 0) { + std::this_thread::sleep_for( + std::chrono::milliseconds(quick_stop_delay_ms_.load())); + } + if (quick_stop_success_.load()) { + velocity_ = 0.0; + return true; + } + return false; + } + double getQ(std::uint8_t) override + { + get_q_started_ = true; + if (get_q_delay_ms_.load() > 0) { + std::this_thread::sleep_for( + std::chrono::milliseconds(get_q_delay_ms_.load())); + } + if (throw_get_q_.load()) { + throw std::runtime_error("injected getQ exception"); + } + if (nonfinite_get_q_.load()) { + return std::numeric_limits::quiet_NaN(); + } + return position_.load(); + } + double getQd(std::uint8_t) override + { + get_qd_started_ = true; + if (nonfinite_get_qd_.load()) { + return std::numeric_limits::quiet_NaN(); + } + return velocity_.load(); + } + + std::atomic hold_position_{false}; + std::atomic calibrate_started_{false}; + std::atomic calibrate_finished_{false}; + std::atomic calibrate_delay_ms_{0}; + std::atomic calibrate_success_{true}; + std::atomic command_started_{false}; + std::atomic quick_stop_count_{0}; + std::atomic quick_stop_success_{true}; + std::atomic quick_stop_delay_ms_{0}; + std::atomic throw_set_mode_{false}; + std::atomic throw_profile_position_{false}; + std::atomic throw_cyclic_position_{false}; + std::atomic throw_get_q_{false}; + std::atomic nonfinite_get_q_{false}; + std::atomic nonfinite_get_qd_{false}; + std::atomic get_qd_started_{false}; + std::atomic profile_position_success_{true}; + std::atomic profile_velocity_success_{true}; + std::atomic torque_on_success_{true}; + std::atomic torque_off_success_{true}; + std::atomic torque_off_count_{0}; + std::atomic profile_delay_ms_{0}; + std::atomic profile_velocity_delay_ms_{0}; + std::atomic profile_velocity_started_{false}; + std::atomic cyclic_position_delay_ms_{0}; + std::atomic cyclic_position_started_{false}; + std::atomic get_q_delay_ms_{0}; + std::atomic get_q_started_{false}; + std::atomic torque_on_delay_ms_{0}; + std::atomic torque_on_started_{false}; + std::atomic operation_counter_{0}; + std::atomic profile_command_order_{0}; + std::atomic torque_on_order_{0}; + std::atomic last_quick_stop_order_{0}; + +private: + std::atomic mode_{msgs::RUN_MODE_UNSPECIFIED}; + std::atomic position_{0.0}; + std::atomic velocity_{0.0}; + std::atomic reached_{false}; +}; + +class FakeMotor final : public device::AbstractMotor { +public: + explicit FakeMotor(const std::uint8_t node_id) + : AbstractMotor(node_id) + { + info_.id = node_id; + info_.joint_name = "test_joint"; + } + + std::string typeName() const override { return "FakeMotor"; } +}; + +class MotorServiceTest : public ::testing::Test { +protected: + void SetUp() override + { + config::DeviceManagerConfig device_config; + auto& device_manager = device::DeviceManager::getInstance(device_config); + + config::MotorConfig motor_config; + manager_ = std::make_shared( + "test-motor-manager", motor_config); + protocol_ = std::make_shared(); + motor_ = std::make_shared(1); + motor_->setProtocol(protocol_); + ASSERT_TRUE(manager_->addMotor(motor_)); + device_manager.registerDevice("test-motor-manager", manager_); + service_ = std::make_unique(); + } + + void TearDown() override + { + if (grpc_server_) { + grpc_server_->Shutdown(); + grpc_server_->Wait(); + grpc_server_.reset(); + } + stub_.reset(); + if (!grpc_socket_path_.empty()) { + std::remove(grpc_socket_path_.c_str()); + grpc_socket_path_.clear(); + } + service_.reset(); + manager_.reset(); + motor_.reset(); + protocol_.reset(); + device::DeviceManager::destroyInstance(); + } + + static api::MotorTarget makeTarget() + { + api::MotorTarget target; + target.mutable_header()->set_device_id("test-motor-manager"); + target.set_motor_id(1); + return target; + } + + bool startGrpcServer() + { + grpc_socket_path_ = + "/tmp/cmvr_motor_service_test_" + + std::to_string(static_cast(::getpid())) + ".sock"; + std::remove(grpc_socket_path_.c_str()); + const std::string server_address = "unix:" + grpc_socket_path_; + + grpc::ServerBuilder builder; + builder.AddListeningPort( + server_address, grpc::InsecureServerCredentials()); + builder.RegisterService(service_.get()); + grpc_server_ = builder.BuildAndStart(); + if (!grpc_server_) { + return false; + } + stub_ = api::MotorService::NewStub( + grpc::CreateChannel( + server_address, + grpc::InsecureChannelCredentials())); + return stub_ != nullptr; + } + + std::shared_ptr protocol_; + std::shared_ptr motor_; + std::shared_ptr manager_; + std::unique_ptr service_; + std::unique_ptr grpc_server_; + std::unique_ptr stub_; + std::string grpc_socket_path_; +}; + +TEST_F(MotorServiceTest, SetZeroAckLossReportsUnknownOutcome) +{ + protocol_->calibrate_success_ = false; + + api::SetMotorZeroRequest request; + *request.mutable_target() = makeTarget(); + grpc::ServerContext context; + api::MotorCommandResponse response; + const auto status = service_->setZero(&context, &request, &response); + + EXPECT_EQ(status.error_code(), grpc::StatusCode::FAILED_PRECONDITION); + EXPECT_FALSE(response.header().success()); + EXPECT_NE(response.header().error_message().find("outcome unknown"), + std::string::npos); + EXPECT_NE(response.header().error_message().find("zero_epoch"), + std::string::npos); + EXPECT_EQ(protocol_->quick_stop_count_.load(), 0); +} + +TEST_F(MotorServiceTest, RejectedSetZeroPreemptedInFlightReturnsAborted) +{ + protocol_->calibrate_success_ = false; + protocol_->calibrate_delay_ms_ = 100; + + api::SetMotorZeroRequest request; + *request.mutable_target() = makeTarget(); + grpc::ServerContext zero_context; + api::MotorCommandResponse zero_response; + grpc::Status zero_status; + std::thread zeroing([&]() { + zero_status = service_->setZero( + &zero_context, &request, &zero_response); + }); + const auto dispatch_deadline = + std::chrono::steady_clock::now() + std::chrono::seconds(1); + while (!protocol_->calibrate_started_.load() && + std::chrono::steady_clock::now() < dispatch_deadline) { + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + } + ASSERT_TRUE(protocol_->calibrate_started_.load()); + + api::EmergencyStopRequest stop_request; + *stop_request.mutable_target() = makeTarget(); + grpc::ServerContext stop_context; + api::MotorCommandResponse stop_response; + const auto stop_status = service_->emergencyStop( + &stop_context, &stop_request, &stop_response); + + zeroing.join(); + ASSERT_TRUE(stop_status.ok()) << stop_status.error_message(); + EXPECT_EQ(zero_status.error_code(), grpc::StatusCode::ABORTED); +} + +TEST_F(MotorServiceTest, SetZeroDeadlineDuringDispatchReportsDeadline) +{ + ASSERT_TRUE(startGrpcServer()); + protocol_->calibrate_success_ = false; + protocol_->calibrate_delay_ms_ = 100; + + api::SetMotorZeroRequest request; + *request.mutable_target() = makeTarget(); + grpc::ClientContext context; + context.set_deadline( + std::chrono::system_clock::now() + std::chrono::milliseconds(50)); + api::MotorCommandResponse response; + const auto status = stub_->setZero(&context, request, &response); + EXPECT_EQ(status.error_code(), grpc::StatusCode::DEADLINE_EXCEEDED); + + const auto completion_deadline = + std::chrono::steady_clock::now() + std::chrono::milliseconds(300); + while (!protocol_->calibrate_finished_.load() && + std::chrono::steady_clock::now() < completion_deadline) { + std::this_thread::sleep_for(std::chrono::milliseconds(2)); + } + EXPECT_TRUE(protocol_->calibrate_finished_.load()); + EXPECT_EQ(protocol_->quick_stop_count_.load(), 0); +} + +TEST_F(MotorServiceTest, ProfilePositionReturnsOnlyAfterTargetIsReached) +{ + api::ProfilePositionRequest request; + *request.mutable_target() = makeTarget(); + request.set_target_position_rad(1.25); + request.set_max_velocity_rad_s(1.0); + request.set_acceleration_rad_s2(2.0); + request.mutable_wait()->set_settle_sample_count(1); + request.mutable_wait()->set_poll_period_ms(1); + + grpc::ServerContext context; + api::MotorCommandResponse response; + const auto status = service_->profilePosition(&context, &request, &response); + + ASSERT_TRUE(status.ok()) << status.error_message(); + EXPECT_TRUE(response.header().success()); + EXPECT_DOUBLE_EQ(response.status().position_rad(), 1.25); + EXPECT_FALSE(response.status().service_busy()); + EXPECT_EQ(response.status().active_control(), api::MOTOR_CONTROL_NONE); +} + +TEST_F(MotorServiceTest, ProfileVelocityReturnsAfterTargetSettles) +{ + api::ProfileVelocityRequest request; + *request.mutable_target() = makeTarget(); + request.set_target_velocity_rad_s(1.5); + request.set_acceleration_rad_s2(2.0); + request.mutable_wait()->set_settle_sample_count(2); + request.mutable_wait()->set_poll_period_ms(1); + request.mutable_wait()->set_velocity_tolerance_rad_s(1e-4); + + grpc::ServerContext context; + api::MotorCommandResponse response; + const auto status = service_->profileVelocity(&context, &request, &response); + + ASSERT_TRUE(status.ok()) << status.error_message(); + EXPECT_TRUE(response.header().success()); + EXPECT_DOUBLE_EQ(response.status().velocity_rad_s(), 1.5); + EXPECT_FALSE(response.status().service_busy()); + EXPECT_EQ(response.status().active_control(), api::MOTOR_CONTROL_NONE); +} + +TEST_F(MotorServiceTest, ProfilePositionStopsImmediatelyOnNonFiniteFeedback) +{ + protocol_->hold_position_ = true; + + api::ProfilePositionRequest request; + *request.mutable_target() = makeTarget(); + request.set_target_position_rad(1.0); + request.set_max_velocity_rad_s(1.0); + request.set_acceleration_rad_s2(1.0); + request.mutable_wait()->set_timeout_ms(5000); + request.mutable_wait()->set_poll_period_ms(2); + + grpc::ServerContext context; + api::MotorCommandResponse response; + grpc::Status status; + const auto started = std::chrono::steady_clock::now(); + std::thread motion([&]() { + status = service_->profilePosition(&context, &request, &response); + }); + + const auto sample_deadline = + std::chrono::steady_clock::now() + std::chrono::seconds(1); + while (!protocol_->get_q_started_.load() && + std::chrono::steady_clock::now() < sample_deadline) { + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + } + if (!protocol_->get_q_started_.load()) { + context.TryCancel(); + motion.join(); + FAIL() << "profile position feedback sampling did not start"; + return; + } + + protocol_->nonfinite_get_q_ = true; + motion.join(); + + EXPECT_EQ(status.error_code(), grpc::StatusCode::UNAVAILABLE); + EXPECT_FALSE(response.header().success()); + EXPECT_FALSE(response.status().emergency_stopped()); + EXPECT_GE(protocol_->quick_stop_count_.load(), 1); + EXPECT_LT(std::chrono::steady_clock::now() - started, + std::chrono::seconds(1)); +} + +TEST_F(MotorServiceTest, + ProfileVelocityFailedStopOnNonFiniteFeedbackLatchesEmergency) +{ + protocol_->quick_stop_success_ = false; + + api::ProfileVelocityRequest request; + *request.mutable_target() = makeTarget(); + request.set_target_velocity_rad_s(1.5); + request.set_acceleration_rad_s2(2.0); + request.mutable_wait()->set_timeout_ms(5000); + request.mutable_wait()->set_poll_period_ms(2); + request.mutable_wait()->set_settle_sample_count(1000); + + grpc::ServerContext context; + api::MotorCommandResponse response; + grpc::Status status; + const auto started = std::chrono::steady_clock::now(); + std::thread motion([&]() { + status = service_->profileVelocity(&context, &request, &response); + }); + + const auto sample_deadline = + std::chrono::steady_clock::now() + std::chrono::seconds(1); + while (!protocol_->get_qd_started_.load() && + std::chrono::steady_clock::now() < sample_deadline) { + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + } + if (!protocol_->get_qd_started_.load()) { + context.TryCancel(); + motion.join(); + FAIL() << "profile velocity feedback sampling did not start"; + return; + } + + protocol_->nonfinite_get_qd_ = true; + motion.join(); + + EXPECT_EQ(status.error_code(), grpc::StatusCode::INTERNAL); + EXPECT_FALSE(response.header().success()); + EXPECT_TRUE(response.status().emergency_stopped()); + EXPECT_GE(protocol_->quick_stop_count_.load(), 1); + EXPECT_LT(std::chrono::steady_clock::now() - started, + std::chrono::seconds(1)); + + api::ProfilePositionRequest blocked_request; + *blocked_request.mutable_target() = makeTarget(); + blocked_request.set_target_position_rad(1.0); + blocked_request.set_max_velocity_rad_s(1.0); + blocked_request.set_acceleration_rad_s2(1.0); + grpc::ServerContext blocked_context; + api::MotorCommandResponse blocked_response; + const auto blocked_status = service_->profilePosition( + &blocked_context, &blocked_request, &blocked_response); + EXPECT_EQ(blocked_status.error_code(), + grpc::StatusCode::FAILED_PRECONDITION); + EXPECT_TRUE(blocked_response.status().emergency_stopped()); +} + +TEST_F(MotorServiceTest, ProfileBackendExceptionReturnsInternalAndQuickStops) +{ + protocol_->throw_profile_position_ = true; + + api::ProfilePositionRequest request; + *request.mutable_target() = makeTarget(); + request.set_target_position_rad(1.0); + request.set_max_velocity_rad_s(1.0); + request.set_acceleration_rad_s2(1.0); + + grpc::ServerContext context; + api::MotorCommandResponse response; + const auto status = service_->profilePosition(&context, &request, &response); + + EXPECT_EQ(status.error_code(), grpc::StatusCode::INTERNAL); + EXPECT_FALSE(response.header().success()); + EXPECT_NE(response.header().error_message().find( + "injected profile position exception"), + std::string::npos); + EXPECT_GE(protocol_->quick_stop_count_.load(), 1); +} + +TEST_F(MotorServiceTest, ExceptionCleanupKeepsMotorReservedUntilQuickStopFinishes) +{ + protocol_->throw_profile_position_ = true; + protocol_->quick_stop_delay_ms_ = 100; + + api::ProfilePositionRequest failing_request; + *failing_request.mutable_target() = makeTarget(); + failing_request.set_target_position_rad(1.0); + failing_request.set_max_velocity_rad_s(1.0); + failing_request.set_acceleration_rad_s2(1.0); + grpc::ServerContext failing_context; + api::MotorCommandResponse failing_response; + grpc::Status failing_status; + std::thread failing([&]() { + failing_status = service_->profilePosition( + &failing_context, &failing_request, &failing_response); + }); + + const auto cleanup_deadline = + std::chrono::steady_clock::now() + std::chrono::seconds(1); + while (protocol_->quick_stop_count_.load() == 0 && + std::chrono::steady_clock::now() < cleanup_deadline) { + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + } + if (protocol_->quick_stop_count_.load() == 0) { + failing.join(); + FAIL() << "exception cleanup did not start"; + return; + } + protocol_->throw_profile_position_ = false; + + api::ProfilePositionRequest competing_request; + *competing_request.mutable_target() = makeTarget(); + competing_request.set_target_position_rad(2.0); + competing_request.set_max_velocity_rad_s(1.0); + competing_request.set_acceleration_rad_s2(1.0); + grpc::ServerContext competing_context; + api::MotorCommandResponse competing_response; + const auto competing_status = service_->profilePosition( + &competing_context, &competing_request, &competing_response); + + failing.join(); + EXPECT_EQ(failing_status.error_code(), grpc::StatusCode::INTERNAL); + EXPECT_EQ(competing_status.error_code(), + grpc::StatusCode::RESOURCE_EXHAUSTED); +} + +TEST_F(MotorServiceTest, WaitingStaleCleanupCannotStopOrReleaseNewOwner) +{ + const auto target = makeTarget(); + gRPCMotorServiceImplTestAccess::bestEffortQuickStop( + *service_, target, "first completed exception cleanup"); + ASSERT_EQ(protocol_->quick_stop_count_.load(), 1); + + auto cleanup_gate = + gRPCMotorServiceImplTestAccess::holdExceptionCleanupMutex( + *service_, target); + std::atomic stale_cleanup_entered{false}; + std::thread stale_cleanup([&]() { + stale_cleanup_entered = true; + gRPCMotorServiceImplTestAccess::bestEffortQuickStop( + *service_, target, "second stale exception cleanup"); + }); + const auto stale_deadline = + std::chrono::steady_clock::now() + std::chrono::seconds(1); + while (!stale_cleanup_entered.load() && + std::chrono::steady_clock::now() < stale_deadline) { + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + } + if (!stale_cleanup_entered.load()) { + cleanup_gate.unlock(); + stale_cleanup.join(); + FAIL() << "second cleanup did not start waiting"; + return; + } + + protocol_->profile_delay_ms_ = 100; + protocol_->command_started_ = false; + api::ProfilePositionRequest request; + *request.mutable_target() = target; + request.set_target_position_rad(1.0); + request.set_max_velocity_rad_s(1.0); + request.set_acceleration_rad_s2(1.0); + request.mutable_wait()->set_settle_sample_count(1); + grpc::ServerContext motion_context; + api::MotorCommandResponse motion_response; + grpc::Status motion_status; + std::thread motion([&]() { + motion_status = service_->profilePosition( + &motion_context, &request, &motion_response); + }); + const auto motion_deadline = + std::chrono::steady_clock::now() + std::chrono::seconds(1); + while (!protocol_->command_started_.load() && + std::chrono::steady_clock::now() < motion_deadline) { + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + } + if (!protocol_->command_started_.load()) { + cleanup_gate.unlock(); + stale_cleanup.join(); + motion_context.TryCancel(); + motion.join(); + FAIL() << "new owner did not dispatch while stale cleanup waited"; + return; + } + + cleanup_gate.unlock(); + stale_cleanup.join(); + EXPECT_EQ(protocol_->quick_stop_count_.load(), 1); + + motion.join(); + ASSERT_TRUE(motion_status.ok()) << motion_status.error_message(); + EXPECT_TRUE(motion_response.header().success()); + EXPECT_EQ(protocol_->quick_stop_count_.load(), 1); +} + +TEST_F(MotorServiceTest, GetStatusBackendExceptionReturnsInternal) +{ + protocol_->throw_get_q_ = true; + + api::GetMotorStatusRequest request; + *request.mutable_target() = makeTarget(); + + grpc::ServerContext context; + api::GetMotorStatusResponse response; + const auto status = service_->getStatus(&context, &request, &response); + + EXPECT_EQ(status.error_code(), grpc::StatusCode::INTERNAL); + EXPECT_FALSE(response.header().success()); + EXPECT_NE(response.header().error_message().find( + "injected getQ exception"), + std::string::npos); + EXPECT_EQ(protocol_->quick_stop_count_.load(), 0); +} + +TEST_F(MotorServiceTest, GetStatusRejectsNonFiniteFeedback) +{ + protocol_->nonfinite_get_q_ = true; + + api::GetMotorStatusRequest request; + *request.mutable_target() = makeTarget(); + + grpc::ServerContext context; + api::GetMotorStatusResponse response; + const auto status = service_->getStatus(&context, &request, &response); + + EXPECT_EQ(status.error_code(), grpc::StatusCode::UNAVAILABLE); + EXPECT_FALSE(response.header().success()); +} + +TEST_F(MotorServiceTest, ProfileRejectsInfiniteWaitTolerance) +{ + api::ProfilePositionRequest request; + *request.mutable_target() = makeTarget(); + request.set_target_position_rad(1.0); + request.set_max_velocity_rad_s(1.0); + request.set_acceleration_rad_s2(1.0); + request.mutable_wait()->set_position_tolerance_rad( + std::numeric_limits::infinity()); + + grpc::ServerContext context; + api::MotorCommandResponse response; + const auto status = service_->profilePosition(&context, &request, &response); + + EXPECT_EQ(status.error_code(), grpc::StatusCode::INVALID_ARGUMENT); + EXPECT_FALSE(response.header().success()); + EXPECT_FALSE(protocol_->command_started_.load()); +} + +TEST_F(MotorServiceTest, ProfileRejectAckLossQuickStopsBeforeReturning) +{ + protocol_->profile_position_success_ = false; + + api::ProfilePositionRequest request; + *request.mutable_target() = makeTarget(); + request.set_target_position_rad(1.0); + request.set_max_velocity_rad_s(1.0); + request.set_acceleration_rad_s2(1.0); + + grpc::ServerContext context; + api::MotorCommandResponse response; + const auto status = service_->profilePosition(&context, &request, &response); + + EXPECT_EQ(status.error_code(), grpc::StatusCode::FAILED_PRECONDITION); + EXPECT_FALSE(response.header().success()); + EXPECT_FALSE(response.status().emergency_stopped()); + EXPECT_GE(protocol_->quick_stop_count_.load(), 1); +} + +TEST_F(MotorServiceTest, ProfileRejectAckLossWithFailedStopReturnsInternal) +{ + protocol_->profile_position_success_ = false; + protocol_->quick_stop_success_ = false; + + api::ProfilePositionRequest request; + *request.mutable_target() = makeTarget(); + request.set_target_position_rad(1.0); + request.set_max_velocity_rad_s(1.0); + request.set_acceleration_rad_s2(1.0); + + grpc::ServerContext context; + api::MotorCommandResponse response; + const auto status = service_->profilePosition(&context, &request, &response); + + EXPECT_EQ(status.error_code(), grpc::StatusCode::INTERNAL); + EXPECT_FALSE(response.header().success()); + EXPECT_TRUE(response.status().emergency_stopped()); + EXPECT_GE(protocol_->quick_stop_count_.load(), 1); + + grpc::ServerContext blocked_context; + api::MotorCommandResponse blocked_response; + const auto blocked_status = service_->profilePosition( + &blocked_context, &request, &blocked_response); + EXPECT_EQ(blocked_status.error_code(), + grpc::StatusCode::FAILED_PRECONDITION); + EXPECT_TRUE(blocked_response.status().emergency_stopped()); + + protocol_->quick_stop_success_ = true; + protocol_->profile_position_success_ = true; + api::SetMotorEnabledRequest enable_request; + *enable_request.mutable_target() = makeTarget(); + enable_request.set_enabled(true); + grpc::ServerContext enable_context; + api::MotorCommandResponse enable_response; + const auto enable_status = service_->setEnabled( + &enable_context, &enable_request, &enable_response); + ASSERT_TRUE(enable_status.ok()) << enable_status.error_message(); + EXPECT_FALSE(enable_response.status().emergency_stopped()); + + grpc::ServerContext recovered_context; + api::MotorCommandResponse recovered_response; + const auto recovered_status = service_->profilePosition( + &recovered_context, &request, &recovered_response); + EXPECT_TRUE(recovered_status.ok()) << recovered_status.error_message(); +} + +TEST_F(MotorServiceTest, ProfileAckLossAfterDeadlineStillQuickStops) +{ + ASSERT_TRUE(startGrpcServer()); + protocol_->profile_position_success_ = false; + protocol_->profile_delay_ms_ = 100; + + api::ProfilePositionRequest request; + *request.mutable_target() = makeTarget(); + request.set_target_position_rad(1.0); + request.set_max_velocity_rad_s(1.0); + request.set_acceleration_rad_s2(1.0); + grpc::ClientContext context; + context.set_deadline( + std::chrono::system_clock::now() + std::chrono::milliseconds(50)); + api::MotorCommandResponse response; + const auto status = stub_->profilePosition(&context, request, &response); + EXPECT_EQ(status.error_code(), grpc::StatusCode::DEADLINE_EXCEEDED); + + const auto cleanup_deadline = + std::chrono::steady_clock::now() + std::chrono::milliseconds(300); + while (protocol_->quick_stop_count_.load() == 0 && + std::chrono::steady_clock::now() < cleanup_deadline) { + std::this_thread::sleep_for(std::chrono::milliseconds(2)); + } + EXPECT_GE(protocol_->quick_stop_count_.load(), 1); +} + +TEST_F(MotorServiceTest, RejectedProfilePositionPreemptedInFlightReturnsAborted) +{ + protocol_->profile_position_success_ = false; + protocol_->profile_delay_ms_ = 100; + + api::ProfilePositionRequest request; + *request.mutable_target() = makeTarget(); + request.set_target_position_rad(1.0); + request.set_max_velocity_rad_s(1.0); + request.set_acceleration_rad_s2(1.0); + + grpc::ServerContext motion_context; + api::MotorCommandResponse motion_response; + grpc::Status motion_status; + std::thread motion([&]() { + motion_status = service_->profilePosition( + &motion_context, &request, &motion_response); + }); + const auto dispatch_deadline = + std::chrono::steady_clock::now() + std::chrono::seconds(1); + while (!protocol_->command_started_.load() && + std::chrono::steady_clock::now() < dispatch_deadline) { + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + } + ASSERT_TRUE(protocol_->command_started_.load()); + + api::EmergencyStopRequest stop_request; + *stop_request.mutable_target() = makeTarget(); + grpc::ServerContext stop_context; + api::MotorCommandResponse stop_response; + const auto stop_status = service_->emergencyStop( + &stop_context, &stop_request, &stop_response); + + motion.join(); + ASSERT_TRUE(stop_status.ok()) << stop_status.error_message(); + EXPECT_EQ(motion_status.error_code(), grpc::StatusCode::ABORTED); +} + +TEST_F(MotorServiceTest, RejectedProfileVelocityPreemptedInFlightReturnsAborted) +{ + protocol_->profile_velocity_success_ = false; + protocol_->profile_velocity_delay_ms_ = 100; + + api::ProfileVelocityRequest request; + *request.mutable_target() = makeTarget(); + request.set_target_velocity_rad_s(1.0); + request.set_acceleration_rad_s2(1.0); + + grpc::ServerContext motion_context; + api::MotorCommandResponse motion_response; + grpc::Status motion_status; + std::thread motion([&]() { + motion_status = service_->profileVelocity( + &motion_context, &request, &motion_response); + }); + const auto dispatch_deadline = + std::chrono::steady_clock::now() + std::chrono::seconds(1); + while (!protocol_->profile_velocity_started_.load() && + std::chrono::steady_clock::now() < dispatch_deadline) { + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + } + ASSERT_TRUE(protocol_->profile_velocity_started_.load()); + + api::EmergencyStopRequest stop_request; + *stop_request.mutable_target() = makeTarget(); + grpc::ServerContext stop_context; + api::MotorCommandResponse stop_response; + const auto stop_status = service_->emergencyStop( + &stop_context, &stop_request, &stop_response); + + motion.join(); + ASSERT_TRUE(stop_status.ok()) << stop_status.error_message(); + EXPECT_EQ(motion_status.error_code(), grpc::StatusCode::ABORTED); +} + +TEST_F(MotorServiceTest, EmergencyStopDuringProfileStatusSampleCannotReturnOk) +{ + protocol_->get_q_delay_ms_ = 100; + + api::ProfilePositionRequest motion_request; + *motion_request.mutable_target() = makeTarget(); + motion_request.set_target_position_rad(1.0); + motion_request.set_max_velocity_rad_s(1.0); + motion_request.set_acceleration_rad_s2(1.0); + motion_request.mutable_wait()->set_settle_sample_count(1); + + grpc::ServerContext motion_context; + api::MotorCommandResponse motion_response; + grpc::Status motion_status; + std::thread motion([&]() { + motion_status = service_->profilePosition( + &motion_context, &motion_request, &motion_response); + }); + + const auto sample_deadline = + std::chrono::steady_clock::now() + std::chrono::seconds(1); + while (!protocol_->get_q_started_.load() && + std::chrono::steady_clock::now() < sample_deadline) { + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + } + if (!protocol_->get_q_started_.load()) { + motion_context.TryCancel(); + motion.join(); + FAIL() << "profile status sampling did not start"; + return; + } + + api::EmergencyStopRequest stop_request; + *stop_request.mutable_target() = makeTarget(); + grpc::ServerContext stop_context; + api::MotorCommandResponse stop_response; + const auto stop_status = service_->emergencyStop( + &stop_context, &stop_request, &stop_response); + + motion.join(); + ASSERT_TRUE(stop_status.ok()) << stop_status.error_message(); + EXPECT_EQ(motion_status.error_code(), grpc::StatusCode::ABORTED); + EXPECT_FALSE(motion_response.header().success()); +} + +TEST_F(MotorServiceTest, EmergencyStopPreemptsBlockingProfilePosition) +{ + protocol_->hold_position_ = true; + protocol_->profile_delay_ms_ = 50; + + api::ProfilePositionRequest motion_request; + *motion_request.mutable_target() = makeTarget(); + motion_request.set_target_position_rad(2.0); + motion_request.set_max_velocity_rad_s(1.0); + motion_request.set_acceleration_rad_s2(1.0); + motion_request.mutable_wait()->set_timeout_ms(5000); + motion_request.mutable_wait()->set_poll_period_ms(1); + + grpc::ServerContext motion_context; + api::MotorCommandResponse motion_response; + grpc::Status motion_status; + std::thread motion([&]() { + motion_status = service_->profilePosition( + &motion_context, &motion_request, &motion_response); + }); + + const auto wait_until = + std::chrono::steady_clock::now() + std::chrono::seconds(1); + while (!protocol_->command_started_.load() && + std::chrono::steady_clock::now() < wait_until) { + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + } + ASSERT_TRUE(protocol_->command_started_.load()); + + api::EmergencyStopRequest stop_request; + *stop_request.mutable_target() = makeTarget(); + grpc::ServerContext stop_context; + api::MotorCommandResponse stop_response; + const auto stop_status = service_->emergencyStop( + &stop_context, &stop_request, &stop_response); + + motion.join(); + ASSERT_TRUE(stop_status.ok()) << stop_status.error_message(); + EXPECT_EQ(motion_status.error_code(), grpc::StatusCode::ABORTED); + EXPECT_GE(protocol_->quick_stop_count_.load(), 1); + EXPECT_GT(protocol_->last_quick_stop_order_.load(), + protocol_->profile_command_order_.load()); + EXPECT_TRUE(stop_response.status().emergency_stopped()); +} + +TEST_F(MotorServiceTest, EnableClearsEmergencyStopLatch) +{ + api::EmergencyStopRequest stop_request; + *stop_request.mutable_target() = makeTarget(); + grpc::ServerContext stop_context; + api::MotorCommandResponse stop_response; + ASSERT_TRUE(service_->emergencyStop( + &stop_context, &stop_request, &stop_response).ok()); + + api::SetMotorEnabledRequest enable_request; + *enable_request.mutable_target() = makeTarget(); + enable_request.set_enabled(true); + grpc::ServerContext enable_context; + api::MotorCommandResponse enable_response; + const auto enable_status = service_->setEnabled( + &enable_context, &enable_request, &enable_response); + + ASSERT_TRUE(enable_status.ok()) << enable_status.error_message(); + EXPECT_FALSE(enable_response.status().emergency_stopped()); +} + +TEST_F(MotorServiceTest, RejectedEnableQuickStopsAndDisables) +{ + protocol_->torque_on_success_ = false; + + api::SetMotorEnabledRequest request; + *request.mutable_target() = makeTarget(); + request.set_enabled(true); + grpc::ServerContext context; + api::MotorCommandResponse response; + const auto status = service_->setEnabled(&context, &request, &response); + + EXPECT_EQ(status.error_code(), grpc::StatusCode::FAILED_PRECONDITION); + EXPECT_FALSE(response.status().emergency_stopped()); + EXPECT_GE(protocol_->quick_stop_count_.load(), 1); + EXPECT_GE(protocol_->torque_off_count_.load(), 1); +} + +TEST_F(MotorServiceTest, RejectedEnableCleanupFailureReturnsInternal) +{ + protocol_->torque_on_success_ = false; + protocol_->torque_off_success_ = false; + + api::SetMotorEnabledRequest request; + *request.mutable_target() = makeTarget(); + request.set_enabled(true); + grpc::ServerContext context; + api::MotorCommandResponse response; + const auto status = service_->setEnabled(&context, &request, &response); + + EXPECT_EQ(status.error_code(), grpc::StatusCode::INTERNAL); + EXPECT_TRUE(response.status().emergency_stopped()); + EXPECT_GE(protocol_->quick_stop_count_.load(), 1); + EXPECT_GE(protocol_->torque_off_count_.load(), 1); +} + +TEST_F(MotorServiceTest, RejectedEnablePreemptedInFlightReturnsAborted) +{ + protocol_->torque_on_success_ = false; + protocol_->torque_on_delay_ms_ = 100; + + api::SetMotorEnabledRequest enable_request; + *enable_request.mutable_target() = makeTarget(); + enable_request.set_enabled(true); + grpc::ServerContext enable_context; + api::MotorCommandResponse enable_response; + grpc::Status enable_status; + std::thread enabling([&]() { + enable_status = service_->setEnabled( + &enable_context, &enable_request, &enable_response); + }); + const auto dispatch_deadline = + std::chrono::steady_clock::now() + std::chrono::seconds(1); + while (!protocol_->torque_on_started_.load() && + std::chrono::steady_clock::now() < dispatch_deadline) { + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + } + ASSERT_TRUE(protocol_->torque_on_started_.load()); + + api::EmergencyStopRequest stop_request; + *stop_request.mutable_target() = makeTarget(); + grpc::ServerContext stop_context; + api::MotorCommandResponse stop_response; + const auto stop_status = service_->emergencyStop( + &stop_context, &stop_request, &stop_response); + + enabling.join(); + ASSERT_TRUE(stop_status.ok()) << stop_status.error_message(); + EXPECT_EQ(enable_status.error_code(), grpc::StatusCode::ABORTED); +} + +TEST_F(MotorServiceTest, SuccessfulEnableCancelledInFlightRollsBack) +{ + ASSERT_TRUE(startGrpcServer()); + protocol_->torque_on_delay_ms_ = 100; + + api::SetMotorEnabledRequest request; + *request.mutable_target() = makeTarget(); + request.set_enabled(true); + grpc::ClientContext context; + context.set_deadline( + std::chrono::system_clock::now() + std::chrono::milliseconds(50)); + api::MotorCommandResponse response; + const auto status = stub_->setEnabled(&context, request, &response); + EXPECT_EQ(status.error_code(), grpc::StatusCode::DEADLINE_EXCEEDED); + + const auto cleanup_deadline = + std::chrono::steady_clock::now() + std::chrono::milliseconds(400); + while ((protocol_->quick_stop_count_.load() == 0 || + protocol_->torque_off_count_.load() == 0) && + std::chrono::steady_clock::now() < cleanup_deadline) { + std::this_thread::sleep_for(std::chrono::milliseconds(2)); + } + EXPECT_GE(protocol_->quick_stop_count_.load(), 1); + EXPECT_GE(protocol_->torque_off_count_.load(), 1); +} + +TEST_F(MotorServiceTest, EmergencyStopPreemptsEnableDuringDispatch) +{ + api::EmergencyStopRequest initial_stop; + *initial_stop.mutable_target() = makeTarget(); + grpc::ServerContext initial_stop_context; + api::MotorCommandResponse initial_stop_response; + ASSERT_TRUE(service_->emergencyStop( + &initial_stop_context, &initial_stop, &initial_stop_response).ok()); + + protocol_->operation_counter_ = 0; + protocol_->last_quick_stop_order_ = 0; + protocol_->torque_on_started_ = false; + protocol_->torque_on_delay_ms_ = 50; + + api::SetMotorEnabledRequest enable_request; + *enable_request.mutable_target() = makeTarget(); + enable_request.set_enabled(true); + grpc::ServerContext enable_context; + api::MotorCommandResponse enable_response; + grpc::Status enable_status; + std::thread enabling([&]() { + enable_status = service_->setEnabled( + &enable_context, &enable_request, &enable_response); + }); + + const auto wait_until = + std::chrono::steady_clock::now() + std::chrono::seconds(1); + while (!protocol_->torque_on_started_.load() && + std::chrono::steady_clock::now() < wait_until) { + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + } + ASSERT_TRUE(protocol_->torque_on_started_.load()); + + api::EmergencyStopRequest stop_request; + *stop_request.mutable_target() = makeTarget(); + grpc::ServerContext stop_context; + api::MotorCommandResponse stop_response; + const auto stop_status = service_->emergencyStop( + &stop_context, &stop_request, &stop_response); + + enabling.join(); + ASSERT_TRUE(stop_status.ok()) << stop_status.error_message(); + EXPECT_EQ(enable_status.error_code(), grpc::StatusCode::ABORTED); + EXPECT_TRUE(stop_response.status().emergency_stopped()); + EXPECT_GT(protocol_->last_quick_stop_order_.load(), + protocol_->torque_on_order_.load()); + EXPECT_GE(protocol_->torque_off_count_.load(), 1); +} + +TEST_F(MotorServiceTest, SuccessfulEnablePreemptCleanupFailureReturnsInternal) +{ + protocol_->torque_on_delay_ms_ = 100; + protocol_->torque_off_success_ = false; + + api::SetMotorEnabledRequest enable_request; + *enable_request.mutable_target() = makeTarget(); + enable_request.set_enabled(true); + grpc::ServerContext enable_context; + api::MotorCommandResponse enable_response; + grpc::Status enable_status; + std::thread enabling([&]() { + enable_status = service_->setEnabled( + &enable_context, &enable_request, &enable_response); + }); + const auto dispatch_deadline = + std::chrono::steady_clock::now() + std::chrono::seconds(1); + while (!protocol_->torque_on_started_.load() && + std::chrono::steady_clock::now() < dispatch_deadline) { + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + } + ASSERT_TRUE(protocol_->torque_on_started_.load()); + + api::EmergencyStopRequest stop_request; + *stop_request.mutable_target() = makeTarget(); + grpc::ServerContext stop_context; + api::MotorCommandResponse stop_response; + const auto stop_status = service_->emergencyStop( + &stop_context, &stop_request, &stop_response); + + enabling.join(); + ASSERT_TRUE(stop_status.ok()) << stop_status.error_message(); + EXPECT_EQ(enable_status.error_code(), grpc::StatusCode::INTERNAL); + EXPECT_GE(protocol_->torque_off_count_.load(), 1); +} + +TEST_F(MotorServiceTest, ConcurrentEmergencyStopsCannotBeClearedByEnable) +{ + protocol_->quick_stop_delay_ms_ = 60; + + api::EmergencyStopRequest first_request; + *first_request.mutable_target() = makeTarget(); + api::EmergencyStopRequest second_request; + *second_request.mutable_target() = makeTarget(); + grpc::ServerContext first_context; + grpc::ServerContext second_context; + api::MotorCommandResponse first_response; + api::MotorCommandResponse second_response; + grpc::Status first_status; + grpc::Status second_status; + + std::thread first([&]() { + first_status = service_->emergencyStop( + &first_context, &first_request, &first_response); + }); + const auto first_stop_deadline = + std::chrono::steady_clock::now() + std::chrono::seconds(1); + while (protocol_->quick_stop_count_.load() < 1 && + std::chrono::steady_clock::now() < first_stop_deadline) { + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + } + if (protocol_->quick_stop_count_.load() < 1) { + first.join(); + FAIL() << "first emergency stop did not dispatch"; + return; + } + + std::thread second([&]() { + second_status = service_->emergencyStop( + &second_context, &second_request, &second_response); + }); + first.join(); + + const auto second_stop_deadline = + std::chrono::steady_clock::now() + std::chrono::seconds(1); + while (protocol_->quick_stop_count_.load() < 3 && + std::chrono::steady_clock::now() < second_stop_deadline) { + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + } + if (protocol_->quick_stop_count_.load() < 3) { + second.join(); + FAIL() << "second emergency stop did not dispatch"; + return; + } + + api::SetMotorEnabledRequest enable_request; + *enable_request.mutable_target() = makeTarget(); + enable_request.set_enabled(true); + grpc::ServerContext enable_context; + api::MotorCommandResponse enable_response; + const auto enable_status = service_->setEnabled( + &enable_context, &enable_request, &enable_response); + + second.join(); + ASSERT_TRUE(first_status.ok()) << first_status.error_message(); + ASSERT_TRUE(second_status.ok()) << second_status.error_message(); + EXPECT_EQ(enable_status.error_code(), grpc::StatusCode::ABORTED); + + api::ProfilePositionRequest motion_request; + *motion_request.mutable_target() = makeTarget(); + motion_request.set_target_position_rad(1.0); + motion_request.set_max_velocity_rad_s(1.0); + motion_request.set_acceleration_rad_s2(1.0); + grpc::ServerContext motion_context; + api::MotorCommandResponse motion_response; + const auto motion_status = service_->profilePosition( + &motion_context, &motion_request, &motion_response); + EXPECT_EQ(motion_status.error_code(), + grpc::StatusCode::FAILED_PRECONDITION); + EXPECT_TRUE(motion_response.status().emergency_stopped()); +} + +TEST_F(MotorServiceTest, ProfileCancellationInterruptsLongPollAndQuickStops) +{ + ASSERT_TRUE(startGrpcServer()); + protocol_->hold_position_ = true; + + api::ProfilePositionRequest request; + *request.mutable_target() = makeTarget(); + request.set_target_position_rad(1.0); + request.set_max_velocity_rad_s(1.0); + request.set_acceleration_rad_s2(1.0); + request.mutable_wait()->set_timeout_ms(5000); + request.mutable_wait()->set_poll_period_ms(1000); + + grpc::ClientContext context; + context.set_deadline( + std::chrono::system_clock::now() + std::chrono::milliseconds(75)); + api::MotorCommandResponse response; + const auto started = std::chrono::steady_clock::now(); + const auto status = stub_->profilePosition(&context, request, &response); + EXPECT_EQ(status.error_code(), grpc::StatusCode::DEADLINE_EXCEEDED); + + const auto stop_deadline = + std::chrono::steady_clock::now() + std::chrono::milliseconds(250); + while (protocol_->quick_stop_count_.load() == 0 && + std::chrono::steady_clock::now() < stop_deadline) { + std::this_thread::sleep_for(std::chrono::milliseconds(2)); + } + EXPECT_GE(protocol_->quick_stop_count_.load(), 1); + EXPECT_LT(std::chrono::steady_clock::now() - started, + std::chrono::milliseconds(350)); +} + +TEST_F(MotorServiceTest, CyclicPositionStreamAppliesSetpointAndStopsOnWritesDone) +{ + ASSERT_TRUE(startGrpcServer()); + + grpc::ClientContext context; + context.set_deadline( + std::chrono::system_clock::now() + std::chrono::seconds(2)); + auto stream = stub_->streamCyclicPosition(&context); + ASSERT_NE(stream, nullptr); + + api::CyclicPositionRequest open_request; + *open_request.mutable_open()->mutable_target() = makeTarget(); + open_request.mutable_open()->set_watchdog_timeout_ms(500); + ASSERT_TRUE(stream->Write(open_request)); + + api::CyclicControlResponse response; + ASSERT_TRUE(stream->Read(&response)); + ASSERT_TRUE(response.header().success()) + << response.header().error_message(); + EXPECT_EQ(response.phase(), api::CYCLIC_STREAM_OPENED); + + api::CyclicPositionRequest setpoint_request; + auto* setpoint = setpoint_request.mutable_setpoint(); + setpoint->set_sequence(1); + setpoint->set_target_position_rad(0.75); + setpoint->set_target_velocity_rad_s(0.2); + ASSERT_TRUE(stream->Write(setpoint_request)); + + response.Clear(); + ASSERT_TRUE(stream->Read(&response)); + ASSERT_TRUE(response.header().success()) + << response.header().error_message(); + EXPECT_EQ(response.phase(), api::CYCLIC_STREAM_APPLIED); + EXPECT_EQ(response.sequence(), 1); + EXPECT_FALSE(response.has_status()); + + ASSERT_TRUE(stream->WritesDone()); + response.Clear(); + ASSERT_TRUE(stream->Read(&response)); + EXPECT_TRUE(response.header().success()); + EXPECT_EQ(response.phase(), api::CYCLIC_STREAM_STOPPED); + EXPECT_EQ(response.sequence(), 1); + EXPECT_FALSE(stream->Read(&response)); + + const auto finish = stream->Finish(); + EXPECT_TRUE(finish.ok()) << finish.error_message(); + EXPECT_GE(protocol_->quick_stop_count_.load(), 1); +} + +TEST_F(MotorServiceTest, CyclicVelocityStreamAppliesAndStopsOnWritesDone) +{ + ASSERT_TRUE(startGrpcServer()); + + grpc::ClientContext context; + context.set_deadline( + std::chrono::system_clock::now() + std::chrono::seconds(2)); + auto stream = stub_->streamCyclicVelocity(&context); + ASSERT_NE(stream, nullptr); + + api::CyclicVelocityRequest open_request; + *open_request.mutable_open()->mutable_target() = makeTarget(); + open_request.mutable_open()->set_watchdog_timeout_ms(500); + ASSERT_TRUE(stream->Write(open_request)); + + api::CyclicControlResponse response; + ASSERT_TRUE(stream->Read(&response)); + ASSERT_TRUE(response.header().success()) + << response.header().error_message(); + EXPECT_EQ(response.phase(), api::CYCLIC_STREAM_OPENED); + EXPECT_TRUE(response.has_status()); + + api::CyclicVelocityRequest setpoint_request; + auto* setpoint = setpoint_request.mutable_setpoint(); + setpoint->set_sequence(1); + setpoint->set_target_velocity_rad_s(0.6); + ASSERT_TRUE(stream->Write(setpoint_request)); + + response.Clear(); + ASSERT_TRUE(stream->Read(&response)); + ASSERT_TRUE(response.header().success()) + << response.header().error_message(); + EXPECT_EQ(response.phase(), api::CYCLIC_STREAM_APPLIED); + EXPECT_EQ(response.sequence(), 1); + EXPECT_FALSE(response.has_status()); + + ASSERT_TRUE(stream->WritesDone()); + response.Clear(); + ASSERT_TRUE(stream->Read(&response)); + EXPECT_TRUE(response.header().success()); + EXPECT_EQ(response.phase(), api::CYCLIC_STREAM_STOPPED); + EXPECT_EQ(response.sequence(), 1); + EXPECT_TRUE(response.has_status()); + EXPECT_FALSE(stream->Read(&response)); + + const auto finish = stream->Finish(); + EXPECT_TRUE(finish.ok()) << finish.error_message(); + EXPECT_GE(protocol_->quick_stop_count_.load(), 1); +} + +TEST_F(MotorServiceTest, EmergencyStopDuringCyclicDispatchCannotPublishApplied) +{ + ASSERT_TRUE(startGrpcServer()); + protocol_->cyclic_position_delay_ms_ = 100; + + grpc::ClientContext context; + context.set_deadline( + std::chrono::system_clock::now() + std::chrono::seconds(3)); + auto stream = stub_->streamCyclicPosition(&context); + ASSERT_NE(stream, nullptr); + + api::CyclicPositionRequest open_request; + *open_request.mutable_open()->mutable_target() = makeTarget(); + ASSERT_TRUE(stream->Write(open_request)); + + api::CyclicControlResponse response; + ASSERT_TRUE(stream->Read(&response)); + ASSERT_EQ(response.phase(), api::CYCLIC_STREAM_OPENED); + + api::CyclicPositionRequest setpoint_request; + setpoint_request.mutable_setpoint()->set_sequence(1); + setpoint_request.mutable_setpoint()->set_target_position_rad(0.5); + ASSERT_TRUE(stream->Write(setpoint_request)); + + const auto dispatch_deadline = + std::chrono::steady_clock::now() + std::chrono::seconds(1); + while (!protocol_->cyclic_position_started_.load() && + std::chrono::steady_clock::now() < dispatch_deadline) { + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + } + ASSERT_TRUE(protocol_->cyclic_position_started_.load()); + + api::EmergencyStopRequest stop_request; + *stop_request.mutable_target() = makeTarget(); + grpc::ServerContext stop_context; + api::MotorCommandResponse stop_response; + grpc::Status stop_status; + std::thread stopping([&]() { + stop_status = service_->emergencyStop( + &stop_context, &stop_request, &stop_response); + }); + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + stream->WritesDone(); + stopping.join(); + ASSERT_TRUE(stop_status.ok()) << stop_status.error_message(); + + response.Clear(); + ASSERT_TRUE(stream->Read(&response)); + EXPECT_NE(response.phase(), api::CYCLIC_STREAM_APPLIED); + EXPECT_EQ(response.phase(), api::CYCLIC_STREAM_FAILED); + EXPECT_FALSE(response.header().success()); + EXPECT_FALSE(stream->Read(&response)); + const auto finish = stream->Finish(); + EXPECT_EQ(finish.error_code(), grpc::StatusCode::ABORTED); +} + +TEST_F(MotorServiceTest, CyclicOpenSetModeExceptionReturnsInternalAndQuickStops) +{ + ASSERT_TRUE(startGrpcServer()); + protocol_->throw_set_mode_ = true; + + grpc::ClientContext context; + context.set_deadline( + std::chrono::system_clock::now() + std::chrono::seconds(2)); + auto stream = stub_->streamCyclicPosition(&context); + ASSERT_NE(stream, nullptr); + + api::CyclicPositionRequest open_request; + *open_request.mutable_open()->mutable_target() = makeTarget(); + ASSERT_TRUE(stream->Write(open_request)); + stream->WritesDone(); + + api::CyclicControlResponse response; + EXPECT_FALSE(stream->Read(&response)); + const auto finish = stream->Finish(); + EXPECT_EQ(finish.error_code(), grpc::StatusCode::INTERNAL); + EXPECT_NE(finish.error_message().find("injected setMode exception"), + std::string::npos); + EXPECT_GE(protocol_->quick_stop_count_.load(), 1); +} + +TEST_F(MotorServiceTest, CyclicStreamReportsFailureWhenQuickStopFails) +{ + ASSERT_TRUE(startGrpcServer()); + protocol_->quick_stop_success_ = false; + + grpc::ClientContext context; + context.set_deadline( + std::chrono::system_clock::now() + std::chrono::seconds(2)); + auto stream = stub_->streamCyclicPosition(&context); + ASSERT_NE(stream, nullptr); + + api::CyclicPositionRequest open_request; + *open_request.mutable_open()->mutable_target() = makeTarget(); + open_request.mutable_open()->set_watchdog_timeout_ms(500); + ASSERT_TRUE(stream->Write(open_request)); + + api::CyclicControlResponse response; + ASSERT_TRUE(stream->Read(&response)); + ASSERT_EQ(response.phase(), api::CYCLIC_STREAM_OPENED); + ASSERT_TRUE(stream->WritesDone()); + + response.Clear(); + ASSERT_TRUE(stream->Read(&response)); + EXPECT_FALSE(response.header().success()); + EXPECT_EQ(response.phase(), api::CYCLIC_STREAM_FAILED); + EXPECT_FALSE(stream->Read(&response)); + const auto finish = stream->Finish(); + EXPECT_EQ(finish.error_code(), grpc::StatusCode::INTERNAL); + + api::ProfilePositionRequest profile_request; + *profile_request.mutable_target() = makeTarget(); + profile_request.set_target_position_rad(1.0); + profile_request.set_max_velocity_rad_s(1.0); + profile_request.set_acceleration_rad_s2(1.0); + grpc::ServerContext profile_context; + api::MotorCommandResponse profile_response; + const auto profile_status = service_->profilePosition( + &profile_context, &profile_request, &profile_response); + EXPECT_EQ(profile_status.error_code(), + grpc::StatusCode::FAILED_PRECONDITION); + EXPECT_TRUE(profile_response.status().emergency_stopped()); +} + +TEST_F(MotorServiceTest, CyclicDriverExceptionReturnsInternalWithoutTerminating) +{ + ASSERT_TRUE(startGrpcServer()); + protocol_->throw_cyclic_position_ = true; + + grpc::ClientContext context; + context.set_deadline( + std::chrono::system_clock::now() + std::chrono::seconds(2)); + auto stream = stub_->streamCyclicPosition(&context); + ASSERT_NE(stream, nullptr); + + api::CyclicPositionRequest open_request; + *open_request.mutable_open()->mutable_target() = makeTarget(); + ASSERT_TRUE(stream->Write(open_request)); + + api::CyclicControlResponse response; + ASSERT_TRUE(stream->Read(&response)); + ASSERT_EQ(response.phase(), api::CYCLIC_STREAM_OPENED); + + api::CyclicPositionRequest setpoint_request; + setpoint_request.mutable_setpoint()->set_sequence(1); + setpoint_request.mutable_setpoint()->set_target_position_rad(0.5); + ASSERT_TRUE(stream->Write(setpoint_request)); + + response.Clear(); + ASSERT_TRUE(stream->Read(&response)); + EXPECT_FALSE(response.header().success()); + EXPECT_EQ(response.phase(), api::CYCLIC_STREAM_FAILED); + EXPECT_TRUE(stream->WritesDone()); + EXPECT_FALSE(stream->Read(&response)); + const auto finish = stream->Finish(); + EXPECT_EQ(finish.error_code(), grpc::StatusCode::INTERNAL); + EXPECT_GE(protocol_->quick_stop_count_.load(), 1); +} + +TEST_F(MotorServiceTest, CyclicPositionStreamWatchdogStopsSilentClient) +{ + ASSERT_TRUE(startGrpcServer()); + + grpc::ClientContext context; + context.set_deadline( + std::chrono::system_clock::now() + std::chrono::seconds(2)); + auto stream = stub_->streamCyclicPosition(&context); + ASSERT_NE(stream, nullptr); + + api::CyclicPositionRequest open_request; + *open_request.mutable_open()->mutable_target() = makeTarget(); + open_request.mutable_open()->set_watchdog_timeout_ms(25); + ASSERT_TRUE(stream->Write(open_request)); + + api::CyclicControlResponse response; + ASSERT_TRUE(stream->Read(&response)); + EXPECT_EQ(response.phase(), api::CYCLIC_STREAM_OPENED); + + response.Clear(); + ASSERT_TRUE(stream->Read(&response)); + EXPECT_FALSE(response.header().success()); + EXPECT_EQ(response.phase(), api::CYCLIC_STREAM_WATCHDOG_EXPIRED); + EXPECT_FALSE(stream->Read(&response)); + + const auto finish = stream->Finish(); + EXPECT_TRUE( + finish.error_code() == grpc::StatusCode::DEADLINE_EXCEEDED || + finish.error_code() == grpc::StatusCode::CANCELLED) + << finish.error_message(); + EXPECT_GE(protocol_->quick_stop_count_.load(), 1); +} + +} // namespace +} // namespace cmvr::service diff --git a/cmvr-es/task/grpc_server_task/include/grpc_server_task.h b/cmvr-es/task/grpc_server_task/include/grpc_server_task.h index 055e2eed..33bd0ebe 100644 --- a/cmvr-es/task/grpc_server_task/include/grpc_server_task.h +++ b/cmvr-es/task/grpc_server_task/include/grpc_server_task.h @@ -55,6 +55,7 @@ private: std::unique_ptr dexhand_service_; std::unique_ptr biohand_service_; std::unique_ptr arm_service_; + std::unique_ptr motor_service_; std::unique_ptr agv_service_; std::unique_ptr hlc_service_; }; diff --git a/cmvr-es/task/grpc_server_task/src/grpc_server_task.cpp b/cmvr-es/task/grpc_server_task/src/grpc_server_task.cpp index 60d8c697..962d2434 100644 --- a/cmvr-es/task/grpc_server_task/src/grpc_server_task.cpp +++ b/cmvr-es/task/grpc_server_task/src/grpc_server_task.cpp @@ -15,6 +15,7 @@ #include "service/grpc/include/grpc_head_service.h" #include "service/grpc/include/grpc_hlc_service.h" #include "service/grpc/include/grpc_microphone_service.h" +#include "service/grpc/include/grpc_motor_service.h" #include "service/grpc/include/grpc_speaker_service.h" #include "service/grpc/include/grpc_system_service.h" #include "task/task_factory.h" @@ -105,6 +106,7 @@ bool GrpcServerTask::start() dexhand_service_ = std::make_unique(); biohand_service_ = std::make_unique(); arm_service_ = std::make_unique(); + motor_service_ = std::make_unique(); agv_service_ = std::make_unique(); hlc_service_ = std::make_unique(); @@ -117,6 +119,7 @@ bool GrpcServerTask::start() builder.RegisterService(dexhand_service_.get()); builder.RegisterService(biohand_service_.get()); builder.RegisterService(arm_service_.get()); + builder.RegisterService(motor_service_.get()); builder.RegisterService(agv_service_.get()); builder.RegisterService(hlc_service_.get()); @@ -254,6 +257,7 @@ void GrpcServerTask::clearServices() { hlc_service_.reset(); agv_service_.reset(); + motor_service_.reset(); arm_service_.reset(); biohand_service_.reset(); dexhand_service_.reset(); diff --git a/docs/motor_service_modbus_tcp.md b/docs/motor_service_modbus_tcp.md new file mode 100644 index 00000000..623aa0a3 --- /dev/null +++ b/docs/motor_service_modbus_tcp.md @@ -0,0 +1,1022 @@ +# MotorService 与 Modbus TCP PLC 电机控制 + +本文说明当前 `MotorService`、CMVR PLC v1 寄存器协议和 Siemens +S7-1215C DC/DC/DC 侧的对接方法。本文只描述当前源码已经存在的接口和 +协议；寄存器表中标为“预留”或“当前未发送”的内容，不代表上层已经开放。 + +## 1. 当前范围 + +当前实现是一条刻意保持简单的单轴控制链： + +```text +gRPC MotorService + | + v +MotorManager + | + v +AbstractMotor + | + v +CmvrPlcMotorProtocol + | + v +ModbusTcpMotorBusRuntime + | + v +libmodbus -> PLC MB_SERVER -> 驱动器/电机 +``` + +各层职责如下： + +- `MotorService`：解析 `MotorManager` ID 和电机选择器，执行单电机互斥、 + 同步等待、gRPC 取消检测、流式 latest-wins 和软件急停抢占。 +- `MotorManager`：按 `motor_id` 或 `joint_name` 返回 `AbstractMotor`。 +- `AbstractMotor`：提供统一的置零、Profile、Cyclic、使能和 Quick Stop + 接口。 +- `CmvrPlcMotorProtocol`：执行关节限位检查，把 SI 单位转换为定点整数， + 并映射成 CMVR PLC v1 命令。 +- `ModbusTcpMotorBusRuntime`：管理一个 PLC 连接、握手、心跳、重连、每轴 + 命令序列、payload/commit 写入和 ACK 轮询。 +- PLC：必须原子接收命令、执行驱动器控制、发布状态，并独立执行通信和 + 周期指令 watchdog。 + +当前服务按“一个 RPC 或一个流独占一个电机”仲裁，不是多轴同步控制器。 +同一个 `MotorManager` 中不同电机可以分别被不同调用者控制。需要多轴同一扫描 +周期锁存时，应在后续版本增加 manager/runtime 级批量 commit，不应依赖客户端 +逐轴调用。 + +runtime 的 `start()` 启动连接 supervisor，而不是要求 PLC 当时已经在线。首次 +同步握手失败时 MotorManager 仍完成注册，runtime 保持 +`connected=false`，并从 `reconnect_min_ms` 起按上限退避持续重试。离线期间 +状态 RPC 返回 `UNAVAILABLE`，运动命令在写 mailbox 前安全失败；PLC 后续上线 +并完成完整身份/session 握手后无需重启 `cmvr_es`。 + +## 2. 实际 gRPC 接口 + +服务定义位于： + +- `protos/cmvr/api/motor_service.proto` +- `protos/cmvr/api/motor_command.proto` + +`MotorTarget.header.device_id` 是 `MotorManager` 的设备 ID，不是单个电机 +的 ID。`MotorTarget.selector` 必须且只能设置一个： + +```text +motor_id uint32，当前服务要求不大于 255 +joint_name 非空字符串 +``` + +### 2.1 Unary RPC + +| RPC | 当前语义 | 是否阻塞 | +| --- | --- | --- | +| `setZero` | 调用 `calibrateZeroQ()`。PLC 后端发送 `SetZero`，等待 PLC 进入成功终态后返回 | 是，PLC 后端安全命令上限当前为 5 s | +| `moveToZero` | 复用 Profile Position，把目标位置设为 `0 rad`；不是 `setZero`，也不发送寄存器图中的 `MoveToZero` opcode | 是，直到到位、取消或超时 | +| `profilePosition` | 设置 Profile Position 的目标位置、最大速度和加速度 | 是，直到到位、取消或超时 | +| `profileVelocity` | 设置 Profile Velocity 的目标速度和加速度 | 是，直到实际速度稳定进入容差 | +| `emergencyStop` | 抢占当前阻塞命令/流，设置服务内软件急停锁存并调用 `quickStop()` | 是，等待 PLC Quick Stop 成功终态 | +| `getStatus` | 读取模式、位置、速度、到位标志和 MotorService 仲裁状态 | 同步快照读取 | +| `setEnabled` | `enabled=true` 调用 `torqueOn()`，成功后解除服务内软件急停锁存；`false` 调用 `torqueOff()` | 是，等待 PLC 成功终态 | + +`moveToZero`、`profilePosition` 和 `profileVelocity` 使用 +`MotorWaitOptions`： + +| 字段 | `0` 时默认值 | 服务端范围/语义 | +| --- | ---: | --- | +| `timeout_ms` | 30000 ms | 1 ms 至 600000 ms | +| `poll_period_ms` | 10 ms | 1 ms 至 1000 ms | +| `position_tolerance_rad` | 0.001 rad | 仅正值覆盖默认值 | +| `velocity_tolerance_rad_s` | 0.01 rad/s | 仅正值覆盖默认值 | +| `settle_sample_count` | 3 | 1 至 1000 个连续样本 | + +`setZero` 只有在 PLC 返回 `Completed + ZeroValid` 且 `zero_epoch` 相比命令前 +变化时才成功。如果 RPC 返回失败、取消、断链或超时，而 commit 可能已经写出， +session/zero epoch 的最终结果是不确定的；禁止直接盲重试，应先通过 PLC/HMI +或维护诊断确认当前 session、`zero_epoch` 和实际零位状态。 + +Profile Position 只有在 PLC/驱动器报告 `target_reached`，同时实际位置和 +速度连续满足容差时才返回成功。Profile Velocity 在实际速度连续满足目标速度 +容差时返回成功；RPC 返回后速度命令仍然有效。正常停车应再发送目标速度为 +`0 rad/s` 的 `profileVelocity`，不能把 RPC 返回理解为“速度控制已经结束”。 + +Profile Position/Velocity 的 PLC payload 使用 600000 ms +`command_timeout_ms` 上限，与 MotorService 允许的最长等待一致。RPC 使用更短 +超时或被取消时，服务端仍会主动发送 Quick Stop；PLC 通信 watchdog 仍是上位机 +失联时的权威保护。该 600000 ms 是 PLC 执行上限，不会把 RPC 默认等待从 +30000 ms 延长。 + +成功 ACK 的 Profile Position/Velocity 会绑定到提交时的 +`connection_epoch`。只要 PLC 断线重连使 epoch 改变，该 Profile 的位置、速度 +和到位反馈就全部失效：`getQ/getQd` 返回 NaN，`reachedTargetQ` 返回 false。 +即使新 session 的安全停止速度恰好也是目标 `0 rad/s`，也不能把数值相等误认为 +旧命令成功。成功的新 Profile 会重新绑定当前 epoch；成功的首个 Cyclic +sample、Quick Stop 或 Disable 会清除该绑定。Enable/SetZero 本身不恢复旧 +Profile 反馈的可信性。 + +当前 `getStatus` 会依次调用模式、位置、速度和到位读取。对于 Modbus TCP 后端， +这些读取不是一次原子寄存器快照，字段可能来自相邻的 PLC 扫描周期。 +它只暴露 `AbstractMotor` 的通用状态和 MotorService 仲裁状态，不包含 +`plc_boot_id`、session、`fault_code`、`zero_epoch` 或 PLC result code。 +结果不确定或需要故障恢复时，应从 PLC/HMI 或维护诊断层读取这些原始字段， +不能只依赖本 RPC。 + +### 2.2 双向流 RPC + +当前有两个双向流： + +```proto +rpc streamCyclicPosition(stream CyclicPositionRequest) + returns (stream CyclicControlResponse); + +rpc streamCyclicVelocity(stream CyclicVelocityRequest) + returns (stream CyclicControlResponse); +``` + +首帧必须是 `open`： + +```text +open.target 选择一个 MotorManager 中的单个电机 +open.watchdog_timeout_ms gRPC 层输入 watchdog +``` + +watchdog 为 `0` 时默认 500 ms，非零值被限制到 20 ms 至 60000 ms。打开 +成功后，服务端先返回： + +```text +phase = CYCLIC_STREAM_OPENED +sequence = 0 +status = 当前电机状态 +``` + +`CYCLIC_STREAM_OPENED` 只表示 MotorService 已解析目标、取得该电机的独占 +控制权并选择了 Cyclic 模式。CMVR PLC 后端采用惰性打开：收到第一个合法 +setpoint 时才依次发送 `OpenCyclic*` 和对应的 `Cyclic*Sample`。因此客户端 +必须等到该 setpoint 的 `CYCLIC_STREAM_APPLIED`，才能确认 PLC 已 ACK 打开 +命令并锁存了首个样本。 + +每次 `OpenCyclicPosition/Velocity` 都创建新的轴级 stream epoch。PLC 必须在 +Open 的同一个原子状态事务中清零 `last_applied_cyclic_sequence`、旧样本去重 +状态和 cyclic watchdog，再设置正确的 CSP/CSV mode 与 `StreamActive=1`。 +runtime 只有确认这些 Open 后置条件才接受 ACK，Protocol 随后才从样本序列 `1` +重新开始。Quick Stop、Disable 或断链关闭旧 epoch；重开后的首个 `1` 必须 +重新应用，不能被旧流的样本 `1` 去重。 + +每次 gRPC cyclic 流首帧触发的 `setMode` 都建立一个新的上层 +`cyclic_generation`，即使模式值与上一条流相同。该 generation 绑定当时的 +`connection_epoch`。活动流遇到重连后会永久锁存失败：同一流的当前和后续 +setpoint 都返回失败，不发送新的 `OpenCyclic*` 或 `Cyclic*Sample`；服务端 +终止并 Quick Stop 该流。客户端必须创建新的 gRPC 流，由新首帧显式建立新的 +generation 后才允许重新 Open。禁止把断链前或断链期间 pending 的 latest +setpoint 自动应用到新 PLC session。 + +后续每帧只能是 `setpoint`： + +- 周期位置：严格递增且非零的 `sequence`、`target_position_rad`，以及可选 + `target_velocity_rad_s`。 +- 周期速度：严格递增且非零的 `sequence`、`target_velocity_rad_s`。 + +序列号允许跳号，但不能为 `0`、重复或倒退。服务端 reader 只保留一个尚未 +处理的最新帧；新帧覆盖旧帧时，`dropped_setpoints` 累计增加。这是低延迟 +latest-wins 语义，不适合必须逐点无损执行的离线轨迹。 + +每次 `AbstractMotor` 接受 setpoint 后返回： + +```text +phase = CYCLIC_STREAM_APPLIED +sequence = 已应用的客户端序列 +dropped_setpoints = 本流累计覆盖数 +``` + +对于 CMVR PLC 后端，`AbstractMotor` 返回成功前，runtime 已经看到相同 +command sequence 的 PLC ACK；周期样本还要求 +`last_applied_cyclic_sequence` 严格等于本次样本序列。对于其他电机后端， +`APPLIED` 只保证对应后端的同步调用返回 `true`。 +为避免每个周期样本额外触发多次现场总线状态读取，`APPLIED` 响应刻意不携带 +`status`。`OPENED` 和终止响应仍携带完整状态；需要连续遥测的客户端应以独立、 +较低频率调用 `getStatus`，不能把 `APPLIED` 当作状态采样接口。 + +客户端正常结束请求流后，服务端先同步调用 `quickStop()`；确认后返回 +`CYCLIC_STREAM_STOPPED` 并释放电机所有权。当前协议没有显式 `close` +请求帧；客户端 half-close 就是结束信号。若 Quick Stop 未被后端确认， +服务端改发 `CYCLIC_STREAM_FAILED` 并以 `INTERNAL` 结束 RPC，不会把失败 +误报成 `STOPPED`。 + +出现以下情况时，服务端会 Quick Stop： + +- gRPC context 被取消或 deadline 到期； +- 输入在 watchdog 窗口内没有新帧； +- 序列号或 payload 非法； +- PLC/后端拒绝 setpoint； +- 客户端停止读取响应； +- `emergencyStop` 增加抢占 generation。 + +服务端流使用同步 `Write()`。慢客户端可能阻塞反馈写入，reader 此时仍会 +覆盖 pending setpoint，但上层 watchdog 检查也可能被延迟。因此： + +1. 客户端必须并发、持续读取反馈； +2. gRPC watchdog 只是第一层保护； +3. PLC 侧通信 watchdog 和周期样本 watchdog 才是断网、进程卡死时的 + 权威保护。 + +### 2.3 当前没有开放的能力 + +当前 `MotorService` 没有以下 RPC： + +- Profile Torque； +- Cyclic Torque 流； +- Clear Fault； +- 普通 Stop/Halt； +- 多轴原子控制。 + +寄存器块预留了 `target_torque`，但 `CmvrPlcMotorProtocol` 当前明确拒绝 +周期力矩。不要因为寄存器存在就让 PLC 项目把该功能标记为已经可用。 + +### 2.4 仲裁和 gRPC 状态 + +同一电机已有阻塞命令或流时，新控制调用返回 +`RESOURCE_EXHAUSTED`。软件急停锁存期间，运动命令返回 +`FAILED_PRECONDITION`；只有成功执行 `setEnabled(enabled=true)` 才清除 +该服务内锁存。 + +任何取消、超时、异常或周期流结束后的 Quick Stop 如果未被后端确认成功， +MotorService 也会按 fail-closed 原则进入同一软件急停锁存。此时 RPC 的 +`INTERNAL` 表示“安全状态尚未确认”，不能继续发送运动命令；应先排查 PLC/ +驱动器状态，再通过成功的 `setEnabled(enabled=true)` 显式恢复。 +显式 `emergencyStop` 会在调用后端前先锁存；若最终 Quick Stop 未确认，它 +返回 `FAILED_PRECONDITION`，但仍保持锁存。两种返回码都不能解释为已经安全 +停机。 + +常见 gRPC 状态包括： + +- `INVALID_ARGUMENT`：选择器、数值、首帧或序列号非法； +- `NOT_FOUND`：MotorManager 或电机不存在； +- `RESOURCE_EXHAUSTED`：同一电机已有 owner； +- `FAILED_PRECONDITION`：软件急停锁存或后端拒绝命令； +- `DEADLINE_EXCEEDED`：同步等待或流 watchdog 超时； +- `ABORTED`：被 `emergencyStop` 抢占； +- `CANCELLED`：客户端取消或停止读取。 +- `UNAVAILABLE`：后端断链，或状态因故障/watchdog 返回非 finite； +- `INTERNAL`：服务异常，或命令失败后的安全清理/Quick Stop 也失败。 + +## 3. MotorManager 配置 + +仓库已提供单 PLC、两轴配置示例 +`cmvr-es/config/devices/motor/plc_motors.pb.txt`，内容如下： + +```textproto +motor { + id: "plc_motors" + + motor_groups { + id: "plc_axis_group" + bus_type: MOTOR_BUS_MODBUS_TCP + vendor: MOTOR_VENDOR_PLC_GENERIC + protocol: MOTOR_PROTOCOL_CMVR_PLC_V1 + + modbus_tcp { + host: "192.168.0.10" + port: 502 + unit_id: 1 + + connect_timeout_ms: 500 + io_timeout_ms: 100 + heartbeat_period_ms: 100 + communication_watchdog_ms: 1000 + cyclic_watchdog_ms: 500 + status_poll_period_ms: 20 + reconnect_min_ms: 100 + reconnect_max_ms: 2000 + command_ack_timeout_ms: 500 + + protocol_major: 1 + protocol_minor: 0 + + axes { motor_id: 1 axis_index: 0 } + axes { motor_id: 2 axis_index: 1 } + } + + joint_limits { + enable: true + source: JOINT_LIMIT_SOURCE_CUSTOM + joints { + joint_name: "PLC_AXIS_1" + q_lb: -3.141592653589793 + q_ub: 3.141592653589793 + qd: 1.0 + qdd: 2.0 + } + joints { + joint_name: "PLC_AXIS_2" + q_lb: -1.5707963267948966 + q_ub: 1.5707963267948966 + qd: 0.5 + qdd: 1.0 + } + } + + motors { + motors { id: 1 joint_name: "PLC_AXIS_1" } + motors { id: 2 joint_name: "PLC_AXIS_2" } + } + } +} +``` + +字段行为： + +- `host` 必须是 IPv4 字面量（例如 `192.168.0.10`），不做 DNS 解析；这样 + TCP 建连和 `stop()` 不会被无界的名称解析阻塞。 +- `port=0` 时 runtime 使用 502；`unit_id=0` 时使用 1。 +- `connect_timeout_ms=0` 时使用 500 ms；该值为 TCP 建连的独立硬超时。 +- `io_timeout_ms=0` 时使用 100 ms，并同时用于 libmodbus response timeout + 和 byte timeout。 +- 心跳、通信 watchdog、周期 watchdog、状态轮询、重连和 ACK timeout + 为 `0` 时，当前 runtime 分别使用 + 100、500、500、20、100、2000、500 ms；上面的实体样例将通信 watchdog + 显式放宽为 1000 ms。 +- `communication_watchdog_ms` 必须同时不少于 + `2 * heartbeat_period_ms` 和 + `3 * io_timeout_ms + 2 * heartbeat_period_ms`； + `command_ack_timeout_ms`、`cyclic_watchdog_ms` 必须不少于 + `3 * io_timeout_ms + status_poll_period_ms`，因为一次可靠状态读取包含 + sequence-before、完整状态块、sequence-after 三次 FC3； + `cyclic_watchdog_ms` 必须在 20 ms 至 60000 ms 内； + `reconnect_max_ms` 不得小于 `reconnect_min_ms`，否则初始化失败。 +- `protocol_major=0` 时使用当前 major `1`。握手严格检查 major； + PLC 的 minor 版本不得低于配置要求。 +- `axis_index` 是 PLC 轴块索引，必须小于 PLC 发布的 `axis_count`，且同一 + group 内不能重复。 +- `encoder_counts_per_rev` 和 `gear_ratio` 对 CMVR PLC v1 不生效，因为 PLC + 协议交换的是 SI 定点值，而不是编码器 count。 + +还需要在 DeviceManager 配置中注册该 MotorSystem： + +```textproto +device_manager { + # 只有未被 RobotArm 选择且仍希望初始化全部电机时才需要 true。 + init_all_motors_when_no_active_joints: true + + devices { + id: "plc_motors" + type: DEVICE_TYPE_MOTOR_SYSTEM + config_file: "devices/motor/plc_motors.pb.txt" + enable: true + } +} +``` + +`id` 必须与 `motor.id` 以及 gRPC +`MotorTarget.header.device_id` 一致。安装产物读取 +`output/bin/config/`；修改源码树配置后需再次执行 `cmake --install build`。 + +## 4. CMVR PLC v1 Holding Register + +### 4.1 地址和 32 位编码 + +本文和 C++ runtime 中的地址都是 **零基 Modbus PDU Holding Register +地址**。地址 `0` 是第一个 Holding Register，PLC/HMI 文档若使用 `40001` +风格显示，则通常对应本文地址 `0`。不要把 `40001` 直接传给 +`modbus_read_registers()`。 + +每个寄存器是 16 位。所有 32 位有符号或无符号值均使用： + +```text +register[offset] = bits 31..16（高 WORD） +register[offset + 1] = bits 15..0 （低 WORD） +``` + +libmodbus 负责单个 16 位寄存器在线路上的字节序；PLC 应显式按“高 WORD +在前、低 WORD 在后”组合 32 位值。TIA 中建议用移位和 OR 的辅助 FC， +不要依赖 `AT` 视图、`BLKMOV` 或 CPU 内部字节布局偶然得到相同结果。 + +### 4.2 全局区 + +全局区从地址 `0` 开始，共 32 个寄存器： + +| 地址 | 长度 | 类型 | 名称 | 方向与说明 | +| ---: | ---: | --- | --- | --- | +| 0 | 1 | `UINT16` | `magic_cm` | PLC -> CMVR，固定 `16#434D` | +| 1 | 1 | `UINT16` | `magic_vr` | PLC -> CMVR，固定 `16#5652` | +| 2 | 1 | `UINT16` | `protocol_major` | PLC -> CMVR，当前为 1 | +| 3 | 1 | `UINT16` | `protocol_minor` | PLC -> CMVR，当前为 0 | +| 4 | 1 | `UINT16` | `axis_count` | PLC -> CMVR，可用轴块数量 | +| 5 | 1 | `UINT16` | `plc_global_state` | PLC -> CMVR，全局状态 | +| 6..7 | 2 | `UINT32` | `plc_boot_id` | PLC -> CMVR，每次 PLC 程序运行实例重启必须改变 | +| 8..9 | 2 | `UINT32` | `cmvr_session_id` | CMVR -> PLC，每次成功握手/重连生成新的非零随机会话 | +| 10..11 | 2 | `UINT32` | `cmvr_heartbeat` | CMVR -> PLC，周期递增 | +| 12..13 | 2 | `UINT32` | `plc_heartbeat` | PLC -> CMVR，PLC 自己的周期计数 | +| 14..15 | 2 | `UINT32` | `communication_watchdog_ms` | CMVR -> PLC，PLC 侧通信 watchdog | +| 16 | 1 | `UINT16` | `global_error` | PLC -> CMVR，全局错误 | +| 17 | 1 | `UINT16` | `owner_state` | PLC -> CMVR，当前 owner/握手状态 | +| 18..19 | 2 | `UINT32` | `owner_session_id` | PLC -> CMVR，当前 Accepted/Rejected 决策对应的候选 session | +| 20..31 | 12 | - | reserved | 写 0，PLC 忽略 | + +当前 runtime 在握手初始快照中检查 magic、版本、`axis_count` 和非零 +`plc_boot_id`，先写通信 watchdog，最后以新 session + 首个 heartbeat 作为 +候选 owner 的发布动作。握手的 +每次轮询以及 owner 接受后的最终完整快照都必须保持相同的 magic、版本、 +`axis_count` 和 boot ID；其中任一变化或 boot ID 变为 0 都会立即断线重连。 +只有 +`owner_session_id` 等于本次 session，且 `owner_state=Accepted`，连接才 +进入可发命令状态。后续心跳周期同时检查 `plc_boot_id`、owner/session 以及 +`plc_heartbeat` 是否持续推进；PLC 心跳在通信 watchdog 窗口内不变化会主动 +断线并重连。`owner_state=Rejected` 也只有在 `owner_session_id` 回显本次 +session 时才表示本次握手被拒；旧 session 遗留的 Rejected 状态会被忽略并继续 +轮询。 + +PLC 检测到新 session 后，发布顺序必须是：先把 `owner_state` 改成 +`Accepting`（此时不能先改 `owner_session_id`），然后安全停止旧 owner、清除 +mailbox/旧 stream，再写候选 `owner_session_id`，最后发布 `Accepted`。拒绝时 +同样先处于 `Accepting`/`None`，写完对应 session 后最后发布 `Rejected`。 +否则新的 session ID 可能与残留的旧 `Accepted` 短暂组合，令 CMVR 过早发命令。 + +### 4.3 每轴布局 + +轴 `i` 的基地址： + +```text +B(i) = 100 + 128 * i +``` + +每轴占 128 个 Holding Registers： + +```text +B + 0 .. B + 63 控制区 +B + 64 .. B + 127 状态区 +``` + +#### 控制区 + +| 相对地址 | 长度 | 类型 | 名称 | 当前说明 | +| ---: | ---: | --- | --- | --- | +| 0..1 | 2 | `UINT32` | `payload_sequence` | 本次轴命令序列 | +| 2 | 1 | `UINT16` | `command_code` | 见命令码表 | +| 3 | 1 | `UINT16` | `command_flags` | 当前写 0 | +| 4..5 | 2 | `INT32` | `target_position` | micro-rad | +| 6..7 | 2 | `INT32` | `target_velocity` | micro-rad/s | +| 8..9 | 2 | `INT32` | `acceleration` | micro-rad/s^2 | +| 10..11 | 2 | `INT32` | `target_torque` | 预留，mN·m | +| 12..13 | 2 | `INT32` | `position_tolerance` | 预留，micro-rad | +| 14..15 | 2 | `INT32` | `velocity_tolerance` | 预留，micro-rad/s | +| 16..17 | 2 | `UINT32` | `command_timeout_ms` | 命令完成时限；Profile 当前写 600000 ms | +| 18..19 | 2 | `UINT32` | `stream_watchdog_ms` | PLC 侧周期样本 watchdog | +| 20..21 | 2 | `UINT32` | `cyclic_sample_sequence` | 周期样本序列 | +| 22..23 | 2 | `UINT32` | `client_monotonic_time_ms` | CMVR steady-clock 低 32 位 | +| 24..25 | 2 | `UINT32` | `expected_zero_epoch` | `SetZero` 写入命令前读到的 zero epoch | +| 26 | 1 | `UINT16` | `disconnect_action` | 预留；当前驱动写 0 | +| 27..28 | 2 | `UINT32` | `command_session_id` | 必须等于当前已接受 session | +| 29..59 | 31 | - | reserved | 写 0 | +| 60..61 | 2 | `UINT32` | `payload_sequence_mirror` | 必须等于 payload sequence | +| 62..63 | 2 | `UINT32` | `commit_sequence` | 单独、最后写入 | + +#### 状态区 + +下表地址相对于 `S = B + 64`： + +| 相对地址 | 长度 | 类型 | 名称 | 说明 | +| ---: | ---: | --- | --- | --- | +| 0..1 | 2 | `UINT32` | `ack_sequence` | PLC 已解析的 command sequence | +| 2..3 | 2 | `UINT32` | `active_sequence` | 当前执行中的 command sequence | +| 4 | 1 | `UINT16` | `command_state` | 命令状态 | +| 5 | 1 | `UINT16` | `result_code` | 结果码 | +| 6 | 1 | `UINT16` | `axis_state` | PLC/驱动器轴状态 | +| 7 | 1 | `UINT16` | `current_mode` | `cmvr.msgs.RunMode` 数值 | +| 8..9 | 2 | `INT32` | `actual_position` | micro-rad | +| 10..11 | 2 | `INT32` | `actual_velocity` | micro-rad/s | +| 12..13 | 2 | `INT32` | `actual_torque` | mN·m | +| 14..15 | 2 | `INT32` | `target_position` | PLC 当前目标，micro-rad | +| 16..17 | 2 | `INT32` | `target_velocity` | PLC 当前目标，micro-rad/s | +| 18 | 1 | bit field | `status_flags` | 见状态位表 | +| 19 | 1 | `UINT16` | `drive_statusword` | 原始驱动器状态字 | +| 20..21 | 2 | `UINT32` | `fault_code` | PLC/驱动器故障码 | +| 22..23 | 2 | `UINT32` | `zero_epoch` | 置零版本 | +| 24..25 | 2 | `UINT32` | `last_applied_cyclic_sequence` | 最后实际锁存的周期样本 | +| 26..27 | 2 | `UINT32` | `state_sequence` | 状态 seqlock；非零偶数才是稳定版本 | +| 28..29 | 2 | `UINT32` | `plc_monotonic_time_ms` | PLC 单调时间低 32 位 | +| 30..31 | 2 | `UINT32` | `heartbeat_age_ms` | PLC 计算的 CMVR 心跳年龄 | +| 32..33 | 2 | `UINT32` | `ack_session_id` | `ack_sequence` 所属 session | +| 34..61 | 28 | - | reserved | PLC 写 0 | +| 62..63 | 2 | `UINT32` | `state_sequence_mirror` | 稳定版本镜像 | + +PLC 状态区使用 seqlock 发布。稳定版本从非零偶数 `2` 开始，每次完整更新增加 +`2`；版本回绕到 `0` 时跳到 `2`。发布顺序必须是： + +1. 更新任何状态字段前，先把 `state_sequence` 写成下一奇数， + `state_sequence_mirror` 保持上一个稳定偶数； +2. 写完 ACK、状态、实际值、flags、heartbeat age 等所有字段； +3. 先把 `state_sequence_mirror` 写成新的非零偶数； +4. 最后把 `state_sequence` 写成相同偶数。 + +runtime 在同一 socket 互斥区内执行三段读取：先单独读取 +`state_sequence`，再读取完整 64-word 状态块，最后再次单独读取 +`state_sequence`。只有 before、after、块内 sequence 和 mirror 四者相等， +且为非零偶数时才接受完整块；失败最多重试 3 次。这样不要求 Siemens +`MB_SERVER` 对 64-word FC3 做原子内存快照，也允许不同的完整状态读取之间版本 +持续推进。 + +为保证活性，PLC 不得在每个高速控制扫描都无条件翻转 Holding 状态版本。建议 +驱动控制状态先写内部 shadow，再由较低频的对外发布任务在字段有意义变化时复制 +到 Holding 状态区，并让每个稳定偶数版本至少覆盖三次连续 FC3 的时间窗口； +也可以使用双缓冲后按上述 seqlock 顺序发布。否则 PLC 每次 FC3 之间都推进版本， +runtime 的 3 次有限重试会按设计失败，而不是返回可能撕裂的状态。 + +### 4.4 命令码 + +| 值 | 名称 | 当前上层使用 | +| ---: | --- | --- | +| 0 | `Nop` | 否 | +| 1 | `SetZero` | `setZero` | +| 2 | `MoveToZero` | 预留；当前 `moveToZero` 发送 `ProfilePosition(target=0)` | +| 3 | `ProfilePosition` | `moveToZero`、`profilePosition` | +| 4 | `ProfileVelocity` | `profileVelocity` | +| 5 | `OpenCyclicPosition` | 第一个周期位置样本前自动发送 | +| 6 | `CyclicPositionSample` | 周期位置样本 | +| 7 | `OpenCyclicVelocity` | 第一个周期速度样本前自动发送 | +| 8 | `CyclicVelocitySample` | 周期速度样本 | +| 9 | `CloseCyclicStream` | 预留；当前流结束使用 `QuickStop` | +| 10 | `QuickStop` | `emergencyStop`、流结束和上层超时 | +| 11 | `Enable` | `setEnabled(true)` | +| 12 | `Disable` | `setEnabled(false)` | + +PLC 必须区分： + +- `SetZero`：按已确认的项目语义设置当前位置基准，不得擅自解释为运动回原点； +- `ProfilePosition(target=0)`：运动到已经建立的零位； +- Homing/寻找原点：当前 gRPC 和寄存器协议没有独立开放。 + +### 4.5 命令状态、结果和状态位 + +`command_state` 定义： + +```text +0 Idle 1 Received 2 Validating +3 Accepted 4 Running 5 TargetReached +6 Completed 7 Rejected 8 Failed +9 TimedOut 10 QuickStopped 11 CommunicationLost +``` + +runtime 会先拒绝未知状态，再按命令检查成功后置条件： + +- `SetZero`：`Completed`、`ZeroValid=1`，且 `zero_epoch` 相比命令前变化； +- `Enable`：`Completed` 且 `Enabled=1`； +- `Disable`：`Completed` 且 `Enabled=0`； +- `QuickStop`：`QuickStopped` 或 `Completed`，且实际速度已接近 0； +- Profile：`Completed` 或 `TargetReached`； +- 非终态 ACK 只接受 `Accepted`、`Running`、`TargetReached`、`Completed`。 + +`Rejected`、`Failed`、`TimedOut`、`CommunicationLost` 始终视为失败。 + +`result_code` 定义： + +```text +0 Ok 1 InvalidCommand +2 InvalidParameter 3 AxisNotReady +4 AxisBusy 5 NotEnabled +6 PositionLimit 7 VelocityLimit +8 AccelerationLimit 9 ZeroNotValid +10 DriveFault 11 CommandTimeout +12 SequenceError 13 SessionMismatch +14 CommunicationWatchdog +15 CyclicWatchdog 16 Unsupported +17 InternalError +``` + +`status_flags`： + +| bit | 名称 | +| ---: | --- | +| 0 | Enabled | +| 1 | Moving | +| 2 | TargetReached | +| 3 | Fault | +| 4 | QuickStopActive | +| 5 | CommunicationWatchdogExpired | +| 6 | CyclicWatchdogExpired | +| 7 | ZeroValid | +| 8 | StreamActive | +| 9 | CommandBusy | + +`CmvrPlcMotorProtocol` 把 `Fault`、`CommunicationWatchdogExpired` 和 +`CyclicWatchdogExpired` 都作为致命反馈状态；失败/未知 `command_state` 或非 +`Ok result_code` 同样无效。此时 `getQ/getQd` 返回 NaN，`reachedTargetQ` +返回 false，MotorService 不得把残留的有限位置/速度误判为到位或成功状态。 + +### 4.6 缩放和范围 + +CMVR PLC v1 使用固定缩放： + +| 量 | gRPC/C++ 单位 | 寄存器值 | +| --- | --- | --- | +| 位置 | rad | `round(rad * 1,000,000)`，micro-rad | +| 速度 | rad/s | `round(rad/s * 1,000,000)`，micro-rad/s | +| 加速度 | rad/s^2 | `round(rad/s^2 * 1,000,000)`，micro-rad/s^2 | +| 力矩 | N·m | 预留为 `round(N·m * 1,000)`，mN·m | + +前三者当前由 `CmvrPlcMotorProtocol` 实际使用，并在转换前检查 finite 和 +`INT32` 范围。缩放为 1,000,000 时，理论可表示范围约为 +`[-2147.483648, 2147.483647]` 个对应 SI 单位。PLC 仍必须再次执行软件限位、 +驱动器限位和状态检查，不能只依赖上位机检查。 + +Modbus PLC 电机初始化强制要求每个轴都有有限、有效的 `q_lb`、`q_ub`、`qd` +和 `qdd`：`q_ub > q_lb`，且 `qd`、`qdd` 均大于 0。缺失、NaN、无穷或非法 +限位会令电机初始化失败，不能以“未配置限位”的方式继续带轴运行。 + +## 5. payload-first、commit-last 和 ACK + +### 5.1 CMVR 写入顺序 + +每轴的 `command_sequence` 独立递增并跳过 `0`。一次命令严格执行： + +1. 仅 `SetZero` 先可靠读取 fresh `zero_epoch`；其他命令不做冗余 baseline + 状态读取； +2. 在本地构造 62 个寄存器的完整 payload； +3. 同时写入 `payload_sequence` 和 `payload_sequence_mirror`； +4. 使用一次 Holding Register 批量写，把 `B+0 .. B+61` 写入 PLC； +5. 再使用第二次写，把同一序列写入 `B+62 .. B+63`； +6. 轮询状态区，直到 `ack_sequence` 等于本次 command sequence； +7. 检查 `command_state` 和 `result_code`； +8. 周期样本还要检查 `last_applied_cyclic_sequence`。 + +PLC 只允许在以下条件全部满足时消费 payload： + +```text +commit_sequence != last_processed_commit +payload_sequence == payload_sequence_mirror +payload_sequence == commit_sequence +command_session_id == owner_session_id +当前 cmvr_session_id 是已取得 owner 的有效 session +``` + +对于 `SetZero`，PLC 还必须要求 `expected_zero_epoch` 等于执行前的当前 +`zero_epoch`；不匹配时以 `Rejected + SequenceError` ACK，避免陈旧或重复的 +置零事务改变新的零位基准。 + +PLC 应先把完整 payload 复制到内部命令快照，再更新 +`last_processed_commit`。不要一边读取 Holding Register，一边执行驱动器动作。 + +无论接受还是拒绝，PLC 都应把 `ack_sequence` 和 `ack_session_id` 更新为 +本次命令的序列和 session，同时填写 `command_state` 和 `result_code`。 +runtime 只有在两者都精确匹配时才接受 ACK，旧连接残留的相同序列不会被误认。 +否则 CMVR 只能得到模糊的 ACK timeout。 + +如果 commit 已写成功但 ACK 读取失败，电机是否已经执行是不确定的。 +CMVR runtime 不会自动重放该运动命令。特别是 `SetZero`、非零速度和使能命令， +调用方不得在未知结果下盲目重试；应先读取 PLC 状态、boot/session、zero epoch +和实际轴状态。 + +### 5.2 boot/session 防重放 + +PLC 必须把全局 session 和每轴 command sequence 共同作为命令命名空间： + +- `cmvr_session_id` 在每次成功握手/重连时重新生成非零随机值。检测到新 + session 时，PLC 必须先把 `owner_state` 置为 `Accepting`，并且这一步必须 + 早于改写 `owner_session_id`；随后安全停止旧 owner 的轴、清除旧的 + stream-active 状态，并清空各轴旧 mailbox 的 commit/payload 接收状态。 + 清理全部完成后再写入新的 `owner_session_id`，最后一步才把 + `owner_state` 发布为 `Accepted`。这样 runtime 不会把“新 session ID + + 旧 Accepted”误认为新 owner 已就绪。拒绝候选 session 时也必须先保持 + `Accepting`、写入对应 `owner_session_id`，最后一步发布 `Rejected`。 + CMVR 只有看到该 + 握手确认后才把连接标记为可用。CMVR + 会把每轴 command sequence 从 `1` 重新开始，PLC 必须在新 session + 命名空间内接受该序列。runtime 至少保证相邻两次连接的 session 不相同， + 避免紧邻重连立即复用旧 mailbox/ACK 命名空间。 +- 每个轴应保存“当前 session 下最后处理的 commit sequence”。相同 commit + 只返回原 ACK，不能再次执行。 +- PLC 启动时必须生成新的非零 `plc_boot_id`，清除 owner 和旧 commit 接受 + 状态，并要求看到新的有效 session/heartbeat 后才接受命令。若 Holding DB + 设置为 retentive，也不能让上次启动遗留的 commit 自动执行。 +- CMVR 周期检查 `plc_boot_id`。boot ID 变化会断开连接；重连成功后 + `connection_epoch` 改变。Protocol 会锁存并拒绝旧 cyclic generation， + 不会自动重新 `OpenCyclic*`。只有客户端新建 gRPC 流并通过首帧建立新 + generation 后才能恢复。 +- Profile 和 Cyclic 命令把 Protocol 已绑定的 `connection_epoch` 作为 + `expected_connection_epoch` 传给 runtime。runtime 在调用入口、等待每轴 + 队列之后，以及持有 I/O 锁准备分配 sequence/写 commit 前都要求 + invocation/current/expected 三者严格一致。因此即使重连恰好发生在 + Protocol 读取 epoch 与 runtime 写 mailbox 之间，旧命令也只会失败，不会 + 写入新 session。Quick Stop/Disable 不绑定旧 expected epoch，它们作为新调用 + 只清理当前 session。 +- command sequence 是 32 位并会回绕。PLC 应使用 session 加序列的状态机， + 明确处理回绕；不能简单把“任何不相等的值”永远视为新命令。 + +TCP 自身有序可靠，但不能替代上述应用层规则：PLC DB 可能保留旧值，PLC 和 +CMVR 也可能独立重启。 + +## 6. S7-1215C DC/DC/DC 与 TIA Portal + +### 6.1 MB_SERVER 数据块 + +建议创建一个专用、非 retentive 的协议数据块，例如： + +```scl +HoldingRegister : ARRAY[0 .. 100 + 128 * AXIS_COUNT - 1] OF WORD; +``` + +数组下标与本文零基 PDU 地址一致。根据使用的 TIA Portal 和 CPU firmware， +`MB_HOLD_REG` 对数据块访问方式可能有要求；若编译器不允许优化 DB 的 +VARIANT/指针映射，应关闭该协议 DB 的 optimized block access。不要把命令 +状态机、驱动器实例 DB 与外部可写 Holding Register 直接重叠。 + +在 OB1 或固定周期 OB 中每个扫描周期调用一个 `MB_SERVER` 实例。典型参数 +包括： + +```text +DISCONNECT = FALSE +CONNECT_ID = 项目内唯一连接 ID +IP_PORT = 502 +MB_HOLD_REG = 协议 HoldingRegister 数组 +NDR/DR/ERROR/STATUS = 诊断输出 +``` + +不同 TIA Portal 版本的块接口和 VARIANT 写法可能略有差异，应以当前工程中 +插入的 `MB_SERVER` 指令帮助为准。一个 server 实例使用自己的 instance DB； +连接 ID 和 TCP 端口不得与其他 OUC/Modbus 实例冲突。 + +`MB_SERVER` 只负责 Modbus TCP 搬运。另建 PLC FB 完成： + +1. 初始化 magic、协议版本、axis count 和 boot ID； +2. 监视 session 与 heartbeat； +3. 对每轴执行 payload/commit 原子接收； +4. 做范围、状态、使能、零位和 command timeout 校验； +5. 调用 Technology Object、PROFINET 驱动器 telegram 或项目已有驱动器 FB； +6. 更新 ACK、命令状态、结果码、实际值和状态位； +7. 执行通信/周期 watchdog 和安全降级。 + +### 6.2 32 位辅助函数 + +PLC 侧应显式实现以下等价逻辑： + +```text +DecodeUDInt(high, low) = + SHL(WORD_TO_DWORD(high), 16) OR WORD_TO_DWORD(low) + +EncodeHigh(value) = DWORD_TO_WORD(SHR(value, 16)) +EncodeLow(value) = DWORD_TO_WORD(value AND 16#0000_FFFF) +``` + +有符号值先按 DWORD 原样组合，再解释为 DINT。负值使用二进制补码；不要分别 +对高、低 WORD 做有符号运算。 + +### 6.3 驱动器动作映射 + +映射必须由实际驱动器/Technology Object 语义决定： + +- `SetZero` 只执行已确认的“当前位置建立零位”动作，不得自动替换成会运动的 + Homing。 +- “回 0 位”当前收到的是 `ProfilePosition(target=0)`。 +- Profile Position/Velocity 的轨迹生成在 PLC/驱动器侧完成；Modbus TCP + 不是驱动器位置环或电流环。 +- Cyclic Position/Velocity 是 CMVR 到 PLC 的软实时 setpoint 更新。PLC + 在本地扫描周期锁存最新样本，再由 PLC/驱动器的确定性周期执行。 +- 稳态周期样本通常需要 5 次 Modbus 事务（payload、commit，以及 + guard/full/guard 三次 ACK 快照读取）；恰逢心跳到期时增加 1 次。首个样本 + 还需要先完成一次惰性的 Open。这个事务模型不承诺固定控制频率，也不是硬 + 实时链路。 +- Quick Stop 的减速度、抱闸时序和重力轴保持策略必须在 PLC/驱动器中配置。 +- Enable/Disable 必须检查故障、STO、抱闸和轴 ready 状态；不能只翻转一个 + 普通布尔位。 + +PLC 必须在执行前再次检查位置、速度、加速度、驱动器状态和项目级互锁，并把 +拒绝原因写入 `result_code`。 + +当前测试只在 x86 loopback fake PLC 上验证功能和协议一致性，尚未给出 +S7-1215C 实机可持续频率。投产前必须在目标 TIA Portal 程序、真实 PLC 扫描 +周期和现场交换网络下阶梯增加 CSP/CSV 发送频率，记录 ACK 延迟的 +P50/P99/最大值、`dropped_setpoints`、Modbus 异常与 watchdog 触发次数。 +`cyclic_watchdog_ms` 应依据实测最坏延迟并保留工程余量设置；在完成这项台架 +测试前，不能宣称支持某个固定 Hz。 + +CMVR 中的 `QuickStop` 和 `Disable` 走 safety-priority 通道：它们会增加该轴 +的取消 generation，令正在等待的普通命令失败，并绕过普通轴命令互斥锁。安全 +命令不先读取状态，也不在 mailbox 前插入心跳写；拿到 socket 后首先发送安全 +payload/commit。它仍与单个 Modbus socket 的一次事务互斥，不会把两条报文 +交错写入；普通命令在 commit 前会再次检查 generation，避免急停完成后补发 +旧运动命令。 + +同一轴的 safety 命令使用独立 safety mutex 串行。每个 safety 调用在等待该锁 +之前就提升普通命令的取消 generation，因此抢占不会被前一个 Quick Stop 阻塞； +但后来的 safety 调用不会取消前一个 safety 调用的 ACK 等待，多个并发 +Quick Stop/Disable 都能得到各自确定的执行结果。 + +### 6.4 心跳、watchdog 和断链 + +CMVR 默认每 100 ms 写一次 `cmvr_session_id + cmvr_heartbeat`。本仓库实体 +样例要求 PLC 在 1000 ms 通信 watchdog 内看到 heartbeat **发生变化** +（字段为 0 时 runtime 默认 500 ms）。PLC 应使用自己的 +单调时间测量“最后一次变化”的年龄，不能只检查 TCP socket 仍连接，也不能把 +重复读到同一个 counter 当作有效心跳。 + +命令提交不会为每个周期样本强制写 heartbeat；runtime 记录最后一次成功写入 +时刻，仅在 `heartbeat_period_ms` 已到期时由当前命令顺带补写。supervisor +worker 仍按周期写心跳并检查 PLC boot/session/heartbeat，因此高频 setpoint +既不会产生一倍额外 Modbus 写流量，也不会饿死通信 watchdog。 + +推荐 PLC 状态机： + +```text +无 owner + -> 收到有效非零 session 且 heartbeat 开始变化 + -> owner active + -> 接受该 session 的 commit + +owner active + -> heartbeat age 超过 communication_watchdog_ms + -> 对所有 owner 轴执行受控停止/Quick Stop + -> 设置 CommunicationWatchdogExpired + -> command_state = CommunicationLost + -> 释放 owner +``` + +周期流还需要每轴独立 watchdog。只有新的 +`Cyclic*Sample.cyclic_sample_sequence` 才刷新它；普通 CMVR 心跳不能让旧的 +非零速度无限保持。周期 watchdog 超时后应停止该轴、清除 `StreamActive`， +设置 `CyclicWatchdogExpired`，并要求重新 `OpenCyclic*`。网络恢复后不得 +自动恢复断链前的速度或 setpoint；旧 gRPC 流必须失败关闭，由客户端新建流。 + +CMVR runtime 遇到 Modbus 读写错误会关闭 socket，并按 +`reconnect_min_ms` 到 `reconnect_max_ms` 指数退避重连。它不会自动重放上一 +条运动命令。PLC 侧安全动作必须在没有 CMVR 参与的情况下独立完成。 + +## 7. 安全边界 + +标准 **S7-1215C DC/DC/DC 不是 failsafe PLC**。`MB_SERVER`、普通 OB/FB、 +普通数字输出以及本服务的 `emergencyStop` 都只是功能性控制，不能提供 +安全等级的急停、STO 或防护门联锁。 + +实际设备至少应按风险评估使用： + +- 硬接线急停回路； +- 合规的安全继电器，或 F-CPU + F-I/O； +- 驱动器 STO 双通道或经认证的安全功能； +- 接触器/抱闸反馈和必要的 EDM； +- 与机械负载、重力轴和制动距离匹配的安全设计。 + +软件 `emergencyStop` 和 Modbus Quick Stop 可以作为操作层的快速停止，但 +不能替代硬接线安全回路。标准 CPU 程序卡死、以太网交换机故障、普通输出粘连 +或软件错误时，硬件安全链仍必须独立切断危险能量。 + +## 8. 构建、依赖和测试 + +### 8.1 x86-64 + +仓库已有 x86-64 的 libmodbus 3.1.11： + +```text +dependency/x86/third_party/modbus/3.1.11/include/modbus +dependency/x86/third_party/modbus/3.1.11/lib/libmodbus.so +dependency/x86/third_party/modbus/3.1.11/lib/libmodbus.so.5 +``` + +`request.txt` 已包含 `third_party/modbus/3.1.11`。标准构建流程： + +```bash +cmake -S . -B build -DBUILD_TESTING=ON +cmake --build build -j"$(nproc)" +ctest --test-dir build --output-on-failure +cmake --install build +ldd -r output/bin/cmvr_es | grep -E 'modbus|not found' +``` + +只验证本次 MotorService/PLC 电机链路时，可执行： + +```bash +cmake --build build --target \ + modbus_tcp_motor_bus_runtime_test \ + grpc_motor_service_test \ + grpc_motor_service_modbus_e2e_test \ + -j4 +ctest --test-dir build \ + -R '^(modbus_tcp_motor_bus_runtime_test|grpc_motor_service_test|grpc_motor_service_modbus_e2e_test)$' \ + --output-on-failure +``` + +当前这 3 个 CTest 目标共包含 63 个 GoogleTest 用例：Modbus runtime 25 个、 +MotorService 36 个、gRPC–Modbus 端到端 2 个。 + +测试分别覆盖： + +- Modbus runtime 的命令、离线启动后上线、重连/stop-start session 隔离、 + stale owner 决策、握手身份漂移、三段 seqlock 撕裂重试、旧 cyclic + generation 跨 epoch 的 fail-closed 锁存、Protocol-to-runtime + epoch TOCTOU 拒绝、Profile 反馈 session 绑定、CSP/CSV 样本 ACK、冗余 + 状态/心跳事务抑制、并发 safety 串行、故障反馈拒绝、非法轴/样本，以及 + 仓库真实样例配置的解析和 runtime 初始化； +- MotorService 的同步 Profile Position/Velocity 等待、取消/超时、急停竞争、 + 异常边界、CSP/CSV 流、流背压和停止失败； +- 单进程真实链路 + gRPC stub → DeviceManager → MotorManager → `AbstractMotor` → + CMVR PLC protocol → Modbus TCP fake PLC，包括使能、Profile + Position/Velocity、CSP、half-close Quick Stop、急停锁存、活动 cyclic 流 + 跨重连失败关闭、新流恢复，以及断线重连不重放。 + +fake PLC 用例会在 loopback 地址启动本地 server，运行环境必须允许本地 TCP +bind/listen。 + +运行安装产物： + +```bash +./output/bin/cmvr_es +``` + +默认 gRPC 端口由 +`cmvr-es/config/tasks/grpc_server_task/grpc_server_task.pb.txt` 配置，当前为 +`50052`。启动前应先用禁能或脱载轴验证 PLC 寄存器和方向。 + +### 8.2 启用配置和 gRPC 调用 + +首次联调前： + +1. 把 `cmvr-es/config/devices/motor/plc_motors.pb.txt` 中的 `host`、轴映射 + 和关节限位改成现场值； +2. 完成 PLC watchdog、驱动器 Quick Stop 和硬件安全链检查； +3. 将 `cmvr-es/config/manager/device_manager.pb.txt` 中 `plc_motors` 的 + `enable` 改为 `true`； +4. 重新执行 `cmake --install build`，再启动 `./output/bin/cmvr_es`。 + +启用 reflection 后，可先确认服务和状态： + +```bash +grpcurl -plaintext 127.0.0.1:50052 list cmvr.api.MotorService + +grpcurl -plaintext \ + -d '{"target":{"header":{"deviceId":"plc_motors"},"jointName":"PLC_AXIS_1"}}' \ + 127.0.0.1:50052 cmvr.api.MotorService/getStatus +``` + +在轴已安全脱载、PLC/驱动器允许使能后，显式使能并执行一个同步位置命令： + +```bash +grpcurl -plaintext \ + -d '{"target":{"header":{"deviceId":"plc_motors"},"jointName":"PLC_AXIS_1"},"enabled":true}' \ + 127.0.0.1:50052 cmvr.api.MotorService/setEnabled + +grpcurl -plaintext \ + -d '{"target":{"header":{"deviceId":"plc_motors"},"jointName":"PLC_AXIS_1"},"targetPositionRad":0.1,"maxVelocityRadS":0.2,"accelerationRadS2":0.5,"wait":{"timeoutMs":30000}}' \ + 127.0.0.1:50052 cmvr.api.MotorService/profilePosition +``` + +软件 Quick Stop： + +```bash +grpcurl -plaintext \ + -d '{"target":{"header":{"deviceId":"plc_motors"},"jointName":"PLC_AXIS_1"}}' \ + 127.0.0.1:50052 cmvr.api.MotorService/emergencyStop +``` + +双向周期流应使用生成的 gRPC client stub，并发写 setpoint、持续读反馈；不要 +用只发送一次 JSON 的 unary 调用方式模拟。客户端必须先收到 `OPENED`，发送 +首个样本，再等待相同 sequence 的 `APPLIED`。正常退出时 half-close 写端并 +继续读取，直到收到 `STOPPED` 和最终 OK status。 + +当前服务默认监听 `0.0.0.0:50052`，使用 insecure gRPC，任何可达客户端都能 +发控制命令。现场至少应绑定可信控制网接口或回环地址并配置防火墙；不得直接 +暴露到办公网或公网。若需要跨不可信网络访问，应在进入设备前增加认证、TLS +和工业安全网关。 + +### 8.3 ARM 当前缺失 + +`dependency/arm/third_party/` 当前没有 libmodbus。现有 +`libmodbus.so.5.1.0` 是 x86-64 ELF，不能复制到 ARM 设备使用。 + +ARM 支持前需要： + +1. 为目标 ARM ABI 编译 libmodbus 3.1.11； +2. 按相同布局放入 + `dependency/arm/third_party/modbus/3.1.11/{include,lib}`； +3. 确认 ARM toolchain/顶层 CMake 选择 `dependency/arm`。当前顶层 + `CMakeLists.txt` 仍把 `ARCH` 设为 `x86`； +4. 在目标设备执行 `file`、`readelf -h` 和 `ldd -r` 验证架构、SONAME 和 + 运行时依赖； +5. 重新执行无硬件测试和 PLC 台架测试。 + +在这些步骤完成前，ARM 构建应视为不支持 Modbus PLC 电机后端。 + +### 8.4 PLC 台架检查 + +建议按以下顺序验证： + +1. PLC 上电后检查 magic、版本、axis count、boot ID。 +2. 只连接 Modbus，确认 session、CMVR heartbeat 和 PLC heartbeat。 +3. 禁能状态验证错误参数、重复 commit、旧 session 和 ACK/result。 +4. 验证 `SetZero` 的项目语义，确认没有意外运动。 +5. 低速、低加速度验证 Profile Position 和 Profile Velocity。 +6. 验证周期流正常结束、gRPC watchdog 和 PLC cyclic watchdog。 +7. 分别拔网线、停止 `cmvr_es`、重启交换机、重启 PLC，确认不会恢复旧速度。 +8. 验证 `emergencyStop` 后必须显式 enable 才能再次运动。 +9. 在硬件安全回路测试合格后，才允许带载运行。 + +## 9. 当前已知约束 + +- MotorService 是单轴 API，没有多轴同扫描周期 commit。 +- `DeviceManager`/`MotorManager` 拓扑在 gRPC 服务运行期间必须保持不变； + 当前管理器只支持启动期注册，不支持在仍有 RPC 或流持有电机时热移除、 + 热替换同 ID 的 MotorManager。需要换配置时，应先停止 gRPC 服务和设备， + 再重建运行时。 +- Profile Torque、Cyclic Torque、Clear Fault 和独立 Homing 未开放。 +- gRPC 流的同步 `Write()` 可能受慢客户端背压；PLC watchdog 必须独立。 +- `getStatus` 不是一次原子 Modbus 快照。 +- Modbus TCP 不提供认证、加密或安全完整性。控制网络应隔离，并在需要时通过 + 防火墙/VPN/工业安全网关限制访问。 +- Modbus TCP 的普通软件停止不具备功能安全等级。 diff --git a/protos/cmvr/api/motor_command.proto b/protos/cmvr/api/motor_command.proto new file mode 100644 index 00000000..a02657d5 --- /dev/null +++ b/protos/cmvr/api/motor_command.proto @@ -0,0 +1,151 @@ +syntax = "proto3"; + +package cmvr.api; + +import "cmvr/api/common.proto"; +import "cmvr/msgs/motor.proto"; + +// Selects exactly one motor inside the MotorManager named by header.device_id. +message MotorTarget { + CommandHeader.Request header = 1; + oneof selector { + uint32 motor_id = 2; + string joint_name = 3; + } +} + +message MotorWaitOptions { + // Zero selects the server default (30 seconds). + uint32 timeout_ms = 1; + // Zero selects the server default (10 milliseconds). + uint32 poll_period_ms = 2; + // Zero selects the server default. + double position_tolerance_rad = 3; + double velocity_tolerance_rad_s = 4; + // Number of consecutive in-tolerance samples. Zero selects the default (3). + uint32 settle_sample_count = 5; +} + +enum MotorControlType { + MOTOR_CONTROL_NONE = 0; + MOTOR_CONTROL_SET_ZERO = 1; + MOTOR_CONTROL_PROFILE_POSITION = 2; + MOTOR_CONTROL_PROFILE_VELOCITY = 3; + MOTOR_CONTROL_CYCLIC_POSITION = 4; + MOTOR_CONTROL_CYCLIC_VELOCITY = 5; + MOTOR_CONTROL_SET_ENABLED = 6; +} + +message MotorStatus { + uint32 motor_id = 1; + string joint_name = 2; + cmvr.msgs.RunMode run_mode = 3; + double position_rad = 4; + double velocity_rad_s = 5; + bool target_reached = 6; + bool service_busy = 7; + MotorControlType active_control = 8; + bool emergency_stopped = 9; + string last_error = 10; +} + +message MotorCommandResponse { + CommandHeader.Feedback header = 1; + MotorStatus status = 2; + uint64 elapsed_ms = 3; +} + +message SetMotorZeroRequest { + MotorTarget target = 1; +} + +message MoveMotorToZeroRequest { + MotorTarget target = 1; + double max_velocity_rad_s = 2; + double acceleration_rad_s2 = 3; + MotorWaitOptions wait = 4; +} + +message ProfilePositionRequest { + MotorTarget target = 1; + double target_position_rad = 2; + double max_velocity_rad_s = 3; + double acceleration_rad_s2 = 4; + MotorWaitOptions wait = 5; +} + +message ProfileVelocityRequest { + MotorTarget target = 1; + double target_velocity_rad_s = 2; + double acceleration_rad_s2 = 3; + MotorWaitOptions wait = 4; +} + +message EmergencyStopRequest { + MotorTarget target = 1; +} + +message GetMotorStatusRequest { + MotorTarget target = 1; +} + +message GetMotorStatusResponse { + CommandHeader.Feedback header = 1; + MotorStatus status = 2; +} + +message SetMotorEnabledRequest { + MotorTarget target = 1; + bool enabled = 2; +} + +message CyclicStreamOpen { + MotorTarget target = 1; + // The PLC/driver watchdog is authoritative. This service watchdog prevents a + // stalled gRPC client from retaining control indefinitely. + uint32 watchdog_timeout_ms = 2; +} + +message CyclicPositionSetpoint { + uint64 sequence = 1; + double target_position_rad = 2; + optional double target_velocity_rad_s = 3; +} + +message CyclicVelocitySetpoint { + uint64 sequence = 1; + double target_velocity_rad_s = 2; +} + +message CyclicPositionRequest { + oneof payload { + CyclicStreamOpen open = 1; + CyclicPositionSetpoint setpoint = 2; + } +} + +message CyclicVelocityRequest { + oneof payload { + CyclicStreamOpen open = 1; + CyclicVelocitySetpoint setpoint = 2; + } +} + +enum CyclicStreamPhase { + CYCLIC_STREAM_PHASE_UNSPECIFIED = 0; + CYCLIC_STREAM_OPENED = 1; + CYCLIC_STREAM_APPLIED = 2; + CYCLIC_STREAM_STOPPED = 3; + CYCLIC_STREAM_WATCHDOG_EXPIRED = 4; + CYCLIC_STREAM_FAILED = 5; +} + +message CyclicControlResponse { + CommandHeader.Feedback header = 1; + CyclicStreamPhase phase = 2; + uint64 sequence = 3; + uint64 dropped_setpoints = 4; + // Present for OPENED and terminal responses. APPLIED deliberately omits + // live status so one cyclic sample does not trigger extra fieldbus reads. + MotorStatus status = 5; +} diff --git a/protos/cmvr/api/motor_service.proto b/protos/cmvr/api/motor_service.proto new file mode 100644 index 00000000..01f127f3 --- /dev/null +++ b/protos/cmvr/api/motor_service.proto @@ -0,0 +1,21 @@ +syntax = "proto3"; + +package cmvr.api; + +import "cmvr/api/motor_command.proto"; + +service MotorService { + rpc setZero(SetMotorZeroRequest) returns (MotorCommandResponse); + rpc moveToZero(MoveMotorToZeroRequest) returns (MotorCommandResponse); + rpc profilePosition(ProfilePositionRequest) returns (MotorCommandResponse); + rpc profileVelocity(ProfileVelocityRequest) returns (MotorCommandResponse); + + rpc streamCyclicPosition(stream CyclicPositionRequest) + returns (stream CyclicControlResponse); + rpc streamCyclicVelocity(stream CyclicVelocityRequest) + returns (stream CyclicControlResponse); + + rpc emergencyStop(EmergencyStopRequest) returns (MotorCommandResponse); + rpc getStatus(GetMotorStatusRequest) returns (GetMotorStatusResponse); + rpc setEnabled(SetMotorEnabledRequest) returns (MotorCommandResponse); +} diff --git a/protos/cmvr/config/motor_config/motor_config.proto b/protos/cmvr/config/motor_config/motor_config.proto index c31a4d8a..0612f821 100644 --- a/protos/cmvr/config/motor_config/motor_config.proto +++ b/protos/cmvr/config/motor_config/motor_config.proto @@ -61,11 +61,35 @@ message MujocoMotorGroupConfig { string world_id = 1; } +message ModbusTcpAxisConfig { + int32 motor_id = 1; + uint32 axis_index = 2; +} + +message ModbusTcpConfig { + string host = 1; + uint32 port = 2; + uint32 unit_id = 3; + uint32 connect_timeout_ms = 4; + uint32 io_timeout_ms = 5; + uint32 heartbeat_period_ms = 6; + uint32 communication_watchdog_ms = 7; + uint32 status_poll_period_ms = 8; + uint32 reconnect_min_ms = 9; + uint32 reconnect_max_ms = 10; + uint32 command_ack_timeout_ms = 11; + uint32 protocol_major = 12; + uint32 protocol_minor = 13; + uint32 cyclic_watchdog_ms = 14; + repeated ModbusTcpAxisConfig axes = 20; +} + enum MotorBusType { MOTOR_BUS_UNKNOWN = 0; MOTOR_BUS_CAN = 1; MOTOR_BUS_ETHERCAT = 2; MOTOR_BUS_MUJOCO = 3; + MOTOR_BUS_MODBUS_TCP = 4; } enum MotorVendor { @@ -73,6 +97,7 @@ enum MotorVendor { MOTOR_VENDOR_TI5 = 1; MOTOR_VENDOR_MUJOCO = 2; MOTOR_VENDOR_EYOU = 3; + MOTOR_VENDOR_PLC_GENERIC = 4; } enum MotorProtocol { @@ -80,6 +105,7 @@ enum MotorProtocol { MOTOR_PROTOCOL_CANOPEN = 1; MOTOR_PROTOCOL_ETHERCAT_CIA402 = 2; MOTOR_PROTOCOL_MUJOCO = 3; + MOTOR_PROTOCOL_CMVR_PLC_V1 = 4; } message MotorGroupConfig { @@ -92,6 +118,7 @@ message MotorGroupConfig { SocketCanConfig can = 10; EtherCATConfig ethercat = 11; MujocoMotorGroupConfig mujoco = 12; + ModbusTcpConfig modbus_tcp = 13; } JointLimitsConfig joint_limits = 30; From af6775193721b9f6c72802349eadfed39809f354 Mon Sep 17 00:00:00 2001 From: xtkuang <87661715@qq.com> Date: Fri, 31 Jul 2026 08:48:04 +0800 Subject: [PATCH 02/20] feat: add safe UME teleoperation framework Add the UME RobotArm and Damiao CAN-FD path, migrate the legacy UME controller, and introduce guarded cross-machine gRPC teleoperation with lifecycle, authority, configuration, and test coverage. --- cmvr-es/CMakeLists.txt | 3 + cmvr-es/algorithms/controllers/CMakeLists.txt | 1 + .../controllers/ume_legacy/CMakeLists.txt | 78 ++ .../pinocchio_ume_legacy_model_adapter.h | 72 ++ .../include/ume_legacy_controller.h | 34 + .../include/ume_legacy_model_adapter.h | 32 + .../ume_legacy/include/ume_legacy_types.h | 110 ++ .../pinocchio_ume_legacy_model_adapter.cpp | 585 +++++++++ .../ume_legacy/src/ume_legacy_controller.cpp | 193 +++ ...inocchio_ume_legacy_model_adapter_test.cpp | 392 ++++++ .../ume_legacy_controller_golden_test.cpp | 216 ++++ cmvr-es/common/types/arm/arm_types.h | 21 + cmvr-es/config/README.md | 48 +- cmvr-es/config/cmvr_es_robot.pb.txt | 11 + cmvr-es/config/cmvr_es_ume.pb.txt | 10 + cmvr-es/config/devices/arm/arm.pb.txt | 4 + cmvr-es/config/devices/arm/ume_arms.pb.txt | 72 ++ cmvr-es/config/manager/device_manager.pb.txt | 17 + .../manager/device_manager_robot.pb.txt | 24 + .../config/manager/device_manager_ume.pb.txt | 24 + cmvr-es/config/manager/task_manager.pb.txt | 9 + .../config/manager/task_manager_robot.pb.txt | 14 + .../config/manager/task_manager_ume.pb.txt | 12 + .../grpc_server_task/grpc_server_task.pb.txt | 18 + .../ume_teleop_task/ume_teleop_task.pb.txt | 35 + cmvr-es/devices/arm/CMakeLists.txt | 2 + .../motor_robot_arm/include/motor_robot_arm.h | 9 + .../motor_robot_arm/src/motor_robot_arm.cpp | 81 +- cmvr-es/devices/arm/robot_arm.h | 30 + cmvr-es/devices/arm/robot_arm_factory.h | 4 + .../devices/arm/ume_robot_arm/CMakeLists.txt | 92 ++ .../include/damiao_can_fd_chain.h | 145 +++ .../ume_robot_arm/include/damiao_mit_codec.h | 135 ++ .../arm/ume_robot_arm/include/ume_robot_arm.h | 181 +++ .../ume_robot_arm/src/damiao_can_fd_chain.cpp | 705 +++++++++++ .../ume_robot_arm/src/damiao_mit_codec.cpp | 254 ++++ .../arm/ume_robot_arm/src/ume_robot_arm.cpp | 1035 +++++++++++++++ .../tests/damiao_can_fd_chain_test.cpp | 407 ++++++ .../tests/damiao_mit_codec_test.cpp | 150 +++ .../tests/ume_robot_arm_test.cpp | 338 +++++ cmvr-es/devices/canbus/CMakeLists.txt | 16 +- cmvr-es/devices/canbus/abstract_canbus.h | 78 +- .../socket/socket_can_client_raw.cc | 450 ++++++- .../can_client/socket/socket_can_client_raw.h | 22 +- .../socket/socket_can_client_raw_test.cc | 256 +++- cmvr-es/devices/canbus/common/canbus_consts.h | 7 +- .../can/src/can_motor_bus_runtime.cpp | 10 +- cmvr-es/main.cpp | 81 +- .../manager/control_authority/CMakeLists.txt | 34 + .../include/control_authority_manager.h | 74 ++ .../src/control_authority_manager.cpp | 152 +++ .../tests/control_authority_manager_test.cpp | 91 ++ cmvr-es/manager/device_manager/CMakeLists.txt | 32 +- .../device_manager/include/device_manager.h | 18 +- .../device_manager/src/device_manager.cpp | 496 +++++++- .../tests/device_manager_lifecycle_test.cpp | 124 ++ .../tests/device_manager_snapshot_test.cpp | 35 +- cmvr-es/manager/task_manager/CMakeLists.txt | 26 + .../task_manager/include/task_manager.h | 7 +- .../manager/task_manager/src/task_manager.cpp | 138 +- .../tests/task_manager_lifecycle_test.cpp | 194 +++ cmvr-es/runtime/CMakeLists.txt | 31 + cmvr-es/runtime/src/cmvr_runtime.cpp | 46 +- .../runtime/tests/runtime_lifecycle_test.cpp | 266 ++++ cmvr-es/service/CMakeLists.txt | 58 + .../service/arm_teleop_client/CMakeLists.txt | 38 + .../include/grpc_arm_teleop_client.h | 80 ++ .../src/grpc_arm_teleop_client.cpp | 223 ++++ .../tests/grpc_arm_teleop_client_test.cpp | 220 ++++ .../grpc/include/grpc_arm_teleop_service.h | 91 ++ .../include/grpc_robot_arm_teleop_backend.h | 18 + cmvr-es/service/grpc/src/grpc_arm_service.cpp | 109 +- .../grpc/src/grpc_arm_teleop_service.cpp | 1111 +++++++++++++++++ .../src/grpc_robot_arm_teleop_backend.cpp | 649 ++++++++++ .../tests/grpc_arm_teleop_service_test.cpp | 671 ++++++++++ .../grpc_robot_arm_teleop_backend_test.cpp | 525 ++++++++ .../include/grpc_server_task.h | 7 + .../grpc_server_task/src/grpc_server_task.cpp | 57 + cmvr-es/task/quic_edge_task/CMakeLists.txt | 9 +- cmvr-es/task/ume_teleop_task/CMakeLists.txt | 41 + .../ume_teleop_task/include/ume_teleop_task.h | 99 ++ .../ume_teleop_task/src/ume_teleop_task.cpp | 651 ++++++++++ .../tests/ume_teleop_task_test.cpp | 429 +++++++ docs/teleoperation/ume_cmvr_architecture.md | 111 ++ docs/teleoperation/ume_cmvr_validation.md | 118 ++ model/ume/README.md | 28 + model/ume/v6_bimanual/robot.xml | 356 ++++++ model/ume/v6_imu/robot.xml | 389 ++++++ protos/cmvr/api/arm_teleop_v1.proto | 133 ++ .../cmvr/config/arm_config/arm_config.proto | 57 + .../grpc_server_config.proto | 24 + .../config/motor_config/motor_config.proto | 15 + .../task_manager_config.proto | 1 + .../ume_teleop_config/ume_teleop_config.proto | 30 + 94 files changed, 14389 insertions(+), 246 deletions(-) create mode 100644 cmvr-es/algorithms/controllers/ume_legacy/CMakeLists.txt create mode 100644 cmvr-es/algorithms/controllers/ume_legacy/include/pinocchio_ume_legacy_model_adapter.h create mode 100644 cmvr-es/algorithms/controllers/ume_legacy/include/ume_legacy_controller.h create mode 100644 cmvr-es/algorithms/controllers/ume_legacy/include/ume_legacy_model_adapter.h create mode 100644 cmvr-es/algorithms/controllers/ume_legacy/include/ume_legacy_types.h create mode 100644 cmvr-es/algorithms/controllers/ume_legacy/src/pinocchio_ume_legacy_model_adapter.cpp create mode 100644 cmvr-es/algorithms/controllers/ume_legacy/src/ume_legacy_controller.cpp create mode 100644 cmvr-es/algorithms/controllers/ume_legacy/tests/pinocchio_ume_legacy_model_adapter_test.cpp create mode 100644 cmvr-es/algorithms/controllers/ume_legacy/tests/ume_legacy_controller_golden_test.cpp create mode 100644 cmvr-es/config/cmvr_es_robot.pb.txt create mode 100644 cmvr-es/config/cmvr_es_ume.pb.txt create mode 100644 cmvr-es/config/devices/arm/ume_arms.pb.txt create mode 100644 cmvr-es/config/manager/device_manager_robot.pb.txt create mode 100644 cmvr-es/config/manager/device_manager_ume.pb.txt create mode 100644 cmvr-es/config/manager/task_manager_robot.pb.txt create mode 100644 cmvr-es/config/manager/task_manager_ume.pb.txt create mode 100644 cmvr-es/config/tasks/ume_teleop_task/ume_teleop_task.pb.txt create mode 100644 cmvr-es/devices/arm/ume_robot_arm/CMakeLists.txt create mode 100644 cmvr-es/devices/arm/ume_robot_arm/include/damiao_can_fd_chain.h create mode 100644 cmvr-es/devices/arm/ume_robot_arm/include/damiao_mit_codec.h create mode 100644 cmvr-es/devices/arm/ume_robot_arm/include/ume_robot_arm.h create mode 100644 cmvr-es/devices/arm/ume_robot_arm/src/damiao_can_fd_chain.cpp create mode 100644 cmvr-es/devices/arm/ume_robot_arm/src/damiao_mit_codec.cpp create mode 100644 cmvr-es/devices/arm/ume_robot_arm/src/ume_robot_arm.cpp create mode 100644 cmvr-es/devices/arm/ume_robot_arm/tests/damiao_can_fd_chain_test.cpp create mode 100644 cmvr-es/devices/arm/ume_robot_arm/tests/damiao_mit_codec_test.cpp create mode 100644 cmvr-es/devices/arm/ume_robot_arm/tests/ume_robot_arm_test.cpp create mode 100644 cmvr-es/manager/control_authority/CMakeLists.txt create mode 100644 cmvr-es/manager/control_authority/include/control_authority_manager.h create mode 100644 cmvr-es/manager/control_authority/src/control_authority_manager.cpp create mode 100644 cmvr-es/manager/control_authority/tests/control_authority_manager_test.cpp create mode 100644 cmvr-es/manager/device_manager/tests/device_manager_lifecycle_test.cpp create mode 100644 cmvr-es/manager/task_manager/tests/task_manager_lifecycle_test.cpp create mode 100644 cmvr-es/runtime/tests/runtime_lifecycle_test.cpp create mode 100644 cmvr-es/service/arm_teleop_client/CMakeLists.txt create mode 100644 cmvr-es/service/arm_teleop_client/include/grpc_arm_teleop_client.h create mode 100644 cmvr-es/service/arm_teleop_client/src/grpc_arm_teleop_client.cpp create mode 100644 cmvr-es/service/arm_teleop_client/tests/grpc_arm_teleop_client_test.cpp create mode 100644 cmvr-es/service/grpc/include/grpc_arm_teleop_service.h create mode 100644 cmvr-es/service/grpc/include/grpc_robot_arm_teleop_backend.h create mode 100644 cmvr-es/service/grpc/src/grpc_arm_teleop_service.cpp create mode 100644 cmvr-es/service/grpc/src/grpc_robot_arm_teleop_backend.cpp create mode 100644 cmvr-es/service/grpc/tests/grpc_arm_teleop_service_test.cpp create mode 100644 cmvr-es/service/grpc/tests/grpc_robot_arm_teleop_backend_test.cpp create mode 100644 cmvr-es/task/ume_teleop_task/CMakeLists.txt create mode 100644 cmvr-es/task/ume_teleop_task/include/ume_teleop_task.h create mode 100644 cmvr-es/task/ume_teleop_task/src/ume_teleop_task.cpp create mode 100644 cmvr-es/task/ume_teleop_task/tests/ume_teleop_task_test.cpp create mode 100644 docs/teleoperation/ume_cmvr_architecture.md create mode 100644 docs/teleoperation/ume_cmvr_validation.md create mode 100644 model/ume/README.md create mode 100644 model/ume/v6_bimanual/robot.xml create mode 100644 model/ume/v6_imu/robot.xml create mode 100644 protos/cmvr/api/arm_teleop_v1.proto create mode 100644 protos/cmvr/config/ume_teleop_config/ume_teleop_config.proto diff --git a/cmvr-es/CMakeLists.txt b/cmvr-es/CMakeLists.txt index 0ff7c443..7ef09113 100644 --- a/cmvr-es/CMakeLists.txt +++ b/cmvr-es/CMakeLists.txt @@ -6,11 +6,14 @@ add_subdirectory(hardware) add_subdirectory(algorithms) add_subdirectory(simulate) add_subdirectory(devices) +add_subdirectory(manager/control_authority) add_subdirectory(manager/device_manager) add_subdirectory(manager/media_source_hub) add_subdirectory(service/quic_edge) +add_subdirectory(service/arm_teleop_client) add_subdirectory(task) add_subdirectory(task/quic_edge_task) +add_subdirectory(task/ume_teleop_task) add_subdirectory(manager/task_manager) add_subdirectory(service) add_subdirectory(runtime) diff --git a/cmvr-es/algorithms/controllers/CMakeLists.txt b/cmvr-es/algorithms/controllers/CMakeLists.txt index 1921842a..4cc76775 100644 --- a/cmvr-es/algorithms/controllers/CMakeLists.txt +++ b/cmvr-es/algorithms/controllers/CMakeLists.txt @@ -1,5 +1,6 @@ add_subdirectory(arm_control) +add_subdirectory(ume_legacy) #find_package(VISP REQUIRED) diff --git a/cmvr-es/algorithms/controllers/ume_legacy/CMakeLists.txt b/cmvr-es/algorithms/controllers/ume_legacy/CMakeLists.txt new file mode 100644 index 00000000..73bc703c --- /dev/null +++ b/cmvr-es/algorithms/controllers/ume_legacy/CMakeLists.txt @@ -0,0 +1,78 @@ +add_library(ume_legacy_controller SHARED + src/ume_legacy_controller.cpp + src/pinocchio_ume_legacy_model_adapter.cpp +) + +target_include_directories(ume_legacy_controller + PUBLIC + ${CMAKE_CURRENT_SOURCE_DIR}/include +) + +target_link_libraries(ume_legacy_controller + PRIVATE + pinocchio_default + pinocchio_parsers +) + +add_library( + cmvr_es::algorithms::ume_legacy + ALIAS ume_legacy_controller +) + +install(TARGETS ume_legacy_controller LIBRARY DESTINATION lib) + +if(BUILD_TESTING) + add_executable(ume_legacy_controller_golden_test + tests/ume_legacy_controller_golden_test.cpp + ) + add_executable(pinocchio_ume_legacy_model_adapter_test + tests/pinocchio_ume_legacy_model_adapter_test.cpp + ) + + target_link_libraries(ume_legacy_controller_golden_test + PRIVATE + cmvr_es::algorithms::ume_legacy + gtest + gtest_main + pthread + ) + target_link_libraries(pinocchio_ume_legacy_model_adapter_test + PRIVATE + cmvr_es::algorithms::ume_legacy + gtest + gtest_main + pthread + ) + + foreach(_ume_legacy_test_target + ume_legacy_controller_golden_test + pinocchio_ume_legacy_model_adapter_test) + target_compile_definitions(${_ume_legacy_test_target} + PRIVATE + CMVR_UME_FIXED_MODEL_PATH="${CMAKE_SOURCE_DIR}/model/ume/v6_bimanual/robot.xml" + CMVR_UME_FLOATING_MODEL_PATH="${CMAKE_SOURCE_DIR}/model/ume/v6_imu/robot.xml" + ) + add_test( + NAME ${_ume_legacy_test_target} + COMMAND ${_ume_legacy_test_target} + ) + endforeach() + + set(_ume_legacy_test_environment + "LD_LIBRARY_PATH=${CMVR_TEST_EXTERNAL_LIBRARY_PATH}" + ) + + if(CMVR_TEST_SYSTEM_LIBSTDCXX) + list(APPEND _ume_legacy_test_environment + "LD_PRELOAD=${CMVR_TEST_SYSTEM_LIBSTDCXX}") + endif() + + foreach(_ume_legacy_test_target + ume_legacy_controller_golden_test + pinocchio_ume_legacy_model_adapter_test) + set_tests_properties(${_ume_legacy_test_target} PROPERTIES + TIMEOUT 10 + ENVIRONMENT "${_ume_legacy_test_environment}" + ) + endforeach() +endif() diff --git a/cmvr-es/algorithms/controllers/ume_legacy/include/pinocchio_ume_legacy_model_adapter.h b/cmvr-es/algorithms/controllers/ume_legacy/include/pinocchio_ume_legacy_model_adapter.h new file mode 100644 index 00000000..720e0f5c --- /dev/null +++ b/cmvr-es/algorithms/controllers/ume_legacy/include/pinocchio_ume_legacy_model_adapter.h @@ -0,0 +1,72 @@ +#ifndef CMVR_ES_PINOCCHIO_UME_LEGACY_MODEL_ADAPTER_H +#define CMVR_ES_PINOCCHIO_UME_LEGACY_MODEL_ADAPTER_H + +#include +#include +#include + +#include "ume_legacy_model_adapter.h" + +namespace cmvr::ume_legacy { + +struct UmeLegacyModelContract { + std::size_t fixed_nq{0}; + std::size_t fixed_nv{0}; + std::size_t floating_nq{0}; + std::size_t floating_nv{0}; + Transform4x4RowMajor base_from_imu{}; +}; + +// Concrete adapter for the two original UME MJCF models. +// +// Construction parses both models and throws std::runtime_error if their +// dimensions, joint ordering, joint coordinate indices, or required frame +// topology differ from the frozen structural legacy contract. +// +// The floating model evaluates the original rnea(q, measured_arm_velocity, 0) +// path: base twist and all accelerations are zero. Consequently its result +// preserves the legacy velocity-dependent terms as well as gravity. +// +// Pinocchio Data objects are mutable workspaces. One adapter instance must be +// used by one controller thread at a time. All Eigen workspaces are allocated +// at construction and reused on the control path. +class PinocchioUmeLegacyModelAdapter final + : public UmeLegacyModelAdapter { +public: + PinocchioUmeLegacyModelAdapter( + std::string fixed_model_path, + std::string floating_model_path); + ~PinocchioUmeLegacyModelAdapter() override; + + PinocchioUmeLegacyModelAdapter( + const PinocchioUmeLegacyModelAdapter&) = delete; + PinocchioUmeLegacyModelAdapter& operator=( + const PinocchioUmeLegacyModelAdapter&) = delete; + PinocchioUmeLegacyModelAdapter( + PinocchioUmeLegacyModelAdapter&&) noexcept; + PinocchioUmeLegacyModelAdapter& operator=( + PinocchioUmeLegacyModelAdapter&&) noexcept; + + bool computeGravityCompensation( + const BimanualModelState& state, + JointVector& right_gravity_nm, + JointVector& left_gravity_nm) const override; + + bool projectHapticFeedback( + const BimanualModelState& state, + ArmSide side, + const RawHapticFeedback& feedback, + ProjectedHapticEffort& projected) const override; + + const UmeLegacyModelContract& contract() const noexcept; + const std::string& fixedModelPath() const noexcept; + const std::string& floatingModelPath() const noexcept; + +private: + class Impl; + std::unique_ptr impl_; +}; + +} // namespace cmvr::ume_legacy + +#endif // CMVR_ES_PINOCCHIO_UME_LEGACY_MODEL_ADAPTER_H diff --git a/cmvr-es/algorithms/controllers/ume_legacy/include/ume_legacy_controller.h b/cmvr-es/algorithms/controllers/ume_legacy/include/ume_legacy_controller.h new file mode 100644 index 00000000..4c706704 --- /dev/null +++ b/cmvr-es/algorithms/controllers/ume_legacy/include/ume_legacy_controller.h @@ -0,0 +1,34 @@ +#ifndef CMVR_ES_UME_LEGACY_CONTROLLER_H +#define CMVR_ES_UME_LEGACY_CONTROLLER_H + +#include "ume_legacy_types.h" + +namespace cmvr::ume_legacy { + +// Constants from UME commit e087df5cd3b281418722e155d9975695f163698e: +// ume/robot/ume/v6_imu/ume_leader/controller.py +// ume/robot/openarm1/teleop_leader_tuning.py +LegacyUmeTuning originalTuning() noexcept; + +JointVector frictionCompensation( + const JointVector& velocity_rad_s, + const LegacyUmeTuning& tuning) noexcept; + +JointVector stictionCompensation( + const JointVector& velocity_rad_s, + const LegacyUmeTuning& tuning) noexcept; + +// error_norm is non-negative in the legacy path because it is produced by +// np.linalg.norm. std::abs is retained here to match the subsequent Python +// expression exactly for direct unit-level use. +double feedbackScale( + double error_norm, + const LegacyUmeTuning& tuning) noexcept; + +SideControlOutput computeSideCommand( + const SideControlInput& input, + const LegacyUmeTuning& tuning = originalTuning()) noexcept; + +} // namespace cmvr::ume_legacy + +#endif // CMVR_ES_UME_LEGACY_CONTROLLER_H diff --git a/cmvr-es/algorithms/controllers/ume_legacy/include/ume_legacy_model_adapter.h b/cmvr-es/algorithms/controllers/ume_legacy/include/ume_legacy_model_adapter.h new file mode 100644 index 00000000..68a6d0a6 --- /dev/null +++ b/cmvr-es/algorithms/controllers/ume_legacy/include/ume_legacy_model_adapter.h @@ -0,0 +1,32 @@ +#ifndef CMVR_ES_UME_LEGACY_MODEL_ADAPTER_H +#define CMVR_ES_UME_LEGACY_MODEL_ADAPTER_H + +#include "ume_legacy_types.h" + +namespace cmvr::ume_legacy { + +// Boundary for the two model operations used by the original IMU controller: +// 1. floating-base RNEA gravity compensation; +// 2. fixed-base J_rot^T projection of shoulder/wrist moments. +// +// Concrete implementations must load and validate their model contract so the +// pure controller cannot silently substitute guessed kinematics or dynamics. +class UmeLegacyModelAdapter { +public: + virtual ~UmeLegacyModelAdapter() = default; + + virtual bool computeGravityCompensation( + const BimanualModelState& state, + JointVector& right_gravity_nm, + JointVector& left_gravity_nm) const = 0; + + virtual bool projectHapticFeedback( + const BimanualModelState& state, + ArmSide side, + const RawHapticFeedback& feedback, + ProjectedHapticEffort& projected) const = 0; +}; + +} // namespace cmvr::ume_legacy + +#endif // CMVR_ES_UME_LEGACY_MODEL_ADAPTER_H diff --git a/cmvr-es/algorithms/controllers/ume_legacy/include/ume_legacy_types.h b/cmvr-es/algorithms/controllers/ume_legacy/include/ume_legacy_types.h new file mode 100644 index 00000000..fc7dc211 --- /dev/null +++ b/cmvr-es/algorithms/controllers/ume_legacy/include/ume_legacy_types.h @@ -0,0 +1,110 @@ +#ifndef CMVR_ES_UME_LEGACY_TYPES_H +#define CMVR_ES_UME_LEGACY_TYPES_H + +#include +#include + +namespace cmvr::ume_legacy { + +inline constexpr std::size_t kArmDof = 8; +inline constexpr std::size_t kTransformElementCount = 16; + +using JointVector = std::array; +using Vector3 = std::array; +using Transform4x4RowMajor = + std::array; + +enum class ArmSide { + Right, + Left +}; + +// Shoulder and wrist entries have already been projected by J_rot^T. The +// elbow and gripper entries are the scalar follower efforts received by the +// original UME controller. Keeping this type separate prevents a 3-D moment +// from being mislabeled as a 6-D Cartesian wrench. +struct ProjectedHapticEffort { + Vector3 shoulder_joint_torque{}; + double elbow_effort{0.0}; + Vector3 wrist_joint_torque{}; + double gripper_effort{0.0}; +}; + +struct TrackingError { + Vector3 shoulder_rotation{}; + double elbow{0.0}; + Vector3 wrist_rotation{}; + double gripper{0.0}; +}; + +struct FeedbackScales { + double shoulder{0.0}; + double elbow{0.0}; + double wrist{0.0}; + double gripper{0.0}; +}; + +struct LegacyUmeTuning { + JointVector friction_coefficient{}; + JointVector friction_max_compensation{}; + JointVector stiction_threshold_min_rad_s{}; + JointVector stiction_threshold_max_rad_s{}; + JointVector stiction_compensation{}; + + double feedback_error_tolerance_rad{0.0}; + double feedback_tanh_sharpness{0.0}; + double feedback_scale{0.0}; + double feedback_limit_dm4340_nm{0.0}; + double feedback_limit_dm4310_nm{0.0}; +}; + +struct SideControlInput { + ArmSide side{ArmSide::Right}; + JointVector joint_velocity_rad_s{}; + JointVector gravity_compensation_nm{}; + ProjectedHapticEffort projected_haptic{}; + TrackingError tracking_error{}; +}; + +struct SideControlOutput { + JointVector friction_compensation_nm{}; + JointVector stiction_compensation_nm{}; + JointVector feedforward_without_haptic_nm{}; + + // This is the interaction effort after the legacy left/right scalar sign + // conventions, but before scaling and clipping. + JointVector signed_interaction_nm{}; + FeedbackScales feedback_scales{}; + + // The legacy algorithm clips only this feedback contribution. It does not + // apply a final clamp to gravity, friction, stiction, or command_torque. + JointVector limited_feedback_nm{}; + JointVector command_torque_nm{}; +}; + +// Pure model inputs/outputs shared by the legacy controller and its +// Pinocchio/MJCF model adapter. +struct BimanualModelState { + // Joint arrays follow the frozen RJ1..RJ8 / LJ1..LJ8 MJCF order. + JointVector right_position_rad{}; + JointVector right_velocity_rad_s{}; + JointVector left_position_rad{}; + JointVector left_velocity_rad_s{}; + + // Homogeneous rigid transform from the IMU frame to the gravity/world + // frame. The adapter rejects non-finite and non-rigid matrices. + Transform4x4RowMajor world_from_imu{}; +}; + +struct RawHapticFeedback { + // Moments use the LOCAL_WORLD_ALIGNED frame expected by the original + // Pinocchio J_rot^T mapping. + Vector3 shoulder_moment{}; + double elbow_effort{0.0}; + Vector3 wrist_moment{}; + double gripper_effort{0.0}; +}; + +} // namespace cmvr::ume_legacy + +#endif // CMVR_ES_UME_LEGACY_TYPES_H diff --git a/cmvr-es/algorithms/controllers/ume_legacy/src/pinocchio_ume_legacy_model_adapter.cpp b/cmvr-es/algorithms/controllers/ume_legacy/src/pinocchio_ume_legacy_model_adapter.cpp new file mode 100644 index 00000000..f298ef3c --- /dev/null +++ b/cmvr-es/algorithms/controllers/ume_legacy/src/pinocchio_ume_legacy_model_adapter.cpp @@ -0,0 +1,585 @@ +#include "pinocchio_ume_legacy_model_adapter.h" + +#include +#include +#include +#include +#include + +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +namespace cmvr::ume_legacy { +namespace { + +using ExpectedArmJointNames = std::array; + +const ExpectedArmJointNames& expectedArmJointNames() +{ + static const ExpectedArmJointNames names{ + "RJ1", "RJ2", "RJ3", "RJ4", + "RJ5", "RJ6", "RJ7", "RJ8", + "LJ1", "LJ2", "LJ3", "LJ4", + "LJ5", "LJ6", "LJ7", "LJ8"}; + return names; +} + +std::runtime_error contractError( + const std::string& model_kind, + const std::string& detail) +{ + return std::runtime_error( + "UME " + model_kind + " MJCF contract violation: " + detail); +} + +void requireDimensions( + const pinocchio::Model& model, + const std::string& model_kind, + int nq, + int nv, + pinocchio::JointIndex njoints) +{ + if (model.nq != nq || + model.nv != nv || + model.njoints != njoints) { + std::ostringstream detail; + detail << "expected nq/nv/njoints " + << nq << "/" << nv << "/" << njoints + << ", got " << model.nq << "/" << model.nv + << "/" << model.njoints; + throw contractError(model_kind, detail.str()); + } +} + +void requireJoint( + const pinocchio::Model& model, + const std::string& model_kind, + pinocchio::JointIndex joint_index, + const std::string& expected_name, + int expected_idx_q, + int expected_nq, + int expected_idx_v, + int expected_nv) +{ + if (joint_index >= model.njoints) { + throw contractError( + model_kind, + "missing joint " + expected_name); + } + + if (model.names[joint_index] != expected_name || + model.idx_qs[joint_index] != expected_idx_q || + model.nqs[joint_index] != expected_nq || + model.idx_vs[joint_index] != expected_idx_v || + model.nvs[joint_index] != expected_nv) { + std::ostringstream detail; + detail << "joint[" << joint_index << "] expected " + << expected_name << " q(" << expected_idx_q + << "," << expected_nq << ") v(" << expected_idx_v + << "," << expected_nv << "), got " + << model.names[joint_index] << " q(" + << model.idx_qs[joint_index] << "," + << model.nqs[joint_index] << ") v(" + << model.idx_vs[joint_index] << "," + << model.nvs[joint_index] << ")"; + throw contractError(model_kind, detail.str()); + } +} + +pinocchio::FrameIndex requireUniqueFrame( + const pinocchio::Model& model, + const std::string& model_kind, + const std::string& frame_name, + const std::string& expected_parent_joint_name) +{ + pinocchio::FrameIndex found = model.nframes; + std::size_t count = 0; + for (pinocchio::FrameIndex index = 0; + index < model.nframes; + ++index) { + if (model.frames[index].name == frame_name) { + found = index; + ++count; + } + } + if (count != 1) { + std::ostringstream detail; + detail << "expected exactly one frame " << frame_name + << ", got " << count; + throw contractError(model_kind, detail.str()); + } + + const auto parent_joint = model.frames[found].parentJoint; + if (parent_joint >= model.njoints || + model.names[parent_joint] != expected_parent_joint_name) { + std::ostringstream detail; + detail << "frame " << frame_name + << " expected parent joint " + << expected_parent_joint_name; + if (parent_joint < model.njoints) { + detail << ", got " << model.names[parent_joint]; + } else { + detail << ", got invalid index " << parent_joint; + } + throw contractError(model_kind, detail.str()); + } + return found; +} + +void validateFixedModel( + const pinocchio::Model& model, + std::array& frame_ids) +{ + requireDimensions(model, "fixed", 16, 16, 17); + const auto& names = expectedArmJointNames(); + for (std::size_t index = 0; index < names.size(); ++index) { + requireJoint( + model, + "fixed", + static_cast(index + 1), + names[index], + static_cast(index), + 1, + static_cast(index), + 1); + } + + frame_ids[0] = + requireUniqueFrame(model, "fixed", "R_shoulder", "RJ3"); + frame_ids[1] = + requireUniqueFrame(model, "fixed", "R_wrist", "RJ7"); + frame_ids[2] = + requireUniqueFrame(model, "fixed", "L_shoulder", "LJ3"); + frame_ids[3] = + requireUniqueFrame(model, "fixed", "L_wrist", "LJ7"); +} + +pinocchio::FrameIndex validateFloatingModel( + const pinocchio::Model& model) +{ + requireDimensions(model, "floating", 23, 22, 18); + requireJoint( + model, + "floating", + 1, + "dm_j4340_2ec_freejoint", + 0, + 7, + 0, + 6); + + const auto& names = expectedArmJointNames(); + for (std::size_t index = 0; index < names.size(); ++index) { + requireJoint( + model, + "floating", + static_cast(index + 2), + names[index], + static_cast(index + 7), + 1, + static_cast(index + 6), + 1); + } + + requireUniqueFrame( + model, "floating", "R_shoulder", "RJ3"); + requireUniqueFrame( + model, "floating", "R_wrist", "RJ7"); + requireUniqueFrame( + model, "floating", "L_shoulder", "LJ3"); + requireUniqueFrame( + model, "floating", "L_wrist", "LJ7"); + return requireUniqueFrame( + model, + "floating", + "imu", + "dm_j4340_2ec_freejoint"); +} + +bool finite(const JointVector& values) noexcept +{ + for (const double value : values) { + if (!std::isfinite(value)) { + return false; + } + } + return true; +} + +bool finite(const Vector3& values) noexcept +{ + for (const double value : values) { + if (!std::isfinite(value)) { + return false; + } + } + return true; +} + +bool toIsometry( + const Transform4x4RowMajor& source, + Eigen::Isometry3d& destination) noexcept +{ + Eigen::Matrix4d matrix; + for (Eigen::Index row = 0; row < 4; ++row) { + for (Eigen::Index column = 0; column < 4; ++column) { + matrix(row, column) = + source[static_cast(row * 4 + column)]; + } + } + if (!matrix.allFinite()) { + return false; + } + + constexpr double kTransformTolerance = 1e-6; + if (std::abs(matrix(3, 0)) > kTransformTolerance || + std::abs(matrix(3, 1)) > kTransformTolerance || + std::abs(matrix(3, 2)) > kTransformTolerance || + std::abs(matrix(3, 3) - 1.0) > kTransformTolerance) { + return false; + } + + const Eigen::Matrix3d rotation = + matrix.template block<3, 3>(0, 0); + if (!(rotation.transpose() * rotation) + .isApprox(Eigen::Matrix3d::Identity(), + kTransformTolerance) || + std::abs(rotation.determinant() - 1.0) > + kTransformTolerance) { + return false; + } + + destination = Eigen::Isometry3d::Identity(); + destination.linear() = rotation; + destination.translation() = + matrix.template block<3, 1>(0, 3); + return true; +} + +Transform4x4RowMajor toRowMajor( + const Eigen::Matrix4d& matrix) noexcept +{ + Transform4x4RowMajor result{}; + for (Eigen::Index row = 0; row < 4; ++row) { + for (Eigen::Index column = 0; column < 4; ++column) { + result[static_cast(row * 4 + column)] = + matrix(row, column); + } + } + return result; +} + +Eigen::Vector3d toEigen(const Vector3& value) noexcept +{ + return {value[0], value[1], value[2]}; +} + +Vector3 fromEigen(const Eigen::Vector3d& value) noexcept +{ + return {value.x(), value.y(), value.z()}; +} + +} // namespace + +class PinocchioUmeLegacyModelAdapter::Impl { +public: + Impl(std::string fixed_path, std::string floating_path) + : fixed_model_path(std::move(fixed_path)), + floating_model_path(std::move(floating_path)) + { + try { + pinocchio::mjcf::buildModel( + fixed_model_path, fixed_model, false); + } catch (const std::exception& error) { + throw std::runtime_error( + "Failed to load fixed UME MJCF '" + + fixed_model_path + "': " + error.what()); + } + try { + pinocchio::mjcf::buildModel( + floating_model_path, floating_model, false); + } catch (const std::exception& error) { + throw std::runtime_error( + "Failed to load floating UME MJCF '" + + floating_model_path + "': " + error.what()); + } + + validateFixedModel(fixed_model, fixed_frame_ids); + const auto imu_frame_id = + validateFloatingModel(floating_model); + + fixed_data = + std::make_unique(fixed_model); + floating_data = + std::make_unique(floating_model); + fixed_q = + Eigen::VectorXd::Zero(fixed_model.nq); + floating_q = + Eigen::VectorXd::Zero(floating_model.nq); + floating_velocity = + Eigen::VectorXd::Zero(floating_model.nv); + floating_acceleration = + Eigen::VectorXd::Zero(floating_model.nv); + shoulder_jacobian = + Eigen::Matrix::Zero( + 6, fixed_model.nv); + wrist_jacobian = + Eigen::Matrix::Zero( + 6, fixed_model.nv); + + Eigen::VectorXd neutral = + pinocchio::neutral(floating_model); + pinocchio::framesForwardKinematics( + floating_model, *floating_data, neutral); + base_from_imu = + floating_data->oMf[imu_frame_id]; + + const Eigen::Matrix4d base_from_imu_matrix = + base_from_imu.toHomogeneousMatrix(); + if (!base_from_imu_matrix.allFinite()) { + throw contractError( + "floating", "non-finite base_from_imu transform"); + } + + contract_info.fixed_nq = + static_cast(fixed_model.nq); + contract_info.fixed_nv = + static_cast(fixed_model.nv); + contract_info.floating_nq = + static_cast(floating_model.nq); + contract_info.floating_nv = + static_cast(floating_model.nv); + contract_info.base_from_imu = + toRowMajor(base_from_imu_matrix); + } + + std::string fixed_model_path; + std::string floating_model_path; + pinocchio::Model fixed_model; + pinocchio::Model floating_model; + std::unique_ptr fixed_data; + std::unique_ptr floating_data; + std::array fixed_frame_ids{}; + pinocchio::SE3 base_from_imu{pinocchio::SE3::Identity()}; + UmeLegacyModelContract contract_info; + + // Reused by the single controller thread. This keeps the 2 kHz legacy + // model path free of avoidable Eigen heap allocation after construction. + Eigen::VectorXd fixed_q; + Eigen::VectorXd floating_q; + Eigen::VectorXd floating_velocity; + Eigen::VectorXd floating_acceleration; + Eigen::Matrix shoulder_jacobian; + Eigen::Matrix wrist_jacobian; +}; + +PinocchioUmeLegacyModelAdapter::PinocchioUmeLegacyModelAdapter( + std::string fixed_model_path, + std::string floating_model_path) + : impl_(std::make_unique( + std::move(fixed_model_path), + std::move(floating_model_path))) +{ +} + +PinocchioUmeLegacyModelAdapter:: + ~PinocchioUmeLegacyModelAdapter() = default; + +PinocchioUmeLegacyModelAdapter::PinocchioUmeLegacyModelAdapter( + PinocchioUmeLegacyModelAdapter&&) noexcept = default; + +PinocchioUmeLegacyModelAdapter& +PinocchioUmeLegacyModelAdapter::operator=( + PinocchioUmeLegacyModelAdapter&&) noexcept = default; + +bool PinocchioUmeLegacyModelAdapter::computeGravityCompensation( + const BimanualModelState& state, + JointVector& right_gravity_nm, + JointVector& left_gravity_nm) const +{ + right_gravity_nm = {}; + left_gravity_nm = {}; + + if (!impl_ || + !finite(state.right_position_rad) || + !finite(state.right_velocity_rad_s) || + !finite(state.left_position_rad) || + !finite(state.left_velocity_rad_s)) { + return false; + } + + Eigen::Isometry3d world_from_imu; + if (!toIsometry(state.world_from_imu, world_from_imu)) { + return false; + } + + Eigen::Isometry3d base_from_imu = + Eigen::Isometry3d::Identity(); + base_from_imu.linear() = + impl_->base_from_imu.rotation(); + base_from_imu.translation() = + impl_->base_from_imu.translation(); + const Eigen::Isometry3d world_from_base = + world_from_imu * base_from_imu.inverse(); + + Eigen::Quaterniond world_q_base( + world_from_base.rotation()); + if (!world_q_base.coeffs().allFinite() || + world_q_base.norm() <= 0.0) { + return false; + } + world_q_base.normalize(); + + auto& q = impl_->floating_q; + auto& velocity = impl_->floating_velocity; + auto& acceleration = impl_->floating_acceleration; + q.setZero(); + velocity.setZero(); + acceleration.setZero(); + + q.segment<3>(0) = world_from_base.translation(); + q.segment<4>(3) = world_q_base.coeffs(); + for (std::size_t index = 0; index < kArmDof; ++index) { + q[static_cast(7 + index)] = + state.right_position_rad[index]; + q[static_cast(15 + index)] = + state.left_position_rad[index]; + velocity[static_cast(6 + index)] = + state.right_velocity_rad_s[index]; + velocity[static_cast(14 + index)] = + state.left_velocity_rad_s[index]; + } + + const auto& torque = pinocchio::rnea( + impl_->floating_model, + *impl_->floating_data, + q, + velocity, + acceleration); + if (!torque.allFinite() || torque.size() != 22) { + return false; + } + + for (std::size_t index = 0; index < kArmDof; ++index) { + right_gravity_nm[index] = + torque[static_cast(6 + index)]; + left_gravity_nm[index] = + torque[static_cast(14 + index)]; + } + return true; +} + +bool PinocchioUmeLegacyModelAdapter::projectHapticFeedback( + const BimanualModelState& state, + ArmSide side, + const RawHapticFeedback& feedback, + ProjectedHapticEffort& projected) const +{ + projected = {}; + if (!impl_ || + (side != ArmSide::Right && + side != ArmSide::Left) || + !finite(state.right_position_rad) || + !finite(state.left_position_rad) || + !finite(feedback.shoulder_moment) || + !finite(feedback.wrist_moment) || + !std::isfinite(feedback.elbow_effort) || + !std::isfinite(feedback.gripper_effort)) { + return false; + } + + auto& q = impl_->fixed_q; + q.setZero(); + for (std::size_t index = 0; index < kArmDof; ++index) { + q[static_cast(index)] = + state.right_position_rad[index]; + q[static_cast(8 + index)] = + state.left_position_rad[index]; + } + + pinocchio::framesForwardKinematics( + impl_->fixed_model, *impl_->fixed_data, q); + + const std::size_t frame_offset = + side == ArmSide::Right ? 0 : 2; + const Eigen::Index shoulder_column = + side == ArmSide::Right ? 0 : 8; + const Eigen::Index wrist_column = + side == ArmSide::Right ? 4 : 12; + + auto& shoulder_jacobian = impl_->shoulder_jacobian; + shoulder_jacobian.setZero(); + pinocchio::computeFrameJacobian( + impl_->fixed_model, + *impl_->fixed_data, + q, + impl_->fixed_frame_ids[frame_offset], + pinocchio::ReferenceFrame::LOCAL_WORLD_ALIGNED, + shoulder_jacobian); + + auto& wrist_jacobian = impl_->wrist_jacobian; + wrist_jacobian.setZero(); + pinocchio::computeFrameJacobian( + impl_->fixed_model, + *impl_->fixed_data, + q, + impl_->fixed_frame_ids[frame_offset + 1], + pinocchio::ReferenceFrame::LOCAL_WORLD_ALIGNED, + wrist_jacobian); + + if (!shoulder_jacobian.allFinite() || + !wrist_jacobian.allFinite()) { + return false; + } + + const Eigen::Vector3d shoulder_torque = + shoulder_jacobian + .block<3, 3>(3, shoulder_column) + .transpose() * + toEigen(feedback.shoulder_moment); + const Eigen::Vector3d wrist_torque = + wrist_jacobian + .block<3, 3>(3, wrist_column) + .transpose() * + toEigen(feedback.wrist_moment); + if (!shoulder_torque.allFinite() || + !wrist_torque.allFinite()) { + return false; + } + + projected.shoulder_joint_torque = + fromEigen(shoulder_torque); + projected.elbow_effort = feedback.elbow_effort; + projected.wrist_joint_torque = + fromEigen(wrist_torque); + projected.gripper_effort = feedback.gripper_effort; + return true; +} + +const UmeLegacyModelContract& +PinocchioUmeLegacyModelAdapter::contract() const noexcept +{ + return impl_->contract_info; +} + +const std::string& +PinocchioUmeLegacyModelAdapter::fixedModelPath() const noexcept +{ + return impl_->fixed_model_path; +} + +const std::string& +PinocchioUmeLegacyModelAdapter::floatingModelPath() const noexcept +{ + return impl_->floating_model_path; +} + +} // namespace cmvr::ume_legacy diff --git a/cmvr-es/algorithms/controllers/ume_legacy/src/ume_legacy_controller.cpp b/cmvr-es/algorithms/controllers/ume_legacy/src/ume_legacy_controller.cpp new file mode 100644 index 00000000..f3192328 --- /dev/null +++ b/cmvr-es/algorithms/controllers/ume_legacy/src/ume_legacy_controller.cpp @@ -0,0 +1,193 @@ +#include "ume_legacy_controller.h" + +#include + +namespace cmvr::ume_legacy { +namespace { + +constexpr double kPi = + 3.141592653589793238462643383279502884; + +double legacyClip(double value, double minimum, double maximum) noexcept +{ + // Explicit comparisons preserve NaN propagation: both comparisons are + // false and value is returned, matching np.clip for a NaN input. + if (value < minimum) { + return minimum; + } + if (value > maximum) { + return maximum; + } + return value; +} + +double norm(const Vector3& value) noexcept +{ + return std::sqrt(value[0] * value[0] + + value[1] * value[1] + + value[2] * value[2]); +} + +JointVector flattenInteraction( + ArmSide side, + const ProjectedHapticEffort& projected) noexcept +{ + JointVector interaction{ + projected.shoulder_joint_torque[0], + projected.shoulder_joint_torque[1], + projected.shoulder_joint_torque[2], + projected.elbow_effort, + projected.wrist_joint_torque[0], + projected.wrist_joint_torque[1], + projected.wrist_joint_torque[2], + projected.gripper_effort}; + + // Exact scalar sign conventions from the legacy controller: + // right elbow +, right gripper - + // left elbow -, left gripper + + if (side == ArmSide::Right) { + interaction[7] = -interaction[7]; + } else { + interaction[3] = -interaction[3]; + } + return interaction; +} + +} // namespace + +LegacyUmeTuning originalTuning() noexcept +{ + LegacyUmeTuning tuning; + tuning.friction_coefficient = + {1.6, 1.6, 1.6, 1.6, 0.032, 0.032, 0.032, 0.032}; + tuning.friction_max_compensation = + {0.4, 0.4, 0.4, 0.4, 0.1, 0.1, 0.1, 0.1}; + + const double one_degree = kPi / 180.0; + const double ten_degrees = 10.0 * one_degree; + tuning.stiction_threshold_min_rad_s = + {one_degree, one_degree, one_degree, one_degree, + one_degree, one_degree, one_degree, one_degree}; + tuning.stiction_threshold_max_rad_s = + {ten_degrees, ten_degrees, ten_degrees, ten_degrees, + ten_degrees, ten_degrees, ten_degrees, ten_degrees}; + tuning.stiction_compensation = + {0.5, 0.5, 0.5, 0.5, 0.0, 0.0, 0.0, 0.0}; + + tuning.feedback_error_tolerance_rad = one_degree; + tuning.feedback_tanh_sharpness = 10.0; + tuning.feedback_scale = 0.5; + tuning.feedback_limit_dm4340_nm = 4.0; + tuning.feedback_limit_dm4310_nm = 1.0; + return tuning; +} + +JointVector frictionCompensation( + const JointVector& velocity_rad_s, + const LegacyUmeTuning& tuning) noexcept +{ + JointVector result{}; + for (std::size_t index = 0; index < kArmDof; ++index) { + const double maximum = + tuning.friction_max_compensation[index]; + result[index] = legacyClip( + tuning.friction_coefficient[index] * + velocity_rad_s[index], + -maximum, + maximum); + } + return result; +} + +JointVector stictionCompensation( + const JointVector& velocity_rad_s, + const LegacyUmeTuning& tuning) noexcept +{ + JointVector result{}; + for (std::size_t index = 0; index < kArmDof; ++index) { + const double velocity = velocity_rad_s[index]; + const double speed = std::abs(velocity); + + // Both inequalities are intentionally strict, matching: + // min < abs(qvel) < max. + if (tuning.stiction_threshold_min_rad_s[index] < speed && + speed < tuning.stiction_threshold_max_rad_s[index]) { + if (velocity > 0.0) { + result[index] = + tuning.stiction_compensation[index]; + } else if (velocity < 0.0) { + result[index] = + -tuning.stiction_compensation[index]; + } + } + } + return result; +} + +double feedbackScale( + double error_norm, + const LegacyUmeTuning& tuning) noexcept +{ + return tuning.feedback_scale * + (std::tanh( + tuning.feedback_tanh_sharpness * + (std::abs(error_norm) - + tuning.feedback_error_tolerance_rad)) + + 1.0) / + 2.0; +} + +SideControlOutput computeSideCommand( + const SideControlInput& input, + const LegacyUmeTuning& tuning) noexcept +{ + SideControlOutput output; + output.friction_compensation_nm = + frictionCompensation(input.joint_velocity_rad_s, tuning); + output.stiction_compensation_nm = + stictionCompensation(input.joint_velocity_rad_s, tuning); + output.signed_interaction_nm = + flattenInteraction(input.side, input.projected_haptic); + + output.feedback_scales.shoulder = + feedbackScale(norm(input.tracking_error.shoulder_rotation), + tuning); + output.feedback_scales.elbow = + feedbackScale(std::abs(input.tracking_error.elbow), tuning); + output.feedback_scales.wrist = + feedbackScale(norm(input.tracking_error.wrist_rotation), + tuning); + output.feedback_scales.gripper = + feedbackScale(std::abs(input.tracking_error.gripper), tuning); + + for (std::size_t index = 0; index < kArmDof; ++index) { + output.feedforward_without_haptic_nm[index] = + input.gravity_compensation_nm[index] + + output.friction_compensation_nm[index] + + output.stiction_compensation_nm[index]; + + double scale = output.feedback_scales.gripper; + double limit = tuning.feedback_limit_dm4310_nm; + if (index < 3) { + scale = output.feedback_scales.shoulder; + limit = tuning.feedback_limit_dm4340_nm; + } else if (index == 3) { + scale = output.feedback_scales.elbow; + limit = tuning.feedback_limit_dm4340_nm; + } else if (index < 7) { + scale = output.feedback_scales.wrist; + } + + output.limited_feedback_nm[index] = legacyClip( + scale * output.signed_interaction_nm[index], + -limit, + limit); + output.command_torque_nm[index] = + output.feedforward_without_haptic_nm[index] - + output.limited_feedback_nm[index]; + } + + return output; +} + +} // namespace cmvr::ume_legacy diff --git a/cmvr-es/algorithms/controllers/ume_legacy/tests/pinocchio_ume_legacy_model_adapter_test.cpp b/cmvr-es/algorithms/controllers/ume_legacy/tests/pinocchio_ume_legacy_model_adapter_test.cpp new file mode 100644 index 00000000..568ff28e --- /dev/null +++ b/cmvr-es/algorithms/controllers/ume_legacy/tests/pinocchio_ume_legacy_model_adapter_test.cpp @@ -0,0 +1,392 @@ +#include "pinocchio_ume_legacy_model_adapter.h" + +#include +#include +#include +#include +#include +#include + +#include + +#ifndef CMVR_UME_FIXED_MODEL_PATH +#error "CMVR_UME_FIXED_MODEL_PATH must identify the deployed fixed UME MJCF" +#endif + +#ifndef CMVR_UME_FLOATING_MODEL_PATH +#error "CMVR_UME_FLOATING_MODEL_PATH must identify the deployed floating UME MJCF" +#endif + +namespace cmvr::ume_legacy { +namespace { + +constexpr double kNumericalTolerance = 1e-10; + +PinocchioUmeLegacyModelAdapter makeAdapter() +{ + return PinocchioUmeLegacyModelAdapter( + CMVR_UME_FIXED_MODEL_PATH, + CMVR_UME_FLOATING_MODEL_PATH); +} + +BimanualModelState makeGoldenState( + const PinocchioUmeLegacyModelAdapter& adapter) +{ + BimanualModelState state; + state.world_from_imu = + adapter.contract().base_from_imu; + state.right_position_rad = + {0.1, -0.2, 0.3, -0.4, + 0.2, -0.1, 0.15, -0.05}; + state.left_position_rad = + {-0.1, 0.2, -0.3, 0.4, + -0.2, 0.1, -0.15, 0.05}; + return state; +} + +template +void expectFinite(const std::array& values) +{ + for (std::size_t index = 0; index < Size; ++index) { + EXPECT_TRUE(std::isfinite(values[index])) + << "index " << index; + } +} + +template +void expectNear( + const std::array& actual, + const std::array& expected, + double tolerance = kNumericalTolerance) +{ + for (std::size_t index = 0; index < Size; ++index) { + EXPECT_NEAR(actual[index], expected[index], tolerance) + << "index " << index; + } +} + +Transform4x4RowMajor multiplyTransforms( + const Transform4x4RowMajor& left, + const Transform4x4RowMajor& right) +{ + Transform4x4RowMajor result{}; + for (std::size_t row = 0; row < 4; ++row) { + for (std::size_t column = 0; column < 4; ++column) { + for (std::size_t inner = 0; inner < 4; ++inner) { + result[row * 4 + column] += + left[row * 4 + inner] * + right[inner * 4 + column]; + } + } + } + return result; +} + +Transform4x4RowMajor makeNoncommutingWorldFromBase() +{ + constexpr double roll = 0.2; + constexpr double pitch = -0.35; + constexpr double yaw = 0.47; + const double sr = std::sin(roll); + const double cr = std::cos(roll); + const double sp = std::sin(pitch); + const double cp = std::cos(pitch); + const double sy = std::sin(yaw); + const double cy = std::cos(yaw); + return { + cy * cp, + cy * sp * sr - sy * cr, + cy * sp * cr + sy * sr, + 0.4, + sy * cp, + sy * sp * sr + cy * cr, + sy * sp * cr - cy * sr, + -0.1, + -sp, + cp * sr, + cp * cr, + 0.8, + 0.0, 0.0, 0.0, 1.0}; +} + +TEST(PinocchioUmeLegacyModelAdapterTest, + LoadsOriginalMjcfWithoutGeometryAssetsAndFreezesContract) +{ + const auto adapter = makeAdapter(); + const auto& contract = adapter.contract(); + + EXPECT_EQ(contract.fixed_nq, 16U); + EXPECT_EQ(contract.fixed_nv, 16U); + EXPECT_EQ(contract.floating_nq, 23U); + EXPECT_EQ(contract.floating_nv, 22U); + EXPECT_EQ(adapter.fixedModelPath(), CMVR_UME_FIXED_MODEL_PATH); + EXPECT_EQ( + adapter.floatingModelPath(), + CMVR_UME_FLOATING_MODEL_PATH); + + // This transform comes from the original floating model's imu site. + // Pinocchio buildModel parses it without loading STL geometry. + const Transform4x4RowMajor expected_base_from_imu{ + 0.0, 0.0, -1.0, -0.0298, + 0.0, 1.0, 0.0, 0.0, + 1.0, 0.0, 0.0, -0.229564, + 0.0, 0.0, 0.0, 1.0}; + expectNear( + contract.base_from_imu, + expected_base_from_imu, + 1e-5); +} + +TEST(PinocchioUmeLegacyModelAdapterTest, + FloatingBaseRneaProducesFiniteFrozenJointEfforts) +{ + const auto adapter = makeAdapter(); + const auto state = makeGoldenState(adapter); + JointVector right{}; + JointVector left{}; + + ASSERT_TRUE( + adapter.computeGravityCompensation( + state, right, left)); + expectFinite(right); + expectFinite(left); + expectNear( + right, + {4.2579946000243867, + -3.101883528283977, + 5.8349126697703291, + -3.2335040596068012, + 0.54313804667559806, + -0.28419763993223052, + 0.075420409617272505, + -0.0050792810993999194}); + expectNear( + left, + {-4.2570142852347947, + 3.1002195124462104, + -5.8319486672094438, + 3.2334454894750602, + -0.54274278459746039, + 0.28419800074629464, + -0.075420524366616282, + 0.0050792800399334561}); +} + +TEST(PinocchioUmeLegacyModelAdapterTest, + ImuDerivedBaseOrientationReversesGravityUnderHalfTurn) +{ + const auto adapter = makeAdapter(); + auto state = makeGoldenState(adapter); + JointVector upright_right{}; + JointVector upright_left{}; + ASSERT_TRUE(adapter.computeGravityCompensation( + state, upright_right, upright_left)); + + const Transform4x4RowMajor world_from_base_half_turn_x{ + 1.0, 0.0, 0.0, 0.0, + 0.0, -1.0, 0.0, 0.0, + 0.0, 0.0, -1.0, 0.0, + 0.0, 0.0, 0.0, 1.0}; + state.world_from_imu = multiplyTransforms( + world_from_base_half_turn_x, + adapter.contract().base_from_imu); + + JointVector inverted_right{}; + JointVector inverted_left{}; + ASSERT_TRUE(adapter.computeGravityCompensation( + state, inverted_right, inverted_left)); + for (std::size_t index = 0; index < kArmDof; ++index) { + EXPECT_NEAR( + inverted_right[index], + -upright_right[index], + kNumericalTolerance) + << "right joint index " << index; + EXPECT_NEAR( + inverted_left[index], + -upright_left[index], + kNumericalTolerance) + << "left joint index " << index; + } +} + +TEST(PinocchioUmeLegacyModelAdapterTest, + NoncommutingImuPoseAndAsymmetricVelocitiesMatchFrozenRnea) +{ + const auto adapter = makeAdapter(); + auto state = makeGoldenState(adapter); + state.world_from_imu = multiplyTransforms( + makeNoncommutingWorldFromBase(), + adapter.contract().base_from_imu); + state.right_velocity_rad_s = + {0.7, -0.4, 0.2, -0.1, + 1.1, -0.8, 0.5, -0.3}; + state.left_velocity_rad_s = + {-0.6, 0.9, -0.2, 0.4, + -1.0, 0.7, -0.5, 0.25}; + + JointVector right{}; + JointVector left{}; + ASSERT_TRUE(adapter.computeGravityCompensation( + state, right, left)); + expectFinite(right); + expectFinite(left); + expectNear( + right, + {2.080988418159027, + -1.3277143244666021, + 2.3194639604892364, + -2.0699331198781317, + 0.40716561983947641, + -0.20631486209184946, + 0.19335176308712126, + -0.01319194690287678}); + expectNear( + left, + {-5.6976332776660232, + 2.9915426497221254, + -2.5700109870406949, + 2.1379083447512071, + -0.36441212045948712, + 0.19226687457105307, + -0.085601264411386338, + 0.0065129700338426369}); +} + +TEST(PinocchioUmeLegacyModelAdapterTest, + FixedModelRotationalProjectionMatchesFrozenValues) +{ + const auto adapter = makeAdapter(); + const auto state = makeGoldenState(adapter); + RawHapticFeedback feedback; + feedback.shoulder_moment = {0.5, -0.2, 0.3}; + feedback.elbow_effort = 1.2; + feedback.wrist_moment = {-0.4, 0.1, 0.6}; + feedback.gripper_effort = -0.7; + + ProjectedHapticEffort right{}; + ASSERT_TRUE(adapter.projectHapticFeedback( + state, ArmSide::Right, feedback, right)); + expectFinite(right.shoulder_joint_torque); + expectFinite(right.wrist_joint_torque); + expectNear( + right.shoulder_joint_torque, + {-0.5, + -0.12674237993402607, + -0.30915027509149773}); + expectNear( + right.wrist_joint_torque, + {-0.065872923184419674, + -0.028130723042130143, + -0.72743566625468636}); + EXPECT_DOUBLE_EQ(right.elbow_effort, feedback.elbow_effort); + EXPECT_DOUBLE_EQ( + right.gripper_effort, + feedback.gripper_effort); + + ProjectedHapticEffort left{}; + ASSERT_TRUE(adapter.projectHapticFeedback( + state, ArmSide::Left, feedback, left)); + expectFinite(left.shoulder_joint_torque); + expectFinite(left.wrist_joint_torque); + expectNear( + left.shoulder_joint_torque, + {-0.5, + -0.36032671657345572, + 0.083245914753940151}); + expectNear( + left.wrist_joint_torque, + {-0.13497315246130309, + 0.15764759010789803, + -0.70779204949804098}); + EXPECT_DOUBLE_EQ(left.elbow_effort, feedback.elbow_effort); + EXPECT_DOUBLE_EQ( + left.gripper_effort, + feedback.gripper_effort); +} + +TEST(PinocchioUmeLegacyModelAdapterTest, + RejectsNonRigidOrNonFiniteInputsAndZerosOutputs) +{ + const auto adapter = makeAdapter(); + auto state = makeGoldenState(adapter); + JointVector right; + JointVector left; + right.fill(1.0); + left.fill(1.0); + + state.world_from_imu = {}; + EXPECT_FALSE(adapter.computeGravityCompensation( + state, right, left)); + expectNear(right, JointVector{}); + expectNear(left, JointVector{}); + + state = makeGoldenState(adapter); + state.right_position_rad[3] = + std::numeric_limits::quiet_NaN(); + right.fill(1.0); + left.fill(1.0); + EXPECT_FALSE(adapter.computeGravityCompensation( + state, right, left)); + expectNear(right, JointVector{}); + expectNear(left, JointVector{}); + + state = makeGoldenState(adapter); + RawHapticFeedback feedback; + feedback.shoulder_moment[1] = + std::numeric_limits::infinity(); + ProjectedHapticEffort projected; + projected.elbow_effort = 1.0; + EXPECT_FALSE(adapter.projectHapticFeedback( + state, ArmSide::Right, feedback, projected)); + expectNear( + projected.shoulder_joint_torque, + Vector3{}); + expectNear(projected.wrist_joint_torque, Vector3{}); + EXPECT_DOUBLE_EQ(projected.elbow_effort, 0.0); + EXPECT_DOUBLE_EQ(projected.gripper_effort, 0.0); + + feedback = {}; + projected.elbow_effort = 1.0; + EXPECT_FALSE(adapter.projectHapticFeedback( + state, + static_cast(99), + feedback, + projected)); + expectNear( + projected.shoulder_joint_torque, + Vector3{}); + expectNear(projected.wrist_joint_torque, Vector3{}); + EXPECT_DOUBLE_EQ(projected.elbow_effort, 0.0); + EXPECT_DOUBLE_EQ(projected.gripper_effort, 0.0); +} + +TEST(PinocchioUmeLegacyModelAdapterTest, + MissingModelFailsAtConstruction) +{ + EXPECT_THROW( + PinocchioUmeLegacyModelAdapter( + "/definitely/missing/ume_fixed.xml", + CMVR_UME_FLOATING_MODEL_PATH), + std::runtime_error); +} + +TEST(PinocchioUmeLegacyModelAdapterTest, + RejectsModelRoleSwapEvenThoughBothMjcfFilesParse) +{ + try { + PinocchioUmeLegacyModelAdapter adapter( + CMVR_UME_FLOATING_MODEL_PATH, + CMVR_UME_FIXED_MODEL_PATH); + (void)adapter; + FAIL() << "swapped fixed/floating models were accepted"; + } catch (const std::runtime_error& error) { + EXPECT_NE( + std::string(error.what()).find( + "UME fixed MJCF contract violation: " + "expected nq/nv/njoints 16/16/17"), + std::string::npos); + } +} + +} // namespace +} // namespace cmvr::ume_legacy diff --git a/cmvr-es/algorithms/controllers/ume_legacy/tests/ume_legacy_controller_golden_test.cpp b/cmvr-es/algorithms/controllers/ume_legacy/tests/ume_legacy_controller_golden_test.cpp new file mode 100644 index 00000000..b5610c20 --- /dev/null +++ b/cmvr-es/algorithms/controllers/ume_legacy/tests/ume_legacy_controller_golden_test.cpp @@ -0,0 +1,216 @@ +#include "ume_legacy_controller.h" +#include "ume_legacy_model_adapter.h" + +#include +#include +#include +#include + +#include + +namespace cmvr::ume_legacy { +namespace { + +constexpr double kTolerance = 1e-12; + +void expectJointVectorNear( + const JointVector& actual, + const JointVector& expected, + double tolerance = kTolerance) +{ + for (std::size_t index = 0; index < kArmDof; ++index) { + EXPECT_NEAR(actual[index], expected[index], tolerance) + << "joint index " << index; + } +} + +TEST(UmeLegacyControllerGoldenTest, + OriginalTuningAndFrictionMatchPythonOracle) +{ + const auto tuning = originalTuning(); + EXPECT_DOUBLE_EQ(tuning.friction_coefficient[0], 1.6); + EXPECT_DOUBLE_EQ(tuning.friction_coefficient[4], 0.032); + EXPECT_DOUBLE_EQ(tuning.feedback_limit_dm4340_nm, 4.0); + EXPECT_DOUBLE_EQ(tuning.feedback_limit_dm4310_nm, 1.0); + + const JointVector velocity{ + -1.0, -0.1, 0.0, 2.0 * std::acos(-1.0) / 180.0, + -10.0, -1.0, 1.0, 10.0}; + const JointVector expected{ + -0.4, -0.16000000000000003, 0.0, + 0.055850536063818547, + -0.1, -0.032, 0.032, 0.1}; + + expectJointVectorNear( + frictionCompensation(velocity, tuning), + expected); +} + +TEST(UmeLegacyControllerGoldenTest, + StictionUsesStrictLegacyThresholds) +{ + const auto tuning = originalTuning(); + const double minimum = + tuning.stiction_threshold_min_rad_s[0]; + const double maximum = + tuning.stiction_threshold_max_rad_s[0]; + const JointVector at_threshold{ + minimum, + -minimum, + maximum, + -maximum, + 0.0, 0.0, 0.0, 0.0}; + expectJointVectorNear( + stictionCompensation(at_threshold, tuning), + JointVector{}); + + const JointVector strictly_inside{ + 2.0 * minimum, + -2.0 * minimum, + std::nextafter( + minimum, std::numeric_limits::infinity()), + std::nextafter(maximum, 0.0), + 2.0 * minimum, + -2.0 * minimum, + std::nextafter( + minimum, std::numeric_limits::infinity()), + std::nextafter(maximum, 0.0)}; + expectJointVectorNear( + stictionCompensation(strictly_inside, tuning), + {0.5, -0.5, 0.5, 0.5, + 0.0, 0.0, 0.0, 0.0}); +} + +TEST(UmeLegacyControllerGoldenTest, + FeedbackScaleMatchesLegacyNormAndTanhGoldenValues) +{ + const auto tuning = originalTuning(); + EXPECT_NEAR( + feedbackScale(0.0, tuning), + 0.20680448412090907, + kTolerance); + EXPECT_DOUBLE_EQ( + feedbackScale(tuning.feedback_error_tolerance_rad, tuning), + 0.25); + EXPECT_NEAR( + feedbackScale(0.05, tuning), + 0.32861047013244982, + kTolerance); + EXPECT_NEAR( + feedbackScale(-0.2, tuning), + 0.48734517579834447, + kTolerance); +} + +TEST(UmeLegacyControllerGoldenTest, + CompleteRightSideCommandMatchesPythonGoldenVector) +{ + SideControlInput input; + input.side = ArmSide::Right; + input.joint_velocity_rad_s = { + -1.0, -0.1, 0.0, 2.0 * std::acos(-1.0) / 180.0, + -10.0, -1.0, 1.0, 10.0}; + input.gravity_compensation_nm = + {0.5, -0.5, 1.0, -1.0, + 0.25, -0.25, 0.75, -0.75}; + input.projected_haptic.shoulder_joint_torque = + {1.2, -3.0, 10.0}; + input.projected_haptic.elbow_effort = 2.0; + input.projected_haptic.wrist_joint_torque = + {0.5, -2.0, 5.0}; + input.projected_haptic.gripper_effort = 3.0; + input.tracking_error.shoulder_rotation = {0.0, 0.0, 0.0}; + input.tracking_error.elbow = + originalTuning().feedback_error_tolerance_rad; + input.tracking_error.wrist_rotation = {0.03, 0.04, 0.0}; + input.tracking_error.gripper = -0.2; + + const auto output = computeSideCommand(input); + + expectJointVectorNear( + output.friction_compensation_nm, + {-0.4, -0.16000000000000003, 0.0, + 0.055850536063818547, + -0.1, -0.032, 0.032, 0.1}); + expectJointVectorNear( + output.stiction_compensation_nm, + {0.0, -0.5, 0.0, 0.5, + 0.0, 0.0, 0.0, 0.0}); + expectJointVectorNear( + output.feedforward_without_haptic_nm, + {0.099999999999999978, -1.1600000000000001, + 1.0, -0.44414946393618149, + 0.14999999999999999, -0.28200000000000003, + 0.78200000000000003, -0.65000000000000002}); + expectJointVectorNear( + output.signed_interaction_nm, + {1.2, -3.0, 10.0, 2.0, + 0.5, -2.0, 5.0, -3.0}); + expectJointVectorNear( + output.limited_feedback_nm, + {0.24816538094509089, -0.62041345236272716, + 2.0680448412090908, 0.5, + 0.16430523506622491, -0.65722094026489963, + 1.0, -1.0}); + expectJointVectorNear( + output.command_torque_nm, + {-0.14816538094509091, -0.53958654763727298, + -1.0680448412090908, -0.94414946393618149, + -0.014305235066224914, 0.37522094026489961, + -0.21799999999999997, 0.34999999999999998}); +} + +TEST(UmeLegacyControllerGoldenTest, + LeftAndRightScalarSignsAndGroupLimitsArePreserved) +{ + SideControlInput input; + input.projected_haptic.shoulder_joint_torque = + {100.0, -100.0, 100.0}; + input.projected_haptic.elbow_effort = 100.0; + input.projected_haptic.wrist_joint_torque = + {100.0, -100.0, 100.0}; + input.projected_haptic.gripper_effort = 100.0; + input.tracking_error.shoulder_rotation = {10.0, 0.0, 0.0}; + input.tracking_error.elbow = 10.0; + input.tracking_error.wrist_rotation = {10.0, 0.0, 0.0}; + input.tracking_error.gripper = 10.0; + + input.side = ArmSide::Right; + const auto right = computeSideCommand(input); + expectJointVectorNear( + right.signed_interaction_nm, + {100.0, -100.0, 100.0, 100.0, + 100.0, -100.0, 100.0, -100.0}); + expectJointVectorNear( + right.command_torque_nm, + {-4.0, 4.0, -4.0, -4.0, + -1.0, 1.0, -1.0, 1.0}); + + input.side = ArmSide::Left; + const auto left = computeSideCommand(input); + expectJointVectorNear( + left.signed_interaction_nm, + {100.0, -100.0, 100.0, -100.0, + 100.0, -100.0, 100.0, 100.0}); + expectJointVectorNear( + left.command_torque_nm, + {-4.0, 4.0, -4.0, 4.0, + -1.0, 1.0, -1.0, -1.0}); +} + +TEST(UmeLegacyControllerGoldenTest, + FeedbackClipDoesNotClampOtherFeedforwardTerms) +{ + SideControlInput input; + input.gravity_compensation_nm = + {50.0, -50.0, 0.0, 0.0, 0.0, 0.0, 20.0, -20.0}; + const auto output = computeSideCommand(input); + + EXPECT_DOUBLE_EQ(output.command_torque_nm[0], 50.0); + EXPECT_DOUBLE_EQ(output.command_torque_nm[1], -50.0); + EXPECT_DOUBLE_EQ(output.command_torque_nm[6], 20.0); + EXPECT_DOUBLE_EQ(output.command_torque_nm[7], -20.0); +} + +} // namespace +} // namespace cmvr::ume_legacy diff --git a/cmvr-es/common/types/arm/arm_types.h b/cmvr-es/common/types/arm/arm_types.h index 567b503c..9d8c819b 100644 --- a/cmvr-es/common/types/arm/arm_types.h +++ b/cmvr-es/common/types/arm/arm_types.h @@ -103,6 +103,11 @@ struct JointGroupState { std::vector position; std::vector velocity; std::vector effort; + std::uint64_t sequence{0}; + std::int64_t sample_monotonic_ns{0}; + bool position_valid{false}; + bool velocity_valid{false}; + bool effort_valid{false}; bool validForModel(const RobotModel& model) const { @@ -165,6 +170,14 @@ struct ServoOptions { double gain{300.0}; }; +struct TorqueServoOptions { + // The UME legacy loop runs at 800 Hz by default. + double period{0.00125}; + // A producer must continuously refresh the latest torque command. A stale + // command latches a fault and disables the actuator chain. + std::uint32_t command_watchdog_ms{20}; +}; + enum class RobotMode { Unknown = 0, Disconnected, @@ -197,6 +210,14 @@ enum class ControlMode { Freedrive }; +enum class JointEffortSource { + Unspecified = 0, + MotorEstimate, + JointSensor, + ForceTorqueSensor, + Observer +}; + struct ArmState { double timestamp{0.0}; RobotMode robot_mode{RobotMode::Unknown}; diff --git a/cmvr-es/config/README.md b/cmvr-es/config/README.md index c1414757..9fb2f0bc 100644 --- a/cmvr-es/config/README.md +++ b/cmvr-es/config/README.md @@ -18,6 +18,8 @@ cmvr_es.pb.txt 入口文件： - [`cmvr_es.pb.txt`](cmvr_es.pb.txt) +- [`cmvr_es_ume.pb.txt`](cmvr_es_ume.pb.txt)：UME 主端样例 +- [`cmvr_es_robot.pb.txt`](cmvr_es_robot.pb.txt)：人形机械臂从端样例 - [`manager/device_manager.pb.txt`](manager/device_manager.pb.txt) - [`manager/task_manager.pb.txt`](manager/task_manager.pb.txt) @@ -29,10 +31,10 @@ cmvr_es.pb.txt /config/cmvr_es.pb.txt ``` -安装后的 `output/bin/cmvr_es` 因此会读取 `output/bin/config/cmvr_es.pb.txt`；直接运行 `build/cmvr_es` 则会查找 `build/config/cmvr_es.pb.txt`，不会自动跳到安装目录。传入显式根配置时： +安装后的 `output/bin/cmvr_es` 因此会读取 `output/bin/config/cmvr_es.pb.txt`；直接运行 `build/cmvr_es` 则会查找 `build/config/cmvr_es.pb.txt`，不会自动跳到安装目录。传入显式根配置时使用 `--config`： ```bash -./output/bin/cmvr_es /etc/cmvr-es/cmvr_es.pb.txt +./output/bin/cmvr_es --config /etc/cmvr-es/cmvr_es.pb.txt ``` 设备、任务和证书等相对配置路径均以根配置文件所在目录解析。模型等资源通过 `ConfigHelper::resolveResourceFile()` 在配置根及父目录中查找；生产部署仍建议使用明确绝对路径。 @@ -109,6 +111,48 @@ output/bin/protoc \ 该命令只验证 Proto Text 解析，不验证文件、设备、证书、网络和跨字段语义。最终仍需运行组件测试和进程烟雾测试。 +## 双边遥操部署样例 + +仓库提供两个相互独立的 CMVR-ES 配置入口： + +- UME 主端：[`cmvr_es_ume.pb.txt`](cmvr_es_ume.pb.txt)，只声明 + `ume_left` 和 `ume_right`。两条机械臂在 DeviceManager 层默认关闭， + [`devices/arm/ume_arms.pb.txt`](devices/arm/ume_arms.pb.txt) 内部的 + `hardware_enabled` 也默认关闭；两层开关必须经过标定与安全验收后分别启用。 + 出站 `ume_teleop` Task 默认关闭，样例不包含机器人地址或凭据。当前 Task + 只实现会话/重连/心跳和 latest-only 指令邮箱，尚无生产算法调用 + `submitSetpoint()`，返回 effort 也尚未接入本地触觉协调器。 +- 人形机械臂从端：[`cmvr_es_robot.pb.txt`](cmvr_es_robot.pb.txt)，通用 gRPC + server 可以启动，但 `ti5_motors` 和 `right_arm` 仍默认关闭。生产 + `ArmTeleop` 已实现真实 `RobotArm` 适配，但服务配置的 `enable` 显式关闭且 + 样例哈希故意留空。`MotorRobotArm.enable_teleop_group_servo` 目前只是预留字段； + 因为现有 `servoJ` 仍是逐关节顺序写，代码即使看到该字段为 true 也会拒绝能力。 + 必须先实现并验收原子或定时的组下发原语。因此启动 gRPC server 不等于允许遥操 + 执行，也不能绕过设备层硬件门。 + +在两台边缘设备各自的源码或安装目录运行： + +```bash +# UME 主端（源码配置） +./output/bin/cmvr_es \ + --config ./cmvr-es/config/cmvr_es_ume.pb.txt + +# 人形机械臂从端（源码配置） +./output/bin/cmvr_es \ + --config ./cmvr-es/config/cmvr_es_robot.pb.txt +``` + +如果使用安装后的配置副本，则相应命令为： + +```bash +./output/bin/cmvr_es --config ./output/bin/config/cmvr_es_ume.pb.txt +./output/bin/cmvr_es --config ./output/bin/config/cmvr_es_robot.pb.txt +``` + +上线前应把两套配置分别复制到两台机器的外部配置目录。主端需要填写从端地址、 +会话 manifest 和认证配置；从端需要换成现场机械臂设备配置，并在真实硬件测试后 +逐层开启。不要把生产 IP、token、私钥或设备标定值提交到仓库样例。 + ## 生产配置 `cmake --install` 会重建 `output/bin/config/`。生产配置应复制到 `/etc/cmvr-es/` 等外部目录并显式传入。 diff --git a/cmvr-es/config/cmvr_es_robot.pb.txt b/cmvr-es/config/cmvr_es_robot.pb.txt new file mode 100644 index 00000000..44f6f00f --- /dev/null +++ b/cmvr-es/config/cmvr_es_robot.pb.txt @@ -0,0 +1,11 @@ +# Follower robot-side CMVR-ES profile. +# +# The RobotArm ArmTeleop backend is implemented but explicitly disabled. The +# physical arm and service backend remain closed. Current MotorRobotArm +# sequential joint writes are rejected as a teleop group-servo capability until +# an atomic/timed group primitive and its safety timing gates are accepted. +cmvr_es { + logger_config_file: "logger/logger.pb.txt" + device_manager_config_file: "manager/device_manager_robot.pb.txt" + task_manager_config_file: "manager/task_manager_robot.pb.txt" +} diff --git a/cmvr-es/config/cmvr_es_ume.pb.txt b/cmvr-es/config/cmvr_es_ume.pb.txt new file mode 100644 index 00000000..e20ef540 --- /dev/null +++ b/cmvr-es/config/cmvr_es_ume.pb.txt @@ -0,0 +1,10 @@ +# UME leader-side CMVR-ES profile. +# +# All relative paths below are resolved from this file's directory. This +# checked-in profile contains no production endpoint, credentials or hardware +# enablement. +cmvr_es { + logger_config_file: "logger/logger.pb.txt" + device_manager_config_file: "manager/device_manager_ume.pb.txt" + task_manager_config_file: "manager/task_manager_ume.pb.txt" +} diff --git a/cmvr-es/config/devices/arm/arm.pb.txt b/cmvr-es/config/devices/arm/arm.pb.txt index 714c5c9b..c1570e7f 100644 --- a/cmvr-es/config/devices/arm/arm.pb.txt +++ b/cmvr-es/config/devices/arm/arm.pb.txt @@ -17,6 +17,10 @@ arm { buffer_size: 50 default_vel: 1.0 default_acc: 2.0 + # Reserved only: current MotorRobotArm servoJ writes joints sequentially, + # so code rejects the teleop group-servo capability even if this is true. + # A reviewed atomic/timed group primitive is required before changing it. + enable_teleop_group_servo: false } kinematics { diff --git a/cmvr-es/config/devices/arm/ume_arms.pb.txt b/cmvr-es/config/devices/arm/ume_arms.pb.txt new file mode 100644 index 00000000..e39f94f4 --- /dev/null +++ b/cmvr-es/config/devices/arm/ume_arms.pb.txt @@ -0,0 +1,72 @@ +# UME leader-arm device templates. They are deliberately disabled in +# manager/device_manager.pb.txt and hardware_enabled remains false here. +# +# Before real hardware use, independently verify interface bitrate +# (1 Mbit/s arbitration, 5 Mbit/s data, FD+BRS), motor/feedback IDs, +# direction, zero offsets, mechanical joint limits and safe torque limits. +# Each joint must also receive reviewed healthy_feedback_status and raw +# temperature thresholds. They are deliberately absent below, so changing +# hardware_enabled alone is insufficient to arm these placeholder profiles. +arm { + robot_arms { + id: "ume_right" + ume { + can { + dev_id: "can4" + channel_id: 4 + interface_name: "can4" + enable_fd: true + bitrate_switch: true + send_timeout_us: 100 + receive_timeout_us: 100 + receive_own_messages: false + enable_error_frames: true + } + control_frequency_hz: 800 + cycle_deadline_us: 1000 + feedback_watchdog_ms: 20 + shutdown_timeout_ms: 50 + hardware_enabled: false + + joints { joint_name: "RJ1" command_id: 1 feedback_id: 17 reported_motor_id: 1 model: DAMIAO_MOTOR_MODEL_DM4340 direction: 1 zero_offset_rad: 0 joint_lower_rad: -12.5 joint_upper_rad: 12.5 max_velocity_rad_s: 8 max_torque_nm: 4 } + joints { joint_name: "RJ2" command_id: 2 feedback_id: 18 reported_motor_id: 2 model: DAMIAO_MOTOR_MODEL_DM4340 direction: 1 zero_offset_rad: 0 joint_lower_rad: -12.5 joint_upper_rad: 12.5 max_velocity_rad_s: 8 max_torque_nm: 4 } + joints { joint_name: "RJ3" command_id: 3 feedback_id: 19 reported_motor_id: 3 model: DAMIAO_MOTOR_MODEL_DM4340 direction: 1 zero_offset_rad: 0 joint_lower_rad: -12.5 joint_upper_rad: 12.5 max_velocity_rad_s: 8 max_torque_nm: 4 } + joints { joint_name: "RJ4" command_id: 4 feedback_id: 20 reported_motor_id: 4 model: DAMIAO_MOTOR_MODEL_DM4340 direction: 1 zero_offset_rad: 0 joint_lower_rad: -12.5 joint_upper_rad: 12.5 max_velocity_rad_s: 8 max_torque_nm: 4 } + joints { joint_name: "RJ5" command_id: 5 feedback_id: 21 reported_motor_id: 5 model: DAMIAO_MOTOR_MODEL_DM4310 direction: 1 zero_offset_rad: 0 joint_lower_rad: -12.5 joint_upper_rad: 12.5 max_velocity_rad_s: 30 max_torque_nm: 1 } + joints { joint_name: "RJ6" command_id: 6 feedback_id: 22 reported_motor_id: 6 model: DAMIAO_MOTOR_MODEL_DM4310 direction: 1 zero_offset_rad: 0 joint_lower_rad: -12.5 joint_upper_rad: 12.5 max_velocity_rad_s: 30 max_torque_nm: 1 } + joints { joint_name: "RJ7" command_id: 7 feedback_id: 23 reported_motor_id: 7 model: DAMIAO_MOTOR_MODEL_DM4310 direction: 1 zero_offset_rad: 0 joint_lower_rad: -12.5 joint_upper_rad: 12.5 max_velocity_rad_s: 30 max_torque_nm: 1 } + joints { joint_name: "RJ8" command_id: 8 feedback_id: 24 reported_motor_id: 8 model: DAMIAO_MOTOR_MODEL_DM4310 direction: 1 zero_offset_rad: 0 joint_lower_rad: -12.5 joint_upper_rad: 12.5 max_velocity_rad_s: 30 max_torque_nm: 1 } + } + } + + robot_arms { + id: "ume_left" + ume { + can { + dev_id: "can5" + channel_id: 5 + interface_name: "can5" + enable_fd: true + bitrate_switch: true + send_timeout_us: 100 + receive_timeout_us: 100 + receive_own_messages: false + enable_error_frames: true + } + control_frequency_hz: 800 + cycle_deadline_us: 1000 + feedback_watchdog_ms: 20 + shutdown_timeout_ms: 50 + hardware_enabled: false + + joints { joint_name: "LJ1" command_id: 1 feedback_id: 17 reported_motor_id: 1 model: DAMIAO_MOTOR_MODEL_DM4340 direction: 1 zero_offset_rad: 0 joint_lower_rad: -12.5 joint_upper_rad: 12.5 max_velocity_rad_s: 8 max_torque_nm: 4 } + joints { joint_name: "LJ2" command_id: 2 feedback_id: 18 reported_motor_id: 2 model: DAMIAO_MOTOR_MODEL_DM4340 direction: 1 zero_offset_rad: 0 joint_lower_rad: -12.5 joint_upper_rad: 12.5 max_velocity_rad_s: 8 max_torque_nm: 4 } + joints { joint_name: "LJ3" command_id: 3 feedback_id: 19 reported_motor_id: 3 model: DAMIAO_MOTOR_MODEL_DM4340 direction: 1 zero_offset_rad: 0 joint_lower_rad: -12.5 joint_upper_rad: 12.5 max_velocity_rad_s: 8 max_torque_nm: 4 } + joints { joint_name: "LJ4" command_id: 4 feedback_id: 20 reported_motor_id: 4 model: DAMIAO_MOTOR_MODEL_DM4340 direction: 1 zero_offset_rad: 0 joint_lower_rad: -12.5 joint_upper_rad: 12.5 max_velocity_rad_s: 8 max_torque_nm: 4 } + joints { joint_name: "LJ5" command_id: 5 feedback_id: 21 reported_motor_id: 5 model: DAMIAO_MOTOR_MODEL_DM4310 direction: 1 zero_offset_rad: 0 joint_lower_rad: -12.5 joint_upper_rad: 12.5 max_velocity_rad_s: 30 max_torque_nm: 1 } + joints { joint_name: "LJ6" command_id: 6 feedback_id: 22 reported_motor_id: 6 model: DAMIAO_MOTOR_MODEL_DM4310 direction: 1 zero_offset_rad: 0 joint_lower_rad: -12.5 joint_upper_rad: 12.5 max_velocity_rad_s: 30 max_torque_nm: 1 } + joints { joint_name: "LJ7" command_id: 7 feedback_id: 23 reported_motor_id: 7 model: DAMIAO_MOTOR_MODEL_DM4310 direction: 1 zero_offset_rad: 0 joint_lower_rad: -12.5 joint_upper_rad: 12.5 max_velocity_rad_s: 30 max_torque_nm: 1 } + joints { joint_name: "LJ8" command_id: 8 feedback_id: 24 reported_motor_id: 8 model: DAMIAO_MOTOR_MODEL_DM4310 direction: 1 zero_offset_rad: 0 joint_lower_rad: -12.5 joint_upper_rad: 12.5 max_velocity_rad_s: 30 max_torque_nm: 1 } + } + } +} diff --git a/cmvr-es/config/manager/device_manager.pb.txt b/cmvr-es/config/manager/device_manager.pb.txt index 5b85ffe4..de5de1da 100644 --- a/cmvr-es/config/manager/device_manager.pb.txt +++ b/cmvr-es/config/manager/device_manager.pb.txt @@ -111,6 +111,23 @@ device_manager { enable: false } + devices { + id: "ume_right" + type: DEVICE_TYPE_ROBOT_ARM + config_file: "devices/arm/ume_arms.pb.txt" + # Two gates must be explicitly changed after the physical safety review: + # this entry and ume.hardware_enabled in the arm config. + enable: false + } + + devices { + id: "ume_left" + type: DEVICE_TYPE_ROBOT_ARM + config_file: "devices/arm/ume_arms.pb.txt" + # Two gates must be explicitly changed after the physical safety review. + enable: false + } + devices { id: "bio_head" type: DEVICE_TYPE_BIO_HEAD_ROBOT diff --git a/cmvr-es/config/manager/device_manager_robot.pb.txt b/cmvr-es/config/manager/device_manager_robot.pb.txt new file mode 100644 index 00000000..efc4a383 --- /dev/null +++ b/cmvr-es/config/manager/device_manager_robot.pb.txt @@ -0,0 +1,24 @@ +# Follower robot-side devices. +device_manager { + name: "cmvr_es_robot" + version: "0.1" + description: "CMVR humanoid follower edge system" + init_all_motors_when_no_active_joints: false + + devices { + id: "ti5_motors" + type: DEVICE_TYPE_MOTOR_SYSTEM + config_file: "devices/motor/ti5_motors.pb.txt" + # Physical motor communication remains fail-closed in this example. + enable: false + } + + devices { + id: "right_arm" + type: DEVICE_TYPE_ROBOT_ARM + config_file: "devices/arm/arm.pb.txt" + # Do not enable until the motor system, URDF, limits, servoJ timing and + # independent emergency-stop path have passed the robot safety checkout. + enable: false + } +} diff --git a/cmvr-es/config/manager/device_manager_ume.pb.txt b/cmvr-es/config/manager/device_manager_ume.pb.txt new file mode 100644 index 00000000..c908e447 --- /dev/null +++ b/cmvr-es/config/manager/device_manager_ume.pb.txt @@ -0,0 +1,24 @@ +# UME leader-side devices only. +device_manager { + name: "cmvr_es_ume" + version: "0.1" + description: "UME leader edge system" + init_all_motors_when_no_active_joints: false + + devices { + id: "ume_right" + type: DEVICE_TYPE_ROBOT_ARM + config_file: "devices/arm/ume_arms.pb.txt" + # Hardware gate 1/2. Gate 2/2 is ume.hardware_enabled in the arm config. + # Keep both false until CAN mapping, limits and physical safety are verified. + enable: false + } + + devices { + id: "ume_left" + type: DEVICE_TYPE_ROBOT_ARM + config_file: "devices/arm/ume_arms.pb.txt" + # Hardware gate 1/2. Gate 2/2 is ume.hardware_enabled in the arm config. + enable: false + } +} diff --git a/cmvr-es/config/manager/task_manager.pb.txt b/cmvr-es/config/manager/task_manager.pb.txt index d6569ea6..4d5df7a7 100644 --- a/cmvr-es/config/manager/task_manager.pb.txt +++ b/cmvr-es/config/manager/task_manager.pb.txt @@ -30,4 +30,13 @@ task_manager { # Host-development default: no QUIC Gateway or physical media devices. enable: true } + tasks { + id: "ume_teleop" + type: TASK_TYPE_UME_TELEOP + run_mode: TASK_RUN_MODE_BLOCKING_SERVICE + config_file: "tasks/ume_teleop_task/ume_teleop_task.pb.txt" + # Fail-safe default: configure the remote robot endpoint, manifest and + # deployment security policy before enabling this outbound control task. + enable: false + } } diff --git a/cmvr-es/config/manager/task_manager_robot.pb.txt b/cmvr-es/config/manager/task_manager_robot.pb.txt new file mode 100644 index 00000000..dc53850a --- /dev/null +++ b/cmvr-es/config/manager/task_manager_robot.pb.txt @@ -0,0 +1,14 @@ +# Follower robot-side tasks. +task_manager { + tasks { + id: "grpc_server" + type: TASK_TYPE_GRPC_SERVER + run_mode: TASK_RUN_MODE_BLOCKING_SERVICE + config_file: "tasks/grpc_server_task/grpc_server_task.pb.txt" + # The generic gRPC server may be enabled for integration. This does not + # enable a physical arm: device entries and the implemented RobotArm + # ArmTeleop adapter are explicitly disabled. Current MotorRobotArm + # sequential joint dispatch also fails the group-servo capability gate. + enable: true + } +} diff --git a/cmvr-es/config/manager/task_manager_ume.pb.txt b/cmvr-es/config/manager/task_manager_ume.pb.txt new file mode 100644 index 00000000..0793bb7d --- /dev/null +++ b/cmvr-es/config/manager/task_manager_ume.pb.txt @@ -0,0 +1,12 @@ +# UME leader-side tasks. +task_manager { + tasks { + id: "ume_teleop" + type: TASK_TYPE_UME_TELEOP + run_mode: TASK_RUN_MODE_BLOCKING_SERVICE + config_file: "tasks/ume_teleop_task/ume_teleop_task.pb.txt" + # Fail-closed: configure the follower endpoint, expected manifest and + # transport security before enabling outbound teleoperation. + enable: false + } +} diff --git a/cmvr-es/config/tasks/grpc_server_task/grpc_server_task.pb.txt b/cmvr-es/config/tasks/grpc_server_task/grpc_server_task.pb.txt index 9001afb0..e351f03c 100644 --- a/cmvr-es/config/tasks/grpc_server_task/grpc_server_task.pb.txt +++ b/cmvr-es/config/tasks/grpc_server_task/grpc_server_task.pb.txt @@ -5,4 +5,22 @@ grpc_server { enable_reflection: true camera_stream_max_pending_frames: 2 camera_stream_max_frame_age_ms: 250 + + # The RobotArm adapter is implemented, but remains explicitly closed until + # the device itself enables teleop group servo, real hashes are provisioned, + # and group-write timing and independent stop behavior pass hardware review. + arm_teleop_backend { + enable: false + device_id: "right_arm" + # Deliberately empty placeholders are invalid when enable=true. + model_sha256: "" + calibration_sha256: "" + base_frame: "PELVIS_S" + tool_frame: "R_FINGER_TIP_FIXED" + servo_period_s: 0.001 + max_apply_duration_us: 800 + require_powered: true + max_initial_position_step_rad: 0.02 + max_position_step_rad: 0.003 + } } diff --git a/cmvr-es/config/tasks/ume_teleop_task/ume_teleop_task.pb.txt b/cmvr-es/config/tasks/ume_teleop_task/ume_teleop_task.pb.txt new file mode 100644 index 00000000..00ac7137 --- /dev/null +++ b/cmvr-es/config/tasks/ume_teleop_task/ume_teleop_task.pb.txt @@ -0,0 +1,35 @@ +ume_teleop { + id: "ume_teleop" + + # Deliberately left empty. The TaskManager entry is disabled by default, and + # init fails closed if it is enabled before a robot endpoint is configured. + server_address: "" + + # M6 implements explicit insecure transport for isolated development only. + # Production deployment must add and configure channel credentials first. + allow_insecure: false + + open_session { + protocol_major: 1 + protocol_minor: 0 + client_instance_id: "ume-controller" + requested_command_rate_hz: 250 + requested_state_rate_hz: 250 + watchdog_timeout_ms: 100 + requested_lease_ms: 500 + + # Replace with the manifest exported by the CMVR-ES robot instance. + expected_robot { + robot_id: "" + position_unit: "rad" + velocity_unit: "rad/s" + effort_unit: "N*m" + } + } + + reconnect { + initial_delay_ms: 100 + maximum_delay_ms: 5000 + multiplier: 2.0 + } +} diff --git a/cmvr-es/devices/arm/CMakeLists.txt b/cmvr-es/devices/arm/CMakeLists.txt index d2780c71..74f23c89 100644 --- a/cmvr-es/devices/arm/CMakeLists.txt +++ b/cmvr-es/devices/arm/CMakeLists.txt @@ -1,6 +1,7 @@ add_subdirectory(motor_robot_arm) add_subdirectory(aubo_arm) add_subdirectory(huayan_arm) +add_subdirectory(ume_robot_arm) add_library(robot_arm INTERFACE) @@ -11,6 +12,7 @@ target_link_libraries(robot_arm cmvr_es::device::motor_robot_arm cmvr_es::device::aubo_arm cmvr_es::device::huayan_arm + cmvr_es::device::ume_robot_arm cmvr_es::proto ) diff --git a/cmvr-es/devices/arm/motor_robot_arm/include/motor_robot_arm.h b/cmvr-es/devices/arm/motor_robot_arm/include/motor_robot_arm.h index 35471e29..5b1d0b9d 100644 --- a/cmvr-es/devices/arm/motor_robot_arm/include/motor_robot_arm.h +++ b/cmvr-es/devices/arm/motor_robot_arm/include/motor_robot_arm.h @@ -36,6 +36,13 @@ public: RobotMode getRobotMode() const override { return RobotMode::Idle; } SafetyMode getSafetyMode() const override; ControlMode getControlMode() const override { return ControlMode::Position; } + bool supportsTeleopGroupServo() const noexcept override + { + // commandCyclicPosition is currently dispatched one joint at a time. + // A config switch cannot turn that partial-write behavior into the + // atomic/timed group primitive required by network teleoperation. + return false; + } Result torqueOn() override; Result torqueOff() override; @@ -125,6 +132,8 @@ private: mutable std::mutex mutex_; std::atomic busy_{false}; + std::atomic powered_on_{false}; + mutable std::atomic joint_state_sequence_{0}; double speed_scaling_{1.0}; bool emergency_stopped_{false}; ServoOptions servo_options_; diff --git a/cmvr-es/devices/arm/motor_robot_arm/src/motor_robot_arm.cpp b/cmvr-es/devices/arm/motor_robot_arm/src/motor_robot_arm.cpp index 5a5db3a3..cce28a7d 100644 --- a/cmvr-es/devices/arm/motor_robot_arm/src/motor_robot_arm.cpp +++ b/cmvr-es/devices/arm/motor_robot_arm/src/motor_robot_arm.cpp @@ -1,6 +1,7 @@ #include "arm/motor_robot_arm/include/motor_robot_arm.h" #include +#include #include #include #include @@ -28,6 +29,23 @@ struct BusyGuard { ~BusyGuard() { busy.store(false); } }; +const config::JointLimitsConfig* configuredJointLimits( + const config::ArmKinematicsConfig& kinematics) +{ + switch (kinematics.algorithm_case()) { + case config::ArmKinematicsConfig::kPinocchioDlsIkSolver: + return &kinematics.pinocchio_dls_ik_solver() + .joint_limit_policy() + .limits(); + case config::ArmKinematicsConfig::kPinocchioQpIkSolver: + return &kinematics.pinocchio_qp_ik_solver() + .joint_limit_policy() + .limits(); + default: + return nullptr; + } +} + } // namespace MotorRobotArm::MotorRobotArm(const config::RobotArmConfig& cfg) @@ -66,6 +84,39 @@ MotorRobotArm::MotorRobotArm(const config::RobotArmConfig& cfg) model_.manufacturer = "cmvr"; model_.dof = static_cast(dof_); model_.joint_names = joint_names_; + + const auto* configured_limits = configuredJointLimits(cfg_.kinematics()); + if (configured_limits != nullptr && configured_limits->enable() && + configured_limits->source() == + config::JOINT_LIMIT_SOURCE_CUSTOM && + configured_limits->joints_size() == dof_) { + bool valid_limits = true; + model_.joint_limits.reserve(static_cast(dof_)); + for (int index = 0; index < dof_; ++index) { + const auto& source = configured_limits->joints(index); + if (source.joint_name() != joint_names_[static_cast(index)] || + !std::isfinite(source.q_lb()) || + !std::isfinite(source.q_ub()) || + !std::isfinite(source.qd()) || + source.q_lb() >= source.q_ub() || + source.qd() <= 0.0) { + valid_limits = false; + break; + } + JointLimit limit; + limit.lower = source.q_lb(); + limit.upper = source.q_ub(); + limit.max_velocity = source.qd(); + limit.max_acceleration = source.qdd(); + model_.joint_limits.push_back(limit); + } + if (!valid_limits) { + model_.joint_limits.clear(); + CMVR_LOG(ERROR) + << "[MotorRobotArm] invalid or misordered custom joint limits: " + << id_; + } + } } MotorRobotArm::~MotorRobotArm() @@ -134,7 +185,7 @@ ArmState MotorRobotArm::getRobotState() const { ArmState state; state.connected = motor_manager_ != nullptr; - state.powered_on = true; + state.powered_on = powered_on_.load(std::memory_order_acquire); state.brake_released = !emergency_stopped_; state.moving = busy(); state.emergency_stopped = emergency_stopped_; @@ -154,15 +205,35 @@ JointGroupState MotorRobotArm::getJointState() const state.position.reserve(joint_names_.size()); state.velocity.reserve(joint_names_.size()); state.effort.reserve(joint_names_.size()); + bool values_valid = true; for (const auto& joint_name : joint_names_) { auto motor = getMotor_(joint_name); if (!motor) { + values_valid = false; continue; } - state.position.push_back(motor->getQ()); - state.velocity.push_back(motor->getQd()); + const double position = motor->getQ(); + const double velocity = motor->getQd(); + values_valid = + values_valid && std::isfinite(position) && std::isfinite(velocity); + state.position.push_back(position); + state.velocity.push_back(velocity); state.effort.push_back(0.0); } + values_valid = + values_valid && state.position.size() == joint_names_.size() && + state.velocity.size() == joint_names_.size(); + state.sequence = + joint_state_sequence_.fetch_add(1, std::memory_order_relaxed) + 1; + state.sample_monotonic_ns = + std::chrono::duration_cast( + std::chrono::steady_clock::now().time_since_epoch()) + .count(); + state.position_valid = values_valid; + state.velocity_valid = values_valid; + // MotorRobotArm currently has no verified effort feedback path. The zero + // placeholders above must never be advertised as measured torque. + state.effort_valid = false; return state; } @@ -202,11 +273,15 @@ Result MotorRobotArm::torqueOn() } } emergency_stopped_ = false; + powered_on_.store(true, std::memory_order_release); return Result::success(); } Result MotorRobotArm::torqueOff() { + // Until every joint reports a successful disable, the aggregate powered + // state is unknown and therefore must not satisfy a require_powered gate. + powered_on_.store(false, std::memory_order_release); for (const auto& joint_name : joint_names_) { auto motor = getMotor_(joint_name); if (!motor) { diff --git a/cmvr-es/devices/arm/robot_arm.h b/cmvr-es/devices/arm/robot_arm.h index 6ab24556..071f3ce9 100644 --- a/cmvr-es/devices/arm/robot_arm.h +++ b/cmvr-es/devices/arm/robot_arm.h @@ -28,6 +28,15 @@ public: virtual SafetyMode getSafetyMode() const = 0; virtual ControlMode getControlMode() const = 0; + // ArmTeleop requires an explicitly reviewed group-servo implementation. + // Existing and vendor arms remain unavailable until their implementations + // override this capability after timing and partial-write validation. + virtual bool supportsTeleopGroupServo() const noexcept { return false; } + virtual JointEffortSource jointEffortSource() const noexcept + { + return JointEffortSource::Unspecified; + } + virtual Result torqueOn() = 0; virtual Result torqueOff() = 0; virtual Result calibrateZeroQ(const std::string& joint_name) = 0; @@ -73,6 +82,27 @@ public: FrameType frame = FrameType::Base) = 0; virtual Result stopServoMode() = 0; + // Torque streaming is optional. Backends which do not provide an atomic + // group torque port retain source compatibility and fail explicitly. + virtual Result startTorqueMode(const TorqueServoOptions&) + { + return Result::failure( + ArmErrorCode::UnsupportedCommand, + "torque servo mode is unsupported by this RobotArm"); + } + virtual Result servoTorque(const JointTorqueCommand&) + { + return Result::failure( + ArmErrorCode::UnsupportedCommand, + "torque servo command is unsupported by this RobotArm"); + } + virtual Result stopTorqueMode() + { + return Result::failure( + ArmErrorCode::UnsupportedCommand, + "torque servo mode is unsupported by this RobotArm"); + } + virtual Result connect(const std::string& ip, int port) = 0; virtual Result disconnect() = 0; virtual bool isConnected() const = 0; diff --git a/cmvr-es/devices/arm/robot_arm_factory.h b/cmvr-es/devices/arm/robot_arm_factory.h index 0a23742c..e86598e2 100644 --- a/cmvr-es/devices/arm/robot_arm_factory.h +++ b/cmvr-es/devices/arm/robot_arm_factory.h @@ -9,6 +9,7 @@ #include "devices/arm/aubo_arm/aubo_arm.h" #include "devices/arm/huayan_arm/huayan_arm.h" #include "devices/arm/motor_robot_arm/include/motor_robot_arm.h" +#include "devices/arm/ume_robot_arm/include/ume_robot_arm.h" namespace cmvr::device { @@ -36,6 +37,9 @@ public: return nullptr; } + case config::RobotArmConfig::kUme: + return std::make_shared(cfg); + case config::RobotArmConfig::BACKEND_NOT_SET: default: { diff --git a/cmvr-es/devices/arm/ume_robot_arm/CMakeLists.txt b/cmvr-es/devices/arm/ume_robot_arm/CMakeLists.txt new file mode 100644 index 00000000..f92b6ecd --- /dev/null +++ b/cmvr-es/devices/arm/ume_robot_arm/CMakeLists.txt @@ -0,0 +1,92 @@ +add_library(ume_robot_arm SHARED + src/damiao_mit_codec.cpp + src/damiao_can_fd_chain.cpp + src/ume_robot_arm.cpp +) + +target_include_directories(ume_robot_arm PUBLIC + ${CMAKE_CURRENT_SOURCE_DIR} +) + +target_link_libraries(ume_robot_arm + PUBLIC + cmvr_es::device::canbus + cmvr_es::ik_solver + cmvr_es::common + PRIVATE + cmvr_es::proto + cmvr_es::logging + pthread +) + +add_library(cmvr_es::device::ume_robot_arm ALIAS ume_robot_arm) +install(TARGETS ume_robot_arm LIBRARY DESTINATION lib) + +if(BUILD_TESTING) + add_executable(damiao_mit_codec_test + tests/damiao_mit_codec_test.cpp + ) + target_link_libraries(damiao_mit_codec_test + PRIVATE + cmvr_es::device::ume_robot_arm + gtest + gtest_main + pthread + ) + add_test( + NAME damiao_mit_codec_test + COMMAND damiao_mit_codec_test + ) + set(_ume_robot_arm_test_environment + "LD_LIBRARY_PATH=${CMVR_TEST_EXTERNAL_LIBRARY_PATH}" + ) + if(CMVR_TEST_SYSTEM_LIBSTDCXX) + list(APPEND _ume_robot_arm_test_environment + "LD_PRELOAD=${CMVR_TEST_SYSTEM_LIBSTDCXX}") + endif() + set_tests_properties(damiao_mit_codec_test PROPERTIES + TIMEOUT 10 + ENVIRONMENT "${_ume_robot_arm_test_environment}" + ) + + add_executable(damiao_can_fd_chain_test + tests/damiao_can_fd_chain_test.cpp + ) + target_link_libraries(damiao_can_fd_chain_test + PRIVATE + cmvr_es::device::ume_robot_arm + gtest + gtest_main + pthread + ) + add_test( + NAME damiao_can_fd_chain_test + COMMAND damiao_can_fd_chain_test + ) + set_tests_properties(damiao_can_fd_chain_test PROPERTIES + TIMEOUT 10 + ENVIRONMENT "${_ume_robot_arm_test_environment}" + ) + + add_executable(ume_robot_arm_test + tests/ume_robot_arm_test.cpp + ) + target_link_libraries(ume_robot_arm_test + PRIVATE + cmvr_es::device::ume_robot_arm + gtest + gtest_main + pthread + ) + target_compile_definitions(ume_robot_arm_test PRIVATE + CMVR_UME_ARM_CONFIG_PATH="${PROJECT_SOURCE_DIR}/cmvr-es/config/devices/arm/ume_arms.pb.txt" + ) + add_test( + NAME ume_robot_arm_test + COMMAND ume_robot_arm_test + ) + set_tests_properties(ume_robot_arm_test PROPERTIES + TIMEOUT 10 + ENVIRONMENT "${_ume_robot_arm_test_environment}" + ) +endif() diff --git a/cmvr-es/devices/arm/ume_robot_arm/include/damiao_can_fd_chain.h b/cmvr-es/devices/arm/ume_robot_arm/include/damiao_can_fd_chain.h new file mode 100644 index 00000000..32bfd0e7 --- /dev/null +++ b/cmvr-es/devices/arm/ume_robot_arm/include/damiao_can_fd_chain.h @@ -0,0 +1,145 @@ +#ifndef CMVR_ES_DAMIAO_CAN_FD_CHAIN_H +#define CMVR_ES_DAMIAO_CAN_FD_CHAIN_H + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "arm/ume_robot_arm/include/damiao_mit_codec.h" +#include "common/types/arm/arm_types.h" + +namespace cmvr::device { + +class AbstractCanbus; + +struct DamiaoJointSpec { + std::string joint_name; + std::uint32_t command_id{0}; + std::uint32_t feedback_id{0}; + std::uint8_t reported_motor_id{0}; + DamiaoMotorModel model{DamiaoMotorModel::Unknown}; + int direction{1}; + double zero_offset_rad{0.0}; + double joint_lower_rad{0.0}; + double joint_upper_rad{0.0}; + double max_velocity_rad_s{0.0}; + double max_torque_nm{0.0}; + std::uint16_t healthy_status_mask{0}; + std::uint8_t max_driver_temperature_raw{0}; + std::uint8_t max_motor_temperature_raw{0}; +}; + +struct DamiaoChainOptions { + bool is_fd{true}; + bool bitrate_switch{true}; + bool hardware_enabled{false}; +}; + +struct DamiaoChainStatistics { + std::uint64_t exchanges{0}; + std::uint64_t deadline_misses{0}; + std::uint64_t unknown_feedback{0}; + std::uint64_t duplicate_feedback{0}; + std::uint64_t rejected_commands{0}; + std::uint64_t protocol_saturations{0}; +}; + +enum class DamiaoChainState : std::uint8_t { + Closed = 0, + Initialized, + Passive, + Armed, + Active, + FaultLatched, + Stopped +}; + +class DamiaoCanFdChain { +public: + DamiaoCanFdChain(std::shared_ptr bus, + std::vector joints, + DamiaoChainOptions options); + ~DamiaoCanFdChain(); + + DamiaoCanFdChain(const DamiaoCanFdChain&) = delete; + DamiaoCanFdChain& operator=(const DamiaoCanFdChain&) = delete; + + Result init(); + Result openPassive(); + Result clearFault(std::chrono::steady_clock::time_point deadline); + Result arm(std::chrono::steady_clock::time_point deadline); + Result setZero(std::size_t joint_index, + std::chrono::steady_clock::time_point deadline); + Result exchange(const DamiaoMitCommand* joint_commands, + std::size_t command_count, + DamiaoJointFeedback* joint_feedback, + std::size_t feedback_count, + std::chrono::steady_clock::time_point deadline); + Result disable() noexcept; + Result latchFault(const std::string& reason) noexcept; + void stop() noexcept; + + DamiaoChainState state() const noexcept { return state_.load(); } + std::size_t size() const noexcept { return joints_.size(); } + bool hardwareEnabled() const noexcept { return options_.hardware_enabled; } + DamiaoChainStatistics statistics() const; + std::string lastError() const; + const std::vector& joints() const noexcept { return joints_; } + +private: + Result validateConfig_() const; + Result sendModeAll_( + DamiaoMode mode, + std::chrono::steady_clock::time_point deadline, + bool expect_feedback); + Result sendModeOne_( + std::size_t joint_index, + DamiaoMode mode, + std::chrono::steady_clock::time_point deadline); + Result receiveCycle_( + DamiaoJointFeedback* feedback, + std::size_t feedback_count, + std::chrono::steady_clock::time_point deadline); + bool sendFrames_( + const std::vector& frames, + std::chrono::steady_clock::time_point deadline) noexcept; + bool sendFramesBestEffort_( + const std::vector& frames) noexcept; + bool feedbackTransportAndHealthValid_( + const CanFrame& frame, + const DamiaoJointSpec& joint, + const DamiaoJointFeedback& feedback) const noexcept; + bool latchFaultAndDisable_(const std::string& reason) noexcept; + bool bestEffortZeroAndDisable_() noexcept; + void setError_(const std::string& error) noexcept; + std::size_t jointIndexForFeedbackId_(std::uint32_t id) const noexcept; + DamiaoMitCommand toMotorCommand_( + const DamiaoJointSpec& spec, + const DamiaoMitCommand& command, + bool& safety_saturated) const noexcept; + void toJointFeedback_(const DamiaoJointSpec& spec, + DamiaoJointFeedback& feedback) const noexcept; + + std::shared_ptr bus_; + std::vector joints_; + DamiaoChainOptions options_; + std::vector tx_frames_; + std::vector rx_frames_; + std::vector feedback_scratch_; + std::vector feedback_seen_; + + mutable std::mutex io_mutex_; + mutable std::mutex status_mutex_; + std::atomic state_{DamiaoChainState::Closed}; + DamiaoChainStatistics statistics_; + std::string last_error_; +}; + +} // namespace cmvr::device + +#endif // CMVR_ES_DAMIAO_CAN_FD_CHAIN_H diff --git a/cmvr-es/devices/arm/ume_robot_arm/include/damiao_mit_codec.h b/cmvr-es/devices/arm/ume_robot_arm/include/damiao_mit_codec.h new file mode 100644 index 00000000..447473c7 --- /dev/null +++ b/cmvr-es/devices/arm/ume_robot_arm/include/damiao_mit_codec.h @@ -0,0 +1,135 @@ +#ifndef CMVR_ES_DAMIAO_MIT_CODEC_H +#define CMVR_ES_DAMIAO_MIT_CODEC_H + +#include + +#include "canbus/abstract_canbus.h" + +namespace cmvr::device { + +enum class DamiaoMotorModel : std::uint8_t { + Unknown = 0, + DM4310, + DM4310_48V, + DM4340, + DM4340_48V, + DM6006, + DM8006, + DM8009, + DM10010L, + DM10010, + DMH3510, + DMH6215, + DMG6220 +}; + +struct DamiaoMotorLimits { + double q_max_rad{0.0}; + double dq_max_rad_s{0.0}; + double tau_max_nm{0.0}; + + bool valid() const noexcept; +}; + +struct DamiaoMitCommand { + double q_rad{0.0}; + double dq_rad_s{0.0}; + double kp{0.0}; + double kd{0.0}; + double tau_ff_nm{0.0}; +}; + +enum DamiaoSaturation : std::uint8_t { + DAMIAO_SATURATION_NONE = 0, + DAMIAO_SATURATION_Q = 1U << 0U, + DAMIAO_SATURATION_DQ = 1U << 1U, + DAMIAO_SATURATION_KP = 1U << 2U, + DAMIAO_SATURATION_KD = 1U << 3U, + DAMIAO_SATURATION_TAU = 1U << 4U +}; + +enum class DamiaoCodecError : std::uint8_t { + None = 0, + UnknownModel, + InvalidLimits, + NonFiniteInput, + InvalidCanId, + InvalidFrame, + UnexpectedFeedbackId +}; + +struct DamiaoEncodeResult { + DamiaoCodecError error{DamiaoCodecError::None}; + std::uint8_t saturation_mask{DAMIAO_SATURATION_NONE}; + + explicit operator bool() const noexcept + { + return error == DamiaoCodecError::None; + } +}; + +struct DamiaoJointFeedback { + std::uint8_t reported_motor_id{0}; + std::uint8_t status{0}; + std::uint8_t driver_temperature_raw{0}; + std::uint8_t motor_temperature_raw{0}; + double q_rad{0.0}; + double dq_rad_s{0.0}; + double tau_nm{0.0}; + std::int64_t rx_monotonic_ns{0}; + bool valid{false}; +}; + +enum class DamiaoMode : std::uint8_t { + ClearFault, + Enable, + Disable, + SetZero +}; + +class DamiaoMitCodec { +public: + static constexpr double kKpMax = 500.0; + static constexpr double kKdMax = 5.0; + + static DamiaoMotorLimits limitsFor(DamiaoMotorModel model) noexcept; + + static DamiaoEncodeResult encodeMit( + std::uint32_t command_id, + DamiaoMotorModel model, + const DamiaoMitCommand& command, + bool is_fd, + bool bitrate_switch, + CanFrame& frame) noexcept; + + static DamiaoCodecError decodeFeedback( + const CanFrame& frame, + std::uint32_t expected_feedback_id, + DamiaoMotorModel model, + DamiaoJointFeedback& feedback) noexcept; + + static DamiaoCodecError encodeMode( + std::uint32_t command_id, + DamiaoMode mode, + bool is_fd, + bool bitrate_switch, + CanFrame& frame) noexcept; + + // Public for protocol golden-vector tests. The unusual +1 decode behavior + // intentionally matches the legacy UME Python implementation. + static std::uint16_t floatToUint( + double value, + double minimum, + double maximum, + unsigned bits, + bool& saturated) noexcept; + static double uintToFloat( + std::uint16_t value, + double minimum, + double maximum, + unsigned bits) noexcept; +}; + +} // namespace cmvr::device + +#endif // CMVR_ES_DAMIAO_MIT_CODEC_H diff --git a/cmvr-es/devices/arm/ume_robot_arm/include/ume_robot_arm.h b/cmvr-es/devices/arm/ume_robot_arm/include/ume_robot_arm.h new file mode 100644 index 00000000..849277f4 --- /dev/null +++ b/cmvr-es/devices/arm/ume_robot_arm/include/ume_robot_arm.h @@ -0,0 +1,181 @@ +#ifndef CMVR_ES_UME_ROBOT_ARM_H +#define CMVR_ES_UME_ROBOT_ARM_H + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "arm/robot_arm.h" +#include "arm/ume_robot_arm/include/damiao_can_fd_chain.h" +#include "cmvr/config/arm_config/arm_config.pb.h" + +namespace cmvr::device { + +class AbstractCanbus; + +struct UmeArmSample { + static constexpr std::size_t kDof = 8; + + std::uint64_t sequence{0}; + std::int64_t sample_monotonic_ns{0}; + std::array q{}; + std::array dq{}; + std::array tau_measured{}; + std::array motor_rx_time_ns{}; + std::uint8_t valid_mask{0}; +}; + +// One UmeRobotArm represents one physical eight-axis leader arm and one +// SocketCAN-FD interface. The class owns its local high-frequency actuator +// loop; networking and follower kinematics remain outside this device. +class UmeRobotArm final : public RobotArm { +public: + explicit UmeRobotArm(const config::RobotArmConfig& cfg); + UmeRobotArm(const config::RobotArmConfig& cfg, + std::shared_ptr canbus); + ~UmeRobotArm() override; + + std::string typeName() const override { return "UmeRobotArm"; } + bool init() override; + bool start() override; + bool stop() override; + DeviceHealthSnapshot healthSnapshot() override; + + RobotModel getRobotModel() const override { return model_; } + std::size_t getDof() const override { return UmeArmSample::kDof; } + ArmState getRobotState() const override; + JointGroupState getJointState() const override; + Result readSample(UmeArmSample& sample) const; + CartesianPose getTcpPose(FrameType frame = FrameType::Base) const override; + RobotMode getRobotMode() const override; + SafetyMode getSafetyMode() const override; + ControlMode getControlMode() const override; + + Result torqueOn() override; + Result torqueOff() override; + Result calibrateZeroQ(const std::string& joint_name) override; + + Result emergencyStop() override; + Result protectiveStop() override; + Result setSpeedScaling(double scaling) override; + double getSpeedScaling() const override { return 1.0; } + bool isProtectiveStopped() const override + { + return protective_stopped_.load(); + } + bool isEmergencyStopped() const override + { + return emergency_stopped_.load(); + } + bool isFault() const override { return fault_latched_.load(); } + + Result moveJ(const JointPositionCommand& target, + const MotionOptions& options) override; + Result speedJ(const JointVelocityCommand& velocity, + double acceleration, + double duration) override; + Result stopJ(double acceleration) override; + Result moveL(const CartesianPose& target, + const MotionOptions& options, + FrameType frame = FrameType::Base) override; + Result speedL(const CartesianVelocity& velocity, + double acceleration, + double duration, + FrameType frame = FrameType::Base) override; + Result stopL(std::optional acceleration = std::nullopt) override; + Result stopMotion() override; + + Result startServoMode(const ServoOptions& options) override; + Result servoJ(const JointPositionCommand& target) override; + Result servoL(const CartesianPose& target, + FrameType frame = FrameType::Base) override; + Result servoSpeedJ(const JointVelocityCommand& velocity) override; + Result servoSpeedL(const CartesianVelocity& velocity, + FrameType frame = FrameType::Base) override; + Result stopServoMode() override; + + Result startTorqueMode(const TorqueServoOptions& options) override; + Result servoTorque(const JointTorqueCommand& target) override; + Result stopTorqueMode() override; + + Result connect(const std::string& ip, int port) override; + Result disconnect() override; + bool isConnected() const override { return initialized_.load(); } + Result powerOn() override { return torqueOn(); } + Result powerOff() override { return torqueOff(); } + Result brakeRelease() override; + Result shutdown() override; + Result clearFault() override; + Result unlockProtectiveStop() override; + Result loadProgram(const std::string& program_name) override; + Result playProgram() override; + Result pauseProgram() override; + Result stopProgram() override; + + std::vector ik(const std::string& base_link, + const std::string& ee_link, + const CartesianPose& pose) override; + std::shared_ptr kinematicsSolver() const override + { + return ik_solver_; + } + CartesianPose fk(const std::string& base_link, + const std::string& ee_link) override; + CartesianPose fk(bool is_tcp = true) override; + CartesianVelocity getSpeedLCommandTwistBase() const override { return {}; } + bool busy() const override { return powered_on_.load(); } + +private: + void normalizeConfig_(); + bool buildModelAndChain_(); + void controlLoop_() noexcept; + void recordFault_(const std::string& message) noexcept; + Result requirePassive_(const std::string& operation) const; + static Result unsupported_(const std::string& operation); + static std::int64_t monotonicNowNs_() noexcept; + + config::RobotArmConfig cfg_; + config::UmeRobotArmBackendConfig ume_cfg_; + std::shared_ptr canbus_; + std::unique_ptr chain_; + std::vector joint_specs_; + RobotModel model_; + std::shared_ptr ik_solver_; + + mutable std::mutex lifecycle_mutex_; + mutable std::mutex command_mutex_; + mutable std::mutex sample_mutex_; + mutable std::mutex status_mutex_; + mutable std::mutex kinematics_mutex_; + std::thread control_thread_; + + std::array latest_torque_command_{}; + UmeArmSample latest_sample_; + std::string last_error_; + + std::atomic initialized_{false}; + std::atomic running_{false}; + std::atomic torque_mode_{false}; + std::atomic powered_on_{false}; + std::atomic fault_latched_{false}; + std::atomic protective_stopped_{false}; + std::atomic emergency_stopped_{false}; + std::atomic command_ready_{false}; + std::atomic command_sequence_{0}; + std::atomic command_time_ns_{0}; + std::atomic loop_period_ns_{1250000}; + std::atomic command_watchdog_ms_{20}; + std::uint32_t cycle_deadline_us_{900}; + std::uint32_t feedback_watchdog_ms_{20}; + std::uint32_t shutdown_timeout_ms_{50}; +}; + +} // namespace cmvr::device + +#endif // CMVR_ES_UME_ROBOT_ARM_H diff --git a/cmvr-es/devices/arm/ume_robot_arm/src/damiao_can_fd_chain.cpp b/cmvr-es/devices/arm/ume_robot_arm/src/damiao_can_fd_chain.cpp new file mode 100644 index 00000000..2c4830e3 --- /dev/null +++ b/cmvr-es/devices/arm/ume_robot_arm/src/damiao_can_fd_chain.cpp @@ -0,0 +1,705 @@ +#include "arm/ume_robot_arm/include/damiao_can_fd_chain.h" + +#include +#include +#include +#include + +#include "canbus/abstract_canbus.h" + +namespace cmvr::device { +namespace { + +Result invalidArgument(const std::string& message) +{ + return Result::failure(ArmErrorCode::InvalidArgument, message); +} + +Result commandFailed(const std::string& message) +{ + return Result::failure(ArmErrorCode::CommandFailed, message); +} + +Result notReady(const std::string& message) +{ + return Result::failure(ArmErrorCode::RobotNotReady, message); +} + +} // namespace + +DamiaoCanFdChain::DamiaoCanFdChain( + std::shared_ptr bus, + std::vector joints, + DamiaoChainOptions options) + : bus_(std::move(bus)), + joints_(std::move(joints)), + options_(options), + feedback_scratch_(joints_.size()), + feedback_seen_(joints_.size(), false) +{ + tx_frames_.reserve(joints_.size()); + rx_frames_.reserve(1); +} + +DamiaoCanFdChain::~DamiaoCanFdChain() +{ + stop(); +} + +Result DamiaoCanFdChain::validateConfig_() const +{ + if (!bus_) { + return invalidArgument("Damiao CAN bus is null"); + } + if (joints_.empty()) { + return invalidArgument("Damiao joint list is empty"); + } + + std::unordered_set names; + std::unordered_set command_ids; + std::unordered_set feedback_ids; + std::unordered_set reported_ids; + for (const auto& joint : joints_) { + if (joint.joint_name.empty() || + !names.insert(joint.joint_name).second) { + return invalidArgument("Damiao joint names must be non-empty and unique"); + } + if (joint.command_id == 0 || joint.command_id > 0x7FFU || + !command_ids.insert(joint.command_id).second) { + return invalidArgument("Damiao command IDs must be unique standard CAN IDs"); + } + if (joint.feedback_id == 0 || joint.feedback_id > 0x7FFU || + !feedback_ids.insert(joint.feedback_id).second) { + return invalidArgument("Damiao feedback IDs must be unique standard CAN IDs"); + } + if (joint.reported_motor_id > 0x0FU || + !reported_ids.insert(joint.reported_motor_id).second) { + return invalidArgument("Damiao reported motor IDs must be unique 4-bit values"); + } + if (!DamiaoMitCodec::limitsFor(joint.model).valid()) { + return invalidArgument("Damiao motor model is unknown"); + } + if (joint.direction != 1 && joint.direction != -1) { + return invalidArgument("Damiao joint direction must be +1 or -1"); + } + if (!std::isfinite(joint.zero_offset_rad) || + !std::isfinite(joint.joint_lower_rad) || + !std::isfinite(joint.joint_upper_rad) || + joint.joint_upper_rad <= joint.joint_lower_rad || + !std::isfinite(joint.max_velocity_rad_s) || + joint.max_velocity_rad_s <= 0.0 || + !std::isfinite(joint.max_torque_nm) || + joint.max_torque_nm <= 0.0) { + return invalidArgument("Damiao mechanical limits are invalid"); + } + if (options_.hardware_enabled && + (joint.healthy_status_mask == 0U || + joint.max_driver_temperature_raw == 0U || + joint.max_motor_temperature_raw == 0U)) { + return invalidArgument( + "Damiao hardware enable requires a reviewed feedback-status " + "whitelist and nonzero raw temperature thresholds"); + } + } + return Result::success(); +} + +Result DamiaoCanFdChain::init() +{ + std::lock_guard lock(io_mutex_); + const auto config_result = validateConfig_(); + if (!config_result.ok()) { + setError_(config_result.message); + state_.store(DamiaoChainState::FaultLatched); + return config_result; + } + if (state_.load() != DamiaoChainState::Closed && + state_.load() != DamiaoChainState::Stopped) { + return Result::success(); + } + if (!bus_->init()) { + setError_("failed to initialize Damiao CAN bus"); + state_.store(DamiaoChainState::FaultLatched); + return notReady(lastError()); + } + state_.store(DamiaoChainState::Initialized); + return Result::success(); +} + +Result DamiaoCanFdChain::openPassive() +{ + std::lock_guard lock(io_mutex_); + if (state_.load() != DamiaoChainState::Initialized) { + return notReady("Damiao chain is not initialized"); + } + if (!bus_->start()) { + setError_("failed to start Damiao CAN bus"); + state_.store(DamiaoChainState::FaultLatched); + return notReady(lastError()); + } + // Deliberately no clear-fault or enable command here. + state_.store(DamiaoChainState::Passive); + return Result::success(); +} + +Result DamiaoCanFdChain::clearFault( + const std::chrono::steady_clock::time_point deadline) +{ + std::lock_guard lock(io_mutex_); + if (!options_.hardware_enabled) { + return Result::failure( + ArmErrorCode::CommandRejected, + "Damiao hardware commands are disabled by configuration"); + } + const auto current = state_.load(); + if (current != DamiaoChainState::Passive && + current != DamiaoChainState::FaultLatched) { + return notReady("clearFault requires a disabled Damiao chain"); + } + const auto result = sendModeAll_( + DamiaoMode::ClearFault, deadline, true); + if (!result.ok()) { + state_.store(DamiaoChainState::FaultLatched); + return result; + } + // Clearing a fault never arms the motors. + state_.store(DamiaoChainState::Passive); + return Result::success(); +} + +Result DamiaoCanFdChain::arm( + const std::chrono::steady_clock::time_point deadline) +{ + std::lock_guard lock(io_mutex_); + if (!options_.hardware_enabled) { + return Result::failure( + ArmErrorCode::CommandRejected, + "Damiao hardware commands are disabled by configuration"); + } + if (state_.load() != DamiaoChainState::Passive) { + return notReady("Damiao chain must be passive before arm"); + } + const auto result = sendModeAll_(DamiaoMode::Enable, deadline, true); + if (!result.ok()) { + latchFaultAndDisable_(result.message); + return Result::failure(result.code, lastError()); + } + state_.store(DamiaoChainState::Armed); + return Result::success(); +} + +Result DamiaoCanFdChain::setZero( + const std::size_t joint_index, + const std::chrono::steady_clock::time_point deadline) +{ + std::lock_guard lock(io_mutex_); + if (!options_.hardware_enabled) { + return Result::failure( + ArmErrorCode::CommandRejected, + "Damiao hardware commands are disabled by configuration"); + } + if (state_.load() != DamiaoChainState::Passive) { + return notReady("setZero requires a passive Damiao chain"); + } + return sendModeOne_(joint_index, DamiaoMode::SetZero, deadline); +} + +DamiaoMitCommand DamiaoCanFdChain::toMotorCommand_( + const DamiaoJointSpec& spec, + const DamiaoMitCommand& command, + bool& safety_saturated) const noexcept +{ + DamiaoMitCommand motor = command; + safety_saturated = false; + + const double limited_q = + std::clamp(command.q_rad, spec.joint_lower_rad, spec.joint_upper_rad); + const double limited_dq = + std::clamp(command.dq_rad_s, + -spec.max_velocity_rad_s, spec.max_velocity_rad_s); + const double limited_tau = + std::clamp(command.tau_ff_nm, + -spec.max_torque_nm, spec.max_torque_nm); + safety_saturated = + limited_q != command.q_rad || + limited_dq != command.dq_rad_s || + limited_tau != command.tau_ff_nm; + + motor.q_rad = + spec.direction * (limited_q - spec.zero_offset_rad); + motor.dq_rad_s = spec.direction * limited_dq; + motor.tau_ff_nm = spec.direction * limited_tau; + return motor; +} + +void DamiaoCanFdChain::toJointFeedback_( + const DamiaoJointSpec& spec, + DamiaoJointFeedback& feedback) const noexcept +{ + feedback.q_rad = + spec.direction * feedback.q_rad + spec.zero_offset_rad; + feedback.dq_rad_s = spec.direction * feedback.dq_rad_s; + feedback.tau_nm = spec.direction * feedback.tau_nm; +} + +Result DamiaoCanFdChain::exchange( + const DamiaoMitCommand* joint_commands, + const std::size_t command_count, + DamiaoJointFeedback* joint_feedback, + const std::size_t feedback_count, + const std::chrono::steady_clock::time_point deadline) +{ + std::lock_guard lock(io_mutex_); + if (!joint_commands || !joint_feedback || + command_count != joints_.size() || + feedback_count != joints_.size()) { + { + std::lock_guard status_lock(status_mutex_); + ++statistics_.rejected_commands; + } + return invalidArgument("Damiao exchange dimensions do not match configured joints"); + } + const auto current = state_.load(); + if (current != DamiaoChainState::Armed && + current != DamiaoChainState::Active) { + return notReady("Damiao chain is not armed"); + } + if (std::chrono::steady_clock::now() >= deadline) { + { + std::lock_guard status_lock(status_mutex_); + ++statistics_.deadline_misses; + } + latchFaultAndDisable_( + "Damiao exchange deadline expired before send"); + return Result::failure(ArmErrorCode::Timeout, lastError()); + } + + tx_frames_.clear(); + std::uint64_t saturation_count = 0; + for (std::size_t i = 0; i < joints_.size(); ++i) { + bool safety_saturated = false; + const auto motor_command = + toMotorCommand_(joints_[i], joint_commands[i], safety_saturated); + CanFrame frame; + const auto encoded = DamiaoMitCodec::encodeMit( + joints_[i].command_id, joints_[i].model, motor_command, + options_.is_fd, options_.bitrate_switch, frame); + if (!encoded) { + { + std::lock_guard status_lock(status_mutex_); + ++statistics_.rejected_commands; + } + return invalidArgument("Damiao command failed protocol validation"); + } + if (safety_saturated || + encoded.saturation_mask != DAMIAO_SATURATION_NONE) { + ++saturation_count; + } + tx_frames_.push_back(frame); + } + if (!bus_->discardPendingFrames()) { + latchFaultAndDisable_( + "failed to drain stale Damiao feedback before command"); + return commandFailed(lastError()); + } + if (std::chrono::steady_clock::now() >= deadline) { + { + std::lock_guard status_lock(status_mutex_); + ++statistics_.deadline_misses; + } + latchFaultAndDisable_( + "Damiao exchange deadline expired before command commit"); + return Result::failure(ArmErrorCode::Timeout, lastError()); + } + if (!sendFrames_(tx_frames_, deadline)) { + latchFaultAndDisable_( + "failed to send Damiao MIT command batch before deadline"); + return commandFailed(lastError()); + } + if (std::chrono::steady_clock::now() >= deadline) { + { + std::lock_guard status_lock(status_mutex_); + ++statistics_.deadline_misses; + } + latchFaultAndDisable_( + "Damiao MIT command batch exceeded its deadline"); + return Result::failure(ArmErrorCode::Timeout, lastError()); + } + + const auto receive_result = + receiveCycle_(joint_feedback, feedback_count, deadline); + { + std::lock_guard status_lock(status_mutex_); + ++statistics_.exchanges; + statistics_.protocol_saturations += saturation_count; + } + if (!receive_result.ok()) { + latchFaultAndDisable_(receive_result.message); + return Result::failure(receive_result.code, lastError()); + } + state_.store(DamiaoChainState::Active); + return Result::success(); +} + +Result DamiaoCanFdChain::sendModeAll_( + const DamiaoMode mode, + const std::chrono::steady_clock::time_point deadline, + const bool expect_feedback) +{ + tx_frames_.clear(); + for (const auto& joint : joints_) { + CanFrame frame; + const auto error = DamiaoMitCodec::encodeMode( + joint.command_id, mode, options_.is_fd, + options_.bitrate_switch, frame); + if (error != DamiaoCodecError::None) { + return invalidArgument("failed to encode Damiao lifecycle command"); + } + tx_frames_.push_back(frame); + } + if (expect_feedback && !bus_->discardPendingFrames()) { + return commandFailed( + "failed to drain stale Damiao lifecycle feedback"); + } + if (std::chrono::steady_clock::now() >= deadline) { + return Result::failure( + ArmErrorCode::Timeout, + "Damiao lifecycle deadline expired before command commit"); + } + if (!sendFrames_(tx_frames_, deadline)) { + return commandFailed( + "failed to send Damiao lifecycle command before deadline"); + } + if (std::chrono::steady_clock::now() >= deadline) { + return Result::failure( + ArmErrorCode::Timeout, + "Damiao lifecycle command exceeded its deadline"); + } + if (!expect_feedback) { + return Result::success(); + } + return receiveCycle_( + feedback_scratch_.data(), feedback_scratch_.size(), deadline); +} + +Result DamiaoCanFdChain::sendModeOne_( + const std::size_t joint_index, + const DamiaoMode mode, + const std::chrono::steady_clock::time_point deadline) +{ + if (joint_index >= joints_.size()) { + return invalidArgument("Damiao joint index is out of range"); + } + CanFrame frame; + const auto error = DamiaoMitCodec::encodeMode( + joints_[joint_index].command_id, mode, options_.is_fd, + options_.bitrate_switch, frame); + if (error != DamiaoCodecError::None) { + return invalidArgument("failed to encode Damiao lifecycle command"); + } + tx_frames_.assign(1, frame); + if (!bus_->discardPendingFrames()) { + return commandFailed( + "failed to drain stale Damiao lifecycle feedback"); + } + if (std::chrono::steady_clock::now() >= deadline) { + return Result::failure( + ArmErrorCode::Timeout, + "Damiao lifecycle deadline expired before command commit"); + } + if (!sendFrames_(tx_frames_, deadline)) { + return commandFailed( + "failed to send Damiao lifecycle command before deadline"); + } + if (std::chrono::steady_clock::now() >= deadline) { + return Result::failure( + ArmErrorCode::Timeout, + "Damiao lifecycle command exceeded its deadline"); + } + + std::fill(feedback_seen_.begin(), feedback_seen_.end(), false); + while (std::chrono::steady_clock::now() < deadline) { + rx_frames_.clear(); + int32_t count = 1; + if (bus_->receive(&rx_frames_, &count) != msgs::ErrorCode::OK) { + continue; + } + for (const auto& received : rx_frames_) { + if (received.id != joints_[joint_index].feedback_id) { + continue; + } + DamiaoJointFeedback feedback; + if (DamiaoMitCodec::decodeFeedback( + received, joints_[joint_index].feedback_id, + joints_[joint_index].model, feedback) != + DamiaoCodecError::None || + feedback.reported_motor_id != + joints_[joint_index].reported_motor_id || + !feedbackTransportAndHealthValid_( + received, joints_[joint_index], feedback)) { + return commandFailed("invalid Damiao lifecycle feedback"); + } + return Result::success(); + } + } + return Result::failure( + ArmErrorCode::Timeout, "Damiao lifecycle feedback timed out"); +} + +Result DamiaoCanFdChain::receiveCycle_( + DamiaoJointFeedback* feedback, + const std::size_t feedback_count, + const std::chrono::steady_clock::time_point deadline) +{ + if (!feedback || feedback_count != joints_.size()) { + return invalidArgument("Damiao feedback dimensions do not match"); + } + + std::fill(feedback_seen_.begin(), feedback_seen_.end(), false); + std::size_t received_count = 0; + while (received_count < joints_.size() && + std::chrono::steady_clock::now() < deadline) { + rx_frames_.clear(); + int32_t count = 1; + if (bus_->receive(&rx_frames_, &count) != msgs::ErrorCode::OK) { + continue; + } + for (const auto& frame : rx_frames_) { + const auto index = jointIndexForFeedbackId_(frame.id); + if (index == joints_.size()) { + std::lock_guard status_lock(status_mutex_); + ++statistics_.unknown_feedback; + continue; + } + if (feedback_seen_[index]) { + std::lock_guard status_lock(status_mutex_); + ++statistics_.duplicate_feedback; + continue; + } + DamiaoJointFeedback decoded; + if (DamiaoMitCodec::decodeFeedback( + frame, joints_[index].feedback_id, + joints_[index].model, decoded) != + DamiaoCodecError::None || + decoded.reported_motor_id != + joints_[index].reported_motor_id || + !feedbackTransportAndHealthValid_( + frame, joints_[index], decoded)) { + return commandFailed("Damiao feedback failed validation"); + } + toJointFeedback_(joints_[index], decoded); + feedback[index] = decoded; + feedback_seen_[index] = true; + ++received_count; + } + } + if (received_count != joints_.size()) { + std::lock_guard status_lock(status_mutex_); + ++statistics_.deadline_misses; + return Result::failure( + ArmErrorCode::Timeout, + "Damiao feedback cycle missed its deadline"); + } + return Result::success(); +} + +bool DamiaoCanFdChain::feedbackTransportAndHealthValid_( + const CanFrame& frame, + const DamiaoJointSpec& joint, + const DamiaoJointFeedback& feedback) const noexcept +{ + if (frame.is_fd != options_.is_fd) { + return false; + } + if (options_.is_fd && options_.bitrate_switch && + !frame.bitrate_switch) { + return false; + } + if (joint.healthy_status_mask == 0U) { + // An empty whitelist is tolerated only while the actuator hardware + // gate is closed, so passive software/configuration checks can run. + return !options_.hardware_enabled; + } + if (feedback.status > 0x0FU || + (joint.healthy_status_mask & + static_cast(1U << feedback.status)) == 0U) { + return false; + } + return feedback.driver_temperature_raw <= + joint.max_driver_temperature_raw && + feedback.motor_temperature_raw <= + joint.max_motor_temperature_raw; +} + +bool DamiaoCanFdChain::sendFrames_( + const std::vector& frames, + const std::chrono::steady_clock::time_point deadline) noexcept +{ + if (frames.empty() || + frames.size() > static_cast( + std::numeric_limits::max())) { + return false; + } + int32_t count = static_cast(frames.size()); + return bus_->sendUntil(frames, &count, deadline) == + msgs::ErrorCode::OK && + count == static_cast(frames.size()); +} + +bool DamiaoCanFdChain::sendFramesBestEffort_( + const std::vector& frames) noexcept +{ + if (frames.empty() || + frames.size() > static_cast( + std::numeric_limits::max())) { + return false; + } + int32_t count = static_cast(frames.size()); + return bus_->send(frames, &count) == msgs::ErrorCode::OK && + count == static_cast(frames.size()); +} + +Result DamiaoCanFdChain::disable() noexcept +{ + std::lock_guard lock(io_mutex_); + if (state_.load() == DamiaoChainState::Closed || + state_.load() == DamiaoChainState::Initialized || + state_.load() == DamiaoChainState::Stopped) { + return Result::success(); + } + const bool disabled = bestEffortZeroAndDisable_(); + if (!disabled) { + setError_( + "failed to send all Damiao zero/disable safety frames"); + state_.store(DamiaoChainState::FaultLatched); + return commandFailed(lastError()); + } + if (state_.load() != DamiaoChainState::FaultLatched) { + state_.store(DamiaoChainState::Passive); + } + return Result::success(); +} + +Result DamiaoCanFdChain::latchFault(const std::string& reason) noexcept +{ + std::lock_guard lock(io_mutex_); + if (!latchFaultAndDisable_(reason)) { + return commandFailed(lastError()); + } + return Result::success(); +} + +bool DamiaoCanFdChain::latchFaultAndDisable_( + const std::string& reason) noexcept +{ + state_.store(DamiaoChainState::FaultLatched); + setError_(reason); + if (bestEffortZeroAndDisable_()) { + return true; + } + setError_( + reason + + "; failed to send all Damiao zero/disable safety frames"); + return false; +} + +bool DamiaoCanFdChain::bestEffortZeroAndDisable_() noexcept +{ + if (!bus_ || !options_.hardware_enabled) { + return true; + } + + const auto current = state_.load(); + if (current != DamiaoChainState::Passive && + current != DamiaoChainState::Armed && + current != DamiaoChainState::Active && + current != DamiaoChainState::FaultLatched) { + return true; + } + + bool all_sent = true; + if (current == DamiaoChainState::Armed || + current == DamiaoChainState::Active || + current == DamiaoChainState::FaultLatched) { + tx_frames_.clear(); + for (const auto& joint : joints_) { + DamiaoMitCommand zero; + CanFrame frame; + if (DamiaoMitCodec::encodeMit( + joint.command_id, joint.model, zero, + options_.is_fd, options_.bitrate_switch, frame)) { + tx_frames_.push_back(frame); + } + } + if (!tx_frames_.empty()) { + all_sent = sendFramesBestEffort_(tx_frames_) && all_sent; + } + } + + tx_frames_.clear(); + for (const auto& joint : joints_) { + CanFrame frame; + if (DamiaoMitCodec::encodeMode( + joint.command_id, DamiaoMode::Disable, + options_.is_fd, options_.bitrate_switch, frame) == + DamiaoCodecError::None) { + tx_frames_.push_back(frame); + } + } + if (!tx_frames_.empty()) { + all_sent = sendFramesBestEffort_(tx_frames_) && all_sent; + } + return all_sent; +} + +void DamiaoCanFdChain::stop() noexcept +{ + std::lock_guard lock(io_mutex_); + const auto current = state_.load(); + if (current == DamiaoChainState::Closed || + current == DamiaoChainState::Stopped) { + return; + } + if (!bestEffortZeroAndDisable_()) { + setError_( + "failed to send all Damiao shutdown safety frames"); + } + if (bus_) { + bus_->stop(); + } + state_.store(DamiaoChainState::Stopped); +} + +std::size_t DamiaoCanFdChain::jointIndexForFeedbackId_( + const std::uint32_t id) const noexcept +{ + for (std::size_t i = 0; i < joints_.size(); ++i) { + if (joints_[i].feedback_id == id) { + return i; + } + } + return joints_.size(); +} + +void DamiaoCanFdChain::setError_(const std::string& error) noexcept +{ + try { + std::lock_guard lock(status_mutex_); + last_error_ = error; + } catch (...) { + } +} + +DamiaoChainStatistics DamiaoCanFdChain::statistics() const +{ + std::lock_guard lock(status_mutex_); + return statistics_; +} + +std::string DamiaoCanFdChain::lastError() const +{ + std::lock_guard lock(status_mutex_); + return last_error_; +} + +} // namespace cmvr::device diff --git a/cmvr-es/devices/arm/ume_robot_arm/src/damiao_mit_codec.cpp b/cmvr-es/devices/arm/ume_robot_arm/src/damiao_mit_codec.cpp new file mode 100644 index 00000000..58e40e69 --- /dev/null +++ b/cmvr-es/devices/arm/ume_robot_arm/src/damiao_mit_codec.cpp @@ -0,0 +1,254 @@ +#include "arm/ume_robot_arm/include/damiao_mit_codec.h" + +#include +#include +#include + +namespace cmvr::device { +namespace { + +constexpr unsigned kPositionBits = 16; +constexpr unsigned kVelocityBits = 12; +constexpr unsigned kGainBits = 12; +constexpr unsigned kTorqueBits = 12; +constexpr std::uint32_t kCanStandardMaxId = 0x7FFU; + +bool finiteCommand(const DamiaoMitCommand& command) noexcept +{ + return std::isfinite(command.q_rad) && + std::isfinite(command.dq_rad_s) && + std::isfinite(command.kp) && + std::isfinite(command.kd) && + std::isfinite(command.tau_ff_nm); +} + +std::uint8_t modeByte(const DamiaoMode mode) noexcept +{ + switch (mode) { + case DamiaoMode::ClearFault: + return 0xFBU; + case DamiaoMode::Enable: + return 0xFCU; + case DamiaoMode::Disable: + return 0xFDU; + case DamiaoMode::SetZero: + return 0xFEU; + } + return 0; +} + +} // namespace + +bool DamiaoMotorLimits::valid() const noexcept +{ + return std::isfinite(q_max_rad) && q_max_rad > 0.0 && + std::isfinite(dq_max_rad_s) && dq_max_rad_s > 0.0 && + std::isfinite(tau_max_nm) && tau_max_nm > 0.0; +} + +DamiaoMotorLimits DamiaoMitCodec::limitsFor( + const DamiaoMotorModel model) noexcept +{ + switch (model) { + case DamiaoMotorModel::DM4310: + return {12.5, 30.0, 10.0}; + case DamiaoMotorModel::DM4310_48V: + return {12.5, 50.0, 10.0}; + case DamiaoMotorModel::DM4340: + return {12.5, 8.0, 28.0}; + case DamiaoMotorModel::DM4340_48V: + return {12.5, 10.0, 28.0}; + case DamiaoMotorModel::DM6006: + return {12.5, 45.0, 20.0}; + case DamiaoMotorModel::DM8006: + return {12.5, 45.0, 40.0}; + case DamiaoMotorModel::DM8009: + return {12.5, 45.0, 54.0}; + case DamiaoMotorModel::DM10010L: + return {12.5, 25.0, 200.0}; + case DamiaoMotorModel::DM10010: + return {12.5, 20.0, 200.0}; + case DamiaoMotorModel::DMH3510: + return {12.5, 280.0, 1.0}; + case DamiaoMotorModel::DMH6215: + return {12.5, 45.0, 10.0}; + case DamiaoMotorModel::DMG6220: + return {12.5, 45.0, 10.0}; + case DamiaoMotorModel::Unknown: + default: + return {}; + } +} + +std::uint16_t DamiaoMitCodec::floatToUint( + const double value, + const double minimum, + const double maximum, + const unsigned bits, + bool& saturated) noexcept +{ + saturated = value < minimum || value > maximum; + if (!std::isfinite(value) || !std::isfinite(minimum) || + !std::isfinite(maximum) || maximum <= minimum || + bits == 0 || bits > 16) { + saturated = true; + return 0; + } + + const double clamped = std::clamp(value, minimum, maximum); + const std::uint32_t levels = (std::uint32_t{1} << bits) - 1U; + const double normalized = (clamped - minimum) / (maximum - minimum); + return static_cast(normalized * levels); +} + +double DamiaoMitCodec::uintToFloat( + const std::uint16_t value, + const double minimum, + const double maximum, + const unsigned bits) noexcept +{ + if (!std::isfinite(minimum) || !std::isfinite(maximum) || + maximum <= minimum || bits == 0 || bits > 16) { + return 0.0; + } + const double span = maximum - minimum; + const double levels = static_cast(std::uint32_t{1} << bits); + return (static_cast(value) + 1.0) * span / levels + minimum; +} + +DamiaoEncodeResult DamiaoMitCodec::encodeMit( + const std::uint32_t command_id, + const DamiaoMotorModel model, + const DamiaoMitCommand& command, + const bool is_fd, + const bool bitrate_switch, + CanFrame& frame) noexcept +{ + DamiaoEncodeResult result; + const auto limits = limitsFor(model); + if (!limits.valid()) { + result.error = DamiaoCodecError::UnknownModel; + return result; + } + if (!finiteCommand(command)) { + result.error = DamiaoCodecError::NonFiniteInput; + return result; + } + if (command_id > kCanStandardMaxId) { + result.error = DamiaoCodecError::InvalidCanId; + return result; + } + + bool saturated = false; + const auto q = floatToUint( + command.q_rad, -limits.q_max_rad, limits.q_max_rad, + kPositionBits, saturated); + if (saturated) result.saturation_mask |= DAMIAO_SATURATION_Q; + + const auto dq = floatToUint( + command.dq_rad_s, -limits.dq_max_rad_s, limits.dq_max_rad_s, + kVelocityBits, saturated); + if (saturated) result.saturation_mask |= DAMIAO_SATURATION_DQ; + + const auto kp = floatToUint( + command.kp, 0.0, kKpMax, kGainBits, saturated); + if (saturated) result.saturation_mask |= DAMIAO_SATURATION_KP; + + const auto kd = floatToUint( + command.kd, 0.0, kKdMax, kGainBits, saturated); + if (saturated) result.saturation_mask |= DAMIAO_SATURATION_KD; + + const auto tau = floatToUint( + command.tau_ff_nm, -limits.tau_max_nm, limits.tau_max_nm, + kTorqueBits, saturated); + if (saturated) result.saturation_mask |= DAMIAO_SATURATION_TAU; + + frame = {}; + frame.id = command_id; + frame.len = 8; + frame.is_fd = is_fd; + frame.bitrate_switch = is_fd && bitrate_switch; + frame.data[0] = static_cast((q >> 8U) & 0xFFU); + frame.data[1] = static_cast(q & 0xFFU); + frame.data[2] = static_cast((dq >> 4U) & 0xFFU); + frame.data[3] = static_cast( + ((dq & 0xFU) << 4U) | ((kp >> 8U) & 0xFU)); + frame.data[4] = static_cast(kp & 0xFFU); + frame.data[5] = static_cast((kd >> 4U) & 0xFFU); + frame.data[6] = static_cast( + ((kd & 0xFU) << 4U) | ((tau >> 8U) & 0xFU)); + frame.data[7] = static_cast(tau & 0xFFU); + return result; +} + +DamiaoCodecError DamiaoMitCodec::decodeFeedback( + const CanFrame& frame, + const std::uint32_t expected_feedback_id, + const DamiaoMotorModel model, + DamiaoJointFeedback& feedback) noexcept +{ + feedback = {}; + const auto limits = limitsFor(model); + if (!limits.valid()) { + return DamiaoCodecError::UnknownModel; + } + if (frame.is_error_frame || frame.is_remote_frame || + frame.is_extended_id || frame.error_state_indicator || + frame.len != 8) { + return DamiaoCodecError::InvalidFrame; + } + if (frame.id != expected_feedback_id) { + return DamiaoCodecError::UnexpectedFeedbackId; + } + + const std::uint16_t q = + static_cast( + (static_cast(frame.data[1]) << 8U) | + frame.data[2]); + const std::uint16_t dq = + static_cast( + (static_cast(frame.data[3]) << 4U) | + (frame.data[4] >> 4U)); + const std::uint16_t tau = + static_cast( + ((static_cast(frame.data[4]) & 0xFU) << 8U) | + frame.data[5]); + + feedback.reported_motor_id = frame.data[0] & 0x0FU; + feedback.status = frame.data[0] >> 4U; + feedback.driver_temperature_raw = frame.data[6]; + feedback.motor_temperature_raw = frame.data[7]; + feedback.q_rad = + uintToFloat(q, -limits.q_max_rad, limits.q_max_rad, kPositionBits); + feedback.dq_rad_s = + uintToFloat(dq, -limits.dq_max_rad_s, limits.dq_max_rad_s, + kVelocityBits); + feedback.tau_nm = + uintToFloat(tau, -limits.tau_max_nm, limits.tau_max_nm, + kTorqueBits); + feedback.rx_monotonic_ns = frame.rx_monotonic_ns; + feedback.valid = true; + return DamiaoCodecError::None; +} + +DamiaoCodecError DamiaoMitCodec::encodeMode( + const std::uint32_t command_id, + const DamiaoMode mode, + const bool is_fd, + const bool bitrate_switch, + CanFrame& frame) noexcept +{ + if (command_id > kCanStandardMaxId) { + return DamiaoCodecError::InvalidCanId; + } + frame = {}; + frame.id = command_id; + frame.len = 8; + frame.is_fd = is_fd; + frame.bitrate_switch = is_fd && bitrate_switch; + std::memset(frame.data, 0xFF, 7); + frame.data[7] = modeByte(mode); + return DamiaoCodecError::None; +} + +} // namespace cmvr::device diff --git a/cmvr-es/devices/arm/ume_robot_arm/src/ume_robot_arm.cpp b/cmvr-es/devices/arm/ume_robot_arm/src/ume_robot_arm.cpp new file mode 100644 index 00000000..5ba7a926 --- /dev/null +++ b/cmvr-es/devices/arm/ume_robot_arm/src/ume_robot_arm.cpp @@ -0,0 +1,1035 @@ +#include "arm/ume_robot_arm/include/ume_robot_arm.h" + +#include +#include +#include +#include +#include +#include + +#include + +#include "algorithms/kinematics/ik_solver/ik_solver_factory.h" +#include "canbus/can_client/socket/socket_can_client_raw.h" +#include "common/base/logging/logger.h" +#include "common/math/transform_math.h" + +namespace cmvr::device { +namespace { + +DamiaoMotorModel toDriverModel(const config::DamiaoMotorModel model) +{ + switch (model) { + case config::DAMIAO_MOTOR_MODEL_DM4310: + return DamiaoMotorModel::DM4310; + case config::DAMIAO_MOTOR_MODEL_DM4310_48V: + return DamiaoMotorModel::DM4310_48V; + case config::DAMIAO_MOTOR_MODEL_DM4340: + return DamiaoMotorModel::DM4340; + case config::DAMIAO_MOTOR_MODEL_DM4340_48V: + return DamiaoMotorModel::DM4340_48V; + case config::DAMIAO_MOTOR_MODEL_DM6006: + return DamiaoMotorModel::DM6006; + case config::DAMIAO_MOTOR_MODEL_DM8006: + return DamiaoMotorModel::DM8006; + case config::DAMIAO_MOTOR_MODEL_DM8009: + return DamiaoMotorModel::DM8009; + case config::DAMIAO_MOTOR_MODEL_DM10010L: + return DamiaoMotorModel::DM10010L; + case config::DAMIAO_MOTOR_MODEL_DM10010: + return DamiaoMotorModel::DM10010; + case config::DAMIAO_MOTOR_MODEL_DMH3510: + return DamiaoMotorModel::DMH3510; + case config::DAMIAO_MOTOR_MODEL_DMH6215: + return DamiaoMotorModel::DMH6215; + case config::DAMIAO_MOTOR_MODEL_DMG6220: + return DamiaoMotorModel::DMG6220; + case config::DAMIAO_MOTOR_MODEL_UNKNOWN: + default: + return DamiaoMotorModel::Unknown; + } +} + +constexpr std::uint8_t kAllJointsValid = 0xFFU; + +} // namespace + +UmeRobotArm::UmeRobotArm(const config::RobotArmConfig& cfg) + : cfg_(cfg), + ume_cfg_(cfg.has_ume() ? cfg.ume() + : config::UmeRobotArmBackendConfig{}) +{ + id_ = cfg_.id(); + normalizeConfig_(); + canbus_ = std::make_shared(ume_cfg_.can()); + buildModelAndChain_(); +} + +UmeRobotArm::UmeRobotArm( + const config::RobotArmConfig& cfg, + std::shared_ptr canbus) + : cfg_(cfg), + ume_cfg_(cfg.has_ume() ? cfg.ume() + : config::UmeRobotArmBackendConfig{}), + canbus_(std::move(canbus)) +{ + id_ = cfg_.id(); + normalizeConfig_(); + buildModelAndChain_(); +} + +UmeRobotArm::~UmeRobotArm() +{ + stop(); +} + +void UmeRobotArm::normalizeConfig_() +{ + if (ume_cfg_.control_frequency_hz() == 0U) { + ume_cfg_.set_control_frequency_hz(800U); + } + const auto period_ns = static_cast( + 1000000000ULL / ume_cfg_.control_frequency_hz()); + loop_period_ns_.store(period_ns); + + if (ume_cfg_.cycle_deadline_us() == 0U) { + const auto period_us = + 1000000U / ume_cfg_.control_frequency_hz(); + ume_cfg_.set_cycle_deadline_us( + std::max(100U, period_us * 4U / 5U)); + } + cycle_deadline_us_ = ume_cfg_.cycle_deadline_us(); + + if (ume_cfg_.feedback_watchdog_ms() == 0U) { + ume_cfg_.set_feedback_watchdog_ms(20U); + } + feedback_watchdog_ms_ = ume_cfg_.feedback_watchdog_ms(); + if (ume_cfg_.shutdown_timeout_ms() == 0U) { + ume_cfg_.set_shutdown_timeout_ms(50U); + } + shutdown_timeout_ms_ = ume_cfg_.shutdown_timeout_ms(); + + auto* can = ume_cfg_.mutable_can(); + if (!can->has_enable_fd()) { + can->set_enable_fd(true); + } + if (!can->has_bitrate_switch()) { + can->set_bitrate_switch(true); + } + if (!can->has_receive_own_messages()) { + can->set_receive_own_messages(false); + } + if (!can->has_receive_timeout_us() || + can->receive_timeout_us() == 0U) { + can->set_receive_timeout_us( + std::max(50U, cycle_deadline_us_ / 4U)); + } + if (!can->has_send_timeout_us() || + can->send_timeout_us() == 0U) { + can->set_send_timeout_us( + std::max(50U, cycle_deadline_us_ / 4U)); + } +} + +bool UmeRobotArm::buildModelAndChain_() +{ + if (id_.empty()) { + recordFault_("UME RobotArm id is empty"); + return false; + } + if (!cfg_.has_ume()) { + recordFault_("UME RobotArm backend config is missing"); + return false; + } + if (ume_cfg_.joints_size() != + static_cast(UmeArmSample::kDof)) { + recordFault_("one UME RobotArm must configure exactly eight joints"); + return false; + } + + model_.name = id_; + model_.manufacturer = "UME"; + model_.dof = UmeArmSample::kDof; + joint_specs_.clear(); + joint_specs_.reserve(UmeArmSample::kDof); + model_.joint_names.reserve(UmeArmSample::kDof); + model_.joint_limits.reserve(UmeArmSample::kDof); + + for (const auto& joint : ume_cfg_.joints()) { + if (joint.reported_motor_id() > 0x0FU || + joint.max_driver_temperature_raw() > 0xFFU || + joint.max_motor_temperature_raw() > 0xFFU || + std::any_of( + joint.healthy_feedback_status().begin(), + joint.healthy_feedback_status().end(), + [](const std::uint32_t status) { + return status > 0x0FU; + })) { + recordFault_( + "Damiao feedback identity/health values exceed protocol " + "field widths"); + return false; + } + DamiaoJointSpec spec; + spec.joint_name = joint.joint_name(); + spec.command_id = joint.command_id(); + spec.feedback_id = joint.feedback_id(); + spec.reported_motor_id = + static_cast(joint.reported_motor_id()); + spec.model = toDriverModel(joint.model()); + spec.direction = joint.direction(); + spec.zero_offset_rad = joint.zero_offset_rad(); + spec.joint_lower_rad = joint.joint_lower_rad(); + spec.joint_upper_rad = joint.joint_upper_rad(); + spec.max_velocity_rad_s = joint.max_velocity_rad_s(); + spec.max_torque_nm = joint.max_torque_nm(); + for (const auto status : joint.healthy_feedback_status()) { + if (status <= 0x0FU) { + spec.healthy_status_mask |= + static_cast(1U << status); + } + } + spec.max_driver_temperature_raw = + joint.max_driver_temperature_raw() <= 0xFFU + ? static_cast( + joint.max_driver_temperature_raw()) + : 0U; + spec.max_motor_temperature_raw = + joint.max_motor_temperature_raw() <= 0xFFU + ? static_cast( + joint.max_motor_temperature_raw()) + : 0U; + joint_specs_.push_back(spec); + + model_.joint_names.push_back(spec.joint_name); + JointLimit limit; + limit.lower = spec.joint_lower_rad; + limit.upper = spec.joint_upper_rad; + limit.max_velocity = spec.max_velocity_rad_s; + limit.max_torque = spec.max_torque_nm; + model_.joint_limits.push_back(limit); + } + + DamiaoChainOptions options; + options.is_fd = ume_cfg_.can().enable_fd(); + options.bitrate_switch = ume_cfg_.can().bitrate_switch(); + options.hardware_enabled = ume_cfg_.hardware_enabled(); + chain_ = std::make_unique( + canbus_, joint_specs_, options); + return true; +} + +bool UmeRobotArm::init() +{ + std::lock_guard lock(lifecycle_mutex_); + if (initialized_.load()) { + return true; + } + if (!chain_ || fault_latched_.load()) { + return false; + } + if (!ume_cfg_.can().enable_fd() || + !ume_cfg_.can().bitrate_switch()) { + recordFault_("UME requires SocketCAN-FD with bitrate switching"); + return false; + } + const auto control_frequency_hz = + ume_cfg_.control_frequency_hz(); + const auto configured_period_us = + control_frequency_hz == 0U + ? 0U + : 1000000U / control_frequency_hz; + if (control_frequency_hz < 50U || + control_frequency_hz > 2000U || + configured_period_us == 0U || + cycle_deadline_us_ > configured_period_us) { + recordFault_( + "UME control rate/deadline must be 50..2000 Hz with " + "cycle_deadline_us no greater than one period"); + return false; + } + if (feedback_watchdog_ms_ * 1000ULL < + static_cast(cycle_deadline_us_)) { + recordFault_( + "UME feedback watchdog is shorter than the cycle deadline"); + return false; + } + if (ume_cfg_.can().receive_own_messages()) { + recordFault_("UME must not receive its own CAN command frames"); + return false; + } + if (ume_cfg_.can().receive_timeout_us() > + cycle_deadline_us_) { + recordFault_( + "SocketCAN receive timeout exceeds the UME cycle deadline"); + return false; + } + if (ume_cfg_.can().send_timeout_us() > + cycle_deadline_us_) { + recordFault_( + "SocketCAN send timeout exceeds the UME cycle deadline"); + return false; + } + const auto minimum_shutdown_us = + static_cast(configured_period_us) + + static_cast(cycle_deadline_us_) + + 6ULL * ume_cfg_.can().send_timeout_us(); + if (static_cast(shutdown_timeout_ms_) * 1000ULL < + minimum_shutdown_us) { + recordFault_( + "UME shutdown timeout is shorter than the bounded loop and " + "zero/disable transport budget"); + return false; + } + + auto result = chain_->init(); + if (result.ok()) { + result = chain_->openPassive(); + } + if (!result.ok()) { + recordFault_(result.message); + return false; + } + + if (cfg_.kinematics().algorithm_case() != + config::ArmKinematicsConfig::ALGORITHM_NOT_SET) { + ik_solver_ = cmvr::IKSolverFactory::create(cfg_.kinematics()); + if (!ik_solver_ || !ik_solver_->init()) { + recordFault_("failed to initialize optional UME kinematics"); + chain_->stop(); + return false; + } + } + + initialized_.store(true); + CMVR_LOG(INFO) << "[UmeRobotArm] initialized passive arm '" << id_ + << "', hardware_enabled=" + << ume_cfg_.hardware_enabled(); + return true; +} + +bool UmeRobotArm::start() +{ + std::lock_guard lock(lifecycle_mutex_); + if (!initialized_.load() || fault_latched_.load()) { + return false; + } + if (running_.exchange(true)) { + return true; + } + try { + control_thread_ = std::thread(&UmeRobotArm::controlLoop_, this); + } catch (const std::exception& error) { + running_.store(false); + recordFault_(std::string("failed to start UME loop: ") + error.what()); + return false; + } + return true; +} + +bool UmeRobotArm::stop() +{ + std::lock_guard lock(lifecycle_mutex_); + const auto stop_started = std::chrono::steady_clock::now(); + running_.store(false); + powered_on_.store(false); + torque_mode_.store(false); + command_ready_.store(false); + Result disable_result = Result::success(); + if (chain_) { + disable_result = chain_->disable(); + } + if (control_thread_.joinable()) { + control_thread_.join(); + } + if (chain_) { + chain_->stop(); + } + initialized_.store(false); + const auto elapsed = + std::chrono::steady_clock::now() - stop_started; + if (!disable_result.ok()) { + recordFault_( + "UME shutdown could not enqueue every zero/disable frame: " + + disable_result.message); + return false; + } + if (elapsed > std::chrono::milliseconds(shutdown_timeout_ms_)) { + recordFault_("UME shutdown exceeded configured timeout"); + return false; + } + return true; +} + +DeviceHealthSnapshot UmeRobotArm::healthSnapshot() +{ + DeviceHealthSnapshot health; + if (fault_latched_.load() || + emergency_stopped_.load()) { + health.state = DeviceHealthState::Fault; + } else if (!initialized_.load()) { + health.state = DeviceHealthState::Unknown; + } else if (!running_.load()) { + health.state = DeviceHealthState::Degraded; + } else { + health.state = DeviceHealthState::Healthy; + } + std::lock_guard lock(status_mutex_); + health.error_message = last_error_; + return health; +} + +ArmState UmeRobotArm::getRobotState() const +{ + ArmState state; + state.timestamp = + static_cast(monotonicNowNs_()) / 1000000000.0; + state.robot_mode = getRobotMode(); + state.safety_mode = getSafetyMode(); + state.control_mode = getControlMode(); + state.connected = initialized_.load(); + state.powered_on = powered_on_.load(); + state.brake_released = powered_on_.load(); + state.moving = powered_on_.load(); + state.protective_stopped = protective_stopped_.load(); + state.emergency_stopped = emergency_stopped_.load(); + state.fault = fault_latched_.load(); + state.actual_joint_state = getJointState(); + state.actual_tcp_pose = getTcpPose(); + return state; +} + +JointGroupState UmeRobotArm::getJointState() const +{ + UmeArmSample sample; + readSample(sample); + JointGroupState state; + state.position.assign(sample.q.begin(), sample.q.end()); + state.velocity.assign(sample.dq.begin(), sample.dq.end()); + state.effort.assign( + sample.tau_measured.begin(), sample.tau_measured.end()); + state.sequence = sample.sequence; + state.sample_monotonic_ns = sample.sample_monotonic_ns; + const bool valid = sample.valid_mask == kAllJointsValid; + state.position_valid = valid; + state.velocity_valid = valid; + state.effort_valid = valid; + return state; +} + +Result UmeRobotArm::readSample(UmeArmSample& sample) const +{ + std::lock_guard lock(sample_mutex_); + sample = latest_sample_; + if (sample.valid_mask != kAllJointsValid) { + return Result::failure( + ArmErrorCode::RobotNotReady, + "UME joint feedback snapshot is not complete"); + } + return Result::success(); +} + +CartesianPose UmeRobotArm::getTcpPose(const FrameType frame) const +{ + (void)frame; + if (!ik_solver_) { + return {}; + } + const auto state = getJointState(); + if (!state.position_valid) { + return {}; + } + Eigen::Matrix4d transform = Eigen::Matrix4d::Identity(); + std::lock_guard lock(kinematics_mutex_); + if (!ik_solver_->fk(state.position, transform, true)) { + return {}; + } + return common::math::matrixToPose(transform); +} + +RobotMode UmeRobotArm::getRobotMode() const +{ + if (fault_latched_.load()) { + return RobotMode::Fault; + } + if (!initialized_.load()) { + return RobotMode::Disconnected; + } + if (powered_on_.load()) { + return RobotMode::Running; + } + if (!running_.load()) { + return RobotMode::Stopped; + } + return RobotMode::Idle; +} + +SafetyMode UmeRobotArm::getSafetyMode() const +{ + if (emergency_stopped_.load()) { + return SafetyMode::EmergencyStop; + } + if (protective_stopped_.load()) { + return SafetyMode::ProtectiveStop; + } + if (fault_latched_.load()) { + return SafetyMode::Fault; + } + return SafetyMode::Normal; +} + +ControlMode UmeRobotArm::getControlMode() const +{ + return torque_mode_.load() ? ControlMode::Torque : ControlMode::None; +} + +Result UmeRobotArm::torqueOn() +{ + std::lock_guard lock(lifecycle_mutex_); + if (!initialized_.load() || !running_.load() || !chain_) { + return Result::failure( + ArmErrorCode::RobotNotReady, + "UME arm is not initialized and running"); + } + if (fault_latched_.load() || + emergency_stopped_.load() || + protective_stopped_.load()) { + return Result::failure( + ArmErrorCode::RobotInFault, + "UME safety latch prevents torque-on"); + } + if (!torque_mode_.load() || !command_ready_.load()) { + return Result::failure( + ArmErrorCode::CommandRejected, + "start torque mode and publish a command before torque-on"); + } + const auto age_ns = + monotonicNowNs_() - command_time_ns_.load(); + if (age_ns < 0 || + age_ns > static_cast( + command_watchdog_ms_.load()) * 1000000LL) { + return Result::failure( + ArmErrorCode::Timeout, + "initial UME torque command is stale"); + } + + const auto result = chain_->arm( + std::chrono::steady_clock::now() + + std::chrono::microseconds(cycle_deadline_us_)); + if (!result.ok()) { + if (chain_->state() == DamiaoChainState::FaultLatched) { + recordFault_(result.message); + } + return result; + } + powered_on_.store(true); + return Result::success(); +} + +Result UmeRobotArm::torqueOff() +{ + std::lock_guard lock(lifecycle_mutex_); + powered_on_.store(false); + if (!chain_) { + return Result::success(); + } + const auto result = chain_->disable(); + if (!result.ok()) { + recordFault_( + "UME torque-off could not enqueue every zero/disable frame: " + + result.message); + } + return result; +} + +Result UmeRobotArm::requirePassive_(const std::string& operation) const +{ + if (!initialized_.load() || !chain_) { + return Result::failure( + ArmErrorCode::RobotNotReady, + operation + " requires an initialized UME arm"); + } + if (powered_on_.load()) { + return Result::failure( + ArmErrorCode::CommandRejected, + operation + " requires torque-off"); + } + return Result::success(); +} + +Result UmeRobotArm::calibrateZeroQ(const std::string& joint_name) +{ + std::lock_guard lock(lifecycle_mutex_); + const auto passive = requirePassive_("calibrateZeroQ"); + if (!passive.ok()) { + return passive; + } + const auto it = std::find( + model_.joint_names.begin(), model_.joint_names.end(), joint_name); + if (it == model_.joint_names.end()) { + return Result::failure( + ArmErrorCode::InvalidArgument, + "unknown UME joint: " + joint_name); + } + return chain_->setZero( + static_cast( + std::distance(model_.joint_names.begin(), it)), + std::chrono::steady_clock::now() + + std::chrono::microseconds(cycle_deadline_us_)); +} + +Result UmeRobotArm::emergencyStop() +{ + emergency_stopped_.store(true); + powered_on_.store(false); + Result stop_result = Result::success(); + if (chain_) { + stop_result = + chain_->latchFault("UME software emergency stop"); + } + recordFault_( + stop_result.ok() + ? "UME software emergency stop" + : "UME software emergency stop; zero/disable failed: " + + stop_result.message); + return stop_result; +} + +Result UmeRobotArm::protectiveStop() +{ + protective_stopped_.store(true); + powered_on_.store(false); + Result stop_result = Result::success(); + if (chain_) { + stop_result = + chain_->latchFault("UME protective stop"); + } + recordFault_( + stop_result.ok() + ? "UME protective stop" + : "UME protective stop; zero/disable failed: " + + stop_result.message); + return stop_result; +} + +Result UmeRobotArm::setSpeedScaling(const double scaling) +{ + (void)scaling; + return unsupported_("setSpeedScaling"); +} + +Result UmeRobotArm::moveJ( + const JointPositionCommand&, const MotionOptions&) +{ + return unsupported_("moveJ"); +} + +Result UmeRobotArm::speedJ( + const JointVelocityCommand&, double, double) +{ + return unsupported_("speedJ"); +} + +Result UmeRobotArm::stopJ(double) +{ + return unsupported_("stopJ"); +} + +Result UmeRobotArm::moveL( + const CartesianPose&, const MotionOptions&, FrameType) +{ + return unsupported_("moveL"); +} + +Result UmeRobotArm::speedL( + const CartesianVelocity&, double, double, FrameType) +{ + return unsupported_("speedL"); +} + +Result UmeRobotArm::stopL(std::optional) +{ + return unsupported_("stopL"); +} + +Result UmeRobotArm::stopMotion() +{ + return torqueOff(); +} + +Result UmeRobotArm::startServoMode(const ServoOptions&) +{ + return unsupported_("startServoMode"); +} + +Result UmeRobotArm::servoJ(const JointPositionCommand&) +{ + return unsupported_("servoJ"); +} + +Result UmeRobotArm::servoL(const CartesianPose&, FrameType) +{ + return unsupported_("servoL"); +} + +Result UmeRobotArm::servoSpeedJ(const JointVelocityCommand&) +{ + return unsupported_("servoSpeedJ"); +} + +Result UmeRobotArm::servoSpeedL( + const CartesianVelocity&, FrameType) +{ + return unsupported_("servoSpeedL"); +} + +Result UmeRobotArm::stopServoMode() +{ + return unsupported_("stopServoMode"); +} + +Result UmeRobotArm::startTorqueMode( + const TorqueServoOptions& options) +{ + if (!std::isfinite(options.period) || + options.period < 0.00025 || + options.period > 0.02 || + options.period * 1000000.0 < + static_cast(cycle_deadline_us_) || + options.command_watchdog_ms == 0U || + static_cast(options.command_watchdog_ms) * 0.001 < + options.period) { + return Result::failure( + ArmErrorCode::InvalidArgument, + "invalid UME torque servo timing options"); + } + std::lock_guard lock(lifecycle_mutex_); + if (!initialized_.load() || !running_.load()) { + return Result::failure( + ArmErrorCode::RobotNotReady, + "UME arm is not initialized and running"); + } + if (powered_on_.load()) { + return Result::failure( + ArmErrorCode::CommandRejected, + "cannot change UME torque timing while powered"); + } + loop_period_ns_.store(static_cast( + std::llround(options.period * 1000000000.0))); + command_watchdog_ms_.store(options.command_watchdog_ms); + torque_mode_.store(true); + command_ready_.store(false); + return Result::success(); +} + +Result UmeRobotArm::servoTorque( + const JointTorqueCommand& target) +{ + if (!target.validForModel(model_)) { + return Result::failure( + ArmErrorCode::InvalidDof, + "UME torque command must contain eight joints"); + } + if (!torque_mode_.load() || fault_latched_.load()) { + return Result::failure( + ArmErrorCode::RobotNotReady, + "UME torque mode is not ready"); + } + for (std::size_t i = 0; i < target.torque.size(); ++i) { + if (!std::isfinite(target.torque[i]) || + std::abs(target.torque[i]) > + joint_specs_[i].max_torque_nm) { + return Result::failure( + ArmErrorCode::OutOfJointLimit, + "UME torque command exceeds configured joint limits"); + } + } + { + std::lock_guard lock(command_mutex_); + std::copy( + target.torque.begin(), target.torque.end(), + latest_torque_command_.begin()); + } + command_time_ns_.store(monotonicNowNs_()); + command_sequence_.fetch_add(1U); + command_ready_.store(true); + return Result::success(); +} + +Result UmeRobotArm::stopTorqueMode() +{ + const auto result = torqueOff(); + torque_mode_.store(false); + command_ready_.store(false); + return result; +} + +Result UmeRobotArm::connect(const std::string&, int) +{ + return unsupported_("connect"); +} + +Result UmeRobotArm::disconnect() +{ + return unsupported_("disconnect"); +} + +Result UmeRobotArm::brakeRelease() +{ + return unsupported_("brakeRelease"); +} + +Result UmeRobotArm::shutdown() +{ + return stop() ? Result::success() + : Result::failure( + ArmErrorCode::CommandFailed, + "failed to stop UME arm"); +} + +Result UmeRobotArm::clearFault() +{ + std::lock_guard lock(lifecycle_mutex_); + const auto passive = requirePassive_("clearFault"); + if (!passive.ok()) { + return passive; + } + if (emergency_stopped_.load()) { + return Result::failure( + ArmErrorCode::RobotInEmergencyStop, + "restart is required after a UME emergency stop"); + } + const auto result = chain_->clearFault( + std::chrono::steady_clock::now() + + std::chrono::microseconds(cycle_deadline_us_)); + if (result.ok()) { + fault_latched_.store(false); + protective_stopped_.store(false); + std::lock_guard status_lock(status_mutex_); + last_error_.clear(); + } + return result; +} + +Result UmeRobotArm::unlockProtectiveStop() +{ + if (powered_on_.load()) { + return Result::failure( + ArmErrorCode::CommandRejected, + "torque-off is required before unlocking a protective stop"); + } + if (fault_latched_.load()) { + return Result::failure( + ArmErrorCode::RobotInFault, + "clear the UME actuator fault before unlocking"); + } + protective_stopped_.store(false); + return Result::success(); +} + +Result UmeRobotArm::loadProgram(const std::string&) +{ + return unsupported_("loadProgram"); +} + +Result UmeRobotArm::playProgram() +{ + return unsupported_("playProgram"); +} + +Result UmeRobotArm::pauseProgram() +{ + return unsupported_("pauseProgram"); +} + +Result UmeRobotArm::stopProgram() +{ + return unsupported_("stopProgram"); +} + +std::vector UmeRobotArm::ik( + const std::string& base_link, + const std::string& ee_link, + const CartesianPose& pose) +{ + (void)base_link; + (void)ee_link; + if (!ik_solver_) { + return {}; + } + auto seed = getJointState().position; + if (seed.size() != getDof()) { + seed.assign(getDof(), 0.0); + } + std::lock_guard lock(kinematics_mutex_); + ik_solver_->update_joints_state(seed); + if (!ik_solver_->ik( + common::math::poseToMatrix(pose), seed, true)) { + return {}; + } + return seed; +} + +CartesianPose UmeRobotArm::fk( + const std::string& base_link, + const std::string& ee_link) +{ + (void)base_link; + (void)ee_link; + return fk(true); +} + +CartesianPose UmeRobotArm::fk(const bool is_tcp) +{ + if (!ik_solver_) { + return {}; + } + const auto state = getJointState(); + if (!state.position_valid) { + return {}; + } + Eigen::Matrix4d transform = Eigen::Matrix4d::Identity(); + std::lock_guard lock(kinematics_mutex_); + if (!ik_solver_->fk(state.position, transform, is_tcp)) { + return {}; + } + return common::math::matrixToPose(transform); +} + +void UmeRobotArm::controlLoop_() noexcept +{ + std::array commands{}; + std::array feedback{}; + auto next_tick = std::chrono::steady_clock::now(); + + while (running_.load()) { + const auto period = + std::chrono::nanoseconds(loop_period_ns_.load()); + next_tick += period; + + if (powered_on_.load()) { + const auto now_ns = monotonicNowNs_(); + const auto command_age_ns = + now_ns - command_time_ns_.load(); + if (!command_ready_.load() || + command_age_ns < 0 || + command_age_ns > + static_cast( + command_watchdog_ms_.load()) * 1000000LL) { + powered_on_.store(false); + Result stop_result = Result::success(); + if (chain_) { + stop_result = chain_->latchFault( + "UME torque command watchdog expired"); + } + recordFault_( + stop_result.ok() + ? "UME torque command watchdog expired" + : "UME torque command watchdog expired; " + "zero/disable failed: " + + stop_result.message); + } else { + { + std::lock_guard lock(command_mutex_); + for (std::size_t i = 0; i < commands.size(); ++i) { + commands[i] = {}; + commands[i].tau_ff_nm = + latest_torque_command_[i]; + } + } + const auto cycle_start = + std::chrono::steady_clock::now(); + const auto result = chain_->exchange( + commands.data(), commands.size(), + feedback.data(), feedback.size(), + cycle_start + + std::chrono::microseconds(cycle_deadline_us_)); + if (!result.ok()) { + powered_on_.store(false); + recordFault_(result.message); + } else { + const auto snapshot_now_ns = monotonicNowNs_(); + UmeArmSample sample; + sample.sample_monotonic_ns = snapshot_now_ns; + bool feedback_fresh = true; + for (std::size_t i = 0; i < feedback.size(); ++i) { + sample.q[i] = feedback[i].q_rad; + sample.dq[i] = feedback[i].dq_rad_s; + sample.tau_measured[i] = feedback[i].tau_nm; + sample.motor_rx_time_ns[i] = + feedback[i].rx_monotonic_ns; + if (feedback[i].valid) { + sample.valid_mask |= + static_cast(1U << i); + } + const auto age = + snapshot_now_ns - + feedback[i].rx_monotonic_ns; + if (!feedback[i].valid || + feedback[i].rx_monotonic_ns <= 0 || + age < 0 || + age > static_cast( + feedback_watchdog_ms_) * + 1000000LL) { + feedback_fresh = false; + } + } + if (!feedback_fresh || + sample.valid_mask != kAllJointsValid) { + powered_on_.store(false); + const auto stop_result = chain_->latchFault( + "UME feedback watchdog expired"); + recordFault_( + stop_result.ok() + ? "UME feedback watchdog expired" + : "UME feedback watchdog expired; " + "zero/disable failed: " + + stop_result.message); + } else { + std::lock_guard lock(sample_mutex_); + sample.sequence = + latest_sample_.sequence + 1U; + latest_sample_ = sample; + } + } + } + } + + const auto now = std::chrono::steady_clock::now(); + if (next_tick <= now) { + next_tick = now; + } else { + std::this_thread::sleep_until(next_tick); + } + } +} + +void UmeRobotArm::recordFault_( + const std::string& message) noexcept +{ + fault_latched_.store(true); + powered_on_.store(false); + try { + std::lock_guard lock(status_mutex_); + last_error_ = message; + } catch (...) { + // Health reporting is best effort; safety latches are already set. + } +} + +Result UmeRobotArm::unsupported_( + const std::string& operation) +{ + return Result::failure( + ArmErrorCode::UnsupportedCommand, + "UmeRobotArm does not support " + operation); +} + +std::int64_t UmeRobotArm::monotonicNowNs_() noexcept +{ + return std::chrono::duration_cast( + std::chrono::steady_clock::now().time_since_epoch()) + .count(); +} + +} // namespace cmvr::device diff --git a/cmvr-es/devices/arm/ume_robot_arm/tests/damiao_can_fd_chain_test.cpp b/cmvr-es/devices/arm/ume_robot_arm/tests/damiao_can_fd_chain_test.cpp new file mode 100644 index 00000000..782ce46c --- /dev/null +++ b/cmvr-es/devices/arm/ume_robot_arm/tests/damiao_can_fd_chain_test.cpp @@ -0,0 +1,407 @@ +#include "arm/ume_robot_arm/include/damiao_can_fd_chain.h" + +#include +#include +#include +#include +#include +#include +#include + +#include + +#include "canbus/abstract_canbus.h" + +namespace cmvr::device { +namespace { + +class FakeCanbus final : public AbstractCanbus { +public: + std::string typeName() const override { return "FakeCanbus"; } + bool init() override + { + initialized = true; + return init_result; + } + bool start() override + { + started = start_result; + is_started_ = started; + return started; + } + bool stop() override + { + stopped = true; + started = false; + is_started_ = false; + return true; + } + + msgs::ErrorCode send(const std::vector& frames, + int32_t* frame_num) override + { + if (!started || !frame_num || + *frame_num != static_cast(frames.size())) { + return msgs::ErrorCode::CAN_CLIENT_ERROR_SEND_FAILED; + } + if (!send_result) { + return msgs::ErrorCode::CAN_CLIENT_ERROR_SEND_FAILED; + } + sent_batches.push_back(frames); + if (!scheduled_replies.empty()) { + for (const auto& reply : scheduled_replies.front()) { + replies.push_back(reply); + } + scheduled_replies.pop_front(); + } + return msgs::ErrorCode::OK; + } + + msgs::ErrorCode receive(std::vector* frames, + int32_t* frame_num) override + { + if (!started || !frames || !frame_num || replies.empty()) { + return msgs::ErrorCode::CAN_CLIENT_ERROR_RECV_FAILED; + } + frames->clear(); + frames->push_back(replies.front()); + replies.pop_front(); + *frame_num = 1; + return msgs::ErrorCode::OK; + } + + bool discardPendingFrames() override + { + ++drain_calls; + if (drain_delay > std::chrono::microseconds::zero()) { + std::this_thread::sleep_for(drain_delay); + } + replies.clear(); + return drain_result; + } + + std::string getErrorString(int32_t) override { return {}; } + + void enqueueReplies(std::vector batch) + { + scheduled_replies.push_back(std::move(batch)); + } + + bool init_result{true}; + bool start_result{true}; + bool initialized{false}; + bool started{false}; + bool stopped{false}; + bool drain_result{true}; + bool send_result{true}; + std::size_t drain_calls{0}; + std::chrono::microseconds drain_delay{0}; + std::vector> sent_batches; + std::deque replies; + std::deque> scheduled_replies; +}; + +DamiaoJointSpec joint(std::string name, + std::uint32_t command_id, + std::uint32_t feedback_id, + std::uint8_t reported_id, + int direction = 1) +{ + DamiaoJointSpec spec; + spec.joint_name = std::move(name); + spec.command_id = command_id; + spec.feedback_id = feedback_id; + spec.reported_motor_id = reported_id; + spec.model = DamiaoMotorModel::DM4310; + spec.direction = direction; + spec.zero_offset_rad = direction == 1 ? 0.1 : -0.2; + spec.joint_lower_rad = -2.0; + spec.joint_upper_rad = 2.0; + spec.max_velocity_rad_s = 3.0; + spec.max_torque_nm = 2.0; + spec.healthy_status_mask = 1U << 0U; + spec.max_driver_temperature_raw = 80U; + spec.max_motor_temperature_raw = 90U; + return spec; +} + +CanFrame feedback(std::uint32_t id, + std::uint8_t reported_id, + std::uint8_t status = 0U) +{ + CanFrame frame; + frame.id = id; + frame.len = 8; + frame.is_fd = true; + frame.bitrate_switch = true; + frame.rx_monotonic_ns = 100; + frame.data[0] = + static_cast((status << 4U) | reported_id); + frame.data[1] = 0x80; + frame.data[2] = 0x00; + frame.data[3] = 0x80; + frame.data[4] = 0x08; + frame.data[5] = 0x00; + frame.data[6] = 30U; + frame.data[7] = 35U; + return frame; +} + +std::chrono::steady_clock::time_point soon() +{ + return std::chrono::steady_clock::now() + + std::chrono::milliseconds(20); +} + +std::size_t countLifecycleByte( + const std::vector>& batches, + const std::uint8_t value) +{ + std::size_t count = 0; + for (const auto& batch : batches) { + for (const auto& frame : batch) { + if (frame.len == 8 && + frame.data[0] == 0xFF && + frame.data[7] == value) { + ++count; + } + } + } + return count; +} + +TEST(DamiaoCanFdChainTest, PassiveOpenNeverEnablesHardware) +{ + auto bus = std::make_shared(); + DamiaoCanFdChain chain( + bus, {joint("J1", 1, 0x11, 1)}, + DamiaoChainOptions{true, true, false}); + + ASSERT_TRUE(chain.init().ok()); + ASSERT_TRUE(chain.openPassive().ok()); + EXPECT_EQ(chain.state(), DamiaoChainState::Passive); + EXPECT_TRUE(bus->sent_batches.empty()); + + const auto arm_result = chain.arm(soon()); + EXPECT_FALSE(arm_result.ok()); + EXPECT_EQ(arm_result.code, ArmErrorCode::CommandRejected); + EXPECT_TRUE(bus->sent_batches.empty()); +} + +TEST(DamiaoCanFdChainTest, ExplicitArmAndExchangeUseUniqueConfiguredFeedback) +{ + auto bus = std::make_shared(); + DamiaoCanFdChain chain( + bus, + {joint("J1", 1, 0x11, 1), + joint("J2", 2, 0x12, 2, -1)}, + DamiaoChainOptions{true, true, true}); + ASSERT_TRUE(chain.init().ok()); + ASSERT_TRUE(chain.openPassive().ok()); + + // A stale invalid frame is already queued before this request. The drain + // must remove it; only replies generated by the subsequent send may be + // accepted. + bus->replies.push_back(feedback(0x11, 1, 2)); + bus->enqueueReplies({ + feedback(0x12, 2), + feedback(0x11, 1), + }); + ASSERT_TRUE(chain.arm(soon()).ok()); + EXPECT_EQ(chain.state(), DamiaoChainState::Armed); + EXPECT_EQ(countLifecycleByte(bus->sent_batches, 0xFC), 2U); + + bus->enqueueReplies({ + feedback(0x11, 1), + feedback(0x12, 2), + }); + DamiaoMitCommand commands[2]{}; + commands[0].tau_ff_nm = 1.0; + commands[1].tau_ff_nm = -1.0; + DamiaoJointFeedback states[2]{}; + ASSERT_TRUE(chain.exchange( + commands, 2, states, 2, soon()).ok()); + EXPECT_EQ(chain.state(), DamiaoChainState::Active); + EXPECT_TRUE(states[0].valid); + EXPECT_TRUE(states[1].valid); + // J2 has direction=-1 and offset=-0.2. + EXPECT_NEAR(states[1].q_rad, -0.2003814697265625, 1e-12); + EXPECT_NEAR(states[1].dq_rad_s, -0.0146484375, 1e-12); + EXPECT_NEAR(states[1].tau_nm, -0.0048828125, 1e-12); +} + +TEST(DamiaoCanFdChainTest, MissedFeedbackLatchesFaultAndNeverReenables) +{ + auto bus = std::make_shared(); + DamiaoCanFdChain chain( + bus, {joint("J1", 1, 0x11, 1)}, + DamiaoChainOptions{true, true, true}); + ASSERT_TRUE(chain.init().ok()); + ASSERT_TRUE(chain.openPassive().ok()); + bus->enqueueReplies({feedback(0x11, 1)}); + ASSERT_TRUE(chain.arm(soon()).ok()); + + DamiaoMitCommand command; + DamiaoJointFeedback state; + const auto result = chain.exchange( + &command, 1, &state, 1, + std::chrono::steady_clock::now() + + std::chrono::milliseconds(1)); + EXPECT_FALSE(result.ok()); + EXPECT_EQ(result.code, ArmErrorCode::Timeout); + EXPECT_EQ(chain.state(), DamiaoChainState::FaultLatched); + EXPECT_EQ(countLifecycleByte(bus->sent_batches, 0xFC), 1U); + EXPECT_GE(countLifecycleByte(bus->sent_batches, 0xFD), 1U); + + // Clearing the fault is explicit and leaves the chain passive. + bus->enqueueReplies({feedback(0x11, 1)}); + ASSERT_TRUE(chain.clearFault(soon()).ok()); + EXPECT_EQ(chain.state(), DamiaoChainState::Passive); + EXPECT_EQ(countLifecycleByte(bus->sent_batches, 0xFC), 1U); +} + +TEST(DamiaoCanFdChainTest, DuplicateFeedbackCannotSatisfyAGroupCycle) +{ + auto bus = std::make_shared(); + DamiaoCanFdChain chain( + bus, + {joint("J1", 1, 0x11, 1), + joint("J2", 2, 0x12, 2)}, + DamiaoChainOptions{true, true, true}); + ASSERT_TRUE(chain.init().ok()); + ASSERT_TRUE(chain.openPassive().ok()); + bus->enqueueReplies({ + feedback(0x11, 1), + feedback(0x12, 2), + }); + ASSERT_TRUE(chain.arm(soon()).ok()); + + bus->enqueueReplies({ + feedback(0x11, 1), + feedback(0x11, 1), + }); + DamiaoMitCommand commands[2]{}; + DamiaoJointFeedback states[2]{}; + EXPECT_FALSE(chain.exchange( + commands, 2, states, 2, + std::chrono::steady_clock::now() + + std::chrono::milliseconds(1)).ok()); + EXPECT_EQ(chain.state(), DamiaoChainState::FaultLatched); + EXPECT_EQ(chain.statistics().duplicate_feedback, 1U); +} + +TEST(DamiaoCanFdChainTest, RejectsUnreviewedStatusAndClassicFrame) +{ + auto bus = std::make_shared(); + DamiaoCanFdChain chain( + bus, {joint("J1", 1, 0x11, 1)}, + DamiaoChainOptions{true, true, true}); + ASSERT_TRUE(chain.init().ok()); + ASSERT_TRUE(chain.openPassive().ok()); + bus->enqueueReplies({feedback(0x11, 1)}); + ASSERT_TRUE(chain.arm(soon()).ok()); + + bus->enqueueReplies({feedback(0x11, 1, 2)}); + DamiaoMitCommand command; + DamiaoJointFeedback state; + EXPECT_FALSE(chain.exchange( + &command, 1, &state, 1, soon()).ok()); + EXPECT_EQ(chain.state(), DamiaoChainState::FaultLatched); + + auto second_bus = std::make_shared(); + DamiaoCanFdChain second( + second_bus, {joint("J1", 1, 0x11, 1)}, + DamiaoChainOptions{true, true, true}); + ASSERT_TRUE(second.init().ok()); + ASSERT_TRUE(second.openPassive().ok()); + auto classic = feedback(0x11, 1); + classic.is_fd = false; + classic.bitrate_switch = false; + second_bus->enqueueReplies({classic}); + EXPECT_FALSE(second.arm(soon()).ok()); + EXPECT_EQ(second.state(), DamiaoChainState::FaultLatched); + + auto third_bus = std::make_shared(); + DamiaoCanFdChain third( + third_bus, {joint("J1", 1, 0x11, 1)}, + DamiaoChainOptions{true, true, true}); + ASSERT_TRUE(third.init().ok()); + ASSERT_TRUE(third.openPassive().ok()); + auto error_passive = feedback(0x11, 1); + error_passive.error_state_indicator = true; + third_bus->enqueueReplies({error_passive}); + EXPECT_FALSE(third.arm(soon()).ok()); + EXPECT_EQ(third.state(), DamiaoChainState::FaultLatched); +} + +TEST(DamiaoCanFdChainTest, HardwareEnableRequiresReviewedHealthContract) +{ + auto bus = std::make_shared(); + auto unreviewed = joint("J1", 1, 0x11, 1); + unreviewed.healthy_status_mask = 0U; + DamiaoCanFdChain chain( + bus, {unreviewed}, + DamiaoChainOptions{true, true, true}); + + const auto result = chain.init(); + EXPECT_FALSE(result.ok()); + EXPECT_EQ(result.code, ArmErrorCode::InvalidArgument); + EXPECT_FALSE(bus->initialized); +} + +TEST(DamiaoCanFdChainTest, DisableReportsUnconfirmedSafetyFrames) +{ + auto bus = std::make_shared(); + DamiaoCanFdChain chain( + bus, {joint("J1", 1, 0x11, 1)}, + DamiaoChainOptions{true, true, true}); + ASSERT_TRUE(chain.init().ok()); + ASSERT_TRUE(chain.openPassive().ok()); + bus->enqueueReplies({feedback(0x11, 1)}); + ASSERT_TRUE(chain.arm(soon()).ok()); + + bus->send_result = false; + const auto result = chain.disable(); + EXPECT_FALSE(result.ok()); + EXPECT_EQ(result.code, ArmErrorCode::CommandFailed); + EXPECT_EQ(chain.state(), DamiaoChainState::FaultLatched); +} + +TEST(DamiaoCanFdChainTest, ExpiredDeadlineAfterDrainNeverCommitsEnable) +{ + auto bus = std::make_shared(); + DamiaoCanFdChain chain( + bus, {joint("J1", 1, 0x11, 1)}, + DamiaoChainOptions{true, true, true}); + ASSERT_TRUE(chain.init().ok()); + ASSERT_TRUE(chain.openPassive().ok()); + + bus->drain_delay = std::chrono::milliseconds(3); + bus->enqueueReplies({feedback(0x11, 1)}); + const auto result = chain.arm( + std::chrono::steady_clock::now() + + std::chrono::milliseconds(1)); + + EXPECT_FALSE(result.ok()); + EXPECT_EQ(result.code, ArmErrorCode::Timeout); + EXPECT_EQ(chain.state(), DamiaoChainState::FaultLatched); + EXPECT_EQ(countLifecycleByte(bus->sent_batches, 0xFC), 0U); + EXPECT_GE(countLifecycleByte(bus->sent_batches, 0xFD), 1U); +} + +TEST(DamiaoCanFdChainTest, ConfigurationRejectsAmbiguousMappings) +{ + auto bus = std::make_shared(); + DamiaoCanFdChain chain( + bus, + {joint("J1", 1, 0x11, 1), + joint("J1", 2, 0x12, 2)}, + DamiaoChainOptions{true, true, false}); + const auto result = chain.init(); + EXPECT_FALSE(result.ok()); + EXPECT_EQ(result.code, ArmErrorCode::InvalidArgument); + EXPECT_FALSE(bus->initialized); +} + +} // namespace +} // namespace cmvr::device diff --git a/cmvr-es/devices/arm/ume_robot_arm/tests/damiao_mit_codec_test.cpp b/cmvr-es/devices/arm/ume_robot_arm/tests/damiao_mit_codec_test.cpp new file mode 100644 index 00000000..5a3bcef1 --- /dev/null +++ b/cmvr-es/devices/arm/ume_robot_arm/tests/damiao_mit_codec_test.cpp @@ -0,0 +1,150 @@ +#include "arm/ume_robot_arm/include/damiao_mit_codec.h" + +#include +#include +#include + +#include + +namespace cmvr::device { +namespace { + +void expectPayload(const CanFrame& frame, + const std::array& expected) +{ + ASSERT_EQ(frame.len, expected.size()); + for (std::size_t i = 0; i < expected.size(); ++i) { + EXPECT_EQ(frame.data[i], expected[i]) << "byte " << i; + } +} + +TEST(DamiaoMitCodecTest, MatchesLegacyPythonGoldenVectors) +{ + CanFrame frame; + DamiaoMitCommand zero; + auto result = DamiaoMitCodec::encodeMit( + 1, DamiaoMotorModel::DM4310, zero, true, true, frame); + ASSERT_TRUE(result); + EXPECT_EQ(result.saturation_mask, DAMIAO_SATURATION_NONE); + EXPECT_TRUE(frame.is_fd); + EXPECT_TRUE(frame.bitrate_switch); + expectPayload(frame, {0x7F, 0xFF, 0x7F, 0xF0, + 0x00, 0x00, 0x07, 0xFF}); + + DamiaoMitCommand nontrivial; + nontrivial.kp = 100.0; + nontrivial.kd = 1.0; + nontrivial.q_rad = 1.25; + nontrivial.dq_rad_s = -2.5; + nontrivial.tau_ff_nm = 3.0; + result = DamiaoMitCodec::encodeMit( + 1, DamiaoMotorModel::DM4310, nontrivial, false, false, frame); + ASSERT_TRUE(result); + expectPayload(frame, {0x8C, 0xCC, 0x75, 0x43, + 0x33, 0x33, 0x3A, 0x65}); +} + +TEST(DamiaoMitCodecTest, ReportsProtocolSaturationWithoutHidingIt) +{ + DamiaoMitCommand command; + command.q_rad = 100.0; + command.dq_rad_s = -100.0; + command.kp = 600.0; + command.kd = -1.0; + command.tau_ff_nm = 100.0; + CanFrame frame; + const auto result = DamiaoMitCodec::encodeMit( + 2, DamiaoMotorModel::DM4310, command, false, false, frame); + ASSERT_TRUE(result); + EXPECT_EQ( + result.saturation_mask, + DAMIAO_SATURATION_Q | DAMIAO_SATURATION_DQ | + DAMIAO_SATURATION_KP | DAMIAO_SATURATION_KD | + DAMIAO_SATURATION_TAU); +} + +TEST(DamiaoMitCodecTest, RejectsNonFiniteInput) +{ + DamiaoMitCommand command; + command.tau_ff_nm = std::numeric_limits::quiet_NaN(); + CanFrame frame; + const auto result = DamiaoMitCodec::encodeMit( + 1, DamiaoMotorModel::DM4310, command, false, false, frame); + EXPECT_FALSE(result); + EXPECT_EQ(result.error, DamiaoCodecError::NonFiniteInput); +} + +TEST(DamiaoMitCodecTest, EncodesLifecycleFramesWithoutEnablingImplicitly) +{ + CanFrame frame; + ASSERT_EQ(DamiaoMitCodec::encodeMode( + 3, DamiaoMode::Enable, true, true, frame), + DamiaoCodecError::None); + expectPayload(frame, {0xFF, 0xFF, 0xFF, 0xFF, + 0xFF, 0xFF, 0xFF, 0xFC}); + + ASSERT_EQ(DamiaoMitCodec::encodeMode( + 3, DamiaoMode::Disable, true, true, frame), + DamiaoCodecError::None); + EXPECT_EQ(frame.data[7], 0xFD); + ASSERT_EQ(DamiaoMitCodec::encodeMode( + 3, DamiaoMode::SetZero, true, true, frame), + DamiaoCodecError::None); + EXPECT_EQ(frame.data[7], 0xFE); + ASSERT_EQ(DamiaoMitCodec::encodeMode( + 3, DamiaoMode::ClearFault, true, true, frame), + DamiaoCodecError::None); + EXPECT_EQ(frame.data[7], 0xFB); +} + +TEST(DamiaoMitCodecTest, DecodesLegacyFeedbackAndRequiresConfiguredId) +{ + CanFrame frame; + frame.id = 0x11; + frame.len = 8; + frame.is_fd = true; + frame.bitrate_switch = true; + frame.rx_monotonic_ns = 1234567; + frame.data[0] = 0xA1; + frame.data[1] = 0x80; + frame.data[2] = 0x00; + frame.data[3] = 0x80; + frame.data[4] = 0x08; + frame.data[5] = 0x00; + frame.data[6] = 40; + frame.data[7] = 41; + + DamiaoJointFeedback feedback; + EXPECT_EQ(DamiaoMitCodec::decodeFeedback( + frame, 0x12, DamiaoMotorModel::DM4310, feedback), + DamiaoCodecError::UnexpectedFeedbackId); + EXPECT_FALSE(feedback.valid); + + ASSERT_EQ(DamiaoMitCodec::decodeFeedback( + frame, 0x11, DamiaoMotorModel::DM4310, feedback), + DamiaoCodecError::None); + EXPECT_TRUE(feedback.valid); + EXPECT_EQ(feedback.reported_motor_id, 1); + EXPECT_EQ(feedback.status, 0x0A); + EXPECT_EQ(feedback.driver_temperature_raw, 40); + EXPECT_EQ(feedback.motor_temperature_raw, 41); + EXPECT_EQ(feedback.rx_monotonic_ns, 1234567); + EXPECT_NEAR(feedback.q_rad, 0.0003814697265625, 1e-12); + EXPECT_NEAR(feedback.dq_rad_s, 0.0146484375, 1e-12); + EXPECT_NEAR(feedback.tau_nm, 0.0048828125, 1e-12); +} + +TEST(DamiaoMitCodecTest, ContainsAllLegacyMotorRanges) +{ + EXPECT_DOUBLE_EQ( + DamiaoMitCodec::limitsFor(DamiaoMotorModel::DM8009).tau_max_nm, + 54.0); + EXPECT_DOUBLE_EQ( + DamiaoMitCodec::limitsFor(DamiaoMotorModel::DMH3510).dq_max_rad_s, + 280.0); + EXPECT_FALSE( + DamiaoMitCodec::limitsFor(DamiaoMotorModel::Unknown).valid()); +} + +} // namespace +} // namespace cmvr::device diff --git a/cmvr-es/devices/arm/ume_robot_arm/tests/ume_robot_arm_test.cpp b/cmvr-es/devices/arm/ume_robot_arm/tests/ume_robot_arm_test.cpp new file mode 100644 index 00000000..e7241ebc --- /dev/null +++ b/cmvr-es/devices/arm/ume_robot_arm/tests/ume_robot_arm_test.cpp @@ -0,0 +1,338 @@ +#include "arm/ume_robot_arm/include/ume_robot_arm.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include "canbus/abstract_canbus.h" +#include "common/io/proto_file_io.h" + +#ifndef CMVR_UME_ARM_CONFIG_PATH +#define CMVR_UME_ARM_CONFIG_PATH "" +#endif + +namespace cmvr::device { +namespace { + +class LoopbackDamiaoBus final : public AbstractCanbus { +public: + std::string typeName() const override { return "LoopbackDamiaoBus"; } + bool init() override + { + std::lock_guard lock(mutex); + initialized = true; + return true; + } + bool start() override + { + std::lock_guard lock(mutex); + started = true; + is_started_ = true; + return true; + } + bool stop() override + { + std::lock_guard lock(mutex); + started = false; + is_started_ = false; + return true; + } + + msgs::ErrorCode send( + const std::vector& frames, + int32_t* frame_num) override + { + std::lock_guard lock(mutex); + if (!started || !frame_num || + *frame_num != static_cast(frames.size())) { + return msgs::ErrorCode::CAN_CLIENT_ERROR_SEND_FAILED; + } + if (!send_result) { + return msgs::ErrorCode::CAN_CLIENT_ERROR_SEND_FAILED; + } + sent_batches.push_back(frames); + for (const auto& frame : frames) { + if (frame.id < 1U || frame.id > 8U) { + continue; + } + CanFrame reply; + reply.id = 0x10U + frame.id; + reply.len = 8U; + reply.is_fd = true; + reply.bitrate_switch = true; + reply.rx_monotonic_ns = + std::chrono::duration_cast( + std::chrono::steady_clock::now().time_since_epoch()) + .count(); + reply.data[0] = static_cast(frame.id); + reply.data[1] = 0x80U; + reply.data[2] = 0x00U; + reply.data[3] = 0x80U; + reply.data[4] = 0x08U; + reply.data[5] = 0x00U; + replies.push_back(reply); + } + return msgs::ErrorCode::OK; + } + + msgs::ErrorCode receive( + std::vector* frames, + int32_t* frame_num) override + { + std::lock_guard lock(mutex); + if (!started || !frames || !frame_num || replies.empty()) { + return msgs::ErrorCode::CAN_CLIENT_ERROR_RECV_FAILED; + } + frames->clear(); + frames->push_back(replies.front()); + replies.pop_front(); + *frame_num = 1; + return msgs::ErrorCode::OK; + } + + bool discardPendingFrames() override + { + std::lock_guard lock(mutex); + replies.clear(); + return started; + } + + std::string getErrorString(int32_t) override { return {}; } + + void setSendResult(const bool result) + { + std::lock_guard lock(mutex); + send_result = result; + } + + std::size_t lifecycleCount(const std::uint8_t byte) const + { + std::lock_guard lock(mutex); + std::size_t count = 0; + for (const auto& batch : sent_batches) { + for (const auto& frame : batch) { + if (frame.len == 8U && + frame.data[0] == 0xFFU && + frame.data[7] == byte) { + ++count; + } + } + } + return count; + } + + bool initialized{false}; + bool started{false}; + bool send_result{true}; + std::deque replies; + std::vector> sent_batches; + mutable std::mutex mutex; +}; + +config::RobotArmConfig configFor(const bool hardware_enabled) +{ + config::RobotArmConfig cfg; + cfg.set_id("ume_right"); + auto* ume = cfg.mutable_ume(); + ume->set_hardware_enabled(hardware_enabled); + ume->set_control_frequency_hz(800U); + ume->set_cycle_deadline_us(1000U); + ume->set_feedback_watchdog_ms(20U); + auto* can = ume->mutable_can(); + can->set_interface_name("fake-can"); + can->set_enable_fd(true); + can->set_bitrate_switch(true); + can->set_send_timeout_us(100U); + can->set_receive_timeout_us(100U); + can->set_receive_own_messages(false); + for (std::uint32_t i = 1; i <= 8U; ++i) { + auto* joint = ume->add_joints(); + joint->set_joint_name("RJ" + std::to_string(i)); + joint->set_command_id(i); + joint->set_feedback_id(0x10U + i); + joint->set_reported_motor_id(i); + joint->set_model(config::DAMIAO_MOTOR_MODEL_DM4310); + joint->set_direction(1); + joint->set_joint_lower_rad(-2.0); + joint->set_joint_upper_rad(2.0); + joint->set_max_velocity_rad_s(3.0); + joint->set_max_torque_nm(2.0); + joint->add_healthy_feedback_status(0U); + joint->set_max_driver_temperature_raw(80U); + joint->set_max_motor_temperature_raw(90U); + } + return cfg; +} + +bool waitUntil( + const std::function& predicate, + const std::chrono::milliseconds timeout) +{ + const auto deadline = std::chrono::steady_clock::now() + timeout; + while (std::chrono::steady_clock::now() < deadline) { + if (predicate()) { + return true; + } + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + } + return predicate(); +} + +TEST(UmeRobotArmTest, LifecycleIsPassiveUntilExplicitFreshTorqueCommand) +{ + auto bus = std::make_shared(); + UmeRobotArm arm(configFor(true), bus); + + ASSERT_TRUE(arm.init()); + EXPECT_EQ(bus->lifecycleCount(0xFCU), 0U); + ASSERT_TRUE(arm.start()); + EXPECT_EQ(bus->lifecycleCount(0xFCU), 0U); + + TorqueServoOptions options; + options.period = 0.00125; + options.command_watchdog_ms = 100U; + ASSERT_TRUE(arm.startTorqueMode(options).ok()); + JointTorqueCommand command; + command.torque.assign(8U, 0.0); + ASSERT_TRUE(arm.servoTorque(command).ok()); + ASSERT_TRUE(arm.torqueOn().ok()); + + ASSERT_TRUE(waitUntil( + [&arm] { return arm.getJointState().sequence > 0U; }, + std::chrono::milliseconds(30))); + const auto state = arm.getJointState(); + EXPECT_TRUE(state.position_valid); + EXPECT_TRUE(state.velocity_valid); + EXPECT_TRUE(state.effort_valid); + EXPECT_EQ(state.position.size(), 8U); + EXPECT_EQ(bus->lifecycleCount(0xFCU), 8U); + EXPECT_EQ(arm.getControlMode(), ControlMode::Torque); + + EXPECT_TRUE(arm.stop()); + EXPECT_GE(bus->lifecycleCount(0xFDU), 8U); +} + +TEST(UmeRobotArmTest, StaleCommandLatchesFaultAndNeverReenables) +{ + auto bus = std::make_shared(); + UmeRobotArm arm(configFor(true), bus); + ASSERT_TRUE(arm.init()); + ASSERT_TRUE(arm.start()); + + TorqueServoOptions options; + options.period = 0.001; + options.command_watchdog_ms = 2U; + ASSERT_TRUE(arm.startTorqueMode(options).ok()); + JointTorqueCommand command; + command.torque.assign(8U, 0.0); + ASSERT_TRUE(arm.servoTorque(command).ok()); + ASSERT_TRUE(arm.torqueOn().ok()); + ASSERT_TRUE(waitUntil( + [&arm] { return arm.isFault(); }, + std::chrono::milliseconds(50))); + + EXPECT_FALSE(arm.busy()); + EXPECT_EQ(bus->lifecycleCount(0xFCU), 8U); + EXPECT_GE(bus->lifecycleCount(0xFDU), 8U); + EXPECT_EQ(arm.healthSnapshot().state, DeviceHealthState::Fault); +} + +TEST(UmeRobotArmTest, HardwareGateRejectsEnableWithoutWritingIt) +{ + auto bus = std::make_shared(); + UmeRobotArm arm(configFor(false), bus); + ASSERT_TRUE(arm.init()); + ASSERT_TRUE(arm.start()); + ASSERT_TRUE(arm.startTorqueMode(TorqueServoOptions{}).ok()); + JointTorqueCommand command; + command.torque.assign(8U, 0.0); + ASSERT_TRUE(arm.servoTorque(command).ok()); + + const auto result = arm.torqueOn(); + EXPECT_FALSE(result.ok()); + EXPECT_EQ(result.code, ArmErrorCode::CommandRejected); + EXPECT_EQ(bus->lifecycleCount(0xFCU), 0U); +} + +TEST(UmeRobotArmTest, PositionServoIsExplicitlyUnsupported) +{ + auto bus = std::make_shared(); + UmeRobotArm arm(configFor(false), bus); + JointPositionCommand command; + command.position.assign(8U, 0.0); + const auto result = arm.servoJ(command); + EXPECT_EQ(result.code, ArmErrorCode::UnsupportedCommand); +} + +TEST(UmeRobotArmTest, RejectsCycleDeadlineLongerThanControlPeriod) +{ + auto cfg = configFor(false); + cfg.mutable_ume()->set_cycle_deadline_us(2000U); + auto bus = std::make_shared(); + UmeRobotArm arm(cfg, bus); + + EXPECT_FALSE(arm.init()); + EXPECT_FALSE(bus->initialized); + EXPECT_EQ( + arm.healthSnapshot().state, + DeviceHealthState::Fault); +} + +TEST(UmeRobotArmTest, RejectsReportedMotorIdBeforeNarrowingConversion) +{ + auto cfg = configFor(false); + cfg.mutable_ume()->mutable_joints(0)->set_reported_motor_id(257U); + auto bus = std::make_shared(); + UmeRobotArm arm(cfg, bus); + + EXPECT_FALSE(arm.init()); + EXPECT_FALSE(bus->initialized); + EXPECT_EQ(arm.healthSnapshot().state, DeviceHealthState::Fault); +} + +TEST(UmeRobotArmTest, EmergencyStopReportsUnconfirmedDisable) +{ + auto bus = std::make_shared(); + UmeRobotArm arm(configFor(true), bus); + ASSERT_TRUE(arm.init()); + ASSERT_TRUE(arm.start()); + + bus->setSendResult(false); + const auto result = arm.emergencyStop(); + + EXPECT_FALSE(result.ok()); + EXPECT_EQ(result.code, ArmErrorCode::CommandFailed); + EXPECT_TRUE(arm.isEmergencyStopped()); + EXPECT_TRUE(arm.isFault()); + EXPECT_NE( + arm.healthSnapshot().error_message.find("zero/disable failed"), + std::string::npos); +} + +TEST(UmeRobotArmTest, CheckedInDualArmConfigParsesAndKeepsHardwareDisabled) +{ + config::ArmRootConfig root; + ASSERT_TRUE(ProtoMessageIo::getProtoFromAsciiFile( + CMVR_UME_ARM_CONFIG_PATH, &root)); + ASSERT_EQ(root.arm().robot_arms_size(), 2); + for (const auto& arm : root.arm().robot_arms()) { + ASSERT_TRUE(arm.has_ume()); + EXPECT_EQ(arm.ume().joints_size(), 8); + EXPECT_FALSE(arm.ume().hardware_enabled()); + EXPECT_TRUE(arm.ume().can().enable_fd()); + EXPECT_TRUE(arm.ume().can().bitrate_switch()); + EXPECT_GT(arm.ume().can().send_timeout_us(), 0U); + EXPECT_FALSE(arm.ume().can().receive_own_messages()); + } +} + +} // namespace +} // namespace cmvr::device diff --git a/cmvr-es/devices/canbus/CMakeLists.txt b/cmvr-es/devices/canbus/CMakeLists.txt index 76b2a460..0ce2cd40 100644 --- a/cmvr-es/devices/canbus/CMakeLists.txt +++ b/cmvr-es/devices/canbus/CMakeLists.txt @@ -31,6 +31,21 @@ target_link_libraries(socket_can_client_raw_test glog cmvr_es::proto ) +add_test( + NAME socket_can_client_raw_test + COMMAND socket_can_client_raw_test +) +set(_socket_can_client_raw_test_environment + "LD_LIBRARY_PATH=${CMVR_TEST_EXTERNAL_LIBRARY_PATH}" +) +if(CMVR_TEST_SYSTEM_LIBSTDCXX) + list(APPEND _socket_can_client_raw_test_environment + "LD_PRELOAD=${CMVR_TEST_SYSTEM_LIBSTDCXX}") +endif() +set_tests_properties(socket_can_client_raw_test PROPERTIES + TIMEOUT 10 + ENVIRONMENT "${_socket_can_client_raw_test_environment}" +) add_executable(protocol_data_test @@ -93,4 +108,3 @@ target_link_libraries(can_receiver_test glog cmvr_es::proto ) - diff --git a/cmvr-es/devices/canbus/abstract_canbus.h b/cmvr-es/devices/canbus/abstract_canbus.h index b8649125..dc683f4b 100644 --- a/cmvr-es/devices/canbus/abstract_canbus.h +++ b/cmvr-es/devices/canbus/abstract_canbus.h @@ -3,6 +3,14 @@ // #pragma once +#include +#include +#include +#include +#include +#include +#include + #include "../abstract_device.h" #include "cmvr/msgs/error_code.pb.h" #include "canbus/common/byte.h" @@ -14,20 +22,26 @@ namespace cmvr::device { */ struct CanFrame { /// Message id - uint32_t id; + uint32_t id{0}; /// Message length - uint8_t len; - /// Message content - uint8_t data[8]; - /// Time stamp - struct timeval timestamp; + uint8_t len{0}; + /// Message content. Classic CAN uses at most the first 8 bytes. + uint8_t data[64]{}; + bool is_extended_id{false}; + bool is_remote_frame{false}; + bool is_error_frame{false}; + bool is_fd{false}; + bool bitrate_switch{false}; + bool error_state_indicator{false}; + /// Local host receive time used for freshness and watchdog checks. + int64_t rx_monotonic_ns{0}; + /// Legacy wall-clock field retained for source compatibility. + struct timeval timestamp{0, 0}; /** * @brief Constructor */ - CanFrame() : id(0), len(0), timestamp{0} { - std::memset(data, 0, sizeof(data)); - } + CanFrame() = default; /** * @brief CanFrame string including essential information about the message. @@ -37,10 +51,15 @@ namespace cmvr::device { std::stringstream output_stream(""); output_stream << "id:0x" << Byte::byte_to_hex(id) << ",len:" << static_cast(len) << ",data:"; - for (uint8_t i = 0; i < len; ++i) { + const auto printable_len = + std::min(len, sizeof(data)); + for (std::size_t i = 0; i < printable_len; ++i) { output_stream << Byte::byte_to_hex(data[i]); } - output_stream << ","; + output_stream << ",fd:" << is_fd + << ",brs:" << bitrate_switch + << ",extended:" << is_extended_id + << ",error:" << is_error_frame << ","; return output_stream.str(); } }; @@ -67,6 +86,28 @@ namespace cmvr::device { virtual cmvr::msgs::ErrorCode send(const std::vector &frames, int32_t *const frame_num) = 0; + /** + * @brief Send messages without starting a batch after an absolute + * local deadline. + * + * Deadline-aware transports should override this method so their + * internal blocking budget is also capped by @p deadline. The default + * preserves source compatibility and at least rejects an already + * expired request before calling send(). + */ + virtual cmvr::msgs::ErrorCode sendUntil( + const std::vector& frames, + int32_t* const frame_num, + const std::chrono::steady_clock::time_point deadline) { + if (std::chrono::steady_clock::now() >= deadline) { + if (frame_num) { + *frame_num = 0; + } + return cmvr::msgs::ErrorCode::CAN_CLIENT_ERROR_SEND_FAILED; + } + return send(frames, frame_num); + } + /** * @brief Send a single message. * @param frames A single-element vector containing only one message. @@ -75,7 +116,9 @@ namespace cmvr::device { virtual cmvr::msgs::ErrorCode sendSingleFrame( const std::vector &frames) { if (frames.size() != 1U) { - CMVR_LOG(FATAL) << "frames size not equal to 1, actual frame size: " << frames.size(); + CMVR_LOG(ERROR) << "frames size not equal to 1, actual frame size: " + << frames.size(); + return cmvr::msgs::ErrorCode::CAN_CLIENT_ERROR_SEND_FAILED; } int32_t n = 1; return send(frames, &n); @@ -91,6 +134,17 @@ namespace cmvr::device { virtual cmvr::msgs::ErrorCode receive(std::vector *const frames, int32_t *const frame_num) = 0; + /** + * @brief Discard frames already queued by the transport. + * + * Command/response protocols without a sequence field can use this + * immediately before sending a new request to reduce the risk that a + * response from an older cycle is accepted as fresh. Implementations + * must keep this call bounded. The conservative default reports that + * the transport cannot provide this guarantee. + */ + virtual bool discardPendingFrames() { return false; } + /** * @brief Get the error string. * @param status The status to get the error string. diff --git a/cmvr-es/devices/canbus/can_client/socket/socket_can_client_raw.cc b/cmvr-es/devices/canbus/can_client/socket/socket_can_client_raw.cc index 21d9340e..61b7157f 100644 --- a/cmvr-es/devices/canbus/can_client/socket/socket_can_client_raw.cc +++ b/cmvr-es/devices/canbus/can_client/socket/socket_can_client_raw.cc @@ -12,9 +12,13 @@ #include "socket_can_client_raw.h" #include "absl/strings/str_cat.h" +#include +#include +#include +#include + namespace cmvr { namespace device { -#define CAN_ID_MASK 0x1FFFF800U // can_filter mask #define CAN_STANDARD_MAX_ID 0x7FFU using cmvr::msgs::ErrorCode; @@ -24,8 +28,25 @@ namespace cmvr { auto channel_id = cfg.channel_id(); port_ = static_cast(channel_id); interface_ = CANCardParameter::NATIVE; - - enable_can_err_check_ = false; + interface_name_ = + cfg.has_interface_name() && !cfg.interface_name().empty() + ? cfg.interface_name() + : cfg.dev_id(); + enable_fd_ = cfg.has_enable_fd() && cfg.enable_fd(); + default_bitrate_switch_ = + cfg.has_bitrate_switch() && cfg.bitrate_switch(); + receive_own_messages_ = + cfg.has_receive_own_messages() && cfg.receive_own_messages(); + receive_timeout_us_ = + cfg.has_receive_timeout_us() && cfg.receive_timeout_us() > 0 + ? cfg.receive_timeout_us() + : 100000U; + send_timeout_us_ = + cfg.has_send_timeout_us() && cfg.send_timeout_us() > 0 + ? cfg.send_timeout_us() + : 100000U; + enable_can_err_check_ = + cfg.has_enable_error_frames() && cfg.enable_error_frames(); } @@ -49,7 +70,7 @@ namespace cmvr { } SocketCanClientRaw::~SocketCanClientRaw() { - if (dev_handler_) { + if (dev_handler_ >= 0) { stop(); } } @@ -59,8 +80,8 @@ namespace cmvr { status_ = ErrorCode::OK; return true; } - struct sockaddr_can addr; - struct ifreq ifr; + struct sockaddr_can addr {}; + struct ifreq ifr {}; // open device // guss net is the device minor number, if one card is 0,1 @@ -91,17 +112,71 @@ namespace cmvr { if (ret < 0) { CMVR_LOG(ERROR) << "add receive msg id filter error code: " << ret; status_ = ErrorCode::CAN_CLIENT_ERROR_BASE; + stop(); return false; } } - // 2. enable reception of can frames. - int enable = 1; - ret = ::setsockopt(dev_handler_, SOL_CAN_RAW, CAN_RAW_FD_FRAMES, &enable, - sizeof(enable)); - if (ret < 0) { - CMVR_LOG(ERROR) << "enable reception of can frame error code: " << ret; + // 2. Explicitly opt into CAN-FD only when configured. This socket + // option does not configure the physical link bitrate or state. + if (enable_fd_) { + int enable = 1; + ret = ::setsockopt(dev_handler_, SOL_CAN_RAW, + CAN_RAW_FD_FRAMES, &enable, sizeof(enable)); + if (ret < 0) { + CMVR_LOG(ERROR) << "enable CAN-FD frames failed: " + << std::strerror(errno); + status_ = ErrorCode::CAN_CLIENT_ERROR_BASE; + stop(); + return false; + } + } + + const int receive_own = receive_own_messages_ ? 1 : 0; + if (::setsockopt(dev_handler_, SOL_CAN_RAW, CAN_RAW_RECV_OWN_MSGS, + &receive_own, sizeof(receive_own)) < 0) { + CMVR_LOG(ERROR) << "configure receive-own-messages failed: " + << std::strerror(errno); status_ = ErrorCode::CAN_CLIENT_ERROR_BASE; + stop(); + return false; + } + + if (enable_can_err_check_) { + const can_err_mask_t error_mask = CAN_ERR_MASK; + if (::setsockopt(dev_handler_, SOL_CAN_RAW, CAN_RAW_ERR_FILTER, + &error_mask, sizeof(error_mask)) < 0) { + CMVR_LOG(ERROR) << "configure CAN error filter failed: " + << std::strerror(errno); + status_ = ErrorCode::CAN_CLIENT_ERROR_BASE; + stop(); + return false; + } + } + + struct timeval receive_timeout { + static_cast(receive_timeout_us_ / 1000000U), + static_cast(receive_timeout_us_ % 1000000U) + }; + if (::setsockopt(dev_handler_, SOL_SOCKET, SO_RCVTIMEO, + &receive_timeout, sizeof(receive_timeout)) < 0) { + CMVR_LOG(ERROR) << "configure CAN receive timeout failed: " + << std::strerror(errno); + status_ = ErrorCode::CAN_CLIENT_ERROR_BASE; + stop(); + return false; + } + + struct timeval send_timeout { + static_cast(send_timeout_us_ / 1000000U), + static_cast(send_timeout_us_ % 1000000U) + }; + if (::setsockopt(dev_handler_, SOL_SOCKET, SO_SNDTIMEO, + &send_timeout, sizeof(send_timeout)) < 0) { + CMVR_LOG(ERROR) << "configure CAN send timeout failed: " + << std::strerror(errno); + status_ = ErrorCode::CAN_CLIENT_ERROR_BASE; + stop(); return false; } @@ -115,13 +190,39 @@ namespace cmvr { interface_prefix = "can"; } - const std::string can_name = absl::StrCat(interface_prefix, port_); - std::strncpy(ifr.ifr_name, can_name.c_str(), IFNAMSIZ); - if (ioctl(dev_handler_, SIOCGIFINDEX, &ifr) < 0) { - CMVR_LOG(ERROR) << "ioctl error"; + const std::string can_name = + interface_name_.empty() + ? absl::StrCat(interface_prefix, port_) + : interface_name_; + if (can_name.size() >= IFNAMSIZ) { + CMVR_LOG(ERROR) << "CAN interface name is too long: " << can_name; status_ = ErrorCode::CAN_CLIENT_ERROR_BASE; + stop(); return false; } + std::strncpy(ifr.ifr_name, can_name.c_str(), IFNAMSIZ); + ifr.ifr_name[IFNAMSIZ - 1] = '\0'; + if (ioctl(dev_handler_, SIOCGIFINDEX, &ifr) < 0) { + CMVR_LOG(ERROR) << "CAN interface not found: " << can_name + << ", error=" << std::strerror(errno); + status_ = ErrorCode::CAN_CLIENT_ERROR_BASE; + stop(); + return false; + } + + if (enable_fd_) { + struct ifreq mtu_request {}; + std::strncpy(mtu_request.ifr_name, can_name.c_str(), IFNAMSIZ); + mtu_request.ifr_name[IFNAMSIZ - 1] = '\0'; + if (::ioctl(dev_handler_, SIOCGIFMTU, &mtu_request) < 0 || + mtu_request.ifr_mtu != CANFD_MTU) { + CMVR_LOG(ERROR) << "CAN-FD requested but interface MTU is not CANFD_MTU: " + << can_name; + status_ = ErrorCode::CAN_CLIENT_ERROR_BASE; + stop(); + return false; + } + } // bind socket to network interface @@ -131,8 +232,10 @@ namespace cmvr { sizeof(addr)); if (ret < 0) { - CMVR_LOG(ERROR) << "bind socket to network interface error code: " << ret; + CMVR_LOG(ERROR) << "bind socket to CAN interface failed: " + << std::strerror(errno); status_ = ErrorCode::CAN_CLIENT_ERROR_BASE; + stop(); return false; } @@ -142,10 +245,11 @@ namespace cmvr { } bool SocketCanClientRaw::stop() { - if (is_started_) { - is_started_ = false; - - int ret = close(dev_handler_); + is_started_ = false; + if (dev_handler_ >= 0) { + const int fd = dev_handler_; + dev_handler_ = -1; + int ret = close(fd); if (ret < 0) { CMVR_LOG(ERROR) << "close error code:" << ret << ", " << getErrorString(ret); return false; @@ -159,48 +263,190 @@ namespace cmvr { // Synchronous transmission of CAN messages ErrorCode SocketCanClientRaw::send(const std::vector &frames, int32_t *const frame_num) { + return sendWithDeadline_( + frames, frame_num, + std::chrono::steady_clock::now() + + std::chrono::microseconds(send_timeout_us_)); + } + + ErrorCode SocketCanClientRaw::sendUntil( + const std::vector& frames, + int32_t* const frame_num, + const std::chrono::steady_clock::time_point deadline) { + return sendWithDeadline_( + frames, frame_num, + std::min( + deadline, + std::chrono::steady_clock::now() + + std::chrono::microseconds(send_timeout_us_))); + } + + ErrorCode SocketCanClientRaw::sendWithDeadline_( + const std::vector& frames, + int32_t* const frame_num, + const std::chrono::steady_clock::time_point send_deadline) { if (frame_num == nullptr) { - CMVR_LOG(FATAL) << "frame_num is null"; + CMVR_LOG(ERROR) << "frame_num is null"; + return ErrorCode::CAN_CLIENT_ERROR_SEND_FAILED; } - if (frames.size() != static_cast(*frame_num)) { - CMVR_LOG(FATAL) << "frames size does not match frame_num"; + if (*frame_num < 0 || + frames.size() != static_cast(*frame_num) || + frames.size() > static_cast(MAX_CAN_SEND_FRAME_LEN)) { + CMVR_LOG(ERROR) << "frames size does not match a valid frame_num"; + return ErrorCode::CAN_CLIENT_ERROR_FRAME_NUM; } if (!is_started_) { CMVR_LOG(ERROR) << "Nvidia can client has not been initiated! Please init first!"; return ErrorCode::CAN_CLIENT_ERROR_SEND_FAILED; } - for (size_t i = 0; i < frames.size() && i < MAX_CAN_SEND_FRAME_LEN; ++i) { - if (frames[i].len > CANBUS_MESSAGE_LENGTH || frames[i].len < 0) { - CMVR_LOG(ERROR) << "frames[" << i << "].len = " << frames[i].len - << ", which is not equal to can message data length (" - << CANBUS_MESSAGE_LENGTH << ")."; - return ErrorCode::CAN_CLIENT_ERROR_SEND_FAILED; - } - if (frames[i].id > CAN_STANDARD_MAX_ID) { - send_frames_[i].can_id = (frames[i].id & CAN_EFF_MASK) | CAN_EFF_FLAG; - } else { - send_frames_[i].can_id = (frames[i].id & CAN_SFF_MASK); - } - // CMVR_LOG(INFO) << "send can id is " << send_frames_[i].can_id; - send_frames_[i].can_dlc = frames[i].len; - std::memcpy(send_frames_[i].data, frames[i].data, frames[i].len); + if (std::chrono::steady_clock::now() >= send_deadline) { + *frame_num = 0; + return ErrorCode::CAN_CLIENT_ERROR_SEND_FAILED; + } - // Synchronous transmission of CAN messages - int ret = static_cast( - write(dev_handler_, &send_frames_[i], sizeof(send_frames_[i]))); - if (ret <= 0) { - CMVR_LOG(ERROR) << "can " << port_ << " send message failed, error code: " << ret; - return ErrorCode::CAN_CLIENT_ERROR_BASE; + // Validate the complete batch before committing its first frame. + // This prevents a malformed later element from causing a valid + // prefix of a cyclic command batch to reach the bus. + for (size_t i = 0; i < frames.size(); ++i) { + const auto& source = frames[i]; + const auto max_length = + source.is_fd ? CANFD_MESSAGE_LENGTH + : CANBUS_MESSAGE_LENGTH; + if (source.len > max_length || + (source.is_remote_frame && source.is_fd) || + (source.is_fd && !enable_fd_)) { + *frame_num = 0; + CMVR_LOG(ERROR) << "invalid CAN frame at index " << i + << ", len=" << static_cast(source.len) + << ", fd=" << source.is_fd; + return ErrorCode::CAN_CLIENT_ERROR_SEND_FAILED; } } + int32_t sent_count = 0; + for (size_t i = 0; i < frames.size(); ++i) { + const auto& source = frames[i]; + if (std::chrono::steady_clock::now() >= send_deadline) { + *frame_num = sent_count; + CMVR_LOG(ERROR) + << "can " << port_ + << " send batch timed out before frame " << i; + return ErrorCode::CAN_CLIENT_ERROR_SEND_FAILED; + } + + canid_t can_id = source.is_extended_id || + source.id > CAN_STANDARD_MAX_ID + ? (source.id & CAN_EFF_MASK) | CAN_EFF_FLAG + : (source.id & CAN_SFF_MASK); + if (source.is_remote_frame) { + can_id |= CAN_RTR_FLAG; + } + if (source.is_error_frame) { + can_id = (source.id & CAN_ERR_MASK) | CAN_ERR_FLAG; + } + + const void* payload = nullptr; + std::size_t expected = 0; + struct canfd_frame fd_frame {}; + struct can_frame classic_frame {}; + if (source.is_fd) { + fd_frame.can_id = can_id; + fd_frame.len = source.len; + if (source.bitrate_switch || default_bitrate_switch_) { + fd_frame.flags |= CANFD_BRS; + } + if (source.error_state_indicator) { + fd_frame.flags |= CANFD_ESI; + } + std::memcpy(fd_frame.data, source.data, source.len); + expected = CANFD_MTU; + payload = &fd_frame; + } else { + classic_frame.can_id = can_id; + classic_frame.can_dlc = source.len; + std::memcpy( + classic_frame.data, source.data, source.len); + expected = CAN_MTU; + payload = &classic_frame; + } + + while (true) { + const auto written = ::send( + dev_handler_, payload, expected, + MSG_DONTWAIT | MSG_NOSIGNAL); + if (written == static_cast(expected)) { + ++sent_count; + break; + } + if (written >= 0) { + *frame_num = sent_count; + CMVR_LOG(ERROR) + << "can " << port_ + << " sent a partial frame"; + return ErrorCode::CAN_CLIENT_ERROR_SEND_FAILED; + } + if (errno == EINTR) { + continue; + } + if (errno != EAGAIN && errno != EWOULDBLOCK) { + *frame_num = sent_count; + CMVR_LOG(ERROR) << "can " << port_ + << " send message failed: " + << std::strerror(errno); + return ErrorCode::CAN_CLIENT_ERROR_SEND_FAILED; + } + + const auto now = std::chrono::steady_clock::now(); + if (now >= send_deadline) { + *frame_num = sent_count; + CMVR_LOG(ERROR) + << "can " << port_ + << " send batch timed out"; + return ErrorCode::CAN_CLIENT_ERROR_SEND_FAILED; + } + const auto remaining = + std::chrono::duration_cast( + send_deadline - now); + struct timespec timeout { + static_cast( + remaining.count() / 1000000000LL), + static_cast( + remaining.count() % 1000000000LL) + }; + struct pollfd writable { + dev_handler_, POLLOUT, 0 + }; + const int ready = + ::ppoll(&writable, 1, &timeout, nullptr); + if (ready == 0) { + *frame_num = sent_count; + CMVR_LOG(ERROR) + << "can " << port_ + << " send batch timed out"; + return ErrorCode::CAN_CLIENT_ERROR_SEND_FAILED; + } + if (ready < 0 && errno != EINTR) { + *frame_num = sent_count; + CMVR_LOG(ERROR) + << "can " << port_ + << " send poll failed: " + << std::strerror(errno); + return ErrorCode::CAN_CLIENT_ERROR_SEND_FAILED; + } + } + } + + *frame_num = sent_count; return ErrorCode::OK; } // buf size must be 8 bytes, every time, we receive only one frame ErrorCode SocketCanClientRaw::receive(std::vector *const frames, int32_t *const frame_num) { + if (frames == nullptr || frame_num == nullptr) { + return ErrorCode::CAN_CLIENT_ERROR_RECV_FAILED; + } if (!is_started_) { CMVR_LOG(ERROR) << "Nvidia can client is not init! Please init first!"; return ErrorCode::CAN_CLIENT_ERROR_RECV_FAILED; @@ -213,39 +459,109 @@ namespace cmvr { return ErrorCode::CAN_CLIENT_ERROR_FRAME_NUM; } - for (int32_t i = 0; i < *frame_num && i < MAX_CAN_RECV_FRAME_LEN; ++i) { + frames->clear(); + const int32_t requested = *frame_num; + *frame_num = 0; + for (int32_t i = 0; i < requested && i < MAX_CAN_RECV_FRAME_LEN; ++i) { CanFrame cf; - auto ret = read(dev_handler_, &recv_frames_[i], sizeof(recv_frames_[i])); + struct canfd_frame raw {}; + const auto ret = ::read(dev_handler_, &raw, CANFD_MTU); if (ret < 0) { - CMVR_LOG(ERROR) << "receive message failed, error code: " << ret; - return ErrorCode::CAN_CLIENT_ERROR_BASE; - } - if (recv_frames_[i].can_dlc > CANBUS_MESSAGE_LENGTH || - recv_frames_[i].can_dlc < 0) { - CMVR_LOG(ERROR) << "recv_frames_[" << i - << "].can_dlc = " << recv_frames_[i].can_dlc - << ", which is not equal to can message data length (" - << CANBUS_MESSAGE_LENGTH << ")."; + if (errno == EAGAIN || errno == EWOULDBLOCK || + errno == EINTR) { + return ErrorCode::CAN_CLIENT_ERROR_RECV_FAILED; + } + CMVR_LOG(ERROR) << "receive CAN message failed: " + << std::strerror(errno); return ErrorCode::CAN_CLIENT_ERROR_RECV_FAILED; } - if (recv_frames_[i].can_id > CAN_STANDARD_MAX_ID) { - cf.id = enable_can_err_check_ - ? recv_frames_[i].can_id & CAN_EFF_MASK | CAN_ERR_FLAG - : recv_frames_[i].can_id & CAN_EFF_MASK; - } else { - cf.id = (recv_frames_[i].can_id & CAN_SFF_MASK); + if (ret != CAN_MTU && ret != CANFD_MTU) { + CMVR_LOG(ERROR) << "unexpected SocketCAN MTU: " << ret; + return ErrorCode::CAN_CLIENT_ERROR_RECV_FAILED; } - // CMVR_LOG(INFO) << "Socket can receive can id is " << recv_frames_[i].can_id; - cf.len = recv_frames_[i].can_dlc; - std::memcpy(cf.data, recv_frames_[i].data, recv_frames_[i].can_dlc); + + const canid_t raw_id = raw.can_id; + cf.is_extended_id = (raw_id & CAN_EFF_FLAG) != 0; + cf.is_remote_frame = (raw_id & CAN_RTR_FLAG) != 0; + cf.is_error_frame = (raw_id & CAN_ERR_FLAG) != 0; + if (cf.is_error_frame) { + cf.id = raw_id & CAN_ERR_MASK; + } else if (cf.is_extended_id) { + cf.id = raw_id & CAN_EFF_MASK; + } else { + cf.id = raw_id & CAN_SFF_MASK; + } + + cf.is_fd = ret == CANFD_MTU; + if (cf.is_fd) { + cf.len = raw.len; + cf.bitrate_switch = (raw.flags & CANFD_BRS) != 0; + cf.error_state_indicator = (raw.flags & CANFD_ESI) != 0; + } else { + const auto* classic = + reinterpret_cast(&raw); + cf.len = classic->can_dlc; + } + const auto max_length = + cf.is_fd ? CANFD_MESSAGE_LENGTH : CANBUS_MESSAGE_LENGTH; + if (cf.len > max_length) { + return ErrorCode::CAN_CLIENT_ERROR_RECV_FAILED; + } + std::memcpy(cf.data, raw.data, cf.len); + + struct timespec monotonic {}; + if (::clock_gettime(CLOCK_MONOTONIC, &monotonic) == 0) { + cf.rx_monotonic_ns = + static_cast(monotonic.tv_sec) * 1000000000LL + + monotonic.tv_nsec; + } + ::gettimeofday(&cf.timestamp, nullptr); frames->push_back(cf); + ++(*frame_num); } return ErrorCode::OK; } - std::string SocketCanClientRaw::getErrorString(const int32_t /*status*/) { - return ""; + bool SocketCanClientRaw::discardPendingFrames() { + if (!is_started_ || dev_handler_ < 0) { + return false; + } + + constexpr std::size_t kMaximumDrainFrames = 4096; + const auto deadline = + std::chrono::steady_clock::now() + + std::chrono::microseconds(send_timeout_us_); + std::size_t count = 0; + while (count < kMaximumDrainFrames && + std::chrono::steady_clock::now() < deadline) { + struct canfd_frame raw {}; + const auto received = ::recv( + dev_handler_, &raw, CANFD_MTU, MSG_DONTWAIT); + if (received == CAN_MTU || received == CANFD_MTU) { + ++count; + continue; + } + if (received < 0 && + (errno == EAGAIN || errno == EWOULDBLOCK)) { + return true; + } + if (received < 0 && errno == EINTR) { + continue; + } + CMVR_LOG(ERROR) + << "failed while draining pending CAN frames: " + << (received < 0 ? std::strerror(errno) + : "unexpected MTU"); + return false; + } + CMVR_LOG(ERROR) + << "CAN receive queue did not drain within its bound"; + return false; + } + + std::string SocketCanClientRaw::getErrorString(const int32_t status) { + return std::strerror(status < 0 ? -status : status); } } } diff --git a/cmvr-es/devices/canbus/can_client/socket/socket_can_client_raw.h b/cmvr-es/devices/canbus/can_client/socket/socket_can_client_raw.h index 7f478c86..ca2c01ab 100644 --- a/cmvr-es/devices/canbus/can_client/socket/socket_can_client_raw.h +++ b/cmvr-es/devices/canbus/can_client/socket/socket_can_client_raw.h @@ -12,11 +12,13 @@ #include #include +#include #include #include #include #include +#include #include #include @@ -51,6 +53,10 @@ namespace cmvr { */ cmvr::msgs::ErrorCode send(const std::vector &frames, int32_t *const frame_num) override; + cmvr::msgs::ErrorCode sendUntil( + const std::vector& frames, + int32_t* const frame_num, + std::chrono::steady_clock::time_point deadline) override; /** * @brief Receive messages @@ -60,6 +66,7 @@ namespace cmvr { */ cmvr::msgs::ErrorCode receive(std::vector *const frames, int32_t *const frame_num) override; + bool discardPendingFrames() override; /** * @brief Get the error string. @@ -67,14 +74,23 @@ namespace cmvr { */ std::string getErrorString(const int32_t status) override; private: - int dev_handler_ = 0; + int dev_handler_{-1}; cmvr::msgs::CANCardParameter::CANChannelId port_; cmvr::msgs::CANCardParameter::CANInterface interface_; - can_frame send_frames_[MAX_CAN_SEND_FRAME_LEN]; - can_frame recv_frames_[MAX_CAN_RECV_FRAME_LEN]; + std::string interface_name_; + bool enable_fd_{false}; + bool default_bitrate_switch_{false}; + bool receive_own_messages_{false}; + uint32_t receive_timeout_us_{100000}; + uint32_t send_timeout_us_{100000}; // bool enable_can_err_check_{false}; + + cmvr::msgs::ErrorCode sendWithDeadline_( + const std::vector& frames, + int32_t* frame_num, + std::chrono::steady_clock::time_point deadline); }; } } diff --git a/cmvr-es/devices/canbus/can_client/socket/socket_can_client_raw_test.cc b/cmvr-es/devices/canbus/can_client/socket/socket_can_client_raw_test.cc index 391be9d3..745600eb 100644 --- a/cmvr-es/devices/canbus/can_client/socket/socket_can_client_raw_test.cc +++ b/cmvr-es/devices/canbus/can_client/socket/socket_can_client_raw_test.cc @@ -1,44 +1,226 @@ -#include "common/base/logging/logger.h" -// -// Created by lgv on 2025/7/16. -// -#include "cmvr/msgs/error_code.pb.h" -#include "cmvr/msgs/can_card_parameter.pb.h" #include "canbus/can_client/socket/socket_can_client_raw.h" -#include "gtest/gtest.h" -namespace cmvr { -namespace device { - using cmvr::msgs::ErrorCode; - using cmvr::msgs::CANCardParameter; - TEST(SocketCanClientRawTest, simple_test) { - CANCardParameter param; - param.set_brand(CANCardParameter::SOCKET_CAN_RAW); - param.set_channel_id(CANCardParameter::CHANNEL_ID_ZERO); +#include +#include +#include +#include +#include +#include +#include - cmvr::config::SocketCanConfig cfg; - cfg.set_channel_id(0); - SocketCanClientRaw socket_can_client(cfg); +#include - // EXPECT_EQ(socket_can_client.start(), ErrorCode::CAN_CLIENT_ERROR_BASE); - socket_can_client.start(); - std::vector frames; - int32_t num = 0; - EXPECT_EQ(socket_can_client.send(frames, &num), - ErrorCode::OK); - ++num; - EXPECT_EQ(socket_can_client.receive(&frames, &num), - ErrorCode::OK); - CMVR_LOG(INFO) << frames.at(0).CanFrameString(); - CanFrame can_frame; - can_frame.id = 0x123; - can_frame.len = 8; - memset(can_frame.data, 0xA3, sizeof(can_frame.data)); - frames.clear(); - frames.push_back(can_frame); - EXPECT_EQ(socket_can_client.sendSingleFrame(frames), - ErrorCode::OK); - socket_can_client.stop(); +namespace cmvr::device { +namespace { + +std::size_t openFileDescriptorCount() +{ + std::error_code error; + std::size_t count = 0; + for (std::filesystem::directory_iterator iterator( + "/proc/self/fd", error); + !error && iterator != std::filesystem::directory_iterator(); + iterator.increment(error)) { + ++count; + } + return error ? 0U : count; +} + +config::SocketCanConfig vcanConfig(const bool enable_fd) +{ + config::SocketCanConfig config; + config.set_interface_name("vcan0"); + config.set_enable_fd(enable_fd); + config.set_bitrate_switch(enable_fd); + config.set_receive_own_messages(false); + config.set_receive_timeout_us(2000U); + config.set_send_timeout_us(2000U); + return config; +} + +bool vcanAvailable() +{ + return ::if_nametoindex("vcan0") != 0U; +} + +TEST(SocketCanClientRawTest, MissingClassicInterfaceFailsWithoutLeakingFd) +{ + config::SocketCanConfig config; + config.set_interface_name("cmvr_no_such_can"); + config.set_enable_fd(false); + config.set_receive_timeout_us(100U); + config.set_send_timeout_us(100U); + SocketCanClientRaw client(config); + + const auto before = openFileDescriptorCount(); + ASSERT_GT(before, 0U); + for (int attempt = 0; attempt < 32; ++attempt) { + EXPECT_FALSE(client.start()); + EXPECT_TRUE(client.stop()); + } + const auto after = openFileDescriptorCount(); + EXPECT_LE(after, before + 1U); +} + +TEST(SocketCanClientRawTest, ClosedClientRejectsClassicSendAndReceive) +{ + config::SocketCanConfig config; + config.set_interface_name("cmvr_no_such_can"); + config.set_enable_fd(false); + SocketCanClientRaw client(config); + + CanFrame frame; + frame.id = 0x123U; + frame.len = 8U; + frame.is_fd = false; + std::fill(std::begin(frame.data), std::end(frame.data), 0xA3U); + std::vector frames{frame}; + int32_t count = 1; + EXPECT_EQ( + client.send(frames, &count), + msgs::ErrorCode::CAN_CLIENT_ERROR_SEND_FAILED); + + count = 1; + EXPECT_EQ( + client.receive(&frames, &count), + msgs::ErrorCode::CAN_CLIENT_ERROR_RECV_FAILED); + EXPECT_NE(frame.CanFrameString().find("fd:0"), std::string::npos); +} + +TEST(SocketCanClientRawTest, VcanTransmitsClassicAndCanFdBatches) +{ + if (!vcanAvailable()) { + GTEST_SKIP() << "vcan0 is not available in this network namespace"; + } + + SocketCanClientRaw classic_tx(vcanConfig(false)); + SocketCanClientRaw fd_rx(vcanConfig(true)); + ASSERT_TRUE(classic_tx.start()); + ASSERT_TRUE(fd_rx.start()); + + CanFrame first; + first.id = 0x123U; + first.len = 8U; + first.data[0] = 0xA1U; + CanFrame second; + second.id = 0x456U; + second.len = 3U; + second.data[0] = 0xB2U; + std::vector classic_frames{first, second}; + int32_t count = 2; + ASSERT_EQ( + classic_tx.send(classic_frames, &count), + msgs::ErrorCode::OK); + ASSERT_EQ(count, 2); + + for (const auto& expected : classic_frames) { + std::vector received; + int32_t receive_count = 1; + ASSERT_EQ( + fd_rx.receive(&received, &receive_count), + msgs::ErrorCode::OK); + ASSERT_EQ(receive_count, 1); + ASSERT_EQ(received.size(), 1U); + EXPECT_FALSE(received.front().is_fd); + EXPECT_EQ(received.front().id, expected.id); + EXPECT_EQ(received.front().len, expected.len); + EXPECT_EQ(received.front().data[0], expected.data[0]); + } + ASSERT_TRUE(classic_tx.stop()); + ASSERT_TRUE(fd_rx.stop()); + + SocketCanClientRaw fd_tx(vcanConfig(true)); + SocketCanClientRaw second_fd_rx(vcanConfig(true)); + ASSERT_TRUE(fd_tx.start()); + ASSERT_TRUE(second_fd_rx.start()); + CanFrame fd_first; + fd_first.id = 0x201U; + fd_first.len = 12U; + fd_first.is_fd = true; + fd_first.bitrate_switch = true; + fd_first.data[11] = 0xC3U; + CanFrame fd_second; + fd_second.id = 0x202U; + fd_second.len = 64U; + fd_second.is_fd = true; + fd_second.bitrate_switch = true; + fd_second.data[63] = 0xD4U; + std::vector fd_frames{fd_first, fd_second}; + count = 2; + ASSERT_EQ(fd_tx.send(fd_frames, &count), msgs::ErrorCode::OK); + ASSERT_EQ(count, 2); + + for (const auto& expected : fd_frames) { + std::vector received; + int32_t receive_count = 1; + ASSERT_EQ( + second_fd_rx.receive(&received, &receive_count), + msgs::ErrorCode::OK); + ASSERT_EQ(received.size(), 1U); + EXPECT_TRUE(received.front().is_fd); + EXPECT_TRUE(received.front().bitrate_switch); + EXPECT_EQ(received.front().id, expected.id); + EXPECT_EQ(received.front().len, expected.len); + EXPECT_EQ( + received.front().data[expected.len - 1U], + expected.data[expected.len - 1U]); } } + +TEST(SocketCanClientRawTest, VcanDrainAndBatchValidationAreFailClosed) +{ + if (!vcanAvailable()) { + GTEST_SKIP() << "vcan0 is not available in this network namespace"; + } + + SocketCanClientRaw tx(vcanConfig(false)); + SocketCanClientRaw rx(vcanConfig(false)); + ASSERT_TRUE(tx.start()); + ASSERT_TRUE(rx.start()); + + CanFrame valid; + valid.id = 0x321U; + valid.len = 8U; + valid.data[0] = 0x5AU; + std::vector one{valid}; + int32_t count = 1; + ASSERT_EQ(tx.send(one, &count), msgs::ErrorCode::OK); + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + ASSERT_TRUE(rx.discardPendingFrames()); + + std::vector received; + int32_t receive_count = 1; + EXPECT_EQ( + rx.receive(&received, &receive_count), + msgs::ErrorCode::CAN_CLIENT_ERROR_RECV_FAILED); + + CanFrame invalid = valid; + invalid.id = 0x322U; + invalid.len = 9U; + std::vector invalid_batch{valid, invalid}; + count = 2; + EXPECT_EQ( + tx.send(invalid_batch, &count), + msgs::ErrorCode::CAN_CLIENT_ERROR_SEND_FAILED); + EXPECT_EQ(count, 0); + receive_count = 1; + EXPECT_EQ( + rx.receive(&received, &receive_count), + msgs::ErrorCode::CAN_CLIENT_ERROR_RECV_FAILED); + + count = 1; + EXPECT_EQ( + tx.sendUntil( + one, &count, + std::chrono::steady_clock::now() - + std::chrono::microseconds(1)), + msgs::ErrorCode::CAN_CLIENT_ERROR_SEND_FAILED); + EXPECT_EQ(count, 0); + receive_count = 1; + EXPECT_EQ( + rx.receive(&received, &receive_count), + msgs::ErrorCode::CAN_CLIENT_ERROR_RECV_FAILED); } + +} // namespace +} // namespace cmvr::device diff --git a/cmvr-es/devices/canbus/common/canbus_consts.h b/cmvr-es/devices/canbus/common/canbus_consts.h index ed42ba19..d46f9223 100644 --- a/cmvr-es/devices/canbus/common/canbus_consts.h +++ b/cmvr-es/devices/canbus/common/canbus_consts.h @@ -26,9 +26,14 @@ namespace cmvr { namespace device { const int32_t CAN_FRAME_SIZE = 8; - const int32_t MAX_CAN_SEND_FRAME_LEN = 1; + const int32_t CAN_FD_FRAME_SIZE = 64; + // One UME cycle may submit a complete arm worth of frames. The receive + // API intentionally remains one-frame-at-a-time so a caller never + // blocks waiting to fill an artificial batch. + const int32_t MAX_CAN_SEND_FRAME_LEN = 64; const int32_t MAX_CAN_RECV_FRAME_LEN = 1; // 这个暂时改为 1 ,大量数据的时候改为 10 const int32_t CANBUS_MESSAGE_LENGTH = 8; // according to ISO-11891-1 + const int32_t CANFD_MESSAGE_LENGTH = 64; } } diff --git a/cmvr-es/devices/motor/bus_runtime/can/src/can_motor_bus_runtime.cpp b/cmvr-es/devices/motor/bus_runtime/can/src/can_motor_bus_runtime.cpp index 09c1284c..18d54623 100644 --- a/cmvr-es/devices/motor/bus_runtime/can/src/can_motor_bus_runtime.cpp +++ b/cmvr-es/devices/motor/bus_runtime/can/src/can_motor_bus_runtime.cpp @@ -68,16 +68,18 @@ bool CanMotorBusRuntime::start() return false; } - auto ret = sender_->Start(); + // Start the receiver first so a protocol response cannot arrive before the + // receive path is ready. + auto ret = receiver_->Start(); if (ret != ErrorCode::OK) { - CMVR_LOG(ERROR) << "[CanMotorBusRuntime] failed to start CAN sender: " << id_; + CMVR_LOG(ERROR) << "[CanMotorBusRuntime] failed to start CAN receiver: " << id_; stop(); return false; } - ret = receiver_->Start(); + ret = sender_->Start(); if (ret != ErrorCode::OK) { - CMVR_LOG(ERROR) << "[CanMotorBusRuntime] failed to start CAN receiver: " << id_; + CMVR_LOG(ERROR) << "[CanMotorBusRuntime] failed to start CAN sender: " << id_; stop(); return false; } diff --git a/cmvr-es/main.cpp b/cmvr-es/main.cpp index 3e46bbfa..c1ac7852 100644 --- a/cmvr-es/main.cpp +++ b/cmvr-es/main.cpp @@ -1,5 +1,9 @@ #include +#include +#include +#include #include +#include #include "common/base/logging/logger.h" #include "runtime/include/cmvr_runtime.h" @@ -14,20 +18,91 @@ bool blockShutdownSignals(sigset_t& shutdown_signals) return pthread_sigmask(SIG_BLOCK, &shutdown_signals, nullptr) == 0; } +struct CommandLineOptions { + std::string config_path; + double control_period_s{0.001}; + bool show_help{false}; +}; + +void printUsage(const char* program) +{ + std::cout + << "Usage: " << program + << " [--config PATH] [--control-period-s SECONDS]\n" + << "\n" + << "With no --config argument, cmvr_es loads config/cmvr_es.pb.txt " + "beside the executable.\n"; +} + +bool parseCommandLine( + const int argc, + char* argv[], + CommandLineOptions& options) +{ + for (int index = 1; index < argc; ++index) { + const std::string argument = argv[index]; + if (argument == "--help" || argument == "-h") { + options.show_help = true; + return true; + } + if (argument == "--config") { + if (++index >= argc || argv[index][0] == '\0') { + std::cerr << "--config requires a path\n"; + return false; + } + options.config_path = argv[index]; + continue; + } + if (argument == "--control-period-s") { + if (++index >= argc) { + std::cerr + << "--control-period-s requires a numeric value\n"; + return false; + } + char* end = nullptr; + const double value = std::strtod(argv[index], &end); + if (!end || *end != '\0' || !std::isfinite(value) || + value <= 0.0 || value > 1.0) { + std::cerr + << "--control-period-s must be in (0, 1]\n"; + return false; + } + options.control_period_s = value; + continue; + } + std::cerr << "unknown argument: " << argument << '\n'; + return false; + } + return true; +} + } // namespace -int main() +int main(int argc, char* argv[]) { + CommandLineOptions options; + if (!parseCommandLine(argc, argv, options)) { + printUsage(argv[0]); + return 2; + } + if (options.show_help) { + printUsage(argv[0]); + return 0; + } + sigset_t shutdown_signals; if (!blockShutdownSignals(shutdown_signals)) { return 1; } cmvr::Runtime runtime; - if (!runtime.init()) { + const bool initialized = options.config_path.empty() + ? runtime.init() + : runtime.init(options.config_path); + if (!initialized) { return 1; } - if (!runtime.startTasks()) { + if (!runtime.startTasks(options.control_period_s)) { return 1; } diff --git a/cmvr-es/manager/control_authority/CMakeLists.txt b/cmvr-es/manager/control_authority/CMakeLists.txt new file mode 100644 index 00000000..f2950a43 --- /dev/null +++ b/cmvr-es/manager/control_authority/CMakeLists.txt @@ -0,0 +1,34 @@ +add_library(control_authority STATIC + src/control_authority_manager.cpp +) +target_compile_features(control_authority PUBLIC cxx_std_17) +target_include_directories(control_authority + PUBLIC + ${PROJECT_SOURCE_DIR}/cmvr-es +) + +add_library( + cmvr_es::control_authority + ALIAS control_authority +) +install(TARGETS control_authority ARCHIVE DESTINATION lib) + +if(BUILD_TESTING) + add_executable(control_authority_manager_test + tests/control_authority_manager_test.cpp + ) + target_link_libraries(control_authority_manager_test + PRIVATE + cmvr_es::control_authority + gtest + gtest_main + pthread + ) + add_test( + NAME control_authority_manager_test + COMMAND control_authority_manager_test + ) + set_tests_properties(control_authority_manager_test PROPERTIES + TIMEOUT 10 + ) +endif() diff --git a/cmvr-es/manager/control_authority/include/control_authority_manager.h b/cmvr-es/manager/control_authority/include/control_authority_manager.h new file mode 100644 index 00000000..4e29f24f --- /dev/null +++ b/cmvr-es/manager/control_authority/include/control_authority_manager.h @@ -0,0 +1,74 @@ +#ifndef CMVR_ES_CONTROL_AUTHORITY_MANAGER_H +#define CMVR_ES_CONTROL_AUTHORITY_MANAGER_H + +#include +#include +#include +#include +#include + +namespace cmvr::control { + +struct ControlLeaseToken { + std::string resource_id; + std::string owner_id; + std::uint64_t generation{0}; + + bool valid() const noexcept + { + return !resource_id.empty() && + !owner_id.empty() && + generation != 0U; + } +}; + +struct ControlAcquireResult { + bool acquired{false}; + ControlLeaseToken token; + std::string detail; +}; + +// Process-wide, transport-independent control ownership. The generation in a +// token prevents a delayed release from an old network session from releasing +// a newer lease on the same arm. +class ControlAuthorityManager { +public: + using Duration = std::chrono::milliseconds; + + static ControlAuthorityManager& instance(); + + ControlAcquireResult tryAcquire( + const std::string& resource_id, + const std::string& owner_id, + Duration ttl); + bool renew(const ControlLeaseToken& token, Duration ttl); + bool validate(const ControlLeaseToken& token); + void release(const ControlLeaseToken& token) noexcept; + + // Safety/control paths which do not possess a lease use this query to + // reject mutating commands. Read-only state and stop/torque-off commands + // are intentionally allowed by their callers. + bool isLeased(const std::string& resource_id); + void revoke(const std::string& resource_id) noexcept; + + // Test/process teardown hook. Runtime code should release/revoke exact + // resources instead of clearing unrelated ownership. + void clear() noexcept; + +private: + struct Entry { + std::string owner_id; + std::uint64_t generation{0}; + std::chrono::steady_clock::time_point deadline; + }; + + bool expired_(const Entry& entry) const noexcept; + + std::mutex mutex_; + std::unordered_map entries_; + std::uint64_t next_generation_{0}; +}; + +} // namespace cmvr::control + +#endif // CMVR_ES_CONTROL_AUTHORITY_MANAGER_H diff --git a/cmvr-es/manager/control_authority/src/control_authority_manager.cpp b/cmvr-es/manager/control_authority/src/control_authority_manager.cpp new file mode 100644 index 00000000..e52d3353 --- /dev/null +++ b/cmvr-es/manager/control_authority/src/control_authority_manager.cpp @@ -0,0 +1,152 @@ +#include "manager/control_authority/include/control_authority_manager.h" + +#include + +namespace cmvr::control { + +ControlAuthorityManager& ControlAuthorityManager::instance() +{ + static ControlAuthorityManager manager; + return manager; +} + +ControlAcquireResult ControlAuthorityManager::tryAcquire( + const std::string& resource_id, + const std::string& owner_id, + const Duration ttl) +{ + if (resource_id.empty() || owner_id.empty() || + ttl <= Duration::zero()) { + return {false, {}, "invalid control lease request"}; + } + + std::lock_guard lock(mutex_); + const auto existing = entries_.find(resource_id); + if (existing != entries_.end()) { + if (!expired_(existing->second)) { + return { + false, + {}, + "control resource is already leased by " + + existing->second.owner_id}; + } + entries_.erase(existing); + } + + ControlLeaseToken token; + token.resource_id = resource_id; + token.owner_id = owner_id; + token.generation = ++next_generation_; + entries_.emplace( + resource_id, + Entry{ + owner_id, + token.generation, + std::chrono::steady_clock::now() + ttl}); + return {true, std::move(token), {}}; +} + +bool ControlAuthorityManager::renew( + const ControlLeaseToken& token, + const Duration ttl) +{ + if (!token.valid() || ttl <= Duration::zero()) { + return false; + } + std::lock_guard lock(mutex_); + const auto found = entries_.find(token.resource_id); + if (found == entries_.end() || + expired_(found->second) || + found->second.owner_id != token.owner_id || + found->second.generation != token.generation) { + if (found != entries_.end() && expired_(found->second)) { + entries_.erase(found); + } + return false; + } + found->second.deadline = + std::chrono::steady_clock::now() + ttl; + return true; +} + +bool ControlAuthorityManager::validate( + const ControlLeaseToken& token) +{ + if (!token.valid()) { + return false; + } + std::lock_guard lock(mutex_); + const auto found = entries_.find(token.resource_id); + if (found == entries_.end()) { + return false; + } + if (expired_(found->second)) { + entries_.erase(found); + return false; + } + return found->second.owner_id == token.owner_id && + found->second.generation == token.generation; +} + +void ControlAuthorityManager::release( + const ControlLeaseToken& token) noexcept +{ + if (!token.valid()) { + return; + } + try { + std::lock_guard lock(mutex_); + const auto found = entries_.find(token.resource_id); + if (found != entries_.end() && + found->second.owner_id == token.owner_id && + found->second.generation == token.generation) { + entries_.erase(found); + } + } catch (...) { + } +} + +bool ControlAuthorityManager::isLeased( + const std::string& resource_id) +{ + if (resource_id.empty()) { + return false; + } + std::lock_guard lock(mutex_); + const auto found = entries_.find(resource_id); + if (found == entries_.end()) { + return false; + } + if (expired_(found->second)) { + entries_.erase(found); + return false; + } + return true; +} + +void ControlAuthorityManager::revoke( + const std::string& resource_id) noexcept +{ + try { + std::lock_guard lock(mutex_); + entries_.erase(resource_id); + } catch (...) { + } +} + +void ControlAuthorityManager::clear() noexcept +{ + try { + std::lock_guard lock(mutex_); + entries_.clear(); + } catch (...) { + } +} + +bool ControlAuthorityManager::expired_( + const Entry& entry) const noexcept +{ + return std::chrono::steady_clock::now() >= entry.deadline; +} + +} // namespace cmvr::control diff --git a/cmvr-es/manager/control_authority/tests/control_authority_manager_test.cpp b/cmvr-es/manager/control_authority/tests/control_authority_manager_test.cpp new file mode 100644 index 00000000..b6b2b965 --- /dev/null +++ b/cmvr-es/manager/control_authority/tests/control_authority_manager_test.cpp @@ -0,0 +1,91 @@ +#include "manager/control_authority/include/control_authority_manager.h" + +#include +#include + +#include + +namespace cmvr::control { +namespace { + +using namespace std::chrono_literals; + +class ControlAuthorityManagerTest : public ::testing::Test { +protected: + void SetUp() override + { + ControlAuthorityManager::instance().clear(); + } + + void TearDown() override + { + ControlAuthorityManager::instance().clear(); + } +}; + +TEST_F(ControlAuthorityManagerTest, LeaseIsExclusiveAndExactReleaseRestoresAccess) +{ + auto& manager = ControlAuthorityManager::instance(); + const auto first = + manager.tryAcquire("right_arm", "session-a", 100ms); + ASSERT_TRUE(first.acquired); + EXPECT_TRUE(manager.validate(first.token)); + EXPECT_TRUE(manager.isLeased("right_arm")); + + const auto conflict = + manager.tryAcquire("right_arm", "session-b", 100ms); + EXPECT_FALSE(conflict.acquired); + + manager.release(first.token); + EXPECT_FALSE(manager.isLeased("right_arm")); + EXPECT_TRUE( + manager.tryAcquire("right_arm", "session-b", 100ms) + .acquired); +} + +TEST_F(ControlAuthorityManagerTest, StaleGenerationCannotReleaseNewLease) +{ + auto& manager = ControlAuthorityManager::instance(); + const auto old = + manager.tryAcquire("right_arm", "session-a", 100ms); + ASSERT_TRUE(old.acquired); + manager.release(old.token); + const auto current = + manager.tryAcquire("right_arm", "session-a", 100ms); + ASSERT_TRUE(current.acquired); + ASSERT_NE( + old.token.generation, + current.token.generation); + + manager.release(old.token); + EXPECT_TRUE(manager.validate(current.token)); +} + +TEST_F(ControlAuthorityManagerTest, ExpiryAndRenewUseMonotonicLocalTime) +{ + auto& manager = ControlAuthorityManager::instance(); + const auto lease = + manager.tryAcquire("right_arm", "session-a", 20ms); + ASSERT_TRUE(lease.acquired); + std::this_thread::sleep_for(10ms); + ASSERT_TRUE(manager.renew(lease.token, 30ms)); + std::this_thread::sleep_for(20ms); + EXPECT_TRUE(manager.validate(lease.token)); + std::this_thread::sleep_for(20ms); + EXPECT_FALSE(manager.validate(lease.token)); + EXPECT_FALSE(manager.isLeased("right_arm")); +} + +TEST_F(ControlAuthorityManagerTest, DifferentArmsCanBeLeasedIndependently) +{ + auto& manager = ControlAuthorityManager::instance(); + EXPECT_TRUE( + manager.tryAcquire("right_arm", "session-a", 100ms) + .acquired); + EXPECT_TRUE( + manager.tryAcquire("left_arm", "session-b", 100ms) + .acquired); +} + +} // namespace +} // namespace cmvr::control diff --git a/cmvr-es/manager/device_manager/CMakeLists.txt b/cmvr-es/manager/device_manager/CMakeLists.txt index a9ed557a..180e9638 100644 --- a/cmvr-es/manager/device_manager/CMakeLists.txt +++ b/cmvr-es/manager/device_manager/CMakeLists.txt @@ -42,9 +42,39 @@ if(BUILD_TESTING) "${CMAKE_BINARY_DIR}/cmvr_compiler_runtime") list(JOIN _device_manager_test_library_dirs ":" _device_manager_test_library_path) + set(_device_manager_snapshot_test_environment + "LD_LIBRARY_PATH=${_device_manager_test_library_path}") + if(CMVR_TEST_SYSTEM_LIBSTDCXX) + list(APPEND _device_manager_snapshot_test_environment + "LD_PRELOAD=${CMVR_TEST_SYSTEM_LIBSTDCXX}") + endif() set_tests_properties(device_manager_snapshot_test PROPERTIES ENVIRONMENT - "LD_LIBRARY_PATH=${_device_manager_test_library_path}" + "${_device_manager_snapshot_test_environment}" ) endif() + + add_executable(device_manager_lifecycle_test + tests/device_manager_lifecycle_test.cpp + ) + target_link_libraries(device_manager_lifecycle_test PRIVATE + cmvr_es::device_manager + gtest + gtest_main + pthread + ) + add_test( + NAME device_manager_lifecycle_test + COMMAND device_manager_lifecycle_test + ) + set(_device_manager_lifecycle_test_environment + "LD_LIBRARY_PATH=${CMVR_TEST_EXTERNAL_LIBRARY_PATH}") + if(CMVR_TEST_SYSTEM_LIBSTDCXX) + list(APPEND _device_manager_lifecycle_test_environment + "LD_PRELOAD=${CMVR_TEST_SYSTEM_LIBSTDCXX}") + endif() + set_tests_properties(device_manager_lifecycle_test PROPERTIES + TIMEOUT 10 + ENVIRONMENT "${_device_manager_lifecycle_test_environment}" + ) endif() diff --git a/cmvr-es/manager/device_manager/include/device_manager.h b/cmvr-es/manager/device_manager/include/device_manager.h index b6726f20..8d1087cc 100644 --- a/cmvr-es/manager/device_manager/include/device_manager.h +++ b/cmvr-es/manager/device_manager/include/device_manager.h @@ -27,9 +27,10 @@ namespace cmvr::device { static DeviceManager& getInstance(); static void destroyInstance(); - void start(); - void restart(); + bool start(); + bool restart(); void stop(); + bool initialized() const noexcept { return initialized_; } void getDeviceList(std::list> &device_list); void registerDevice(const std::shared_ptr& device); @@ -54,14 +55,19 @@ namespace cmvr::device { std::unordered_map devices_; std::unordered_map device_statuses_; std::unique_ptr dev_factory_; + bool initialized_{false}; explicit DeviceManager(const config::DeviceManagerConfig &cfg); void log_device_plan_() const; - void pre_scan_robot_arm_dependencies_() const; - void init_devices_(); + bool pre_scan_robot_arm_dependencies_() const; + bool init_devices_(); void configure_mujoco_viewer_pip_(); - void start_devices_(); - void stop_devices_(); + void initialize_device_statuses_(); + void mark_initializing_statuses_error_(const std::string& error_message); + void update_device_status_(const std::string& device_id, + ManagedDeviceState state, + const std::string& error_message = {}); + void stop_devices_(bool update_status = true); }; } // cmvr diff --git a/cmvr-es/manager/device_manager/src/device_manager.cpp b/cmvr-es/manager/device_manager/src/device_manager.cpp index db84586f..123797f9 100644 --- a/cmvr-es/manager/device_manager/src/device_manager.cpp +++ b/cmvr-es/manager/device_manager/src/device_manager.cpp @@ -6,7 +6,10 @@ #include "../include/device_manager.h" #include +#include #include +#include +#include #include "devices/agv/abstract_agv.h" #include "devices/arm/robot_arm.h" @@ -31,6 +34,51 @@ namespace { using GroupJointSelection = std::unordered_map>; using MotorJointSelections = std::unordered_map; +constexpr std::size_t kMaxDeviceErrorLength = 512; + +std::uint64_t unixTimeMs() noexcept +{ + const auto elapsed = std::chrono::duration_cast( + std::chrono::system_clock::now().time_since_epoch()); + return elapsed.count() > 0 + ? static_cast(elapsed.count()) + : 1U; +} + +std::string truncateDeviceError(const std::string& message) +{ + return message.substr(0, kMaxDeviceErrorLength); +} + +DeviceKind deviceTypeToKind( + const cmvr::config::DeviceConfigEntry::DeviceType type) +{ + switch (type) { + case cmvr::config::DeviceConfigEntry::DEVICE_TYPE_BIO_HEAD_ROBOT: + return DeviceKind::BioHead; + case cmvr::config::DeviceConfigEntry::DEVICE_TYPE_MOTOR_SYSTEM: + return DeviceKind::MotorSystem; + case cmvr::config::DeviceConfigEntry::DEVICE_TYPE_ROBOT_ARM: + return DeviceKind::Arm; + case cmvr::config::DeviceConfigEntry::DEVICE_TYPE_CAMERA: + return DeviceKind::Camera; + case cmvr::config::DeviceConfigEntry::DEVICE_TYPE_DEXHAND: + return DeviceKind::DexHand; + case cmvr::config::DeviceConfigEntry::DEVICE_TYPE_MICROPHONE: + return DeviceKind::Microphone; + case cmvr::config::DeviceConfigEntry::DEVICE_TYPE_SPEAKER: + return DeviceKind::Speaker; + case cmvr::config::DeviceConfigEntry::DEVICE_TYPE_AGV: + return DeviceKind::AGV; + case cmvr::config::DeviceConfigEntry::DEVICE_TYPE_MUJOCO_WORLD: + return DeviceKind::MujocoWorld; + case cmvr::config::DeviceConfigEntry::DEVICE_TYPE_MUJOCO_VIEWER: + return DeviceKind::MujocoViewer; + case cmvr::config::DeviceConfigEntry::DEVICE_TYPE_UNKNOWN: + default: + return DeviceKind::Unknown; + } +} void logSection(const char* title) { @@ -126,12 +174,24 @@ DeviceManager::DeviceManager(const config::DeviceManagerConfig& cfg) { cfg_ = cfg; dev_factory_ = std::make_unique(); + initialize_device_statuses_(); logSection("Device Plan"); log_device_plan_(); - pre_scan_robot_arm_dependencies_(); + const bool dependencies_valid = pre_scan_robot_arm_dependencies_(); logSection("Initialize Devices"); - init_devices_(); - configure_mujoco_viewer_pip_(); + if (!dependencies_valid) { + mark_initializing_statuses_error_( + "device dependency validation failed"); + } + const bool devices_initialized = + dependencies_valid ? init_devices_() : false; + initialized_ = dependencies_valid && devices_initialized; + if (initialized_) { + configure_mujoco_viewer_pip_(); + } else { + CMVR_LOG(ERROR) << "[DeviceManager]: Initialization failed for at " + "least one enabled device"; + } } DeviceManager& DeviceManager::getInstance(const config::DeviceManagerConfig& cfg) { @@ -156,34 +216,133 @@ void DeviceManager::destroyInstance() { MotorManager::clearActiveJoints(); } -void DeviceManager::start(){ - for (auto& [id, record] : devices_) { - if (!record.device) { - CMVR_LOG(WARNING) << "[DeviceManager]: Null pointer for device " << id; - continue; - } - if (record.device->start()) { - CMVR_LOG(INFO) << "[DeviceManager]: Start device " << id << " Success"; - } else { - CMVR_LOG(ERROR) << "[DeviceManager]: Start device " << id << " Failed"; +bool DeviceManager::start(){ + std::lock_guard lifecycle_lock(lifecycle_mutex_); + if (!initialized_) { + CMVR_LOG(ERROR) << "[DeviceManager]: Refusing to start because " + "initialization did not complete"; + stop_devices_(false); + return false; + } + + std::vector>> + devices; + { + std::shared_lock lock(devices_mutex_); + devices.reserve(devices_.size()); + for (const auto& [id, record] : devices_) { + devices.emplace_back(id, record.device); } } + + bool all_started = true; + for (const auto& [id, device] : devices) { + if (!device) { + CMVR_LOG(WARNING) << "[DeviceManager]: Null pointer for device " << id; + update_device_status_( + id, ManagedDeviceState::Error, + "cannot start null device: " + id); + all_started = false; + continue; + } + bool started = false; + std::string error_message; + try { + started = device->start(); + if (!started) { + error_message = "device start returned false: " + id; + } + } catch (const std::exception& error) { + error_message = + "device start threw for " + id + ": " + error.what(); + CMVR_LOG(ERROR) << "[DeviceManager]: Start device " << id + << " threw: " << error.what(); + } catch (...) { + error_message = + "device start threw an unknown exception: " + id; + CMVR_LOG(ERROR) << "[DeviceManager]: Start device " << id + << " threw an unknown exception"; + } + if (started) { + update_device_status_(id, ManagedDeviceState::Running); + CMVR_LOG(INFO) << "[DeviceManager]: Start device " << id << " Success"; + } else { + update_device_status_( + id, ManagedDeviceState::Error, error_message); + CMVR_LOG(ERROR) << "[DeviceManager]: Start device " << id << " Failed"; + all_started = false; + } + } + if (!all_started) { + CMVR_LOG(ERROR) << "[DeviceManager]: At least one enabled device failed " + "to start; stopping all devices"; + // Rollback is a physical cleanup operation. Preserve the start + // results in the status table so the failure is diagnosable; an + // explicit stop() records Stopped/Error transitions. + stop_devices_(false); + } + return all_started; } -void DeviceManager::restart() { +bool DeviceManager::restart() { stop(); - start(); + return start(); } void DeviceManager::stop() { - for (auto& [id, record] : devices_) { - if (!record.device) { + std::lock_guard lifecycle_lock(lifecycle_mutex_); + stop_devices_(); +} + +void DeviceManager::stop_devices_(const bool update_status) { + std::vector>> + devices; + { + std::shared_lock lock(devices_mutex_); + devices.reserve(devices_.size()); + for (const auto& [id, record] : devices_) { + devices.emplace_back(id, record.device); + } + } + + for (const auto& [id, device] : devices) { + if (!device) { CMVR_LOG(WARNING) << "[DeviceManager]: Null pointer for device " << id; + if (update_status) { + update_device_status_( + id, ManagedDeviceState::Error, + "cannot stop null device: " + id); + } continue; } - if (record.device->stop()) { + bool stopped = false; + std::string error_message; + try { + stopped = device->stop(); + if (!stopped) { + error_message = "device stop returned false: " + id; + } + } catch (const std::exception& error) { + error_message = + "device stop threw for " + id + ": " + error.what(); + CMVR_LOG(ERROR) << "[DeviceManager]: Stop device " << id + << " threw: " << error.what(); + } catch (...) { + error_message = + "device stop threw an unknown exception: " + id; + CMVR_LOG(ERROR) << "[DeviceManager]: Stop device " << id + << " threw an unknown exception"; + } + if (stopped) { + if (update_status) { + update_device_status_(id, ManagedDeviceState::Stopped); + } CMVR_LOG(INFO) << "[DeviceManager]: Stop device " << id << " Success"; } else { + if (update_status) { + update_device_status_( + id, ManagedDeviceState::Error, error_message); + } CMVR_LOG(ERROR) << "[DeviceManager]: Stop device " << id << " Failed"; } } @@ -192,6 +351,7 @@ void DeviceManager::stop() { template std::shared_ptr DeviceManager::getDevice(const std::string& device_id) { + std::shared_lock lock(devices_mutex_); auto it = devices_.find(device_id); if (it == devices_.end()) { CMVR_LOG(WARNING) << "[DeviceManager]: Device ID " << device_id << " not found."; @@ -219,6 +379,7 @@ std::shared_ptr DeviceManager::getDeviceBase(const std::string& void DeviceManager::getDeviceList(std::list>& device_list){ device_list.clear(); + std::shared_lock lock(devices_mutex_); for (const auto& [device_id, record] : devices_) { device_list.emplace_back(device_id, record.type_name); } @@ -244,22 +405,115 @@ void DeviceManager::registerDevice(const std::string& device_id, CMVR_LOG(ERROR) << "[DeviceManager]: Cannot register device with empty id"; return; } - if (devices_.count(device_id)) { - CMVR_LOG(ERROR) << "[DeviceManager]: Duplicate device ID " << device_id; - return; - } DeviceRecord record; record.id = device_id; record.kind = device->kind(); record.type_name = device->typeName(); record.device = device; - devices_.emplace(record.id, std::move(record)); + { + std::unique_lock lock(devices_mutex_); + if (devices_.count(device_id)) { + CMVR_LOG(ERROR) << "[DeviceManager]: Duplicate device ID " << device_id; + return; + } + + ManagedDeviceSnapshot status; + status.id = record.id; + status.kind = record.kind; + status.type_name = record.type_name; + status.enabled = true; + status.state = ManagedDeviceState::Registered; + status.status_updated_at_unix_ms = unixTimeMs(); + devices_.emplace(record.id, std::move(record)); + device_statuses_[device_id] = std::move(status); + } CMVR_LOG(INFO) << "[DeviceManager]: Register device success" << ", id=" << device_id << ", type=" << device->typeName() << ", kind=" << toString(device->kind()); } +void DeviceManager::initialize_device_statuses_() +{ + std::unique_lock lock(devices_mutex_); + for (const auto& entry : cfg_.devices()) { + const auto kind = deviceTypeToKind(entry.type()); + ManagedDeviceSnapshot status; + status.id = entry.id(); + status.kind = kind; + status.type_name = toString(kind); + status.enabled = entry.enable(); + status.state = entry.enable() + ? ManagedDeviceState::Initializing + : ManagedDeviceState::Disabled; + status.status_updated_at_unix_ms = unixTimeMs(); + + if (entry.id().empty()) { + status.state = ManagedDeviceState::Error; + status.abnormal = true; + status.error_message = + "configured device id must not be empty"; + } + + const auto [it, inserted] = + device_statuses_.emplace(entry.id(), std::move(status)); + if (!inserted) { + auto& duplicate_status = it->second; + duplicate_status.enabled = + duplicate_status.enabled || entry.enable(); + duplicate_status.state = ManagedDeviceState::Error; + duplicate_status.abnormal = true; + duplicate_status.error_message = truncateDeviceError( + "duplicate configured device id: " + entry.id()); + duplicate_status.status_updated_at_unix_ms = unixTimeMs(); + } + } +} + +void DeviceManager::mark_initializing_statuses_error_( + const std::string& error_message) +{ + std::unique_lock lock(devices_mutex_); + for (auto& [id, status] : device_statuses_) { + if (status.state != ManagedDeviceState::Initializing) { + continue; + } + status.state = ManagedDeviceState::Error; + status.abnormal = true; + status.error_message = truncateDeviceError( + error_message + ": " + id); + status.status_updated_at_unix_ms = unixTimeMs(); + } +} + +void DeviceManager::update_device_status_( + const std::string& device_id, + const ManagedDeviceState state, + const std::string& error_message) +{ + std::unique_lock lock(devices_mutex_); + auto& status = device_statuses_[device_id]; + if (status.id.empty()) { + status.id = device_id; + } + const auto device_it = devices_.find(device_id); + if (device_it != devices_.end()) { + status.kind = device_it->second.kind; + status.type_name = device_it->second.type_name; + } + status.enabled = true; + status.state = state; + status.abnormal = state == ManagedDeviceState::Error; + status.error_message = + state == ManagedDeviceState::Error + ? truncateDeviceError( + error_message.empty() + ? "device lifecycle operation failed: " + device_id + : error_message) + : std::string{}; + status.status_updated_at_unix_ms = unixTimeMs(); +} + DeviceManagerSnapshot DeviceManager::snapshot() const { struct SnapshotSource { @@ -270,15 +524,14 @@ DeviceManagerSnapshot DeviceManager::snapshot() const std::vector sources; { std::shared_lock lock(devices_mutex_); - sources.reserve(devices_.size()); - for (const auto& [id, record] : devices_) { + sources.reserve(device_statuses_.size()); + for (const auto& [id, stored_status] : device_statuses_) { SnapshotSource source; - source.status.id = id; - source.status.kind = record.kind; - source.status.type_name = record.type_name; - source.status.enabled = true; - source.status.state = ManagedDeviceState::Ready; - source.device = record.device; + source.status = stored_status; + const auto device_it = devices_.find(id); + if (device_it != devices_.end()) { + source.device = device_it->second.device; + } sources.push_back(std::move(source)); } } @@ -302,10 +555,23 @@ DeviceManagerSnapshot DeviceManager::snapshot() const "device health snapshot threw an unknown exception"; } } - source.status.abnormal = + source.status.health.error_message = + truncateDeviceError(source.status.health.error_message); + const bool lifecycle_error = + source.status.state == ManagedDeviceState::Error; + const bool health_error = source.status.health.state == DeviceHealthState::Degraded || source.status.health.state == DeviceHealthState::Fault; - source.status.error_message = source.status.health.error_message; + source.status.abnormal = lifecycle_error || health_error; + if (source.status.error_message.empty()) { + source.status.error_message = + source.status.health.error_message; + } + source.status.error_message = + truncateDeviceError(source.status.error_message); + if (source.status.status_updated_at_unix_ms == 0) { + source.status.status_updated_at_unix_ms = unixTimeMs(); + } result.devices.push_back(std::move(source.status)); } @@ -354,10 +620,11 @@ void DeviceManager::log_device_plan_() const CMVR_LOG(INFO) << "[DeviceManager]: Device plan end"; } -void DeviceManager::pre_scan_robot_arm_dependencies_() const +bool DeviceManager::pre_scan_robot_arm_dependencies_() const { MotorJointSelections selections; std::unordered_map motor_roots; + MotorManager::clearActiveJoints(); for (const auto& entry : cfg_.devices()) { if (!entry.enable() || entry.type() != config::DeviceConfigEntry::DEVICE_TYPE_MOTOR_SYSTEM) { @@ -365,22 +632,22 @@ void DeviceManager::pre_scan_robot_arm_dependencies_() const } if (entry.id().empty()) { CMVR_LOG(ERROR) << "[DeviceManager]: Enabled MotorManager device id is empty"; - return; + return false; } if (entry.config_file().empty()) { CMVR_LOG(ERROR) << "[DeviceManager]: Enabled MotorManager config_file is empty: " << entry.id(); - return; + return false; } config::MotorRootConfig root_cfg; if (!ConfigHelper::loadConfigFileSilent(entry.config_file(), root_cfg)) { CMVR_LOG(ERROR) << "[DeviceManager]: Failed to load motor config: " << entry.config_file(); - return; + return false; } if (!root_cfg.motor().id().empty() && root_cfg.motor().id() != entry.id()) { CMVR_LOG(ERROR) << "[DeviceManager]: MotorManager entry id '" << entry.id() << "' does not match config id '" << root_cfg.motor().id() << "'"; - return; + return false; } motor_roots.emplace(entry.id(), std::move(root_cfg)); } @@ -391,17 +658,17 @@ void DeviceManager::pre_scan_robot_arm_dependencies_() const } if (entry.id().empty()) { CMVR_LOG(ERROR) << "[DeviceManager]: Enabled RobotArm device id is empty"; - return; + return false; } if (entry.config_file().empty()) { CMVR_LOG(ERROR) << "[DeviceManager]: Enabled RobotArm config_file is empty: " << entry.id(); - return; + return false; } config::ArmRootConfig root_cfg; if (!ConfigHelper::loadConfigFileSilent(entry.config_file(), root_cfg)) { CMVR_LOG(ERROR) << "[DeviceManager]: Failed to load arm config: " << entry.config_file(); - return; + return false; } const config::RobotArmConfig* arm_cfg = nullptr; @@ -414,29 +681,30 @@ void DeviceManager::pre_scan_robot_arm_dependencies_() const if (!arm_cfg) { CMVR_LOG(ERROR) << "[DeviceManager]: RobotArm ID '" << entry.id() << "' not found in config: " << entry.config_file(); - return; + return false; } - if (arm_cfg->backend_case() == config::RobotArmConfig::kVendor) { + if (arm_cfg->backend_case() == config::RobotArmConfig::kVendor || + arm_cfg->backend_case() == config::RobotArmConfig::kUme) { continue; } if (arm_cfg->backend_case() != config::RobotArmConfig::kMotor) { CMVR_LOG(ERROR) << "[DeviceManager]: RobotArm backend is not configured: " << entry.id(); - return; + return false; } const auto& motor_config = arm_cfg->motor(); if (motor_config.motor_system_id().empty()) { CMVR_LOG(ERROR) << "[DeviceManager]: RobotArm missing motor_system_id: " << entry.id(); - return; + return false; } if (motor_config.motor_group_ids_size() == 0) { CMVR_LOG(ERROR) << "[DeviceManager]: RobotArm missing motor_group_ids: " << entry.id(); - return; + return false; } if (motor_config.joint_names_size() == 0) { CMVR_LOG(ERROR) << "[DeviceManager]: RobotArm missing joint_names: " << entry.id(); - return; + return false; } const auto motor_root_it = motor_roots.find(motor_config.motor_system_id()); @@ -444,7 +712,7 @@ void DeviceManager::pre_scan_robot_arm_dependencies_() const CMVR_LOG(ERROR) << "[DeviceManager]: RobotArm '" << entry.id() << "' depends on disabled or missing MotorManager: " << motor_config.motor_system_id(); - return; + return false; } std::unordered_set allowed_groups; @@ -452,7 +720,7 @@ void DeviceManager::pre_scan_robot_arm_dependencies_() const for (const auto& group_id : motor_config.motor_group_ids()) { if (group_id.empty()) { CMVR_LOG(ERROR) << "[DeviceManager]: RobotArm has empty motor_group_id: " << entry.id(); - return; + return false; } allowed_groups.insert(group_id); } @@ -461,7 +729,7 @@ void DeviceManager::pre_scan_robot_arm_dependencies_() const for (const auto& joint_name : motor_config.joint_names()) { if (joint_name.empty()) { CMVR_LOG(ERROR) << "[DeviceManager]: RobotArm has empty joint_name: " << entry.id(); - return; + return false; } std::string matched_group; @@ -480,7 +748,7 @@ void DeviceManager::pre_scan_robot_arm_dependencies_() const CMVR_LOG(ERROR) << "[DeviceManager]: RobotArm '" << entry.id() << "' joint '" << joint_name << "' not found in configured motor_group_ids"; - return; + return false; } group_selection[matched_group].insert(joint_name); } @@ -495,46 +763,132 @@ void DeviceManager::pre_scan_robot_arm_dependencies_() const } } - MotorManager::clearActiveJoints(); for (auto& [motor_system_id, group_selection] : selections) { MotorManager::setActiveJoints(motor_system_id, std::move(group_selection)); } + return true; } -void DeviceManager::init_devices_() { +bool DeviceManager::init_devices_() { + bool all_initialized = true; for (const auto& entry : cfg_.devices()) { if (!entry.enable()) { continue; } + { + std::shared_lock lock(devices_mutex_); + const auto status_it = device_statuses_.find(entry.id()); + if (status_it != device_statuses_.end() && + status_it->second.state == ManagedDeviceState::Error) { + all_initialized = false; + continue; + } + } + CMVR_LOG(INFO) << "[DeviceManager]: Initialize device begin" << ", id=" << entry.id() << ", type=" << deviceTypeToString(entry.type()) << ", config_file=" << ConfigHelper::resolveConfigFile(entry.config_file()); - DeviceRecord record = dev_factory_->create(entry); + DeviceRecord record; + try { + record = dev_factory_->create(entry); + } catch (const std::exception& error) { + update_device_status_( + entry.id(), ManagedDeviceState::Error, + "device creation threw for " + entry.id() + ": " + + error.what()); + CMVR_LOG(ERROR) << "[DeviceManager]: Device creation threw for " + << entry.id() << ": " << error.what(); + all_initialized = false; + continue; + } catch (...) { + update_device_status_( + entry.id(), ManagedDeviceState::Error, + "device creation threw an unknown exception: " + + entry.id()); + CMVR_LOG(ERROR) << "[DeviceManager]: Device creation threw an " + "unknown exception for " << entry.id(); + all_initialized = false; + continue; + } if (!record.device || record.id.empty()) { + update_device_status_( + entry.id(), ManagedDeviceState::Error, + "failed to create configured device: " + entry.id()); CMVR_LOG(ERROR) << "[DeviceManager]: Failed to create device for entry id=" << entry.id(); + all_initialized = false; continue; } CMVR_LOG(INFO) << "[DeviceManager]: Create device object success" << ", id=" << record.id << ", type=" << record.type_name << ", kind=" << toString(record.kind); - if (devices_.count(record.id)) { + bool duplicate_device = false; + { + std::shared_lock lock(devices_mutex_); + duplicate_device = devices_.count(record.id) != 0; + } + if (duplicate_device) { + update_device_status_( + entry.id(), ManagedDeviceState::Error, + "duplicate configured device id: " + record.id); CMVR_LOG(ERROR) << "[DeviceManager]: Duplicate " << record.type_name << " Device ID " << record.id; + all_initialized = false; continue; } CMVR_LOG(INFO) << "[DeviceManager]: Init device object begin" << ", id=" << record.id << ", type=" << record.type_name << ", kind=" << toString(record.kind); - if (!record.device->init()) { + bool device_initialized = false; + std::string init_error_message; + try { + device_initialized = record.device->init(); + if (!device_initialized) { + init_error_message = + "device init returned false: " + record.id; + } + } catch (const std::exception& error) { + init_error_message = + "device init threw for " + record.id + ": " + + error.what(); + CMVR_LOG(ERROR) << "[DeviceManager]: Init device object threw" + << ", id=" << record.id + << ", error=" << error.what(); + } catch (...) { + init_error_message = + "device init threw an unknown exception: " + record.id; + CMVR_LOG(ERROR) << "[DeviceManager]: Init device object threw an " + "unknown exception, id=" << record.id; + } + if (!device_initialized) { + update_device_status_( + entry.id(), ManagedDeviceState::Error, + init_error_message); CMVR_LOG(ERROR) << "[DeviceManager]: Init device object failed" << ", id=" << record.id << ", type=" << record.type_name << ", kind=" << toString(record.kind) << ", config_file=" << entry.config_file(); + try { + if (!record.device->stop()) { + CMVR_LOG(ERROR) + << "[DeviceManager]: Cleanup after failed init " + "returned false, id=" << record.id; + } + } catch (const std::exception& error) { + CMVR_LOG(ERROR) + << "[DeviceManager]: Cleanup after failed init threw" + << ", id=" << record.id + << ", error=" << error.what(); + } catch (...) { + CMVR_LOG(ERROR) + << "[DeviceManager]: Cleanup after failed init threw an " + "unknown exception, id=" << record.id; + } + all_initialized = false; continue; } CMVR_LOG(INFO) << "[DeviceManager]: Init device object success" @@ -542,8 +896,36 @@ void DeviceManager::init_devices_() { << ", type=" << record.type_name << ", kind=" << toString(record.kind) << ", config_file=" << entry.config_file(); - devices_.emplace(record.id, std::move(record)); + { + std::unique_lock lock(devices_mutex_); + const auto id = record.id; + const auto kind = record.kind; + const auto type_name = record.type_name; + const auto [device_it, inserted] = + devices_.emplace(id, std::move(record)); + if (!inserted) { + auto& status = device_statuses_[entry.id()]; + status.state = ManagedDeviceState::Error; + status.abnormal = true; + status.error_message = truncateDeviceError( + "duplicate configured device id: " + id); + status.status_updated_at_unix_ms = unixTimeMs(); + all_initialized = false; + continue; + } + + auto& status = device_statuses_[id]; + status.id = id; + status.kind = kind; + status.type_name = type_name; + status.enabled = true; + status.state = ManagedDeviceState::Ready; + status.abnormal = false; + status.error_message.clear(); + status.status_updated_at_unix_ms = unixTimeMs(); + } } + return all_initialized; } void DeviceManager::configure_mujoco_viewer_pip_() diff --git a/cmvr-es/manager/device_manager/tests/device_manager_lifecycle_test.cpp b/cmvr-es/manager/device_manager/tests/device_manager_lifecycle_test.cpp new file mode 100644 index 00000000..a9009350 --- /dev/null +++ b/cmvr-es/manager/device_manager/tests/device_manager_lifecycle_test.cpp @@ -0,0 +1,124 @@ +#include "manager/device_manager/include/device_manager.h" + +#include + +#include + +namespace { + +class LifecycleDevice final : public cmvr::device::AbstractDevice { +public: + explicit LifecycleDevice(const std::string& id) + : AbstractDevice(id) + { + } + + cmvr::device::DeviceKind kind() const noexcept override + { + return cmvr::device::DeviceKind::Camera; + } + + std::string typeName() const override { return "LifecycleDevice"; } + + bool start() override + { + ++start_calls; + if (throw_on_start) { + throw std::runtime_error("start failure"); + } + return start_result; + } + + bool stop() override + { + ++stop_calls; + return true; + } + + bool start_result{true}; + bool throw_on_start{false}; + int start_calls{0}; + int stop_calls{0}; +}; + +class DeviceManagerLifecycleTest : public ::testing::Test { +protected: + void SetUp() override + { + cmvr::device::DeviceManager::destroyInstance(); + } + + void TearDown() override + { + cmvr::device::DeviceManager::destroyInstance(); + } +}; + +TEST_F(DeviceManagerLifecycleTest, + EnabledDeviceCreationFailureMarksInitializationFailed) +{ + cmvr::config::DeviceManagerConfig config; + auto* entry = config.add_devices(); + entry->set_id("unsupported"); + entry->set_type( + cmvr::config::DeviceConfigEntry::DEVICE_TYPE_UNKNOWN); + entry->set_enable(true); + + auto& manager = + cmvr::device::DeviceManager::getInstance(config); + EXPECT_FALSE(manager.initialized()); + EXPECT_FALSE(manager.start()); +} + +TEST_F(DeviceManagerLifecycleTest, DisabledInvalidDeviceIsIgnored) +{ + cmvr::config::DeviceManagerConfig config; + auto* entry = config.add_devices(); + entry->set_id("disabled"); + entry->set_type( + cmvr::config::DeviceConfigEntry::DEVICE_TYPE_UNKNOWN); + entry->set_enable(false); + + auto& manager = + cmvr::device::DeviceManager::getInstance(config); + EXPECT_TRUE(manager.initialized()); + EXPECT_TRUE(manager.start()); +} + +TEST_F(DeviceManagerLifecycleTest, + DeviceStartFailureIsReturnedAndTriggersStop) +{ + cmvr::config::DeviceManagerConfig config; + auto& manager = + cmvr::device::DeviceManager::getInstance(config); + ASSERT_TRUE(manager.initialized()); + + auto device = + std::make_shared("start_failure"); + device->start_result = false; + manager.registerDevice(device); + + EXPECT_FALSE(manager.start()); + EXPECT_EQ(device->start_calls, 1); + EXPECT_EQ(device->stop_calls, 1); +} + +TEST_F(DeviceManagerLifecycleTest, + DeviceStartExceptionIsReturnedAndTriggersStop) +{ + cmvr::config::DeviceManagerConfig config; + auto& manager = + cmvr::device::DeviceManager::getInstance(config); + ASSERT_TRUE(manager.initialized()); + + auto device = + std::make_shared("start_exception"); + device->throw_on_start = true; + manager.registerDevice(device); + + EXPECT_FALSE(manager.start()); + EXPECT_EQ(device->start_calls, 1); + EXPECT_EQ(device->stop_calls, 1); +} + +} // namespace diff --git a/cmvr-es/manager/device_manager/tests/device_manager_snapshot_test.cpp b/cmvr-es/manager/device_manager/tests/device_manager_snapshot_test.cpp index 05d5bf8c..a5191824 100644 --- a/cmvr-es/manager/device_manager/tests/device_manager_snapshot_test.cpp +++ b/cmvr-es/manager/device_manager/tests/device_manager_snapshot_test.cpp @@ -253,6 +253,13 @@ bool testConfiguredAndDynamicSnapshots() CHECK_TRUE(duplicate_status->error_message == "duplicate configured device id: duplicate_device"); + // Configuration failures deliberately make this manager ineligible for + // start(). Use a fresh, valid manager for dynamic registration and + // lifecycle transitions so the test does not weaken fail-closed startup. + DeviceManager::destroyInstance(); + cmvr::config::DeviceManagerConfig dynamic_config; + auto& dynamic_manager = DeviceManager::getInstance(dynamic_config); + auto healthy = std::make_shared("z_healthy"); auto degraded = std::make_shared("a_degraded"); degraded->health = { @@ -264,18 +271,18 @@ bool testConfiguredAndDynamicSnapshots() auto health_throw = std::make_shared("b_health_throw"); health_throw->throw_on_health = true; - manager.registerDevice(healthy); - manager.registerDevice(degraded); - manager.registerDevice(start_fail); - manager.registerDevice(stop_fail); - manager.registerDevice(health_throw); + dynamic_manager.registerDevice(healthy); + dynamic_manager.registerDevice(degraded); + dynamic_manager.registerDevice(start_fail); + dynamic_manager.registerDevice(stop_fail); + dynamic_manager.registerDevice(health_throw); // Duplicate registration must retain the original object and status. - manager.registerDevice( + dynamic_manager.registerDevice( std::make_shared("z_healthy", DeviceKind::Speaker)); - CHECK_TRUE(manager.getDeviceBase("z_healthy") == healthy); + CHECK_TRUE(dynamic_manager.getDeviceBase("z_healthy") == healthy); - const auto registered = manager.snapshot(); + const auto registered = dynamic_manager.snapshot(); CHECK_TRUE(isSorted(registered)); const auto* healthy_registered = findDevice(registered, "z_healthy"); @@ -304,8 +311,8 @@ bool testConfiguredAndDynamicSnapshots() CHECK_TRUE(thrown_health->health.error_message.size() <= 512); CHECK_TRUE(thrown_health->error_message.size() <= 512); - manager.start(); - const auto running = manager.snapshot(); + CHECK_TRUE(!dynamic_manager.start()); + const auto running = dynamic_manager.snapshot(); CHECK_TRUE(findDevice(running, "z_healthy")->state == ManagedDeviceState::Running); CHECK_TRUE(findDevice(running, "m_start_fail")->state == @@ -319,14 +326,16 @@ bool testConfiguredAndDynamicSnapshots() CHECK_TRUE(healthy_registered->state == ManagedDeviceState::Registered); - manager.stop(); - const auto stopped = manager.snapshot(); + dynamic_manager.stop(); + const auto stopped = dynamic_manager.snapshot(); CHECK_TRUE(findDevice(stopped, "z_healthy")->state == ManagedDeviceState::Stopped); CHECK_TRUE(findDevice(stopped, "n_stop_fail")->state == ManagedDeviceState::Error); CHECK_TRUE(findDevice(stopped, "n_stop_fail")->abnormal); - CHECK_TRUE(healthy->stop_calls.load() == 1); + // Failed start rolls back every device once; explicit stop performs the + // second best-effort stop. + CHECK_TRUE(healthy->stop_calls.load() == 2); return true; } diff --git a/cmvr-es/manager/task_manager/CMakeLists.txt b/cmvr-es/manager/task_manager/CMakeLists.txt index 40b1eb48..12768c1d 100644 --- a/cmvr-es/manager/task_manager/CMakeLists.txt +++ b/cmvr-es/manager/task_manager/CMakeLists.txt @@ -15,3 +15,29 @@ target_link_libraries(task_manager add_library(cmvr_es::task_manager ALIAS task_manager) install(TARGETS task_manager LIBRARY DESTINATION lib) + +if(BUILD_TESTING) + add_executable(task_manager_lifecycle_test + tests/task_manager_lifecycle_test.cpp + ) + target_link_libraries(task_manager_lifecycle_test PRIVATE + cmvr_es::task_manager + gtest + gtest_main + pthread + ) + add_test( + NAME task_manager_lifecycle_test + COMMAND task_manager_lifecycle_test + ) + set(_task_manager_lifecycle_test_environment + "LD_LIBRARY_PATH=${CMVR_TEST_EXTERNAL_LIBRARY_PATH}") + if(CMVR_TEST_SYSTEM_LIBSTDCXX) + list(APPEND _task_manager_lifecycle_test_environment + "LD_PRELOAD=${CMVR_TEST_SYSTEM_LIBSTDCXX}") + endif() + set_tests_properties(task_manager_lifecycle_test PROPERTIES + TIMEOUT 10 + ENVIRONMENT "${_task_manager_lifecycle_test_environment}" + ) +endif() diff --git a/cmvr-es/manager/task_manager/include/task_manager.h b/cmvr-es/manager/task_manager/include/task_manager.h index b1d0609f..339f95c6 100644 --- a/cmvr-es/manager/task_manager/include/task_manager.h +++ b/cmvr-es/manager/task_manager/include/task_manager.h @@ -26,9 +26,10 @@ namespace cmvr::task { ~TaskManager(); - void startRunTask(double control_period_s = 0.001); + bool startRunTask(double control_period_s = 0.001); void stopRunTask(); bool running() const { return running_.load(); } + bool initialized() const noexcept { return initialized_; } std::shared_ptr getTask(const std::string& task_id) const; std::shared_ptr getTouchScreenTask(const std::string& task_id = "touch_screen") const; @@ -37,7 +38,7 @@ namespace cmvr::task { explicit TaskManager(const config::TaskManagerConfig& cfg); void logTaskPlan() const; - void initTasks(); + bool initTasks(); void runTaskLoop(double control_period_s); static TaskRunMode toTaskRunMode(config::TaskConfigEntry::TaskRunMode run_mode); @@ -49,8 +50,10 @@ namespace cmvr::task { std::unordered_map task_period_s_; std::unordered_map next_step_time_; mutable std::mutex tasks_mutex_; + std::mutex lifecycle_mutex_; std::atomic running_{false}; std::thread run_thread_; + bool initialized_{false}; }; } // namespace cmvr::task diff --git a/cmvr-es/manager/task_manager/src/task_manager.cpp b/cmvr-es/manager/task_manager/src/task_manager.cpp index a8af771c..cb34859d 100644 --- a/cmvr-es/manager/task_manager/src/task_manager.cpp +++ b/cmvr-es/manager/task_manager/src/task_manager.cpp @@ -40,6 +40,8 @@ const char* taskConfigTypeToString(const config::TaskConfigEntry::TaskType type) return "TASK_TYPE_SELF_COLLISION"; case config::TaskConfigEntry::TASK_TYPE_QUIC_EDGE: return "TASK_TYPE_QUIC_EDGE"; + case config::TaskConfigEntry::TASK_TYPE_UME_TELEOP: + return "TASK_TYPE_UME_TELEOP"; case config::TaskConfigEntry::TASK_TYPE_UNKNOWN: default: return "TASK_TYPE_UNKNOWN"; @@ -59,6 +61,21 @@ const char* taskConfigRunModeToString(const config::TaskConfigEntry::TaskRunMode } } +void stopTaskNoThrow(const std::shared_ptr& task) +{ + if (!task) { + return; + } + try { + task->stop(); + } catch (const std::exception& error) { + CMVR_LOG(ERROR) << "[TaskManager] task stop threw: " + << error.what(); + } catch (...) { + CMVR_LOG(ERROR) << "[TaskManager] task stop threw an unknown exception"; + } +} + } // namespace std::shared_ptr TaskManager::instance_ = nullptr; @@ -70,7 +87,11 @@ TaskManager::TaskManager(const config::TaskManagerConfig& cfg) logSection("Task Plan"); logTaskPlan(); logSection("Initialize Tasks"); - initTasks(); + initialized_ = initTasks(); + if (!initialized_) { + CMVR_LOG(ERROR) << "[TaskManager] Initialization failed for at least " + "one enabled task"; + } } TaskManager::~TaskManager() @@ -126,16 +147,20 @@ std::shared_ptr TaskManager::getTask(const std::string& task_id) const return it->second; } -void TaskManager::startRunTask(const double control_period_s) +bool TaskManager::startRunTask(const double control_period_s) { + std::lock_guard lifecycle_lock(lifecycle_mutex_); + if (!initialized_) { + CMVR_LOG(ERROR) << "[TaskManager] refusing to start because " + "initialization did not complete"; + return false; + } if (!std::isfinite(control_period_s) || control_period_s <= 0.0) { CMVR_LOG(ERROR) << "[TaskManager] invalid control_period_s"; - return; + return false; } - - bool expected = false; - if (!running_.compare_exchange_strong(expected, true)) { - return; + if (running_.load()) { + return true; } std::vector> tasks; @@ -151,39 +176,57 @@ void TaskManager::startRunTask(const double control_period_s) std::vector> started_tasks; for (const auto& task : tasks) { - if (!task->start()) { + bool started = false; + try { + started = task->start(); + } catch (const std::exception& error) { + CMVR_LOG(ERROR) << "[TaskManager] task start threw: " + << task->id() << ", error=" << error.what(); + } catch (...) { + CMVR_LOG(ERROR) << "[TaskManager] task start threw an unknown " + "exception: " << task->id(); + } + if (!started) { CMVR_LOG(ERROR) << "[TaskManager] task start failed: " << task->id(); - for (const auto& started_task : started_tasks) { - try { - started_task->stop(); - } catch (...) { - } + stopTaskNoThrow(task); + for (auto it = started_tasks.rbegin(); + it != started_tasks.rend(); ++it) { + stopTaskNoThrow(*it); } running_.store(false); - return; + return false; } started_tasks.push_back(task); } + running_.store(true); try { run_thread_ = std::thread(&TaskManager::runTaskLoop, this, control_period_s); } catch (const std::exception& e) { CMVR_LOG(ERROR) << "[TaskManager] failed to start run thread: " << e.what(); running_.store(false); - for (const auto& task : started_tasks) { - try { - task->stop(); - } catch (...) { - } + for (auto it = started_tasks.rbegin(); + it != started_tasks.rend(); ++it) { + stopTaskNoThrow(*it); } - return; + return false; + } catch (...) { + CMVR_LOG(ERROR) << "[TaskManager] failed to start run thread with an " + "unknown exception"; + running_.store(false); + for (auto it = started_tasks.rbegin(); + it != started_tasks.rend(); ++it) { + stopTaskNoThrow(*it); + } + return false; } + return true; } void TaskManager::stopRunTask() { - bool expected = true; - if (!running_.compare_exchange_strong(expected, false)) { + std::lock_guard lifecycle_lock(lifecycle_mutex_); + if (!running_.exchange(false)) { return; } @@ -202,21 +245,20 @@ void TaskManager::stopRunTask() } } for (const auto& task : tasks) { - try { - task->stop(); - } catch (...) { - } + stopTaskNoThrow(task); } } -void TaskManager::initTasks() +bool TaskManager::initTasks() { + bool all_initialized = true; for (const auto& entry : cfg_.tasks()) { if (!entry.enable()) { continue; } if (entry.id().empty()) { CMVR_LOG(ERROR) << "[TaskManager] Task ID is empty"; + all_initialized = false; continue; } @@ -226,9 +268,30 @@ void TaskManager::initTasks() << ", run_mode=" << taskConfigRunModeToString(entry.run_mode()) << ", config_file=" << ConfigHelper::resolveConfigFile(entry.config_file()); - auto task = TaskFactory::create(entry); + std::shared_ptr task; + try { + task = TaskFactory::create(entry); + } catch (const std::exception& error) { + CMVR_LOG(ERROR) << "[TaskManager] Task creation threw: " + << entry.id() << ", error=" << error.what(); + all_initialized = false; + continue; + } catch (...) { + CMVR_LOG(ERROR) << "[TaskManager] Task creation threw an unknown " + "exception: " << entry.id(); + all_initialized = false; + continue; + } if (!task || task->id() != entry.id()) { CMVR_LOG(ERROR) << "[TaskManager] Task ID mismatch: " << entry.id(); + all_initialized = false; + continue; + } + if (entry.run_mode() == + config::TaskConfigEntry::TASK_RUN_MODE_UNKNOWN) { + CMVR_LOG(ERROR) << "[TaskManager] Task run_mode is unknown: " + << entry.id(); + all_initialized = false; continue; } const TaskRunMode configured_run_mode = toTaskRunMode(entry.run_mode()); @@ -236,23 +299,39 @@ void TaskManager::initTasks() CMVR_LOG(ERROR) << "[TaskManager] Task run_mode mismatch: id=" << entry.id() << ", configured=" << taskRunModeToString(configured_run_mode) << ", actual=" << taskRunModeToString(task->runMode()); + all_initialized = false; continue; } double control_period_s = entry.control_period_s(); if (configured_run_mode == TaskRunMode::PERIODIC_STEP && (!std::isfinite(control_period_s) || control_period_s <= 0.0)) { CMVR_LOG(ERROR) << "[TaskManager] invalid control_period_s for task: " << entry.id(); + all_initialized = false; continue; } - if (!task->init()) { + bool task_initialized = false; + try { + task_initialized = task->init(); + } catch (const std::exception& error) { + CMVR_LOG(ERROR) << "[TaskManager] Task init threw: " + << entry.id() << ", error=" << error.what(); + } catch (...) { + CMVR_LOG(ERROR) << "[TaskManager] Task init threw an unknown " + "exception: " << entry.id(); + } + if (!task_initialized) { CMVR_LOG(ERROR) << "[TaskManager] Task init failed: " << entry.id() << ", status=" << task->detailStatusString(); + stopTaskNoThrow(task); + all_initialized = false; continue; } { std::lock_guard lock(tasks_mutex_); if (tasks_.count(entry.id())) { CMVR_LOG(ERROR) << "[TaskManager] Duplicate task ID: " << entry.id(); + stopTaskNoThrow(task); + all_initialized = false; continue; } if (configured_run_mode == TaskRunMode::PERIODIC_STEP) { @@ -261,6 +340,7 @@ void TaskManager::initTasks() tasks_.emplace(entry.id(), std::move(task)); } } + return all_initialized; } void TaskManager::logTaskPlan() const diff --git a/cmvr-es/manager/task_manager/tests/task_manager_lifecycle_test.cpp b/cmvr-es/manager/task_manager/tests/task_manager_lifecycle_test.cpp new file mode 100644 index 00000000..5efeeb12 --- /dev/null +++ b/cmvr-es/manager/task_manager/tests/task_manager_lifecycle_test.cpp @@ -0,0 +1,194 @@ +#include "manager/task_manager/include/task_manager.h" + +#include +#include + +#include + +#include "task/task_factory.h" + +namespace { + +struct TaskBehavior { + bool init_result{true}; + bool start_result{true}; + bool throw_on_start{false}; +}; + +TaskBehavior task_behavior; + +class LifecycleTask final : public cmvr::task::Task { +public: + explicit LifecycleTask(std::string id) + : id_(std::move(id)) + { + } + + const std::string& id() const override { return id_; } + cmvr::task::TaskRunMode runMode() const override + { + return cmvr::task::TaskRunMode::BLOCKING_SERVICE; + } + + bool init() override + { + ++init_calls; + state_ = task_behavior.init_result + ? cmvr::task::TaskState::IDLE + : cmvr::task::TaskState::FAILED; + return task_behavior.init_result; + } + + bool start() override + { + ++start_calls; + if (task_behavior.throw_on_start) { + throw std::runtime_error("start failure"); + } + state_ = task_behavior.start_result + ? cmvr::task::TaskState::RUNNING + : cmvr::task::TaskState::FAILED; + return task_behavior.start_result; + } + + bool step(double) override { return true; } + + void stop() override + { + ++stop_calls; + state_ = cmvr::task::TaskState::STOPPED; + } + + cmvr::task::TaskState state() const override { return state_; } + bool isBusy() const override + { + return state_ == cmvr::task::TaskState::RUNNING; + } + bool isFinished() const override + { + return state_ == cmvr::task::TaskState::STOPPED; + } + bool isFailed() const override + { + return state_ == cmvr::task::TaskState::FAILED; + } + std::string stateString() const override + { + return cmvr::task::taskStateToString(state_); + } + std::string detailStatusString() const override + { + return stateString(); + } + + int init_calls{0}; + int start_calls{0}; + int stop_calls{0}; + +private: + std::string id_; + cmvr::task::TaskState state_{ + cmvr::task::TaskState::UNINITIALIZED}; +}; + +std::shared_ptr created_task; + +cmvr::config::TaskManagerConfig enabledTaskConfig() +{ + cmvr::config::TaskManagerConfig config; + auto* entry = config.add_tasks(); + entry->set_id("lifecycle_task"); + entry->set_type( + cmvr::config::TaskConfigEntry::TASK_TYPE_UME_TELEOP); + entry->set_enable(true); + entry->set_run_mode( + cmvr::config::TaskConfigEntry:: + TASK_RUN_MODE_BLOCKING_SERVICE); + return config; +} + +class TaskManagerLifecycleTest : public ::testing::Test { +protected: + void SetUp() override + { + cmvr::task::TaskManager::destroyInstance(); + task_behavior = {}; + created_task.reset(); + cmvr::task::TaskFactory::registerCreator( + cmvr::config::TaskConfigEntry::TASK_TYPE_UME_TELEOP, + [](const cmvr::config::TaskConfigEntry& entry) { + created_task = + std::make_shared(entry.id()); + return created_task; + }); + } + + void TearDown() override + { + cmvr::task::TaskManager::destroyInstance(); + created_task.reset(); + } +}; + +TEST_F(TaskManagerLifecycleTest, + EnabledTaskInitFailureMarksInitializationFailed) +{ + task_behavior.init_result = false; + auto& manager = + cmvr::task::TaskManager::getInstance(enabledTaskConfig()); + + ASSERT_NE(created_task, nullptr); + EXPECT_FALSE(manager.initialized()); + EXPECT_FALSE(manager.startRunTask()); + EXPECT_FALSE(manager.running()); + EXPECT_EQ(created_task->init_calls, 1); + EXPECT_EQ(created_task->start_calls, 0); + EXPECT_EQ(created_task->stop_calls, 1); +} + +TEST_F(TaskManagerLifecycleTest, + TaskStartFailureIsReturnedAndRunningRemainsFalse) +{ + task_behavior.start_result = false; + auto& manager = + cmvr::task::TaskManager::getInstance(enabledTaskConfig()); + + ASSERT_TRUE(manager.initialized()); + ASSERT_NE(created_task, nullptr); + EXPECT_FALSE(manager.startRunTask()); + EXPECT_FALSE(manager.running()); + EXPECT_EQ(created_task->start_calls, 1); + EXPECT_EQ(created_task->stop_calls, 1); +} + +TEST_F(TaskManagerLifecycleTest, + TaskStartExceptionIsReturnedAndRunningRemainsFalse) +{ + task_behavior.throw_on_start = true; + auto& manager = + cmvr::task::TaskManager::getInstance(enabledTaskConfig()); + + ASSERT_TRUE(manager.initialized()); + ASSERT_NE(created_task, nullptr); + EXPECT_FALSE(manager.startRunTask()); + EXPECT_FALSE(manager.running()); + EXPECT_EQ(created_task->start_calls, 1); + EXPECT_EQ(created_task->stop_calls, 1); +} + +TEST_F(TaskManagerLifecycleTest, SuccessfulStartAndStopAreReported) +{ + auto& manager = + cmvr::task::TaskManager::getInstance(enabledTaskConfig()); + + ASSERT_TRUE(manager.initialized()); + ASSERT_NE(created_task, nullptr); + EXPECT_TRUE(manager.startRunTask()); + EXPECT_TRUE(manager.running()); + manager.stopRunTask(); + EXPECT_FALSE(manager.running()); + EXPECT_EQ(created_task->start_calls, 1); + EXPECT_EQ(created_task->stop_calls, 1); +} + +} // namespace diff --git a/cmvr-es/runtime/CMakeLists.txt b/cmvr-es/runtime/CMakeLists.txt index f8a9a628..91c68a2c 100644 --- a/cmvr-es/runtime/CMakeLists.txt +++ b/cmvr-es/runtime/CMakeLists.txt @@ -13,8 +13,39 @@ target_link_libraries(cmvr_runtime PUBLIC cmvr_es::task_manager cmvr_es::service cmvr_es::quic_edge_task + cmvr_es::ume_teleop_task cmvr_es::mujoco_viewer ) add_library(cmvr_es::runtime ALIAS cmvr_runtime) install(TARGETS cmvr_runtime LIBRARY DESTINATION lib ARCHIVE DESTINATION lib) + +if(BUILD_TESTING) + add_executable(runtime_lifecycle_test + tests/runtime_lifecycle_test.cpp + ) + target_compile_features(runtime_lifecycle_test PRIVATE cxx_std_17) + target_include_directories(runtime_lifecycle_test PRIVATE + ${CMAKE_SOURCE_DIR}/cmvr-es + ) + target_link_libraries(runtime_lifecycle_test PRIVATE + cmvr_es::runtime + gtest + gtest_main + pthread + ) + add_test( + NAME runtime_lifecycle_test + COMMAND runtime_lifecycle_test + ) + set(_runtime_lifecycle_test_environment + "LD_LIBRARY_PATH=${CMVR_TEST_EXTERNAL_LIBRARY_PATH}") + if(CMVR_TEST_SYSTEM_LIBSTDCXX) + list(APPEND _runtime_lifecycle_test_environment + "LD_PRELOAD=${CMVR_TEST_SYSTEM_LIBSTDCXX}") + endif() + set_tests_properties(runtime_lifecycle_test PROPERTIES + TIMEOUT 20 + ENVIRONMENT "${_runtime_lifecycle_test_environment}" + ) +endif() diff --git a/cmvr-es/runtime/src/cmvr_runtime.cpp b/cmvr-es/runtime/src/cmvr_runtime.cpp index 6f0378ae..b39d51d7 100644 --- a/cmvr-es/runtime/src/cmvr_runtime.cpp +++ b/cmvr-es/runtime/src/cmvr_runtime.cpp @@ -12,6 +12,7 @@ #include "common/io/proto_file_io.h" #include "task/grpc_server_task/include/grpc_server_task.h" #include "task/quic_edge_task/include/quic_edge_task.h" +#include "task/ume_teleop_task/include/ume_teleop_task.h" namespace cmvr { namespace { @@ -100,13 +101,27 @@ bool Runtime::init_(const std::string& config_path, return false; } CMVR_LOG(INFO) << "[Startup] Initialize DeviceManager"; - device::DeviceManager::getInstance(device_manager_root.device_manager()); + auto& device_manager = + device::DeviceManager::getInstance( + device_manager_root.device_manager()); + if (!device_manager.initialized()) { + CMVR_LOG(ERROR) << "[Startup] DeviceManager initialization failed"; + device_manager.stop(); + device::DeviceManager::destroyInstance(); + return false; + } + const auto rollback_device_manager = [&device_manager]() { + device_manager.stop(); + device::DeviceManager::destroyInstance(); + }; task::registerGrpcServerTaskFactory(); task::registerQuicEdgeTaskFactory(); + task::registerUmeTeleopTaskFactory(); if (app_config.task_manager_config_file().empty()) { CMVR_LOG(ERROR) << "TaskManager config file is empty"; + rollback_device_manager(); return false; } logSection("TaskManager"); @@ -116,11 +131,19 @@ bool Runtime::init_(const std::string& config_path, if (!ConfigHelper::loadConfigFile(app_config.task_manager_config_file(), task_manager_root)) { CMVR_LOG(ERROR) << "Failed to load TaskManager config: " << app_config.task_manager_config_file(); + rollback_device_manager(); return false; } CMVR_LOG(INFO) << "[Startup] Initialize TaskManager"; - task::TaskManager::getInstance(task_manager_root.task_manager()); + auto& task_manager = + task::TaskManager::getInstance(task_manager_root.task_manager()); + if (!task_manager.initialized()) { + CMVR_LOG(ERROR) << "[Startup] TaskManager initialization failed"; + task::TaskManager::destroyInstance(); + rollback_device_manager(); + return false; + } initialized_ = true; return true; } @@ -134,9 +157,26 @@ bool Runtime::startTasks(const double control_period_s) return true; } + // Device init() constructs and validates resources; start() owns worker + // threads. Start devices before any task can publish commands or sample + // them. UME start remains passive and never enables actuators. + logSection("Start Devices"); + CMVR_LOG(INFO) << "[Startup] Start devices"; + if (!device::DeviceManager::getInstance().start()) { + CMVR_LOG(ERROR) << "[Startup] One or more enabled devices failed " + "to start; tasks will not be started"; + return false; + } + logSection("Start Tasks"); CMVR_LOG(INFO) << "[Startup] Start tasks"; - task::TaskManager::getInstance().startRunTask(control_period_s); + if (!task::TaskManager::getInstance().startRunTask(control_period_s)) { + CMVR_LOG(ERROR) << "[Startup] One or more enabled tasks failed " + "to start; stopping devices"; + device::DeviceManager::getInstance().stop(); + tasks_started_ = false; + return false; + } tasks_started_ = true; return true; } diff --git a/cmvr-es/runtime/tests/runtime_lifecycle_test.cpp b/cmvr-es/runtime/tests/runtime_lifecycle_test.cpp new file mode 100644 index 00000000..60f8d7ca --- /dev/null +++ b/cmvr-es/runtime/tests/runtime_lifecycle_test.cpp @@ -0,0 +1,266 @@ +#include "runtime/include/cmvr_runtime.h" + +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include + +#include "cmvr/config/cmvr_es_config/cmvr_es_config.pb.h" +#include "cmvr/config/device_manager_config/device_manager_config.pb.h" +#include "cmvr/config/grpc_server_config/grpc_server_config.pb.h" +#include "cmvr/config/logger_config/logger_config.pb.h" +#include "cmvr/config/task_manager_config/task_manager_config.pb.h" +#include "common/io/proto_file_io.h" + +namespace { + +class TempConfigTree { +public: + TempConfigTree() + { + std::array pattern{}; + const std::string value = + "/tmp/cmvr-runtime-lifecycle-XXXXXX"; + std::copy(value.begin(), value.end(), pattern.begin()); + char* created = ::mkdtemp(pattern.data()); + if (created) { + root_ = created; + } + } + + ~TempConfigTree() + { + if (root_.empty()) { + return; + } + std::error_code error; + std::filesystem::remove_all(root_, error); + } + + bool valid() const { return !root_.empty(); } + + template + bool write(const std::string& name, const Message& message) const + { + return ProtoMessageIo::setProtoToAsciiFile( + message, (root_ / name).string()); + } + + std::string path(const std::string& name) const + { + return (root_ / name).string(); + } + + bool writeLogger() const + { + cmvr::config::LoggerRootConfig root; + auto* logger = root.mutable_logger(); + logger->set_minimum_level( + cmvr::config::LOG_LEVEL_INFO); + auto* route = logger->add_routes(); + route->set_level(cmvr::config::LOG_LEVEL_INFO); + route->set_terminal(false); + route->set_file(false); + return write("logger.pb.txt", root); + } + + bool writeRoot() const + { + cmvr::config::CMVRESRootConfig root; + auto* config = root.mutable_cmvr_es(); + config->set_logger_config_file("logger.pb.txt"); + config->set_device_manager_config_file( + "device_manager.pb.txt"); + config->set_task_manager_config_file( + "task_manager.pb.txt"); + return write("cmvr_es.pb.txt", root); + } + +private: + std::filesystem::path root_; +}; + +class OccupiedTcpPort { +public: + OccupiedTcpPort() + { + fd_ = ::socket(AF_INET, SOCK_STREAM | SOCK_CLOEXEC, 0); + if (fd_ < 0) { + return; + } + + sockaddr_in address{}; + address.sin_family = AF_INET; + address.sin_addr.s_addr = htonl(INADDR_LOOPBACK); + address.sin_port = 0; + if (::bind(fd_, reinterpret_cast(&address), + sizeof(address)) != 0 || + ::listen(fd_, 1) != 0) { + ::close(fd_); + fd_ = -1; + return; + } + + socklen_t length = sizeof(address); + if (::getsockname(fd_, + reinterpret_cast(&address), + &length) != 0) { + ::close(fd_); + fd_ = -1; + return; + } + port_ = ntohs(address.sin_port); + } + + ~OccupiedTcpPort() + { + if (fd_ >= 0) { + ::close(fd_); + } + } + + bool valid() const { return fd_ >= 0 && port_ != 0; } + std::string port() const { return std::to_string(port_); } + +private: + int fd_{-1}; + unsigned short port_{0}; +}; + +TEST(RuntimeLifecycleTest, EnabledDeviceInitFailureFailsRuntimeInit) +{ + TempConfigTree tree; + ASSERT_TRUE(tree.valid()); + ASSERT_TRUE(tree.writeLogger()); + ASSERT_TRUE(tree.writeRoot()); + + cmvr::config::DeviceManagerRootConfig devices; + auto* entry = + devices.mutable_device_manager()->add_devices(); + entry->set_id("unsupported"); + entry->set_type( + cmvr::config::DeviceConfigEntry::DEVICE_TYPE_UNKNOWN); + entry->set_enable(true); + ASSERT_TRUE(tree.write("device_manager.pb.txt", devices)); + + cmvr::config::TaskManagerRootConfig tasks; + ASSERT_TRUE(tree.write("task_manager.pb.txt", tasks)); + + cmvr::Runtime runtime; + EXPECT_FALSE(runtime.init(tree.path("cmvr_es.pb.txt"))); + EXPECT_FALSE(runtime.initialized()); + EXPECT_FALSE(runtime.tasksStarted()); +} + +TEST(RuntimeLifecycleTest, EnabledTaskInitFailureFailsRuntimeInit) +{ + TempConfigTree tree; + ASSERT_TRUE(tree.valid()); + ASSERT_TRUE(tree.writeLogger()); + ASSERT_TRUE(tree.writeRoot()); + + cmvr::config::DeviceManagerRootConfig devices; + ASSERT_TRUE(tree.write("device_manager.pb.txt", devices)); + + cmvr::config::TaskManagerRootConfig tasks; + auto* entry = tasks.mutable_task_manager()->add_tasks(); + entry->set_id("unsupported"); + entry->set_type( + cmvr::config::TaskConfigEntry::TASK_TYPE_UNKNOWN); + entry->set_enable(true); + entry->set_run_mode( + cmvr::config::TaskConfigEntry:: + TASK_RUN_MODE_BLOCKING_SERVICE); + ASSERT_TRUE(tree.write("task_manager.pb.txt", tasks)); + + cmvr::Runtime runtime; + EXPECT_FALSE(runtime.init(tree.path("cmvr_es.pb.txt"))); + EXPECT_FALSE(runtime.initialized()); + EXPECT_FALSE(runtime.tasksStarted()); +} + +TEST(RuntimeLifecycleTest, + TaskConfigLoadFailureRollsBackDeviceManagerSingleton) +{ + TempConfigTree first_tree; + ASSERT_TRUE(first_tree.valid()); + ASSERT_TRUE(first_tree.writeLogger()); + ASSERT_TRUE(first_tree.writeRoot()); + + cmvr::config::DeviceManagerRootConfig first_devices; + first_devices.mutable_device_manager()->set_name("first"); + ASSERT_TRUE(first_tree.write( + "device_manager.pb.txt", first_devices)); + // Deliberately do not create task_manager.pb.txt. + + cmvr::Runtime runtime; + ASSERT_FALSE( + runtime.init(first_tree.path("cmvr_es.pb.txt"))); + ASSERT_FALSE(runtime.initialized()); + + TempConfigTree second_tree; + ASSERT_TRUE(second_tree.valid()); + ASSERT_TRUE(second_tree.writeLogger()); + ASSERT_TRUE(second_tree.writeRoot()); + + cmvr::config::DeviceManagerRootConfig second_devices; + second_devices.mutable_device_manager()->set_name("second"); + ASSERT_TRUE(second_tree.write( + "device_manager.pb.txt", second_devices)); + cmvr::config::TaskManagerRootConfig second_tasks; + ASSERT_TRUE(second_tree.write( + "task_manager.pb.txt", second_tasks)); + + ASSERT_TRUE( + runtime.init(second_tree.path("cmvr_es.pb.txt"))); + EXPECT_EQ(runtime.deviceManager().name(), "second"); +} + +TEST(RuntimeLifecycleTest, GrpcBindFailureDoesNotMarkTasksStarted) +{ + OccupiedTcpPort occupied_port; + ASSERT_TRUE(occupied_port.valid()); + + TempConfigTree tree; + ASSERT_TRUE(tree.valid()); + ASSERT_TRUE(tree.writeLogger()); + ASSERT_TRUE(tree.writeRoot()); + + cmvr::config::DeviceManagerRootConfig devices; + ASSERT_TRUE(tree.write("device_manager.pb.txt", devices)); + + cmvr::config::GRPCServerRootConfig grpc; + auto* grpc_config = grpc.mutable_grpc_server(); + grpc_config->set_id("grpc_server"); + grpc_config->set_host("127.0.0.1"); + grpc_config->set_port(occupied_port.port()); + ASSERT_TRUE(tree.write("grpc.pb.txt", grpc)); + + cmvr::config::TaskManagerRootConfig tasks; + auto* entry = tasks.mutable_task_manager()->add_tasks(); + entry->set_id("grpc_server"); + entry->set_type( + cmvr::config::TaskConfigEntry::TASK_TYPE_GRPC_SERVER); + entry->set_enable(true); + entry->set_run_mode( + cmvr::config::TaskConfigEntry:: + TASK_RUN_MODE_BLOCKING_SERVICE); + entry->set_config_file("grpc.pb.txt"); + ASSERT_TRUE(tree.write("task_manager.pb.txt", tasks)); + + cmvr::Runtime runtime; + ASSERT_TRUE(runtime.init(tree.path("cmvr_es.pb.txt"))); + EXPECT_FALSE(runtime.startTasks()); + EXPECT_FALSE(runtime.tasksStarted()); + EXPECT_FALSE(runtime.taskManager().running()); +} + +} // namespace diff --git a/cmvr-es/service/CMakeLists.txt b/cmvr-es/service/CMakeLists.txt index b46b443b..6da68349 100644 --- a/cmvr-es/service/CMakeLists.txt +++ b/cmvr-es/service/CMakeLists.txt @@ -7,6 +7,8 @@ add_library(service grpc/src/grpc_head_service.cpp grpc/src/grpc_dexhand_service.cpp grpc/src/grpc_arm_service.cpp + grpc/src/grpc_arm_teleop_service.cpp + grpc/src/grpc_robot_arm_teleop_backend.cpp grpc/src/grpc_motor_service.cpp grpc/src/grpc_agv_service.cpp grpc/src/grpc_hlc_service.cpp @@ -18,6 +20,7 @@ target_include_directories(service PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) target_link_libraries(service PRIVATE cmvr_es::proto osqp + cmvr_es::control_authority cmvr_es::device_manager cmvr_es::task_manager cmvr_es::algorithms::controller @@ -44,6 +47,61 @@ if(BUILD_TESTING) ) set_tests_properties(grpc_camera_stream_policy_test PROPERTIES TIMEOUT 10) + add_executable(grpc_arm_teleop_service_test + grpc/tests/grpc_arm_teleop_service_test.cpp + ) + target_include_directories(grpc_arm_teleop_service_test + PRIVATE + ${CMAKE_SOURCE_DIR}/cmvr-es + ) + target_link_libraries(grpc_arm_teleop_service_test + PRIVATE + service + cmvr_es::proto + gtest + gtest_main + pthread + ) + add_test( + NAME grpc_arm_teleop_service_test + COMMAND grpc_arm_teleop_service_test + ) + set(_grpc_arm_teleop_test_environment + "LD_LIBRARY_PATH=${CMVR_TEST_EXTERNAL_LIBRARY_PATH}" + ) + if(CMVR_TEST_SYSTEM_LIBSTDCXX) + list(APPEND _grpc_arm_teleop_test_environment + "LD_PRELOAD=${CMVR_TEST_SYSTEM_LIBSTDCXX}") + endif() + set_tests_properties(grpc_arm_teleop_service_test PROPERTIES + TIMEOUT 20 + ENVIRONMENT "${_grpc_arm_teleop_test_environment}" + ) + + add_executable(grpc_robot_arm_teleop_backend_test + grpc/tests/grpc_robot_arm_teleop_backend_test.cpp + ) + target_include_directories(grpc_robot_arm_teleop_backend_test + PRIVATE + ${CMAKE_SOURCE_DIR}/cmvr-es + ) + target_link_libraries(grpc_robot_arm_teleop_backend_test + PRIVATE + service + cmvr_es::proto + gtest + gtest_main + pthread + ) + add_test( + NAME grpc_robot_arm_teleop_backend_test + COMMAND grpc_robot_arm_teleop_backend_test + ) + set_tests_properties(grpc_robot_arm_teleop_backend_test PROPERTIES + TIMEOUT 10 + ENVIRONMENT "${_grpc_arm_teleop_test_environment}" + ) + add_executable(grpc_motor_service_test grpc/tests/grpc_motor_service_test.cpp ) diff --git a/cmvr-es/service/arm_teleop_client/CMakeLists.txt b/cmvr-es/service/arm_teleop_client/CMakeLists.txt new file mode 100644 index 00000000..8dd1b377 --- /dev/null +++ b/cmvr-es/service/arm_teleop_client/CMakeLists.txt @@ -0,0 +1,38 @@ +find_package(Threads REQUIRED) + +add_library(arm_teleop_client STATIC + src/grpc_arm_teleop_client.cpp +) +target_compile_features(arm_teleop_client PUBLIC cxx_std_17) +target_include_directories(arm_teleop_client PUBLIC ${PROJECT_SOURCE_DIR}/cmvr-es) +target_link_libraries(arm_teleop_client + PUBLIC + cmvr_es::proto + PRIVATE + Threads::Threads +) + +add_library(cmvr_es::arm_teleop_client ALIAS arm_teleop_client) +install(TARGETS arm_teleop_client ARCHIVE DESTINATION lib) + +if(BUILD_TESTING) + add_executable(grpc_arm_teleop_client_test + tests/grpc_arm_teleop_client_test.cpp + ) + target_compile_features(grpc_arm_teleop_client_test PRIVATE cxx_std_17) + target_link_libraries(grpc_arm_teleop_client_test + PRIVATE + cmvr_es::arm_teleop_client + Threads::Threads + ) + add_test(NAME grpc_arm_teleop_client_test COMMAND grpc_arm_teleop_client_test) + set(_arm_teleop_client_test_environment + "LD_LIBRARY_PATH=${CMVR_TEST_EXTERNAL_LIBRARY_PATH}") + if(CMVR_TEST_SYSTEM_LIBSTDCXX) + list(APPEND _arm_teleop_client_test_environment + "LD_PRELOAD=${CMVR_TEST_SYSTEM_LIBSTDCXX}") + endif() + set_tests_properties(grpc_arm_teleop_client_test PROPERTIES + TIMEOUT 10 + ENVIRONMENT "${_arm_teleop_client_test_environment}") +endif() diff --git a/cmvr-es/service/arm_teleop_client/include/grpc_arm_teleop_client.h b/cmvr-es/service/arm_teleop_client/include/grpc_arm_teleop_client.h new file mode 100644 index 00000000..731a718d --- /dev/null +++ b/cmvr-es/service/arm_teleop_client/include/grpc_arm_teleop_client.h @@ -0,0 +1,80 @@ +#ifndef CMVR_ES_GRPC_ARM_TELEOP_CLIENT_H +#define CMVR_ES_GRPC_ARM_TELEOP_CLIENT_H + +#include +#include +#include +#include + +#include +#include +#include +#include + +#include "cmvr/api/arm_teleop_v1.grpc.pb.h" + +namespace cmvr::teleop { + +// One synchronous gRPC stream/session. Connection retry and worker ownership +// belong to UmeTeleopTask; robot algorithms and kinematics belong to UME. +class GrpcArmTeleopClient final { +public: + using Api = api::armteleop::v1::ArmTeleopService; + using ClientFrame = api::armteleop::v1::ClientFrame; + using ServerFrame = api::armteleop::v1::ServerFrame; + using OpenSession = api::armteleop::v1::OpenSession; + using JointSetpoint = api::armteleop::v1::JointSetpoint; + using ClientHeartbeat = api::armteleop::v1::ClientHeartbeat; + using StopSession = api::armteleop::v1::StopSession; + using FrameCallback = std::function; + using CancelPredicate = std::function; + + explicit GrpcArmTeleopClient( + std::shared_ptr channel); + ~GrpcArmTeleopClient(); + + GrpcArmTeleopClient(const GrpcArmTeleopClient&) = delete; + GrpcArmTeleopClient& operator=(const GrpcArmTeleopClient&) = delete; + + // Blocks until the peer closes the stream or tryCancel() is called. The + // OpenSession frame is always the first client frame. + grpc::Status runSession(const OpenSession& open_session, + FrameCallback callback = {}, + CancelPredicate cancel_requested = {}); + + // These methods only transport already-computed protocol values. + bool sendSetpoint(const JointSetpoint& setpoint, + std::uint64_t expected_session_generation = 0); + bool sendHeartbeat(const ClientHeartbeat& heartbeat, + std::uint64_t expected_session_generation = 0); + bool sendStop(const StopSession& stop, + std::uint64_t expected_session_generation = 0); + + bool isSessionActive() const; + std::uint64_t activeSessionGeneration() const; + + // Thread-safe and intentionally named after the gRPC primitive used. It + // interrupts a blocked Read/Write/Finish so the owning Task can join. + void tryCancel(); + +private: + using Stream = grpc::ClientReaderWriterInterface; + + bool writeFrame(const ClientFrame& frame, + std::uint64_t expected_session_generation); + void clearSession(const std::shared_ptr& context, + const std::shared_ptr& stream); + + std::unique_ptr stub_; + + mutable std::mutex lifecycle_mutex_; + std::mutex write_mutex_; + std::shared_ptr active_context_; + std::shared_ptr active_stream_; + std::uint64_t next_session_generation_{0}; + std::uint64_t active_session_generation_{0}; +}; + +} // namespace cmvr::teleop + +#endif // CMVR_ES_GRPC_ARM_TELEOP_CLIENT_H diff --git a/cmvr-es/service/arm_teleop_client/src/grpc_arm_teleop_client.cpp b/cmvr-es/service/arm_teleop_client/src/grpc_arm_teleop_client.cpp new file mode 100644 index 00000000..0c686937 --- /dev/null +++ b/cmvr-es/service/arm_teleop_client/src/grpc_arm_teleop_client.cpp @@ -0,0 +1,223 @@ +#include "service/arm_teleop_client/include/grpc_arm_teleop_client.h" + +#include +#include +#include + +namespace cmvr::teleop { +namespace { + +grpc::Status clientStatus(const grpc::StatusCode code, const char* detail) +{ + return grpc::Status(code, detail); +} + +} // namespace + +GrpcArmTeleopClient::GrpcArmTeleopClient( + std::shared_ptr channel) +{ + if (channel) { + stub_ = Api::NewStub(channel); + } +} + +GrpcArmTeleopClient::~GrpcArmTeleopClient() +{ + tryCancel(); +} + +grpc::Status GrpcArmTeleopClient::runSession( + const OpenSession& open_session, + FrameCallback callback, + CancelPredicate cancel_requested) +{ + if (!stub_) { + return clientStatus( + grpc::StatusCode::FAILED_PRECONDITION, + "arm teleop client has no channel"); + } + if (cancel_requested && cancel_requested()) { + return clientStatus( + grpc::StatusCode::CANCELLED, + "arm teleop session cancelled before start"); + } + + auto context = std::make_shared(); + { + std::lock_guard lock(lifecycle_mutex_); + if (active_context_) { + return clientStatus( + grpc::StatusCode::ALREADY_EXISTS, + "arm teleop session is already active"); + } + // Publish the context before opening/writing the stream so tryCancel() + // can interrupt every blocking phase of the synchronous RPC. + active_context_ = context; + } + // Closes the small race where the owning Task requests stop immediately + // before active_context_ becomes visible to tryCancel(). + if (cancel_requested && cancel_requested()) { + context->TryCancel(); + } + + auto unique_stream = stub_->Teleoperate(context.get()); + if (!unique_stream) { + clearSession(context, {}); + return clientStatus( + grpc::StatusCode::UNAVAILABLE, + "failed to create arm teleop stream"); + } + auto stream = std::shared_ptr(std::move(unique_stream)); + + ClientFrame first_frame; + *first_frame.mutable_open() = open_session; + { + std::lock_guard write_lock(write_mutex_); + if (!stream->Write(first_frame)) { + const grpc::Status status = stream->Finish(); + clearSession(context, stream); + return status.ok() + ? clientStatus( + grpc::StatusCode::UNAVAILABLE, + "peer closed before OpenSession was written") + : status; + } + } + + { + std::lock_guard lock(lifecycle_mutex_); + // Cancellation can race the initial Write. Keeping the stream visible + // is safe; subsequent writes will fail and runSession will clean it. + if (active_context_ == context) { + active_stream_ = stream; + active_session_generation_ = ++next_session_generation_; + } + } + + bool callback_failed = false; + std::string callback_error; + ServerFrame frame; + while (stream->Read(&frame)) { + if (!callback) { + continue; + } + try { + callback(frame); + } catch (const std::exception& error) { + callback_failed = true; + callback_error = error.what(); + context->TryCancel(); + break; + } catch (...) { + callback_failed = true; + callback_error = "server-frame callback raised an unknown exception"; + context->TryCancel(); + break; + } + } + + { + std::lock_guard write_lock(write_mutex_); + stream->WritesDone(); + } + const grpc::Status status = stream->Finish(); + clearSession(context, stream); + + if (callback_failed) { + return grpc::Status( + grpc::StatusCode::INTERNAL, + "arm teleop callback failed: " + callback_error); + } + return status; +} + +bool GrpcArmTeleopClient::sendSetpoint( + const JointSetpoint& setpoint, + const std::uint64_t expected_session_generation) +{ + ClientFrame frame; + *frame.mutable_setpoint() = setpoint; + return writeFrame(frame, expected_session_generation); +} + +bool GrpcArmTeleopClient::sendHeartbeat( + const ClientHeartbeat& heartbeat, + const std::uint64_t expected_session_generation) +{ + ClientFrame frame; + *frame.mutable_heartbeat() = heartbeat; + return writeFrame(frame, expected_session_generation); +} + +bool GrpcArmTeleopClient::sendStop( + const StopSession& stop, + const std::uint64_t expected_session_generation) +{ + ClientFrame frame; + *frame.mutable_stop() = stop; + return writeFrame(frame, expected_session_generation); +} + +bool GrpcArmTeleopClient::isSessionActive() const +{ + std::lock_guard lock(lifecycle_mutex_); + return active_stream_ != nullptr; +} + +std::uint64_t GrpcArmTeleopClient::activeSessionGeneration() const +{ + std::lock_guard lock(lifecycle_mutex_); + return active_session_generation_; +} + +void GrpcArmTeleopClient::tryCancel() +{ + std::shared_ptr context; + { + std::lock_guard lock(lifecycle_mutex_); + context = active_context_; + } + if (context) { + context->TryCancel(); + } +} + +bool GrpcArmTeleopClient::writeFrame( + const ClientFrame& frame, + const std::uint64_t expected_session_generation) +{ + std::shared_ptr stream; + { + std::lock_guard lock(lifecycle_mutex_); + if (expected_session_generation != 0 && + expected_session_generation != active_session_generation_) { + return false; + } + stream = active_stream_; + } + if (!stream) { + return false; + } + + // gRPC permits one read and one write concurrently, but concurrent writes + // must be serialized by the application. + std::lock_guard write_lock(write_mutex_); + return stream->Write(frame); +} + +void GrpcArmTeleopClient::clearSession( + const std::shared_ptr& context, + const std::shared_ptr& stream) +{ + std::lock_guard lock(lifecycle_mutex_); + if (active_context_ == context) { + active_context_.reset(); + } + if (!stream || active_stream_ == stream) { + active_stream_.reset(); + active_session_generation_ = 0; + } +} + +} // namespace cmvr::teleop diff --git a/cmvr-es/service/arm_teleop_client/tests/grpc_arm_teleop_client_test.cpp b/cmvr-es/service/arm_teleop_client/tests/grpc_arm_teleop_client_test.cpp new file mode 100644 index 00000000..8be21e70 --- /dev/null +++ b/cmvr-es/service/arm_teleop_client/tests/grpc_arm_teleop_client_test.cpp @@ -0,0 +1,220 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include "cmvr/api/arm_teleop_v1.grpc.pb.h" +#include "service/arm_teleop_client/include/grpc_arm_teleop_client.h" + +namespace { + +using namespace std::chrono_literals; +namespace api = cmvr::api::armteleop::v1; + +class TestArmTeleopService final : public api::ArmTeleopService::Service { +public: + grpc::Status Teleoperate( + grpc::ServerContext*, + grpc::ServerReaderWriter* stream) override + { + api::ClientFrame frame; + if (!stream->Read(&frame) || !frame.has_open()) { + return grpc::Status( + grpc::StatusCode::INVALID_ARGUMENT, + "OpenSession must be first"); + } + + { + std::lock_guard lock(mutex_); + open_received_ = true; + } + condition_.notify_all(); + + api::ServerFrame opened; + opened.mutable_status()->set_session_id("client-test-session"); + opened.mutable_status()->set_phase(api::SESSION_PHASE_OPENED); + if (!stream->Write(opened)) { + return grpc::Status::OK; + } + + while (stream->Read(&frame)) { + if (frame.has_heartbeat()) { + heartbeat_received_.store(true); + condition_.notify_all(); + } + } + handler_finished_.store(true); + condition_.notify_all(); + return grpc::Status::OK; + } + + bool waitForOpen(const std::chrono::milliseconds timeout) + { + std::unique_lock lock(mutex_); + return condition_.wait_for(lock, timeout, [this] { return open_received_; }); + } + + bool waitForHeartbeat(const std::chrono::milliseconds timeout) + { + std::unique_lock lock(mutex_); + return condition_.wait_for( + lock, timeout, [this] { return heartbeat_received_.load(); }); + } + + bool waitForHandlerFinish(const std::chrono::milliseconds timeout) + { + std::unique_lock lock(mutex_); + return condition_.wait_for( + lock, timeout, [this] { return handler_finished_.load(); }); + } + +private: + std::mutex mutex_; + std::condition_variable condition_; + bool open_received_{false}; + std::atomic heartbeat_received_{false}; + std::atomic handler_finished_{false}; +}; + +int fail(const std::string& detail) +{ + std::cerr << "grpc_arm_teleop_client_test: " << detail << '\n'; + return 1; +} + +} // namespace + +int main() +{ + TestArmTeleopService service; + const std::string socket_path = + "/tmp/cmvr_arm_teleop_client_test_" + + std::to_string(static_cast(::getpid())) + ".sock"; + std::remove(socket_path.c_str()); + const std::string endpoint = "unix:" + socket_path; + + grpc::ServerBuilder builder; + builder.AddListeningPort( + endpoint, + grpc::InsecureServerCredentials()); + builder.RegisterService(&service); + std::unique_ptr server = builder.BuildAndStart(); + if (!server) { + return fail("failed to start in-process gRPC server"); + } + + auto channel = grpc::CreateChannel( + endpoint, + grpc::InsecureChannelCredentials()); + cmvr::teleop::GrpcArmTeleopClient client(channel); + + api::OpenSession open; + open.set_protocol_major(1); + open.set_protocol_minor(0); + open.set_client_instance_id("grpc-client-test"); + open.mutable_expected_robot()->set_robot_id("test-arm"); + open.set_watchdog_timeout_ms(100); + open.set_requested_lease_ms(500); + + std::mutex frame_mutex; + std::condition_variable frame_condition; + bool opened_received = false; + grpc::Status session_status; + std::thread session_thread([&] { + session_status = client.runSession( + open, + [&](const api::ServerFrame& frame) { + if (frame.has_status() && + frame.status().phase() == api::SESSION_PHASE_OPENED) { + { + std::lock_guard lock(frame_mutex); + opened_received = true; + } + frame_condition.notify_all(); + } + }); + }); + + if (!service.waitForOpen(2s)) { + client.tryCancel(); + session_thread.join(); + server->Shutdown(); + return fail("server did not receive OpenSession"); + } + { + std::unique_lock lock(frame_mutex); + if (!frame_condition.wait_for(lock, 2s, [&] { return opened_received; })) { + client.tryCancel(); + session_thread.join(); + server->Shutdown(); + return fail("client did not receive OPENED status"); + } + } + if (!client.isSessionActive()) { + client.tryCancel(); + session_thread.join(); + server->Shutdown(); + return fail("client did not expose an active session"); + } + const std::uint64_t generation = + client.activeSessionGeneration(); + if (generation == 0) { + client.tryCancel(); + session_thread.join(); + server->Shutdown(); + return fail("active stream did not expose a session generation"); + } + + api::ClientHeartbeat heartbeat; + heartbeat.set_sequence(1); + if (client.sendHeartbeat(heartbeat, generation + 1)) { + client.tryCancel(); + session_thread.join(); + server->Shutdown(); + return fail("stale session generation was allowed to write"); + } + if (!client.sendHeartbeat(heartbeat, generation) || + !service.waitForHeartbeat(2s)) { + client.tryCancel(); + session_thread.join(); + server->Shutdown(); + return fail("heartbeat did not traverse the active stream"); + } + + const auto cancel_begin = std::chrono::steady_clock::now(); + client.tryCancel(); + session_thread.join(); + const auto cancel_elapsed = std::chrono::steady_clock::now() - cancel_begin; + + if (cancel_elapsed > 2s) { + server->Shutdown(); + return fail("TryCancel did not unblock and join the session promptly"); + } + if (session_status.error_code() != grpc::StatusCode::CANCELLED) { + server->Shutdown(); + return fail( + "cancelled session returned unexpected status: " + + std::to_string(session_status.error_code())); + } + if (client.isSessionActive()) { + server->Shutdown(); + return fail("client retained an active stream after cancellation"); + } + if (!service.waitForHandlerFinish(2s)) { + server->Shutdown(); + return fail("server handler did not observe client cancellation"); + } + + server->Shutdown(); + std::remove(socket_path.c_str()); + std::cout << "grpc_arm_teleop_client_test: PASS\n"; + return 0; +} diff --git a/cmvr-es/service/grpc/include/grpc_arm_teleop_service.h b/cmvr-es/service/grpc/include/grpc_arm_teleop_service.h new file mode 100644 index 00000000..8684f91d --- /dev/null +++ b/cmvr-es/service/grpc/include/grpc_arm_teleop_service.h @@ -0,0 +1,91 @@ +#pragma once + +#include +#include +#include +#include +#include + +#include + +#include "cmvr/api/arm_teleop_v1.grpc.pb.h" +#include "manager/control_authority/include/control_authority_manager.h" + +namespace cmvr::service { + +namespace arm_teleop = cmvr::api::armteleop::v1; + +struct ArmTeleopBackendResult { + bool success{false}; + grpc::StatusCode status_code{grpc::StatusCode::INTERNAL}; + std::string detail; + + static ArmTeleopBackendResult ok() + { + return {true, grpc::StatusCode::OK, {}}; + } + + static ArmTeleopBackendResult failure( + const grpc::StatusCode code, + std::string message) + { + return {false, code, std::move(message)}; + } +}; + +struct ArmTeleopBackendSnapshot { + arm_teleop::JointState joint_state; + arm_teleop::RobotSafetyState safety; +}; + +// Execution boundary for ArmTeleopService. The first implementation registers a +// disabled backend in production and injects a fake backend in tests. A future +// RobotArm adapter must live behind this interface so the gRPC reader thread can +// remain a bounded mailbox producer and never touch hardware. Implementations +// must keep every call bounded and non-blocking with respect to hardware I/O; +// snapshot() must return cached state rather than synchronously polling a bus. +class ArmTeleopBackend { +public: + virtual ~ArmTeleopBackend() = default; + + virtual bool available() const noexcept = 0; + virtual std::string unavailableReason() const { return {}; } + virtual arm_teleop::RobotManifest manifest() const = 0; + virtual bool supportsForceFeedback() const noexcept = 0; + virtual ArmTeleopBackendResult open( + const arm_teleop::OpenSession& request) = 0; + // The deadline is computed from the receiver's local monotonic clock. + // Implementations must re-check it immediately before committing a + // hardware command; the protobuf valid_for duration is never interpreted + // as a cross-machine absolute timestamp. + virtual ArmTeleopBackendResult applySetpoint( + const arm_teleop::JointSetpoint& setpoint, + std::chrono::steady_clock::time_point deadline) = 0; + virtual ArmTeleopBackendResult stop( + arm_teleop::StopReason reason, + const std::string& detail) = 0; + virtual ArmTeleopBackendSnapshot snapshot() const = 0; +}; + +std::shared_ptr makeDisabledArmTeleopBackend(); + +class ArmTeleopServiceImpl final + : public arm_teleop::ArmTeleopService::Service { +public: + explicit ArmTeleopServiceImpl( + std::shared_ptr backend = + makeDisabledArmTeleopBackend(), + control::ControlAuthorityManager* authority = nullptr); + ~ArmTeleopServiceImpl() override = default; + + grpc::Status Teleoperate( + grpc::ServerContext* context, + grpc::ServerReaderWriter* stream) override; + +private: + std::shared_ptr backend_; + control::ControlAuthorityManager* authority_{nullptr}; +}; + +} // namespace cmvr::service diff --git a/cmvr-es/service/grpc/include/grpc_robot_arm_teleop_backend.h b/cmvr-es/service/grpc/include/grpc_robot_arm_teleop_backend.h new file mode 100644 index 00000000..c15c3562 --- /dev/null +++ b/cmvr-es/service/grpc/include/grpc_robot_arm_teleop_backend.h @@ -0,0 +1,18 @@ +#pragma once + +#include + +#include "cmvr/config/grpc_server_config/grpc_server_config.pb.h" +#include "devices/arm/robot_arm.h" +#include "service/grpc/include/grpc_arm_teleop_service.h" + +namespace cmvr::service { + +// Creates a fail-closed adapter from the process RobotArm abstraction to the +// session-based ArmTeleop backend. available() remains false unless the config, +// RobotModel and RobotArm capability all pass static validation. +std::shared_ptr makeRobotArmTeleopBackend( + std::shared_ptr arm, + const config::ArmTeleopBackendConfig& config); + +} // namespace cmvr::service diff --git a/cmvr-es/service/grpc/src/grpc_arm_service.cpp b/cmvr-es/service/grpc/src/grpc_arm_service.cpp index 1ae88019..6c3ca566 100644 --- a/cmvr-es/service/grpc/src/grpc_arm_service.cpp +++ b/cmvr-es/service/grpc/src/grpc_arm_service.cpp @@ -1,8 +1,13 @@ #include "service/grpc/include/grpc_arm_service.h" +#include +#include +#include + #include #include "common/base/logging/logger.h" +#include "manager/control_authority/include/control_authority_manager.h" using google::protobuf::util::TimeUtil; @@ -119,6 +124,64 @@ grpc::Status setDeviceNotFound(Response* response, const std::string& device_id) return grpc::Status(grpc::StatusCode::NOT_FOUND, message); } +grpc::Status setControlLeaseConflict( + api::CommandHeader_Feedback* response, + const std::string& device_id) +{ + const std::string message = + "RobotArm control is leased by another active control operation: " + + device_id; + fillFeedback(response, false, message); + return grpc::Status( + grpc::StatusCode::FAILED_PRECONDITION, message); +} + +template +grpc::Status setControlLeaseConflict( + Response* response, + const std::string& device_id) +{ + return setControlLeaseConflict( + response->mutable_header(), device_id); +} + +class ScopedUnaryControlLease final { +public: + ScopedUnaryControlLease( + const std::string& device_id, + const char* operation) + : manager_(control::ControlAuthorityManager::instance()) + { + static std::atomic sequence{0}; + const std::string owner = + std::string("grpc-arm-unary:") + operation + ":" + + std::to_string( + sequence.fetch_add( + 1U, std::memory_order_relaxed) + + 1U); + auto acquired = manager_.tryAcquire( + device_id, + owner, + std::chrono::duration_cast< + control::ControlAuthorityManager::Duration>( + std::chrono::hours(24))); + acquired_ = acquired.acquired; + token_ = std::move(acquired.token); + } + + ~ScopedUnaryControlLease() + { + manager_.release(token_); + } + + bool acquired() const noexcept { return acquired_; } + +private: + control::ControlAuthorityManager& manager_; + control::ControlLeaseToken token_; + bool acquired_{false}; +}; + } // namespace gRPCArmServiceImpl::gRPCArmServiceImpl() @@ -136,6 +199,10 @@ grpc::Status gRPCArmServiceImpl::torqueOff(grpc::ServerContext*, if (!arm) { return setDeviceNotFound(response, device_id); } + // A safety-disable command preempts any network teleoperation lease. + // The teleoperation executor must fail its next renew before it can + // dispatch another setpoint. + control::ControlAuthorityManager::instance().revoke(device_id); const auto result = arm->torqueOff(); fillFeedback(response, result.ok(), result.ok() ? "" : result.message); if (result.ok()) { @@ -158,6 +225,11 @@ grpc::Status gRPCArmServiceImpl::torqueOn(grpc::ServerContext*, if (!arm) { return setDeviceNotFound(response, device_id); } + ScopedUnaryControlLease control_lease( + device_id, "torqueOn"); + if (!control_lease.acquired()) { + return setControlLeaseConflict(response, device_id); + } const auto result = arm->torqueOn(); fillFeedback(response, result.ok(), result.ok() ? "" : result.message); if (result.ok()) { @@ -180,6 +252,11 @@ grpc::Status gRPCArmServiceImpl::moveJ(grpc::ServerContext*, if (!arm) { return setDeviceNotFound(response, device_id); } + ScopedUnaryControlLease control_lease( + device_id, "moveJ"); + if (!control_lease.acquired()) { + return setControlLeaseConflict(response, device_id); + } const auto result = arm->moveJ(toJointPositionCommand(request->target()), toMotionOptions(request->options())); if (result.ok()) { @@ -203,6 +280,11 @@ grpc::Status gRPCArmServiceImpl::moveL(grpc::ServerContext*, if (!arm) { return setDeviceNotFound(response, device_id); } + ScopedUnaryControlLease control_lease( + device_id, "moveL"); + if (!control_lease.acquired()) { + return setControlLeaseConflict(response, device_id); + } const auto result = arm->moveL(toCartesianPose(request->target()), toMotionOptions(request->options()), toFrameType(request->frame())); @@ -227,6 +309,11 @@ grpc::Status gRPCArmServiceImpl::speedJ(grpc::ServerContext*, if (!arm) { return setDeviceNotFound(response, device_id); } + ScopedUnaryControlLease control_lease( + device_id, "speedJ"); + if (!control_lease.acquired()) { + return setControlLeaseConflict(response, device_id); + } const auto result = arm->speedJ(toJointVelocityCommand(request->velocity()), request->acceleration(), request->duration()); @@ -253,6 +340,11 @@ grpc::Status gRPCArmServiceImpl::speedL(grpc::ServerContext*, if (!arm) { return setDeviceNotFound(response, device_id); } + ScopedUnaryControlLease control_lease( + device_id, "speedL"); + if (!control_lease.acquired()) { + return setControlLeaseConflict(response, device_id); + } const auto result = arm->speedL(toCartesianVelocity(request->velocity()), request->acceleration(), request->duration(), @@ -280,6 +372,11 @@ grpc::Status gRPCArmServiceImpl::servoJ(grpc::ServerContext*, if (!arm) { return setDeviceNotFound(response, device_id); } + ScopedUnaryControlLease control_lease( + device_id, "servoJ"); + if (!control_lease.acquired()) { + return setControlLeaseConflict(response, device_id); + } const auto result = arm->servoJ(toJointPositionCommand(request->target())); if (result.ok()) { CMVR_LOG(DEBUG) << "[gRPCArmServiceImpl] (servoJ): success, id=" << device_id @@ -302,6 +399,7 @@ grpc::Status gRPCArmServiceImpl::stopMotion(grpc::ServerContext*, if (!arm) { return setDeviceNotFound(response, device_id); } + control::ControlAuthorityManager::instance().revoke(device_id); const auto result = arm->stopMotion(); fillFeedback(response, result.ok(), result.ok() ? "" : result.message); if (result.ok()) { @@ -379,6 +477,11 @@ grpc::Status gRPCArmServiceImpl::calibrateZeroQ(grpc::ServerContext*, if (!arm) { return setDeviceNotFound(response, device_id); } + ScopedUnaryControlLease control_lease( + device_id, "calibrateZeroQ"); + if (!control_lease.acquired()) { + return setControlLeaseConflict(response, device_id); + } const auto result = arm->calibrateZeroQ(request->joint_name()); if (result.ok()) { CMVR_LOG(DEBUG) << "[gRPCArmServiceImpl] (calibrateZeroQ): success, id=" << device_id @@ -417,6 +520,11 @@ grpc::Status gRPCArmServiceImpl::clearFault(grpc::ServerContext *context, if (!arm) { return setDeviceNotFound(response, device_id); } + ScopedUnaryControlLease control_lease( + device_id, "clearFault"); + if (!control_lease.acquired()) { + return setControlLeaseConflict(response, device_id); + } const auto result = arm->clearFault(); fillFeedback(response, result.ok(), result.ok() ? "" : result.message); return resultToStatus(result); @@ -426,4 +534,3 @@ grpc::Status gRPCArmServiceImpl::clearFault(grpc::ServerContext *context, } } } // namespace cmvr::service - diff --git a/cmvr-es/service/grpc/src/grpc_arm_teleop_service.cpp b/cmvr-es/service/grpc/src/grpc_arm_teleop_service.cpp new file mode 100644 index 00000000..21e301d1 --- /dev/null +++ b/cmvr-es/service/grpc/src/grpc_arm_teleop_service.cpp @@ -0,0 +1,1111 @@ +#include "service/grpc/include/grpc_arm_teleop_service.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace cmvr::service { + +namespace { + +using Clock = std::chrono::steady_clock; + +constexpr std::uint32_t kProtocolMajor = 1; +constexpr std::uint32_t kProtocolMinor = 0; +constexpr std::uint32_t kDefaultWatchdogMs = 250; +constexpr std::uint32_t kMinimumWatchdogMs = 20; +constexpr std::uint32_t kMaximumWatchdogMs = 60000; +constexpr std::uint32_t kDefaultLeaseMs = 10000; +constexpr std::uint32_t kMaximumLeaseMs = 600000; +constexpr std::uint32_t kMaximumRateHz = 1000; +constexpr std::size_t kMaximumJoints = 64; +constexpr std::size_t kMaximumIdentifierLength = 128; +constexpr auto kLoopSlice = std::chrono::milliseconds(5); +constexpr auto kReaderJoinGrace = std::chrono::milliseconds(50); + +std::atomic g_session_sequence{0}; + +class ScopeExit final { +public: + explicit ScopeExit(std::function callback) + : callback_(std::move(callback)) + { + } + + ~ScopeExit() noexcept + { + if (!callback_) { + return; + } + try { + callback_(); + } catch (...) { + } + } + + void release() noexcept { callback_ = {}; } + +private: + std::function callback_; +}; + +class DisabledArmTeleopBackend final : public ArmTeleopBackend { +public: + bool available() const noexcept override { return false; } + + arm_teleop::RobotManifest manifest() const override { return {}; } + + bool supportsForceFeedback() const noexcept override { return false; } + + ArmTeleopBackendResult open( + const arm_teleop::OpenSession&) override + { + return ArmTeleopBackendResult::failure( + grpc::StatusCode::FAILED_PRECONDITION, + "arm teleoperation backend is disabled"); + } + + ArmTeleopBackendResult applySetpoint( + const arm_teleop::JointSetpoint&, + std::chrono::steady_clock::time_point) override + { + return ArmTeleopBackendResult::failure( + grpc::StatusCode::FAILED_PRECONDITION, + "arm teleoperation backend is disabled"); + } + + ArmTeleopBackendResult stop( + const arm_teleop::StopReason, + const std::string&) override + { + return ArmTeleopBackendResult::ok(); + } + + ArmTeleopBackendSnapshot snapshot() const override + { + ArmTeleopBackendSnapshot result; + result.safety.set_connected(false); + result.safety.set_powered_on(false); + result.safety.set_fault(true); + result.safety.set_fault_detail( + "arm teleoperation backend is disabled"); + return result; + } +}; + +struct NegotiatedOpen { + std::uint32_t watchdog_ms{kDefaultWatchdogMs}; + std::uint32_t lease_ms{kDefaultLeaseMs}; +}; + +struct SessionRuntime { + std::string session_id; + std::uint64_t received_sequence{0}; + std::uint64_t applied_sequence{0}; + std::uint64_t dropped_setpoints{0}; + std::uint64_t rejected_setpoints{0}; + std::uint32_t watchdog_ms{kDefaultWatchdogMs}; + std::uint32_t lease_ms{kDefaultLeaseMs}; + Clock::time_point lease_deadline{}; +}; + +struct PendingFrame { + arm_teleop::ClientFrame frame; + Clock::time_point arrived{}; + std::uint64_t ordinal{0}; +}; + +bool isHexDigest(const std::string& value) +{ + return value.size() == 64 && + std::all_of(value.begin(), value.end(), [](const unsigned char ch) { + return std::isxdigit(ch) != 0; + }); +} + +grpc::Status validateManifestSyntax( + const arm_teleop::RobotManifest& manifest) +{ + if (manifest.robot_id().empty() || + manifest.robot_id().size() > kMaximumIdentifierLength) { + return grpc::Status( + grpc::StatusCode::INVALID_ARGUMENT, + "expected_robot.robot_id is required and must not exceed 128 bytes"); + } + if (!isHexDigest(manifest.model_sha256())) { + return grpc::Status( + grpc::StatusCode::INVALID_ARGUMENT, + "expected_robot.model_sha256 must contain 64 hexadecimal characters"); + } + if (!isHexDigest(manifest.calibration_sha256())) { + return grpc::Status( + grpc::StatusCode::INVALID_ARGUMENT, + "expected_robot.calibration_sha256 must contain 64 hexadecimal characters"); + } + if (manifest.joint_names_size() == 0 || + manifest.joint_names_size() > + static_cast(kMaximumJoints)) { + return grpc::Status( + grpc::StatusCode::INVALID_ARGUMENT, + "expected_robot.joint_names must contain between 1 and 64 joints"); + } + + std::unordered_set joint_names; + joint_names.reserve( + static_cast(manifest.joint_names_size())); + for (const auto& joint_name : manifest.joint_names()) { + if (joint_name.empty() || + joint_name.size() > kMaximumIdentifierLength || + !joint_names.insert(joint_name).second) { + return grpc::Status( + grpc::StatusCode::INVALID_ARGUMENT, + "expected_robot.joint_names must be non-empty and unique"); + } + } + if (manifest.position_unit().empty() || + manifest.velocity_unit().empty() || + manifest.effort_unit().empty()) { + return grpc::Status( + grpc::StatusCode::INVALID_ARGUMENT, + "expected_robot position, velocity, and effort units are required"); + } + if (manifest.base_frame().empty() || manifest.tool_frame().empty()) { + return grpc::Status( + grpc::StatusCode::INVALID_ARGUMENT, + "expected_robot base_frame and tool_frame are required"); + } + return grpc::Status::OK; +} + +grpc::Status validateOpen( + const arm_teleop::OpenSession& open, + NegotiatedOpen& negotiated) +{ + if (open.protocol_major() != kProtocolMajor) { + return grpc::Status( + grpc::StatusCode::FAILED_PRECONDITION, + "unsupported arm teleoperation protocol major"); + } + if (open.protocol_minor() > kProtocolMinor) { + return grpc::Status( + grpc::StatusCode::FAILED_PRECONDITION, + "unsupported arm teleoperation protocol minor"); + } + if (open.client_instance_id().empty() || + open.client_instance_id().size() > kMaximumIdentifierLength) { + return grpc::Status( + grpc::StatusCode::INVALID_ARGUMENT, + "client_instance_id is required and must not exceed 128 bytes"); + } + + const auto manifest_status = + validateManifestSyntax(open.expected_robot()); + if (!manifest_status.ok()) { + return manifest_status; + } + + if (open.requested_command_rate_hz() == 0 || + open.requested_command_rate_hz() > kMaximumRateHz) { + return grpc::Status( + grpc::StatusCode::INVALID_ARGUMENT, + "requested_command_rate_hz must be in [1, 1000]"); + } + if (open.requested_state_rate_hz() == 0 || + open.requested_state_rate_hz() > kMaximumRateHz) { + return grpc::Status( + grpc::StatusCode::INVALID_ARGUMENT, + "requested_state_rate_hz must be in [1, 1000]"); + } + + negotiated.watchdog_ms = + open.watchdog_timeout_ms() == 0 + ? kDefaultWatchdogMs + : open.watchdog_timeout_ms(); + if (negotiated.watchdog_ms < kMinimumWatchdogMs || + negotiated.watchdog_ms > kMaximumWatchdogMs) { + return grpc::Status( + grpc::StatusCode::INVALID_ARGUMENT, + "watchdog_timeout_ms must be zero or in [20, 60000]"); + } + + negotiated.lease_ms = + open.requested_lease_ms() == 0 + ? kDefaultLeaseMs + : open.requested_lease_ms(); + if (negotiated.lease_ms < negotiated.watchdog_ms || + negotiated.lease_ms > kMaximumLeaseMs) { + return grpc::Status( + grpc::StatusCode::INVALID_ARGUMENT, + "requested_lease_ms must be zero or between watchdog_timeout_ms and 600000"); + } + return grpc::Status::OK; +} + +grpc::Status compareManifests( + const arm_teleop::RobotManifest& expected, + const arm_teleop::RobotManifest& actual) +{ + if (expected.robot_id() != actual.robot_id()) { + return grpc::Status( + grpc::StatusCode::FAILED_PRECONDITION, + "robot_id does not match the teleoperation backend"); + } + if (expected.model_sha256() != actual.model_sha256()) { + return grpc::Status( + grpc::StatusCode::FAILED_PRECONDITION, + "model_sha256 does not match the teleoperation backend"); + } + if (expected.calibration_sha256() != + actual.calibration_sha256()) { + return grpc::Status( + grpc::StatusCode::FAILED_PRECONDITION, + "calibration_sha256 does not match the teleoperation backend"); + } + if (expected.joint_names_size() != actual.joint_names_size()) { + return grpc::Status( + grpc::StatusCode::FAILED_PRECONDITION, + "joint count does not match the teleoperation backend"); + } + for (int index = 0; index < expected.joint_names_size(); ++index) { + if (expected.joint_names(index) != actual.joint_names(index)) { + return grpc::Status( + grpc::StatusCode::FAILED_PRECONDITION, + "joint order does not match the teleoperation backend"); + } + } + if (expected.position_unit() != actual.position_unit() || + expected.velocity_unit() != actual.velocity_unit() || + expected.effort_unit() != actual.effort_unit()) { + return grpc::Status( + grpc::StatusCode::FAILED_PRECONDITION, + "joint units do not match the teleoperation backend"); + } + if (expected.base_frame() != actual.base_frame() || + expected.tool_frame() != actual.tool_frame()) { + return grpc::Status( + grpc::StatusCode::FAILED_PRECONDITION, + "base or tool frame does not match the teleoperation backend"); + } + return grpc::Status::OK; +} + +grpc::Status validateSetpoint( + const arm_teleop::JointSetpoint& setpoint, + const std::size_t joint_count, + const std::uint32_t watchdog_ms) +{ + if (setpoint.valid_for_us() == 0) { + return grpc::Status( + grpc::StatusCode::INVALID_ARGUMENT, + "setpoint.valid_for_us must be non-zero"); + } + const std::uint64_t maximum_validity_us = + static_cast(watchdog_ms) * 1000U; + if (setpoint.valid_for_us() > maximum_validity_us) { + return grpc::Status( + grpc::StatusCode::INVALID_ARGUMENT, + "setpoint.valid_for_us must not exceed the negotiated watchdog"); + } + if (setpoint.position_rad_size() != + static_cast(joint_count) || + setpoint.velocity_rad_s_size() != + static_cast(joint_count)) { + return grpc::Status( + grpc::StatusCode::INVALID_ARGUMENT, + "setpoint position and velocity dimensions must match the robot manifest"); + } + for (const double value : setpoint.position_rad()) { + if (!std::isfinite(value)) { + return grpc::Status( + grpc::StatusCode::INVALID_ARGUMENT, + "setpoint positions must be finite"); + } + } + for (const double value : setpoint.velocity_rad_s()) { + if (!std::isfinite(value)) { + return grpc::Status( + grpc::StatusCode::INVALID_ARGUMENT, + "setpoint velocities must be finite"); + } + } + return grpc::Status::OK; +} + +std::string nextSessionId() +{ + const auto sequence = + g_session_sequence.fetch_add(1, std::memory_order_relaxed) + 1; + const auto timestamp = std::chrono::duration_cast( + Clock::now().time_since_epoch()) + .count(); + std::ostringstream output; + output << "arm-teleop-" << timestamp << "-" << sequence; + return output.str(); +} + +std::uint32_t leaseRemainingMs( + const Clock::time_point now, + const Clock::time_point deadline) +{ + if (now >= deadline) { + return 0; + } + const auto remaining = + std::chrono::duration_cast( + deadline - now); + const auto value = remaining.count(); + if (value <= 0) { + return 1; + } + return static_cast( + std::min(value, kMaximumLeaseMs)); +} + +void fillServerFrame( + const SessionRuntime& session, + const arm_teleop::SessionPhase phase, + const arm_teleop::StopReason stop_reason, + const std::string& detail, + const ArmTeleopBackendSnapshot& snapshot, + arm_teleop::ServerFrame& frame) +{ + auto* status = frame.mutable_status(); + status->set_session_id(session.session_id); + status->set_phase(phase); + status->set_received_sequence(session.received_sequence); + status->set_applied_sequence(session.applied_sequence); + status->set_dropped_setpoints(session.dropped_setpoints); + status->set_rejected_setpoints(session.rejected_setpoints); + status->set_negotiated_watchdog_ms(session.watchdog_ms); + status->set_lease_remaining_ms( + leaseRemainingMs(Clock::now(), session.lease_deadline)); + status->set_stop_reason(stop_reason); + status->set_detail(detail); + *frame.mutable_joint_state() = snapshot.joint_state; + *frame.mutable_safety() = snapshot.safety; +} + +bool writeBareRejection( + grpc::ServerReaderWriter* stream, + const grpc::Status& status) +{ + arm_teleop::ServerFrame frame; + frame.mutable_status()->set_phase( + arm_teleop::SESSION_PHASE_REJECTED); + frame.mutable_status()->set_stop_reason( + arm_teleop::STOP_REASON_PROTOCOL_ERROR); + frame.mutable_status()->set_detail(status.error_message()); + frame.mutable_safety()->set_connected(false); + frame.mutable_safety()->set_powered_on(false); + frame.mutable_safety()->set_fault(true); + frame.mutable_safety()->set_fault_detail(status.error_message()); + return stream->Write(frame); +} + +grpc::Status cancelledStatus(grpc::ServerContext* context) +{ + if (std::chrono::system_clock::now() >= context->deadline()) { + return grpc::Status( + grpc::StatusCode::DEADLINE_EXCEEDED, + "arm teleoperation RPC deadline exceeded"); + } + return grpc::Status( + grpc::StatusCode::CANCELLED, + "arm teleoperation RPC cancelled"); +} + +} // namespace + +std::shared_ptr makeDisabledArmTeleopBackend() +{ + return std::make_shared(); +} + +ArmTeleopServiceImpl::ArmTeleopServiceImpl( + std::shared_ptr backend, + control::ControlAuthorityManager* authority) + : backend_(std::move(backend)), + authority_( + authority ? authority + : &control::ControlAuthorityManager::instance()) +{ + if (!backend_) { + backend_ = makeDisabledArmTeleopBackend(); + } +} + +grpc::Status ArmTeleopServiceImpl::Teleoperate( + grpc::ServerContext* context, + grpc::ServerReaderWriter* stream) +{ + if (context == nullptr || stream == nullptr) { + return grpc::Status( + grpc::StatusCode::INTERNAL, + "arm teleoperation server received a null stream"); + } + + arm_teleop::ClientFrame first_frame; + if (!stream->Read(&first_frame)) { + return context->IsCancelled() + ? cancelledStatus(context) + : grpc::Status( + grpc::StatusCode::INVALID_ARGUMENT, + "the first client frame must be OpenSession"); + } + if (!first_frame.has_open()) { + const grpc::Status status( + grpc::StatusCode::INVALID_ARGUMENT, + "the first client frame must be OpenSession"); + writeBareRejection(stream, status); + return status; + } + + NegotiatedOpen negotiated; + const auto open_status = + validateOpen(first_frame.open(), negotiated); + if (!open_status.ok()) { + writeBareRejection(stream, open_status); + return open_status; + } + if (!backend_->available()) { + const grpc::Status status( + grpc::StatusCode::FAILED_PRECONDITION, + "arm teleoperation backend is disabled"); + writeBareRejection(stream, status); + return status; + } + + try { + const auto backend_manifest = backend_->manifest(); + const auto manifest_status = compareManifests( + first_frame.open().expected_robot(), backend_manifest); + if (!manifest_status.ok()) { + writeBareRejection(stream, manifest_status); + return manifest_status; + } + + const std::string session_id = nextSessionId(); + const auto acquired = authority_->tryAcquire( + backend_manifest.robot_id(), + session_id, + std::chrono::milliseconds(negotiated.lease_ms)); + if (!acquired.acquired) { + const grpc::Status status( + grpc::StatusCode::RESOURCE_EXHAUSTED, + acquired.detail.empty() + ? "another controller owns the arm control lease" + : acquired.detail); + writeBareRejection(stream, status); + return status; + } + const auto control_lease = acquired.token; + ScopeExit lease_guard([this, control_lease]() { + authority_->release(control_lease); + }); + + if (first_frame.open().request_force_feedback() && + !backend_->supportsForceFeedback()) { + const grpc::Status status( + grpc::StatusCode::FAILED_PRECONDITION, + "force feedback was requested but is unavailable"); + writeBareRejection(stream, status); + return status; + } + + bool backend_open_attempted = true; + bool backend_stopped = false; + const auto safeStop = + [&](const arm_teleop::StopReason reason, + const std::string& detail) noexcept { + if (!backend_open_attempted || backend_stopped) { + return ArmTeleopBackendResult::ok(); + } + backend_stopped = true; + try { + return backend_->stop(reason, detail); + } catch (const std::exception& error) { + return ArmTeleopBackendResult::failure( + grpc::StatusCode::INTERNAL, + std::string("teleoperation backend stop exception: ") + + error.what()); + } catch (...) { + return ArmTeleopBackendResult::failure( + grpc::StatusCode::INTERNAL, + "teleoperation backend stop exception"); + } + }; + ScopeExit backend_guard([&]() { + safeStop( + arm_teleop::STOP_REASON_PROTOCOL_ERROR, + "arm teleoperation handler terminated unexpectedly"); + }); + + const auto backend_open = backend_->open(first_frame.open()); + if (!backend_open.success) { + const auto stopped = safeStop( + arm_teleop::STOP_REASON_PROTOCOL_ERROR, + backend_open.detail); + const std::string detail = + stopped.success + ? backend_open.detail + : backend_open.detail + "; " + stopped.detail; + const grpc::Status status( + stopped.success ? backend_open.status_code + : grpc::StatusCode::INTERNAL, + detail); + writeBareRejection(stream, status); + return status; + } + + SessionRuntime session; + session.session_id = session_id; + session.watchdog_ms = negotiated.watchdog_ms; + session.lease_ms = negotiated.lease_ms; + session.lease_deadline = + Clock::now() + std::chrono::milliseconds(session.lease_ms); + const std::size_t joint_count = static_cast( + backend_manifest.joint_names_size()); + + const auto backendSnapshot = + [&]() noexcept { + ArmTeleopBackendSnapshot snapshot; + try { + snapshot = backend_->snapshot(); + } catch (const std::exception& error) { + snapshot.safety.set_fault(true); + snapshot.safety.set_fault_detail( + std::string("teleoperation backend snapshot exception: ") + + error.what()); + } catch (...) { + snapshot.safety.set_fault(true); + snapshot.safety.set_fault_detail( + "teleoperation backend snapshot exception"); + } + return snapshot; + }; + const auto writeStatus = + [&](const arm_teleop::SessionPhase phase, + const arm_teleop::StopReason reason, + const std::string& detail) { + arm_teleop::ServerFrame frame; + fillServerFrame( + session, phase, reason, detail, + backendSnapshot(), frame); + return stream->Write(frame); + }; + + if (!writeStatus( + arm_teleop::SESSION_PHASE_OPENED, + arm_teleop::STOP_REASON_UNSPECIFIED, {})) { + safeStop( + arm_teleop::STOP_REASON_CLIENT_SHUTDOWN, + "client stopped reading while opening"); + return grpc::Status( + grpc::StatusCode::CANCELLED, + "client stopped reading while opening"); + } + if (!writeStatus( + arm_teleop::SESSION_PHASE_READY, + arm_teleop::STOP_REASON_UNSPECIFIED, {})) { + safeStop( + arm_teleop::STOP_REASON_CLIENT_SHUTDOWN, + "client stopped reading while entering ready state"); + return grpc::Status( + grpc::StatusCode::CANCELLED, + "client stopped reading while entering ready state"); + } + + struct InputSlot { + std::mutex mutex; + std::condition_variable cv; + std::optional latest_setpoint; + std::optional latest_heartbeat; + std::optional terminal; + bool ended{false}; + bool reader_failed{false}; + std::string reader_error; + std::uint64_t dropped_setpoints{0}; + std::uint64_t next_ordinal{0}; + } input; + + std::thread reader; + bool reader_joined = false; + const auto joinReader = + [&](const bool cancel_context) noexcept { + if (cancel_context) { + bool ended = false; + try { + std::unique_lock lock(input.mutex); + input.cv.wait_for( + lock, kReaderJoinGrace, + [&]() { return input.ended; }); + ended = input.ended; + } catch (...) { + } + if (!ended) { + context->TryCancel(); + } + } + input.cv.notify_all(); + if (reader.joinable()) { + try { + reader.join(); + } catch (...) { + } + } + reader_joined = true; + }; + ScopeExit stream_guard([&]() { + safeStop( + arm_teleop::STOP_REASON_PROTOCOL_ERROR, + "arm teleoperation stream terminated unexpectedly"); + if (!reader_joined) { + joinReader(true); + } + }); + + try { + reader = std::thread([&]() { + try { + arm_teleop::ClientFrame incoming; + while (stream->Read(&incoming)) { + PendingFrame pending; + pending.arrived = Clock::now(); + pending.frame = std::move(incoming); + bool terminal = false; + { + std::lock_guard lock(input.mutex); + pending.ordinal = ++input.next_ordinal; + if (pending.frame.has_setpoint()) { + if (input.latest_setpoint.has_value()) { + ++input.dropped_setpoints; + } + input.latest_setpoint = std::move(pending); + } else if (pending.frame.has_heartbeat()) { + input.latest_heartbeat = std::move(pending); + } else { + if (input.latest_setpoint.has_value()) { + ++input.dropped_setpoints; + } + input.latest_setpoint.reset(); + input.latest_heartbeat.reset(); + input.terminal = std::move(pending); + terminal = true; + } + } + input.cv.notify_one(); + incoming.Clear(); + if (terminal) { + break; + } + } + } catch (const std::exception& error) { + std::lock_guard lock(input.mutex); + input.reader_failed = true; + input.reader_error = error.what(); + } catch (...) { + std::lock_guard lock(input.mutex); + input.reader_failed = true; + input.reader_error = "unknown reader exception"; + } + { + std::lock_guard lock(input.mutex); + input.ended = true; + } + input.cv.notify_one(); + }); + } catch (const std::exception& error) { + const std::string detail = + std::string("failed to start teleoperation reader: ") + + error.what(); + safeStop( + arm_teleop::STOP_REASON_PROTOCOL_ERROR, detail); + return grpc::Status( + grpc::StatusCode::INTERNAL, detail); + } + + Clock::time_point last_valid_activity = Clock::now(); + + const auto finish = + [&](const arm_teleop::SessionPhase requested_phase, + const arm_teleop::StopReason reason, + const std::string& requested_detail, + const grpc::Status& requested_status, + const bool cancel_reader) { + const auto stopped = safeStop(reason, requested_detail); + const auto phase = + stopped.success + ? requested_phase + : arm_teleop::SESSION_PHASE_FAILED; + const std::string detail = + stopped.success + ? requested_detail + : requested_detail + "; " + stopped.detail; + const bool wrote = + writeStatus(phase, reason, detail); + bool reader_has_ended = false; + { + std::lock_guard lock(input.mutex); + reader_has_ended = input.ended; + } + // Protocol errors are terminal even if a misbehaving client + // keeps its write half open. Never wait indefinitely for the + // reader's blocking Read in that case. + joinReader(cancel_reader || !reader_has_ended); + stream_guard.release(); + if (!stopped.success) { + return grpc::Status( + grpc::StatusCode::INTERNAL, detail); + } + if (!wrote) { + return grpc::Status( + grpc::StatusCode::CANCELLED, + "client stopped reading the terminal teleoperation status"); + } + return requested_status; + }; + + for (;;) { + if (context->IsCancelled() || + std::chrono::system_clock::now() >= context->deadline()) { + const auto status = cancelledStatus(context); + const auto stopped = safeStop( + arm_teleop::STOP_REASON_CLIENT_SHUTDOWN, + status.error_message()); + joinReader(true); + stream_guard.release(); + return stopped.success + ? status + : grpc::Status( + grpc::StatusCode::INTERNAL, + status.error_message() + "; " + + stopped.detail); + } + + std::optional pending; + bool ended = false; + bool reader_failed = false; + std::string reader_error; + { + std::unique_lock lock(input.mutex); + input.cv.wait_for(lock, kLoopSlice, [&]() { + return input.latest_setpoint.has_value() || + input.latest_heartbeat.has_value() || + input.terminal.has_value() || + input.ended; + }); + + if (input.terminal.has_value()) { + pending = std::move(input.terminal); + input.terminal.reset(); + } else if (input.latest_setpoint.has_value() && + input.latest_heartbeat.has_value()) { + if (input.latest_setpoint->ordinal < + input.latest_heartbeat->ordinal) { + pending = std::move(input.latest_setpoint); + input.latest_setpoint.reset(); + } else { + pending = std::move(input.latest_heartbeat); + input.latest_heartbeat.reset(); + } + } else if (input.latest_setpoint.has_value()) { + pending = std::move(input.latest_setpoint); + input.latest_setpoint.reset(); + } else if (input.latest_heartbeat.has_value()) { + pending = std::move(input.latest_heartbeat); + input.latest_heartbeat.reset(); + } + session.dropped_setpoints = + input.dropped_setpoints; + ended = input.ended; + reader_failed = input.reader_failed; + reader_error = input.reader_error; + } + + const auto now = Clock::now(); + if (pending.has_value() && pending->frame.has_stop()) { + const auto requested_reason = + pending->frame.stop().reason(); + if (requested_reason == + arm_teleop::STOP_REASON_UNSPECIFIED) { + const std::string detail = + "StopSession.reason must be specified"; + ++session.rejected_setpoints; + return finish( + arm_teleop::SESSION_PHASE_REJECTED, + arm_teleop::STOP_REASON_PROTOCOL_ERROR, + detail, + grpc::Status( + grpc::StatusCode::INVALID_ARGUMENT, + detail), + false); + } + return finish( + arm_teleop::SESSION_PHASE_STOPPED, + requested_reason, + pending->frame.stop().detail(), + grpc::Status::OK, false); + } + if (now >= session.lease_deadline) { + const std::string detail = + "arm teleoperation control lease expired"; + return finish( + arm_teleop::SESSION_PHASE_LEASE_LOST, + arm_teleop::STOP_REASON_LEASE_REVOKED, + detail, + grpc::Status( + grpc::StatusCode::ABORTED, detail), + true); + } + if (!pending.has_value()) { + if (reader_failed) { + const std::string detail = + "teleoperation reader failed: " + reader_error; + return finish( + arm_teleop::SESSION_PHASE_FAILED, + arm_teleop::STOP_REASON_PROTOCOL_ERROR, + detail, + grpc::Status( + grpc::StatusCode::INTERNAL, detail), + false); + } + if (ended) { + return finish( + arm_teleop::SESSION_PHASE_STOPPED, + arm_teleop::STOP_REASON_CLIENT_SHUTDOWN, + "client closed the teleoperation input stream", + grpc::Status::OK, false); + } + if (now - last_valid_activity >= + std::chrono::milliseconds(session.watchdog_ms)) { + const std::string detail = + "arm teleoperation watchdog expired"; + return finish( + arm_teleop::SESSION_PHASE_WATCHDOG_EXPIRED, + arm_teleop::STOP_REASON_WATCHDOG, + detail, + grpc::Status( + grpc::StatusCode::DEADLINE_EXCEEDED, + detail), + true); + } + continue; + } + + if (pending->frame.has_open() || + pending->frame.payload_case() == + arm_teleop::ClientFrame::PAYLOAD_NOT_SET) { + const std::string detail = + "OpenSession is only valid as the first client frame"; + return finish( + arm_teleop::SESSION_PHASE_REJECTED, + arm_teleop::STOP_REASON_PROTOCOL_ERROR, + detail, + grpc::Status( + grpc::StatusCode::INVALID_ARGUMENT, detail), + false); + } + + const std::uint64_t sequence = + pending->frame.has_setpoint() + ? pending->frame.setpoint().sequence() + : pending->frame.heartbeat().sequence(); + if (sequence == 0 || + sequence <= session.received_sequence) { + const std::string detail = + "client sequence must be strictly increasing and non-zero"; + if (pending->frame.has_setpoint()) { + ++session.rejected_setpoints; + } + return finish( + arm_teleop::SESSION_PHASE_REJECTED, + arm_teleop::STOP_REASON_PROTOCOL_ERROR, + detail, + grpc::Status( + grpc::StatusCode::INVALID_ARGUMENT, detail), + false); + } + if (pending->arrived - last_valid_activity >= + std::chrono::milliseconds(session.watchdog_ms)) { + const std::string detail = + "arm teleoperation watchdog expired before the next valid client frame"; + return finish( + arm_teleop::SESSION_PHASE_WATCHDOG_EXPIRED, + arm_teleop::STOP_REASON_WATCHDOG, + detail, + grpc::Status( + grpc::StatusCode::DEADLINE_EXCEEDED, + detail), + true); + } + + if (pending->frame.has_heartbeat()) { + if (!authority_->renew( + control_lease, + std::chrono::milliseconds( + session.lease_ms))) { + const std::string detail = + "arm teleoperation control authority was revoked"; + return finish( + arm_teleop::SESSION_PHASE_LEASE_LOST, + arm_teleop::STOP_REASON_LEASE_REVOKED, + detail, + grpc::Status( + grpc::StatusCode::ABORTED, detail), + true); + } + session.received_sequence = sequence; + last_valid_activity = pending->arrived; + session.lease_deadline = + pending->arrived + + std::chrono::milliseconds(session.lease_ms); + if (!writeStatus( + session.applied_sequence == 0 + ? arm_teleop::SESSION_PHASE_READY + : arm_teleop::SESSION_PHASE_ACTIVE, + arm_teleop::STOP_REASON_UNSPECIFIED, {})) { + const std::string detail = + "client stopped reading heartbeat status"; + const auto stopped = safeStop( + arm_teleop::STOP_REASON_CLIENT_SHUTDOWN, + detail); + joinReader(true); + stream_guard.release(); + return stopped.success + ? grpc::Status( + grpc::StatusCode::CANCELLED, + detail) + : grpc::Status( + grpc::StatusCode::INTERNAL, + detail + "; " + stopped.detail); + } + continue; + } + + const auto& setpoint = pending->frame.setpoint(); + const auto setpoint_status = validateSetpoint( + setpoint, joint_count, session.watchdog_ms); + if (!setpoint_status.ok()) { + ++session.rejected_setpoints; + return finish( + arm_teleop::SESSION_PHASE_REJECTED, + arm_teleop::STOP_REASON_PROTOCOL_ERROR, + setpoint_status.error_message(), + setpoint_status, false); + } + session.received_sequence = sequence; + + const auto command_deadline = + pending->arrived + + std::chrono::microseconds(setpoint.valid_for_us()); + if (Clock::now() >= command_deadline) { + ++session.rejected_setpoints; + if (Clock::now() - last_valid_activity >= + std::chrono::milliseconds(session.watchdog_ms)) { + const std::string detail = + "arm teleoperation watchdog expired while rejecting stale setpoints"; + return finish( + arm_teleop::SESSION_PHASE_WATCHDOG_EXPIRED, + arm_teleop::STOP_REASON_WATCHDOG, + detail, + grpc::Status( + grpc::StatusCode::DEADLINE_EXCEEDED, + detail), + true); + } + if (!writeStatus( + arm_teleop::SESSION_PHASE_HOLDING, + arm_teleop::STOP_REASON_UNSPECIFIED, + "setpoint expired before backend dispatch")) { + const std::string detail = + "client stopped reading expired-setpoint status"; + const auto stopped = safeStop( + arm_teleop::STOP_REASON_CLIENT_SHUTDOWN, + detail); + joinReader(true); + stream_guard.release(); + return stopped.success + ? grpc::Status( + grpc::StatusCode::CANCELLED, + detail) + : grpc::Status( + grpc::StatusCode::INTERNAL, + detail + "; " + stopped.detail); + } + continue; + } + + if (!authority_->renew( + control_lease, + std::chrono::milliseconds( + session.lease_ms))) { + const std::string detail = + "arm teleoperation control authority was revoked"; + return finish( + arm_teleop::SESSION_PHASE_LEASE_LOST, + arm_teleop::STOP_REASON_LEASE_REVOKED, + detail, + grpc::Status( + grpc::StatusCode::ABORTED, detail), + true); + } + session.lease_deadline = + pending->arrived + + std::chrono::milliseconds(session.lease_ms); + const auto applied = + backend_->applySetpoint(setpoint, command_deadline); + if (!applied.success) { + ++session.rejected_setpoints; + return finish( + arm_teleop::SESSION_PHASE_FAILED, + arm_teleop::STOP_REASON_ROBOT_FAULT, + applied.detail, + grpc::Status(applied.status_code, applied.detail), + true); + } + session.applied_sequence = sequence; + last_valid_activity = pending->arrived; + if (!writeStatus( + arm_teleop::SESSION_PHASE_ACTIVE, + arm_teleop::STOP_REASON_UNSPECIFIED, {})) { + const std::string detail = + "client stopped reading active teleoperation status"; + const auto stopped = safeStop( + arm_teleop::STOP_REASON_CLIENT_SHUTDOWN, + detail); + joinReader(true); + stream_guard.release(); + return stopped.success + ? grpc::Status( + grpc::StatusCode::CANCELLED, + detail) + : grpc::Status( + grpc::StatusCode::INTERNAL, + detail + "; " + stopped.detail); + } + } + } catch (const std::exception& error) { + return grpc::Status( + grpc::StatusCode::INTERNAL, + std::string("arm teleoperation service exception: ") + + error.what()); + } catch (...) { + return grpc::Status( + grpc::StatusCode::INTERNAL, + "arm teleoperation service exception"); + } +} + +} // namespace cmvr::service diff --git a/cmvr-es/service/grpc/src/grpc_robot_arm_teleop_backend.cpp b/cmvr-es/service/grpc/src/grpc_robot_arm_teleop_backend.cpp new file mode 100644 index 00000000..d839718c --- /dev/null +++ b/cmvr-es/service/grpc/src/grpc_robot_arm_teleop_backend.cpp @@ -0,0 +1,649 @@ +#include "service/grpc/include/grpc_robot_arm_teleop_backend.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace cmvr::service { +namespace { + +using Clock = std::chrono::steady_clock; + +constexpr double kMinimumServoPeriodS = 0.0001; +constexpr double kMaximumServoPeriodS = 0.1; + +bool isDigest(const std::string& value) +{ + return value.size() == 64 && + std::all_of( + value.begin(), value.end(), [](const unsigned char value) { + return std::isxdigit(value) != 0; + }); +} + +arm_teleop::EffortSource toProtoEffortSource( + const device::JointEffortSource source) +{ + switch (source) { + case device::JointEffortSource::MotorEstimate: + return arm_teleop::EFFORT_SOURCE_MOTOR_ESTIMATE; + case device::JointEffortSource::JointSensor: + return arm_teleop::EFFORT_SOURCE_JOINT_SENSOR; + case device::JointEffortSource::ForceTorqueSensor: + return arm_teleop::EFFORT_SOURCE_FORCE_TORQUE_SENSOR; + case device::JointEffortSource::Observer: + return arm_teleop::EFFORT_SOURCE_OBSERVER; + case device::JointEffortSource::Unspecified: + default: + return arm_teleop::EFFORT_SOURCE_UNSPECIFIED; + } +} + +grpc::StatusCode toStatusCode(const device::ArmErrorCode code) +{ + switch (code) { + case device::ArmErrorCode::InvalidArgument: + case device::ArmErrorCode::InvalidDof: + return grpc::StatusCode::INVALID_ARGUMENT; + case device::ArmErrorCode::OutOfJointLimit: + case device::ArmErrorCode::OutOfVelocityLimit: + case device::ArmErrorCode::OutOfAccelerationLimit: + case device::ArmErrorCode::OutOfWorkspace: + return grpc::StatusCode::OUT_OF_RANGE; + case device::ArmErrorCode::NotConnected: + case device::ArmErrorCode::RobotNotReady: + case device::ArmErrorCode::RobotNotPowered: + case device::ArmErrorCode::RobotInFault: + case device::ArmErrorCode::RobotInProtectiveStop: + case device::ArmErrorCode::RobotInEmergencyStop: + case device::ArmErrorCode::CommandRejected: + case device::ArmErrorCode::UnsupportedCommand: + return grpc::StatusCode::FAILED_PRECONDITION; + case device::ArmErrorCode::Timeout: + return grpc::StatusCode::DEADLINE_EXCEEDED; + case device::ArmErrorCode::ConnectionFailed: + return grpc::StatusCode::UNAVAILABLE; + case device::ArmErrorCode::AlreadyConnected: + return grpc::StatusCode::ALREADY_EXISTS; + case device::ArmErrorCode::CommandFailed: + case device::ArmErrorCode::UnknownError: + case device::ArmErrorCode::OK: + default: + return grpc::StatusCode::INTERNAL; + } +} + +ArmTeleopBackendResult fromArmResult( + const device::Result& result, + const char* operation) +{ + if (result.ok()) { + return ArmTeleopBackendResult::ok(); + } + std::string detail(operation); + detail += " failed"; + if (!result.message.empty()) { + detail += ": " + result.message; + } + return ArmTeleopBackendResult::failure( + toStatusCode(result.code), std::move(detail)); +} + +class RobotArmTeleopBackend final : public ArmTeleopBackend { +public: + RobotArmTeleopBackend( + std::shared_ptr arm, + config::ArmTeleopBackendConfig config) + : arm_(std::move(arm)), + config_(std::move(config)), + require_powered_( + !config_.has_require_powered() || + config_.require_powered()) + { + validateStaticConfiguration(); + } + + bool available() const noexcept override + { + return unavailable_reason_.empty(); + } + + std::string unavailableReason() const override + { + return unavailable_reason_; + } + + arm_teleop::RobotManifest manifest() const override + { + return manifest_; + } + + bool supportsForceFeedback() const noexcept override + { + // A non-zero effort vector is not enough. The RobotArm must explicitly + // identify a verified effort source. + return available() && arm_->jointEffortSource() != + device::JointEffortSource::Unspecified; + } + + ArmTeleopBackendResult open( + const arm_teleop::OpenSession& request) override + { + std::lock_guard operation_lock(operation_mutex_); + if (!available()) { + return ArmTeleopBackendResult::failure( + grpc::StatusCode::FAILED_PRECONDITION, + unavailable_reason_); + } + if (session_open_) { + return ArmTeleopBackendResult::failure( + grpc::StatusCode::ALREADY_EXISTS, + "RobotArm teleoperation servo mode is already open"); + } + + const auto state = arm_->getRobotState(); + cacheState(state); + const auto safety_result = validateSafety(state); + if (!safety_result.success) { + return safety_result; + } + if (!validMeasuredPosition(state.actual_joint_state)) { + return ArmTeleopBackendResult::failure( + grpc::StatusCode::FAILED_PRECONDITION, + "RobotArm initial joint position cache is invalid"); + } + if (request.requested_command_rate_hz() == 0) { + return ArmTeleopBackendResult::failure( + grpc::StatusCode::INVALID_ARGUMENT, + "requested_command_rate_hz must be non-zero"); + } + const double requested_period_s = + 1.0 / + static_cast(request.requested_command_rate_hz()); + // A client may request a slower command stream, but it may not claim a + // rate faster than the reviewed RobotArm servo period. + if (requested_period_s + 1e-12 < + config_.servo_period_s()) { + return ArmTeleopBackendResult::failure( + grpc::StatusCode::FAILED_PRECONDITION, + "requested command rate exceeds configured RobotArm servo rate"); + } + minimum_dispatch_period_ = + std::chrono::duration_cast( + std::chrono::duration( + std::max( + requested_period_s, + config_.servo_period_s()))); + + device::ServoOptions options; + options.period = config_.servo_period_s(); + const auto start = Clock::now(); + const auto result = arm_->startServoMode(options); + const auto elapsed = Clock::now() - start; + if (!result.ok()) { + return fromArmResult(result, "startServoMode"); + } + if (elapsed > std::chrono::microseconds( + config_.max_apply_duration_us())) { + bestEffortStop(); + return ArmTeleopBackendResult::failure( + grpc::StatusCode::DEADLINE_EXCEEDED, + "startServoMode exceeded max_apply_duration_us"); + } + + initial_position_ = state.actual_joint_state.position; + last_position_.clear(); + last_dispatch_time_ = Clock::time_point{}; + session_open_ = true; + return ArmTeleopBackendResult::ok(); + } + + ArmTeleopBackendResult applySetpoint( + const arm_teleop::JointSetpoint& setpoint, + const Clock::time_point deadline) override + { + std::lock_guard operation_lock(operation_mutex_); + if (Clock::now() >= deadline) { + return ArmTeleopBackendResult::failure( + grpc::StatusCode::DEADLINE_EXCEEDED, + "setpoint expired before RobotArm backend validation"); + } + if (!session_open_) { + return ArmTeleopBackendResult::failure( + grpc::StatusCode::FAILED_PRECONDITION, + "RobotArm teleoperation servo mode is not open"); + } + + const auto input_result = validateSetpointInput(setpoint); + if (!input_result.success) { + return input_result; + } + + const auto state = arm_->getRobotState(); + cacheState(state); + const auto safety_result = validateSafety(state); + if (!safety_result.success) { + return safety_result; + } + + const std::vector target( + setpoint.position_rad().begin(), + setpoint.position_rad().end()); + const auto& reference = + last_position_.empty() ? initial_position_ : last_position_; + const double allowed_step = + last_position_.empty() + ? config_.max_initial_position_step_rad() + : config_.max_position_step_rad(); + for (std::size_t index = 0; index < target.size(); ++index) { + const double delta = std::abs(target[index] - reference[index]); + if (delta > allowed_step) { + std::ostringstream detail; + detail << "joint " << model_.joint_names[index] + << " position step " << delta + << " exceeds configured limit " << allowed_step; + return ArmTeleopBackendResult::failure( + grpc::StatusCode::OUT_OF_RANGE, detail.str()); + } + // Once a target has been accepted, also enforce the RobotModel + // velocity limit on target-to-target motion. + if (!last_position_.empty() && + delta / + std::chrono::duration( + minimum_dispatch_period_) + .count() > + model_.joint_limits[index].max_velocity) { + std::ostringstream detail; + detail << "joint " << model_.joint_names[index] + << " target delta exceeds RobotModel velocity limit"; + return ArmTeleopBackendResult::failure( + grpc::StatusCode::OUT_OF_RANGE, detail.str()); + } + } + + const auto dispatch_time = Clock::now(); + if (last_dispatch_time_ != Clock::time_point{} && + dispatch_time - last_dispatch_time_ < + minimum_dispatch_period_) { + return ArmTeleopBackendResult::failure( + grpc::StatusCode::RESOURCE_EXHAUSTED, + "setpoint arrived before the negotiated RobotArm dispatch period"); + } + + device::JointPositionCommand command; + command.position = target; + // Robot state validation and command preparation may consume the + // remaining validity window. Re-check on the receiver's monotonic + // timeline at the last point before the RobotArm commit. + const auto start = Clock::now(); + if (start >= deadline) { + return ArmTeleopBackendResult::failure( + grpc::StatusCode::DEADLINE_EXCEEDED, + "setpoint expired before RobotArm command dispatch"); + } + if (deadline - start < + std::chrono::microseconds( + config_.max_apply_duration_us())) { + return ArmTeleopBackendResult::failure( + grpc::StatusCode::DEADLINE_EXCEEDED, + "setpoint lacks the configured RobotArm apply-time budget"); + } + last_dispatch_time_ = start; + const auto result = arm_->servoJ(command); + const auto elapsed = Clock::now() - start; + if (!result.ok()) { + return fromArmResult(result, "servoJ"); + } + + // servoJ has already accepted this target even when the local timing + // contract is exceeded; remember it before returning the failure so a + // caller can never treat an older target as the last applied command. + last_position_ = target; + if (elapsed > std::chrono::microseconds( + config_.max_apply_duration_us())) { + return ArmTeleopBackendResult::failure( + grpc::StatusCode::DEADLINE_EXCEEDED, + "servoJ exceeded max_apply_duration_us"); + } + return ArmTeleopBackendResult::ok(); + } + + ArmTeleopBackendResult stop( + const arm_teleop::StopReason, + const std::string&) override + { + std::lock_guard operation_lock(operation_mutex_); + const auto motion_result = arm_ ? arm_->stopMotion() + : device::Result::success(); + // This is deliberately called even when stopMotion fails. + const auto servo_result = arm_ ? arm_->stopServoMode() + : device::Result::success(); + session_open_ = false; + initial_position_.clear(); + last_position_.clear(); + last_dispatch_time_ = Clock::time_point{}; + minimum_dispatch_period_ = Clock::duration::zero(); + if (arm_) { + cacheState(arm_->getRobotState()); + } + if (!motion_result.ok()) { + return fromArmResult(motion_result, "stopMotion"); + } + return fromArmResult(servo_result, "stopServoMode"); + } + + ArmTeleopBackendSnapshot snapshot() const override + { + std::lock_guard cache_lock(cache_mutex_); + auto result = cached_snapshot_; + if (cache_time_ != Clock::time_point{}) { + const auto age = + std::chrono::duration_cast( + Clock::now() - cache_time_) + .count(); + result.joint_state.set_sample_age_us( + age > 0 ? static_cast(age) : 0U); + } + return result; + } + +private: + void validateStaticConfiguration() + { + if (!config_.enable()) { + unavailable_reason_ = + "RobotArm teleoperation backend is explicitly disabled"; + return; + } + if (!arm_) { + unavailable_reason_ = + "configured RobotArm device was not found"; + return; + } + if (config_.device_id().empty() || + config_.device_id() != arm_->id()) { + unavailable_reason_ = + "arm_teleop device_id must exactly match RobotArm.id"; + return; + } + if (!arm_->supportsTeleopGroupServo()) { + unavailable_reason_ = + "RobotArm teleop group-servo capability is not enabled"; + return; + } + if (!isDigest(config_.model_sha256()) || + !isDigest(config_.calibration_sha256())) { + unavailable_reason_ = + "arm_teleop model and calibration SHA256 values must be 64 hex characters"; + return; + } + if (config_.base_frame().empty() || + config_.tool_frame().empty()) { + unavailable_reason_ = + "arm_teleop base_frame and tool_frame are required"; + return; + } + if (!std::isfinite(config_.servo_period_s()) || + config_.servo_period_s() < kMinimumServoPeriodS || + config_.servo_period_s() > kMaximumServoPeriodS) { + unavailable_reason_ = + "arm_teleop servo_period_s must be in [0.0001, 0.1]"; + return; + } + const double period_us = + config_.servo_period_s() * 1000000.0; + if (config_.max_apply_duration_us() == 0 || + static_cast(config_.max_apply_duration_us()) > + period_us) { + unavailable_reason_ = + "arm_teleop max_apply_duration_us must be non-zero and no greater than one servo period"; + return; + } + if (!std::isfinite(config_.max_initial_position_step_rad()) || + config_.max_initial_position_step_rad() <= 0.0 || + !std::isfinite(config_.max_position_step_rad()) || + config_.max_position_step_rad() <= 0.0) { + unavailable_reason_ = + "arm_teleop position step limits must be finite and positive"; + return; + } + + model_ = arm_->getRobotModel(); + if (!model_.valid() || + model_.joint_limits.size() != model_.dof) { + unavailable_reason_ = + "RobotModel must contain one safety limit for every joint"; + return; + } + std::unordered_set names; + for (std::size_t index = 0; index < model_.dof; ++index) { + const auto& name = model_.joint_names[index]; + const auto& limit = model_.joint_limits[index]; + if (name.empty() || !names.insert(name).second || + !std::isfinite(limit.lower) || + !std::isfinite(limit.upper) || + !std::isfinite(limit.max_velocity) || + limit.lower >= limit.upper || + limit.max_velocity <= 0.0) { + unavailable_reason_ = + "RobotModel joint names and position/velocity limits are invalid"; + return; + } + } + + manifest_.set_robot_id(config_.device_id()); + manifest_.set_model_sha256(config_.model_sha256()); + manifest_.set_calibration_sha256( + config_.calibration_sha256()); + for (const auto& name : model_.joint_names) { + manifest_.add_joint_names(name); + } + manifest_.set_position_unit("rad"); + manifest_.set_velocity_unit("rad/s"); + manifest_.set_effort_unit("N*m"); + manifest_.set_base_frame(config_.base_frame()); + manifest_.set_tool_frame(config_.tool_frame()); + } + + ArmTeleopBackendResult validateSafety( + const device::ArmState& state) const + { + if (!state.connected) { + return ArmTeleopBackendResult::failure( + grpc::StatusCode::FAILED_PRECONDITION, + "RobotArm is not connected"); + } + if (require_powered_ && !state.powered_on) { + return ArmTeleopBackendResult::failure( + grpc::StatusCode::FAILED_PRECONDITION, + "RobotArm is not powered on"); + } + if (state.emergency_stopped || + state.safety_mode == device::SafetyMode::EmergencyStop || + state.safety_mode == device::SafetyMode::SystemEmergencyStop) { + return ArmTeleopBackendResult::failure( + grpc::StatusCode::FAILED_PRECONDITION, + "RobotArm emergency stop is active"); + } + if (state.protective_stopped || + state.safety_mode == device::SafetyMode::ProtectiveStop || + state.safety_mode == device::SafetyMode::SafeguardStop) { + return ArmTeleopBackendResult::failure( + grpc::StatusCode::FAILED_PRECONDITION, + "RobotArm protective stop is active"); + } + if (state.fault || + state.robot_mode == device::RobotMode::Fault || + state.safety_mode == device::SafetyMode::Fault) { + return ArmTeleopBackendResult::failure( + grpc::StatusCode::FAILED_PRECONDITION, + "RobotArm fault is active"); + } + return ArmTeleopBackendResult::ok(); + } + + bool validMeasuredPosition( + const device::JointGroupState& state) const + { + if (!state.position_valid || + state.position.size() != model_.dof) { + return false; + } + for (std::size_t index = 0; index < model_.dof; ++index) { + const double value = state.position[index]; + const auto& limit = model_.joint_limits[index]; + if (!std::isfinite(value) || + value < limit.lower || value > limit.upper) { + return false; + } + } + return true; + } + + ArmTeleopBackendResult validateSetpointInput( + const arm_teleop::JointSetpoint& setpoint) const + { + if (setpoint.position_rad_size() != + static_cast(model_.dof) || + setpoint.velocity_rad_s_size() != + static_cast(model_.dof)) { + return ArmTeleopBackendResult::failure( + grpc::StatusCode::INVALID_ARGUMENT, + "setpoint dimensions do not match RobotModel"); + } + for (std::size_t index = 0; index < model_.dof; ++index) { + const double position = + setpoint.position_rad(static_cast(index)); + const double velocity = + setpoint.velocity_rad_s(static_cast(index)); + const auto& limit = model_.joint_limits[index]; + if (!std::isfinite(position) || !std::isfinite(velocity)) { + return ArmTeleopBackendResult::failure( + grpc::StatusCode::INVALID_ARGUMENT, + "setpoint position and velocity must be finite"); + } + if (position < limit.lower || position > limit.upper) { + return ArmTeleopBackendResult::failure( + grpc::StatusCode::OUT_OF_RANGE, + "setpoint position exceeds RobotModel joint limit"); + } + if (std::abs(velocity) > limit.max_velocity) { + return ArmTeleopBackendResult::failure( + grpc::StatusCode::OUT_OF_RANGE, + "setpoint velocity exceeds RobotModel joint limit"); + } + } + return ArmTeleopBackendResult::ok(); + } + + void cacheState(const device::ArmState& state) const + { + ArmTeleopBackendSnapshot snapshot; + auto* joint = &snapshot.joint_state; + const auto& source = state.actual_joint_state; + joint->set_sample_sequence(source.sequence); + for (const double value : source.position) { + joint->add_position_rad(value); + } + for (const double value : source.velocity) { + joint->add_velocity_rad_s(value); + } + + const auto effort_source = + toProtoEffortSource(arm_->jointEffortSource()); + const bool effort_valid = + source.effort_valid && + source.effort.size() == model_.dof && + effort_source != arm_teleop::EFFORT_SOURCE_UNSPECIFIED && + std::all_of( + source.effort.begin(), source.effort.end(), + [](const double value) { return std::isfinite(value); }); + if (effort_valid) { + for (const double value : source.effort) { + joint->add_effort_nm(value); + } + } + joint->set_position_valid(validMeasuredPosition(source)); + joint->set_velocity_valid( + source.velocity_valid && + source.velocity.size() == model_.dof && + std::all_of( + source.velocity.begin(), source.velocity.end(), + [](const double value) { return std::isfinite(value); })); + joint->set_effort_valid(effort_valid); + joint->set_effort_source( + effort_valid ? effort_source + : arm_teleop::EFFORT_SOURCE_UNSPECIFIED); + + auto* safety = &snapshot.safety; + safety->set_connected(state.connected); + safety->set_powered_on(state.powered_on); + safety->set_protective_stopped(state.protective_stopped); + safety->set_emergency_stopped(state.emergency_stopped); + safety->set_fault( + state.fault || + state.robot_mode == device::RobotMode::Fault || + state.safety_mode == device::SafetyMode::Fault); + if (safety->fault()) { + safety->set_fault_detail("RobotArm reports a fault"); + } + + std::lock_guard cache_lock(cache_mutex_); + cached_snapshot_ = std::move(snapshot); + cache_time_ = Clock::now(); + } + + void bestEffortStop() noexcept + { + try { + arm_->stopMotion(); + } catch (...) { + } + try { + arm_->stopServoMode(); + } catch (...) { + } + session_open_ = false; + initial_position_.clear(); + last_position_.clear(); + last_dispatch_time_ = Clock::time_point{}; + minimum_dispatch_period_ = Clock::duration::zero(); + } + + std::shared_ptr arm_; + config::ArmTeleopBackendConfig config_; + bool require_powered_{true}; + device::RobotModel model_; + arm_teleop::RobotManifest manifest_; + std::string unavailable_reason_; + + mutable std::mutex operation_mutex_; + bool session_open_{false}; + std::vector initial_position_; + std::vector last_position_; + Clock::time_point last_dispatch_time_{}; + Clock::duration minimum_dispatch_period_{Clock::duration::zero()}; + + mutable std::mutex cache_mutex_; + mutable ArmTeleopBackendSnapshot cached_snapshot_; + mutable Clock::time_point cache_time_{}; +}; + +} // namespace + +std::shared_ptr makeRobotArmTeleopBackend( + std::shared_ptr arm, + const config::ArmTeleopBackendConfig& config) +{ + return std::make_shared( + std::move(arm), config); +} + +} // namespace cmvr::service diff --git a/cmvr-es/service/grpc/tests/grpc_arm_teleop_service_test.cpp b/cmvr-es/service/grpc/tests/grpc_arm_teleop_service_test.cpp new file mode 100644 index 00000000..1bbdc818 --- /dev/null +++ b/cmvr-es/service/grpc/tests/grpc_arm_teleop_service_test.cpp @@ -0,0 +1,671 @@ +#include "service/grpc/include/grpc_arm_teleop_service.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +namespace cmvr::service { +namespace { + +using namespace std::chrono_literals; + +std::atomic g_socket_sequence{0}; + +arm_teleop::RobotManifest makeManifest() +{ + arm_teleop::RobotManifest manifest; + manifest.set_robot_id("fake_humanoid_arm"); + manifest.set_model_sha256(std::string(64, 'a')); + manifest.set_calibration_sha256(std::string(64, 'b')); + manifest.add_joint_names("shoulder_joint"); + manifest.add_joint_names("elbow_joint"); + manifest.set_position_unit("rad"); + manifest.set_velocity_unit("rad/s"); + manifest.set_effort_unit("N*m"); + manifest.set_base_frame("base_link"); + manifest.set_tool_frame("tool_link"); + return manifest; +} + +arm_teleop::ClientFrame makeOpenFrame( + const arm_teleop::RobotManifest& manifest, + const std::uint32_t watchdog_ms = 100, + const std::uint32_t lease_ms = 2000, + const bool request_force_feedback = false) +{ + arm_teleop::ClientFrame frame; + auto* open = frame.mutable_open(); + open->set_protocol_major(1); + open->set_protocol_minor(0); + open->set_client_instance_id("test-client"); + *open->mutable_expected_robot() = manifest; + open->set_requested_command_rate_hz(200); + open->set_requested_state_rate_hz(100); + open->set_watchdog_timeout_ms(watchdog_ms); + open->set_requested_lease_ms(lease_ms); + open->set_request_force_feedback(request_force_feedback); + return frame; +} + +arm_teleop::ClientFrame makeSetpoint( + const std::uint64_t sequence, + const std::uint32_t valid_for_us = 50000) +{ + arm_teleop::ClientFrame frame; + auto* setpoint = frame.mutable_setpoint(); + setpoint->set_sequence(sequence); + setpoint->add_position_rad(0.1 * static_cast(sequence)); + setpoint->add_position_rad(0.2 * static_cast(sequence)); + setpoint->add_velocity_rad_s(0.01); + setpoint->add_velocity_rad_s(0.02); + setpoint->set_valid_for_us(valid_for_us); + return frame; +} + +arm_teleop::ClientFrame makeHeartbeat(const std::uint64_t sequence) +{ + arm_teleop::ClientFrame frame; + frame.mutable_heartbeat()->set_sequence(sequence); + return frame; +} + +arm_teleop::ClientFrame makeStop( + const arm_teleop::StopReason reason = + arm_teleop::STOP_REASON_OPERATOR_REQUEST) +{ + arm_teleop::ClientFrame frame; + frame.mutable_stop()->set_reason(reason); + frame.mutable_stop()->set_detail("test stop"); + return frame; +} + +class FakeArmTeleopBackend final : public ArmTeleopBackend { +public: + explicit FakeArmTeleopBackend( + arm_teleop::RobotManifest manifest = makeManifest()) + : manifest_(std::move(manifest)) + { + } + + bool available() const noexcept override { return true; } + + arm_teleop::RobotManifest manifest() const override + { + recordThread(); + return manifest_; + } + + bool supportsForceFeedback() const noexcept override { return true; } + + ArmTeleopBackendResult open( + const arm_teleop::OpenSession&) override + { + recordThread(); + std::lock_guard lock(mutex_); + ++open_calls_; + return open_result_; + } + + ArmTeleopBackendResult applySetpoint( + const arm_teleop::JointSetpoint& setpoint, + const std::chrono::steady_clock::time_point deadline) override + { + if (std::chrono::steady_clock::now() >= deadline) { + return ArmTeleopBackendResult::failure( + grpc::StatusCode::DEADLINE_EXCEEDED, + "fake backend received an expired setpoint"); + } + recordThread(); + std::unique_lock lock(mutex_); + apply_entered_ = true; + apply_entered_sequence_ = setpoint.sequence(); + cv_.notify_all(); + cv_.wait(lock, [&]() { return !block_apply_; }); + applied_sequences_.push_back(setpoint.sequence()); + return apply_result_; + } + + ArmTeleopBackendResult stop( + const arm_teleop::StopReason reason, + const std::string&) override + { + recordThread(); + std::lock_guard lock(mutex_); + stop_reasons_.push_back(reason); + return stop_result_; + } + + ArmTeleopBackendSnapshot snapshot() const override + { + recordThread(); + std::lock_guard lock(mutex_); + ArmTeleopBackendSnapshot snapshot; + auto* state = &snapshot.joint_state; + state->set_sample_sequence(++sample_sequence_); + state->add_position_rad(0.1); + state->add_position_rad(0.2); + state->add_velocity_rad_s(0.01); + state->add_velocity_rad_s(0.02); + state->add_effort_nm(1.0); + state->add_effort_nm(2.0); + state->set_position_valid(true); + state->set_velocity_valid(true); + state->set_effort_valid(true); + state->set_effort_source( + arm_teleop::EFFORT_SOURCE_JOINT_SENSOR); + snapshot.safety.set_connected(true); + snapshot.safety.set_powered_on(true); + return snapshot; + } + + void blockApply() + { + std::lock_guard lock(mutex_); + block_apply_ = true; + apply_entered_ = false; + apply_entered_sequence_ = 0; + } + + void releaseApply() + { + { + std::lock_guard lock(mutex_); + block_apply_ = false; + } + cv_.notify_all(); + } + + bool waitForApply( + const std::uint64_t sequence, + const std::chrono::milliseconds timeout) + { + std::unique_lock lock(mutex_); + return cv_.wait_for(lock, timeout, [&]() { + return apply_entered_ && + apply_entered_sequence_ == sequence; + }); + } + + int openCalls() const + { + std::lock_guard lock(mutex_); + return open_calls_; + } + + std::vector appliedSequences() const + { + std::lock_guard lock(mutex_); + return applied_sequences_; + } + + std::vector stopReasons() const + { + std::lock_guard lock(mutex_); + return stop_reasons_; + } + + std::size_t backendThreadCount() const + { + std::lock_guard lock(thread_mutex_); + return backend_threads_.size(); + } + +private: + void recordThread() const + { + std::lock_guard lock(thread_mutex_); + backend_threads_.insert(std::this_thread::get_id()); + } + + arm_teleop::RobotManifest manifest_; + mutable std::mutex mutex_; + mutable std::condition_variable cv_; + bool block_apply_{false}; + bool apply_entered_{false}; + std::uint64_t apply_entered_sequence_{0}; + int open_calls_{0}; + std::vector applied_sequences_; + std::vector stop_reasons_; + mutable std::uint64_t sample_sequence_{0}; + ArmTeleopBackendResult open_result_{ + ArmTeleopBackendResult::ok()}; + ArmTeleopBackendResult apply_result_{ + ArmTeleopBackendResult::ok()}; + ArmTeleopBackendResult stop_result_{ + ArmTeleopBackendResult::ok()}; + + mutable std::mutex thread_mutex_; + mutable std::set backend_threads_; +}; + +class TeleopServerHarness final { +public: + explicit TeleopServerHarness( + std::shared_ptr backend) + : service_(std::move(backend)) + { + socket_path_ = + "/tmp/cmvr_arm_teleop_service_test_" + + std::to_string(static_cast(::getpid())) + "_" + + std::to_string( + g_socket_sequence.fetch_add( + 1, std::memory_order_relaxed)) + + ".sock"; + std::remove(socket_path_.c_str()); + const std::string address = "unix:" + socket_path_; + + grpc::ServerBuilder builder; + builder.AddListeningPort( + address, grpc::InsecureServerCredentials()); + builder.RegisterService(&service_); + server_ = builder.BuildAndStart(); + if (!server_) { + throw std::runtime_error( + "failed to start in-process arm teleoperation server"); + } + channel_ = grpc::CreateChannel( + address, grpc::InsecureChannelCredentials()); + if (!channel_->WaitForConnected( + std::chrono::system_clock::now() + 2s)) { + throw std::runtime_error( + "failed to connect arm teleoperation test channel"); + } + stub_ = arm_teleop::ArmTeleopService::NewStub(channel_); + } + + ~TeleopServerHarness() + { + if (server_) { + server_->Shutdown(); + server_->Wait(); + } + if (!socket_path_.empty()) { + std::remove(socket_path_.c_str()); + } + } + + arm_teleop::ArmTeleopService::Stub& stub() { return *stub_; } + +private: + ArmTeleopServiceImpl service_; + std::unique_ptr server_; + std::shared_ptr channel_; + std::unique_ptr stub_; + std::string socket_path_; +}; + +template +void expectOpeningFrames(Stream& stream) +{ + arm_teleop::ServerFrame response; + ASSERT_TRUE(stream.Read(&response)); + EXPECT_EQ( + response.status().phase(), + arm_teleop::SESSION_PHASE_OPENED); + EXPECT_FALSE(response.status().session_id().empty()); + ASSERT_TRUE(stream.Read(&response)); + EXPECT_EQ( + response.status().phase(), + arm_teleop::SESSION_PHASE_READY); + EXPECT_TRUE(response.safety().connected()); +} + +TEST(ArmTeleopServiceTest, ProductionDisabledBackendRejectsOpen) +{ + TeleopServerHarness harness(makeDisabledArmTeleopBackend()); + grpc::ClientContext context; + context.set_deadline(std::chrono::system_clock::now() + 2s); + auto stream = harness.stub().Teleoperate(&context); + + ASSERT_TRUE(stream->Write(makeOpenFrame(makeManifest()))); + ASSERT_TRUE(stream->WritesDone()); + + arm_teleop::ServerFrame response; + ASSERT_TRUE(stream->Read(&response)); + EXPECT_EQ( + response.status().phase(), + arm_teleop::SESSION_PHASE_REJECTED); + EXPECT_TRUE(response.safety().fault()); + const grpc::Status status = stream->Finish(); + EXPECT_EQ( + status.error_code(), + grpc::StatusCode::FAILED_PRECONDITION); +} + +TEST(ArmTeleopServiceTest, RequiresOpenAsFirstFrame) +{ + auto backend = std::make_shared(); + TeleopServerHarness harness(backend); + grpc::ClientContext context; + context.set_deadline(std::chrono::system_clock::now() + 2s); + auto stream = harness.stub().Teleoperate(&context); + + ASSERT_TRUE(stream->Write(makeSetpoint(1))); + ASSERT_TRUE(stream->WritesDone()); + + arm_teleop::ServerFrame response; + ASSERT_TRUE(stream->Read(&response)); + EXPECT_EQ( + response.status().phase(), + arm_teleop::SESSION_PHASE_REJECTED); + const grpc::Status status = stream->Finish(); + EXPECT_EQ( + status.error_code(), + grpc::StatusCode::INVALID_ARGUMENT); + EXPECT_EQ(backend->openCalls(), 0); +} + +TEST(ArmTeleopServiceTest, RejectsManifestMismatchBeforeBackendOpen) +{ + auto backend = std::make_shared(); + TeleopServerHarness harness(backend); + grpc::ClientContext context; + context.set_deadline(std::chrono::system_clock::now() + 2s); + auto stream = harness.stub().Teleoperate(&context); + + auto mismatched = makeManifest(); + mismatched.set_calibration_sha256(std::string(64, 'c')); + ASSERT_TRUE(stream->Write(makeOpenFrame(mismatched))); + ASSERT_TRUE(stream->WritesDone()); + + arm_teleop::ServerFrame response; + ASSERT_TRUE(stream->Read(&response)); + EXPECT_EQ( + response.status().phase(), + arm_teleop::SESSION_PHASE_REJECTED); + const grpc::Status status = stream->Finish(); + EXPECT_EQ( + status.error_code(), + grpc::StatusCode::FAILED_PRECONDITION); + EXPECT_EQ(backend->openCalls(), 0); +} + +TEST(ArmTeleopServiceTest, RejectsNonIncreasingSequenceAndStops) +{ + auto backend = std::make_shared(); + TeleopServerHarness harness(backend); + grpc::ClientContext context; + context.set_deadline(std::chrono::system_clock::now() + 2s); + auto stream = harness.stub().Teleoperate(&context); + + ASSERT_TRUE(stream->Write(makeOpenFrame(makeManifest()))); + expectOpeningFrames(*stream); + ASSERT_TRUE(stream->Write(makeSetpoint(1))); + + arm_teleop::ServerFrame response; + ASSERT_TRUE(stream->Read(&response)); + EXPECT_EQ( + response.status().phase(), + arm_teleop::SESSION_PHASE_ACTIVE); + EXPECT_EQ(response.status().applied_sequence(), 1U); + + ASSERT_TRUE(stream->Write(makeHeartbeat(1))); + ASSERT_TRUE(stream->WritesDone()); + ASSERT_TRUE(stream->Read(&response)); + EXPECT_EQ( + response.status().phase(), + arm_teleop::SESSION_PHASE_REJECTED); + EXPECT_EQ( + response.status().stop_reason(), + arm_teleop::STOP_REASON_PROTOCOL_ERROR); + + const grpc::Status status = stream->Finish(); + EXPECT_EQ( + status.error_code(), + grpc::StatusCode::INVALID_ARGUMENT); + ASSERT_FALSE(backend->stopReasons().empty()); + EXPECT_EQ( + backend->stopReasons().back(), + arm_teleop::STOP_REASON_PROTOCOL_ERROR); + EXPECT_EQ(backend->backendThreadCount(), 1U); +} + +TEST(ArmTeleopServiceTest, ProtocolErrorCancelsReaderWithoutClientHalfClose) +{ + auto backend = std::make_shared(); + TeleopServerHarness harness(backend); + grpc::ClientContext context; + context.set_deadline(std::chrono::system_clock::now() + 2s); + auto stream = harness.stub().Teleoperate(&context); + + ASSERT_TRUE(stream->Write(makeOpenFrame(makeManifest()))); + expectOpeningFrames(*stream); + ASSERT_TRUE(stream->Write(makeHeartbeat(1))); + + arm_teleop::ServerFrame response; + ASSERT_TRUE(stream->Read(&response)); + EXPECT_EQ( + response.status().phase(), + arm_teleop::SESSION_PHASE_READY); + + // Deliberately keep the client write half open after the duplicate + // sequence. The server must cancel its reader and return promptly. + ASSERT_TRUE(stream->Write(makeHeartbeat(1))); + ASSERT_TRUE(stream->Read(&response)); + EXPECT_EQ( + response.status().phase(), + arm_teleop::SESSION_PHASE_REJECTED); + EXPECT_EQ( + response.status().stop_reason(), + arm_teleop::STOP_REASON_PROTOCOL_ERROR); + const auto status = stream->Finish(); + // TryCancel is required to interrupt the server reader's blocking Read + // when the peer deliberately keeps its write half open. Depending on + // gRPC completion ordering, the client may therefore observe CANCELLED + // after it has already received the explicit protocol-error frame. + EXPECT_TRUE( + status.error_code() == grpc::StatusCode::INVALID_ARGUMENT || + status.error_code() == grpc::StatusCode::CANCELLED) + << status.error_message(); +} + +TEST(ArmTeleopServiceTest, WatchdogExpiresWithoutValidClientActivity) +{ + auto backend = std::make_shared(); + TeleopServerHarness harness(backend); + grpc::ClientContext context; + context.set_deadline(std::chrono::system_clock::now() + 2s); + auto stream = harness.stub().Teleoperate(&context); + + ASSERT_TRUE(stream->Write( + makeOpenFrame(makeManifest(), 20, 2000))); + expectOpeningFrames(*stream); + + arm_teleop::ServerFrame response; + ASSERT_TRUE(stream->Read(&response)); + EXPECT_EQ( + response.status().phase(), + arm_teleop::SESSION_PHASE_WATCHDOG_EXPIRED); + EXPECT_EQ( + response.status().stop_reason(), + arm_teleop::STOP_REASON_WATCHDOG); + + const grpc::Status status = stream->Finish(); + EXPECT_TRUE( + status.error_code() == grpc::StatusCode::DEADLINE_EXCEEDED || + status.error_code() == grpc::StatusCode::CANCELLED) + << status.error_message(); + ASSERT_FALSE(backend->stopReasons().empty()); + EXPECT_EQ( + backend->stopReasons().back(), + arm_teleop::STOP_REASON_WATCHDOG); +} + +TEST(ArmTeleopServiceTest, ValidHeartbeatsRenewControlLease) +{ + auto backend = std::make_shared(); + TeleopServerHarness harness(backend); + grpc::ClientContext context; + context.set_deadline(std::chrono::system_clock::now() + 3s); + auto stream = harness.stub().Teleoperate(&context); + + ASSERT_TRUE(stream->Write( + makeOpenFrame(makeManifest(), 100, 200))); + expectOpeningFrames(*stream); + + arm_teleop::ServerFrame response; + for (std::uint64_t sequence = 1; sequence <= 7; ++sequence) { + std::this_thread::sleep_for(40ms); + ASSERT_TRUE(stream->Write(makeHeartbeat(sequence))); + ASSERT_TRUE(stream->Read(&response)); + EXPECT_EQ( + response.status().phase(), + arm_teleop::SESSION_PHASE_READY); + EXPECT_EQ( + response.status().received_sequence(), + sequence); + EXPECT_GT(response.status().lease_remaining_ms(), 0U); + } + + ASSERT_TRUE(stream->Write(makeStop())); + ASSERT_TRUE(stream->WritesDone()); + ASSERT_TRUE(stream->Read(&response)); + EXPECT_EQ( + response.status().phase(), + arm_teleop::SESSION_PHASE_STOPPED); + EXPECT_TRUE(stream->Finish().ok()); +} + +TEST(ArmTeleopServiceTest, RejectsSecondControllerWhileLeaseIsActive) +{ + auto backend = std::make_shared(); + TeleopServerHarness harness(backend); + + grpc::ClientContext first_context; + first_context.set_deadline( + std::chrono::system_clock::now() + 3s); + auto first_stream = + harness.stub().Teleoperate(&first_context); + ASSERT_TRUE(first_stream->Write( + makeOpenFrame(makeManifest(), 500, 2000))); + expectOpeningFrames(*first_stream); + + grpc::ClientContext second_context; + second_context.set_deadline( + std::chrono::system_clock::now() + 2s); + auto second_stream = + harness.stub().Teleoperate(&second_context); + ASSERT_TRUE(second_stream->Write( + makeOpenFrame(makeManifest(), 500, 2000))); + ASSERT_TRUE(second_stream->WritesDone()); + + arm_teleop::ServerFrame response; + ASSERT_TRUE(second_stream->Read(&response)); + EXPECT_EQ( + response.status().phase(), + arm_teleop::SESSION_PHASE_REJECTED); + const grpc::Status second_status = + second_stream->Finish(); + EXPECT_EQ( + second_status.error_code(), + grpc::StatusCode::RESOURCE_EXHAUSTED); + + ASSERT_TRUE(first_stream->Write(makeStop())); + ASSERT_TRUE(first_stream->WritesDone()); + ASSERT_TRUE(first_stream->Read(&response)); + EXPECT_EQ( + response.status().phase(), + arm_teleop::SESSION_PHASE_STOPPED); + EXPECT_TRUE(first_stream->Finish().ok()); +} + +TEST(ArmTeleopServiceTest, LatestOnlySlotDropsIntermediateSetpoints) +{ + auto backend = std::make_shared(); + TeleopServerHarness harness(backend); + grpc::ClientContext context; + context.set_deadline(std::chrono::system_clock::now() + 3s); + auto stream = harness.stub().Teleoperate(&context); + + ASSERT_TRUE(stream->Write( + makeOpenFrame(makeManifest(), 500, 2000))); + expectOpeningFrames(*stream); + + backend->blockApply(); + ASSERT_TRUE(stream->Write(makeSetpoint(1, 400000))); + ASSERT_TRUE(backend->waitForApply(1, 1s)); + ASSERT_TRUE(stream->Write(makeSetpoint(2, 400000))); + ASSERT_TRUE(stream->Write(makeSetpoint(3, 400000))); + ASSERT_TRUE(stream->Write(makeSetpoint(4, 400000))); + std::this_thread::sleep_for(30ms); + backend->releaseApply(); + + arm_teleop::ServerFrame response; + ASSERT_TRUE(stream->Read(&response)); + EXPECT_EQ(response.status().applied_sequence(), 1U); + ASSERT_TRUE(stream->Read(&response)); + EXPECT_EQ(response.status().applied_sequence(), 4U); + EXPECT_GE(response.status().dropped_setpoints(), 2U); + + ASSERT_TRUE(stream->Write(makeStop())); + ASSERT_TRUE(stream->WritesDone()); + ASSERT_TRUE(stream->Read(&response)); + EXPECT_EQ( + response.status().phase(), + arm_teleop::SESSION_PHASE_STOPPED); + EXPECT_TRUE(stream->Finish().ok()); + + const auto applied = backend->appliedSequences(); + ASSERT_EQ(applied.size(), 2U); + EXPECT_EQ(applied[0], 1U); + EXPECT_EQ(applied[1], 4U); + EXPECT_EQ(backend->backendThreadCount(), 1U); +} + +TEST(ArmTeleopServiceTest, ExpiredSetpointIsNeverDispatched) +{ + auto backend = std::make_shared(); + TeleopServerHarness harness(backend); + grpc::ClientContext context; + context.set_deadline(std::chrono::system_clock::now() + 3s); + auto stream = harness.stub().Teleoperate(&context); + + ASSERT_TRUE(stream->Write( + makeOpenFrame(makeManifest(), 500, 2000))); + expectOpeningFrames(*stream); + + backend->blockApply(); + ASSERT_TRUE(stream->Write(makeSetpoint(1, 400000))); + ASSERT_TRUE(backend->waitForApply(1, 1s)); + ASSERT_TRUE(stream->Write(makeSetpoint(2, 1000))); + std::this_thread::sleep_for(30ms); + backend->releaseApply(); + + arm_teleop::ServerFrame response; + ASSERT_TRUE(stream->Read(&response)); + EXPECT_EQ(response.status().applied_sequence(), 1U); + ASSERT_TRUE(stream->Read(&response)); + EXPECT_EQ( + response.status().phase(), + arm_teleop::SESSION_PHASE_HOLDING); + EXPECT_EQ(response.status().received_sequence(), 2U); + EXPECT_EQ(response.status().applied_sequence(), 1U); + EXPECT_EQ(response.status().rejected_setpoints(), 1U); + + ASSERT_TRUE(stream->Write(makeStop())); + ASSERT_TRUE(stream->WritesDone()); + ASSERT_TRUE(stream->Read(&response)); + EXPECT_EQ( + response.status().phase(), + arm_teleop::SESSION_PHASE_STOPPED); + EXPECT_TRUE(stream->Finish().ok()); + + const auto applied = backend->appliedSequences(); + ASSERT_EQ(applied.size(), 1U); + EXPECT_EQ(applied.front(), 1U); +} + +} // namespace +} // namespace cmvr::service diff --git a/cmvr-es/service/grpc/tests/grpc_robot_arm_teleop_backend_test.cpp b/cmvr-es/service/grpc/tests/grpc_robot_arm_teleop_backend_test.cpp new file mode 100644 index 00000000..7cc01f07 --- /dev/null +++ b/cmvr-es/service/grpc/tests/grpc_robot_arm_teleop_backend_test.cpp @@ -0,0 +1,525 @@ +#include "service/grpc/include/grpc_robot_arm_teleop_backend.h" + +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +namespace cmvr::service { +namespace { + +class FakeRobotArm final : public device::RobotArm { +public: + FakeRobotArm() + { + id_ = "right_arm"; + model_.name = id_; + model_.dof = 2; + model_.joint_names = {"joint_1", "joint_2"}; + model_.joint_limits = { + {-1.0, 1.0, 2.0, 5.0, 10.0}, + {-0.5, 0.5, 3.0, 6.0, 10.0}}; + + state_.connected = true; + state_.powered_on = true; + state_.robot_mode = device::RobotMode::Idle; + state_.safety_mode = device::SafetyMode::Normal; + state_.actual_joint_state.position = {0.0, 0.0}; + state_.actual_joint_state.velocity = {0.0, 0.0}; + state_.actual_joint_state.effort = {1.0, 2.0}; + state_.actual_joint_state.sequence = 7; + state_.actual_joint_state.position_valid = true; + state_.actual_joint_state.velocity_valid = true; + state_.actual_joint_state.effort_valid = true; + } + + std::string typeName() const override { return "FakeRobotArm"; } + device::RobotModel getRobotModel() const override { return model_; } + std::size_t getDof() const override { return model_.dof; } + device::ArmState getRobotState() const override + { + ++get_state_calls; + return state_; + } + device::JointGroupState getJointState() const override + { + return state_.actual_joint_state; + } + device::CartesianPose getTcpPose( + device::FrameType = device::FrameType::Base) const override + { + return {}; + } + device::RobotMode getRobotMode() const override + { + return state_.robot_mode; + } + device::SafetyMode getSafetyMode() const override + { + return state_.safety_mode; + } + device::ControlMode getControlMode() const override + { + return device::ControlMode::Servo; + } + bool supportsTeleopGroupServo() const noexcept override + { + return group_servo_capability; + } + device::JointEffortSource jointEffortSource() const noexcept override + { + return effort_source; + } + + device::Result torqueOn() override + { + ++torque_on_calls; + return device::Result::success(); + } + device::Result torqueOff() override { return device::Result::success(); } + device::Result calibrateZeroQ(const std::string&) override + { + return device::Result::success(); + } + device::Result emergencyStop() override + { + return device::Result::success(); + } + device::Result protectiveStop() override + { + return device::Result::success(); + } + device::Result setSpeedScaling(double) override + { + return device::Result::success(); + } + double getSpeedScaling() const override { return 1.0; } + bool isProtectiveStopped() const override { return false; } + bool isEmergencyStopped() const override { return false; } + bool isFault() const override { return state_.fault; } + + device::Result moveJ( + const device::JointPositionCommand&, + const device::MotionOptions&) override + { + return device::Result::success(); + } + device::Result speedJ( + const device::JointVelocityCommand&, double, double) override + { + return device::Result::success(); + } + device::Result stopJ(double) override + { + return device::Result::success(); + } + device::Result moveL( + const device::CartesianPose&, + const device::MotionOptions&, + device::FrameType = device::FrameType::Base) override + { + return device::Result::success(); + } + device::Result speedL( + const device::CartesianVelocity&, + double, + double, + device::FrameType = device::FrameType::Base) override + { + return device::Result::success(); + } + device::Result stopL(std::optional = std::nullopt) override + { + return device::Result::success(); + } + device::Result stopMotion() override + { + ++stop_motion_calls; + return stop_motion_result; + } + + device::Result startServoMode( + const device::ServoOptions& options) override + { + ++start_servo_calls; + last_servo_period = options.period; + return start_servo_result; + } + device::Result servoJ( + const device::JointPositionCommand& target) override + { + ++servo_j_calls; + last_command = target.position; + if (servo_sleep.count() > 0) { + std::this_thread::sleep_for(servo_sleep); + } + return servo_j_result; + } + device::Result servoL( + const device::CartesianPose&, + device::FrameType = device::FrameType::Base) override + { + return device::Result::success(); + } + device::Result servoSpeedJ( + const device::JointVelocityCommand&) override + { + return device::Result::success(); + } + device::Result servoSpeedL( + const device::CartesianVelocity&, + device::FrameType = device::FrameType::Base) override + { + return device::Result::success(); + } + device::Result stopServoMode() override + { + ++stop_servo_calls; + return stop_servo_result; + } + + device::Result connect(const std::string&, int) override + { + return device::Result::success(); + } + device::Result disconnect() override + { + return device::Result::success(); + } + bool isConnected() const override { return state_.connected; } + device::Result powerOn() override { return torqueOn(); } + device::Result powerOff() override { return torqueOff(); } + device::Result brakeRelease() override + { + return device::Result::success(); + } + device::Result shutdown() override + { + return device::Result::success(); + } + device::Result clearFault() override + { + return device::Result::success(); + } + device::Result unlockProtectiveStop() override + { + return device::Result::success(); + } + device::Result loadProgram(const std::string&) override + { + return device::Result::success(); + } + device::Result playProgram() override + { + return device::Result::success(); + } + device::Result pauseProgram() override + { + return device::Result::success(); + } + device::Result stopProgram() override + { + return device::Result::success(); + } + std::vector ik( + const std::string&, + const std::string&, + const device::CartesianPose&) override + { + return {}; + } + std::shared_ptr kinematicsSolver() const override + { + return nullptr; + } + device::CartesianPose fk( + const std::string&, const std::string&) override + { + return {}; + } + device::CartesianPose fk(bool = true) override { return {}; } + device::CartesianVelocity getSpeedLCommandTwistBase() const override + { + return {}; + } + bool busy() const override { return false; } + + bool group_servo_capability{true}; + device::JointEffortSource effort_source{ + device::JointEffortSource::Unspecified}; + mutable int get_state_calls{0}; + int torque_on_calls{0}; + int start_servo_calls{0}; + int servo_j_calls{0}; + int stop_motion_calls{0}; + int stop_servo_calls{0}; + double last_servo_period{0.0}; + std::vector last_command; + std::chrono::microseconds servo_sleep{0}; + device::Result start_servo_result{device::Result::success()}; + device::Result servo_j_result{device::Result::success()}; + device::Result stop_motion_result{device::Result::success()}; + device::Result stop_servo_result{device::Result::success()}; + device::RobotModel model_; + device::ArmState state_; +}; + +config::ArmTeleopBackendConfig validConfig() +{ + config::ArmTeleopBackendConfig config; + config.set_enable(true); + config.set_device_id("right_arm"); + config.set_model_sha256(std::string(64, 'a')); + config.set_calibration_sha256(std::string(64, 'b')); + config.set_base_frame("base_link"); + config.set_tool_frame("tool_link"); + config.set_servo_period_s(0.01); + config.set_max_apply_duration_us(5000); + config.set_require_powered(true); + config.set_max_initial_position_step_rad(0.2); + config.set_max_position_step_rad(0.015); + return config; +} + +arm_teleop::JointSetpoint setpoint( + const double first, + const double second, + const double first_velocity = 0.0, + const double second_velocity = 0.0) +{ + arm_teleop::JointSetpoint value; + value.set_sequence(1); + value.add_position_rad(first); + value.add_position_rad(second); + value.add_velocity_rad_s(first_velocity); + value.add_velocity_rad_s(second_velocity); + value.set_valid_for_us(5000); + return value; +} + +arm_teleop::OpenSession openRequest() +{ + arm_teleop::OpenSession request; + request.set_requested_command_rate_hz(100); + return request; +} + +std::chrono::steady_clock::time_point liveDeadline() +{ + return std::chrono::steady_clock::now() + + std::chrono::seconds(1); +} + +TEST(RobotArmTeleopBackendTest, RejectsArmWithoutExplicitGroupServoCapability) +{ + auto arm = std::make_shared(); + arm->group_servo_capability = false; + const auto backend = + makeRobotArmTeleopBackend(arm, validConfig()); + + EXPECT_FALSE(backend->available()); + EXPECT_NE( + backend->unavailableReason().find("capability"), + std::string::npos); + EXPECT_EQ(arm->start_servo_calls, 0); +} + +TEST(RobotArmTeleopBackendTest, OpenStartsServoButNeverPowersArm) +{ + auto arm = std::make_shared(); + const auto backend = + makeRobotArmTeleopBackend(arm, validConfig()); + + ASSERT_TRUE(backend->available()) + << backend->unavailableReason(); + EXPECT_TRUE(backend->open(openRequest()).success); + EXPECT_EQ(arm->start_servo_calls, 1); + EXPECT_DOUBLE_EQ(arm->last_servo_period, 0.01); + EXPECT_EQ(arm->torque_on_calls, 0); +} + +TEST(RobotArmTeleopBackendTest, AppliesSetpointAndAlwaysRunsBothStopPaths) +{ + auto arm = std::make_shared(); + const auto backend = + makeRobotArmTeleopBackend(arm, validConfig()); + ASSERT_TRUE(backend->open(openRequest()).success); + + ASSERT_TRUE( + backend->applySetpoint( + setpoint(0.1, 0.1), liveDeadline()).success); + EXPECT_EQ(arm->servo_j_calls, 1); + EXPECT_EQ(arm->last_command, (std::vector{0.1, 0.1})); + + arm->stop_motion_result = device::Result::failure( + device::ArmErrorCode::CommandFailed, "motion stop failed"); + const auto result = backend->stop( + arm_teleop::STOP_REASON_OPERATOR_REQUEST, "test"); + EXPECT_FALSE(result.success); + EXPECT_EQ(arm->stop_motion_calls, 1); + EXPECT_EQ(arm->stop_servo_calls, 1); +} + +TEST(RobotArmTeleopBackendTest, RejectsInitialAndContinuousPositionSteps) +{ + auto arm = std::make_shared(); + const auto backend = + makeRobotArmTeleopBackend(arm, validConfig()); + ASSERT_TRUE(backend->open(openRequest()).success); + + EXPECT_EQ( + backend->applySetpoint( + setpoint(0.21, 0.0), liveDeadline()).status_code, + grpc::StatusCode::OUT_OF_RANGE); + ASSERT_TRUE( + backend->applySetpoint( + setpoint(0.1, 0.1), liveDeadline()).success); + EXPECT_EQ( + backend->applySetpoint( + setpoint(0.12, 0.1), liveDeadline()).status_code, + grpc::StatusCode::OUT_OF_RANGE); + EXPECT_EQ(arm->servo_j_calls, 1); +} + +TEST(RobotArmTeleopBackendTest, RejectsJointPositionAndVelocityLimits) +{ + auto arm = std::make_shared(); + const auto backend = + makeRobotArmTeleopBackend(arm, validConfig()); + ASSERT_TRUE(backend->open(openRequest()).success); + + EXPECT_EQ( + backend->applySetpoint( + setpoint(1.01, 0.0), liveDeadline()).status_code, + grpc::StatusCode::OUT_OF_RANGE); + EXPECT_EQ( + backend->applySetpoint( + setpoint(0.0, 0.0, 2.01, 0.0), liveDeadline()) + .status_code, + grpc::StatusCode::OUT_OF_RANGE); + EXPECT_EQ(arm->servo_j_calls, 0); +} + +TEST(RobotArmTeleopBackendTest, DetectsServoApplyTimeout) +{ + auto arm = std::make_shared(); + auto config = validConfig(); + config.set_max_apply_duration_us(500); + const auto backend = + makeRobotArmTeleopBackend(arm, config); + ASSERT_TRUE(backend->open(openRequest()).success); + arm->servo_sleep = std::chrono::microseconds(1500); + + const auto result = + backend->applySetpoint( + setpoint(0.1, 0.1), liveDeadline()); + EXPECT_FALSE(result.success); + EXPECT_EQ( + result.status_code, + grpc::StatusCode::DEADLINE_EXCEEDED); + EXPECT_EQ(arm->servo_j_calls, 1); +} + +TEST(RobotArmTeleopBackendTest, RejectsInsufficientDeadlineBudgetBeforeDispatch) +{ + auto arm = std::make_shared(); + const auto backend = + makeRobotArmTeleopBackend(arm, validConfig()); + ASSERT_TRUE(backend->open(openRequest()).success); + + const auto result = backend->applySetpoint( + setpoint(0.1, 0.1), + std::chrono::steady_clock::now() + + std::chrono::microseconds(100)); + EXPECT_FALSE(result.success); + EXPECT_EQ( + result.status_code, + grpc::StatusCode::DEADLINE_EXCEEDED); + EXPECT_EQ(arm->servo_j_calls, 0); +} + +TEST(RobotArmTeleopBackendTest, ExpiredDeadlineNeverDispatchesServoCommand) +{ + auto arm = std::make_shared(); + const auto backend = + makeRobotArmTeleopBackend(arm, validConfig()); + ASSERT_TRUE(backend->open(openRequest()).success); + + const auto result = backend->applySetpoint( + setpoint(0.1, 0.1), + std::chrono::steady_clock::now()); + EXPECT_FALSE(result.success); + EXPECT_EQ( + result.status_code, + grpc::StatusCode::DEADLINE_EXCEEDED); + EXPECT_EQ(arm->servo_j_calls, 0); +} + +TEST(RobotArmTeleopBackendTest, RejectsUnsupportedRateAndEarlyRedispatch) +{ + auto arm = std::make_shared(); + const auto backend = + makeRobotArmTeleopBackend(arm, validConfig()); + auto too_fast = openRequest(); + too_fast.set_requested_command_rate_hz(101); + EXPECT_EQ( + backend->open(too_fast).status_code, + grpc::StatusCode::FAILED_PRECONDITION); + EXPECT_EQ(arm->start_servo_calls, 0); + + ASSERT_TRUE(backend->open(openRequest()).success); + ASSERT_TRUE( + backend->applySetpoint( + setpoint(0.1, 0.1), liveDeadline()).success); + EXPECT_EQ( + backend->applySetpoint( + setpoint(0.105, 0.105), liveDeadline()).status_code, + grpc::StatusCode::RESOURCE_EXHAUSTED); + EXPECT_EQ(arm->servo_j_calls, 1); +} + +TEST(RobotArmTeleopBackendTest, SnapshotUsesCacheAndHidesUnverifiedEffort) +{ + auto arm = std::make_shared(); + const auto backend = + makeRobotArmTeleopBackend(arm, validConfig()); + ASSERT_TRUE(backend->open(openRequest()).success); + const int calls_after_open = arm->get_state_calls; + + const auto first = backend->snapshot(); + const auto second = backend->snapshot(); + EXPECT_EQ(arm->get_state_calls, calls_after_open); + EXPECT_TRUE(first.joint_state.position_valid()); + EXPECT_TRUE(first.joint_state.velocity_valid()); + EXPECT_FALSE(first.joint_state.effort_valid()); + EXPECT_EQ(first.joint_state.effort_nm_size(), 0); + EXPECT_EQ( + second.joint_state.effort_source(), + arm_teleop::EFFORT_SOURCE_UNSPECIFIED); +} + +TEST(RobotArmTeleopBackendTest, PublishesEffortOnlyWithExplicitSource) +{ + auto arm = std::make_shared(); + arm->effort_source = device::JointEffortSource::JointSensor; + const auto backend = + makeRobotArmTeleopBackend(arm, validConfig()); + ASSERT_TRUE(backend->open(openRequest()).success); + + const auto snapshot = backend->snapshot(); + EXPECT_TRUE(backend->supportsForceFeedback()); + EXPECT_TRUE(snapshot.joint_state.effort_valid()); + EXPECT_EQ(snapshot.joint_state.effort_nm_size(), 2); + EXPECT_EQ( + snapshot.joint_state.effort_source(), + arm_teleop::EFFORT_SOURCE_JOINT_SENSOR); +} + +} // namespace +} // namespace cmvr::service diff --git a/cmvr-es/task/grpc_server_task/include/grpc_server_task.h b/cmvr-es/task/grpc_server_task/include/grpc_server_task.h index 33bd0ebe..fb3efbcc 100644 --- a/cmvr-es/task/grpc_server_task/include/grpc_server_task.h +++ b/cmvr-es/task/grpc_server_task/include/grpc_server_task.h @@ -11,6 +11,10 @@ #include "cmvr/config/grpc_server_config/grpc_server_config.pb.h" #include "task/task.h" +namespace cmvr::service { +class ArmTeleopBackend; +} + namespace cmvr::task { class GrpcServerTask final : public Task { @@ -55,6 +59,9 @@ private: std::unique_ptr dexhand_service_; std::unique_ptr biohand_service_; std::unique_ptr arm_service_; + std::unique_ptr arm_teleop_service_; + std::shared_ptr + arm_teleop_backend_; std::unique_ptr motor_service_; std::unique_ptr agv_service_; std::unique_ptr hlc_service_; diff --git a/cmvr-es/task/grpc_server_task/src/grpc_server_task.cpp b/cmvr-es/task/grpc_server_task/src/grpc_server_task.cpp index 962d2434..46a0bf61 100644 --- a/cmvr-es/task/grpc_server_task/src/grpc_server_task.cpp +++ b/cmvr-es/task/grpc_server_task/src/grpc_server_task.cpp @@ -8,8 +8,12 @@ #include "cmvr/config/task_manager_config/task_manager_config.pb.h" #include "common/base/logging/logger.h" #include "common/config/config_files.h" +#include "devices/arm/robot_arm.h" +#include "manager/device_manager/include/device_manager.h" #include "service/grpc/include/grpc_agv_service.h" #include "service/grpc/include/grpc_arm_service.h" +#include "service/grpc/include/grpc_arm_teleop_service.h" +#include "service/grpc/include/grpc_robot_arm_teleop_backend.h" #include "service/grpc/include/grpc_camera_service.h" #include "service/grpc/include/grpc_dexhand_service.h" #include "service/grpc/include/grpc_head_service.h" @@ -106,6 +110,10 @@ bool GrpcServerTask::start() dexhand_service_ = std::make_unique(); biohand_service_ = std::make_unique(); arm_service_ = std::make_unique(); + arm_teleop_service_ = + std::make_unique( + arm_teleop_backend_ ? arm_teleop_backend_ + : service::makeDisabledArmTeleopBackend()); motor_service_ = std::make_unique(); agv_service_ = std::make_unique(); hlc_service_ = std::make_unique(); @@ -119,6 +127,7 @@ bool GrpcServerTask::start() builder.RegisterService(dexhand_service_.get()); builder.RegisterService(biohand_service_.get()); builder.RegisterService(arm_service_.get()); + builder.RegisterService(arm_teleop_service_.get()); builder.RegisterService(motor_service_.get()); builder.RegisterService(agv_service_.get()); builder.RegisterService(hlc_service_.get()); @@ -152,6 +161,53 @@ bool GrpcServerTask::init() state_ = TaskState::FAILED; return false; } + + arm_teleop_backend_ = service::makeDisabledArmTeleopBackend(); + if (cfg_.has_arm_teleop_backend() && + cfg_.arm_teleop_backend().enable()) { + const auto& backend_config = cfg_.arm_teleop_backend(); + if (backend_config.device_id().empty()) { + last_error_ = + "enabled ArmTeleop backend requires device_id"; + state_ = TaskState::FAILED; + return false; + } + auto arm = + device::DeviceManager::getInstance() + .getDevice( + backend_config.device_id()); + if (!arm) { + last_error_ = + "ArmTeleop RobotArm device was not found: " + + backend_config.device_id(); + state_ = TaskState::FAILED; + return false; + } + auto backend = + service::makeRobotArmTeleopBackend( + std::move(arm), backend_config); + if (!backend->available()) { + last_error_ = + "ArmTeleop backend rejected configuration: " + + backend->unavailableReason(); + state_ = TaskState::FAILED; + return false; + } + // RobotArmTeleopBackend builds robot_id directly from device_id. Keep + // this assertion at the registration boundary so the process-wide + // control lease resource and unary ArmService device ID cannot drift. + if (backend->manifest().robot_id() != + backend_config.device_id()) { + last_error_ = + "ArmTeleop lease resource must equal device_id"; + state_ = TaskState::FAILED; + return false; + } + arm_teleop_backend_ = std::move(backend); + CMVR_LOG(INFO) + << "[GrpcServerTask] ArmTeleop RobotArm backend enabled for " + << backend_config.device_id(); + } last_error_.clear(); state_ = TaskState::IDLE; return true; @@ -258,6 +314,7 @@ void GrpcServerTask::clearServices() hlc_service_.reset(); agv_service_.reset(); motor_service_.reset(); + arm_teleop_service_.reset(); arm_service_.reset(); biohand_service_.reset(); dexhand_service_.reset(); diff --git a/cmvr-es/task/quic_edge_task/CMakeLists.txt b/cmvr-es/task/quic_edge_task/CMakeLists.txt index 009388df..48d66107 100644 --- a/cmvr-es/task/quic_edge_task/CMakeLists.txt +++ b/cmvr-es/task/quic_edge_task/CMakeLists.txt @@ -21,7 +21,14 @@ if(BUILD_TESTING) get_property(_quic_task_test_library_dirs DIRECTORY PROPERTY LINK_DIRECTORIES) list(PREPEND _quic_task_test_library_dirs "${CMAKE_BINARY_DIR}/cmvr_compiler_runtime") list(JOIN _quic_task_test_library_dirs ":" _quic_task_test_library_path) + set(_quic_task_test_environment + "LD_LIBRARY_PATH=${_quic_task_test_library_path}") + if(CMVR_TEST_SYSTEM_LIBSTDCXX) + list(APPEND _quic_task_test_environment + "LD_PRELOAD=${CMVR_TEST_SYSTEM_LIBSTDCXX}") + endif() set_tests_properties(quic_edge_task_test PROPERTIES - ENVIRONMENT "LD_LIBRARY_PATH=${_quic_task_test_library_path}") + TIMEOUT 10 + ENVIRONMENT "${_quic_task_test_environment}") endif() endif() diff --git a/cmvr-es/task/ume_teleop_task/CMakeLists.txt b/cmvr-es/task/ume_teleop_task/CMakeLists.txt new file mode 100644 index 00000000..d624679f --- /dev/null +++ b/cmvr-es/task/ume_teleop_task/CMakeLists.txt @@ -0,0 +1,41 @@ +find_package(Threads REQUIRED) + +add_library(ume_teleop_task STATIC + src/ume_teleop_task.cpp +) +target_compile_features(ume_teleop_task PUBLIC cxx_std_17) +target_include_directories(ume_teleop_task PUBLIC ${PROJECT_SOURCE_DIR}/cmvr-es) +target_link_libraries(ume_teleop_task + PUBLIC + cmvr_es::task + cmvr_es::arm_teleop_client + cmvr_es::proto + PRIVATE + cmvr_es::logging + Threads::Threads +) + +add_library(cmvr_es::ume_teleop_task ALIAS ume_teleop_task) +install(TARGETS ume_teleop_task ARCHIVE DESTINATION lib) + +if(BUILD_TESTING) + add_executable(ume_teleop_task_test + tests/ume_teleop_task_test.cpp + ) + target_compile_features(ume_teleop_task_test PRIVATE cxx_std_17) + target_link_libraries(ume_teleop_task_test + PRIVATE + cmvr_es::ume_teleop_task + Threads::Threads + ) + add_test(NAME ume_teleop_task_test COMMAND ume_teleop_task_test) + set(_ume_teleop_task_test_environment + "LD_LIBRARY_PATH=${CMVR_TEST_EXTERNAL_LIBRARY_PATH}") + if(CMVR_TEST_SYSTEM_LIBSTDCXX) + list(APPEND _ume_teleop_task_test_environment + "LD_PRELOAD=${CMVR_TEST_SYSTEM_LIBSTDCXX}") + endif() + set_tests_properties(ume_teleop_task_test PROPERTIES + TIMEOUT 10 + ENVIRONMENT "${_ume_teleop_task_test_environment}") +endif() diff --git a/cmvr-es/task/ume_teleop_task/include/ume_teleop_task.h b/cmvr-es/task/ume_teleop_task/include/ume_teleop_task.h new file mode 100644 index 00000000..0c3da0ae --- /dev/null +++ b/cmvr-es/task/ume_teleop_task/include/ume_teleop_task.h @@ -0,0 +1,99 @@ +#ifndef CMVR_ES_UME_TELEOP_TASK_H +#define CMVR_ES_UME_TELEOP_TASK_H + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "cmvr/config/ume_teleop_config/ume_teleop_config.pb.h" +#include "service/arm_teleop_client/include/grpc_arm_teleop_client.h" +#include "task/task.h" + +namespace cmvr::task { + +class UmeTeleopTask final : public Task { +public: + explicit UmeTeleopTask( + const config::UmeTeleopConfig& config, + std::shared_ptr client = {}); + ~UmeTeleopTask() override; + + const std::string& id() const override { return id_; } + TaskRunMode runMode() const override { return TaskRunMode::BLOCKING_SERVICE; } + bool init() override; + bool start() override; + bool step(double dt) override; + void stop() override; + + TaskState state() const override; + bool isBusy() const override; + bool isFinished() const override; + bool isFailed() const override; + std::string stateString() const override; + std::string detailStatusString() const override; + + // Thread-safe, capacity-one command mailbox. The caller provides only + // already-computed joint values; sequence is assigned by the sender loop. + // A newer command replaces an unsent older command. Submission is rejected + // unless the current session is ready, and pending values are discarded + // across disconnect/reconnect so motion cannot resume from stale intent. + bool submitSetpoint( + const api::armteleop::v1::JointSetpoint& setpoint); + +private: + using Clock = std::chrono::steady_clock; + + struct PendingSetpoint { + api::armteleop::v1::JointSetpoint value; + Clock::time_point submitted; + }; + + bool validateConfig(std::string& error) const; + void run(); + void runSender(); + void handleServerFrame( + const api::armteleop::v1::ServerFrame& frame); + + config::UmeTeleopConfig config_; + std::string id_; + std::shared_ptr client_; + + mutable std::mutex mutex_; + std::condition_variable stop_condition_; + std::thread worker_; + std::thread sender_; + std::atomic stop_requested_{false}; + TaskState state_{TaskState::UNINITIALIZED}; + std::string last_error_; + std::string session_id_; + std::string server_detail_; + api::armteleop::v1::SessionPhase server_phase_{ + api::armteleop::v1::SESSION_PHASE_UNSPECIFIED}; + std::uint64_t connection_attempts_{0}; + + bool receiver_session_active_{false}; + bool session_ready_{false}; + std::uint64_t client_session_generation_{0}; + std::uint32_t negotiated_watchdog_ms_{0}; + + std::optional pending_setpoint_; + std::uint64_t mailbox_replacements_{0}; + std::uint64_t stale_setpoints_dropped_{0}; + std::uint64_t heartbeats_sent_{0}; + std::uint64_t setpoints_sent_{0}; + + bool stop_write_attempted_{false}; + bool stop_write_succeeded_{false}; +}; + +void registerUmeTeleopTaskFactory(); + +} // namespace cmvr::task + +#endif // CMVR_ES_UME_TELEOP_TASK_H diff --git a/cmvr-es/task/ume_teleop_task/src/ume_teleop_task.cpp b/cmvr-es/task/ume_teleop_task/src/ume_teleop_task.cpp new file mode 100644 index 00000000..314102af --- /dev/null +++ b/cmvr-es/task/ume_teleop_task/src/ume_teleop_task.cpp @@ -0,0 +1,651 @@ +#include "task/ume_teleop_task/include/ume_teleop_task.h" + +#include +#include +#include +#include +#include + +#include +#include + +#include "cmvr/config/task_manager_config/task_manager_config.pb.h" +#include "common/base/logging/logger.h" +#include "common/config/config_files.h" +#include "task/task_factory.h" + +namespace cmvr::task { +namespace { + +constexpr auto kStopWriteGrace = std::chrono::milliseconds(50); +constexpr auto kStopAckGrace = std::chrono::milliseconds(20); + +std::shared_ptr createUmeTeleopTask( + const config::TaskConfigEntry& entry) +{ + if (entry.id().empty() || entry.config_file().empty()) { + CMVR_LOG(ERROR) << "[UmeTeleopTask] Task id or config_file is empty"; + return nullptr; + } + + config::UmeTeleopRootConfig root; + if (!ConfigHelper::loadConfigFile(entry.config_file(), root)) { + CMVR_LOG(ERROR) << "[UmeTeleopTask] Failed to load config: " + << entry.config_file(); + return nullptr; + } + const auto& config = root.ume_teleop(); + if (config.id().empty() || config.id() != entry.id()) { + CMVR_LOG(ERROR) << "[UmeTeleopTask] Task ID mismatch: manager=" + << entry.id() << ", config=" << config.id(); + return nullptr; + } + return std::make_shared(config); +} + +std::string grpcStatusDetail(const grpc::Status& status) +{ + std::ostringstream output; + output << "gRPC code=" << static_cast(status.error_code()); + if (!status.error_message().empty()) { + output << " message=" << status.error_message(); + } + return output.str(); +} + +} // namespace + +UmeTeleopTask::UmeTeleopTask( + const config::UmeTeleopConfig& config, + std::shared_ptr client) + : config_(config), id_(config.id()), client_(std::move(client)) +{ +} + +UmeTeleopTask::~UmeTeleopTask() +{ + stop(); +} + +bool UmeTeleopTask::init() +{ + std::lock_guard lock(mutex_); + if (state_ == TaskState::IDLE) { + return true; + } + if (worker_.joinable() || sender_.joinable()) { + last_error_ = "cannot initialize while worker is running"; + state_ = TaskState::FAILED; + return false; + } + + std::string error; + if (!validateConfig(error)) { + last_error_ = std::move(error); + state_ = TaskState::FAILED; + return false; + } + + if (!client_) { + auto channel = grpc::CreateChannel( + config_.server_address(), + grpc::InsecureChannelCredentials()); + if (!channel) { + last_error_ = "failed to create gRPC channel"; + state_ = TaskState::FAILED; + return false; + } + client_ = std::make_shared( + std::move(channel)); + } + + stop_requested_ = false; + last_error_.clear(); + session_id_.clear(); + server_detail_.clear(); + server_phase_ = api::armteleop::v1::SESSION_PHASE_UNSPECIFIED; + connection_attempts_ = 0; + receiver_session_active_ = false; + session_ready_ = false; + client_session_generation_ = 0; + negotiated_watchdog_ms_ = 0; + pending_setpoint_.reset(); + mailbox_replacements_ = 0; + stale_setpoints_dropped_ = 0; + heartbeats_sent_ = 0; + setpoints_sent_ = 0; + stop_write_attempted_ = false; + stop_write_succeeded_ = false; + state_ = TaskState::IDLE; + return true; +} + +bool UmeTeleopTask::start() +{ + std::unique_lock lock(mutex_); + if (state_ == TaskState::RUNNING) { + return true; + } + if ((state_ != TaskState::IDLE && state_ != TaskState::STOPPED) || + !client_ || worker_.joinable() || sender_.joinable()) { + last_error_ = "UME teleop task is not initialized"; + state_ = TaskState::FAILED; + return false; + } + + stop_requested_ = false; + session_ready_ = false; + client_session_generation_ = 0; + negotiated_watchdog_ms_ = 0; + pending_setpoint_.reset(); + stop_write_attempted_ = false; + stop_write_succeeded_ = false; + try { + worker_ = std::thread(&UmeTeleopTask::run, this); + sender_ = std::thread(&UmeTeleopTask::runSender, this); + } catch (const std::exception& error) { + last_error_ = std::string("failed to start worker: ") + error.what(); + stop_requested_ = true; + stop_condition_.notify_all(); + auto client = client_; + std::thread worker; + std::thread sender; + if (worker_.joinable()) { + worker = std::move(worker_); + } + if (sender_.joinable()) { + sender = std::move(sender_); + } + lock.unlock(); + client->tryCancel(); + if (sender.joinable()) { + sender.join(); + } + if (worker.joinable()) { + worker.join(); + } + lock.lock(); + state_ = TaskState::FAILED; + return false; + } catch (...) { + last_error_ = "failed to start worker"; + stop_requested_ = true; + stop_condition_.notify_all(); + auto client = client_; + std::thread worker; + std::thread sender; + if (worker_.joinable()) { + worker = std::move(worker_); + } + if (sender_.joinable()) { + sender = std::move(sender_); + } + lock.unlock(); + client->tryCancel(); + if (sender.joinable()) { + sender.join(); + } + if (worker.joinable()) { + worker.join(); + } + lock.lock(); + state_ = TaskState::FAILED; + return false; + } + + state_ = TaskState::RUNNING; + return true; +} + +bool UmeTeleopTask::step(const double dt) +{ + (void)dt; + return !isFailed(); +} + +void UmeTeleopTask::stop() +{ + std::shared_ptr client; + std::thread worker; + std::thread sender; + { + std::unique_lock lock(mutex_); + stop_requested_ = true; + stop_condition_.notify_all(); + client = client_; + + // The sender is the only normal writer. Give it a bounded opportunity + // to put StopSession on the stream before cancellation interrupts a + // blocked Write or Read. + if (sender_.joinable()) { + stop_condition_.wait_for( + lock, + kStopWriteGrace, + [this] { return stop_write_attempted_; }); + if (stop_write_succeeded_ && receiver_session_active_) { + stop_condition_.wait_for( + lock, + kStopAckGrace, + [this] { return !receiver_session_active_; }); + } + sender = std::move(sender_); + } + if (worker_.joinable()) { + worker = std::move(worker_); + } + } + + // Do not hold the task mutex while cancelling or joining: the receive + // callback and the worker exit path both update task status under it. + if (client) { + client->tryCancel(); + } + if (sender.joinable()) { + sender.join(); + } + if (worker.joinable()) { + worker.join(); + } + + std::lock_guard lock(mutex_); + if (state_ != TaskState::FAILED) { + state_ = TaskState::STOPPED; + } +} + +TaskState UmeTeleopTask::state() const +{ + std::lock_guard lock(mutex_); + return state_; +} + +bool UmeTeleopTask::isBusy() const +{ + return state() == TaskState::RUNNING; +} + +bool UmeTeleopTask::isFinished() const +{ + return state() == TaskState::STOPPED; +} + +bool UmeTeleopTask::isFailed() const +{ + return state() == TaskState::FAILED; +} + +std::string UmeTeleopTask::stateString() const +{ + return taskStateToString(state()); +} + +std::string UmeTeleopTask::detailStatusString() const +{ + std::lock_guard lock(mutex_); + std::ostringstream output; + output << taskStateToString(state_) + << " target=" << config_.server_address() + << " attempts=" << connection_attempts_ + << " phase=" + << api::armteleop::v1::SessionPhase_Name(server_phase_) + << " watchdog_ms=" << negotiated_watchdog_ms_ + << " heartbeats=" << heartbeats_sent_ + << " setpoints=" << setpoints_sent_ + << " mailbox_replacements=" << mailbox_replacements_ + << " stale_dropped=" << stale_setpoints_dropped_; + if (!session_id_.empty()) { + output << " session=" << session_id_; + } + if (!server_detail_.empty()) { + output << " server_detail=" << server_detail_; + } + if (!last_error_.empty()) { + output << " error=" << last_error_; + } + return output.str(); +} + +bool UmeTeleopTask::submitSetpoint( + const api::armteleop::v1::JointSetpoint& setpoint) +{ + if (setpoint.valid_for_us() == 0U) { + return false; + } + + std::lock_guard lock(mutex_); + if (state_ != TaskState::RUNNING || + stop_requested_.load(std::memory_order_acquire) || + !session_ready_ || + client_session_generation_ == 0U) { + return false; + } + if (pending_setpoint_.has_value()) { + ++mailbox_replacements_; + } + + PendingSetpoint pending; + pending.value = setpoint; + // The Task owns the only session sequence generator. + pending.value.set_sequence(0); + pending.submitted = Clock::now(); + pending_setpoint_ = std::move(pending); + stop_condition_.notify_all(); + return true; +} + +bool UmeTeleopTask::validateConfig(std::string& error) const +{ + if (id_.empty()) { + error = "UME teleop task id is empty"; + return false; + } + if (config_.server_address().empty()) { + error = "UME teleop server_address is empty"; + return false; + } + if (!config_.allow_insecure()) { + error = + "M6 requires explicit allow_insecure=true; TLS is not configured"; + return false; + } + + const auto& open = config_.open_session(); + if (open.protocol_major() == 0U || + open.client_instance_id().empty() || + open.expected_robot().robot_id().empty() || + open.expected_robot().joint_names().empty() || + open.requested_command_rate_hz() == 0U || + open.requested_state_rate_hz() == 0U || + open.watchdog_timeout_ms() == 0U || + open.requested_lease_ms() == 0U) { + error = "UME teleop OpenSession configuration is incomplete"; + return false; + } + + const auto& reconnect = config_.reconnect(); + if (reconnect.initial_delay_ms() == 0U || + reconnect.maximum_delay_ms() < reconnect.initial_delay_ms() || + !std::isfinite(reconnect.multiplier()) || + reconnect.multiplier() < 1.0) { + error = "UME teleop reconnect configuration is invalid"; + return false; + } + return true; +} + +void UmeTeleopTask::run() +{ + auto backoff = + std::chrono::milliseconds(config_.reconnect().initial_delay_ms()); + const auto maximum_backoff = + std::chrono::milliseconds(config_.reconnect().maximum_delay_ms()); + + while (true) { + { + std::lock_guard lock(mutex_); + if (stop_requested_) { + break; + } + ++connection_attempts_; + receiver_session_active_ = true; + session_ready_ = false; + client_session_generation_ = 0; + negotiated_watchdog_ms_ = 0; + // A command produced for an old or disconnected session must + // never become the first motion command after reconnect. + pending_setpoint_.reset(); + stop_condition_.notify_all(); + } + + const grpc::Status status = client_->runSession( + config_.open_session(), + [this](const api::armteleop::v1::ServerFrame& frame) { + handleServerFrame(frame); + }, + [this] { + return stop_requested_.load(std::memory_order_acquire); + }); + + std::unique_lock lock(mutex_); + receiver_session_active_ = false; + session_ready_ = false; + client_session_generation_ = 0; + negotiated_watchdog_ms_ = 0; + pending_setpoint_.reset(); + stop_condition_.notify_all(); + if (stop_requested_) { + break; + } + + last_error_ = status.ok() + ? "arm teleop peer closed the session" + : grpcStatusDetail(status); + if (stop_condition_.wait_for( + lock, + backoff, + [this] { + return stop_requested_.load(std::memory_order_acquire); + })) { + break; + } + + const double multiplied = + static_cast(backoff.count()) * + config_.reconnect().multiplier(); + const auto next_count = static_cast( + std::min(multiplied, static_cast(maximum_backoff.count()))); + backoff = std::chrono::milliseconds(std::max(1, next_count)); + } +} + +void UmeTeleopTask::runSender() +{ + const auto command_period = std::chrono::microseconds( + std::max( + 1U, + 1000000ULL / + static_cast( + config_.open_session().requested_command_rate_hz()))); + + std::uint64_t observed_generation = 0; + std::uint64_t next_sequence = 0; + auto next_command_time = Clock::now(); + auto next_heartbeat_time = Clock::time_point::max(); + + for (;;) { + std::optional command; + bool heartbeat = false; + bool stop = false; + std::uint64_t generation = 0; + std::uint64_t sequence = 0; + std::chrono::microseconds heartbeat_period{0}; + + { + std::unique_lock lock(mutex_); + for (;;) { + if (stop_requested_.load(std::memory_order_acquire)) { + stop = true; + generation = client_session_generation_; + break; + } + + if (!session_ready_ || + client_session_generation_ == 0 || + negotiated_watchdog_ms_ == 0U) { + stop_condition_.wait(lock, [this] { + return stop_requested_.load( + std::memory_order_acquire) || + (session_ready_ && + client_session_generation_ != 0 && + negotiated_watchdog_ms_ != 0U); + }); + continue; + } + + if (observed_generation != client_session_generation_) { + observed_generation = client_session_generation_; + next_sequence = 0; + next_command_time = Clock::now(); + heartbeat_period = std::chrono::microseconds( + std::max( + 1000U, + static_cast( + negotiated_watchdog_ms_) * + 1000ULL / 3ULL)); + next_heartbeat_time = Clock::now() + heartbeat_period; + } else { + heartbeat_period = std::chrono::microseconds( + std::max( + 1000U, + static_cast( + negotiated_watchdog_ms_) * + 1000ULL / 3ULL)); + } + + const auto now = Clock::now(); + if (pending_setpoint_.has_value()) { + const auto queued_age = + std::chrono::duration_cast( + now - pending_setpoint_->submitted); + if (queued_age.count() >= + pending_setpoint_->value.valid_for_us()) { + pending_setpoint_.reset(); + ++stale_setpoints_dropped_; + } + } + + if (pending_setpoint_.has_value() && + now >= next_command_time) { + command = std::move(pending_setpoint_); + pending_setpoint_.reset(); + generation = client_session_generation_; + sequence = ++next_sequence; + break; + } + if (now >= next_heartbeat_time) { + heartbeat = true; + generation = client_session_generation_; + sequence = ++next_sequence; + break; + } + + auto wake_time = next_heartbeat_time; + if (pending_setpoint_.has_value()) { + wake_time = std::min(wake_time, next_command_time); + } + stop_condition_.wait_until(lock, wake_time); + } + } + + if (stop) { + api::armteleop::v1::StopSession stop_frame; + stop_frame.set_reason( + api::armteleop::v1::STOP_REASON_CLIENT_SHUTDOWN); + stop_frame.set_detail("UME teleoperation task stopped"); + const bool sent = client_->sendStop(stop_frame, generation); + { + std::lock_guard lock(mutex_); + stop_write_attempted_ = true; + stop_write_succeeded_ = sent; + } + stop_condition_.notify_all(); + return; + } + + bool sent = false; + if (command.has_value()) { + const auto queued_age = + std::chrono::duration_cast( + Clock::now() - command->submitted); + const auto original_validity = + static_cast( + command->value.valid_for_us()); + if (queued_age.count() >= 0 && + static_cast(queued_age.count()) < + original_validity) { + command->value.set_sequence(sequence); + command->value.set_valid_for_us( + static_cast( + original_validity - + static_cast(queued_age.count()))); + sent = client_->sendSetpoint( + command->value, generation); + } else { + std::lock_guard lock(mutex_); + ++stale_setpoints_dropped_; + continue; + } + } else if (heartbeat) { + api::armteleop::v1::ClientHeartbeat heartbeat_frame; + heartbeat_frame.set_sequence(sequence); + sent = client_->sendHeartbeat( + heartbeat_frame, generation); + } + + const auto sent_at = Clock::now(); + bool cancel_session = false; + { + std::lock_guard lock(mutex_); + if (generation == client_session_generation_) { + if (sent) { + next_heartbeat_time = + sent_at + heartbeat_period; + if (command.has_value()) { + ++setpoints_sent_; + next_command_time = + sent_at + command_period; + } else if (heartbeat) { + ++heartbeats_sent_; + } + } else { + session_ready_ = false; + cancel_session = true; + } + } + } + if (cancel_session) { + stop_condition_.notify_all(); + client_->tryCancel(); + } + } +} + +void UmeTeleopTask::handleServerFrame( + const api::armteleop::v1::ServerFrame& frame) +{ + if (!frame.has_status()) { + return; + } + + const std::uint64_t generation = + client_->activeSessionGeneration(); + std::lock_guard lock(mutex_); + server_phase_ = frame.status().phase(); + session_id_ = frame.status().session_id(); + server_detail_ = frame.status().detail(); + if (server_phase_ == api::armteleop::v1::SESSION_PHASE_OPENED || + server_phase_ == api::armteleop::v1::SESSION_PHASE_READY || + server_phase_ == api::armteleop::v1::SESSION_PHASE_ACTIVE) { + last_error_.clear(); + if (generation != 0 && + frame.status().negotiated_watchdog_ms() != 0U) { + client_session_generation_ = generation; + negotiated_watchdog_ms_ = + frame.status().negotiated_watchdog_ms(); + session_ready_ = true; + } + } else { + session_ready_ = false; + pending_setpoint_.reset(); + } + stop_condition_.notify_all(); +} + +void registerUmeTeleopTaskFactory() +{ + TaskFactory::registerCreator( + config::TaskConfigEntry::TASK_TYPE_UME_TELEOP, + createUmeTeleopTask); +} + +} // namespace cmvr::task diff --git a/cmvr-es/task/ume_teleop_task/tests/ume_teleop_task_test.cpp b/cmvr-es/task/ume_teleop_task/tests/ume_teleop_task_test.cpp new file mode 100644 index 00000000..60ff3e1c --- /dev/null +++ b/cmvr-es/task/ume_teleop_task/tests/ume_teleop_task_test.cpp @@ -0,0 +1,429 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include "cmvr/api/arm_teleop_v1.grpc.pb.h" +#include "service/arm_teleop_client/include/grpc_arm_teleop_client.h" +#include "task/ume_teleop_task/include/ume_teleop_task.h" + +namespace { + +using namespace std::chrono_literals; +namespace api = cmvr::api::armteleop::v1; + +class WatchdogArmTeleopService final : public api::ArmTeleopService::Service { +public: + explicit WatchdogArmTeleopService( + const std::chrono::milliseconds watchdog) + : watchdog_(watchdog) + { + } + + grpc::Status Teleoperate( + grpc::ServerContext* context, + grpc::ServerReaderWriter* stream) override + { + api::ClientFrame frame; + if (!stream->Read(&frame) || !frame.has_open()) { + return grpc::Status( + grpc::StatusCode::INVALID_ARGUMENT, + "OpenSession must be first"); + } + { + std::lock_guard lock(mutex_); + open_received_ = true; + last_activity_ = std::chrono::steady_clock::now(); + } + condition_.notify_all(); + + api::ServerFrame opened; + opened.mutable_status()->set_session_id("task-test-session"); + opened.mutable_status()->set_phase(api::SESSION_PHASE_OPENED); + opened.mutable_status()->set_negotiated_watchdog_ms( + static_cast(watchdog_.count())); + if (!stream->Write(opened)) { + return grpc::Status::OK; + } + + api::ServerFrame ready; + ready.mutable_status()->set_session_id("task-test-session"); + ready.mutable_status()->set_phase(api::SESSION_PHASE_READY); + ready.mutable_status()->set_negotiated_watchdog_ms( + static_cast(watchdog_.count())); + if (!stream->Write(ready)) { + return grpc::Status::OK; + } + + std::thread reader([&] { + api::ClientFrame incoming; + while (stream->Read(&incoming)) { + const auto arrived = std::chrono::steady_clock::now(); + bool terminal = false; + { + std::lock_guard lock(mutex_); + if (incoming.has_heartbeat() || + incoming.has_setpoint()) { + const std::uint64_t sequence = + incoming.has_heartbeat() + ? incoming.heartbeat().sequence() + : incoming.setpoint().sequence(); + if (sequence == 0 || sequence <= last_sequence_) { + sequence_valid_ = false; + } + last_sequence_ = sequence; + last_activity_ = arrived; + ++activity_version_; + if (incoming.has_heartbeat()) { + ++heartbeat_count_; + } else { + setpoints_.push_back(incoming.setpoint()); + } + } else if (incoming.has_stop()) { + stop_received_ = true; + terminal = true; + } + if (terminal) { + reader_finished_ = true; + } + } + condition_.notify_all(); + incoming.Clear(); + if (terminal) { + break; + } + } + { + std::lock_guard lock(mutex_); + reader_finished_ = true; + } + condition_.notify_all(); + }); + + bool expired = false; + { + std::unique_lock lock(mutex_); + std::uint64_t observed_activity = activity_version_; + while (!reader_finished_) { + const auto deadline = last_activity_ + watchdog_; + if (!condition_.wait_until( + lock, + deadline, + [&] { + return reader_finished_ || + activity_version_ != observed_activity; + })) { + watchdog_expired_ = true; + expired = true; + break; + } + observed_activity = activity_version_; + } + } + if (expired) { + context->TryCancel(); + } + if (reader.joinable()) { + reader.join(); + } + { + std::lock_guard lock(mutex_); + handler_finished_ = true; + } + condition_.notify_all(); + return expired + ? grpc::Status( + grpc::StatusCode::DEADLINE_EXCEEDED, + "test watchdog expired") + : grpc::Status::OK; + } + + bool waitForOpen(const std::chrono::milliseconds timeout) + { + std::unique_lock lock(mutex_); + return condition_.wait_for(lock, timeout, [this] { return open_received_; }); + } + + bool waitForHandlerFinish(const std::chrono::milliseconds timeout) + { + std::unique_lock lock(mutex_); + return condition_.wait_for( + lock, timeout, [this] { return handler_finished_; }); + } + + bool waitForHeartbeatCount( + const std::size_t count, + const std::chrono::milliseconds timeout) + { + std::unique_lock lock(mutex_); + return condition_.wait_for( + lock, timeout, [&] { return heartbeat_count_ >= count; }); + } + + bool waitForSetpointCount( + const std::size_t count, + const std::chrono::milliseconds timeout) + { + std::unique_lock lock(mutex_); + return condition_.wait_for( + lock, timeout, [&] { return setpoints_.size() >= count; }); + } + + std::size_t setpointCount() const + { + std::lock_guard lock(mutex_); + return setpoints_.size(); + } + + api::JointSetpoint lastSetpoint() const + { + std::lock_guard lock(mutex_); + return setpoints_.empty() + ? api::JointSetpoint{} + : setpoints_.back(); + } + + bool watchdogExpired() const + { + std::lock_guard lock(mutex_); + return watchdog_expired_; + } + + bool sequenceValid() const + { + std::lock_guard lock(mutex_); + return sequence_valid_; + } + + bool stopReceived() const + { + std::lock_guard lock(mutex_); + return stop_received_; + } + +private: + const std::chrono::milliseconds watchdog_; + mutable std::mutex mutex_; + std::condition_variable condition_; + bool open_received_{false}; + bool reader_finished_{false}; + bool handler_finished_{false}; + bool watchdog_expired_{false}; + bool sequence_valid_{true}; + bool stop_received_{false}; + std::uint64_t activity_version_{0}; + std::uint64_t last_sequence_{0}; + std::size_t heartbeat_count_{0}; + std::vector setpoints_; + std::chrono::steady_clock::time_point last_activity_{}; +}; + +cmvr::config::UmeTeleopConfig validConfig(const std::string& endpoint) +{ + cmvr::config::UmeTeleopConfig config; + config.set_id("ume_teleop_test"); + config.set_server_address(endpoint); + config.set_allow_insecure(true); + + auto* open = config.mutable_open_session(); + open->set_protocol_major(1); + open->set_protocol_minor(0); + open->set_client_instance_id("ume-task-test"); + open->set_requested_command_rate_hz(20); + open->set_requested_state_rate_hz(250); + open->set_watchdog_timeout_ms(120); + open->set_requested_lease_ms(500); + open->mutable_expected_robot()->set_robot_id("test-arm"); + open->mutable_expected_robot()->add_joint_names("joint1"); + + config.mutable_reconnect()->set_initial_delay_ms(10); + config.mutable_reconnect()->set_maximum_delay_ms(50); + config.mutable_reconnect()->set_multiplier(2.0); + return config; +} + +int fail(const std::string& detail) +{ + std::cerr << "ume_teleop_task_test: " << detail << '\n'; + return 1; +} + +} // namespace + +int main() +{ + { + cmvr::config::UmeTeleopConfig invalid; + invalid.set_id("invalid"); + cmvr::task::UmeTeleopTask task(invalid); + if (task.init() || task.state() != cmvr::task::TaskState::FAILED) { + return fail("invalid transport configuration was not rejected"); + } + task.stop(); + if (task.state() != cmvr::task::TaskState::FAILED) { + return fail("stop did not preserve an initialization failure"); + } + } + + { + // Exercise the start/stop race before a synchronous stream necessarily + // publishes its ClientContext. The Task's cancellation predicate closes + // this gap, while tryCancel() interrupts it once the context is visible. + const std::string unavailable_endpoint = + "unix:/tmp/cmvr_ume_teleop_unavailable_" + + std::to_string(static_cast(::getpid())) + ".sock"; + auto channel = grpc::CreateChannel( + unavailable_endpoint, + grpc::InsecureChannelCredentials()); + auto client = + std::make_shared(channel); + cmvr::task::UmeTeleopTask task( + validConfig(unavailable_endpoint), client); + if (!task.init() || !task.start()) { + return fail("immediate-stop task did not initialize and start"); + } + api::JointSetpoint disconnected_setpoint; + disconnected_setpoint.add_position_rad(1.0); + disconnected_setpoint.set_valid_for_us(100000); + if (task.submitSetpoint(disconnected_setpoint)) { + task.stop(); + return fail( + "task accepted a motion command without a ready session"); + } + const auto stop_begin = std::chrono::steady_clock::now(); + task.stop(); + if (std::chrono::steady_clock::now() - stop_begin > 2s || + task.state() != cmvr::task::TaskState::STOPPED) { + return fail("immediate Task stop did not cancel and join promptly"); + } + } + + WatchdogArmTeleopService service(120ms); + const std::string socket_path = + "/tmp/cmvr_ume_teleop_task_test_" + + std::to_string(static_cast(::getpid())) + ".sock"; + std::remove(socket_path.c_str()); + const std::string endpoint = "unix:" + socket_path; + + grpc::ServerBuilder builder; + builder.AddListeningPort( + endpoint, + grpc::InsecureServerCredentials()); + builder.RegisterService(&service); + std::unique_ptr server = builder.BuildAndStart(); + if (!server) { + return fail("failed to start in-process gRPC server"); + } + + auto channel = grpc::CreateChannel( + endpoint, + grpc::InsecureChannelCredentials()); + auto client = + std::make_shared(channel); + cmvr::task::UmeTeleopTask task(validConfig(endpoint), client); + + if (task.runMode() != cmvr::task::TaskRunMode::BLOCKING_SERVICE) { + server->Shutdown(); + return fail("task is not a BLOCKING_SERVICE"); + } + if (!task.init() || !task.start()) { + server->Shutdown(); + return fail("valid task did not initialize and start"); + } + if (!service.waitForOpen(2s)) { + task.stop(); + server->Shutdown(); + return fail("task worker did not open the teleoperation stream"); + } + + // No application commands are submitted here. Heartbeats alone must keep + // the session alive for longer than the negotiated watchdog. + if (!service.waitForHeartbeatCount(4, 2s) || + service.watchdogExpired() || !task.isBusy()) { + task.stop(); + server->Shutdown(); + return fail("silent command stream did not survive via heartbeats"); + } + + api::JointSetpoint first; + first.set_sequence(999); // Task must replace caller-owned sequence values. + first.add_position_rad(1.0); + first.add_velocity_rad_s(0.0); + first.set_valid_for_us(100000); + if (!task.submitSetpoint(first) || + !service.waitForSetpointCount(1, 2s)) { + task.stop(); + server->Shutdown(); + return fail("first mailbox setpoint was not sent"); + } + + // The sender rate is 20 Hz. These arrive inside one command period, so the + // capacity-one mailbox must publish only the newest value. + for (int value = 2; value <= 4; ++value) { + api::JointSetpoint setpoint; + setpoint.set_sequence(1000 + value); + setpoint.add_position_rad(static_cast(value)); + setpoint.add_velocity_rad_s(0.0); + setpoint.set_valid_for_us(100000); + if (!task.submitSetpoint(setpoint)) { + task.stop(); + server->Shutdown(); + return fail("latest-only mailbox rejected a valid setpoint"); + } + } + if (!service.waitForSetpointCount(2, 2s)) { + task.stop(); + server->Shutdown(); + return fail("latest mailbox setpoint was not sent"); + } + std::this_thread::sleep_for(70ms); + const api::JointSetpoint latest = service.lastSetpoint(); + if (service.setpointCount() != 2 || + latest.position_rad_size() != 1 || + latest.position_rad(0) != 4.0 || + latest.sequence() == 0 || + latest.sequence() >= 1000) { + task.stop(); + server->Shutdown(); + return fail("mailbox did not collapse queued commands to the latest value"); + } + + const auto stop_begin = std::chrono::steady_clock::now(); + task.stop(); + const auto stop_elapsed = std::chrono::steady_clock::now() - stop_begin; + if (stop_elapsed > 2s) { + server->Shutdown(); + return fail("Task stop did not TryCancel and join promptly"); + } + if (task.state() != cmvr::task::TaskState::STOPPED || + task.isBusy() || !task.isFinished()) { + server->Shutdown(); + return fail("task did not reach STOPPED after joining its worker"); + } + if (!service.waitForHandlerFinish(2s)) { + server->Shutdown(); + return fail("server handler did not observe Task cancellation"); + } + if (!service.stopReceived()) { + server->Shutdown(); + return fail("Task did not send StopSession before TryCancel"); + } + if (service.watchdogExpired() || !service.sequenceValid()) { + server->Shutdown(); + return fail("heartbeat/setpoint sequence or watchdog contract failed"); + } + + server->Shutdown(); + std::remove(socket_path.c_str()); + std::cout << "ume_teleop_task_test: PASS\n"; + return 0; +} diff --git a/docs/teleoperation/ume_cmvr_architecture.md b/docs/teleoperation/ume_cmvr_architecture.md new file mode 100644 index 00000000..b86f9c4f --- /dev/null +++ b/docs/teleoperation/ume_cmvr_architecture.md @@ -0,0 +1,111 @@ +# UME to CMVR-ES Teleoperation Architecture + +## Scope + +This document freezes the first implementation stage of the wired, +cross-machine teleoperation path: + +- both edge computers run `cmvr_es`; +- the UME computer owns the Damiao SocketCAN-FD interfaces; +- `UmeRobotArm` is a `RobotArm` backend; +- the UME computer performs the leader/follower model calculations; +- the robot computer validates and executes joint-servo references; +- the transport is a versioned gRPC bidirectional stream; +- the original UME algorithm is migrated before any SEW fusion work. + +SEW fusion, passivity research, paper experiments, and physical human testing +are deliberately outside this implementation stage. + +## Target process and ownership boundary + +```text +UME cmvr_es + UmeRobotArm + Damiao SocketCAN-FD + UmeHapticLoop + local safety guard + | + +-- UmeLegacyAlgorithm + | + UmeTeleopTask + GrpcArmTeleopClient + | + | wired Ethernet / gRPC bidi stream + v +Robot cmvr_es + ArmTeleopService + control lease + latest-only command slot + watchdog + safe servo executor + | + v + RobotArm +``` + +The UME high-frequency loop never performs a network RPC. Network workers and +hardware loops exchange only bounded latest-state snapshots. + +## Implemented boundary in this revision + +This revision establishes the device, algorithm-library, session-transport and +robot-backend boundaries, but it intentionally does not connect them into a +physical end-to-end controller: + +- `UmeRobotArm` owns one eight-axis Damiao bus and a bounded local torque loop. + It starts passive, requires an explicit fresh torque command before + `torqueOn`, and latches watchdog/transport faults. +- a successful SocketCAN send means that the complete frame batch was accepted + by the local kernel before its deadline. The current MIT transport has no + reviewed Disable acknowledgement, so software must not describe that result + as actuator-confirmed torque-off. +- the original UME dynamics, friction, stiction and haptic projection code is a + standalone tested library under `algorithms/controllers/ume_legacy`; +- `UmeTeleopTask` owns reconnect, heartbeat, sequence and a capacity-one + outbound mailbox. Its `submitSetpoint()` input is deliberately an algorithm + boundary; no production source calls it yet; +- the server-side `RobotArmTeleopBackend` is implemented and tested behind an + explicit capability gate. The current `MotorRobotArm` remains unavailable + because its `servoJ` path is sequential per joint rather than an accepted + atomic/timed group primitive; +- server state is currently returned with session events. The configured + requested state rate is not yet an independent periodic publisher; +- follower effort is validated and transported when a backend declares a + verified source, but it is not yet consumed by a UME haptic coordinator. + +Accordingly, this code is an M0-M9 fail-closed framework and original-algorithm +migration, not a claim of runnable force-feedback teleoperation. The next +integration step must add a concrete UME-to-follower retargeting producer, +connect verified follower effort to the local haptic coordinator, and retain +the high-frequency/network-thread separation above. + +## Safety invariants + +1. Opening a CAN interface never enables a motor. +2. Clearing a fault never arms a motor. +3. A reconnect creates a new session and never restores active motion. +4. Cross-machine `steady_clock` values are diagnostic only. A receiver derives + command expiry from its local receive time plus `valid_for_us`. +5. Commands are strictly increasing by sequence within one session. +6. Queues on the cyclic path are latest-only and bounded. +7. Invalid or stale follower effort ramps haptic feedback to zero. +8. Model, joint order, units, and calibration hashes must match before motion. +9. New physical hardware configurations remain disabled by default. +10. A software stop does not replace an independent physical emergency stop. +11. UME hardware enable also requires an explicit firmware-reviewed feedback + status whitelist and raw temperature thresholds; empty values never mean + "accept all". +12. UME shutdown timing is diagnosed against a configured budget, but physical + torque removal still requires an independent emergency-stop path until a + reviewed actuator Disable acknowledgement exists. + +## Initial rate boundary + +- UME local hardware/haptic loop: configurable, initially 800 Hz to match the + legacy UME setup. +- network command rate: configurable independently of the local loop. +- robot servo rate: selected from the robot backend capability and never + inferred from the network rate. + +No hard real-time or stability claim is made until the target computers and +physical devices have completed staged validation. diff --git a/docs/teleoperation/ume_cmvr_validation.md b/docs/teleoperation/ume_cmvr_validation.md new file mode 100644 index 00000000..8fb1cda2 --- /dev/null +++ b/docs/teleoperation/ume_cmvr_validation.md @@ -0,0 +1,118 @@ +# UME / CMVR-ES validation gates + +This checklist is part of the first UME migration. Passing a software gate +does not authorize a physical power-on. The checked-in UME device entries and +their `hardware_enabled` fields remain `false`. + +The current revision has no production setpoint producer for +`UmeTeleopTask::submitSetpoint()` and no haptic consumer for returned follower +effort. M9 therefore validates the framework, protocol and migrated original +algorithm separately; it is not an end-to-end motion or force-feedback test. + +## M9: software and network validation + +Run these gates on both target CPU architectures before deploying: + +1. Build the complete `cmvr_es` target with tests enabled. +2. Run the UME legacy controller and Pinocchio model-adapter golden tests. +3. Run the Damiao codec, CAN-FD chain, and `UmeRobotArm` lifecycle tests. +4. Run the process-wide control-authority tests. +5. Run the gRPC client, `UmeTeleopTask`, and `ArmTeleopService` tests. +6. Repeat the concurrent client/task/service tests to screen for shutdown and + reconnect races. +7. Start each checked-in edge profile without UME hardware and verify that it + never opens `can4`/`can5` or issues actuator enable frames. + +The communication tests must demonstrate all of the following: + +- an `OpenSession` manifest mismatch is rejected before backend activation; +- a second controller cannot acquire the same robot control resource; +- sequence numbers are non-zero and strictly increasing per session; +- the sender and receiver use capacity-one, latest-only command storage; +- a setpoint whose local validity has expired is never dispatched; +- heartbeat loss, lease loss, stream cancellation, and backend failure call + the robot safe-stop boundary; +- reconnect clears pending motion intent and starts a new sequence space; +- `StopSession` is attempted before client cancellation; +- the legacy unary ArmService cannot issue motion, enable, calibration, or + fault-reset commands while the teleoperation lease is active; +- `torqueOff` and `stopMotion` remain available as safety overrides. + +Before a real follower backend can be enabled, control authority must also be +extended to every local arm task and to the underlying MotorService resources. +The current process-wide lease covers ArmTeleop and unary ArmService only. + +For a wired two-computer run, record at least: + +- one-way command age at the robot ingress; +- command mailbox overwrite and rejection counters; +- heartbeat and control-lease remaining time; +- servo apply duration and deadline misses; +- disconnect detection-to-safe-stop time; +- packet loss, reordering, and delay from an explicit network impairment + profile rather than an assumed LAN condition. + +No end-to-end stability or transparency claim is supported until those logs +are tied to a specified controller rate, robot servo period, payload, motion +envelope, and network impairment profile. + +## M10: staged physical commissioning + +Every row is a separate sign-off. Do not combine first power-on with a human +wearing the UME. + +- [ ] Independent physical emergency stop is installed and verified. +- [ ] CAN arbitration/data bitrates and CAN-FD+BRS MTU are verified for each + interface. +- [ ] Motor product, firmware, command ID, feedback ID, and reported motor ID + are read back and matched to the configuration. +- [ ] The four-bit Damiao feedback status meanings and raw temperature limits + are verified for the exact product/firmware and entered as an explicit + per-joint whitelist/threshold contract. +- [ ] Joint direction and zero reference are verified one joint at a time with + the mechanism unloaded. +- [ ] Mechanical position, velocity, and torque limits replace the checked-in + placeholders and receive an independent review. +- [ ] Motor feedback timestamps and the 800 Hz cycle are measured on the UME + target computer under load. +- [ ] The exact Damiao firmware's Disable acknowledgement semantics are + documented and verified. Until then, SocketCAN send success is only + evidence that the local kernel accepted the complete frame batch. +- [ ] Because MIT feedback has no sequence field, stale request/reply + correlation is resolved by a reviewed firmware marker or by measured, + enforced bus timing; draining only the frames already queued is not + sufficient evidence. +- [ ] Every UME control-thread I/O operation is shown to be deadline-bounded + and interruptible on the target kernel. The configured shutdown timeout + is currently a diagnostic failure threshold, not a C++ timed-join + primitive. +- [ ] With torque disabled, both edge profiles run for at least 30 minutes + without sequence, deadline, lease, or reconnect anomalies. +- [ ] With the UME fixed to a stand, each joint is enabled independently at a + low torque limit and its watchdog disable path is measured. +- [ ] Both UME arms are tested together on stands; CAN and CPU deadline margins + are recorded. +- [ ] The robot backend's group `servoJ` semantics and worst-case call duration + are measured. Sequential per-joint dispatch is not accepted as an + atomic group backend without a documented skew bound. +- [ ] gRPC writer backpressure cannot stall the independent robot watchdog, + and an expired lease cannot be regranted until safe stop is confirmed. +- [ ] TouchScreenTask and direct MotorService commands are either disabled by + the deployment profile or participate in the same resource authority. +- [ ] Robot-only low-speed setpoint execution is validated before connecting + the leader-side algorithm. +- [ ] Wired-network cable removal, peer process kill, delayed packets, stale + commands, duplicate sequences, lease theft, and robot fault injection + all lead to a bounded safe stop. +- [ ] The original UME gravity/friction/feedback controller is commissioned on + a stand with force feedback initially clamped to zero, then increased in + reviewed steps. +- [ ] Only after all previous evidence is archived may a supervised human test + be considered under a separate risk assessment. + +## Evidence record + +For each completed physical gate, archive the exact Git revision, installed +`output/` checksum, configuration files, model and calibration SHA-256 values, +test operator, hardware serial numbers, raw logs, and pass/fail decision. A +successful build or simulator run must not be recorded as physical validation. diff --git a/model/ume/README.md b/model/ume/README.md new file mode 100644 index 00000000..c7ac5076 --- /dev/null +++ b/model/ume/README.md @@ -0,0 +1,28 @@ +# UME legacy dynamics models + +These MJCF files are exact copies from Universal-Manipulation-Exoskeleton +commit `e087df5cd3b281418722e155d9975695f163698e`: + +- `v6_bimanual/robot.xml`: fixed-base model used for + `LOCAL_WORLD_ALIGNED` shoulder/wrist rotational Jacobians. + SHA-256: + `c185fe505ab52275d1be9c5b563df5c6505a3da81e0e622cffd66324e3839f3b`. +- `v6_imu/robot.xml`: floating-base model used for the original + `rnea(q, v, 0)` compensation path. + SHA-256: + `db1dad82ebca9413edf268a980c5be5a307c990b86654db840098095d8bd5e14`. + +The source paths are +`ume/robot/ume/v6_bimanual/models/robot.xml` and +`ume/robot/ume/v6_imu/models/robot.xml`, respectively. + +Only the model topology/dynamics parser is used. Pinocchio 3.6 +`mjcf::buildModel` and the adapter tests load these files without resolving +their visual STL assets, so no geometry files are duplicated here. + +The adapter enforces the model's structural contract. Integrations that require +byte-for-byte model identity must also compare the SHA-256 values above before +arming motion. + +The repository's root install rule copies `model/` to the deployment tree at +`bin/model/`. diff --git a/model/ume/v6_bimanual/robot.xml b/model/ume/v6_bimanual/robot.xml new file mode 100644 index 00000000..de84254c --- /dev/null +++ b/model/ume/v6_bimanual/robot.xml @@ -0,0 +1,356 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/model/ume/v6_imu/robot.xml b/model/ume/v6_imu/robot.xml new file mode 100644 index 00000000..684292c4 --- /dev/null +++ b/model/ume/v6_imu/robot.xml @@ -0,0 +1,389 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/protos/cmvr/api/arm_teleop_v1.proto b/protos/cmvr/api/arm_teleop_v1.proto new file mode 100644 index 00000000..d7095a95 --- /dev/null +++ b/protos/cmvr/api/arm_teleop_v1.proto @@ -0,0 +1,133 @@ +syntax = "proto3"; + +package cmvr.api.armteleop.v1; + +// Versioned, session-oriented protocol for wired arm teleoperation. Existing +// unary ArmService RPCs intentionally remain unchanged. +service ArmTeleopService { + rpc Teleoperate(stream ClientFrame) returns (stream ServerFrame); +} + +enum SessionPhase { + SESSION_PHASE_UNSPECIFIED = 0; + SESSION_PHASE_OPENED = 1; + SESSION_PHASE_READY = 2; + SESSION_PHASE_ACTIVE = 3; + SESSION_PHASE_HOLDING = 4; + SESSION_PHASE_STOPPED = 5; + SESSION_PHASE_WATCHDOG_EXPIRED = 6; + SESSION_PHASE_LEASE_LOST = 7; + SESSION_PHASE_REJECTED = 8; + SESSION_PHASE_FAILED = 9; +} + +enum StopReason { + STOP_REASON_UNSPECIFIED = 0; + STOP_REASON_OPERATOR_REQUEST = 1; + STOP_REASON_CLIENT_SHUTDOWN = 2; + STOP_REASON_WATCHDOG = 3; + STOP_REASON_LEASE_REVOKED = 4; + STOP_REASON_ROBOT_FAULT = 5; + STOP_REASON_EMERGENCY_STOP = 6; + STOP_REASON_PROTOCOL_ERROR = 7; +} + +enum EffortSource { + EFFORT_SOURCE_UNSPECIFIED = 0; + EFFORT_SOURCE_MOTOR_ESTIMATE = 1; + EFFORT_SOURCE_JOINT_SENSOR = 2; + EFFORT_SOURCE_FORCE_TORQUE_SENSOR = 3; + EFFORT_SOURCE_OBSERVER = 4; +} + +message RobotManifest { + string robot_id = 1; + string model_sha256 = 2; + string calibration_sha256 = 3; + repeated string joint_names = 4; + string position_unit = 5; + string velocity_unit = 6; + string effort_unit = 7; + string base_frame = 8; + string tool_frame = 9; +} + +message OpenSession { + uint32 protocol_major = 1; + uint32 protocol_minor = 2; + string client_instance_id = 3; + RobotManifest expected_robot = 4; + uint32 requested_command_rate_hz = 5; + uint32 requested_state_rate_hz = 6; + uint32 watchdog_timeout_ms = 7; + uint32 requested_lease_ms = 8; + bool request_force_feedback = 9; +} + +message JointSetpoint { + // Strictly increasing and non-zero within a session. + uint64 sequence = 1; + repeated double position_rad = 2; + repeated double velocity_rad_s = 3; + // The receiver computes its deadline from local arrival time plus this + // duration. Zero is invalid for an active setpoint. + uint32 valid_for_us = 4; +} + +message ClientHeartbeat { + uint64 sequence = 1; +} + +message StopSession { + StopReason reason = 1; + string detail = 2; +} + +message ClientFrame { + oneof payload { + OpenSession open = 1; + JointSetpoint setpoint = 2; + ClientHeartbeat heartbeat = 3; + StopSession stop = 4; + } +} + +message JointState { + uint64 sample_sequence = 1; + repeated double position_rad = 2; + repeated double velocity_rad_s = 3; + repeated double effort_nm = 4; + bool position_valid = 5; + bool velocity_valid = 6; + bool effort_valid = 7; + EffortSource effort_source = 8; + uint64 sample_age_us = 9; +} + +message SessionStatus { + string session_id = 1; + SessionPhase phase = 2; + uint64 received_sequence = 3; + uint64 applied_sequence = 4; + uint64 dropped_setpoints = 5; + uint64 rejected_setpoints = 6; + uint32 negotiated_watchdog_ms = 7; + uint32 lease_remaining_ms = 8; + StopReason stop_reason = 9; + string detail = 10; +} + +message RobotSafetyState { + bool connected = 1; + bool powered_on = 2; + bool protective_stopped = 3; + bool emergency_stopped = 4; + bool fault = 5; + string fault_detail = 6; +} + +message ServerFrame { + SessionStatus status = 1; + JointState joint_state = 2; + RobotSafetyState safety = 3; +} diff --git a/protos/cmvr/config/arm_config/arm_config.proto b/protos/cmvr/config/arm_config/arm_config.proto index 06c0c33b..a97be752 100644 --- a/protos/cmvr/config/arm_config/arm_config.proto +++ b/protos/cmvr/config/arm_config/arm_config.proto @@ -6,6 +6,7 @@ import "cmvr/config/pinocchio_dls_ik_config.proto"; import "cmvr/config/pinocchio_qp_ik_config.proto"; import "cmvr/config/srs_ik_config.proto"; import "cmvr/config/cartesian_motion_validation_config.proto"; +import "cmvr/config/motor_config/motor_config.proto"; enum ToppraPathType { TOPPRA_PATH_TYPE_UNKNOWN = 0; @@ -24,6 +25,10 @@ message MotorRobotArmBackendConfig { double default_vel = 7; double default_acc = 8; repeated string motor_group_ids = 9; + // Reserved opt-in for a future atomic/timed group position-servo primitive. + // MotorRobotArm's current sequential per-joint servoJ implementation + // deliberately rejects this capability even if this field is true. + bool enable_teleop_group_servo = 10; } enum VendorRobotArmBrand { @@ -45,6 +50,57 @@ message VendorRobotArmBackendConfig { string password = 10; } +enum DamiaoMotorModel { + DAMIAO_MOTOR_MODEL_UNKNOWN = 0; + DAMIAO_MOTOR_MODEL_DM4310 = 1; + DAMIAO_MOTOR_MODEL_DM4310_48V = 2; + DAMIAO_MOTOR_MODEL_DM4340 = 3; + DAMIAO_MOTOR_MODEL_DM4340_48V = 4; + DAMIAO_MOTOR_MODEL_DM6006 = 5; + DAMIAO_MOTOR_MODEL_DM8006 = 6; + DAMIAO_MOTOR_MODEL_DM8009 = 7; + DAMIAO_MOTOR_MODEL_DM10010L = 8; + DAMIAO_MOTOR_MODEL_DM10010 = 9; + DAMIAO_MOTOR_MODEL_DMH3510 = 10; + DAMIAO_MOTOR_MODEL_DMH6215 = 11; + DAMIAO_MOTOR_MODEL_DMG6220 = 12; +} + +message DamiaoJointConfig { + string joint_name = 1; + uint32 command_id = 2; + uint32 feedback_id = 3; + uint32 reported_motor_id = 4; + DamiaoMotorModel model = 5; + // Only +1 and -1 are accepted. + int32 direction = 6; + // q_joint = direction * q_motor + zero_offset_rad. + double zero_offset_rad = 7; + double joint_lower_rad = 8; + double joint_upper_rad = 9; + double max_velocity_rad_s = 10; + double max_torque_nm = 11; + // Explicit whitelist for the four-bit status nibble in MIT feedback. + // The code intentionally does not guess vendor/firmware meanings. At least + // one reviewed value is required before hardware_enabled may be true. + repeated uint32 healthy_feedback_status = 12; + // Raw byte thresholds, interpreted only as ordered protocol values. Nonzero + // reviewed limits are required before hardware_enabled may be true. + uint32 max_driver_temperature_raw = 13; + uint32 max_motor_temperature_raw = 14; +} + +message UmeRobotArmBackendConfig { + SocketCanConfig can = 1; + repeated DamiaoJointConfig joints = 2; + uint32 control_frequency_hz = 3; + uint32 cycle_deadline_us = 4; + uint32 feedback_watchdog_ms = 5; + uint32 shutdown_timeout_ms = 6; + // This is deliberately false in every checked-in configuration. + bool hardware_enabled = 7; +} + message SpeedLPlannerConfig { double linear_velocity_max = 1; double linear_acceleration_max = 2; @@ -133,6 +189,7 @@ message RobotArmConfig { oneof backend { MotorRobotArmBackendConfig motor = 10; VendorRobotArmBackendConfig vendor = 11; + UmeRobotArmBackendConfig ume = 12; } ArmKinematicsConfig kinematics = 20; diff --git a/protos/cmvr/config/grpc_server_config/grpc_server_config.proto b/protos/cmvr/config/grpc_server_config/grpc_server_config.proto index 0650c735..927cb7fc 100644 --- a/protos/cmvr/config/grpc_server_config/grpc_server_config.proto +++ b/protos/cmvr/config/grpc_server_config/grpc_server_config.proto @@ -1,6 +1,29 @@ syntax = "proto3"; package cmvr.config; +message ArmTeleopBackendConfig { + // Two independent gates are required: this service-level switch and the + // RobotArm implementation's teleop group-servo capability. + bool enable = 1; + // Also becomes RobotManifest.robot_id and the process-wide control lease + // resource. It must exactly match RobotArm.id(). + string device_id = 2; + string model_sha256 = 3; + string calibration_sha256 = 4; + string base_frame = 5; + string tool_frame = 6; + double servo_period_s = 7; + // Maximum wall time allowed for one RobotArm::servoJ call. + uint32 max_apply_duration_us = 8; + // Omission is interpreted as true by the backend. Explicit false is intended + // only for simulation and independently supervised commissioning. + optional bool require_powered = 9; + // Bounds the first target relative to the cached measured position. + double max_initial_position_step_rad = 10; + // Bounds every later target relative to the last accepted target. + double max_position_step_rad = 11; +} + message GRPCServerConfig { string host = 1; string port = 2; @@ -12,6 +35,7 @@ message GRPCServerConfig { // Frames older than this monotonic age are not sent. Zero uses the service // default so configurations written before these fields remain low-latency. uint32 camera_stream_max_frame_age_ms = 6; + ArmTeleopBackendConfig arm_teleop_backend = 7; } message GRPCServerRootConfig { GRPCServerConfig grpc_server = 1; diff --git a/protos/cmvr/config/motor_config/motor_config.proto b/protos/cmvr/config/motor_config/motor_config.proto index 0612f821..6ec4ec3b 100644 --- a/protos/cmvr/config/motor_config/motor_config.proto +++ b/protos/cmvr/config/motor_config/motor_config.proto @@ -45,6 +45,21 @@ message EtherCATDcConfig { message SocketCanConfig { string dev_id = 1; int32 channel_id = 2; + // Explicit Linux interface name (for example can0 or vcan0). When empty, + // the legacy channel_id based naming remains in use. + optional string interface_name = 3; + // Allows CAN-FD frames on the raw socket. This does not configure the + // physical interface bitrate or bring the interface up. + optional bool enable_fd = 4; + // Default BRS flag for callers that explicitly construct an FD frame. + optional bool bitrate_switch = 5; + // Bounded receive wait. Zero selects the implementation safety default. + optional uint32 receive_timeout_us = 6; + optional bool receive_own_messages = 7; + optional bool enable_error_frames = 8; + // Total bounded wait for one send() batch. Zero selects the implementation + // safety default. UME profiles set this below their control-cycle deadline. + optional uint32 send_timeout_us = 9; } message EtherCATConfig { diff --git a/protos/cmvr/config/task_manager_config/task_manager_config.proto b/protos/cmvr/config/task_manager_config/task_manager_config.proto index 8c60e370..cf6479e5 100644 --- a/protos/cmvr/config/task_manager_config/task_manager_config.proto +++ b/protos/cmvr/config/task_manager_config/task_manager_config.proto @@ -8,6 +8,7 @@ message TaskConfigEntry { TASK_TYPE_GRPC_SERVER = 3; TASK_TYPE_SELF_COLLISION = 4; TASK_TYPE_QUIC_EDGE = 5; + TASK_TYPE_UME_TELEOP = 6; } enum TaskRunMode { diff --git a/protos/cmvr/config/ume_teleop_config/ume_teleop_config.proto b/protos/cmvr/config/ume_teleop_config/ume_teleop_config.proto new file mode 100644 index 00000000..fd2a3366 --- /dev/null +++ b/protos/cmvr/config/ume_teleop_config/ume_teleop_config.proto @@ -0,0 +1,30 @@ +syntax = "proto3"; + +package cmvr.config; + +import "cmvr/api/arm_teleop_v1.proto"; + +message UmeTeleopReconnectConfig { + uint32 initial_delay_ms = 1; + uint32 maximum_delay_ms = 2; + double multiplier = 3; +} + +// Transport/session configuration only. Robot kinematics, SEW, FK and IK stay +// in UME and publish already-computed joint setpoints through the client API. +message UmeTeleopConfig { + string id = 1; + string server_address = 2; + + // M6 deliberately supports only an explicitly opted-in insecure channel. + // Keep the task disabled until the endpoint and deployment security policy + // are configured. TLS credentials can be added without changing the v1 API. + bool allow_insecure = 3; + + cmvr.api.armteleop.v1.OpenSession open_session = 4; + UmeTeleopReconnectConfig reconnect = 5; +} + +message UmeTeleopRootConfig { + UmeTeleopConfig ume_teleop = 1; +} From b54c2936beec3091f0120da655b77eb568dc63bd Mon Sep 17 00:00:00 2001 From: xtkuang <87661715@qq.com> Date: Fri, 31 Jul 2026 09:39:21 +0800 Subject: [PATCH 03/20] refactor: restore single root configuration Remove redundant UME and robot root profiles, keep the canonical cmvr_es configuration tree, and document per-host external deployment configuration. --- cmvr-es/config/README.md | 61 +++++++++---------- cmvr-es/config/cmvr_es_robot.pb.txt | 11 ---- cmvr-es/config/cmvr_es_ume.pb.txt | 10 --- .../manager/device_manager_robot.pb.txt | 24 -------- .../config/manager/device_manager_ume.pb.txt | 24 -------- .../config/manager/task_manager_robot.pb.txt | 14 ----- .../config/manager/task_manager_ume.pb.txt | 12 ---- 7 files changed, 30 insertions(+), 126 deletions(-) delete mode 100644 cmvr-es/config/cmvr_es_robot.pb.txt delete mode 100644 cmvr-es/config/cmvr_es_ume.pb.txt delete mode 100644 cmvr-es/config/manager/device_manager_robot.pb.txt delete mode 100644 cmvr-es/config/manager/device_manager_ume.pb.txt delete mode 100644 cmvr-es/config/manager/task_manager_robot.pb.txt delete mode 100644 cmvr-es/config/manager/task_manager_ume.pb.txt diff --git a/cmvr-es/config/README.md b/cmvr-es/config/README.md index 9fb2f0bc..e127e5c4 100644 --- a/cmvr-es/config/README.md +++ b/cmvr-es/config/README.md @@ -18,8 +18,6 @@ cmvr_es.pb.txt 入口文件： - [`cmvr_es.pb.txt`](cmvr_es.pb.txt) -- [`cmvr_es_ume.pb.txt`](cmvr_es_ume.pb.txt)：UME 主端样例 -- [`cmvr_es_robot.pb.txt`](cmvr_es_robot.pb.txt)：人形机械臂从端样例 - [`manager/device_manager.pb.txt`](manager/device_manager.pb.txt) - [`manager/task_manager.pb.txt`](manager/task_manager.pb.txt) @@ -111,47 +109,48 @@ output/bin/protoc \ 该命令只验证 Proto Text 解析，不验证文件、设备、证书、网络和跨字段语义。最终仍需运行组件测试和进程烟雾测试。 -## 双边遥操部署样例 +## 双边遥操配置 -仓库提供两个相互独立的 CMVR-ES 配置入口： +源码仓库保持唯一根入口 [`cmvr_es.pb.txt`](cmvr_es.pb.txt)。统一的 +[`manager/device_manager.pb.txt`](manager/device_manager.pb.txt) 已声明 +`ume_left`、`ume_right`、`ti5_motors` 和 `right_arm`；统一的 +[`manager/task_manager.pb.txt`](manager/task_manager.pb.txt) 已声明 +`ume_teleop` 和 gRPC server。角色差异不通过增加新的源码根配置文件表达，而由 +两台机器各自的外部部署配置决定。 -- UME 主端：[`cmvr_es_ume.pb.txt`](cmvr_es_ume.pb.txt)，只声明 - `ume_left` 和 `ume_right`。两条机械臂在 DeviceManager 层默认关闭， - [`devices/arm/ume_arms.pb.txt`](devices/arm/ume_arms.pb.txt) 内部的 - `hardware_enabled` 也默认关闭；两层开关必须经过标定与安全验收后分别启用。 - 出站 `ume_teleop` Task 默认关闭，样例不包含机器人地址或凭据。当前 Task - 只实现会话/重连/心跳和 latest-only 指令邮箱，尚无生产算法调用 +UME 主端部署配置应只启用本机需要的 UME 设备和 `ume_teleop` Task： + +- `ume_left`、`ume_right` 在 DeviceManager 层默认关闭； +- [`devices/arm/ume_arms.pb.txt`](devices/arm/ume_arms.pb.txt) 内部的 + `hardware_enabled` 也默认关闭； +- 两层硬件门必须在完成 CAN 映射、限位标定和安全验收后分别启用； +- 当前 Task 只实现会话、重连、心跳和 latest-only 指令邮箱，尚无生产算法调用 `submitSetpoint()`，返回 effort 也尚未接入本地触觉协调器。 -- 人形机械臂从端：[`cmvr_es_robot.pb.txt`](cmvr_es_robot.pb.txt)，通用 gRPC - server 可以启动，但 `ti5_motors` 和 `right_arm` 仍默认关闭。生产 - `ArmTeleop` 已实现真实 `RobotArm` 适配，但服务配置的 `enable` 显式关闭且 - 样例哈希故意留空。`MotorRobotArm.enable_teleop_group_servo` 目前只是预留字段； - 因为现有 `servoJ` 仍是逐关节顺序写，代码即使看到该字段为 true 也会拒绝能力。 - 必须先实现并验收原子或定时的组下发原语。因此启动 gRPC server 不等于允许遥操 - 执行，也不能绕过设备层硬件门。 -在两台边缘设备各自的源码或安装目录运行： +机器人从端部署配置应只启用经过验收的机械臂设备以及所需的 gRPC server： -```bash -# UME 主端（源码配置） -./output/bin/cmvr_es \ - --config ./cmvr-es/config/cmvr_es_ume.pb.txt +- `ti5_motors`、`right_arm` 默认关闭； +- `ArmTeleop` 服务后端默认关闭，模型哈希必须由部署配置明确给出； +- 当前 `MotorRobotArm::servoJ()` 仍是逐关节顺序写，不满足遥操作组伺服能力门； +- 启动 gRPC server 不代表允许遥操作执行，也不能绕过设备层硬件门。 -# 人形机械臂从端（源码配置） -./output/bin/cmvr_es \ - --config ./cmvr-es/config/cmvr_es_robot.pb.txt +部署时应把完整配置树分别复制到两台机器的外部目录，并继续使用相同的标准文件名： + +```text +/etc/cmvr-es/ume/cmvr_es.pb.txt +/etc/cmvr-es/robot/cmvr_es.pb.txt ``` -如果使用安装后的配置副本，则相应命令为： +两套根配置都继续引用各自目录下同名的 +`manager/device_manager.pb.txt` 和 `manager/task_manager.pb.txt`。运行命令为： ```bash -./output/bin/cmvr_es --config ./output/bin/config/cmvr_es_ume.pb.txt -./output/bin/cmvr_es --config ./output/bin/config/cmvr_es_robot.pb.txt +./output/bin/cmvr_es --config /etc/cmvr-es/ume/cmvr_es.pb.txt +./output/bin/cmvr_es --config /etc/cmvr-es/robot/cmvr_es.pb.txt ``` -上线前应把两套配置分别复制到两台机器的外部配置目录。主端需要填写从端地址、 -会话 manifest 和认证配置；从端需要换成现场机械臂设备配置，并在真实硬件测试后 -逐层开启。不要把生产 IP、token、私钥或设备标定值提交到仓库样例。 +UME 主端需要填写从端地址、会话 manifest 和认证配置；机器人从端需要填写现场 +机械臂配置。不要把生产 IP、token、私钥或设备标定值提交到仓库默认配置。 ## 生产配置 diff --git a/cmvr-es/config/cmvr_es_robot.pb.txt b/cmvr-es/config/cmvr_es_robot.pb.txt deleted file mode 100644 index 44f6f00f..00000000 --- a/cmvr-es/config/cmvr_es_robot.pb.txt +++ /dev/null @@ -1,11 +0,0 @@ -# Follower robot-side CMVR-ES profile. -# -# The RobotArm ArmTeleop backend is implemented but explicitly disabled. The -# physical arm and service backend remain closed. Current MotorRobotArm -# sequential joint writes are rejected as a teleop group-servo capability until -# an atomic/timed group primitive and its safety timing gates are accepted. -cmvr_es { - logger_config_file: "logger/logger.pb.txt" - device_manager_config_file: "manager/device_manager_robot.pb.txt" - task_manager_config_file: "manager/task_manager_robot.pb.txt" -} diff --git a/cmvr-es/config/cmvr_es_ume.pb.txt b/cmvr-es/config/cmvr_es_ume.pb.txt deleted file mode 100644 index e20ef540..00000000 --- a/cmvr-es/config/cmvr_es_ume.pb.txt +++ /dev/null @@ -1,10 +0,0 @@ -# UME leader-side CMVR-ES profile. -# -# All relative paths below are resolved from this file's directory. This -# checked-in profile contains no production endpoint, credentials or hardware -# enablement. -cmvr_es { - logger_config_file: "logger/logger.pb.txt" - device_manager_config_file: "manager/device_manager_ume.pb.txt" - task_manager_config_file: "manager/task_manager_ume.pb.txt" -} diff --git a/cmvr-es/config/manager/device_manager_robot.pb.txt b/cmvr-es/config/manager/device_manager_robot.pb.txt deleted file mode 100644 index efc4a383..00000000 --- a/cmvr-es/config/manager/device_manager_robot.pb.txt +++ /dev/null @@ -1,24 +0,0 @@ -# Follower robot-side devices. -device_manager { - name: "cmvr_es_robot" - version: "0.1" - description: "CMVR humanoid follower edge system" - init_all_motors_when_no_active_joints: false - - devices { - id: "ti5_motors" - type: DEVICE_TYPE_MOTOR_SYSTEM - config_file: "devices/motor/ti5_motors.pb.txt" - # Physical motor communication remains fail-closed in this example. - enable: false - } - - devices { - id: "right_arm" - type: DEVICE_TYPE_ROBOT_ARM - config_file: "devices/arm/arm.pb.txt" - # Do not enable until the motor system, URDF, limits, servoJ timing and - # independent emergency-stop path have passed the robot safety checkout. - enable: false - } -} diff --git a/cmvr-es/config/manager/device_manager_ume.pb.txt b/cmvr-es/config/manager/device_manager_ume.pb.txt deleted file mode 100644 index c908e447..00000000 --- a/cmvr-es/config/manager/device_manager_ume.pb.txt +++ /dev/null @@ -1,24 +0,0 @@ -# UME leader-side devices only. -device_manager { - name: "cmvr_es_ume" - version: "0.1" - description: "UME leader edge system" - init_all_motors_when_no_active_joints: false - - devices { - id: "ume_right" - type: DEVICE_TYPE_ROBOT_ARM - config_file: "devices/arm/ume_arms.pb.txt" - # Hardware gate 1/2. Gate 2/2 is ume.hardware_enabled in the arm config. - # Keep both false until CAN mapping, limits and physical safety are verified. - enable: false - } - - devices { - id: "ume_left" - type: DEVICE_TYPE_ROBOT_ARM - config_file: "devices/arm/ume_arms.pb.txt" - # Hardware gate 1/2. Gate 2/2 is ume.hardware_enabled in the arm config. - enable: false - } -} diff --git a/cmvr-es/config/manager/task_manager_robot.pb.txt b/cmvr-es/config/manager/task_manager_robot.pb.txt deleted file mode 100644 index dc53850a..00000000 --- a/cmvr-es/config/manager/task_manager_robot.pb.txt +++ /dev/null @@ -1,14 +0,0 @@ -# Follower robot-side tasks. -task_manager { - tasks { - id: "grpc_server" - type: TASK_TYPE_GRPC_SERVER - run_mode: TASK_RUN_MODE_BLOCKING_SERVICE - config_file: "tasks/grpc_server_task/grpc_server_task.pb.txt" - # The generic gRPC server may be enabled for integration. This does not - # enable a physical arm: device entries and the implemented RobotArm - # ArmTeleop adapter are explicitly disabled. Current MotorRobotArm - # sequential joint dispatch also fails the group-servo capability gate. - enable: true - } -} diff --git a/cmvr-es/config/manager/task_manager_ume.pb.txt b/cmvr-es/config/manager/task_manager_ume.pb.txt deleted file mode 100644 index 0793bb7d..00000000 --- a/cmvr-es/config/manager/task_manager_ume.pb.txt +++ /dev/null @@ -1,12 +0,0 @@ -# UME leader-side tasks. -task_manager { - tasks { - id: "ume_teleop" - type: TASK_TYPE_UME_TELEOP - run_mode: TASK_RUN_MODE_BLOCKING_SERVICE - config_file: "tasks/ume_teleop_task/ume_teleop_task.pb.txt" - # Fail-closed: configure the follower endpoint, expected manifest and - # transport security before enabling outbound teleoperation. - enable: false - } -} From edb01463ff8aea869ca39778ea4f006ba674cd44 Mon Sep 17 00:00:00 2001 From: xtkuang <87661715@qq.com> Date: Fri, 31 Jul 2026 09:39:44 +0800 Subject: [PATCH 04/20] docs: move hardware guides into component directories Keep the root README concise while documenting MotorService, the Modbus TCP PLC runtime, the motor stack, and AUBO cabinet IO next to their owning code. --- README.md | 123 ++--------------- cmvr-es/devices/README.md | 4 +- cmvr-es/devices/arm/aubo_arm/README.md | 124 ++++++++++++++++++ cmvr-es/devices/motor/README.md | 82 ++++++++++++ .../motor/bus_runtime/modbus_tcp/README.md | 93 +++++++++++++ cmvr-es/service/README.md | 36 +++++ 6 files changed, 347 insertions(+), 115 deletions(-) create mode 100644 cmvr-es/devices/arm/aubo_arm/README.md create mode 100644 cmvr-es/devices/motor/README.md create mode 100644 cmvr-es/devices/motor/bus_runtime/modbus_tcp/README.md diff --git a/README.md b/README.md index 5d0468e2..9da1699d 100644 --- a/README.md +++ b/README.md @@ -139,119 +139,16 @@ sudo script/ethercat/stop_ethercat.sh eno1 sudo script/ethercat/stop_ethercat.sh eno1 --restore-network ``` -## MotorService 与 Modbus TCP PLC +## 组件文档 -工程包含从 gRPC `MotorService`、`MotorManager`、`AbstractMotor` 到 -`ModbusTcpMotorBusRuntime` 的 CMVR PLC v1 电机控制链，x86-64 的 libmodbus -3.1.11 已放在 `dependency/x86/third_party/modbus/3.1.11`。 +具体能力、配置、协议和安全边界由对应代码目录下的 README 维护： -PLC 对接时特别注意： +- [MotorService gRPC 接口](cmvr-es/service/README.md#motorservice) +- [电机设备模块](cmvr-es/devices/motor/README.md) +- [Modbus TCP PLC runtime](cmvr-es/devices/motor/bus_runtime/modbus_tcp/README.md) +- [MotorService 与 CMVR PLC v1 完整协议](docs/motor_service_modbus_tcp.md) +- [AUBO 控制柜 Standard 数字 IO](cmvr-es/devices/arm/aubo_arm/README.md) +- [配置与部署规则](cmvr-es/config/README.md) -- `host` 必须配置为 IPv4 字面量，PLC boot ID 必须非零且每次重启变化； -- owner 决策、命令 ACK 都必须回显对应 session，重连不得执行旧 mailbox； -- PLC 在进程启动时可以离线；连接 supervisor 会继续退避重试，离线期间状态 - 返回不可用且运动命令不会写 mailbox； -- 状态区按 odd/even seqlock 发布，上位机使用 - sequence-before → 64-word block → sequence-after 三段读取验证； -- 每次 `OpenCyclicPosition/Velocity` 创建新 stream epoch，PLC 必须原子清零 - `last_applied_cyclic_sequence`、旧样本去重状态和 cyclic watchdog，确认 - `StreamActive` 与正确 mode 后才 ACK；重开后的首样本序列从 `1` 开始并必须 - 重新应用； -- 活动 cyclic 流跨 `connection_epoch` 后不会自动重开；旧流的当前和后续 - setpoint 均被拒绝并在 Quick Stop 后终止，客户端必须新建 gRPC 流。断链前 - 或断链期间 pending 的 setpoint 不会应用到新 session； -- 任何清理 Quick Stop 未确认时，MotorService 都会 fail-closed 锁存，并在 - 成功执行 `setEnabled(true)` 前拒绝新的运动命令； -- Modbus Quick Stop 只是功能性停止，不能替代硬接线急停或驱动器 STO。 -- 当前 Modbus 后端只提供 x86-64 的 libmodbus 3.1.11；`dependency/arm` - 尚无对应库，因此 ARM 构建暂不支持该后端。 - -完整 gRPC 语义、配置样例、寄存器表、TIA Portal 要求、构建测试和安全边界见 -[`docs/motor_service_modbus_tcp.md`](docs/motor_service_modbus_tcp.md)。 - -## AUBO 控制柜 IO - -`AuboArm` 通过通用的 `executeJsonCommand` 接口提供控制柜 Standard 数字 IO -读写。第一版支持以下命令： - -| `operation` | 说明 | 必填字段 | -| --- | --- | --- | -| `get_di` | 读取控制柜数字输入 | `index` | -| `get_do` | 读取控制柜数字输出及其 runstate | `index` | -| `set_do` | 设置控制柜数字输出 | `index`、`value` | - -JSON 命令示例： - -```json -{"command":"cabinet_io","operation":"get_di","index":0} -{"command":"cabinet_io","operation":"get_do","index":0} -{"command":"cabinet_io","operation":"set_do","index":0,"value":true} -``` - -其中 `index` 从 `0` 开始，运行时会根据控制器返回的 IO 数量检查范围。 -`set_do.value` 必须是 JSON 布尔值 `true` 或 `false`，不接受 `0/1` 或字符串。 -`set_do` 成功响应中的 `requested_value` 表示 SDK 已接受的请求值；需要确认控制器 -当前输出状态时，再调用一次 `get_do` 读取实际值。 - -成功响应示例： - -```json -{ - "success": true, - "command": "cabinet_io", - "operation": "get_di", - "index": 0, - "count": 16, - "value": false -} -``` - -### 通过 gRPC 调用 - -该功能复用 `cmvr.api.SystemService/ExecuteJsonCommand`。默认配置中的 gRPC -端口是 `50052`，读取 DI0： - -```shell -grpcurl -plaintext \ - -d '{ - "header":{"deviceId":"aubo_arm"}, - "requestJson":"{\"command\":\"cabinet_io\",\"operation\":\"get_di\",\"index\":0}" - }' \ - 127.0.0.1:50052 \ - cmvr.api.SystemService/ExecuteJsonCommand -``` - -设置 DO0 为高电平： - -```shell -grpcurl -plaintext \ - -d '{ - "header":{"deviceId":"aubo_arm"}, - "requestJson":"{\"command\":\"cabinet_io\",\"operation\":\"set_do\",\"index\":0,\"value\":true}" - }' \ - 127.0.0.1:50052 \ - cmvr.api.SystemService/ExecuteJsonCommand -``` - -使用源码树默认配置时，先在 `cmvr-es/config/manager/device_manager.pb.txt` 中把 -`aubo_arm` 的 `enable` 改为 `true`，并在 -`cmvr-es/config/devices/arm/aubo_arm.pb.txt` 中配置正确的控制器地址和登录信息， -然后重新安装配置并启动安装产物： - -```shell -cmake --install build -./output/bin/cmvr_es -``` - -`output/bin/cmvr_es` 读取的是 `output/bin/config/`；如果进程使用显式配置路径， -应修改该配置根下的对应文件。 -设备未启用或初始化失败时，gRPC 会返回 `Device not found: aubo_arm`。 - -### 安全约束 - -- 该接口只访问控制柜 Standard 数字 IO，不操作工具端 IO、可配置 IO 或安全 IO。 -- `set_do` 不会修改控制器的输出 runstate。只有目标通道的 runstate 为 - `StandardOutputRunState::None` 时才允许写入，否则返回 - `output_managed_by_runstate`。 -- 接口不会调用会重置全部输出配置的 `setDigitalOutputRunstateDefault()`。 -- 模拟量 IO 涉及 domain、单位和量程，第一版暂不通过该 JSON 接口开放。 +Modbus Quick Stop 只是功能性停止，不能替代硬接线急停或驱动器 STO。AUBO +JSON 接口只访问控制柜 Standard 数字 IO，不访问安全 IO。 diff --git a/cmvr-es/devices/README.md b/cmvr-es/devices/README.md index aaf43066..1fa6ac7c 100644 --- a/cmvr-es/devices/README.md +++ b/cmvr-es/devices/README.md @@ -37,12 +37,12 @@ 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 | -| RobotArm | [`arm/robot_arm.h`](arm/robot_arm.h) | [`arm/robot_arm_factory.h`](arm/robot_arm_factory.h) | MotorRobotArm、AUBO、Huayan | +| 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 | | Speaker | [`speaker/abstract_speaker.h`](speaker/abstract_speaker.h) | [`speaker/speaker_factory.h`](speaker/speaker_factory.h) | FFmpeg | | BioHead | [`biohead/abstract_biohead.h`](biohead/abstract_biohead.h) | DeviceFactory 直接创建 | BioHeadRobot | -| MotorSystem | `motor/motor_system/` | DeviceFactory 直接创建 | CAN/MuJoCo motor group | +| MotorSystem | [`motor/`](motor/README.md) | DeviceFactory 直接创建 | CAN、MuJoCo、EtherCAT、Modbus TCP PLC | 代码目录存在不等于已经接入配置创建链： diff --git a/cmvr-es/devices/arm/aubo_arm/README.md b/cmvr-es/devices/arm/aubo_arm/README.md new file mode 100644 index 00000000..6d63e6b7 --- /dev/null +++ b/cmvr-es/devices/arm/aubo_arm/README.md @@ -0,0 +1,124 @@ +# AUBO RobotArm 与控制柜 IO + +`AuboArm` 是 AUBO SDK v0.27.1 的 `RobotArm` 后端。控制柜 Standard 数字 IO +通过设备通用的 `executeJsonCommand` 接口访问，远程调用复用 +`cmvr.api.SystemService/ExecuteJsonCommand`，不经过 `ArmService` 或 +`MotorService`。 + +返回 [Devices 模块指南](../../README.md) 或 [项目总览](../../../../README.md)。 + +## 代码与配置 + +- 实现：[`aubo_arm.h`](aubo_arm.h)、[`aubo_arm.cpp`](aubo_arm.cpp) +- 测试：[`tests/aubo_arm_json_command_test.cpp`](tests/aubo_arm_json_command_test.cpp) +- 设备配置：[`../../../config/devices/arm/aubo_arm.pb.txt`](../../../config/devices/arm/aubo_arm.pb.txt) +- DeviceManager 配置： + [`../../../config/manager/device_manager.pb.txt`](../../../config/manager/device_manager.pb.txt) +- SystemService 实现： + [`../../../service/grpc/src/grpc_system_service.cpp`](../../../service/grpc/src/grpc_system_service.cpp) +- Proto：[`../../../../protos/cmvr/api/system_service.proto`](../../../../protos/cmvr/api/system_service.proto) + +仓库配置使用 SDK RPC 端口 `30004`。现场部署必须填写真实控制器地址和凭据， +不要把生产密码提交到默认配置。 + +## 控制柜 Standard 数字 IO + +当前支持： + +| `operation` | 说明 | 必填字段 | +| --- | --- | --- | +| `get_di` | 读取控制柜数字输入 | `index` | +| `get_do` | 读取控制柜数字输出及其 runstate | `index` | +| `set_do` | 设置控制柜数字输出 | `index`、`value` | + +JSON 命令： + +```json +{"command":"cabinet_io","operation":"get_di","index":0} +{"command":"cabinet_io","operation":"get_do","index":0} +{"command":"cabinet_io","operation":"set_do","index":0,"value":true} +``` + +`index` 从 `0` 开始，运行时根据控制器返回的 IO 数量检查范围。 +`set_do.value` 必须是 JSON 布尔值 `true` 或 `false`，不接受 `0/1` 或字符串。 +`set_do` 成功响应中的 `requested_value` 只表示 SDK 已接受请求；确认实际输出时 +必须再调用 `get_do`。 + +读取成功响应示例： + +```json +{ + "success": true, + "command": "cabinet_io", + "operation": "get_di", + "index": 0, + "count": 16, + "value": false +} +``` + +## 通过 gRPC 调用 + +默认 gRPC 端口为 `50052`。读取 DI0： + +```shell +grpcurl -plaintext \ + -d '{ + "header":{"deviceId":"aubo_arm"}, + "requestJson":"{\"command\":\"cabinet_io\",\"operation\":\"get_di\",\"index\":0}" + }' \ + 127.0.0.1:50052 \ + cmvr.api.SystemService/ExecuteJsonCommand +``` + +设置 DO0 为高电平： + +```shell +grpcurl -plaintext \ + -d '{ + "header":{"deviceId":"aubo_arm"}, + "requestJson":"{\"command\":\"cabinet_io\",\"operation\":\"set_do\",\"index\":0,\"value\":true}" + }' \ + 127.0.0.1:50052 \ + cmvr.api.SystemService/ExecuteJsonCommand +``` + +使用源码默认配置时： + +1. 在 `cmvr-es/config/devices/arm/aubo_arm.pb.txt` 填写正确地址和登录信息； +2. 在 `cmvr-es/config/manager/device_manager.pb.txt` 将 `aubo_arm.enable` + 改为 `true`； +3. 重新安装配置并启动安装产物。 + +```shell +cmake --install build +./output/bin/cmvr_es +``` + +`output/bin/cmvr_es` 默认读取 `output/bin/config/`。使用 `--config` 时，应修改 +对应外部配置根。设备未启用或初始化失败时，gRPC 返回 +`Device not found: aubo_arm`。 + +## 安全与语义边界 + +- 只访问控制柜 Standard 数字 IO，不访问工具端 IO、可配置 IO 或安全 IO； +- `set_do` 不修改输出 runstate； +- 只有 `StandardOutputRunState::None` 的通道允许写入，否则返回 + `output_managed_by_runstate`； +- 普通访问不会调用会重置全部输出配置的 + `setDigitalOutputRunstateDefault()`； +- 模拟量 IO 涉及 domain、单位和量程，当前 JSON 接口不开放； +- gRPC/JSON 返回成功不代表目标 IO 具备功能安全等级； +- 真实写测试前应确认通道用途、负载、电气隔离、默认电平和控制器程序所有权。 + +## 测试 + +```bash +cmake --build build --target aubo_arm_json_command_test -j4 +ctest --test-dir build \ + -R '^aubo_arm_json_command_test$' \ + --output-on-failure +``` + +该测试覆盖 JSON 校验和无硬件错误路径，不代表已在真实 AUBO 控制柜完成 DI/DO +读取、写入或 runstate 拒绝验证。 diff --git a/cmvr-es/devices/motor/README.md b/cmvr-es/devices/motor/README.md new file mode 100644 index 00000000..a10ba7fc --- /dev/null +++ b/cmvr-es/devices/motor/README.md @@ -0,0 +1,82 @@ +# Motor 设备模块 + +`devices/motor/` 提供电机管理、协议适配、总线 runtime 和厂商驱动。Service、 +RobotArm 和业务 Task 只依赖 `MotorManager`/`AbstractMotor` 的稳定接口，不应 +直接访问 libmodbus、CAN、EtherCAT 或厂商 SDK。 + +返回 [Devices 模块指南](../README.md) 或 [项目总览](../../../README.md)。 + +## 目录职责 + +| 目录 | 职责 | +| --- | --- | +| `manager/` | 创建 MotorGroup，按 `motor_id`/`joint_name` 暴露 `AbstractMotor` | +| `bus_runtime/` | 连接、收发、重连、watchdog 和总线生命周期 | +| `drivers/` | CANopen、EtherCAT、MuJoCo、Modbus PLC 等具体后端 | +| `drivers/modbus_plc_motor/` | 关节限位、SI 单位和 CMVR PLC v1 命令映射 | + +PLC 电机链路为： + +```text +gRPC MotorService + | + v +MotorManager -> AbstractMotor + | + v +CmvrPlcMotorProtocol + | + v +ModbusTcpMotorBusRuntime + | + v +libmodbus -> PLC -> 驱动器/电机 +``` + +各层边界： + +- `MotorService` 负责 API 校验、单电机控制权、deadline/cancellation 和 + fail-closed Quick Stop； +- `MotorManager` 负责电机查找与统一抽象； +- `CmvrPlcMotorProtocol` 负责关节限位、SI 单位和 CMVR PLC v1 命令映射； +- `ModbusTcpMotorBusRuntime` 负责 PLC session、mailbox、ACK、状态和重连； +- PLC/驱动器必须独立实现通信 watchdog、周期 watchdog 和硬件安全动作。 + +## Modbus TCP PLC + +实现、配置、依赖和测试入口见： + +- [Modbus TCP runtime README](bus_runtime/modbus_tcp/README.md) +- [完整 MotorService/CMVR PLC v1 协议](../../../docs/motor_service_modbus_tcp.md) +- [MotorService 文档](../../service/README.md#motorservice) +- [`plc_motors.pb.txt`](../../config/devices/motor/plc_motors.pb.txt) + +当前 Modbus 后端只提供 x86-64 的 libmodbus 3.1.11。ARM 目录没有对应库， +不能把 x86 ELF 复制到 ARM 设备使用。 + +## 安全边界 + +- MotorService 是单轴 API，不提供多轴同扫描周期的原子 commit； +- PLC/Modbus 的 Profile 或 cyclic 能力不能直接当成毫秒级机械臂组伺服； +- 软件 `emergencyStop`、Quick Stop 和普通 PLC 输出都不是安全急停； +- 真实设备必须具有独立的硬接线急停、安全继电器或 F-CPU/F-I/O，以及驱动器 + STO 等经风险评估确定的安全链； +- 新硬件配置保持 `enable: false`，完成方向、限位、watchdog 和故障注入验证后 + 才能启用。 + +## 测试 + +```bash +cmake --build build --target \ + modbus_tcp_motor_bus_runtime_test \ + grpc_motor_service_test \ + grpc_motor_service_modbus_e2e_test \ + -j4 + +ctest --test-dir build \ + -R '^(modbus_tcp_motor_bus_runtime_test|grpc_motor_service_test|grpc_motor_service_modbus_e2e_test)$' \ + --output-on-failure +``` + +端到端 fake PLC 测试需要本地 TCP bind/listen 权限。软件测试不能代替真实 +S7-1215C、驱动器、STO 和断网故障台架。 diff --git a/cmvr-es/devices/motor/bus_runtime/modbus_tcp/README.md b/cmvr-es/devices/motor/bus_runtime/modbus_tcp/README.md new file mode 100644 index 00000000..f1ac4f18 --- /dev/null +++ b/cmvr-es/devices/motor/bus_runtime/modbus_tcp/README.md @@ -0,0 +1,93 @@ +# Modbus TCP PLC Runtime + +本目录实现 CMVR PLC v1 的 Modbus TCP 总线 runtime。它负责 PLC 连接、身份 +握手、session、心跳、重连、命令 mailbox、ACK 轮询和状态快照，不负责 gRPC +请求解析，也不提供功能安全急停。 + +返回 [Motor 设备模块](../../README.md) 或 [Devices 模块指南](../../../README.md)。 + +## 代码与配置 + +- [`include/modbus_tcp_client.h`](include/modbus_tcp_client.h)：有界超时的 + libmodbus client +- [`include/modbus_tcp_motor_bus_runtime.h`](include/modbus_tcp_motor_bus_runtime.h)： + 连接 supervisor、命令和状态 runtime +- [`include/cmvr_plc_register_map.h`](include/cmvr_plc_register_map.h)： + CMVR PLC v1 寄存器常量 +- [`src/modbus_tcp_client.cpp`](src/modbus_tcp_client.cpp) +- [`src/modbus_tcp_motor_bus_runtime.cpp`](src/modbus_tcp_motor_bus_runtime.cpp) +- [`tests/modbus_tcp_motor_bus_runtime_test.cpp`](tests/modbus_tcp_motor_bus_runtime_test.cpp) +- [`../../../../config/devices/motor/plc_motors.pb.txt`](../../../../config/devices/motor/plc_motors.pb.txt) + +完整寄存器表、TIA Portal 数据块、gRPC 语义和台架步骤由 +[MotorService 与 CMVR PLC v1 完整协议](../../../../../docs/motor_service_modbus_tcp.md) +统一维护。 + +## 连接与协议约束 + +- `host` 必须是 IPv4 字面量，避免 DNS 让建连或停止出现无界等待； +- PLC boot ID 必须非零且每次 PLC 重启变化； +- owner 决策和命令 ACK 必须回显当前 session，旧 session 的 mailbox 不得执行； +- runtime `start()` 启动连接 supervisor，PLC 可以在进程启动时离线； +- 离线期间状态不可用，运动命令必须在写 mailbox 前失败； +- 重连必须重新完成身份、版本、boot ID 和 session 握手，不得重放旧命令； +- 状态区按 odd/even seqlock 发布，CMVR 使用 + sequence-before → 64-word block → sequence-after 三段读取； +- payload 必须先完整写入，commit sequence 最后写入，PLC 只原子消费新的 + commit； +- Quick Stop、Disable、故障和通信 watchdog 的状态必须通过当前 session + 的 ACK/状态确认，不能把本地写成功解释为驱动器已经安全停止。 + +## Cyclic stream + +每次 `OpenCyclicPosition/Velocity` 都创建新的轴级 stream epoch。PLC 必须在 +同一个原子状态事务中： + +1. 清零 `last_applied_cyclic_sequence`； +2. 清理旧样本去重状态； +3. 重置 cyclic watchdog； +4. 设置正确的 CSP/CSV mode； +5. 发布 `StreamActive=1` 后再 ACK。 + +重开后的首个样本序列从 `1` 开始，必须重新应用。活动 cyclic 流跨 +`connection_epoch` 后不会自动重开；旧流的当前和后续 setpoint 都被拒绝， +Quick Stop 后客户端必须建立新的 gRPC 流。断链前或断链期间 pending 的 +setpoint 不得进入新 session。 + +## 依赖与平台 + +x86-64 的 libmodbus 3.1.11 位于： + +```text +dependency/x86/third_party/modbus/3.1.11 +``` + +`dependency/arm/third_party/` 当前没有对应 libmodbus，因此 ARM 构建不支持 +该后端。支持 ARM 前必须为目标 ABI 单独编译并验证库，不能复用 x86 二进制。 + +## 安全边界 + +标准 S7-1215C DC/DC/DC 不是 failsafe PLC。Modbus Quick Stop、 +MotorService `emergencyStop`、普通 OB/FB 和普通数字输出都只是功能性控制， +不能替代： + +- 硬接线急停； +- 安全继电器或 F-CPU/F-I/O； +- 驱动器双通道 STO； +- 接触器、抱闸反馈与必要的 EDM。 + +PLC 侧通信 watchdog 和 cyclic watchdog 必须在没有 CMVR 进程参与时独立停止 +危险运动。真实启用前必须在禁能或脱载轴上验证寄存器、方向、限位、断网、PLC +重启、交换机故障和 Quick Stop 失败。 + +## 测试 + +```bash +cmake --build build --target modbus_tcp_motor_bus_runtime_test -j4 +ctest --test-dir build \ + -R '^modbus_tcp_motor_bus_runtime_test$' \ + --output-on-failure +``` + +fake PLC 测试需要本地 TCP bind/listen 权限。测试通过只证明软件协议和故障注入 +路径，不代表真实 PLC、驱动器或硬件安全链已经验收。 diff --git a/cmvr-es/service/README.md b/cmvr-es/service/README.md index 0ed71894..59959741 100644 --- a/cmvr-es/service/README.md +++ b/cmvr-es/service/README.md @@ -21,6 +21,42 @@ gRPC 和 QUIC 的职责边界： - 实时音视频使用 QUIC DATAGRAM； - `quic_edge/` 不是平台 Gateway，也不是浏览器服务器。 +## MotorService + +`MotorService` 将 gRPC 电机命令适配到已经由 `DeviceManager` 创建的 +`MotorManager` 和 `AbstractMotor`，不直接持有 PLC、现场总线或厂商驱动。 + +关键文件： + +- Proto：[`../../protos/cmvr/api/motor_service.proto`](../../protos/cmvr/api/motor_service.proto) + 和 [`../../protos/cmvr/api/motor_command.proto`](../../protos/cmvr/api/motor_command.proto) +- 实现：[`grpc/include/grpc_motor_service.h`](grpc/include/grpc_motor_service.h) + 和 [`grpc/src/grpc_motor_service.cpp`](grpc/src/grpc_motor_service.cpp) +- 注册：[`../task/grpc_server_task/src/grpc_server_task.cpp`](../task/grpc_server_task/src/grpc_server_task.cpp) +- 单元测试：[`grpc/tests/grpc_motor_service_test.cpp`](grpc/tests/grpc_motor_service_test.cpp) +- gRPC–Modbus 端到端测试： + [`grpc/tests/grpc_motor_service_modbus_e2e_test.cpp`](grpc/tests/grpc_motor_service_modbus_e2e_test.cpp) + +服务按单电机仲裁。同步 Profile 命令、Cyclic Position/Velocity 双向流、 +`setEnabled`、状态读取和软件 `emergencyStop` 共用同一控制权状态： + +- 同一电机已有 owner 时拒绝新的控制调用； +- cyclic 流首帧必须是 `open`，后续 setpoint sequence 必须严格递增； +- reader 使用 latest-wins 邮箱，客户端必须持续并发读取反馈； +- 取消、deadline、watchdog、非法帧、后端拒绝或写失败都会触发 Quick Stop； +- 任何清理 Quick Stop 未确认时，服务进入 fail-closed 锁存； +- 只有成功执行 `setEnabled(true)` 才解除服务内软件急停锁存； +- 服务层 Quick Stop 和 `emergencyStop` 都不具备功能安全等级。 + +PLC 后端的连接 epoch、stream epoch、寄存器、ACK 和 TIA Portal 要求见： + +- [Modbus TCP PLC runtime](../devices/motor/bus_runtime/modbus_tcp/README.md) +- [MotorService 与 CMVR PLC v1 完整协议](../../docs/motor_service_modbus_tcp.md) + +AUBO 控制柜 IO 不经过 `MotorService` 或 `ArmService`，而是复用 +`SystemService/ExecuteJsonCommand`。厂商命令和安全约束见 +[AUBO 控制柜 IO](../devices/arm/aubo_arm/README.md)。 + ## 新增 gRPC Service 当前没有动态 service registry，必须完成以下全部步骤。 From 31b2d98625fd90fc934b1f54244bfaf1ab940f87 Mon Sep 17 00:00:00 2001 From: xtkuang <87661715@qq.com> Date: Fri, 31 Jul 2026 11:12:35 +0800 Subject: [PATCH 05/20] fix: acquire SRC1100 control authority before commands --- cmvr-es/config/devices/agv/src1100.pb.txt | 1 + cmvr-es/devices/agv/src1100/CMakeLists.txt | 28 ++ .../devices/agv/src1100/include/src1100_agv.h | 11 + .../devices/agv/src1100/src/src1100_agv.cpp | 127 ++++- .../tests/src1100_control_authority_test.cpp | 435 ++++++++++++++++++ .../cmvr/config/agv_config/agv_config.proto | 2 + 6 files changed, 591 insertions(+), 13 deletions(-) create mode 100644 cmvr-es/devices/agv/src1100/tests/src1100_control_authority_test.cpp diff --git a/cmvr-es/config/devices/agv/src1100.pb.txt b/cmvr-es/config/devices/agv/src1100.pb.txt index 833bde58..16867ce7 100644 --- a/cmvr-es/config/devices/agv/src1100.pb.txt +++ b/cmvr-es/config/devices/agv/src1100.pb.txt @@ -18,6 +18,7 @@ agv { port_other: 19210 port_push: 19301 recv_timeout_ms: 1000 + control_nick_name: "cmvr-es" enable_state_push: true state_push_interval_ms: 200 state_push_included_fields: "x" diff --git a/cmvr-es/devices/agv/src1100/CMakeLists.txt b/cmvr-es/devices/agv/src1100/CMakeLists.txt index 6ad1f3fe..705542e2 100644 --- a/cmvr-es/devices/agv/src1100/CMakeLists.txt +++ b/cmvr-es/devices/agv/src1100/CMakeLists.txt @@ -10,3 +10,31 @@ target_link_libraries(src1100_agv 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/include/src1100_agv.h b/cmvr-es/devices/agv/src1100/include/src1100_agv.h index ab5bcb92..466c6ced 100644 --- a/cmvr-es/devices/agv/src1100/include/src1100_agv.h +++ b/cmvr-es/devices/agv/src1100/include/src1100_agv.h @@ -17,6 +17,8 @@ namespace cmvr::device { +class Src1100AgvTestPeer; + class Src1100Agv final : public AbstractAGV { public: explicit Src1100Agv(const config::Src1100AgvConfig& cfg); @@ -64,6 +66,8 @@ public: AgvResult stopMapping() override; private: + friend class Src1100AgvTestPeer; + struct Ports { int status{19204}; int control{19205}; @@ -80,6 +84,11 @@ private: void closeSocket_(int& sock) const; bool connected_() const; + AgvResult acquireControl_() const; + AgvResult sendControlledCommand_(int sock, + std::uint16_t command, + const Json::Value& payload, + Json::Value* response) const; AgvResult sendCommand_(int sock, std::uint16_t command, const Json::Value& payload, @@ -141,6 +150,7 @@ private: config::Src1100AgvConfig config_; std::string ip_; + std::string control_nick_name_; int recv_timeout_ms_{1000}; Ports ports_; bool state_push_enabled_{false}; @@ -149,6 +159,7 @@ private: std::size_t map_update_history_size_{8}; mutable std::mutex mutex_; + mutable std::mutex control_sequence_mutex_; int sock_status_{-1}; int sock_control_{-1}; int sock_navigation_{-1}; diff --git a/cmvr-es/devices/agv/src1100/src/src1100_agv.cpp b/cmvr-es/devices/agv/src1100/src/src1100_agv.cpp index 7c1b1b2b..eb26fdc2 100644 --- a/cmvr-es/devices/agv/src1100/src/src1100_agv.cpp +++ b/cmvr-es/devices/agv/src1100/src/src1100_agv.cpp @@ -42,6 +42,7 @@ 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; @@ -339,6 +340,10 @@ AgvTaskType toTaskType(const int value) 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()), @@ -556,12 +561,62 @@ AgvResult Src1100Agv::disconnect_() AgvResult Src1100Agv::emergencyStop() { - return cancelNavigation(); + // 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_); + 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); + } + + 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); + return result.ok() ? resultFromResponse_(response) : result; + }; + + const auto motion_stop = send_stop(sock_control_, kRobotControlStop); + const auto navigation_cancel = send_stop(sock_navigation_, kRobotTaskCancel); + if (!motion_stop.ok()) { + const std::string detail = motion_stop.message.empty() ? "unknown error" : motion_stop.message; + if (!navigation_cancel.ok()) { + const std::string cancel_detail = navigation_cancel.message.empty() + ? "unknown error" + : navigation_cancel.message; + return AgvResult::failure( + motion_stop.code, + "SRC1100 software stop failed: control stop: " + detail + + "; cancel navigation: " + cancel_detail); + } + return AgvResult::failure( + motion_stop.code, + "SRC1100 software stop failed: control stop: " + detail); + } + if (!navigation_cancel.ok()) { + const std::string detail = navigation_cancel.message.empty() + ? "unknown error" + : navigation_cancel.message; + return AgvResult::failure( + navigation_cancel.code, + "SRC1100 software stop failed: cancel navigation: " + detail); + } + return AgvResult::success(); } AgvResult Src1100Agv::clearFault() { - return AgvResult::success(); + return AgvResult::failure( + AgvErrorCode::UnsupportedCommand, + "SRC1100 clearFault command is not implemented"); } AgvResult Src1100Agv::navigateToPose( @@ -580,7 +635,7 @@ AgvResult Src1100Agv::navigateToPose( applyMotionOptions_(payload, options); applyAdapterParams_(payload, adapter_params); Json::Value response; - auto result = sendCommand_(sock_navigation_, kRobotTaskGoTarget, payload, &response); + auto result = sendControlledCommand_(sock_navigation_, kRobotTaskGoTarget, payload, &response); return result.ok() ? resultFromResponse_(response) : result; } @@ -595,7 +650,7 @@ AgvResult Src1100Agv::navigateToStation( applyMotionOptions_(payload, options); applyAdapterParams_(payload, adapter_params); Json::Value response; - auto result = sendCommand_(sock_navigation_, kRobotTaskGoTarget, payload, &response); + auto result = sendControlledCommand_(sock_navigation_, kRobotTaskGoTarget, payload, &response); return result.ok() ? resultFromResponse_(response) : result; } @@ -613,28 +668,40 @@ AgvResult Src1100Agv::followPath(const std::vector& path) } jsonMember(payload, "move_task_list") = tasks; Json::Value response; - auto result = sendCommand_(sock_navigation_, kRobotTaskGoTargetList, payload, &response); + auto result = sendControlledCommand_(sock_navigation_, kRobotTaskGoTargetList, payload, &response); return result.ok() ? resultFromResponse_(response) : result; } AgvResult Src1100Agv::pauseNavigation() { Json::Value response; - auto result = sendCommand_(sock_navigation_, kRobotTaskPause, Json::Value(Json::objectValue), &response); + auto result = sendControlledCommand_( + sock_navigation_, + kRobotTaskPause, + Json::Value(Json::objectValue), + &response); return result.ok() ? resultFromResponse_(response) : result; } AgvResult Src1100Agv::resumeNavigation() { Json::Value response; - auto result = sendCommand_(sock_navigation_, kRobotTaskResume, Json::Value(Json::objectValue), &response); + auto result = sendControlledCommand_( + sock_navigation_, + kRobotTaskResume, + Json::Value(Json::objectValue), + &response); return result.ok() ? resultFromResponse_(response) : result; } AgvResult Src1100Agv::cancelNavigation() { Json::Value response; - auto result = sendCommand_(sock_navigation_, kRobotTaskCancel, Json::Value(Json::objectValue), &response); + auto result = sendControlledCommand_( + sock_navigation_, + kRobotTaskCancel, + Json::Value(Json::objectValue), + &response); return result.ok() ? resultFromResponse_(response) : result; } @@ -646,7 +713,7 @@ AgvResult Src1100Agv::setVelocity(const AgvVelocity& velocity) jsonMember(payload, "w") = velocity.wz; jsonMember(payload, "duration") = -1; Json::Value response; - auto result = sendCommand_(sock_control_, kRobotControlMotion, payload, &response); + auto result = sendControlledCommand_(sock_control_, kRobotControlMotion, payload, &response); return result.ok() ? resultFromResponse_(response) : result; } @@ -690,7 +757,7 @@ AgvResult Src1100Agv::switchMap(const std::string& map_name) Json::Value payload(Json::objectValue); jsonMember(payload, "map_name") = map_name; Json::Value response; - auto result = sendCommand_(sock_control_, kRobotControlLoadMap, payload, &response); + auto result = sendControlledCommand_(sock_control_, kRobotControlLoadMap, payload, &response); return result.ok() ? resultFromResponse_(response) : result; } @@ -700,7 +767,7 @@ AgvResult Src1100Agv::uploadMap(const std::string& map_name, const std::string& jsonMember(payload, "map_name") = map_name; jsonMember(payload, "map_content") = content; Json::Value response; - auto result = sendCommand_(sock_config_, kRobotConfigUploadMap, payload, &response); + auto result = sendControlledCommand_(sock_config_, kRobotConfigUploadMap, payload, &response); return result.ok() ? resultFromResponse_(response) : result; } @@ -728,7 +795,7 @@ AgvResult Src1100Agv::startMapping(const AgvMappingOptions& options) } Json::Value response; - result = sendCommand_(sock_other_, kRobotOtherStartMapping, payload, &response); + result = sendControlledCommand_(sock_other_, kRobotOtherStartMapping, payload, &response); result = result.ok() ? resultFromResponse_(response) : result; if (result.ok()) { { @@ -1440,7 +1507,11 @@ AgvResult Src1100Agv::stopMapping() if (!result.ok()) return result; Json::Value response; - result = sendCommand_(sock_other_, kRobotOtherStopMapping, Json::Value(Json::objectValue), &response); + result = sendControlledCommand_( + sock_other_, + kRobotOtherStopMapping, + Json::Value(Json::objectValue), + &response); return result.ok() ? resultFromResponse_(response) : result; } @@ -1496,6 +1567,36 @@ 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; +} + +AgvResult Src1100Agv::sendControlledCommand_( + const int sock, + const std::uint16_t command, + const Json::Value& payload, + Json::Value* response) const +{ + // Keep the permission acquisition and the following write ordered with + // respect to other control RPCs in this process. sendCommand_ has its own + // socket mutex, so this must remain a distinct lock. + std::lock_guard sequence_lock(control_sequence_mutex_); + 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); + } + return sendCommand_(sock, command, payload, response); +} + AgvResult Src1100Agv::sendCommand_( const int sock, const std::uint16_t command, 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 new file mode 100644 index 00000000..cc834dd1 --- /dev/null +++ b/cmvr-es/devices/agv/src1100/tests/src1100_control_authority_test.cpp @@ -0,0 +1,435 @@ +#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 control, + const int navigation, + const int config, + const int other) + { + agv.sock_control_ = control; + agv.sock_navigation_ = navigation; + agv.sock_config_ = config; + agv.sock_other_ = other; + } +}; + +namespace { + +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 { + Control = 0, + Navigation, + Config, + Other, + Count +}; + +struct CommandRecord { + std::uint16_t command{0}; + std::string payload; +}; + +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 request_command, + const int ret_code) +{ + const std::string payload = ret_code == 0 + ? R"({"ret_code":0,"err_msg":""})" + : "{\"ret_code\":" + std::to_string(ret_code) + + R"(,"err_msg":"simulated command failure"})"; + 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); + const auto response_command = static_cast(request_command + 10000U); + 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; +} + +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; + } + + 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, std::move(payload)}); + } + int ret_code = 0; + { + std::lock_guard lock(response_codes_mutex_); + const auto response = response_codes_.find(command); + if (response != response_codes_.end()) { + ret_code = response->second; + } + } + if (!sendAll(fd, responseFrame(command, ret_code))) { + return; + } + } + } + + std::array(Channel::Count)> endpoints_; + mutable std::mutex records_mutex_; + std::vector records_; + std::mutex response_codes_mutex_; + std::unordered_map response_codes_; +}; + +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"); + agv_ = std::make_unique(cfg); + Src1100AgvTestPeer::installSockets( + *agv_, + controller_.takeClient(Channel::Control), + controller_.takeClient(Channel::Navigation), + controller_.takeClient(Channel::Config), + controller_.takeClient(Channel::Other)); + } + + 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) +{ + expectControlled(kRobotTaskGoTarget, [this]() { + return agv_->navigateToPose(math::Pose2d{1.0, 2.0, 0.5}); + }); + 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); + 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); + 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); + 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); +} + +} // namespace +} // namespace cmvr::device diff --git a/protos/cmvr/config/agv_config/agv_config.proto b/protos/cmvr/config/agv_config/agv_config.proto index c8ec8c56..10dae43b 100644 --- a/protos/cmvr/config/agv_config/agv_config.proto +++ b/protos/cmvr/config/agv_config/agv_config.proto @@ -49,6 +49,8 @@ message Src1100AgvConfig { int32 map_update_interval_ms = 17; // 统一地图更新缓存条数。0 表示使用适配器默认值；缓存满后会丢弃最旧更新。 uint32 map_update_history_size = 18; + // 抢占 SRC1100 控制权时上报的稳定昵称。为空时适配器使用 "cmvr-es:"。 + string control_nick_name = 19; } // 单个 AGV 设备配置。 From f092e2539d1697b972e122dcfb7aace62ce04e44 Mon Sep 17 00:00:00 2001 From: xtkuang <87661715@qq.com> Date: Fri, 31 Jul 2026 12:21:19 +0800 Subject: [PATCH 06/20] fix: correct SRC1100 navigation and velocity commands --- .../devices/agv/src1100/include/src1100_agv.h | 7 +- .../devices/agv/src1100/src/src1100_agv.cpp | 79 ++++---- .../tests/src1100_control_authority_test.cpp | 175 +++++++++++++++++- cmvr-es/service/CMakeLists.txt | 31 ++++ .../grpc/tests/grpc_agv_service_test.cpp | 163 ++++++++++++++++ 5 files changed, 417 insertions(+), 38 deletions(-) create mode 100644 cmvr-es/service/grpc/tests/grpc_agv_service_test.cpp diff --git a/cmvr-es/devices/agv/src1100/include/src1100_agv.h b/cmvr-es/devices/agv/src1100/include/src1100_agv.h index 466c6ced..3d745590 100644 --- a/cmvr-es/devices/agv/src1100/include/src1100_agv.h +++ b/cmvr-es/devices/agv/src1100/include/src1100_agv.h @@ -142,9 +142,10 @@ private: static bool parseJson_(const std::string& input, Json::Value& output, std::string& error); static std::string extractJson_(const std::string& raw); static AgvResult receiveFrame_(int sock, std::uint16_t& command, std::string& payload); - static int optionalInt_(const AgvAdapterParams& params, const std::string& key, int fallback); - static double optionalDouble_(const AgvAdapterParams& params, const std::string& key, double fallback); - static void applyMotionOptions_(Json::Value& payload, const AgvMotionOptions& options); + static void applyMotionOptions_( + Json::Value& payload, + const AgvMotionOptions& options, + bool include_reach_options = true); static void applyAdapterParams_(Json::Value& payload, const AgvAdapterParams& params); static AgvResult resultFromResponse_(const Json::Value& response); diff --git a/cmvr-es/devices/agv/src1100/src/src1100_agv.cpp b/cmvr-es/devices/agv/src1100/src/src1100_agv.cpp index eb26fdc2..1158473a 100644 --- a/cmvr-es/devices/agv/src1100/src/src1100_agv.cpp +++ b/cmvr-es/devices/agv/src1100/src/src1100_agv.cpp @@ -40,6 +40,7 @@ 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 kRobotTaskGoPoint = 3050; constexpr std::uint16_t kRobotTaskGoTarget = 3051; constexpr std::uint16_t kRobotTaskGoTargetList = 3066; constexpr std::uint16_t kRobotConfigLock = 4005; @@ -624,18 +625,19 @@ AgvResult Src1100Agv::navigateToPose( const AgvMotionOptions& options, const AgvAdapterParams& adapter_params) { + (void)adapter_params; + + // API 3050 is the controller's arbitrary world-coordinate navigation + // command. Do not encode a map pose as API 3051/freeGo: that extension is + // only defined for differential-drive chassis, and a multi-steer chassis + // may accept the command before the navigation task fails. Json::Value payload(Json::objectValue); - jsonMember(payload, "source_id") = adapter_params.getString("source_id").value_or("SELF_POSITION"); - jsonMember(payload, "id") = adapter_params.getString("target_id").value_or(""); - jsonMember(payload, "skill_name") = adapter_params.getString("skill_name").value_or("GotoSpecifiedPose"); - auto& free_go = jsonMember(payload, "freeGo"); - jsonMember(free_go, "x") = pose.x; - jsonMember(free_go, "y") = pose.y; - jsonMember(free_go, "theta") = pose.theta; - applyMotionOptions_(payload, options); - applyAdapterParams_(payload, adapter_params); + jsonMember(payload, "x") = pose.x; + jsonMember(payload, "y") = pose.y; + jsonMember(payload, "angle") = pose.theta; + applyMotionOptions_(payload, options, false); Json::Value response; - auto result = sendControlledCommand_(sock_navigation_, kRobotTaskGoTarget, payload, &response); + auto result = sendControlledCommand_(sock_navigation_, kRobotTaskGoPoint, payload, &response); return result.ok() ? resultFromResponse_(response) : result; } @@ -647,8 +649,10 @@ AgvResult Src1100Agv::navigateToStation( Json::Value payload(Json::objectValue); jsonMember(payload, "source_id") = adapter_params.getString("source_id").value_or("SELF_POSITION"); jsonMember(payload, "id") = station_id; - applyMotionOptions_(payload, options); 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; auto result = sendControlledCommand_(sock_navigation_, kRobotTaskGoTarget, payload, &response); return result.ok() ? resultFromResponse_(response) : result; @@ -711,7 +715,6 @@ AgvResult Src1100Agv::setVelocity(const AgvVelocity& velocity) jsonMember(payload, "vx") = velocity.vx; jsonMember(payload, "vy") = velocity.vy; jsonMember(payload, "w") = velocity.wz; - jsonMember(payload, "duration") = -1; Json::Value response; auto result = sendControlledCommand_(sock_control_, kRobotControlMotion, payload, &response); return result.ok() ? resultFromResponse_(response) : result; @@ -1949,40 +1952,45 @@ AgvResult Src1100Agv::receiveFrame_(const int sock, std::uint16_t& command, std: return AgvResult::success(); } -int Src1100Agv::optionalInt_(const AgvAdapterParams& params, const std::string& key, const int fallback) -{ - const auto value = params.getDouble(key); - return value ? static_cast(*value) : fallback; -} - -double Src1100Agv::optionalDouble_(const AgvAdapterParams& params, const std::string& key, const double fallback) -{ - const auto value = params.getDouble(key); - return value ? *value : fallback; -} - -void Src1100Agv::applyMotionOptions_(Json::Value& payload, const AgvMotionOptions& options) +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 (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; + 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) { + 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; } - jsonMember(payload, "jack_height") = optionalDouble_( - params, - "jack_height", - jsonGet(payload, "jack_height", 0.0).asDouble()); + if (const auto jack_height = params.getDouble("jack_height")) { + jsonMember(payload, "jack_height") = *jack_height; + } } AgvResult Src1100Agv::resultFromResponse_(const Json::Value& response) @@ -1992,8 +2000,11 @@ AgvResult Src1100Agv::resultFromResponse_(const Json::Value& response) if (ret_code == 0) { return AgvResult::success(); } - return AgvResult::failure(AgvErrorCode::CommandFailed, - message.empty() ? "SRC1100 command failed: " + std::to_string(ret_code) : message); + std::string detail = "SRC1100 command failed: ret_code=" + std::to_string(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 index cc834dd1..f83b37ba 100644 --- a/cmvr-es/devices/agv/src1100/tests/src1100_control_authority_test.cpp +++ b/cmvr-es/devices/agv/src1100/tests/src1100_control_authority_test.cpp @@ -47,6 +47,7 @@ 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 kRobotTaskGoPoint = 3050; constexpr std::uint16_t kRobotTaskGoTarget = 3051; constexpr std::uint16_t kRobotTaskGoTargetList = 3066; constexpr std::uint16_t kRobotConfigLock = 4005; @@ -68,6 +69,39 @@ struct CommandRecord { 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); @@ -318,7 +352,7 @@ protected: TEST_F(Src1100ControlAuthorityTest, EveryImplementedMutatingOperationAcquiresAuthorityFirst) { - expectControlled(kRobotTaskGoTarget, [this]() { + expectControlled(kRobotTaskGoPoint, [this]() { return agv_->navigateToPose(math::Pose2d{1.0, 2.0, 0.5}); }); expectControlled(kRobotTaskGoTarget, [this]() { @@ -369,6 +403,7 @@ TEST_F(Src1100ControlAuthorityTest, AcquisitionFailureDoesNotSendControlCommand) 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); @@ -384,6 +419,7 @@ TEST_F(Src1100ControlAuthorityTest, EmergencyStopAcquisitionFailureDoesNotSendSt 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); @@ -400,6 +436,8 @@ TEST_F(Src1100ControlAuthorityTest, EmergencyStopAttemptsBothStopsAndAggregatesF 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); @@ -431,5 +469,140 @@ TEST_F(Src1100ControlAuthorityTest, ReadOnlyMapDownloadDoesNotAcquireAuthority) EXPECT_EQ(records[0].command, kRobotConfigDownloadMap); } +TEST_F(Src1100ControlAuthorityTest, NavigateToPoseUsesGoPointWithTypedMotionLimits) +{ + 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_EQ(records.size(), 2U); + EXPECT_EQ(records[0].command, kRobotConfigLock); + EXPECT_EQ(records[1].command, kRobotTaskGoPoint); + + const auto payload = parsePayload(records[1]); + EXPECT_TRUE(payloadValue(payload, "x").isNumeric()); + EXPECT_TRUE(payloadValue(payload, "y").isNumeric()); + EXPECT_TRUE(payloadValue(payload, "angle").isNumeric()); + EXPECT_DOUBLE_EQ(payloadValue(payload, "x").asDouble(), 1.0); + EXPECT_DOUBLE_EQ(payloadValue(payload, "y").asDouble(), 2.0); + EXPECT_DOUBLE_EQ(payloadValue(payload, "angle").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_FALSE(payloadHas(payload, "id")); + EXPECT_FALSE(payloadHas(payload, "source_id")); + EXPECT_FALSE(payloadHas(payload, "skill_name")); + EXPECT_FALSE(payloadHas(payload, "freeGo")); + EXPECT_FALSE(payloadHas(payload, "reach_dist")); + EXPECT_FALSE(payloadHas(payload, "reach_angle")); + EXPECT_FALSE(payloadHas(payload, "jack_height")); +} + +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); +} + } // namespace } // namespace cmvr::device diff --git a/cmvr-es/service/CMakeLists.txt b/cmvr-es/service/CMakeLists.txt index 6da68349..adccab30 100644 --- a/cmvr-es/service/CMakeLists.txt +++ b/cmvr-es/service/CMakeLists.txt @@ -133,6 +133,37 @@ if(BUILD_TESTING) ENVIRONMENT "${_grpc_motor_test_environment}" ) + add_executable(grpc_agv_service_test + grpc/tests/grpc_agv_service_test.cpp + ) + target_include_directories(grpc_agv_service_test + PRIVATE + ${CMAKE_SOURCE_DIR}/cmvr-es + ${CMAKE_SOURCE_DIR}/cmvr-es/manager/device_manager + ) + target_link_libraries(grpc_agv_service_test + PRIVATE + service + gtest + gtest_main + pthread + ) + add_test( + NAME grpc_agv_service_test + COMMAND grpc_agv_service_test + ) + set(_grpc_agv_test_environment + "LD_LIBRARY_PATH=${CMVR_TEST_EXTERNAL_LIBRARY_PATH}" + ) + if(CMVR_TEST_SYSTEM_LIBSTDCXX) + list(APPEND _grpc_agv_test_environment + "LD_PRELOAD=${CMVR_TEST_SYSTEM_LIBSTDCXX}") + endif() + set_tests_properties(grpc_agv_service_test PROPERTIES + TIMEOUT 10 + ENVIRONMENT "${_grpc_agv_test_environment}" + ) + set(_grpc_motor_modbus_e2e_libmodbus_root "${CMAKE_SOURCE_DIR}/dependency/${ARCH}/third_party/modbus/3.1.11") add_executable(grpc_motor_service_modbus_e2e_test diff --git a/cmvr-es/service/grpc/tests/grpc_agv_service_test.cpp b/cmvr-es/service/grpc/tests/grpc_agv_service_test.cpp new file mode 100644 index 00000000..6a3077a8 --- /dev/null +++ b/cmvr-es/service/grpc/tests/grpc_agv_service_test.cpp @@ -0,0 +1,163 @@ +#include "service/grpc/include/grpc_agv_service.h" + +#include +#include + +#include +#include + +#include "cmvr/config/device_manager_config/device_manager_config.pb.h" +#include "manager/device_manager/include/device_manager.h" + +namespace cmvr::service { +namespace { + +constexpr char kNativeErrorMessage[] = + "SRC1100 command failed: ret_code=41200, err_msg=speed_illegal"; + +class FakeAgv final : public device::AbstractAGV { +public: + FakeAgv() + { + id_ = "test-agv"; + } + + std::string typeName() const override { return "FakeAgv"; } + + device::AgvResult navigateToPose( + const math::Pose2d& pose, + const device::AgvMotionOptions& options, + const device::AgvAdapterParams&) override + { + pose_ = pose; + pose_options_ = options; + return device::AgvResult::success(); + } + + device::AgvResult navigateToStation( + const std::string& station_id, + const device::AgvMotionOptions& options, + const device::AgvAdapterParams&) override + { + station_id_ = station_id; + station_options_ = options; + return device::AgvResult::success(); + } + + device::AgvResult setVelocity(const device::AgvVelocity&) override + { + return device::AgvResult::failure( + device::AgvErrorCode::CommandFailed, + kNativeErrorMessage); + } + + math::Pose2d pose_; + device::AgvMotionOptions pose_options_; + std::string station_id_; + device::AgvMotionOptions station_options_; +}; + +class GrpcAgvServiceTest : public ::testing::Test { +protected: + void SetUp() override + { + config::DeviceManagerConfig config; + auto& manager = device::DeviceManager::getInstance(config); + agv_ = std::make_shared(); + manager.registerDevice(agv_); + service_ = std::make_unique(); + } + + void TearDown() override + { + service_.reset(); + agv_.reset(); + device::DeviceManager::destroyInstance(); + } + + std::shared_ptr agv_; + std::unique_ptr service_; +}; + +void setMotionOptions(msgs::AgvMotionOptions* options) +{ + options->set_max_speed(0.4); + options->set_max_angular_speed(0.5); + options->set_max_acceleration(0.6); + options->set_max_angular_acceleration(0.7); + options->set_reach_distance(0.08); + options->set_reach_angle(0.09); +} + +void expectMotionOptions(const device::AgvMotionOptions& options) +{ + EXPECT_DOUBLE_EQ(options.max_speed, 0.4); + EXPECT_DOUBLE_EQ(options.max_angular_speed, 0.5); + EXPECT_DOUBLE_EQ(options.max_acceleration, 0.6); + 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); +} + +TEST_F(GrpcAgvServiceTest, NavigationRpcsForwardSpeedAndAccelerationOptions) +{ + api::AgvNavigateToPoseCommand_Request pose_request; + pose_request.mutable_header()->set_device_id("test-agv"); + pose_request.mutable_pose()->set_x(1.0); + pose_request.mutable_pose()->set_y(2.0); + pose_request.mutable_pose()->set_theta(0.5); + setMotionOptions(pose_request.mutable_options()); + api::AgvNavigateToPoseCommand_Feedback pose_response; + grpc::ServerContext pose_context; + + const auto pose_status = service_->navigateToPose( + &pose_context, + &pose_request, + &pose_response); + + ASSERT_TRUE(pose_status.ok()) << pose_status.error_message(); + EXPECT_TRUE(pose_response.header().success()); + EXPECT_DOUBLE_EQ(agv_->pose_.x, 1.0); + EXPECT_DOUBLE_EQ(agv_->pose_.y, 2.0); + EXPECT_DOUBLE_EQ(agv_->pose_.theta, 0.5); + expectMotionOptions(agv_->pose_options_); + + api::AgvNavigateToStationCommand_Request station_request; + station_request.mutable_header()->set_device_id("test-agv"); + station_request.set_station_id("station-1"); + setMotionOptions(station_request.mutable_options()); + api::AgvNavigateToStationCommand_Feedback station_response; + grpc::ServerContext station_context; + + const auto station_status = service_->navigateToStation( + &station_context, + &station_request, + &station_response); + + ASSERT_TRUE(station_status.ok()) << station_status.error_message(); + EXPECT_TRUE(station_response.header().success()); + EXPECT_EQ(agv_->station_id_, "station-1"); + expectMotionOptions(agv_->station_options_); +} + +TEST_F(GrpcAgvServiceTest, NativeControllerCodeIsReturnedInGrpcMessage) +{ + api::AgvSetVelocityCommand_Request request; + request.mutable_header()->set_device_id("test-agv"); + request.mutable_velocity()->set_vx(0.1); + api::AgvSetVelocityCommand_Feedback response; + grpc::ServerContext context; + + const auto status = service_->setVelocity( + &context, + &request, + &response); + + EXPECT_EQ(status.error_code(), grpc::StatusCode::INTERNAL); + EXPECT_EQ(status.error_message(), kNativeErrorMessage); + EXPECT_FALSE(response.header().success()); + EXPECT_EQ(response.header().error_message(), kNativeErrorMessage); +} + +} // namespace +} // namespace cmvr::service From b5c6d50022093c0ca7e156921e1dd8f3213b259f Mon Sep 17 00:00:00 2001 From: xtkuang <87661715@qq.com> Date: Fri, 31 Jul 2026 14:27:20 +0800 Subject: [PATCH 07/20] fix: restore SRC1100 free navigation protocol --- .../devices/agv/src1100/include/src1100_agv.h | 21 +- .../devices/agv/src1100/src/src1100_agv.cpp | 554 +++++++++++-- .../tests/src1100_control_authority_test.cpp | 764 +++++++++++++++++- .../grpc/tests/grpc_agv_service_test.cpp | 30 +- 4 files changed, 1265 insertions(+), 104 deletions(-) diff --git a/cmvr-es/devices/agv/src1100/include/src1100_agv.h b/cmvr-es/devices/agv/src1100/include/src1100_agv.h index 3d745590..3aa08913 100644 --- a/cmvr-es/devices/agv/src1100/include/src1100_agv.h +++ b/cmvr-es/devices/agv/src1100/include/src1100_agv.h @@ -85,10 +85,14 @@ private: bool connected_() const; AgvResult acquireControl_() const; + AgvResult confirmPoseNavigationStarted_( + const std::string& task_id, + std::uint64_t navigation_generation) const; AgvResult sendControlledCommand_(int sock, std::uint16_t command, const Json::Value& payload, - Json::Value* response) const; + Json::Value* response, + std::uint64_t* accepted_navigation_generation = nullptr) const; AgvResult sendCommand_(int sock, std::uint16_t command, const Json::Value& payload, @@ -160,13 +164,16 @@ private: std::size_t map_update_history_size_{8}; mutable std::mutex mutex_; + mutable std::mutex status_io_mutex_; mutable std::mutex control_sequence_mutex_; - int sock_status_{-1}; - int sock_control_{-1}; - int sock_navigation_{-1}; - int sock_config_{-1}; - int sock_other_{-1}; - int sock_push_{-1}; + mutable std::atomic navigation_generation_{0}; + mutable std::atomic pose_task_sequence_{0}; + mutable int sock_status_{-1}; + mutable int sock_control_{-1}; + mutable int sock_navigation_{-1}; + mutable int sock_config_{-1}; + mutable int sock_other_{-1}; + mutable int sock_push_{-1}; std::string last_error_; std::atomic push_running_{false}; diff --git a/cmvr-es/devices/agv/src1100/src/src1100_agv.cpp b/cmvr-es/devices/agv/src1100/src/src1100_agv.cpp index 1158473a..6c9f5ff8 100644 --- a/cmvr-es/devices/agv/src1100/src/src1100_agv.cpp +++ b/cmvr-es/devices/agv/src1100/src/src1100_agv.cpp @@ -30,6 +30,7 @@ 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; @@ -40,7 +41,6 @@ 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 kRobotTaskGoPoint = 3050; constexpr std::uint16_t kRobotTaskGoTarget = 3051; constexpr std::uint16_t kRobotTaskGoTargetList = 3066; constexpr std::uint16_t kRobotConfigLock = 4005; @@ -52,6 +52,8 @@ 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 kDefaultMapUpdateIntervalMs = 1000; constexpr std::size_t kDefaultMapUpdateHistorySize = 8; constexpr std::uint64_t kMapSnapshotSequenceStart = 1; @@ -171,6 +173,16 @@ 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); @@ -191,6 +203,28 @@ std::string jsonValueToString(const Json::Value& value) 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, @@ -461,6 +495,12 @@ AgvNavigationStatus Src1100Agv::navigationStatus() const status.message = result.message; return status; } + const auto controller_result = resultFromResponse_(response); + if (!controller_result.ok()) { + status.state = AgvTaskState::Failed; + status.message = controller_result.message; + return status; + } status.state = toTaskState(jsonGet(response, "task_status", 0).asInt()); status.type = toTaskType(jsonGet(response, "task_type", 0).asInt()); @@ -477,6 +517,10 @@ AgvResult Src1100Agv::connect_() 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_); @@ -550,6 +594,7 @@ AgvResult Src1100Agv::disconnect_() { stopMapUpdateThread_(); stopPushThread_(); + std::lock_guard status_io_lock(status_io_mutex_); std::lock_guard lock(mutex_); closeSocket_(sock_status_); closeSocket_(sock_control_); @@ -575,6 +620,10 @@ AgvResult Src1100Agv::emergencyStop() "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_( @@ -582,32 +631,60 @@ AgvResult Src1100Agv::emergencyStop() command, Json::Value(Json::objectValue), &response); - return result.ok() ? resultFromResponse_(response) : result; + 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); - if (!motion_stop.ok()) { - const std::string detail = motion_stop.message.empty() ? "unknown error" : motion_stop.message; - if (!navigation_cancel.ok()) { - const std::string cancel_detail = navigation_cancel.message.empty() + advance_generation_if_needed(navigation_cancel); + + 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.message; + : navigation_cancel.result.message; return AgvResult::failure( - motion_stop.code, + motion_stop.result.code, "SRC1100 software stop failed: control stop: " + detail + "; cancel navigation: " + cancel_detail); } return AgvResult::failure( - motion_stop.code, + motion_stop.result.code, "SRC1100 software stop failed: control stop: " + detail); } - if (!navigation_cancel.ok()) { - const std::string detail = navigation_cancel.message.empty() + if (!navigation_cancel.result.ok()) { + const std::string detail = navigation_cancel.result.message.empty() ? "unknown error" - : navigation_cancel.message; + : navigation_cancel.result.message; return AgvResult::failure( - navigation_cancel.code, + navigation_cancel.result.code, "SRC1100 software stop failed: cancel navigation: " + detail); } return AgvResult::success(); @@ -625,20 +702,73 @@ AgvResult Src1100Agv::navigateToPose( const AgvMotionOptions& options, const AgvAdapterParams& adapter_params) { - (void)adapter_params; + // This SRC1100 firmware exposes arbitrary-pose navigation through the + // vendor-specific freeGo extension of API 3051. Keep the required station + // identifiers non-empty even though the controller ignores id when freeGo + // is present. + 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"); + } + std::string target_id = adapter_params.getString("target_id").value_or("SELF_POSITION"); + if (target_id.empty()) { + target_id = "SELF_POSITION"; + } + if (target_id != "SELF_POSITION") { + return AgvResult::failure( + AgvErrorCode::InvalidArgument, + "SRC1100 free navigation target_id must be SELF_POSITION so a " + "malformed freeGo request cannot fall back to station navigation"); + } + + const auto skill_name = adapter_params.getString("skill_name"); + if (skill_name && !skill_name->empty() && *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; + const std::string task_id_prefix = + adapter_params.getString("task_id").value_or(id_); + const std::string task_id = makePoseTaskId(task_id_prefix, task_sequence); - // API 3050 is the controller's arbitrary world-coordinate navigation - // command. Do not encode a map pose as API 3051/freeGo: that extension is - // only defined for differential-drive chassis, and a multi-steer chassis - // may accept the command before the navigation task fails. Json::Value payload(Json::objectValue); - jsonMember(payload, "x") = pose.x; - jsonMember(payload, "y") = pose.y; - jsonMember(payload, "angle") = pose.theta; - applyMotionOptions_(payload, options, false); + jsonMember(payload, "source_id") = source_id; + jsonMember(payload, "id") = target_id; + jsonMember(payload, "task_id") = task_id; + if (skill_name && !skill_name->empty()) { + 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); + Json::Value response; - auto result = sendControlledCommand_(sock_navigation_, kRobotTaskGoPoint, payload, &response); - return result.ok() ? resultFromResponse_(response) : result; + std::uint64_t navigation_generation = 0; + auto result = sendControlledCommand_( + sock_navigation_, + kRobotTaskGoTarget, + payload, + &response, + &navigation_generation); + if (!result.ok()) { + return result; + } + return confirmPoseNavigationStarted_(task_id, navigation_generation); } AgvResult Src1100Agv::navigateToStation( @@ -654,8 +784,13 @@ AgvResult Src1100Agv::navigateToStation( // extensions so the SRC controller receives JSON numbers. applyMotionOptions_(payload, options); Json::Value response; - auto result = sendControlledCommand_(sock_navigation_, kRobotTaskGoTarget, payload, &response); - return result.ok() ? resultFromResponse_(response) : result; + std::uint64_t accepted_generation = 0; + return sendControlledCommand_( + sock_navigation_, + kRobotTaskGoTarget, + payload, + &response, + &accepted_generation); } AgvResult Src1100Agv::followPath(const std::vector& path) @@ -672,41 +807,49 @@ AgvResult Src1100Agv::followPath(const std::vector& path) } jsonMember(payload, "move_task_list") = tasks; Json::Value response; - auto result = sendControlledCommand_(sock_navigation_, kRobotTaskGoTargetList, payload, &response); - return result.ok() ? resultFromResponse_(response) : result; + std::uint64_t accepted_generation = 0; + return sendControlledCommand_( + sock_navigation_, + kRobotTaskGoTargetList, + payload, + &response, + &accepted_generation); } AgvResult Src1100Agv::pauseNavigation() { Json::Value response; - auto result = sendControlledCommand_( + std::uint64_t accepted_generation = 0; + return sendControlledCommand_( sock_navigation_, kRobotTaskPause, Json::Value(Json::objectValue), - &response); - return result.ok() ? resultFromResponse_(response) : result; + &response, + &accepted_generation); } AgvResult Src1100Agv::resumeNavigation() { Json::Value response; - auto result = sendControlledCommand_( + std::uint64_t accepted_generation = 0; + return sendControlledCommand_( sock_navigation_, kRobotTaskResume, Json::Value(Json::objectValue), - &response); - return result.ok() ? resultFromResponse_(response) : result; + &response, + &accepted_generation); } AgvResult Src1100Agv::cancelNavigation() { Json::Value response; - auto result = sendControlledCommand_( + std::uint64_t accepted_generation = 0; + return sendControlledCommand_( sock_navigation_, kRobotTaskCancel, Json::Value(Json::objectValue), - &response); - return result.ok() ? resultFromResponse_(response) : result; + &response, + &accepted_generation); } AgvResult Src1100Agv::setVelocity(const AgvVelocity& velocity) @@ -716,8 +859,13 @@ AgvResult Src1100Agv::setVelocity(const AgvVelocity& velocity) jsonMember(payload, "vy") = velocity.vy; jsonMember(payload, "w") = velocity.wz; Json::Value response; - auto result = sendControlledCommand_(sock_control_, kRobotControlMotion, payload, &response); - return result.ok() ? resultFromResponse_(response) : result; + std::uint64_t accepted_generation = 0; + return sendControlledCommand_( + sock_control_, + kRobotControlMotion, + payload, + &response, + &accepted_generation); } AgvResult Src1100Agv::listMaps(std::vector& maps) const @@ -1580,15 +1728,156 @@ AgvResult Src1100Agv::acquireControl_() const return result.ok() ? resultFromResponse_(response) : result; } +AgvResult Src1100Agv::confirmPoseNavigationStarted_( + const std::string& task_id, + const std::uint64_t navigation_generation) const +{ + const auto deadline = std::chrono::steady_clock::now() + kPoseNavigationStartTimeout; + std::string last_status = "no task status received"; + + const auto cached_fault_detail = [this]() { + std::lock_guard lock(runtime_state_mutex_); + if (!cached_runtime_state_valid_ || !cached_runtime_state_.fault) { + return std::string{}; + } + return cached_runtime_state_.last_error; + }; + + while (true) { + if (navigation_generation_.load(std::memory_order_relaxed) != navigation_generation) { + 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"); + } + + 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; + const auto query_result = sendCommand_( + sock_status_, + kRobotStatusTaskPackage, + payload, + &response); + 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"); + } + + const auto controller_result = resultFromResponse_(response); + if (!controller_result.ok()) { + return AgvResult::failure( + controller_result.code, + "SRC1100 accepted the free-navigation command, but task status " + "query failed: " + controller_result.message); + } + + if (navigation_generation_.load(std::memory_order_relaxed) != navigation_generation) { + 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* package = jsonFind(response, "task_status_package"); + const auto* status_list = package ? jsonFind(*package, "task_status_list") : nullptr; + bool matching_task_found = false; + if (status_list && status_list->isArray()) { + for (const auto& item : *status_list) { + if (jsonGet(item, "task_id", "").asString() != task_id) { + continue; + } + matching_task_found = true; + const int task_state = jsonGet(item, "status", 0).asInt(); + const int task_type = jsonGet(item, "type", 0).asInt(); + last_status = "task_id=" + task_id + + ", task_status=" + std::to_string(task_state) + + ", task_type=" + std::to_string(task_type); + if (package) { + const std::string info = jsonGet(*package, "info", "").asString(); + if (!info.empty()) { + last_status += ", info=" + info; + } + } + + if (task_type != 1) { + return AgvResult::failure( + AgvErrorCode::TaskRejected, + "SRC1100 created an unexpected task type for free navigation: " + + last_status); + } + if (task_state == 1 || task_state == 2 || task_state == 4) { + return AgvResult::success(); + } + if (task_state == 3) { + 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_state == 5 || task_state == 6) { + std::string detail = last_status; + const std::string fault = cached_fault_detail(); + if (!fault.empty()) { + detail += ", " + fault; + } + return AgvResult::failure( + task_state == 5 ? AgvErrorCode::TaskFailed : AgvErrorCode::TaskCanceled, + task_state == 5 + ? "SRC1100 free-navigation task failed: " + detail + : "SRC1100 free-navigation task was canceled: " + detail); + } + break; + } + } + if (!matching_task_found) { + last_status = "task_id=" + task_id + " not present in task_status_package"; + } + + if (std::chrono::steady_clock::now() >= deadline) { + break; + } + std::this_thread::sleep_for(kPoseNavigationPollInterval); + } + + std::string detail = last_status; + const std::string fault = cached_fault_detail(); + if (!fault.empty()) { + detail += ", " + fault; + } + return AgvResult::failure( + AgvErrorCode::TaskRejected, + "SRC1100 accepted the free-navigation command, but no 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) const + Json::Value* response, + std::uint64_t* accepted_navigation_generation) const { // Keep the permission acquisition and the following write ordered with - // respect to other control RPCs in this process. sendCommand_ has its own - // socket mutex, so this must remain a distinct lock. + // 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 authority = acquireControl_(); if (!authority.ok()) { @@ -1597,7 +1886,45 @@ AgvResult Src1100Agv::sendControlledCommand_( authority.code, "SRC1100 acquire control authority failed: " + detail); } - return sendCommand_(sock, command, payload, response); + 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. + *accepted_navigation_generation = + navigation_generation_.fetch_add(1, std::memory_order_relaxed) + 1; + return withUnknownControllerOutcome(std::move(result)); + } + return result; + } + if (!accepted_navigation_generation) { + return result; + } + if (!response) { + *accepted_navigation_generation = + navigation_generation_.fetch_add(1, std::memory_order_relaxed) + 1; + return withUnknownControllerOutcome(AgvResult::failure( + AgvErrorCode::CommandFailed, + "SRC1100 cannot confirm navigation command without a response")); + } + if (!hasNumericControllerRetCode(*response)) { + *accepted_navigation_generation = + navigation_generation_.fetch_add(1, std::memory_order_relaxed) + 1; + 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. + *accepted_navigation_generation = + navigation_generation_.fetch_add(1, std::memory_order_relaxed) + 1; + return result; } AgvResult Src1100Agv::sendCommand_( @@ -1634,28 +1961,109 @@ AgvResult Src1100Agv::sendCommandRaw_( const Json::Value& payload, std::string* response_payload) const { - std::lock_guard lock(mutex_); - if (sock < 0) { - return AgvResult::failure(AgvErrorCode::NotConnected, "SRC1100 socket not connected"); + 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_; } - 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()) { + 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; } - (void)response_command; - if (response_payload) { - *response_payload = std::move(payload_text_response); + + 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"); } - return AgvResult::success(); + auto result = exchange(); + if (!result.ok()) { + close_matching_socket_locked(); + return mark_channel_desynchronized(std::move(result)); + } + return result; } AgvResult Src1100Agv::sendCommandNoResponse_( @@ -1837,6 +2245,17 @@ void Src1100Agv::updateCachedRuntimeState_(const Json::Value& payload) state.moving = std::hypot(state.velocity.vx, state.velocity.vy) > 1e-4 || std::abs(state.velocity.wz) > 1e-4; state.fault = hasFaultArray(payload, "fatals") || hasFaultArray(payload, "errors"); + if (state.fault) { + std::ostringstream detail; + detail << "SRC1100 controller fault"; + if (const auto* fatals = jsonFind(payload, "fatals"); fatals && !fatals->empty()) { + detail << ": fatals=" << jsonValueToString(*fatals); + } + if (const auto* errors = jsonFind(payload, "errors"); errors && !errors->empty()) { + detail << ": errors=" << jsonValueToString(*errors); + } + state.last_error = detail.str(); + } if (state.emergency_stopped) { state.mode = AgvMode::EmergencyStop; } else if (state.fault) { @@ -1995,12 +2414,21 @@ void Src1100Agv::applyAdapterParams_(Json::Value& payload, const AgvAdapterParam AgvResult Src1100Agv::resultFromResponse_(const Json::Value& response) { - const int ret_code = jsonGet(response, "ret_code", 0).asInt(); + 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 (ret_code == 0) { + if (success) { return AgvResult::success(); } - std::string detail = "SRC1100 command failed: ret_code=" + std::to_string(ret_code); + std::string detail = "SRC1100 command failed: ret_code=" + ret_code; if (!message.empty()) { detail += ", err_msg=" + message; } 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 index f83b37ba..5d7b4712 100644 --- a/cmvr-es/devices/agv/src1100/tests/src1100_control_authority_test.cpp +++ b/cmvr-es/devices/agv/src1100/tests/src1100_control_authority_test.cpp @@ -1,8 +1,11 @@ #include #include +#include #include +#include #include #include +#include #include #include #include @@ -27,27 +30,53 @@ 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 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 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 kRobotTaskGoPoint = 3050; constexpr std::uint16_t kRobotTaskGoTarget = 3051; constexpr std::uint16_t kRobotTaskGoTargetList = 3066; constexpr std::uint16_t kRobotConfigLock = 4005; @@ -57,7 +86,8 @@ constexpr std::uint16_t kRobotOtherStartMapping = 6100; constexpr std::uint16_t kRobotOtherStopMapping = 6101; enum class Channel : std::size_t { - Control = 0, + Status = 0, + Control, Navigation, Config, Other, @@ -142,13 +172,9 @@ bool sendAll(const int fd, const std::vector& data) } std::vector responseFrame( - const std::uint16_t request_command, - const int ret_code) + const std::uint16_t response_command, + const std::string& payload) { - const std::string payload = ret_code == 0 - ? R"({"ret_code":0,"err_msg":""})" - : "{\"ret_code\":" + std::to_string(ret_code) - + R"(,"err_msg":"simulated command failure"})"; std::vector frame(16 + payload.size(), 0); frame[0] = 0x5A; frame[1] = 0x01; @@ -158,13 +184,45 @@ std::vector responseFrame( frame[5] = static_cast((length >> 16U) & 0xFFU); frame[6] = static_cast((length >> 8U) & 0xFFU); frame[7] = static_cast(length & 0xFFU); - const auto response_command = static_cast(request_command + 10000U); 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() @@ -220,6 +278,37 @@ public: { 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() @@ -261,17 +350,48 @@ private: } { std::lock_guard lock(records_mutex_); - records_.push_back({command, std::move(payload)}); + 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 response = response_codes_.find(command); - if (response != response_codes_.end()) { - ret_code = response->second; + 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 (!sendAll(fd, responseFrame(command, ret_code))) { + 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; } } @@ -282,6 +402,9 @@ private: 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 { @@ -293,13 +416,25 @@ protected: cfg.set_ip("invalid-ip"); cfg.set_recv_timeout_ms(100); cfg.set_control_nick_name("cmvr-test"); + 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_, - controller_.takeClient(Channel::Control), - controller_.takeClient(Channel::Navigation), - controller_.takeClient(Channel::Config), - controller_.takeClient(Channel::Other)); + status_socket, + control_socket, + navigation_socket, + config_socket, + other_socket); } void TearDown() override @@ -352,7 +487,7 @@ protected: TEST_F(Src1100ControlAuthorityTest, EveryImplementedMutatingOperationAcquiresAuthorityFirst) { - expectControlled(kRobotTaskGoPoint, [this]() { + expectControlledSequence({kRobotTaskGoTarget, kRobotStatusTaskPackage}, [this]() { return agv_->navigateToPose(math::Pose2d{1.0, 2.0, 0.5}); }); expectControlled(kRobotTaskGoTarget, [this]() { @@ -469,7 +604,7 @@ TEST_F(Src1100ControlAuthorityTest, ReadOnlyMapDownloadDoesNotAcquireAuthority) EXPECT_EQ(records[0].command, kRobotConfigDownloadMap); } -TEST_F(Src1100ControlAuthorityTest, NavigateToPoseUsesGoPointWithTypedMotionLimits) +TEST_F(Src1100ControlAuthorityTest, NavigateToPoseUsesFreeGoWithTypedMotionLimits) { AgvMotionOptions options; options.max_speed = 0.6; @@ -486,28 +621,579 @@ TEST_F(Src1100ControlAuthorityTest, NavigateToPoseUsesGoPointWithTypedMotionLimi ASSERT_TRUE(result.ok()) << result.message; const auto records = controller_.records(); - ASSERT_EQ(records.size(), 2U); + ASSERT_EQ(records.size(), 3U); EXPECT_EQ(records[0].command, kRobotConfigLock); - EXPECT_EQ(records[1].command, kRobotTaskGoPoint); + EXPECT_EQ(records[1].command, kRobotTaskGoTarget); + EXPECT_EQ(records[2].command, kRobotStatusTaskPackage); const auto payload = parsePayload(records[1]); - EXPECT_TRUE(payloadValue(payload, "x").isNumeric()); - EXPECT_TRUE(payloadValue(payload, "y").isNumeric()); - EXPECT_TRUE(payloadValue(payload, "angle").isNumeric()); - EXPECT_DOUBLE_EQ(payloadValue(payload, "x").asDouble(), 1.0); - EXPECT_DOUBLE_EQ(payloadValue(payload, "y").asDouble(), 2.0); - EXPECT_DOUBLE_EQ(payloadValue(payload, "angle").asDouble(), 0.5); + EXPECT_EQ(payloadValue(payload, "source_id").asString(), "SELF_POSITION"); + EXPECT_EQ(payloadValue(payload, "id").asString(), "SELF_POSITION"); + 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_FALSE(payloadHas(payload, "id")); - EXPECT_FALSE(payloadHas(payload, "source_id")); + EXPECT_DOUBLE_EQ(payloadValue(payload, "reach_dist").asDouble(), 0.1); + EXPECT_DOUBLE_EQ(payloadValue(payload, "reach_angle").asDouble(), 0.2); EXPECT_FALSE(payloadHas(payload, "skill_name")); - EXPECT_FALSE(payloadHas(payload, "freeGo")); - EXPECT_FALSE(payloadHas(payload, "reach_dist")); - EXPECT_FALSE(payloadHas(payload, "reach_angle")); + 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()); +} + +TEST_F(Src1100ControlAuthorityTest, NavigateToPoseWhitelistsAdapterFieldsAndKeepsOriginFreeGo) +{ + 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_EQ(records.size(), 3U); + EXPECT_EQ(records[0].command, kRobotConfigLock); + EXPECT_EQ(records[1].command, kRobotTaskGoTarget); + EXPECT_EQ(records[2].command, kRobotStatusTaskPackage); + + const auto payload = parsePayload(records[1]); + EXPECT_EQ(payloadValue(payload, "source_id").asString(), "SELF_POSITION"); + EXPECT_EQ(payloadValue(payload, "id").asString(), "SELF_POSITION"); + EXPECT_EQ(payloadValue(payload, "task_id").asString().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")); +} + +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 SELF_POSITION"), 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, 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_EQ(records.size(), 4U); + EXPECT_EQ(records[0].command, kRobotConfigLock); + EXPECT_EQ(records[1].command, kRobotTaskGoTarget); + EXPECT_EQ(records[2].command, kRobotStatusTaskPackage); + EXPECT_EQ(records[3].command, kRobotStatusTaskPackage); +} + +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, NavigateToPoseIncludesCachedControllerFaultCodes) +{ + 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); + controller_.setResponsePayload( + kRobotStatusTaskPackage, + R"({"ret_code":0,"task_status_package":{"info":"navigation 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("E_NAV_42"), std::string::npos); + EXPECT_NE(result.message.find("planner alarm"), 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)); + } + ASSERT_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) @@ -604,5 +1290,17 @@ TEST_F(Src1100ControlAuthorityTest, ControllerErrorCodeIsPreservedInResultMessag EXPECT_NE(result.message.find("err_msg=simulated command failure"), std::string::npos); } +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); +} + } // namespace } // namespace cmvr::device 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 6a3077a8..da988fdd 100644 --- a/cmvr-es/service/grpc/tests/grpc_agv_service_test.cpp +++ b/cmvr-es/service/grpc/tests/grpc_agv_service_test.cpp @@ -14,6 +14,8 @@ namespace { constexpr char kNativeErrorMessage[] = "SRC1100 command failed: ret_code=41200, err_msg=speed_illegal"; +constexpr char kNativeNavigationErrorMessage[] = + "SRC1100 command failed: ret_code=43051, err_msg=planner_rejected_pose"; class FakeAgv final : public device::AbstractAGV { public: @@ -31,7 +33,7 @@ public: { pose_ = pose; pose_options_ = options; - return device::AgvResult::success(); + return pose_result_; } device::AgvResult navigateToStation( @@ -53,6 +55,7 @@ public: math::Pose2d pose_; device::AgvMotionOptions pose_options_; + device::AgvResult pose_result_{device::AgvResult::success()}; std::string station_id_; device::AgvMotionOptions station_options_; }; @@ -159,5 +162,30 @@ TEST_F(GrpcAgvServiceTest, NativeControllerCodeIsReturnedInGrpcMessage) EXPECT_EQ(response.header().error_message(), kNativeErrorMessage); } +TEST_F(GrpcAgvServiceTest, NativeNavigationCodeIsReturnedInGrpcMessage) +{ + agv_->pose_result_ = device::AgvResult::failure( + device::AgvErrorCode::CommandFailed, + kNativeNavigationErrorMessage); + api::AgvNavigateToPoseCommand_Request request; + request.mutable_header()->set_device_id("test-agv"); + request.mutable_pose()->set_x(1.0); + request.mutable_pose()->set_y(2.0); + api::AgvNavigateToPoseCommand_Feedback response; + grpc::ServerContext context; + + const auto status = service_->navigateToPose( + &context, + &request, + &response); + + EXPECT_EQ(status.error_code(), grpc::StatusCode::INTERNAL); + EXPECT_EQ(status.error_message(), kNativeNavigationErrorMessage); + EXPECT_FALSE(response.header().success()); + EXPECT_EQ( + response.header().error_message(), + kNativeNavigationErrorMessage); +} + } // namespace } // namespace cmvr::service From 26a7ad5d4b89058f1a9225e9dc008e21c61f5d29 Mon Sep 17 00:00:00 2001 From: xtkuang <87661715@qq.com> Date: Fri, 31 Jul 2026 17:50:49 +0800 Subject: [PATCH 08/20] fix: harden SRC1100 free navigation handling --- .../devices/agv/src1100/include/src1100_agv.h | 61 +- .../devices/agv/src1100/src/src1100_agv.cpp | 1464 ++++++++++++++-- .../tests/src1100_control_authority_test.cpp | 1491 ++++++++++++++++- 3 files changed, 2838 insertions(+), 178 deletions(-) diff --git a/cmvr-es/devices/agv/src1100/include/src1100_agv.h b/cmvr-es/devices/agv/src1100/include/src1100_agv.h index 3aa08913..e9fa3a82 100644 --- a/cmvr-es/devices/agv/src1100/include/src1100_agv.h +++ b/cmvr-es/devices/agv/src1100/include/src1100_agv.h @@ -2,6 +2,7 @@ #define CMVR_ES_SRC1100_AGV_H #include +#include #include #include #include @@ -77,6 +78,25 @@ private: int push{19301}; }; + struct PoseTaskStatus { + bool found{false}; + int state{0}; + int type{0}; + double progress{0.0}; + std::string detail; + }; + + struct PoseTaskContext { + std::string task_id; + math::Pose2d target{}; + double reach_distance{0.0}; + double reach_angle{0.0}; + std::uint64_t navigation_generation{0}; + std::uint64_t controller_fault_sequence_at_start{0}; + std::uint64_t control_attempt_sequence_at_start{0}; + std::uint64_t controller_fault_channel_epoch_at_start{0}; + }; + AgvResult connect_(); AgvResult disconnect_(); AgvResult connectSocket_(int& sock, int port); @@ -86,13 +106,37 @@ private: AgvResult acquireControl_() const; AgvResult confirmPoseNavigationStarted_( + const PoseTaskContext& context) const; + AgvResult queryPoseTaskStatus_( const std::string& task_id, - std::uint64_t navigation_generation) const; + PoseTaskStatus& status) const; + bool poseTargetReached_( + const PoseTaskContext& context, + std::string& detail) const; + std::string cachedControllerFaultDetail_( + std::uint64_t after_sequence = 0, + int wait_ms = 0, + std::uint64_t* associated_control_attempt = nullptr) const; + int controllerFaultCaptureGraceMs_() const; + int controllerFaultStateMaxAgeMs_() const; + std::string freeNavigationFaultStateUnavailableDetail_() const; + void rememberPoseTask_(const PoseTaskContext& context) const; + void advancePoseTaskGeneration_( + std::uint64_t navigation_generation, + std::uint64_t control_attempt_sequence) const; + void advancePoseTaskControlAttempt_( + std::uint64_t control_attempt_sequence) const; + void clearPoseTask_(std::uint64_t navigation_generation) const; + bool currentPoseTask_(PoseTaskContext& context) const; AgvResult sendControlledCommand_(int sock, std::uint16_t command, const Json::Value& payload, Json::Value* response, - std::uint64_t* accepted_navigation_generation = nullptr) const; + std::uint64_t* accepted_navigation_generation = nullptr, + 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; AgvResult sendCommand_(int sock, std::uint16_t command, const Json::Value& payload, @@ -106,6 +150,7 @@ private: void startPushThread_(); void stopPushThread_(); void pushLoop_(); + void invalidateControllerFaultState_(); AgvRuntimeState queryRuntimeState_() const; void updateCachedRuntimeState_(const Json::Value& payload); void startMapUpdateThread_(); @@ -168,6 +213,10 @@ private: mutable std::mutex control_sequence_mutex_; mutable std::atomic navigation_generation_{0}; mutable std::atomic pose_task_sequence_{0}; + mutable std::atomic control_attempt_sequence_{0}; + mutable std::atomic controller_fault_channel_epoch_{0}; + mutable std::mutex pose_task_mutex_; + mutable PoseTaskContext pose_task_context_; mutable int sock_status_{-1}; mutable int sock_control_{-1}; mutable int sock_navigation_{-1}; @@ -179,8 +228,16 @@ private: std::atomic push_running_{false}; std::thread push_thread_; mutable std::mutex runtime_state_mutex_; + mutable std::condition_variable runtime_state_cv_; AgvRuntimeState cached_runtime_state_; bool cached_runtime_state_valid_{false}; + std::uint64_t controller_fault_sequence_{0}; + bool controller_fault_state_observed_{false}; + std::chrono::steady_clock::time_point controller_fault_state_observed_at_{}; + std::string active_controller_fault_detail_; + double last_controller_fault_timestamp_{0.0}; + std::string last_controller_fault_detail_; + std::uint64_t last_controller_fault_control_attempt_{0}; mutable std::atomic map_update_running_{false}; mutable std::thread map_update_thread_; diff --git a/cmvr-es/devices/agv/src1100/src/src1100_agv.cpp b/cmvr-es/devices/agv/src1100/src/src1100_agv.cpp index 6c9f5ff8..d523c80d 100644 --- a/cmvr-es/devices/agv/src1100/src/src1100_agv.cpp +++ b/cmvr-es/devices/agv/src1100/src/src1100_agv.cpp @@ -54,6 +54,16 @@ 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; @@ -168,6 +178,11 @@ double nowSeconds() 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; @@ -370,6 +385,54 @@ AgvTaskType toTaskType(const int value) } } +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) @@ -429,28 +492,48 @@ bool Src1100Agv::update() 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_) { - auto state = cached_runtime_state_; - state.connected = connected_(); - state.last_error = last_error_; - if (!state.connected) { - state.mode = AgvMode::Disconnected; - } - return state; + 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; - state.connected = connected_(); + { + std::lock_guard lock(mutex_); + state.connected = connected_(); + state.last_error = last_error_; + } state.mode = state.connected ? AgvMode::Idle : AgvMode::Disconnected; - state.last_error = last_error_; Json::Value loc; if (sendCommand_(sock_status_, kRobotStatusLoc, Json::Value(Json::objectValue), &loc).ok()) { @@ -485,6 +568,248 @@ AgvRuntimeState Src1100Agv::queryRuntimeState_() const 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; @@ -492,19 +817,33 @@ AgvNavigationStatus Src1100Agv::navigationStatus() const const auto result = sendCommand_(sock_status_, kRobotStatusTask, payload, &response); if (!result.ok()) { status.state = AgvTaskState::Failed; - status.message = result.message; + 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 = controller_result.message; + 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(); } @@ -513,6 +852,9 @@ AgvNavigationStatus Src1100Agv::navigationStatus() const AgvResult Src1100Agv::connect_() { + const auto lifecycle_generation = + navigation_generation_.fetch_add(1, std::memory_order_relaxed) + 1; + clearPoseTask_(lifecycle_generation); stopPushThread_(); stopMapUpdateThread_(); @@ -592,6 +934,9 @@ AgvResult Src1100Agv::connect_() 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_); @@ -612,6 +957,10 @@ AgvResult Src1100Agv::emergencyStop() // 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; @@ -661,6 +1010,10 @@ AgvResult Src1100Agv::emergencyStop() 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() @@ -702,10 +1055,24 @@ AgvResult Src1100Agv::navigateToPose( 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. Keep the required station - // identifiers non-empty even though the controller ignores id when freeGo - // is present. + // 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"; @@ -715,19 +1082,23 @@ AgvResult Src1100Agv::navigateToPose( AgvErrorCode::InvalidArgument, "SRC1100 free navigation source_id must be SELF_POSITION"); } - std::string target_id = adapter_params.getString("target_id").value_or("SELF_POSITION"); - if (target_id.empty()) { - target_id = "SELF_POSITION"; - } - if (target_id != "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 SELF_POSITION so a " + "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; - const auto skill_name = adapter_params.getString("skill_name"); - if (skill_name && !skill_name->empty() && *skill_name != "GotoSpecifiedPose") { + 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"); @@ -735,17 +1106,19 @@ AgvResult Src1100Agv::navigateToPose( const auto task_sequence = pose_task_sequence_.fetch_add(1, std::memory_order_relaxed) + 1; - const std::string task_id_prefix = + std::string task_id_prefix = adapter_params.getString("task_id").value_or(id_); - const std::string task_id = makePoseTaskId(task_id_prefix, task_sequence); + 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; - if (skill_name && !skill_name->empty()) { - jsonMember(payload, "skill_name") = *skill_name; - } + jsonMember(payload, "skill_name") = skill_name; auto& free_go = jsonMember(payload, "freeGo"); jsonMember(free_go, "x") = pose.x; @@ -757,6 +1130,16 @@ AgvResult Src1100Agv::navigateToPose( // 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_( @@ -764,11 +1147,15 @@ AgvResult Src1100Agv::navigateToPose( kRobotTaskGoTarget, payload, &response, - &navigation_generation); + &navigation_generation, + nullptr, + nullptr, + &context, + true); if (!result.ok()) { return result; } - return confirmPoseNavigationStarted_(task_id, navigation_generation); + return confirmPoseNavigationStarted_(context); } AgvResult Src1100Agv::navigateToStation( @@ -776,6 +1163,22 @@ AgvResult Src1100Agv::navigateToStation( 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; @@ -785,12 +1188,16 @@ AgvResult Src1100Agv::navigateToStation( applyMotionOptions_(payload, options); Json::Value response; std::uint64_t accepted_generation = 0; - return sendControlledCommand_( + 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) @@ -808,64 +1215,121 @@ AgvResult Src1100Agv::followPath(const std::vector& path) jsonMember(payload, "move_task_list") = tasks; Json::Value response; std::uint64_t accepted_generation = 0; - return sendControlledCommand_( + 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; - return sendControlledCommand_( + std::uint64_t control_attempt_sequence = 0; + auto result = sendControlledCommand_( sock_navigation_, kRobotTaskPause, Json::Value(Json::objectValue), &response, - &accepted_generation); + &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; - return sendControlledCommand_( + std::uint64_t control_attempt_sequence = 0; + auto result = sendControlledCommand_( sock_navigation_, kRobotTaskResume, Json::Value(Json::objectValue), &response, - &accepted_generation); + &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; - return sendControlledCommand_( + 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; - return sendControlledCommand_( + 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 @@ -908,8 +1372,17 @@ AgvResult Src1100Agv::switchMap(const std::string& map_name) Json::Value payload(Json::objectValue); jsonMember(payload, "map_name") = map_name; Json::Value response; - auto result = sendControlledCommand_(sock_control_, kRobotControlLoadMap, payload, &response); - return result.ok() ? resultFromResponse_(response) : result; + 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) @@ -946,8 +1419,16 @@ AgvResult Src1100Agv::startMapping(const AgvMappingOptions& options) } Json::Value response; - result = sendControlledCommand_(sock_other_, kRobotOtherStartMapping, payload, &response); - result = result.ok() ? resultFromResponse_(response) : result; + 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_); @@ -1658,12 +2139,17 @@ AgvResult Src1100Agv::stopMapping() if (!result.ok()) return result; Json::Value response; + std::uint64_t accepted_generation = 0; result = sendControlledCommand_( sock_other_, kRobotOtherStopMapping, Json::Value(Json::objectValue), - &response); - return result.ok() ? resultFromResponse_(response) : result; + &response, + &accepted_generation); + if (accepted_generation != 0) { + clearPoseTask_(accepted_generation); + } + return result; } AgvResult Src1100Agv::connectSocket_(int& sock, const int port) @@ -1728,42 +2214,399 @@ AgvResult Src1100Agv::acquireControl_() const return result.ok() ? resultFromResponse_(response) : result; } -AgvResult Src1100Agv::confirmPoseNavigationStarted_( - const std::string& task_id, +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 cached_fault_detail = [this]() { - std::lock_guard lock(runtime_state_mutex_); - if (!cached_runtime_state_valid_ || !cached_runtime_state_.fault) { - return std::string{}; - } - return cached_runtime_state_.last_error; + 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 (navigation_generation_.load(std::memory_order_relaxed) != navigation_generation) { - 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"); + if (superseded()) { + return superseded_result(); + } + if (const std::string unavailable = + fault_monitoring_unavailable(); + !unavailable.empty()) { + return fault_monitoring_unavailable_result(unavailable); } - 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; - const auto query_result = sendCommand_( - sock_status_, - kRobotStatusTaskPackage, - payload, - &response); + 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" @@ -1775,94 +2618,275 @@ AgvResult Src1100Agv::confirmPoseNavigationStarted_( + "; do not retry automatically before checking or canceling navigation"); } - const auto controller_result = resultFromResponse_(response); - if (!controller_result.ok()) { - return AgvResult::failure( - controller_result.code, - "SRC1100 accepted the free-navigation command, but task status " - "query failed: " + controller_result.message); + if (superseded()) { + return superseded_result(); + } + if (const std::string unavailable = + fault_monitoring_unavailable(); + !unavailable.empty()) { + return fault_monitoring_unavailable_result(unavailable); } - if (navigation_generation_.load(std::memory_order_relaxed) != navigation_generation) { - 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"); - } + 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); + } - const auto* package = jsonFind(response, "task_status_package"); - const auto* status_list = package ? jsonFind(*package, "task_status_list") : nullptr; - bool matching_task_found = false; - if (status_list && status_list->isArray()) { - for (const auto& item : *status_list) { - if (jsonGet(item, "task_id", "").asString() != task_id) { - continue; + 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; } - matching_task_found = true; - const int task_state = jsonGet(item, "status", 0).asInt(); - const int task_type = jsonGet(item, "type", 0).asInt(); - last_status = "task_id=" + task_id - + ", task_status=" + std::to_string(task_state) - + ", task_type=" + std::to_string(task_type); - if (package) { - const std::string info = jsonGet(*package, "info", "").asString(); - if (!info.empty()) { - last_status += ", info=" + info; + ++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); } - } - - if (task_type != 1) { return AgvResult::failure( - AgvErrorCode::TaskRejected, - "SRC1100 created an unexpected task type for free navigation: " - + last_status); + 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 (task_state == 1 || task_state == 2 || task_state == 4) { + 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(); } - if (task_state == 3) { - 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"); + } 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 (task_state == 5 || task_state == 6) { - std::string detail = last_status; - const std::string fault = cached_fault_detail(); - if (!fault.empty()) { - detail += ", " + fault; + if (!fault.empty()) { + if (fault_attribution_is_ambiguous( + fault_control_attempt)) { + return ambiguous_fault_result(last_status, fault); } return AgvResult::failure( - task_state == 5 ? AgvErrorCode::TaskFailed : AgvErrorCode::TaskCanceled, - task_state == 5 - ? "SRC1100 free-navigation task failed: " + detail - : "SRC1100 free-navigation task was canceled: " + detail); + 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"); } - break; + 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 (!matching_task_found) { - last_status = "task_id=" + task_id + " not present in task_status_package"; + 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; } - if (std::chrono::steady_clock::now() >= deadline) { + 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; - const std::string fault = cached_fault_detail(); + 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 matching pose " - "task was established within " + "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"); @@ -1873,12 +2897,28 @@ AgvResult Src1100Agv::sendControlledCommand_( const std::uint16_t command, const Json::Value& payload, Json::Value* response, - std::uint64_t* accepted_navigation_generation) const + 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; @@ -1886,14 +2926,79 @@ AgvResult Src1100Agv::sendControlledCommand_( 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. - *accepted_navigation_generation = - navigation_generation_.fetch_add(1, std::memory_order_relaxed) + 1; + publish_navigation_generation(); return withUnknownControllerOutcome(std::move(result)); } return result; @@ -1902,15 +3007,13 @@ AgvResult Src1100Agv::sendControlledCommand_( return result; } if (!response) { - *accepted_navigation_generation = - navigation_generation_.fetch_add(1, std::memory_order_relaxed) + 1; + publish_navigation_generation(); return withUnknownControllerOutcome(AgvResult::failure( AgvErrorCode::CommandFailed, "SRC1100 cannot confirm navigation command without a response")); } if (!hasNumericControllerRetCode(*response)) { - *accepted_navigation_generation = - navigation_generation_.fetch_add(1, std::memory_order_relaxed) + 1; + publish_navigation_generation(); return withUnknownControllerOutcome(resultFromResponse_(*response)); } result = resultFromResponse_(*response); @@ -1922,8 +3025,7 @@ AgvResult Src1100Agv::sendControlledCommand_( // 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. - *accepted_navigation_generation = - navigation_generation_.fetch_add(1, std::memory_order_relaxed) + 1; + publish_navigation_generation(); return result; } @@ -2158,6 +3260,7 @@ void Src1100Agv::stopPushThread_() if (push_thread_.joinable()) { push_thread_.join(); } + invalidateControllerFaultState_(); } void Src1100Agv::pushLoop_() @@ -2181,8 +3284,10 @@ void Src1100Agv::pushLoop_() } if (!result.ok()) { if (result.code != AgvErrorCode::Timeout) { + invalidateControllerFaultState_(); std::lock_guard lock(mutex_); last_error_ = result.message; + closeSocket_(sock_push_); } continue; } @@ -2193,6 +3298,7 @@ void Src1100Agv::pushLoop_() Json::Value parsed; std::string error; if (!parseJson_(payload, parsed, error)) { + invalidateControllerFaultState_(); std::lock_guard lock(mutex_); last_error_ = error; continue; @@ -2201,13 +3307,24 @@ void Src1100Agv::pushLoop_() } } +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; - state.last_error.clear(); 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(); @@ -2244,17 +3361,65 @@ void Src1100Agv::updateCachedRuntimeState_(const Json::Value& payload) } state.moving = std::hypot(state.velocity.vx, state.velocity.vy) > 1e-4 || std::abs(state.velocity.wz) > 1e-4; - state.fault = hasFaultArray(payload, "fatals") || hasFaultArray(payload, "errors"); - if (state.fault) { - std::ostringstream detail; - detail << "SRC1100 controller fault"; - if (const auto* fatals = jsonFind(payload, "fatals"); fatals && !fatals->empty()) { - detail << ": fatals=" << jsonValueToString(*fatals); + 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_ = {}; } - if (const auto* errors = jsonFind(payload, "errors"); errors && !errors->empty()) { - detail << ": errors=" << jsonValueToString(*errors); + + 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(); } - state.last_error = detail.str(); } if (state.emergency_stopped) { state.mode = AgvMode::EmergencyStop; @@ -2270,6 +3435,7 @@ void Src1100Agv::updateCachedRuntimeState_(const Json::Value& payload) cached_runtime_state_ = state; cached_runtime_state_valid_ = true; + runtime_state_cv_.notify_all(); } std::vector Src1100Agv::buildFrame_( 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 index 5d7b4712..da0034ea 100644 --- a/cmvr-es/devices/agv/src1100/tests/src1100_control_authority_test.cpp +++ b/cmvr-es/devices/agv/src1100/tests/src1100_control_authority_test.cpp @@ -7,6 +7,7 @@ #include #include #include +#include #include #include #include @@ -48,6 +49,39 @@ public: 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) @@ -70,6 +104,7 @@ public: 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; @@ -416,6 +451,8 @@ protected: 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]})"); @@ -435,6 +472,16 @@ protected: 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 @@ -487,9 +534,17 @@ protected: TEST_F(Src1100ControlAuthorityTest, EveryImplementedMutatingOperationAcquiresAuthorityFirst) { - expectControlledSequence({kRobotTaskGoTarget, kRobotStatusTaskPackage}, [this]() { - return agv_->navigateToPose(math::Pose2d{1.0, 2.0, 0.5}); - }); + 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"); }); @@ -604,7 +659,9 @@ TEST_F(Src1100ControlAuthorityTest, ReadOnlyMapDownloadDoesNotAcquireAuthority) EXPECT_EQ(records[0].command, kRobotConfigDownloadMap); } -TEST_F(Src1100ControlAuthorityTest, NavigateToPoseUsesFreeGoWithTypedMotionLimits) +TEST_F( + Src1100ControlAuthorityTest, + NavigateToPoseUsesLegacyCompatibleFreeGoPayloadWithTypedMotionLimits) { AgvMotionOptions options; options.max_speed = 0.6; @@ -621,14 +678,16 @@ TEST_F(Src1100ControlAuthorityTest, NavigateToPoseUsesFreeGoWithTypedMotionLimit ASSERT_TRUE(result.ok()) << result.message; const auto records = controller_.records(); - ASSERT_EQ(records.size(), 3U); + ASSERT_GE(records.size(), 4U); EXPECT_EQ(records[0].command, kRobotConfigLock); EXPECT_EQ(records[1].command, kRobotTaskGoTarget); - EXPECT_EQ(records[2].command, kRobotStatusTaskPackage); + 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(), "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()); @@ -643,7 +702,9 @@ TEST_F(Src1100ControlAuthorityTest, NavigateToPoseUsesFreeGoWithTypedMotionLimit 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_FALSE(payloadHas(payload, "skill_name")); + EXPECT_EQ( + payloadValue(payload, "skill_name").asString(), + "GotoSpecifiedPose"); EXPECT_FALSE(payloadHas(payload, "x")); EXPECT_FALSE(payloadHas(payload, "y")); EXPECT_FALSE(payloadHas(payload, "angle")); @@ -657,9 +718,19 @@ TEST_F(Src1100ControlAuthorityTest, NavigateToPoseUsesFreeGoWithTypedMotionLimit 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, NavigateToPoseWhitelistsAdapterFieldsAndKeepsOriginFreeGo) +TEST_F( + Src1100ControlAuthorityTest, + NavigateToPoseUsesExplicitTaskIdAsUniquePrefixAndWhitelistsAdapterFields) { controller_.setResponsePayload( kRobotStatusTaskPackage, @@ -682,15 +753,21 @@ TEST_F(Src1100ControlAuthorityTest, NavigateToPoseWhitelistsAdapterFieldsAndKeep ASSERT_TRUE(result.ok()) << result.message; const auto records = controller_.records(); - ASSERT_EQ(records.size(), 3U); + ASSERT_GE(records.size(), 4U); EXPECT_EQ(records[0].command, kRobotConfigLock); EXPECT_EQ(records[1].command, kRobotTaskGoTarget); - EXPECT_EQ(records[2].command, kRobotStatusTaskPackage); + 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(), "SELF_POSITION"); - EXPECT_EQ(payloadValue(payload, "task_id").asString().find("pose-task_pose_"), 0U); + 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); @@ -700,6 +777,20 @@ TEST_F(Src1100ControlAuthorityTest, NavigateToPoseWhitelistsAdapterFieldsAndKeep 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) @@ -729,7 +820,7 @@ TEST_F(Src1100ControlAuthorityTest, NavigateToPoseRejectsUnsafeFallbackStationAn EXPECT_FALSE(result.ok()); EXPECT_EQ(result.code, AgvErrorCode::InvalidArgument); - EXPECT_NE(result.message.find("target_id must be SELF_POSITION"), std::string::npos); + EXPECT_NE(result.message.find("target_id must be empty"), std::string::npos); EXPECT_TRUE(controller_.records().empty()); adapter_params.values.clear(); @@ -747,6 +838,71 @@ TEST_F(Src1100ControlAuthorityTest, NavigateToPoseRejectsUnsafeFallbackStationAn 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( @@ -761,13 +917,644 @@ TEST_F(Src1100ControlAuthorityTest, NavigateToPoseIgnoresUncorrelatedStatusUntil ASSERT_TRUE(result.ok()) << result.message; const auto records = controller_.records(); - ASSERT_EQ(records.size(), 4U); + 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( @@ -866,22 +1653,97 @@ TEST_F(Src1100ControlAuthorityTest, NavigateToPoseReportsPausedTaskExplicitly) EXPECT_NE(result.message.find("do not retry automatically"), std::string::npos); } -TEST_F(Src1100ControlAuthorityTest, NavigateToPoseIncludesCachedControllerFaultCodes) +TEST_F( + Src1100ControlAuthorityTest, + NavigateToPosePreservesControllerFaultWhenTaskImmediatelyPauses) { - Json::Value push_payload(Json::objectValue); + 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); - 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); + 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}]}})"); - controller_.clearRecords(); - - const auto result = agv_->navigateToPose(math::Pose2d{1.0, 2.0, 0.5}); + pose_thread.join(); EXPECT_FALSE(result.ok()); EXPECT_EQ(result.code, AgvErrorCode::TaskFailed); @@ -889,6 +1751,165 @@ TEST_F(Src1100ControlAuthorityTest, NavigateToPoseIncludesCachedControllerFaultC 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( @@ -955,7 +1976,7 @@ TEST_F(Src1100ControlAuthorityTest, FailedCancelDoesNotSupersedePoseStartConfirm } std::this_thread::sleep_for(std::chrono::milliseconds(1)); } - ASSERT_TRUE(status_query_observed); + EXPECT_TRUE(status_query_observed); controller_.setResponseCode(kRobotConfigLock, 17); const auto authority_failure = agv_->cancelNavigation(); @@ -1290,6 +2311,419 @@ TEST_F(Src1100ControlAuthorityTest, ControllerErrorCodeIsPreservedInResultMessag 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); @@ -1300,6 +2734,9 @@ TEST_F(Src1100ControlAuthorityTest, NavigationStatusPreservesControllerErrorCode 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 From 52d30ee41232268922eec2da72f5bcdebeef2458 Mon Sep 17 00:00:00 2001 From: xtkuang <87661715@qq.com> Date: Mon, 3 Aug 2026 15:05:04 +0800 Subject: [PATCH 09/20] feat: extend and refactor SEER Robokit AGV backend --- cmvr-es/common/types/agv/agv_types.h | 11 +- .../{src1100.pb.txt => seer_robokit.pb.txt} | 3 +- cmvr-es/config/manager/device_manager.pb.txt | 3 +- cmvr-es/devices/README.md | 2 +- cmvr-es/devices/agv/CMakeLists.txt | 4 +- cmvr-es/devices/agv/abstract_agv.h | 16 +- cmvr-es/devices/agv/agv_factory.h | 10 +- .../devices/agv/seer_robokit/CMakeLists.txt | 57 + cmvr-es/devices/agv/seer_robokit/README.md | 394 ++ .../include/seer_robokit_agv.h} | 121 +- .../include/seer_robokit_navigation_utils.h | 257 + .../include/seer_robokit_pgv_utils.h | 141 + .../include/seer_robokit_protocol.h | 38 + .../seer_robokit/include/seer_robokit_utils.h | 92 + .../agv/seer_robokit/src/seer_robokit_agv.cpp | 175 + .../seer_robokit/src/seer_robokit_control.cpp | 378 ++ .../agv/seer_robokit/src/seer_robokit_map.cpp | 1016 ++++ .../src/seer_robokit_navigation.cpp | 1378 +++++ .../src/seer_robokit_navigation_wait.cpp | 1554 +++++ .../seer_robokit/src/seer_robokit_status.cpp | 725 +++ .../src/seer_robokit_transport.cpp | 362 ++ .../seer_robokit_control_authority_test.cpp | 5403 +++++++++++++++++ cmvr-es/devices/agv/src1100/CMakeLists.txt | 40 - .../devices/agv/src1100/src/src1100_agv.cpp | 3604 ----------- .../tests/src1100_control_authority_test.cpp | 2743 --------- cmvr-es/service/grpc/src/grpc_agv_service.cpp | 64 +- .../grpc/tests/grpc_agv_service_test.cpp | 203 +- .../tests/quic_edge_protocol_test.cpp | 2 +- protos/cmvr/api/agv_command.proto | 6 +- .../cmvr/config/agv_config/agv_config.proto | 12 +- protos/cmvr/msgs/agv.proto | 8 +- ...0_map3d.proto => seer_robokit_map3d.proto} | 2 +- 32 files changed, 12384 insertions(+), 6440 deletions(-) rename cmvr-es/config/devices/agv/{src1100.pb.txt => seer_robokit.pb.txt} (91%) create mode 100644 cmvr-es/devices/agv/seer_robokit/CMakeLists.txt create mode 100644 cmvr-es/devices/agv/seer_robokit/README.md rename cmvr-es/devices/agv/{src1100/include/src1100_agv.h => seer_robokit/include/seer_robokit_agv.h} (67%) create mode 100644 cmvr-es/devices/agv/seer_robokit/include/seer_robokit_navigation_utils.h create mode 100644 cmvr-es/devices/agv/seer_robokit/include/seer_robokit_pgv_utils.h create mode 100644 cmvr-es/devices/agv/seer_robokit/include/seer_robokit_protocol.h create mode 100644 cmvr-es/devices/agv/seer_robokit/include/seer_robokit_utils.h create mode 100644 cmvr-es/devices/agv/seer_robokit/src/seer_robokit_agv.cpp create mode 100644 cmvr-es/devices/agv/seer_robokit/src/seer_robokit_control.cpp create mode 100644 cmvr-es/devices/agv/seer_robokit/src/seer_robokit_map.cpp create mode 100644 cmvr-es/devices/agv/seer_robokit/src/seer_robokit_navigation.cpp create mode 100644 cmvr-es/devices/agv/seer_robokit/src/seer_robokit_navigation_wait.cpp create mode 100644 cmvr-es/devices/agv/seer_robokit/src/seer_robokit_status.cpp create mode 100644 cmvr-es/devices/agv/seer_robokit/src/seer_robokit_transport.cpp create mode 100644 cmvr-es/devices/agv/seer_robokit/tests/seer_robokit_control_authority_test.cpp delete mode 100644 cmvr-es/devices/agv/src1100/CMakeLists.txt delete mode 100644 cmvr-es/devices/agv/src1100/src/src1100_agv.cpp delete mode 100644 cmvr-es/devices/agv/src1100/tests/src1100_control_authority_test.cpp rename protos/rbk/protocol/{src1100_map3d.proto => seer_robokit_map3d.proto} (97%) 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 作为未知字段跳过。 // 地图坐标系下的三维位置，单位：米。 From 490c141ca8a844bc98c422dc266b427defe68311 Mon Sep 17 00:00:00 2001 From: xtkuang <87661715@qq.com> Date: Mon, 3 Aug 2026 15:56:30 +0800 Subject: [PATCH 10/20] refactor(proto): move AGV messages under API --- cmvr-es/devices/agv/seer_robokit/README.md | 3 ++- protos/cmvr/api/agv_command.proto | 2 +- protos/cmvr/{msgs/agv.proto => api/agv_utils.proto} | 2 ++ 3 files changed, 5 insertions(+), 2 deletions(-) rename protos/cmvr/{msgs/agv.proto => api/agv_utils.proto} (98%) diff --git a/cmvr-es/devices/agv/seer_robokit/README.md b/cmvr-es/devices/agv/seer_robokit/README.md index bd0b9460..b2ee3d00 100644 --- a/cmvr-es/devices/agv/seer_robokit/README.md +++ b/cmvr-es/devices/agv/seer_robokit/README.md @@ -43,7 +43,8 @@ [`../../../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) + [`../../../../protos/cmvr/api/agv_command.proto`](../../../../protos/cmvr/api/agv_command.proto)、 + [`../../../../protos/cmvr/api/agv_utils.proto`](../../../../protos/cmvr/api/agv_utils.proto) ## 配置和启动 diff --git a/protos/cmvr/api/agv_command.proto b/protos/cmvr/api/agv_command.proto index da4414ab..81c7676d 100644 --- a/protos/cmvr/api/agv_command.proto +++ b/protos/cmvr/api/agv_command.proto @@ -3,7 +3,7 @@ syntax = "proto3"; package cmvr.api; import "cmvr/api/common.proto"; -import "cmvr/msgs/agv.proto"; +import "cmvr/api/agv_utils.proto"; // 查询 AGV 运行状态命令。 message AgvRuntimeStateCommand { diff --git a/protos/cmvr/msgs/agv.proto b/protos/cmvr/api/agv_utils.proto similarity index 98% rename from protos/cmvr/msgs/agv.proto rename to protos/cmvr/api/agv_utils.proto index ac1583c9..e3f697a9 100644 --- a/protos/cmvr/msgs/agv.proto +++ b/protos/cmvr/api/agv_utils.proto @@ -1,5 +1,7 @@ syntax = "proto3"; +// 文件随 AGV API 定义统一放在 cmvr/api 下，但保留 cmvr.msgs package， +// 以兼容既有生成代码、消息完整名称和 Any type URL。 package cmvr.msgs; // AGV 在地图平面坐标系中的二维位姿。 From b4ec07c8512b2620534a961512fa5c896fe7d8e0 Mon Sep 17 00:00:00 2001 From: xtkuang <87661715@qq.com> Date: Tue, 4 Aug 2026 09:52:51 +0800 Subject: [PATCH 11/20] feat: add device inventory and move arm JSON RPC --- CMakeLists.txt | 5 +- cmvr-es/devices/arm/aubo_arm/README.md | 16 +- cmvr-es/service/CMakeLists.txt | 57 +++ cmvr-es/service/README.md | 24 +- .../service/grpc/include/grpc_arm_service.h | 3 + .../grpc/include/grpc_system_service.h | 2 +- cmvr-es/service/grpc/src/grpc_arm_service.cpp | 34 ++ .../service/grpc/src/grpc_system_service.cpp | 177 ++++++-- .../grpc/tests/grpc_arm_service_test.cpp | 378 ++++++++++++++++++ .../grpc/tests/grpc_system_service_test.cpp | 291 ++++++++++++++ protos/cmvr/api/arm_service.proto | 3 + protos/cmvr/api/system_command.proto | 73 +++- protos/cmvr/api/system_service.proto | 3 +- 13 files changed, 1019 insertions(+), 47 deletions(-) create mode 100644 cmvr-es/service/grpc/tests/grpc_arm_service_test.cpp create mode 100644 cmvr-es/service/grpc/tests/grpc_system_service_test.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index bb267040..961b77e2 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -70,8 +70,9 @@ file(GLOB_RECURSE PROTO_FILES ${PROTO_IMPORT_DIR}/*.proto) set(Protobuf_PROTOC_EXECUTABLE "${CMAKE_INSTALL_PREFIX}/bin/protoc" CACHE FILEPATH "" FORCE) -set_property(TARGET gRPC::grpc_cpp_plugin - PROPERTY IMPORTED_LOCATION "${CMAKE_INSTALL_PREFIX}/bin/grpc_cpp_plugin" +set_target_properties(gRPC::grpc_cpp_plugin PROPERTIES + IMPORTED_LOCATION "${CMAKE_INSTALL_PREFIX}/bin/grpc_cpp_plugin" + IMPORTED_LOCATION_RELEASE "${CMAKE_INSTALL_PREFIX}/bin/grpc_cpp_plugin" ) # 1) 先做 OBJECT：只负责生成/编译 pb.cc diff --git a/cmvr-es/devices/arm/aubo_arm/README.md b/cmvr-es/devices/arm/aubo_arm/README.md index 6d63e6b7..0045108b 100644 --- a/cmvr-es/devices/arm/aubo_arm/README.md +++ b/cmvr-es/devices/arm/aubo_arm/README.md @@ -2,8 +2,10 @@ `AuboArm` 是 AUBO SDK v0.27.1 的 `RobotArm` 后端。控制柜 Standard 数字 IO 通过设备通用的 `executeJsonCommand` 接口访问，远程调用复用 -`cmvr.api.SystemService/ExecuteJsonCommand`，不经过 `ArmService` 或 -`MotorService`。 +`cmvr.api.ArmService/ExecuteJsonCommand`，不经过 `SystemService` 或 +`MotorService`。该 RPC 只路由到 `RobotArm`，不会把 JSON 命令转发给其他设备类型。 +旧的 `cmvr.api.SystemService/ExecuteJsonCommand` 不再注册，调用方必须更新服务路径； +请求和响应消息结构保持不变。 返回 [Devices 模块指南](../../README.md) 或 [项目总览](../../../../README.md)。 @@ -14,9 +16,9 @@ - 设备配置：[`../../../config/devices/arm/aubo_arm.pb.txt`](../../../config/devices/arm/aubo_arm.pb.txt) - DeviceManager 配置： [`../../../config/manager/device_manager.pb.txt`](../../../config/manager/device_manager.pb.txt) -- SystemService 实现： - [`../../../service/grpc/src/grpc_system_service.cpp`](../../../service/grpc/src/grpc_system_service.cpp) -- Proto：[`../../../../protos/cmvr/api/system_service.proto`](../../../../protos/cmvr/api/system_service.proto) +- ArmService 实现： + [`../../../service/grpc/src/grpc_arm_service.cpp`](../../../service/grpc/src/grpc_arm_service.cpp) +- Proto：[`../../../../protos/cmvr/api/arm_service.proto`](../../../../protos/cmvr/api/arm_service.proto) 仓库配置使用 SDK RPC 端口 `30004`。现场部署必须填写真实控制器地址和凭据， 不要把生产密码提交到默认配置。 @@ -68,7 +70,7 @@ grpcurl -plaintext \ "requestJson":"{\"command\":\"cabinet_io\",\"operation\":\"get_di\",\"index\":0}" }' \ 127.0.0.1:50052 \ - cmvr.api.SystemService/ExecuteJsonCommand + cmvr.api.ArmService/ExecuteJsonCommand ``` 设置 DO0 为高电平： @@ -80,7 +82,7 @@ grpcurl -plaintext \ "requestJson":"{\"command\":\"cabinet_io\",\"operation\":\"set_do\",\"index\":0,\"value\":true}" }' \ 127.0.0.1:50052 \ - cmvr.api.SystemService/ExecuteJsonCommand + cmvr.api.ArmService/ExecuteJsonCommand ``` 使用源码默认配置时： diff --git a/cmvr-es/service/CMakeLists.txt b/cmvr-es/service/CMakeLists.txt index adccab30..9f54e4df 100644 --- a/cmvr-es/service/CMakeLists.txt +++ b/cmvr-es/service/CMakeLists.txt @@ -47,6 +47,63 @@ if(BUILD_TESTING) ) set_tests_properties(grpc_camera_stream_policy_test PROPERTIES TIMEOUT 10) + add_executable(grpc_system_service_test + grpc/tests/grpc_system_service_test.cpp + ) + target_include_directories(grpc_system_service_test + PRIVATE + ${CMAKE_SOURCE_DIR}/cmvr-es + ${CMAKE_SOURCE_DIR}/cmvr-es/manager/device_manager + ) + target_link_libraries(grpc_system_service_test + PRIVATE + service + cmvr_es::proto + gtest + gtest_main + pthread + ) + add_test( + NAME grpc_system_service_test + COMMAND grpc_system_service_test + ) + set(_grpc_system_test_environment + "LD_LIBRARY_PATH=${CMVR_TEST_EXTERNAL_LIBRARY_PATH}" + ) + if(CMVR_TEST_SYSTEM_LIBSTDCXX) + list(APPEND _grpc_system_test_environment + "LD_PRELOAD=${CMVR_TEST_SYSTEM_LIBSTDCXX}") + endif() + set_tests_properties(grpc_system_service_test PROPERTIES + TIMEOUT 10 + ENVIRONMENT "${_grpc_system_test_environment}" + ) + + add_executable(grpc_arm_service_test + grpc/tests/grpc_arm_service_test.cpp + ) + target_include_directories(grpc_arm_service_test + PRIVATE + ${CMAKE_SOURCE_DIR}/cmvr-es + ${CMAKE_SOURCE_DIR}/cmvr-es/manager/device_manager + ) + target_link_libraries(grpc_arm_service_test + PRIVATE + service + cmvr_es::proto + gtest + gtest_main + pthread + ) + add_test( + NAME grpc_arm_service_test + COMMAND grpc_arm_service_test + ) + set_tests_properties(grpc_arm_service_test PROPERTIES + TIMEOUT 10 + ENVIRONMENT "${_grpc_system_test_environment}" + ) + add_executable(grpc_arm_teleop_service_test grpc/tests/grpc_arm_teleop_service_test.cpp ) diff --git a/cmvr-es/service/README.md b/cmvr-es/service/README.md index 59959741..49e053cc 100644 --- a/cmvr-es/service/README.md +++ b/cmvr-es/service/README.md @@ -21,6 +21,24 @@ gRPC 和 QUIC 的职责边界： - 实时音视频使用 QUIC DATAGRAM； - `quic_edge/` 不是平台 Gateway，也不是浏览器服务器。 +## SystemService 设备清单 + +`SystemService/GetDeviceList` 返回 `DeviceManager` 的当前只读快照，只包含 +`enabled=true` 的设备。启用但创建、初始化、启动或健康检查失败的设备仍会返回， +并通过 `manager_state`、`health`、`has_error` 和 `error_message` 描述异常。 +接口同时返回稳定的 `device_type` 和仅用于展示/诊断的具体 `type_name`；调用方 +不得使用 `type_name` 做设备类别判断。 + +该 RPC 不修改配置、不动态注册设备，也不触发设备生命周期操作。启用 reflection +后可直接查询： + +```bash +grpcurl -plaintext \ + -d '{}' \ + 127.0.0.1:50052 \ + cmvr.api.SystemService/GetDeviceList +``` + ## MotorService `MotorService` 将 gRPC 电机命令适配到已经由 `DeviceManager` 创建的 @@ -53,9 +71,11 @@ PLC 后端的连接 epoch、stream epoch、寄存器、ACK 和 TIA Portal 要求 - [Modbus TCP PLC runtime](../devices/motor/bus_runtime/modbus_tcp/README.md) - [MotorService 与 CMVR PLC v1 完整协议](../../docs/motor_service_modbus_tcp.md) -AUBO 控制柜 IO 不经过 `MotorService` 或 `ArmService`，而是复用 -`SystemService/ExecuteJsonCommand`。厂商命令和安全约束见 +AUBO 控制柜 IO 不经过 `MotorService`，由 +`ArmService/ExecuteJsonCommand` 转发到目标 `RobotArm`。厂商命令和安全约束见 [AUBO 控制柜 IO](../devices/arm/aubo_arm/README.md)。 +旧的 `SystemService/ExecuteJsonCommand` 已移除；相机 PTZ 应使用类型化的 +`CameraService/ControlPtz`。 ## 新增 gRPC Service diff --git a/cmvr-es/service/grpc/include/grpc_arm_service.h b/cmvr-es/service/grpc/include/grpc_arm_service.h index 07a2dc30..230e021e 100644 --- a/cmvr-es/service/grpc/include/grpc_arm_service.h +++ b/cmvr-es/service/grpc/include/grpc_arm_service.h @@ -50,6 +50,9 @@ public: grpc::Status computeForwardKinematics(grpc::ServerContext* context, const api::ComputeForwardKinematics_Request* request, api::ComputeForwardKinematics_Response* response) override; + grpc::Status ExecuteJsonCommand(grpc::ServerContext* context, + const api::JsonDeviceCommand_Request* request, + api::JsonDeviceCommand_Feedback* response) override; grpc::Status clearFault(grpc::ServerContext *context, const cmvr::api::CommandHeader_Request *request, cmvr::api::CommandHeader_Feedback *response) override; diff --git a/cmvr-es/service/grpc/include/grpc_system_service.h b/cmvr-es/service/grpc/include/grpc_system_service.h index 41fb2646..978ca2f4 100644 --- a/cmvr-es/service/grpc/include/grpc_system_service.h +++ b/cmvr-es/service/grpc/include/grpc_system_service.h @@ -17,8 +17,8 @@ namespace cmvr::service ~gRPCSystemServiceImpl() override = default; grpc::Status GetSystemInfo(grpc::ServerContext* context, const api::GetSystemInfoCommand_Request* request, api::GetSystemInfoCommand_Feedback* response) override; grpc::Status GetSystemStatus(grpc::ServerContext* context, const api::GetSystemStatusCommand_Request* request, api::GetSystemStatusCommand_Feedback* response) override; + grpc::Status GetDeviceList(grpc::ServerContext* context, const api::GetDeviceListCommand_Request* request, api::GetDeviceListCommand_Feedback* response) override; grpc::Status UpdateParams(grpc::ServerContext* context, const cmvr::api::UpdateParamsCommand_Request* request, cmvr::api::UpdateParamsCommand_Feedback* response) override; - grpc::Status ExecuteJsonCommand(grpc::ServerContext* context, const cmvr::api::JsonDeviceCommand_Request* request, cmvr::api::JsonDeviceCommand_Feedback* response) override; grpc::Status StopAll(grpc::ServerContext* context, const cmvr::api::StopAllCommand_Request* request, cmvr::api::StopAllCommand_Feedback* response) override; private: device::DeviceManager& dmgr_; diff --git a/cmvr-es/service/grpc/src/grpc_arm_service.cpp b/cmvr-es/service/grpc/src/grpc_arm_service.cpp index 6c3ca566..b1567f00 100644 --- a/cmvr-es/service/grpc/src/grpc_arm_service.cpp +++ b/cmvr-es/service/grpc/src/grpc_arm_service.cpp @@ -510,6 +510,40 @@ grpc::Status gRPCArmServiceImpl::computeForwardKinematics(grpc::ServerContext*, return grpc::Status(grpc::StatusCode::UNIMPLEMENTED, "computeForwardKinematics is not implemented"); } +grpc::Status gRPCArmServiceImpl::ExecuteJsonCommand( + grpc::ServerContext*, + const api::JsonDeviceCommand_Request* request, + api::JsonDeviceCommand_Feedback* response) +{ + try { + const std::string device_id = request->header().device_id(); + auto arm = dmgr_.getDevice(device_id); + if (!arm) { + fillFeedback( + response->mutable_header(), + false, + "Device not found: " + device_id); + return grpc::Status::OK; + } + + std::string response_json; + const bool success = arm->executeJsonCommand( + request->request_json(), response_json); + fillFeedback( + response->mutable_header(), + success, + success ? "" : response_json); + response->set_response_json(response_json); + if (success) { + logRpcSuccess("ExecuteJsonCommand", device_id); + } + return grpc::Status::OK; + } catch (const std::exception& e) { + fillFeedback(response->mutable_header(), false, e.what()); + return grpc::Status::OK; + } +} + grpc::Status gRPCArmServiceImpl::clearFault(grpc::ServerContext *context, const cmvr::api::CommandHeader_Request *request, cmvr::api::CommandHeader_Feedback *response) diff --git a/cmvr-es/service/grpc/src/grpc_system_service.cpp b/cmvr-es/service/grpc/src/grpc_system_service.cpp index a9ac6a31..908f4c91 100644 --- a/cmvr-es/service/grpc/src/grpc_system_service.cpp +++ b/cmvr-es/service/grpc/src/grpc_system_service.cpp @@ -4,12 +4,107 @@ #include "../include/grpc_system_service.h" +#include +#include + #include "common/base/logging/logger.h" -using namespace cmvr::device; using namespace cmvr::device; using namespace cmvr::service; +namespace { + +std::uint64_t unixTimeMs() noexcept +{ + const auto elapsed = std::chrono::duration_cast( + std::chrono::system_clock::now().time_since_epoch()); + return elapsed.count() > 0 + ? static_cast(elapsed.count()) + : 0U; +} + +cmvr::api::SystemDeviceType toApiDeviceType( + const cmvr::device::DeviceKind kind) noexcept +{ + switch (kind) { + case cmvr::device::DeviceKind::AGV: + return cmvr::api::SYSTEM_DEVICE_TYPE_AGV; + case cmvr::device::DeviceKind::Arm: + return cmvr::api::SYSTEM_DEVICE_TYPE_ARM; + case cmvr::device::DeviceKind::Battery: + return cmvr::api::SYSTEM_DEVICE_TYPE_BATTERY; + case cmvr::device::DeviceKind::BioHead: + return cmvr::api::SYSTEM_DEVICE_TYPE_BIO_HEAD; + case cmvr::device::DeviceKind::Camera: + return cmvr::api::SYSTEM_DEVICE_TYPE_CAMERA; + case cmvr::device::DeviceKind::CanBus: + return cmvr::api::SYSTEM_DEVICE_TYPE_CAN_BUS; + case cmvr::device::DeviceKind::DexHand: + return cmvr::api::SYSTEM_DEVICE_TYPE_DEX_HAND; + case cmvr::device::DeviceKind::Gripper: + return cmvr::api::SYSTEM_DEVICE_TYPE_GRIPPER; + case cmvr::device::DeviceKind::Microphone: + return cmvr::api::SYSTEM_DEVICE_TYPE_MICROPHONE; + case cmvr::device::DeviceKind::Motor: + return cmvr::api::SYSTEM_DEVICE_TYPE_MOTOR; + case cmvr::device::DeviceKind::MotorSystem: + return cmvr::api::SYSTEM_DEVICE_TYPE_MOTOR_SYSTEM; + case cmvr::device::DeviceKind::MujocoViewer: + return cmvr::api::SYSTEM_DEVICE_TYPE_MUJOCO_VIEWER; + case cmvr::device::DeviceKind::MujocoWorld: + return cmvr::api::SYSTEM_DEVICE_TYPE_MUJOCO_WORLD; + case cmvr::device::DeviceKind::Robot: + return cmvr::api::SYSTEM_DEVICE_TYPE_ROBOT; + case cmvr::device::DeviceKind::Speaker: + return cmvr::api::SYSTEM_DEVICE_TYPE_SPEAKER; + case cmvr::device::DeviceKind::Unknown: + break; + } + return cmvr::api::SYSTEM_DEVICE_TYPE_UNSPECIFIED; +} + +cmvr::api::SystemDeviceState toApiDeviceState( + const cmvr::device::ManagedDeviceState state) noexcept +{ + switch (state) { + case cmvr::device::ManagedDeviceState::Disabled: + return cmvr::api::SYSTEM_DEVICE_STATE_DISABLED; + case cmvr::device::ManagedDeviceState::Initializing: + return cmvr::api::SYSTEM_DEVICE_STATE_INITIALIZING; + case cmvr::device::ManagedDeviceState::Registered: + return cmvr::api::SYSTEM_DEVICE_STATE_REGISTERED; + case cmvr::device::ManagedDeviceState::Ready: + return cmvr::api::SYSTEM_DEVICE_STATE_READY; + case cmvr::device::ManagedDeviceState::Running: + return cmvr::api::SYSTEM_DEVICE_STATE_RUNNING; + case cmvr::device::ManagedDeviceState::Stopped: + return cmvr::api::SYSTEM_DEVICE_STATE_STOPPED; + case cmvr::device::ManagedDeviceState::Error: + return cmvr::api::SYSTEM_DEVICE_STATE_ERROR; + case cmvr::device::ManagedDeviceState::Unknown: + break; + } + return cmvr::api::SYSTEM_DEVICE_STATE_UNSPECIFIED; +} + +cmvr::api::SystemDeviceHealth toApiDeviceHealth( + const cmvr::device::DeviceHealthState state) noexcept +{ + switch (state) { + case cmvr::device::DeviceHealthState::Healthy: + return cmvr::api::SYSTEM_DEVICE_HEALTH_HEALTHY; + case cmvr::device::DeviceHealthState::Degraded: + return cmvr::api::SYSTEM_DEVICE_HEALTH_DEGRADED; + case cmvr::device::DeviceHealthState::Fault: + return cmvr::api::SYSTEM_DEVICE_HEALTH_FAULT; + case cmvr::device::DeviceHealthState::Unknown: + break; + } + return cmvr::api::SYSTEM_DEVICE_HEALTH_UNSPECIFIED; +} + +} // namespace + gRPCSystemServiceImpl::gRPCSystemServiceImpl(): dmgr_(DeviceManager::getInstance()) {} grpc::Status gRPCSystemServiceImpl::GetSystemInfo(grpc::ServerContext* context, @@ -83,6 +178,55 @@ grpc::Status gRPCSystemServiceImpl::GetSystemStatus(grpc::ServerContext* context } } +grpc::Status gRPCSystemServiceImpl::GetDeviceList( + grpc::ServerContext* context, + const api::GetDeviceListCommand_Request* request, + api::GetDeviceListCommand_Feedback* response) +{ + (void)context; + (void)request; + try { + const auto snapshot = dmgr_.snapshot(); + response->set_manager_name(snapshot.name); + response->set_manager_version(snapshot.version); + response->set_manager_description(snapshot.description); + response->set_sampled_at_unix_ms(unixTimeMs()); + + for (const auto& source : snapshot.devices) { + // The public inventory contains enabled entries only. Keep enabled + // devices visible even when their lifecycle or health is in error. + if (!source.enabled) { + continue; + } + + auto* destination = response->add_device_list(); + destination->set_device_id(source.id); + destination->set_device_type(toApiDeviceType(source.kind)); + destination->set_type_name(source.type_name); + destination->set_enabled(true); + destination->set_manager_state(toApiDeviceState(source.state)); + destination->set_health(toApiDeviceHealth(source.health.state)); + destination->set_has_error(source.abnormal); + destination->set_error_message(source.error_message); + destination->set_status_updated_at_unix_ms( + source.status_updated_at_unix_ms); + } + + response->mutable_header()->set_success(true); + setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); + CMVR_LOG(DEBUG) << "[gRPCSystemServiceImpl] (GetDeviceList): success, devices=" + << response->device_list_size(); + return grpc::Status::OK; + } + catch (const std::exception& e) { + response->Clear(); + response->mutable_header()->set_success(false); + response->mutable_header()->set_error_message(e.what()); + setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); + return grpc::Status::OK; + } +} + grpc::Status gRPCSystemServiceImpl::UpdateParams(grpc::ServerContext* context, const cmvr::api::UpdateParamsCommand_Request* request, cmvr::api::UpdateParamsCommand_Feedback* response) { response->mutable_header()->set_success(false); @@ -91,37 +235,6 @@ grpc::Status gRPCSystemServiceImpl::UpdateParams(grpc::ServerContext* context, c return grpc::Status::OK; } -grpc::Status gRPCSystemServiceImpl::ExecuteJsonCommand(grpc::ServerContext* context, - const cmvr::api::JsonDeviceCommand_Request* request, cmvr::api::JsonDeviceCommand_Feedback* response) -{ - try { - const std::string& dev_id = request->header().device_id(); - auto dev = dmgr_.getDeviceBase(dev_id); - if (!dev) { - response->mutable_header()->set_success(false); - response->mutable_header()->set_error_message("Device not found: " + dev_id); - setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); - return grpc::Status::OK; - } - - std::string response_json; - const bool success = dev->executeJsonCommand(request->request_json(), response_json); - response->mutable_header()->set_success(success); - if (!success) { - response->mutable_header()->set_error_message(response_json); - } - response->set_response_json(response_json); - setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); - return grpc::Status::OK; - } - catch (std::exception& e) { - response->mutable_header()->set_success(false); - response->mutable_header()->set_error_message(e.what()); - setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); - return grpc::Status::OK; - } -} - grpc::Status gRPCSystemServiceImpl::StopAll(grpc::ServerContext* context, const cmvr::api::StopAllCommand_Request* request, cmvr::api::StopAllCommand_Feedback* response) { diff --git a/cmvr-es/service/grpc/tests/grpc_arm_service_test.cpp b/cmvr-es/service/grpc/tests/grpc_arm_service_test.cpp new file mode 100644 index 00000000..57c9d060 --- /dev/null +++ b/cmvr-es/service/grpc/tests/grpc_arm_service_test.cpp @@ -0,0 +1,378 @@ +#include "service/grpc/include/grpc_arm_service.h" + +#include +#include +#include +#include +#include + +#include +#include +#include + +#include "cmvr/config/device_manager_config/device_manager_config.pb.h" +#include "manager/device_manager/include/device_manager.h" + +namespace cmvr::service { +namespace { + +class JsonCommandRobotArm final : public device::RobotArm { +public: + explicit JsonCommandRobotArm(std::string id) + { + id_ = std::move(id); + } + + std::string typeName() const override { return "JsonCommandRobotArm"; } + + bool executeJsonCommand(const std::string& request_json, + std::string& response_json) override + { + ++execute_calls; + last_request_json = request_json; + response_json = next_response_json; + return next_success; + } + + device::RobotModel getRobotModel() const override { return {}; } + std::size_t getDof() const override { return 0U; } + device::ArmState getRobotState() const override { return {}; } + device::JointGroupState getJointState() const override { return {}; } + device::CartesianPose getTcpPose( + device::FrameType = device::FrameType::Base) const override + { + return {}; + } + device::RobotMode getRobotMode() const override + { + return device::RobotMode::Unknown; + } + device::SafetyMode getSafetyMode() const override + { + return device::SafetyMode::Unknown; + } + device::ControlMode getControlMode() const override + { + return device::ControlMode::None; + } + + device::Result torqueOn() override { return device::Result::success(); } + device::Result torqueOff() override { return device::Result::success(); } + device::Result calibrateZeroQ(const std::string&) override + { + return device::Result::success(); + } + device::Result emergencyStop() override + { + return device::Result::success(); + } + device::Result protectiveStop() override + { + return device::Result::success(); + } + device::Result setSpeedScaling(double) override + { + return device::Result::success(); + } + double getSpeedScaling() const override { return 1.0; } + bool isProtectiveStopped() const override { return false; } + bool isEmergencyStopped() const override { return false; } + bool isFault() const override { return false; } + + device::Result moveJ(const device::JointPositionCommand&, + const device::MotionOptions&) override + { + return device::Result::success(); + } + device::Result speedJ(const device::JointVelocityCommand&, + double, + double) override + { + return device::Result::success(); + } + device::Result stopJ(double) override + { + return device::Result::success(); + } + device::Result moveL( + const device::CartesianPose&, + const device::MotionOptions&, + device::FrameType = device::FrameType::Base) override + { + return device::Result::success(); + } + device::Result speedL( + const device::CartesianVelocity&, + double, + double, + device::FrameType = device::FrameType::Base) override + { + return device::Result::success(); + } + device::Result stopL(std::optional = std::nullopt) override + { + return device::Result::success(); + } + device::Result stopMotion() override + { + return device::Result::success(); + } + + device::Result startServoMode(const device::ServoOptions&) override + { + return device::Result::success(); + } + device::Result servoJ(const device::JointPositionCommand&) override + { + return device::Result::success(); + } + device::Result servoL( + const device::CartesianPose&, + device::FrameType = device::FrameType::Base) override + { + return device::Result::success(); + } + device::Result servoSpeedJ(const device::JointVelocityCommand&) override + { + return device::Result::success(); + } + device::Result servoSpeedL( + const device::CartesianVelocity&, + device::FrameType = device::FrameType::Base) override + { + return device::Result::success(); + } + device::Result stopServoMode() override + { + return device::Result::success(); + } + + device::Result connect(const std::string&, int) override + { + return device::Result::success(); + } + device::Result disconnect() override + { + return device::Result::success(); + } + bool isConnected() const override { return true; } + device::Result powerOn() override { return device::Result::success(); } + device::Result powerOff() override { return device::Result::success(); } + device::Result brakeRelease() override + { + return device::Result::success(); + } + device::Result shutdown() override + { + return device::Result::success(); + } + device::Result clearFault() override + { + return device::Result::success(); + } + device::Result unlockProtectiveStop() override + { + return device::Result::success(); + } + device::Result loadProgram(const std::string&) override + { + return device::Result::success(); + } + device::Result playProgram() override + { + return device::Result::success(); + } + device::Result pauseProgram() override + { + return device::Result::success(); + } + device::Result stopProgram() override + { + return device::Result::success(); + } + + std::vector ik(const std::string&, + const std::string&, + const device::CartesianPose&) override + { + return {}; + } + std::shared_ptr kinematicsSolver() const override + { + return nullptr; + } + device::CartesianPose fk(const std::string&, + const std::string&) override + { + return {}; + } + device::CartesianPose fk(bool = true) override { return {}; } + device::CartesianVelocity getSpeedLCommandTwistBase() const override + { + return {}; + } + bool busy() const override { return false; } + + int execute_calls{0}; + bool next_success{true}; + std::string next_response_json; + std::string last_request_json; +}; + +class JsonCommandNonArmDevice final : public device::AbstractDevice { +public: + explicit JsonCommandNonArmDevice(std::string id) + : AbstractDevice(std::move(id)) + { + } + + std::string typeName() const override { return "JsonCommandNonArmDevice"; } + + bool executeJsonCommand(const std::string&, + std::string& response_json) override + { + ++execute_calls; + response_json = R"({"success":true})"; + return true; + } + + int execute_calls{0}; +}; + +class GrpcArmServiceTest : public ::testing::Test { +protected: + void SetUp() override + { + device::DeviceManager::destroyInstance(); + config::DeviceManagerConfig config; + auto& manager = device::DeviceManager::getInstance(config); + + left_arm_ = std::make_shared("left_arm"); + aubo_arm_ = std::make_shared("aubo_arm"); + non_arm_ = std::make_shared("camera"); + manager.registerDevice(left_arm_); + manager.registerDevice(aubo_arm_); + manager.registerDevice(non_arm_); + service_ = std::make_unique(); + } + + void TearDown() override + { + service_.reset(); + non_arm_.reset(); + aubo_arm_.reset(); + left_arm_.reset(); + device::DeviceManager::destroyInstance(); + } + + grpc::Status execute(const std::string& device_id, + const std::string& request_json, + api::JsonDeviceCommand_Feedback& response) + { + api::JsonDeviceCommand_Request request; + request.mutable_header()->set_device_id(device_id); + request.set_request_json(request_json); + grpc::ServerContext context; + return service_->ExecuteJsonCommand(&context, &request, &response); + } + + std::shared_ptr left_arm_; + std::shared_ptr aubo_arm_; + std::shared_ptr non_arm_; + std::unique_ptr service_; +}; + +TEST(GrpcArmServiceDescriptorTest, + ExecuteJsonCommandBelongsOnlyToArmService) +{ + const auto* pool = google::protobuf::DescriptorPool::generated_pool(); + const auto* arm_service = + pool->FindServiceByName("cmvr.api.ArmService"); + const auto* system_service = + pool->FindServiceByName("cmvr.api.SystemService"); + + ASSERT_NE(arm_service, nullptr); + ASSERT_NE(system_service, nullptr); + EXPECT_NE(arm_service->FindMethodByName("ExecuteJsonCommand"), nullptr); + EXPECT_EQ(system_service->FindMethodByName("ExecuteJsonCommand"), nullptr); +} + +TEST_F(GrpcArmServiceTest, RoutesByHeaderDeviceIdAndForwardsSuccessfulJson) +{ + const std::string request_json = + R"({"command":"cabinet_io","operation":"get_di","index":0})"; + const std::string response_json = + R"({"success":true,"operation":"get_di","index":0,"value":false})"; + aubo_arm_->next_response_json = response_json; + + api::JsonDeviceCommand_Feedback response; + const auto status = execute("aubo_arm", request_json, response); + + ASSERT_TRUE(status.ok()) << status.error_message(); + EXPECT_TRUE(response.header().success()); + EXPECT_TRUE(response.header().error_message().empty()); + EXPECT_TRUE(response.header().has_timestamp()); + EXPECT_GT(response.header().timestamp().seconds(), 0); + EXPECT_EQ(response.response_json(), response_json); + EXPECT_EQ(aubo_arm_->execute_calls, 1); + EXPECT_EQ(aubo_arm_->last_request_json, request_json); + EXPECT_EQ(left_arm_->execute_calls, 0); +} + +TEST_F(GrpcArmServiceTest, ForwardsDeviceJsonFailureWithLegacyGrpcOkSemantics) +{ + const std::string response_json = + R"({"success":false,"error_code":"not_connected"})"; + aubo_arm_->next_success = false; + aubo_arm_->next_response_json = response_json; + + api::JsonDeviceCommand_Feedback response; + const auto status = execute( + "aubo_arm", + R"({"command":"cabinet_io","operation":"get_do","index":0})", + response); + + ASSERT_TRUE(status.ok()) << status.error_message(); + EXPECT_FALSE(response.header().success()); + EXPECT_EQ(response.header().error_message(), response_json); + EXPECT_TRUE(response.header().has_timestamp()); + EXPECT_GT(response.header().timestamp().seconds(), 0); + EXPECT_EQ(response.response_json(), response_json); + EXPECT_EQ(aubo_arm_->execute_calls, 1); + EXPECT_EQ(left_arm_->execute_calls, 0); +} + +TEST_F(GrpcArmServiceTest, + MissingOrNonArmIdReturnsBusinessFailureWithoutBackendDispatch) +{ + api::JsonDeviceCommand_Feedback non_arm_response; + const auto non_arm_status = execute( + "camera", R"({"command":"cabinet_io"})", non_arm_response); + + ASSERT_TRUE(non_arm_status.ok()) << non_arm_status.error_message(); + EXPECT_FALSE(non_arm_response.header().success()); + EXPECT_EQ(non_arm_response.header().error_message(), + "Device not found: camera"); + EXPECT_TRUE(non_arm_response.header().has_timestamp()); + EXPECT_TRUE(non_arm_response.response_json().empty()); + EXPECT_EQ(non_arm_->execute_calls, 0); + EXPECT_EQ(aubo_arm_->execute_calls, 0); + EXPECT_EQ(left_arm_->execute_calls, 0); + + api::JsonDeviceCommand_Feedback missing_response; + const auto missing_status = execute( + "missing_arm", R"({"command":"cabinet_io"})", missing_response); + + ASSERT_TRUE(missing_status.ok()) << missing_status.error_message(); + EXPECT_FALSE(missing_response.header().success()); + EXPECT_EQ(missing_response.header().error_message(), + "Device not found: missing_arm"); + EXPECT_TRUE(missing_response.header().has_timestamp()); + EXPECT_TRUE(missing_response.response_json().empty()); + EXPECT_EQ(non_arm_->execute_calls, 0); + EXPECT_EQ(aubo_arm_->execute_calls, 0); + EXPECT_EQ(left_arm_->execute_calls, 0); +} + +} // namespace +} // namespace cmvr::service diff --git a/cmvr-es/service/grpc/tests/grpc_system_service_test.cpp b/cmvr-es/service/grpc/tests/grpc_system_service_test.cpp new file mode 100644 index 00000000..8642a5b4 --- /dev/null +++ b/cmvr-es/service/grpc/tests/grpc_system_service_test.cpp @@ -0,0 +1,291 @@ +#include "service/grpc/include/grpc_system_service.h" + +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include "cmvr/config/device_manager_config/device_manager_config.pb.h" +#include "manager/device_manager/include/device_manager.h" + +namespace cmvr::service { +namespace { + +class SnapshotDevice final : public device::AbstractDevice { +public: + SnapshotDevice(std::string id, + device::DeviceKind kind, + std::string type_name, + device::DeviceHealthSnapshot health = {}) + : AbstractDevice(std::move(id)), + kind_(kind), + type_name_(std::move(type_name)), + health_(std::move(health)) + { + } + + device::DeviceKind kind() const noexcept override { return kind_; } + std::string typeName() const override { return type_name_; } + device::DeviceHealthSnapshot healthSnapshot() override + { + return health_; + } + +private: + device::DeviceKind kind_; + std::string type_name_; + device::DeviceHealthSnapshot health_; +}; + +std::uint64_t currentUnixTimeMs() +{ + const auto elapsed = std::chrono::duration_cast( + std::chrono::system_clock::now().time_since_epoch()); + return static_cast(elapsed.count()); +} + +const api::SystemDeviceInfo* findDevice( + const api::GetDeviceListCommand_Feedback& response, + const std::string& id) +{ + for (const auto& device : response.device_list()) { + if (device.device_id() == id) { + return &device; + } + } + return nullptr; +} + +class GrpcSystemServiceTest : public ::testing::Test { +protected: + void SetUp() override + { + device::DeviceManager::destroyInstance(); + } + + void TearDown() override + { + service_.reset(); + owned_devices_.clear(); + device::DeviceManager::destroyInstance(); + } + + api::GetDeviceListCommand_Feedback getDeviceList() + { + api::GetDeviceListCommand_Request request; + api::GetDeviceListCommand_Feedback response; + grpc::ServerContext context; + + const auto status = service_->GetDeviceList( + &context, &request, &response); + EXPECT_TRUE(status.ok()) << status.error_message(); + return response; + } + + void registerDevice(device::DeviceManager& manager, + std::shared_ptr device) + { + manager.registerDevice(device); + owned_devices_.push_back(std::move(device)); + } + + std::unique_ptr service_; + std::vector> owned_devices_; +}; + +TEST_F(GrpcSystemServiceTest, + ReturnsOnlyEnabledDevicesAndPreservesEnabledErrors) +{ + config::DeviceManagerConfig config; + config.set_name("system-service-test"); + config.set_version("9.2"); + config.set_description("GetDeviceList snapshot test"); + + auto* disabled = config.add_devices(); + disabled->set_id("b_disabled_camera"); + disabled->set_type(config::DeviceConfigEntry::DEVICE_TYPE_CAMERA); + disabled->set_enable(false); + + // An enabled unsupported entry remains in the manager snapshot as ERROR. + // GetDeviceList must expose it rather than filtering by lifecycle state. + auto* enabled_error = config.add_devices(); + enabled_error->set_id("c_enabled_error"); + enabled_error->set_type(config::DeviceConfigEntry::DEVICE_TYPE_UNKNOWN); + enabled_error->set_enable(true); + + auto& manager = device::DeviceManager::getInstance(config); + registerDevice( + manager, + std::make_shared( + "z_camera", + device::DeviceKind::Camera, + "VendorCamera", + device::DeviceHealthSnapshot{ + device::DeviceHealthState::Healthy, {}})); + registerDevice( + manager, + std::make_shared( + "a_unknown", + device::DeviceKind::Unknown, + "MysteryDriver", + device::DeviceHealthSnapshot{ + device::DeviceHealthState::Degraded, + "device health is degraded"})); + service_ = std::make_unique(); + + const auto response = getDeviceList(); + + ASSERT_TRUE(response.header().success()) + << response.header().error_message(); + EXPECT_TRUE(response.header().has_timestamp()); + EXPECT_GT(response.header().timestamp().seconds(), 0); + EXPECT_EQ(response.manager_name(), "system-service-test"); + EXPECT_EQ(response.manager_version(), "9.2"); + EXPECT_EQ(response.manager_description(), "GetDeviceList snapshot test"); + ASSERT_EQ(response.device_list_size(), 3); + + // DeviceManager::snapshot() supplies stable device-id ordering, and the + // service must preserve it while filtering disabled entries. + EXPECT_EQ(response.device_list(0).device_id(), "a_unknown"); + EXPECT_EQ(response.device_list(1).device_id(), "c_enabled_error"); + EXPECT_EQ(response.device_list(2).device_id(), "z_camera"); + EXPECT_EQ(findDevice(response, "b_disabled_camera"), nullptr); + + const auto* unknown = findDevice(response, "a_unknown"); + ASSERT_NE(unknown, nullptr); + EXPECT_TRUE(unknown->enabled()); + EXPECT_EQ(unknown->type_name(), "MysteryDriver"); + EXPECT_EQ(unknown->device_type(), + api::SYSTEM_DEVICE_TYPE_UNSPECIFIED); + EXPECT_NE(unknown->device_type(), api::SYSTEM_DEVICE_TYPE_AGV); + EXPECT_EQ(unknown->manager_state(), + api::SYSTEM_DEVICE_STATE_REGISTERED); + EXPECT_EQ(unknown->health(), api::SYSTEM_DEVICE_HEALTH_DEGRADED); + EXPECT_TRUE(unknown->has_error()); + EXPECT_EQ(unknown->error_message(), "device health is degraded"); + EXPECT_GT(unknown->status_updated_at_unix_ms(), 0U); + + const auto* error = findDevice(response, "c_enabled_error"); + ASSERT_NE(error, nullptr); + EXPECT_TRUE(error->enabled()); + EXPECT_EQ(error->type_name(), "Unknown"); + EXPECT_EQ(error->device_type(), api::SYSTEM_DEVICE_TYPE_UNSPECIFIED); + EXPECT_EQ(error->manager_state(), api::SYSTEM_DEVICE_STATE_ERROR); + EXPECT_EQ(error->health(), api::SYSTEM_DEVICE_HEALTH_UNSPECIFIED); + EXPECT_TRUE(error->has_error()); + EXPECT_FALSE(error->error_message().empty()); + EXPECT_GT(error->status_updated_at_unix_ms(), 0U); + + const auto* camera = findDevice(response, "z_camera"); + ASSERT_NE(camera, nullptr); + EXPECT_TRUE(camera->enabled()); + EXPECT_EQ(camera->type_name(), "VendorCamera"); + EXPECT_EQ(camera->device_type(), api::SYSTEM_DEVICE_TYPE_CAMERA); + EXPECT_EQ(camera->manager_state(), + api::SYSTEM_DEVICE_STATE_REGISTERED); + EXPECT_EQ(camera->health(), api::SYSTEM_DEVICE_HEALTH_HEALTHY); + EXPECT_FALSE(camera->has_error()); + EXPECT_TRUE(camera->error_message().empty()); + EXPECT_GT(camera->status_updated_at_unix_ms(), 0U); +} + +TEST_F(GrpcSystemServiceTest, EmptyListReturnsMetadataAndTimestamps) +{ + config::DeviceManagerConfig config; + config.set_name("empty-manager"); + config.set_version("1.2.3"); + config.set_description("manager without devices"); + device::DeviceManager::getInstance(config); + service_ = std::make_unique(); + + const auto before_ms = currentUnixTimeMs(); + const auto response = getDeviceList(); + const auto after_ms = currentUnixTimeMs(); + + ASSERT_TRUE(response.header().success()) + << response.header().error_message(); + EXPECT_TRUE(response.header().has_timestamp()); + EXPECT_GT(response.header().timestamp().seconds(), 0); + EXPECT_EQ(response.device_list_size(), 0); + EXPECT_EQ(response.manager_name(), "empty-manager"); + EXPECT_EQ(response.manager_version(), "1.2.3"); + EXPECT_EQ(response.manager_description(), "manager without devices"); + EXPECT_GE(response.sampled_at_unix_ms(), before_ms); + EXPECT_LE(response.sampled_at_unix_ms(), after_ms); +} + +TEST_F(GrpcSystemServiceTest, MapsEveryKnownDeviceKind) +{ + struct ExpectedMapping { + const char* id; + device::DeviceKind source; + api::SystemDeviceType destination; + }; + constexpr std::array mappings{{ + {"01_agv", device::DeviceKind::AGV, + api::SYSTEM_DEVICE_TYPE_AGV}, + {"02_arm", device::DeviceKind::Arm, + api::SYSTEM_DEVICE_TYPE_ARM}, + {"03_battery", device::DeviceKind::Battery, + api::SYSTEM_DEVICE_TYPE_BATTERY}, + {"04_bio_head", device::DeviceKind::BioHead, + api::SYSTEM_DEVICE_TYPE_BIO_HEAD}, + {"05_camera", device::DeviceKind::Camera, + api::SYSTEM_DEVICE_TYPE_CAMERA}, + {"06_can_bus", device::DeviceKind::CanBus, + api::SYSTEM_DEVICE_TYPE_CAN_BUS}, + {"07_dex_hand", device::DeviceKind::DexHand, + api::SYSTEM_DEVICE_TYPE_DEX_HAND}, + {"08_gripper", device::DeviceKind::Gripper, + api::SYSTEM_DEVICE_TYPE_GRIPPER}, + {"09_microphone", device::DeviceKind::Microphone, + api::SYSTEM_DEVICE_TYPE_MICROPHONE}, + {"10_motor", device::DeviceKind::Motor, + api::SYSTEM_DEVICE_TYPE_MOTOR}, + {"11_motor_system", device::DeviceKind::MotorSystem, + api::SYSTEM_DEVICE_TYPE_MOTOR_SYSTEM}, + {"12_mujoco_viewer", device::DeviceKind::MujocoViewer, + api::SYSTEM_DEVICE_TYPE_MUJOCO_VIEWER}, + {"13_mujoco_world", device::DeviceKind::MujocoWorld, + api::SYSTEM_DEVICE_TYPE_MUJOCO_WORLD}, + {"14_robot", device::DeviceKind::Robot, + api::SYSTEM_DEVICE_TYPE_ROBOT}, + {"15_speaker", device::DeviceKind::Speaker, + api::SYSTEM_DEVICE_TYPE_SPEAKER}, + }}; + + config::DeviceManagerConfig config; + auto& manager = device::DeviceManager::getInstance(config); + for (const auto& mapping : mappings) { + registerDevice( + manager, + std::make_shared( + mapping.id, mapping.source, "MappedBackend")); + } + service_ = std::make_unique(); + + const auto response = getDeviceList(); + + ASSERT_TRUE(response.header().success()) + << response.header().error_message(); + ASSERT_EQ(response.device_list_size(), + static_cast(mappings.size())); + for (std::size_t index = 0; index < mappings.size(); ++index) { + SCOPED_TRACE(mappings[index].id); + const auto& actual = response.device_list( + static_cast(index)); + EXPECT_EQ(actual.device_id(), mappings[index].id); + EXPECT_EQ(actual.device_type(), mappings[index].destination); + EXPECT_NE(actual.device_type(), + api::SYSTEM_DEVICE_TYPE_UNSPECIFIED); + } +} + +} // namespace +} // namespace cmvr::service diff --git a/protos/cmvr/api/arm_service.proto b/protos/cmvr/api/arm_service.proto index 92562f4a..a050532a 100644 --- a/protos/cmvr/api/arm_service.proto +++ b/protos/cmvr/api/arm_service.proto @@ -20,4 +20,7 @@ service ArmService { rpc calibrateZeroQ(CalibrateZeroQ.Request) returns (CalibrateZeroQ.Response); rpc getPoseMatrix(GetPoseMatrix.Request) returns (GetPoseMatrix.Response); rpc computeForwardKinematics(ComputeForwardKinematics.Request) returns (ComputeForwardKinematics.Response); + + // Vendor-specific arm extension currently used for AUBO cabinet IO. + rpc ExecuteJsonCommand(JsonDeviceCommand.Request) returns (JsonDeviceCommand.Feedback); } diff --git a/protos/cmvr/api/system_command.proto b/protos/cmvr/api/system_command.proto index 52b99221..c61fa36d 100644 --- a/protos/cmvr/api/system_command.proto +++ b/protos/cmvr/api/system_command.proto @@ -21,6 +21,77 @@ message DeviceList { DeviceType device_type = 2; } +// Stable device categories used by GetDeviceList. This intentionally does not +// reuse the legacy DeviceType enum above: its zero value is AGV and it does not +// cover all DeviceManager categories. +enum SystemDeviceType { + SYSTEM_DEVICE_TYPE_UNSPECIFIED = 0; + SYSTEM_DEVICE_TYPE_AGV = 1; + SYSTEM_DEVICE_TYPE_ARM = 2; + SYSTEM_DEVICE_TYPE_BATTERY = 3; + SYSTEM_DEVICE_TYPE_BIO_HEAD = 4; + SYSTEM_DEVICE_TYPE_CAMERA = 5; + SYSTEM_DEVICE_TYPE_CAN_BUS = 6; + SYSTEM_DEVICE_TYPE_DEX_HAND = 7; + SYSTEM_DEVICE_TYPE_GRIPPER = 8; + SYSTEM_DEVICE_TYPE_MICROPHONE = 9; + SYSTEM_DEVICE_TYPE_MOTOR = 10; + SYSTEM_DEVICE_TYPE_MOTOR_SYSTEM = 11; + SYSTEM_DEVICE_TYPE_MUJOCO_VIEWER = 12; + SYSTEM_DEVICE_TYPE_MUJOCO_WORLD = 13; + SYSTEM_DEVICE_TYPE_ROBOT = 14; + SYSTEM_DEVICE_TYPE_SPEAKER = 15; +} + +enum SystemDeviceState { + SYSTEM_DEVICE_STATE_UNSPECIFIED = 0; + SYSTEM_DEVICE_STATE_DISABLED = 1; + SYSTEM_DEVICE_STATE_INITIALIZING = 2; + SYSTEM_DEVICE_STATE_REGISTERED = 3; + SYSTEM_DEVICE_STATE_READY = 4; + SYSTEM_DEVICE_STATE_RUNNING = 5; + SYSTEM_DEVICE_STATE_STOPPED = 6; + SYSTEM_DEVICE_STATE_ERROR = 7; +} + +enum SystemDeviceHealth { + SYSTEM_DEVICE_HEALTH_UNSPECIFIED = 0; + SYSTEM_DEVICE_HEALTH_HEALTHY = 1; + SYSTEM_DEVICE_HEALTH_DEGRADED = 2; + SYSTEM_DEVICE_HEALTH_FAULT = 3; +} + +message SystemDeviceInfo { + string device_id = 1; + SystemDeviceType device_type = 2; + + // Concrete backend name for display and diagnostics only. Consumers must + // use device_type, rather than this free-form string, for decisions. + string type_name = 3; + + // GetDeviceList currently publishes only enabled entries. Keep this field + // explicit so each row remains self-describing and future-compatible. + bool enabled = 4; + SystemDeviceState manager_state = 5; + SystemDeviceHealth health = 6; + bool has_error = 7; + string error_message = 8; + uint64 status_updated_at_unix_ms = 9; +} + +message GetDeviceListCommand { + message Request {} + + message Feedback { + CommandHeader.Feedback header = 1; + string manager_name = 2; + string manager_version = 3; + string manager_description = 4; + repeated SystemDeviceInfo device_list = 5; + uint64 sampled_at_unix_ms = 6; + } +} + message GetSystemInfoCommand { message Request {} @@ -69,4 +140,4 @@ message StopAllCommand { message Feedback { CommandHeader.Feedback header = 1; } -} \ No newline at end of file +} diff --git a/protos/cmvr/api/system_service.proto b/protos/cmvr/api/system_service.proto index 84d1c9bd..83f19afd 100644 --- a/protos/cmvr/api/system_service.proto +++ b/protos/cmvr/api/system_service.proto @@ -1,6 +1,5 @@ syntax = "proto3"; -import "cmvr/api/common.proto"; import "cmvr/api/system_command.proto"; package cmvr.api; @@ -9,9 +8,9 @@ package cmvr.api; service SystemService { rpc GetSystemInfo(GetSystemInfoCommand.Request) returns (GetSystemInfoCommand.Feedback) {} rpc GetSystemStatus(GetSystemStatusCommand.Request) returns (GetSystemStatusCommand.Feedback) {} + rpc GetDeviceList(GetDeviceListCommand.Request) returns (GetDeviceListCommand.Feedback) {} rpc UpdateParams(UpdateParamsCommand.Request) returns (UpdateParamsCommand.Feedback) {} - rpc ExecuteJsonCommand(JsonDeviceCommand.Request) returns (JsonDeviceCommand.Feedback) {} rpc StopAll(StopAllCommand.Request) returns (StopAllCommand.Feedback) {} } From 60098ee01e837a6598aeee187fe5916219bf4f17 Mon Sep 17 00:00:00 2001 From: xtkuang <87661715@qq.com> Date: Tue, 4 Aug 2026 10:36:23 +0800 Subject: [PATCH 12/20] refactor: remove PLC motor control backend --- README.md | 5 +- .../config/devices/motor/plc_motors.pb.txt | 56 - cmvr-es/config/manager/device_manager.pb.txt | 8 - cmvr-es/devices/README.md | 2 +- cmvr-es/devices/motor/CMakeLists.txt | 47 - cmvr-es/devices/motor/README.md | 71 +- .../devices/motor/bus_runtime/CMakeLists.txt | 13 +- .../motor/bus_runtime/modbus_tcp/README.md | 93 - .../include/cmvr_plc_register_map.h | 209 --- .../modbus_tcp/include/modbus_tcp_client.h | 47 - .../include/modbus_tcp_motor_bus_runtime.h | 173 -- .../modbus_tcp/src/modbus_tcp_client.cpp | 188 -- .../src/modbus_tcp_motor_bus_runtime.cpp | 1013 ----------- .../modbus_tcp_motor_bus_runtime_test.cpp | 1586 ----------------- .../drivers/modbus_plc_motor/CMakeLists.txt | 21 - .../include/cmvr_plc_motor_protocol.h | 89 - .../include/modbus_plc_motor.h | 21 - .../src/cmvr_plc_motor_protocol.cpp | 582 ------ .../modbus_plc_motor/src/modbus_plc_motor.cpp | 55 - cmvr-es/devices/motor/manager/CMakeLists.txt | 1 - .../motor/manager/include/motor_manager.h | 4 - .../motor/manager/src/motor_manager.cpp | 61 - cmvr-es/service/CMakeLists.txt | 38 - cmvr-es/service/README.md | 9 +- .../service/grpc/src/grpc_motor_service.cpp | 4 +- .../grpc_motor_service_modbus_e2e_test.cpp | 907 ---------- .../grpc/tests/grpc_motor_service_test.cpp | 4 +- docs/motor_service_modbus_tcp.md | 1022 ----------- protos/cmvr/api/motor_command.proto | 2 +- .../config/motor_config/motor_config.proto | 36 +- 30 files changed, 36 insertions(+), 6331 deletions(-) delete mode 100644 cmvr-es/config/devices/motor/plc_motors.pb.txt delete mode 100644 cmvr-es/devices/motor/bus_runtime/modbus_tcp/README.md delete mode 100644 cmvr-es/devices/motor/bus_runtime/modbus_tcp/include/cmvr_plc_register_map.h delete mode 100644 cmvr-es/devices/motor/bus_runtime/modbus_tcp/include/modbus_tcp_client.h delete mode 100644 cmvr-es/devices/motor/bus_runtime/modbus_tcp/include/modbus_tcp_motor_bus_runtime.h delete mode 100644 cmvr-es/devices/motor/bus_runtime/modbus_tcp/src/modbus_tcp_client.cpp delete mode 100644 cmvr-es/devices/motor/bus_runtime/modbus_tcp/src/modbus_tcp_motor_bus_runtime.cpp delete mode 100644 cmvr-es/devices/motor/bus_runtime/modbus_tcp/tests/modbus_tcp_motor_bus_runtime_test.cpp delete mode 100644 cmvr-es/devices/motor/drivers/modbus_plc_motor/CMakeLists.txt delete mode 100644 cmvr-es/devices/motor/drivers/modbus_plc_motor/include/cmvr_plc_motor_protocol.h delete mode 100644 cmvr-es/devices/motor/drivers/modbus_plc_motor/include/modbus_plc_motor.h delete mode 100644 cmvr-es/devices/motor/drivers/modbus_plc_motor/src/cmvr_plc_motor_protocol.cpp delete mode 100644 cmvr-es/devices/motor/drivers/modbus_plc_motor/src/modbus_plc_motor.cpp delete mode 100644 cmvr-es/service/grpc/tests/grpc_motor_service_modbus_e2e_test.cpp delete mode 100644 docs/motor_service_modbus_tcp.md diff --git a/README.md b/README.md index 9da1699d..5913f0fd 100644 --- a/README.md +++ b/README.md @@ -145,10 +145,7 @@ sudo script/ethercat/stop_ethercat.sh eno1 --restore-network - [MotorService gRPC 接口](cmvr-es/service/README.md#motorservice) - [电机设备模块](cmvr-es/devices/motor/README.md) -- [Modbus TCP PLC runtime](cmvr-es/devices/motor/bus_runtime/modbus_tcp/README.md) -- [MotorService 与 CMVR PLC v1 完整协议](docs/motor_service_modbus_tcp.md) - [AUBO 控制柜 Standard 数字 IO](cmvr-es/devices/arm/aubo_arm/README.md) - [配置与部署规则](cmvr-es/config/README.md) -Modbus Quick Stop 只是功能性停止，不能替代硬接线急停或驱动器 STO。AUBO -JSON 接口只访问控制柜 Standard 数字 IO，不访问安全 IO。 +AUBO JSON 接口只访问控制柜 Standard 数字 IO，不访问安全 IO。 diff --git a/cmvr-es/config/devices/motor/plc_motors.pb.txt b/cmvr-es/config/devices/motor/plc_motors.pb.txt deleted file mode 100644 index 0a1581c2..00000000 --- a/cmvr-es/config/devices/motor/plc_motors.pb.txt +++ /dev/null @@ -1,56 +0,0 @@ -motor { - id: "plc_motors" - - motor_groups { - id: "plc_axis_group" - bus_type: MOTOR_BUS_MODBUS_TCP - vendor: MOTOR_VENDOR_PLC_GENERIC - protocol: MOTOR_PROTOCOL_CMVR_PLC_V1 - - modbus_tcp { - host: "192.168.0.10" - port: 502 - unit_id: 1 - - connect_timeout_ms: 500 - io_timeout_ms: 100 - heartbeat_period_ms: 100 - communication_watchdog_ms: 1000 - cyclic_watchdog_ms: 500 - status_poll_period_ms: 20 - reconnect_min_ms: 100 - reconnect_max_ms: 2000 - command_ack_timeout_ms: 500 - - protocol_major: 1 - protocol_minor: 0 - - axes { motor_id: 1 axis_index: 0 } - axes { motor_id: 2 axis_index: 1 } - } - - joint_limits { - enable: true - source: JOINT_LIMIT_SOURCE_CUSTOM - joints { - joint_name: "PLC_AXIS_1" - q_lb: -3.141592653589793 - q_ub: 3.141592653589793 - qd: 1.0 - qdd: 2.0 - } - joints { - joint_name: "PLC_AXIS_2" - q_lb: -1.5707963267948966 - q_ub: 1.5707963267948966 - qd: 0.5 - qdd: 1.0 - } - } - - motors { - motors { id: 1 joint_name: "PLC_AXIS_1" } - motors { id: 2 joint_name: "PLC_AXIS_2" } - } - } -} diff --git a/cmvr-es/config/manager/device_manager.pb.txt b/cmvr-es/config/manager/device_manager.pb.txt index 07204d9f..8c8c691d 100644 --- a/cmvr-es/config/manager/device_manager.pb.txt +++ b/cmvr-es/config/manager/device_manager.pb.txt @@ -82,14 +82,6 @@ device_manager { enable: false } - devices { - id: "plc_motors" - type: DEVICE_TYPE_MOTOR_SYSTEM - config_file: "devices/motor/plc_motors.pb.txt" - # Configure the PLC endpoint and complete the safety checkout before enabling. - enable: false - } - devices { id: "right_arm" type: DEVICE_TYPE_ROBOT_ARM diff --git a/cmvr-es/devices/README.md b/cmvr-es/devices/README.md index 2642c4f6..b979284f 100644 --- a/cmvr-es/devices/README.md +++ b/cmvr-es/devices/README.md @@ -42,7 +42,7 @@ config/cmvr_es.pb.txt | Microphone | [`microphone/abstract_microphone.h`](microphone/abstract_microphone.h) | [`microphone/microphone_factory.h`](microphone/microphone_factory.h) | FFmpeg | | Speaker | [`speaker/abstract_speaker.h`](speaker/abstract_speaker.h) | [`speaker/speaker_factory.h`](speaker/speaker_factory.h) | FFmpeg | | BioHead | [`biohead/abstract_biohead.h`](biohead/abstract_biohead.h) | DeviceFactory 直接创建 | BioHeadRobot | -| MotorSystem | [`motor/`](motor/README.md) | DeviceFactory 直接创建 | CAN、MuJoCo、EtherCAT、Modbus TCP PLC | +| MotorSystem | [`motor/`](motor/README.md) | DeviceFactory 直接创建 | CAN、MuJoCo、EtherCAT | 代码目录存在不等于已经接入配置创建链： diff --git a/cmvr-es/devices/motor/CMakeLists.txt b/cmvr-es/devices/motor/CMakeLists.txt index 08fa5f13..a7399736 100644 --- a/cmvr-es/devices/motor/CMakeLists.txt +++ b/cmvr-es/devices/motor/CMakeLists.txt @@ -17,52 +17,5 @@ add_subdirectory(drivers/ti5_canopen) add_subdirectory(drivers/mujoco) add_subdirectory(bus_runtime) add_subdirectory(drivers/ethercat_motor) -add_subdirectory(drivers/modbus_plc_motor) - -if(BUILD_TESTING) - enable_testing() - set(CMVR_MOTOR_LIBMODBUS_ROOT - ${CMAKE_SOURCE_DIR}/dependency/${ARCH}/third_party/modbus/3.1.11) - add_executable(modbus_tcp_motor_bus_runtime_test - bus_runtime/modbus_tcp/tests/modbus_tcp_motor_bus_runtime_test.cpp - ) - target_include_directories(modbus_tcp_motor_bus_runtime_test - PRIVATE - ${CMAKE_SOURCE_DIR}/cmvr-es - ${CMVR_MOTOR_LIBMODBUS_ROOT}/include - ) - target_link_directories(modbus_tcp_motor_bus_runtime_test - PRIVATE - ${CMVR_MOTOR_LIBMODBUS_ROOT}/lib - ) - target_link_libraries(modbus_tcp_motor_bus_runtime_test - PRIVATE - cmvr_es::device::motor_bus_runtime - cmvr_es::device::modbus_plc_motor_driver - cmvr_es::proto - modbus - gtest - gtest_main - pthread - glog - ) - target_compile_definitions(modbus_tcp_motor_bus_runtime_test - PRIVATE - CMVR_PLC_MOTOR_SAMPLE_CONFIG_PATH="${CMAKE_SOURCE_DIR}/cmvr-es/config/devices/motor/plc_motors.pb.txt" - ) - add_test(NAME modbus_tcp_motor_bus_runtime_test - COMMAND modbus_tcp_motor_bus_runtime_test) - set(_modbus_motor_test_environment - "LD_LIBRARY_PATH=${CMVR_TEST_EXTERNAL_LIBRARY_PATH}" - ) - if(CMVR_TEST_SYSTEM_LIBSTDCXX) - list(APPEND _modbus_motor_test_environment - "LD_PRELOAD=${CMVR_TEST_SYSTEM_LIBSTDCXX}") - endif() - set_tests_properties(modbus_tcp_motor_bus_runtime_test PROPERTIES - TIMEOUT 15 - ENVIRONMENT "${_modbus_motor_test_environment}" - ) -endif() add_subdirectory(manager) diff --git a/cmvr-es/devices/motor/README.md b/cmvr-es/devices/motor/README.md index a10ba7fc..46949387 100644 --- a/cmvr-es/devices/motor/README.md +++ b/cmvr-es/devices/motor/README.md @@ -2,7 +2,7 @@ `devices/motor/` 提供电机管理、协议适配、总线 runtime 和厂商驱动。Service、 RobotArm 和业务 Task 只依赖 `MotorManager`/`AbstractMotor` 的稳定接口，不应 -直接访问 libmodbus、CAN、EtherCAT 或厂商 SDK。 +直接访问 CAN、EtherCAT、MuJoCo 或厂商 SDK。 返回 [Devices 模块指南](../README.md) 或 [项目总览](../../../README.md)。 @@ -11,72 +11,37 @@ RobotArm 和业务 Task 只依赖 `MotorManager`/`AbstractMotor` 的稳定接口 | 目录 | 职责 | | --- | --- | | `manager/` | 创建 MotorGroup，按 `motor_id`/`joint_name` 暴露 `AbstractMotor` | -| `bus_runtime/` | 连接、收发、重连、watchdog 和总线生命周期 | -| `drivers/` | CANopen、EtherCAT、MuJoCo、Modbus PLC 等具体后端 | -| `drivers/modbus_plc_motor/` | 关节限位、SI 单位和 CMVR PLC v1 命令映射 | +| `bus_runtime/` | 连接、收发和总线生命周期 | +| `drivers/` | CANopen、EtherCAT 和 MuJoCo 等具体后端 | -PLC 电机链路为： +## 当前后端 -```text -gRPC MotorService - | - v -MotorManager -> AbstractMotor - | - v -CmvrPlcMotorProtocol - | - v -ModbusTcpMotorBusRuntime - | - v -libmodbus -> PLC -> 驱动器/电机 -``` +- CAN + TI5 CANopen； +- EtherCAT + EYOU CiA 402； +- MuJoCo 仿真电机。 -各层边界： +配置示例： -- `MotorService` 负责 API 校验、单电机控制权、deadline/cancellation 和 - fail-closed Quick Stop； -- `MotorManager` 负责电机查找与统一抽象； -- `CmvrPlcMotorProtocol` 负责关节限位、SI 单位和 CMVR PLC v1 命令映射； -- `ModbusTcpMotorBusRuntime` 负责 PLC session、mailbox、ACK、状态和重连； -- PLC/驱动器必须独立实现通信 watchdog、周期 watchdog 和硬件安全动作。 +- [`ti5_motors.pb.txt`](../../config/devices/motor/ti5_motors.pb.txt) +- [`ethercat_motors.pb.txt`](../../config/devices/motor/ethercat_motors.pb.txt) +- [`mujoco_motors.pb.txt`](../../config/devices/motor/mujoco_motors.pb.txt) -## Modbus TCP PLC - -实现、配置、依赖和测试入口见： - -- [Modbus TCP runtime README](bus_runtime/modbus_tcp/README.md) -- [完整 MotorService/CMVR PLC v1 协议](../../../docs/motor_service_modbus_tcp.md) -- [MotorService 文档](../../service/README.md#motorservice) -- [`plc_motors.pb.txt`](../../config/devices/motor/plc_motors.pb.txt) - -当前 Modbus 后端只提供 x86-64 的 libmodbus 3.1.11。ARM 目录没有对应库， -不能把 x86 ELF 复制到 ARM 设备使用。 +对外接口与控制权语义见 [MotorService 文档](../../service/README.md#motorservice)。 ## 安全边界 - MotorService 是单轴 API，不提供多轴同扫描周期的原子 commit； -- PLC/Modbus 的 Profile 或 cyclic 能力不能直接当成毫秒级机械臂组伺服； -- 软件 `emergencyStop`、Quick Stop 和普通 PLC 输出都不是安全急停； -- 真实设备必须具有独立的硬接线急停、安全继电器或 F-CPU/F-I/O，以及驱动器 - STO 等经风险评估确定的安全链； -- 新硬件配置保持 `enable: false`，完成方向、限位、watchdog 和故障注入验证后 - 才能启用。 +- 软件 `emergencyStop` 和 Quick Stop 不具备功能安全等级； +- 真实设备必须具有经风险评估确定的硬接线急停、安全继电器和驱动器安全链； +- 新硬件配置保持 `enable: false`，完成方向、限位和故障注入验证后才能启用。 ## 测试 ```bash -cmake --build build --target \ - modbus_tcp_motor_bus_runtime_test \ - grpc_motor_service_test \ - grpc_motor_service_modbus_e2e_test \ - -j4 - +cmake --build build --target grpc_motor_service_test -j4 ctest --test-dir build \ - -R '^(modbus_tcp_motor_bus_runtime_test|grpc_motor_service_test|grpc_motor_service_modbus_e2e_test)$' \ + -R '^grpc_motor_service_test$' \ --output-on-failure ``` -端到端 fake PLC 测试需要本地 TCP bind/listen 权限。软件测试不能代替真实 -S7-1215C、驱动器、STO 和断网故障台架。 +该测试使用 fake motor，不替代真实总线、驱动器或安全链验证。 diff --git a/cmvr-es/devices/motor/bus_runtime/CMakeLists.txt b/cmvr-es/devices/motor/bus_runtime/CMakeLists.txt index e2a9c6f2..458b8e46 100644 --- a/cmvr-es/devices/motor/bus_runtime/CMakeLists.txt +++ b/cmvr-es/devices/motor/bus_runtime/CMakeLists.txt @@ -2,29 +2,19 @@ add_library(motor_bus_runtime SHARED can/src/can_motor_bus_runtime.cpp mujoco/src/mujoco_motor_bus_runtime.cpp ethercat/src/ethercat_motor_bus_runtime.cpp - modbus_tcp/src/modbus_tcp_client.cpp - modbus_tcp/src/modbus_tcp_motor_bus_runtime.cpp ) set(IGH_ETHERCAT_ROOT ${CMAKE_SOURCE_DIR}/dependency/x86/third_party/ethercat/v1.7.0 ) -set(CMVR_LIBMODBUS_ROOT - ${CMAKE_SOURCE_DIR}/dependency/${ARCH}/third_party/modbus/3.1.11 -) - target_include_directories(motor_bus_runtime PUBLIC ${CMAKE_CURRENT_SOURCE_DIR} PRIVATE ${IGH_ETHERCAT_ROOT}/include - ${CMVR_LIBMODBUS_ROOT}/include ) -target_link_directories(motor_bus_runtime PRIVATE - ${IGH_ETHERCAT_ROOT}/lib - ${CMVR_LIBMODBUS_ROOT}/lib -) +target_link_directories(motor_bus_runtime PRIVATE ${IGH_ETHERCAT_ROOT}/lib) target_link_libraries(motor_bus_runtime PUBLIC @@ -33,7 +23,6 @@ target_link_libraries(motor_bus_runtime cmvr_es::mujoco_world PRIVATE ethercat - modbus cmvr_es::device::canbus glog ) diff --git a/cmvr-es/devices/motor/bus_runtime/modbus_tcp/README.md b/cmvr-es/devices/motor/bus_runtime/modbus_tcp/README.md deleted file mode 100644 index f1ac4f18..00000000 --- a/cmvr-es/devices/motor/bus_runtime/modbus_tcp/README.md +++ /dev/null @@ -1,93 +0,0 @@ -# Modbus TCP PLC Runtime - -本目录实现 CMVR PLC v1 的 Modbus TCP 总线 runtime。它负责 PLC 连接、身份 -握手、session、心跳、重连、命令 mailbox、ACK 轮询和状态快照，不负责 gRPC -请求解析，也不提供功能安全急停。 - -返回 [Motor 设备模块](../../README.md) 或 [Devices 模块指南](../../../README.md)。 - -## 代码与配置 - -- [`include/modbus_tcp_client.h`](include/modbus_tcp_client.h)：有界超时的 - libmodbus client -- [`include/modbus_tcp_motor_bus_runtime.h`](include/modbus_tcp_motor_bus_runtime.h)： - 连接 supervisor、命令和状态 runtime -- [`include/cmvr_plc_register_map.h`](include/cmvr_plc_register_map.h)： - CMVR PLC v1 寄存器常量 -- [`src/modbus_tcp_client.cpp`](src/modbus_tcp_client.cpp) -- [`src/modbus_tcp_motor_bus_runtime.cpp`](src/modbus_tcp_motor_bus_runtime.cpp) -- [`tests/modbus_tcp_motor_bus_runtime_test.cpp`](tests/modbus_tcp_motor_bus_runtime_test.cpp) -- [`../../../../config/devices/motor/plc_motors.pb.txt`](../../../../config/devices/motor/plc_motors.pb.txt) - -完整寄存器表、TIA Portal 数据块、gRPC 语义和台架步骤由 -[MotorService 与 CMVR PLC v1 完整协议](../../../../../docs/motor_service_modbus_tcp.md) -统一维护。 - -## 连接与协议约束 - -- `host` 必须是 IPv4 字面量，避免 DNS 让建连或停止出现无界等待； -- PLC boot ID 必须非零且每次 PLC 重启变化； -- owner 决策和命令 ACK 必须回显当前 session，旧 session 的 mailbox 不得执行； -- runtime `start()` 启动连接 supervisor，PLC 可以在进程启动时离线； -- 离线期间状态不可用，运动命令必须在写 mailbox 前失败； -- 重连必须重新完成身份、版本、boot ID 和 session 握手，不得重放旧命令； -- 状态区按 odd/even seqlock 发布，CMVR 使用 - sequence-before → 64-word block → sequence-after 三段读取； -- payload 必须先完整写入，commit sequence 最后写入，PLC 只原子消费新的 - commit； -- Quick Stop、Disable、故障和通信 watchdog 的状态必须通过当前 session - 的 ACK/状态确认，不能把本地写成功解释为驱动器已经安全停止。 - -## Cyclic stream - -每次 `OpenCyclicPosition/Velocity` 都创建新的轴级 stream epoch。PLC 必须在 -同一个原子状态事务中： - -1. 清零 `last_applied_cyclic_sequence`； -2. 清理旧样本去重状态； -3. 重置 cyclic watchdog； -4. 设置正确的 CSP/CSV mode； -5. 发布 `StreamActive=1` 后再 ACK。 - -重开后的首个样本序列从 `1` 开始，必须重新应用。活动 cyclic 流跨 -`connection_epoch` 后不会自动重开；旧流的当前和后续 setpoint 都被拒绝， -Quick Stop 后客户端必须建立新的 gRPC 流。断链前或断链期间 pending 的 -setpoint 不得进入新 session。 - -## 依赖与平台 - -x86-64 的 libmodbus 3.1.11 位于： - -```text -dependency/x86/third_party/modbus/3.1.11 -``` - -`dependency/arm/third_party/` 当前没有对应 libmodbus，因此 ARM 构建不支持 -该后端。支持 ARM 前必须为目标 ABI 单独编译并验证库，不能复用 x86 二进制。 - -## 安全边界 - -标准 S7-1215C DC/DC/DC 不是 failsafe PLC。Modbus Quick Stop、 -MotorService `emergencyStop`、普通 OB/FB 和普通数字输出都只是功能性控制， -不能替代： - -- 硬接线急停； -- 安全继电器或 F-CPU/F-I/O； -- 驱动器双通道 STO； -- 接触器、抱闸反馈与必要的 EDM。 - -PLC 侧通信 watchdog 和 cyclic watchdog 必须在没有 CMVR 进程参与时独立停止 -危险运动。真实启用前必须在禁能或脱载轴上验证寄存器、方向、限位、断网、PLC -重启、交换机故障和 Quick Stop 失败。 - -## 测试 - -```bash -cmake --build build --target modbus_tcp_motor_bus_runtime_test -j4 -ctest --test-dir build \ - -R '^modbus_tcp_motor_bus_runtime_test$' \ - --output-on-failure -``` - -fake PLC 测试需要本地 TCP bind/listen 权限。测试通过只证明软件协议和故障注入 -路径，不代表真实 PLC、驱动器或硬件安全链已经验收。 diff --git a/cmvr-es/devices/motor/bus_runtime/modbus_tcp/include/cmvr_plc_register_map.h b/cmvr-es/devices/motor/bus_runtime/modbus_tcp/include/cmvr_plc_register_map.h deleted file mode 100644 index 4c09afa9..00000000 --- a/cmvr-es/devices/motor/bus_runtime/modbus_tcp/include/cmvr_plc_register_map.h +++ /dev/null @@ -1,209 +0,0 @@ -#ifndef CMVR_ES_CMVR_PLC_REGISTER_MAP_H -#define CMVR_ES_CMVR_PLC_REGISTER_MAP_H - -#include -#include -#include - -namespace cmvr::device::cmvr_plc { - -constexpr std::uint16_t kMagicCm = 0x434d; -constexpr std::uint16_t kMagicVr = 0x5652; -constexpr std::uint16_t kProtocolMajor = 1; -constexpr std::uint16_t kProtocolMinor = 0; -constexpr double kPositionScale = 1000000.0; -constexpr double kVelocityScale = 1000000.0; -constexpr double kAccelerationScale = 1000000.0; -constexpr double kTorqueScale = 1000.0; - -constexpr int kGlobalRegisterCount = 32; -constexpr int kMagicCmOffset = 0; -constexpr int kMagicVrOffset = 1; -constexpr int kProtocolMajorOffset = 2; -constexpr int kProtocolMinorOffset = 3; -constexpr int kAxisCountOffset = 4; -constexpr int kPlcGlobalStateOffset = 5; -constexpr int kPlcBootIdOffset = 6; -constexpr int kCmvrSessionIdOffset = 8; -constexpr int kCmvrHeartbeatOffset = 10; -constexpr int kPlcHeartbeatOffset = 12; -constexpr int kCommunicationWatchdogOffset = 14; -constexpr int kGlobalErrorOffset = 16; -constexpr int kOwnerStateOffset = 17; -constexpr int kOwnerSessionIdOffset = 18; - -constexpr int kAxisFirstOffset = 100; -constexpr int kAxisRegisterStride = 128; -constexpr int kAxisControlRegisterCount = 64; -constexpr int kAxisStatusRelativeOffset = 64; -constexpr int kAxisStatusRegisterCount = 64; - -constexpr int axisBase(const std::uint32_t axis_index) -{ - return kAxisFirstOffset + static_cast(axis_index) * kAxisRegisterStride; -} - -enum class CommandCode : std::uint16_t { - Nop = 0, - SetZero = 1, - MoveToZero = 2, - ProfilePosition = 3, - ProfileVelocity = 4, - OpenCyclicPosition = 5, - CyclicPositionSample = 6, - OpenCyclicVelocity = 7, - CyclicVelocitySample = 8, - CloseCyclicStream = 9, - QuickStop = 10, - Enable = 11, - Disable = 12, -}; - -enum class CommandState : std::uint16_t { - Idle = 0, - Received = 1, - Validating = 2, - Accepted = 3, - Running = 4, - TargetReached = 5, - Completed = 6, - Rejected = 7, - Failed = 8, - TimedOut = 9, - QuickStopped = 10, - CommunicationLost = 11, -}; - -enum class ResultCode : std::uint16_t { - Ok = 0, - InvalidCommand = 1, - InvalidParameter = 2, - AxisNotReady = 3, - AxisBusy = 4, - NotEnabled = 5, - PositionLimit = 6, - VelocityLimit = 7, - AccelerationLimit = 8, - ZeroNotValid = 9, - DriveFault = 10, - CommandTimeout = 11, - SequenceError = 12, - SessionMismatch = 13, - CommunicationWatchdog = 14, - CyclicWatchdog = 15, - Unsupported = 16, - InternalError = 17, -}; - -enum StatusFlag : std::uint16_t { - Enabled = 1U << 0U, - Moving = 1U << 1U, - TargetReached = 1U << 2U, - Fault = 1U << 3U, - QuickStopActive = 1U << 4U, - CommunicationWatchdogExpired = 1U << 5U, - CyclicWatchdogExpired = 1U << 6U, - ZeroValid = 1U << 7U, - StreamActive = 1U << 8U, - CommandBusy = 1U << 9U, -}; - -constexpr std::size_t kCommandPayloadRegisterCount = 62; -constexpr int kCommitSequenceRelativeOffset = 62; - -constexpr std::size_t kPayloadSequence = 0; -constexpr std::size_t kCommandCode = 2; -constexpr std::size_t kCommandFlags = 3; -constexpr std::size_t kTargetPosition = 4; -constexpr std::size_t kTargetVelocity = 6; -constexpr std::size_t kAcceleration = 8; -constexpr std::size_t kTargetTorque = 10; -constexpr std::size_t kPositionTolerance = 12; -constexpr std::size_t kVelocityTolerance = 14; -constexpr std::size_t kCommandTimeout = 16; -constexpr std::size_t kStreamWatchdog = 18; -constexpr std::size_t kCyclicSampleSequence = 20; -constexpr std::size_t kClientMonotonicTime = 22; -constexpr std::size_t kExpectedZeroEpoch = 24; -constexpr std::size_t kDisconnectAction = 26; -constexpr std::size_t kCommandSessionId = 27; -constexpr std::size_t kPayloadSequenceMirror = 60; - -constexpr std::size_t kAckSequence = 0; -constexpr std::size_t kActiveSequence = 2; -constexpr std::size_t kCommandState = 4; -constexpr std::size_t kResultCode = 5; -constexpr std::size_t kAxisState = 6; -constexpr std::size_t kCurrentMode = 7; -constexpr std::size_t kActualPosition = 8; -constexpr std::size_t kActualVelocity = 10; -constexpr std::size_t kActualTorque = 12; -constexpr std::size_t kTargetPositionStatus = 14; -constexpr std::size_t kTargetVelocityStatus = 16; -constexpr std::size_t kStatusFlags = 18; -constexpr std::size_t kDriveStatusword = 19; -constexpr std::size_t kFaultCode = 20; -constexpr std::size_t kZeroEpoch = 22; -constexpr std::size_t kLastAppliedCyclicSequence = 24; -constexpr std::size_t kStateSequence = 26; -constexpr std::size_t kPlcMonotonicTime = 28; -constexpr std::size_t kHeartbeatAge = 30; -constexpr std::size_t kAckSessionId = 32; -constexpr std::size_t kStateSequenceMirror = 62; - -enum class OwnerState : std::uint16_t { - None = 0, - Accepting = 1, - Accepted = 2, - Rejected = 3, -}; - -inline void encodeUint32(std::uint16_t* registers, - const std::size_t offset, - const std::uint32_t value) -{ - registers[offset] = static_cast(value >> 16U); - registers[offset + 1] = static_cast(value & 0xffffU); -} - -inline void encodeInt32(std::uint16_t* registers, - const std::size_t offset, - const std::int32_t value) -{ - encodeUint32(registers, offset, static_cast(value)); -} - -inline std::uint32_t decodeUint32(const std::uint16_t* registers, - const std::size_t offset) -{ - return (static_cast(registers[offset]) << 16U) | - static_cast(registers[offset + 1]); -} - -inline std::int32_t decodeInt32(const std::uint16_t* registers, - const std::size_t offset) -{ - return static_cast(decodeUint32(registers, offset)); -} - -inline bool isTerminal(const CommandState state) -{ - return state == CommandState::Completed || - state == CommandState::Rejected || - state == CommandState::Failed || - state == CommandState::TimedOut || - state == CommandState::QuickStopped || - state == CommandState::CommunicationLost; -} - -inline bool isFailure(const CommandState state) -{ - return state == CommandState::Rejected || - state == CommandState::Failed || - state == CommandState::TimedOut || - state == CommandState::CommunicationLost; -} - -} // namespace cmvr::device::cmvr_plc - -#endif // CMVR_ES_CMVR_PLC_REGISTER_MAP_H diff --git a/cmvr-es/devices/motor/bus_runtime/modbus_tcp/include/modbus_tcp_client.h b/cmvr-es/devices/motor/bus_runtime/modbus_tcp/include/modbus_tcp_client.h deleted file mode 100644 index 2a8015bc..00000000 --- a/cmvr-es/devices/motor/bus_runtime/modbus_tcp/include/modbus_tcp_client.h +++ /dev/null @@ -1,47 +0,0 @@ -#ifndef CMVR_ES_MODBUS_TCP_CLIENT_H -#define CMVR_ES_MODBUS_TCP_CLIENT_H - -#include -#include -#include -#include - -struct _modbus; -using modbus_t = struct _modbus; - -namespace cmvr::device { - -class ModbusTcpClient { -public: - ModbusTcpClient() = default; - ~ModbusTcpClient(); - - ModbusTcpClient(const ModbusTcpClient&) = delete; - ModbusTcpClient& operator=(const ModbusTcpClient&) = delete; - - bool open(const std::string& host, - std::uint16_t port, - std::uint8_t unit_id, - std::uint32_t connect_timeout_ms, - std::uint32_t response_timeout_ms); - void close(); - void shutdown(); - bool connected() const { return context_ != nullptr; } - - bool readHoldingRegisters(int address, int count, std::vector& values); - bool writeHoldingRegisters(int address, const std::uint16_t* values, int count); - bool writeHoldingRegisters(int address, const std::vector& values); - - const std::string& lastError() const { return last_error_; } - -private: - void setLastErrnoError_(const char* operation); - - modbus_t* context_{nullptr}; - std::atomic socket_fd_{-1}; - std::string last_error_; -}; - -} // namespace cmvr::device - -#endif // CMVR_ES_MODBUS_TCP_CLIENT_H diff --git a/cmvr-es/devices/motor/bus_runtime/modbus_tcp/include/modbus_tcp_motor_bus_runtime.h b/cmvr-es/devices/motor/bus_runtime/modbus_tcp/include/modbus_tcp_motor_bus_runtime.h deleted file mode 100644 index 110dbd3f..00000000 --- a/cmvr-es/devices/motor/bus_runtime/modbus_tcp/include/modbus_tcp_motor_bus_runtime.h +++ /dev/null @@ -1,173 +0,0 @@ -#ifndef CMVR_ES_MODBUS_TCP_MOTOR_BUS_RUNTIME_H -#define CMVR_ES_MODBUS_TCP_MOTOR_BUS_RUNTIME_H - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "devices/motor/bus_runtime/abstract_motor_bus_runtime.h" -#include "devices/motor/bus_runtime/modbus_tcp/include/cmvr_plc_register_map.h" -#include "devices/motor/bus_runtime/modbus_tcp/include/modbus_tcp_client.h" - -namespace cmvr::device { - -struct ModbusTcpMotorBusRuntimeTestAccess; - -struct CmvrPlcAxisCommand { - cmvr_plc::CommandCode code{cmvr_plc::CommandCode::Nop}; - std::uint16_t flags{0}; - std::int32_t target_position{0}; - std::int32_t target_velocity{0}; - std::int32_t acceleration{0}; - std::int32_t target_torque{0}; - std::int32_t position_tolerance{0}; - std::int32_t velocity_tolerance{0}; - std::uint32_t command_timeout_ms{0}; - std::uint32_t stream_watchdog_ms{0}; - std::uint32_t cyclic_sample_sequence{0}; - std::uint32_t client_monotonic_time_ms{0}; - std::uint32_t expected_zero_epoch{0}; - std::uint16_t disconnect_action{0}; -}; - -struct CmvrPlcAxisStatus { - std::uint32_t ack_sequence{0}; - std::uint32_t active_sequence{0}; - cmvr_plc::CommandState command_state{cmvr_plc::CommandState::Idle}; - cmvr_plc::ResultCode result_code{cmvr_plc::ResultCode::Ok}; - std::uint16_t axis_state{0}; - std::uint16_t current_mode{0}; - std::int32_t actual_position{0}; - std::int32_t actual_velocity{0}; - std::int32_t actual_torque{0}; - std::int32_t target_position{0}; - std::int32_t target_velocity{0}; - std::uint16_t status_flags{0}; - std::uint16_t drive_statusword{0}; - std::uint32_t fault_code{0}; - std::uint32_t zero_epoch{0}; - std::uint32_t last_applied_cyclic_sequence{0}; - std::uint32_t state_sequence{0}; - std::uint32_t plc_monotonic_time_ms{0}; - std::uint32_t heartbeat_age_ms{0}; - std::uint32_t ack_session_id{0}; -}; - -class ModbusTcpMotorBusRuntime final : public AbstractMotorBusRuntime { -public: - ModbusTcpMotorBusRuntime(); - ~ModbusTcpMotorBusRuntime() override; - - bool init(const config::MotorGroupConfig& group_cfg) override; - bool start() override; - void stop() override; - config::MotorBusType busType() const override { return config::MOTOR_BUS_MODBUS_TCP; } - - bool hasMotor(std::uint8_t motor_id) const; - bool axisForMotor(std::uint8_t motor_id, std::uint32_t& axis_index) const; - bool connected() const { return connected_.load(); } - std::uint32_t sessionId() const { return session_id_.load(); } - std::uint32_t plcBootId() const { return plc_boot_id_.load(); } - std::uint64_t connectionEpoch() const { return connection_epoch_.load(); } - std::uint32_t streamWatchdogMs() const { return stream_watchdog_ms_; } - std::uint32_t commandAckTimeoutMs() const { return command_ack_timeout_ms_; } - const std::string& id() const { return id_; } - std::string lastError() const; - - bool readAxisStatus(std::uint8_t motor_id, CmvrPlcAxisStatus& status); - bool submitAxisCommand(std::uint8_t motor_id, - const CmvrPlcAxisCommand& command, - bool wait_for_terminal_state = false, - std::optional - expected_connection_epoch = std::nullopt); - bool submitAxisSafetyCommand(std::uint8_t motor_id, - const CmvrPlcAxisCommand& command); - -private: - friend struct ModbusTcpMotorBusRuntimeTestAccess; - - bool connectAndHandshakeLocked_(); - void markDisconnectedLocked_(const std::string& error); - bool readRegistersLocked_(int address, int count, std::vector& values); - bool writeRegistersLocked_(int address, const std::uint16_t* values, int count); - bool writeHeartbeatLocked_(); - bool readAxisStatusByIndex_(std::uint32_t axis_index, CmvrPlcAxisStatus& status); - bool waitForCommand_(std::uint32_t axis_index, - std::uint32_t command_sequence, - std::uint32_t command_session_id, - std::uint64_t connection_epoch, - std::uint64_t cancel_generation, - bool cancel_on_safety_preemption, - const CmvrPlcAxisCommand& command, - bool wait_for_terminal_state, - const CmvrPlcAxisStatus& initial_status); - bool submitAxisCommandImpl_(std::uint8_t motor_id, - const CmvrPlcAxisCommand& command, - bool wait_for_terminal_state, - bool safety_priority, - std::optional - expected_connection_epoch); - bool writeClientHeartbeatLocked_(); - std::shared_ptr axisMutex_(std::uint8_t motor_id) const; - std::shared_ptr safetyMutex_(std::uint8_t motor_id) const; - void workerLoop_(); - static std::uint32_t randomNonZeroSessionId_(); - static std::uint32_t monotonicMilliseconds_(); - - std::string id_; - config::ModbusTcpConfig config_; - std::unordered_map motor_axes_; - mutable std::unordered_map> axis_mutexes_; - mutable std::unordered_map> safety_mutexes_; - std::unordered_map command_sequences_; - std::unordered_map cancel_generations_; - // Monotonic admission counter protected by state_mutex_. Besides being - // useful when diagnosing queueing, it gives concurrency tests an exact - // synchronization point after an invocation has captured its safety - // generation and connection epoch, but before it waits on a command - // serialization mutex. - std::uint64_t command_admission_count_{0}; - - mutable std::mutex io_mutex_; - mutable std::mutex state_mutex_; - mutable std::mutex lifecycle_mutex_; - ModbusTcpClient client_; - std::string last_error_; - std::thread worker_; - std::mutex worker_wait_mutex_; - std::condition_variable worker_wait_cv_; - std::atomic running_{false}; - std::atomic connected_{false}; - std::atomic plc_boot_id_{0}; - std::atomic connection_epoch_{0}; - std::atomic session_id_{0}; - std::uint32_t last_session_id_{0}; - std::uint32_t heartbeat_counter_{0}; - std::chrono::steady_clock::time_point last_client_heartbeat_write_at_{}; - std::uint32_t plc_heartbeat_counter_{0}; - std::chrono::steady_clock::time_point plc_heartbeat_changed_at_{}; - std::uint32_t connect_timeout_ms_{500}; - std::uint32_t io_timeout_ms_{100}; - std::uint32_t heartbeat_period_ms_{100}; - std::uint32_t communication_watchdog_ms_{500}; - std::uint32_t status_poll_period_ms_{20}; - std::uint32_t reconnect_min_ms_{100}; - std::uint32_t reconnect_max_ms_{2000}; - std::uint32_t command_ack_timeout_ms_{500}; - std::uint32_t stream_watchdog_ms_{500}; - std::uint16_t protocol_major_{cmvr_plc::kProtocolMajor}; - std::uint16_t protocol_minor_{cmvr_plc::kProtocolMinor}; -}; - -} // namespace cmvr::device - -#endif // CMVR_ES_MODBUS_TCP_MOTOR_BUS_RUNTIME_H diff --git a/cmvr-es/devices/motor/bus_runtime/modbus_tcp/src/modbus_tcp_client.cpp b/cmvr-es/devices/motor/bus_runtime/modbus_tcp/src/modbus_tcp_client.cpp deleted file mode 100644 index c2905f5e..00000000 --- a/cmvr-es/devices/motor/bus_runtime/modbus_tcp/src/modbus_tcp_client.cpp +++ /dev/null @@ -1,188 +0,0 @@ -#include "devices/motor/bus_runtime/modbus_tcp/include/modbus_tcp_client.h" - -#include -#include -#include -#include -#include -#include -#include - -#include - -namespace cmvr::device { - -ModbusTcpClient::~ModbusTcpClient() -{ - close(); -} - -bool ModbusTcpClient::open(const std::string& host, - const std::uint16_t port, - const std::uint8_t unit_id, - const std::uint32_t connect_timeout_ms, - const std::uint32_t response_timeout_ms) -{ - close(); - context_ = modbus_new_tcp(host.c_str(), static_cast(port)); - if (!context_) { - last_error_ = "modbus_new_tcp failed"; - return false; - } - if (modbus_set_slave(context_, static_cast(unit_id)) == -1) { - setLastErrnoError_("modbus_set_slave"); - close(); - return false; - } - - const auto timeout = response_timeout_ms == 0 ? 200U : response_timeout_ms; - if (modbus_set_response_timeout(context_, timeout / 1000U, - (timeout % 1000U) * 1000U) == -1 || - modbus_set_byte_timeout(context_, timeout / 1000U, - (timeout % 1000U) * 1000U) == -1) { - setLastErrnoError_("modbus_set_timeout"); - close(); - return false; - } - addrinfo hints{}; - hints.ai_family = AF_INET; - hints.ai_socktype = SOCK_STREAM; - hints.ai_flags = AI_NUMERICHOST; - addrinfo* addresses = nullptr; - const auto service = std::to_string(port); - const auto resolve_result = - getaddrinfo(host.c_str(), service.c_str(), &hints, &addresses); - if (resolve_result != 0) { - last_error_ = std::string("host must be a numeric IPv4 address: ") + - gai_strerror(resolve_result); - close(); - return false; - } - int connected_fd = -1; - for (auto* address = addresses; address; address = address->ai_next) { - const int fd = ::socket(address->ai_family, address->ai_socktype, address->ai_protocol); - if (fd < 0) { - continue; - } - const int old_flags = fcntl(fd, F_GETFL, 0); - if (old_flags < 0 || fcntl(fd, F_SETFL, old_flags | O_NONBLOCK) < 0) { - ::close(fd); - continue; - } - socket_fd_.store(fd); - const int rc = ::connect(fd, address->ai_addr, address->ai_addrlen); - if (rc == 0 || errno == EINPROGRESS) { - pollfd descriptor{fd, POLLOUT, 0}; - const auto timeout = static_cast( - connect_timeout_ms == 0 ? 1000U : connect_timeout_ms); - if (rc == 0 || poll(&descriptor, 1, timeout) > 0) { - int socket_error = 0; - socklen_t error_size = sizeof(socket_error); - if (getsockopt(fd, SOL_SOCKET, SO_ERROR, &socket_error, &error_size) == 0 && - socket_error == 0) { - if (fcntl(fd, F_SETFL, old_flags) == 0) { - connected_fd = fd; - break; - } - } - } - } - socket_fd_.store(-1); - ::close(fd); - } - freeaddrinfo(addresses); - if (connected_fd < 0 || modbus_set_socket(context_, connected_fd) == -1) { - if (connected_fd >= 0) { - ::close(connected_fd); - } - last_error_ = "Modbus TCP connect timed out or failed"; - close(); - return false; - } - socket_fd_.store(connected_fd); - last_error_.clear(); - return true; -} - -void ModbusTcpClient::close() -{ - socket_fd_.store(-1); - if (context_) { - modbus_close(context_); - modbus_free(context_); - context_ = nullptr; - } -} - -void ModbusTcpClient::shutdown() -{ - const auto fd = socket_fd_.load(); - if (fd >= 0) { - ::shutdown(fd, SHUT_RDWR); - } -} - -bool ModbusTcpClient::readHoldingRegisters( - const int address, - const int count, - std::vector& values) -{ - if (!context_) { - last_error_ = "Modbus TCP connection is closed"; - return false; - } - if (address < 0 || count <= 0 || count > MODBUS_MAX_READ_REGISTERS) { - last_error_ = "invalid holding-register read range"; - return false; - } - - values.assign(static_cast(count), 0); - const auto rc = modbus_read_registers(context_, address, count, values.data()); - if (rc != count) { - setLastErrnoError_("modbus_read_registers"); - return false; - } - last_error_.clear(); - return true; -} - -bool ModbusTcpClient::writeHoldingRegisters( - const int address, - const std::uint16_t* values, - const int count) -{ - if (!context_) { - last_error_ = "Modbus TCP connection is closed"; - return false; - } - if (address < 0 || !values || count <= 0 || count > MODBUS_MAX_WRITE_REGISTERS) { - last_error_ = "invalid holding-register write range"; - return false; - } - - const auto rc = modbus_write_registers(context_, address, count, values); - if (rc != count) { - setLastErrnoError_("modbus_write_registers"); - return false; - } - last_error_.clear(); - return true; -} - -bool ModbusTcpClient::writeHoldingRegisters( - const int address, - const std::vector& values) -{ - if (values.size() > static_cast(std::numeric_limits::max())) { - last_error_ = "holding-register write is too large"; - return false; - } - return writeHoldingRegisters(address, values.data(), static_cast(values.size())); -} - -void ModbusTcpClient::setLastErrnoError_(const char* operation) -{ - last_error_ = std::string(operation) + ": " + modbus_strerror(errno); -} - -} // namespace cmvr::device diff --git a/cmvr-es/devices/motor/bus_runtime/modbus_tcp/src/modbus_tcp_motor_bus_runtime.cpp b/cmvr-es/devices/motor/bus_runtime/modbus_tcp/src/modbus_tcp_motor_bus_runtime.cpp deleted file mode 100644 index 849a6358..00000000 --- a/cmvr-es/devices/motor/bus_runtime/modbus_tcp/src/modbus_tcp_motor_bus_runtime.cpp +++ /dev/null @@ -1,1013 +0,0 @@ -#include "devices/motor/bus_runtime/modbus_tcp/include/modbus_tcp_motor_bus_runtime.h" - -#include -#include -#include -#include -#include -#include -#include -#include - -#include "cmvr/msgs/motor.pb.h" -#include "common/base/logging/logger.h" - -namespace cmvr::device { -namespace { - -std::uint32_t valueOrDefault(const std::uint32_t value, const std::uint32_t fallback) -{ - return value == 0 ? fallback : value; -} - -bool validCommandState(const cmvr_plc::CommandState state) -{ - return static_cast(state) <= - static_cast(cmvr_plc::CommandState::CommunicationLost); -} - -} // namespace - -ModbusTcpMotorBusRuntime::ModbusTcpMotorBusRuntime() = default; - -ModbusTcpMotorBusRuntime::~ModbusTcpMotorBusRuntime() -{ - stop(); -} - -bool ModbusTcpMotorBusRuntime::init(const config::MotorGroupConfig& group_cfg) -{ - stop(); - id_ = group_cfg.id(); - if (id_.empty() || group_cfg.bus_type() != config::MOTOR_BUS_MODBUS_TCP || - !group_cfg.has_modbus_tcp()) { - CMVR_LOG(ERROR) << "[ModbusTcpMotorBusRuntime] invalid group configuration"; - return false; - } - if (group_cfg.protocol() != config::MOTOR_PROTOCOL_CMVR_PLC_V1) { - CMVR_LOG(ERROR) << "[ModbusTcpMotorBusRuntime] unsupported protocol for group: " << id_; - return false; - } - - config_ = group_cfg.modbus_tcp(); - in_addr ipv4_address{}; - if (config_.host().empty() || - inet_pton(AF_INET, config_.host().c_str(), &ipv4_address) != 1 || - config_.port() > 65535U || config_.unit_id() > 247U) { - CMVR_LOG(ERROR) << "[ModbusTcpMotorBusRuntime] invalid endpoint for group: " << id_; - return false; - } - if (config_.axes().empty()) { - CMVR_LOG(ERROR) << "[ModbusTcpMotorBusRuntime] no PLC axes configured for group: " << id_; - return false; - } - - motor_axes_.clear(); - axis_mutexes_.clear(); - safety_mutexes_.clear(); - command_sequences_.clear(); - cancel_generations_.clear(); - command_admission_count_ = 0; - std::unordered_map axes_to_motors; - for (const auto& axis : config_.axes()) { - if (axis.motor_id() < 0 || axis.motor_id() > 255) { - CMVR_LOG(ERROR) << "[ModbusTcpMotorBusRuntime] motor id is outside uint8 range"; - return false; - } - const auto motor_id = static_cast(axis.motor_id()); - const auto last_register = - static_cast(cmvr_plc::kAxisFirstOffset) + - static_cast(axis.axis_index()) * - static_cast(cmvr_plc::kAxisRegisterStride) + - static_cast(cmvr_plc::kAxisRegisterStride - 1); - if (last_register > 65535U || - last_register > static_cast(std::numeric_limits::max())) { - CMVR_LOG(ERROR) << "[ModbusTcpMotorBusRuntime] axis_index maps outside the " - "Modbus address space: " - << axis.axis_index(); - return false; - } - if (!motor_axes_.emplace(motor_id, axis.axis_index()).second || - !axes_to_motors.emplace(axis.axis_index(), motor_id).second) { - CMVR_LOG(ERROR) << "[ModbusTcpMotorBusRuntime] duplicate motor or axis mapping in " - << id_; - return false; - } - axis_mutexes_.emplace(motor_id, std::make_shared()); - safety_mutexes_.emplace(motor_id, std::make_shared()); - command_sequences_.emplace(motor_id, 0U); - cancel_generations_.emplace(motor_id, 0U); - } - - connect_timeout_ms_ = valueOrDefault(config_.connect_timeout_ms(), 500U); - io_timeout_ms_ = valueOrDefault(config_.io_timeout_ms(), 100U); - heartbeat_period_ms_ = valueOrDefault(config_.heartbeat_period_ms(), 100U); - communication_watchdog_ms_ = - valueOrDefault(config_.communication_watchdog_ms(), 500U); - status_poll_period_ms_ = valueOrDefault(config_.status_poll_period_ms(), 20U); - reconnect_min_ms_ = valueOrDefault(config_.reconnect_min_ms(), 100U); - reconnect_max_ms_ = valueOrDefault(config_.reconnect_max_ms(), 2000U); - command_ack_timeout_ms_ = valueOrDefault(config_.command_ack_timeout_ms(), 500U); - protocol_major_ = static_cast( - valueOrDefault(config_.protocol_major(), cmvr_plc::kProtocolMajor)); - protocol_minor_ = static_cast(config_.protocol_minor()); - stream_watchdog_ms_ = valueOrDefault(config_.cyclic_watchdog_ms(), 500U); - if (config_.protocol_major() > std::numeric_limits::max() || - config_.protocol_minor() > std::numeric_limits::max() || - connect_timeout_ms_ < 10U || connect_timeout_ms_ > 10000U || - io_timeout_ms_ < 10U || io_timeout_ms_ > 2000U || - heartbeat_period_ms_ < 10U || heartbeat_period_ms_ > 60000U || - communication_watchdog_ms_ > 120000U || - static_cast(communication_watchdog_ms_) < - static_cast(heartbeat_period_ms_) * 2U || - status_poll_period_ms_ < 1U || status_poll_period_ms_ > 1000U || - reconnect_min_ms_ < 10U || reconnect_min_ms_ > 60000U || - reconnect_max_ms_ > 120000U || - command_ack_timeout_ms_ < io_timeout_ms_ * 3U + status_poll_period_ms_ || - command_ack_timeout_ms_ > 60000U || - stream_watchdog_ms_ < 20U || stream_watchdog_ms_ > 60000U || - communication_watchdog_ms_ < - io_timeout_ms_ * 3U + heartbeat_period_ms_ * 2U || - stream_watchdog_ms_ < io_timeout_ms_ * 3U + status_poll_period_ms_ || - reconnect_max_ms_ < reconnect_min_ms_) { - CMVR_LOG(ERROR) << "[ModbusTcpMotorBusRuntime] invalid watchdog/reconnect timing for " - << id_; - return false; - } - session_id_ = 0; - heartbeat_counter_ = 0; - return true; -} - -bool ModbusTcpMotorBusRuntime::start() -{ - std::lock_guard lifecycle_lock(lifecycle_mutex_); - if (running_.load()) { - return true; - } - if (id_.empty() || motor_axes_.empty()) { - CMVR_LOG(ERROR) << "[ModbusTcpMotorBusRuntime] runtime is not initialized"; - return false; - } - - running_.store(true); - { - std::lock_guard lock(io_mutex_); - if (!connectAndHandshakeLocked_()) { - CMVR_LOG(WARNING) - << "[ModbusTcpMotorBusRuntime] PLC is offline or incompatible at " - << "startup; supervisor will retry group=" << id_ - << ", error=" << lastError(); - } - } - if (running_.load()) { - worker_ = std::thread(&ModbusTcpMotorBusRuntime::workerLoop_, this); - } - return true; -} - -void ModbusTcpMotorBusRuntime::stop() -{ - running_.store(false); - connected_.store(false); - session_id_.store(0); - connection_epoch_.fetch_add(1); - { - std::lock_guard lock(state_mutex_); - for (auto& [motor, generation] : cancel_generations_) { - (void)motor; - ++generation; - } - } - worker_wait_cv_.notify_all(); - client_.shutdown(); - std::lock_guard lifecycle_lock(lifecycle_mutex_); - const bool restarted_during_stop = running_.exchange(false); - connected_.store(false); - session_id_.store(0); - if (restarted_during_stop) { - connection_epoch_.fetch_add(1); - std::lock_guard state_lock(state_mutex_); - for (auto& [motor, generation] : cancel_generations_) { - (void)motor; - ++generation; - } - } - worker_wait_cv_.notify_all(); - client_.shutdown(); - if (worker_.joinable()) { - worker_.join(); - } - std::lock_guard lock(io_mutex_); - client_.close(); - connected_.store(false); - session_id_.store(0); -} - -bool ModbusTcpMotorBusRuntime::hasMotor(const std::uint8_t motor_id) const -{ - return motor_axes_.count(motor_id) != 0; -} - -bool ModbusTcpMotorBusRuntime::axisForMotor( - const std::uint8_t motor_id, - std::uint32_t& axis_index) const -{ - const auto it = motor_axes_.find(motor_id); - if (it == motor_axes_.end()) { - return false; - } - axis_index = it->second; - return true; -} - -std::string ModbusTcpMotorBusRuntime::lastError() const -{ - std::lock_guard lock(state_mutex_); - return last_error_; -} - -bool ModbusTcpMotorBusRuntime::readAxisStatus( - const std::uint8_t motor_id, - CmvrPlcAxisStatus& status) -{ - std::uint32_t axis_index = 0; - if (!axisForMotor(motor_id, axis_index)) { - std::lock_guard lock(state_mutex_); - last_error_ = "motor id has no PLC axis mapping"; - return false; - } - return readAxisStatusByIndex_(axis_index, status); -} - -bool ModbusTcpMotorBusRuntime::submitAxisCommand( - const std::uint8_t motor_id, - const CmvrPlcAxisCommand& command, - const bool wait_for_terminal_state, - const std::optional expected_connection_epoch) -{ - return submitAxisCommandImpl_( - motor_id, command, wait_for_terminal_state, false, - expected_connection_epoch); -} - -bool ModbusTcpMotorBusRuntime::submitAxisSafetyCommand( - const std::uint8_t motor_id, - const CmvrPlcAxisCommand& command) -{ - return submitAxisCommandImpl_( - motor_id, command, true, true, std::nullopt); -} - -bool ModbusTcpMotorBusRuntime::submitAxisCommandImpl_( - const std::uint8_t motor_id, - const CmvrPlcAxisCommand& command, - const bool wait_for_terminal_state, - const bool safety_priority, - const std::optional expected_connection_epoch) -{ - // Snapshot the connection generation at invocation entry. In particular, - // do not let a caller that arrived while the runtime was offline, or one - // that later waits behind a queue/handshake, silently attach itself to a - // newer PLC ownership session. - const auto invocation_connection_epoch = - connection_epoch_.load(std::memory_order_acquire); - if (!running_.load(std::memory_order_acquire) || - !connected_.load(std::memory_order_acquire) || - connection_epoch_.load(std::memory_order_acquire) != - invocation_connection_epoch || - (expected_connection_epoch.has_value() && - invocation_connection_epoch != *expected_connection_epoch)) { - std::lock_guard lock(state_mutex_); - last_error_ = expected_connection_epoch.has_value() && - invocation_connection_epoch != - *expected_connection_epoch - ? "command expected a different connection epoch" - : "Modbus TCP connection is unavailable at command entry"; - return false; - } - - std::uint32_t axis_index = 0; - auto axis_mutex = axisMutex_(motor_id); - auto safety_mutex = safetyMutex_(motor_id); - if (!axis_mutex || !safety_mutex || !axisForMotor(motor_id, axis_index)) { - std::lock_guard lock(state_mutex_); - last_error_ = "motor id has no PLC axis mapping"; - return false; - } - std::unique_lock axis_lock; - std::unique_lock safety_lock; - std::uint64_t cancel_generation = 0; - { - std::lock_guard lock(state_mutex_); - if (safety_priority) { - ++cancel_generations_[motor_id]; - } - cancel_generation = cancel_generations_[motor_id]; - ++command_admission_count_; - } - if (safety_priority) { - // Cancel ordinary motion immediately, then serialize only with other - // safety commands. A later safety command must not cancel this one's - // result while it is awaiting its ACK. - safety_lock = std::unique_lock(*safety_mutex); - } else { - // Capture above before waiting for the per-axis queue. A safety - // command that arrives while this normal command is queued must make - // the queued command stale rather than becoming its new baseline. - axis_lock = std::unique_lock(*axis_mutex); - std::lock_guard lock(state_mutex_); - if (cancel_generations_[motor_id] != cancel_generation) { - last_error_ = - "PLC command was preempted while waiting for the axis queue"; - return false; - } - } - - if (!running_.load(std::memory_order_acquire) || - !connected_.load(std::memory_order_acquire) || - connection_epoch_.load(std::memory_order_acquire) != - invocation_connection_epoch || - (expected_connection_epoch.has_value() && - invocation_connection_epoch != *expected_connection_epoch)) { - std::lock_guard lock(state_mutex_); - last_error_ = "connection epoch changed while waiting for command queue"; - return false; - } - - CmvrPlcAxisStatus initial_status; - // Only SetZero needs a fresh pre-command zero_epoch. Other commands avoid - // a three-FC3 baseline read; QuickStop/Disable therefore spend the first - // available transaction on the safety mailbox itself. - if (!safety_priority && - command.code == cmvr_plc::CommandCode::SetZero && - !readAxisStatusByIndex_(axis_index, initial_status)) { - return false; - } - - std::uint32_t command_sequence = 0; - std::uint32_t command_session_id = 0; - std::uint64_t command_connection_epoch = 0; - std::array payload{}; - payload[cmvr_plc::kCommandCode] = static_cast(command.code); - payload[cmvr_plc::kCommandFlags] = command.flags; - cmvr_plc::encodeInt32(payload.data(), cmvr_plc::kTargetPosition, command.target_position); - cmvr_plc::encodeInt32(payload.data(), cmvr_plc::kTargetVelocity, command.target_velocity); - cmvr_plc::encodeInt32(payload.data(), cmvr_plc::kAcceleration, command.acceleration); - cmvr_plc::encodeInt32(payload.data(), cmvr_plc::kTargetTorque, command.target_torque); - cmvr_plc::encodeInt32(payload.data(), cmvr_plc::kPositionTolerance, - command.position_tolerance); - cmvr_plc::encodeInt32(payload.data(), cmvr_plc::kVelocityTolerance, - command.velocity_tolerance); - cmvr_plc::encodeUint32(payload.data(), cmvr_plc::kCommandTimeout, - command.command_timeout_ms); - cmvr_plc::encodeUint32(payload.data(), cmvr_plc::kStreamWatchdog, - command.stream_watchdog_ms); - cmvr_plc::encodeUint32(payload.data(), cmvr_plc::kCyclicSampleSequence, - command.cyclic_sample_sequence); - cmvr_plc::encodeUint32( - payload.data(), cmvr_plc::kClientMonotonicTime, - command.client_monotonic_time_ms == 0 ? monotonicMilliseconds_() - : command.client_monotonic_time_ms); - const auto expected_zero_epoch = - command.code == cmvr_plc::CommandCode::SetZero - ? initial_status.zero_epoch - : command.expected_zero_epoch; - cmvr_plc::encodeUint32(payload.data(), cmvr_plc::kExpectedZeroEpoch, - expected_zero_epoch); - payload[cmvr_plc::kDisconnectAction] = command.disconnect_action; - - const auto base = cmvr_plc::axisBase(axis_index); - { - std::lock_guard lock(io_mutex_); - if (!running_.load(std::memory_order_acquire) || - !connected_.load(std::memory_order_acquire) || - connection_epoch_.load(std::memory_order_acquire) != - invocation_connection_epoch || - (expected_connection_epoch.has_value() && - invocation_connection_epoch != *expected_connection_epoch)) { - std::lock_guard state_lock(state_mutex_); - last_error_ = - "Modbus TCP connection changed before mailbox commit"; - return false; - } - { - std::lock_guard state_lock(state_mutex_); - if (!safety_priority && - cancel_generations_[motor_id] != cancel_generation) { - last_error_ = "PLC command was preempted before mailbox commit"; - return false; - } - command_sequence = ++command_sequences_[motor_id]; - if (command_sequence == 0) { - command_sequence = ++command_sequences_[motor_id]; - } - } - command_session_id = session_id_.load(); - command_connection_epoch = invocation_connection_epoch; - cmvr_plc::encodeUint32(payload.data(), cmvr_plc::kPayloadSequence, - command_sequence); - cmvr_plc::encodeUint32(payload.data(), cmvr_plc::kPayloadSequenceMirror, - command_sequence); - cmvr_plc::encodeUint32(payload.data(), cmvr_plc::kCommandSessionId, - command_session_id); - const auto heartbeat_due = - std::chrono::steady_clock::now() - - last_client_heartbeat_write_at_ >= - std::chrono::milliseconds(heartbeat_period_ms_); - if (!safety_priority && heartbeat_due && - !writeClientHeartbeatLocked_()) { - return false; - } - if (!writeRegistersLocked_(base, payload.data(), static_cast(payload.size()))) { - return false; - } - std::array commit{}; - cmvr_plc::encodeUint32(commit.data(), 0, command_sequence); - if (!writeRegistersLocked_(base + cmvr_plc::kCommitSequenceRelativeOffset, - commit.data(), static_cast(commit.size()))) { - return false; - } - } - return waitForCommand_(axis_index, command_sequence, command_session_id, - command_connection_epoch, cancel_generation, - !safety_priority, command, - wait_for_terminal_state, initial_status); -} - -bool ModbusTcpMotorBusRuntime::connectAndHandshakeLocked_() -{ - client_.close(); - connected_.store(false); - const auto port = static_cast( - config_.port() == 0 ? 502U : config_.port()); - const auto unit_id = static_cast( - config_.unit_id() == 0 ? 1U : config_.unit_id()); - if (!client_.open(config_.host(), port, unit_id, - connect_timeout_ms_, io_timeout_ms_)) { - std::lock_guard state_lock(state_mutex_); - last_error_ = client_.lastError(); - return false; - } - - std::vector global; - if (!readRegistersLocked_(0, cmvr_plc::kGlobalRegisterCount, global)) { - return false; - } - const auto initial_protocol_major = - global[cmvr_plc::kProtocolMajorOffset]; - const auto initial_protocol_minor = - global[cmvr_plc::kProtocolMinorOffset]; - const auto initial_axis_count = global[cmvr_plc::kAxisCountOffset]; - const auto initial_boot_id = cmvr_plc::decodeUint32( - global.data(), cmvr_plc::kPlcBootIdOffset); - if (global[cmvr_plc::kMagicCmOffset] != cmvr_plc::kMagicCm || - global[cmvr_plc::kMagicVrOffset] != cmvr_plc::kMagicVr) { - markDisconnectedLocked_("PLC register map magic does not match CMVR"); - return false; - } - if (initial_protocol_major != protocol_major_) { - markDisconnectedLocked_("PLC protocol major version mismatch"); - return false; - } - if (initial_protocol_minor < protocol_minor_) { - markDisconnectedLocked_("PLC protocol minor version is older than required"); - return false; - } - if (initial_boot_id == 0U) { - markDisconnectedLocked_("PLC boot_id must be non-zero"); - return false; - } - for (const auto& [motor_id, axis_index] : motor_axes_) { - (void)motor_id; - if (axis_index >= initial_axis_count) { - markDisconnectedLocked_("configured PLC axis is outside PLC axis_count"); - return false; - } - } - const auto validate_handshake_identity = - [&](const std::vector& snapshot) { - if (snapshot[cmvr_plc::kMagicCmOffset] != cmvr_plc::kMagicCm || - snapshot[cmvr_plc::kMagicVrOffset] != cmvr_plc::kMagicVr || - snapshot[cmvr_plc::kProtocolMajorOffset] != initial_protocol_major || - snapshot[cmvr_plc::kProtocolMinorOffset] != initial_protocol_minor || - snapshot[cmvr_plc::kAxisCountOffset] != initial_axis_count) { - markDisconnectedLocked_( - "PLC register-map identity changed during session handshake"); - return false; - } - const auto boot_id = cmvr_plc::decodeUint32( - snapshot.data(), cmvr_plc::kPlcBootIdOffset); - if (boot_id == 0U || boot_id != initial_boot_id) { - markDisconnectedLocked_( - "PLC boot_id changed or became zero during session handshake"); - return false; - } - return true; - }; - - std::uint32_t next_session_id = 0; - do { - next_session_id = randomNonZeroSessionId_(); - } while (next_session_id == last_session_id_); - // A reconnect must not immediately reuse its preceding command namespace. - // This does not claim global lifetime uniqueness, only adjacent-session - // separation, which is what stale mailbox/ACK rejection requires. - session_id_ = next_session_id; - last_session_id_ = next_session_id; - heartbeat_counter_ = 0; - { - std::lock_guard state_lock(state_mutex_); - for (auto& [motor_id, sequence] : command_sequences_) { - (void)motor_id; - sequence = 0; - } - } - - std::array watchdog{}; - cmvr_plc::encodeUint32(watchdog.data(), 0, communication_watchdog_ms_); - if (!writeRegistersLocked_(cmvr_plc::kCommunicationWatchdogOffset, - watchdog.data(), static_cast(watchdog.size()))) { - return false; - } - // Publish the new owner candidate only after all parameters it depends on - // are visible to the PLC. - std::array session_and_heartbeat{}; - cmvr_plc::encodeUint32(session_and_heartbeat.data(), 0, session_id_.load()); - cmvr_plc::encodeUint32(session_and_heartbeat.data(), 2, ++heartbeat_counter_); - if (!writeRegistersLocked_(cmvr_plc::kCmvrSessionIdOffset, - session_and_heartbeat.data(), - static_cast(session_and_heartbeat.size()))) { - return false; - } - last_client_heartbeat_write_at_ = std::chrono::steady_clock::now(); - - const auto handshake_deadline = std::chrono::steady_clock::now() + - std::chrono::milliseconds(command_ack_timeout_ms_); - do { - if (!readRegistersLocked_(0, cmvr_plc::kGlobalRegisterCount, global)) { - return false; - } - if (!validate_handshake_identity(global)) { - return false; - } - const auto owner_session = cmvr_plc::decodeUint32( - global.data(), cmvr_plc::kOwnerSessionIdOffset); - const auto owner_state = - static_cast(global[cmvr_plc::kOwnerStateOffset]); - if (owner_session == session_id_.load() && - owner_state == cmvr_plc::OwnerState::Accepted) { - break; - } - if (owner_session == session_id_.load() && - owner_state == cmvr_plc::OwnerState::Rejected) { - markDisconnectedLocked_("PLC rejected CMVR session ownership"); - return false; - } - std::this_thread::sleep_for(std::chrono::milliseconds(status_poll_period_ms_)); - } while (std::chrono::steady_clock::now() < handshake_deadline); - if (cmvr_plc::decodeUint32(global.data(), cmvr_plc::kOwnerSessionIdOffset) != - session_id_.load() || - static_cast(global[cmvr_plc::kOwnerStateOffset]) != - cmvr_plc::OwnerState::Accepted) { - markDisconnectedLocked_("PLC session ownership handshake timed out"); - return false; - } - - // Confirm one complete immutable snapshot after owner acceptance. This - // closes the race where the PLC restarts or swaps register-map versions - // between the accepted-session observation and connected_=true. - if (!readRegistersLocked_(0, cmvr_plc::kGlobalRegisterCount, global) || - !validate_handshake_identity(global)) { - return false; - } - if (cmvr_plc::decodeUint32(global.data(), - cmvr_plc::kOwnerSessionIdOffset) != - session_id_.load() || - static_cast( - global[cmvr_plc::kOwnerStateOffset]) != - cmvr_plc::OwnerState::Accepted) { - markDisconnectedLocked_( - "PLC session ownership changed during final handshake confirmation"); - return false; - } - - if (!running_.load()) { - markDisconnectedLocked_( - "runtime stopped during PLC session handshake"); - return false; - } - plc_boot_id_.store(initial_boot_id); - plc_heartbeat_counter_ = cmvr_plc::decodeUint32( - global.data(), cmvr_plc::kPlcHeartbeatOffset); - plc_heartbeat_changed_at_ = std::chrono::steady_clock::now(); - connected_.store(true); - connection_epoch_.fetch_add(1); - { - std::lock_guard state_lock(state_mutex_); - last_error_.clear(); - } - CMVR_LOG(INFO) << "[ModbusTcpMotorBusRuntime] connected group=" << id_ - << ", endpoint=" << config_.host() << ":" << port - << ", boot_id=" << plc_boot_id_.load() - << ", protocol=" << global[cmvr_plc::kProtocolMajorOffset] - << "." << global[cmvr_plc::kProtocolMinorOffset]; - return true; -} - -void ModbusTcpMotorBusRuntime::markDisconnectedLocked_(const std::string& error) -{ - connected_.store(false); - client_.close(); - std::lock_guard state_lock(state_mutex_); - last_error_ = error; -} - -bool ModbusTcpMotorBusRuntime::readRegistersLocked_( - const int address, - const int count, - std::vector& values) -{ - if (!client_.readHoldingRegisters(address, count, values)) { - markDisconnectedLocked_(client_.lastError()); - return false; - } - return true; -} - -bool ModbusTcpMotorBusRuntime::writeRegistersLocked_( - const int address, - const std::uint16_t* values, - const int count) -{ - if (!client_.writeHoldingRegisters(address, values, count)) { - markDisconnectedLocked_(client_.lastError()); - return false; - } - return true; -} - -bool ModbusTcpMotorBusRuntime::writeClientHeartbeatLocked_() -{ - std::array values{}; - cmvr_plc::encodeUint32(values.data(), 0, session_id_.load()); - cmvr_plc::encodeUint32(values.data(), 2, ++heartbeat_counter_); - if (!writeRegistersLocked_(cmvr_plc::kCmvrSessionIdOffset, - values.data(), static_cast(values.size()))) { - return false; - } - last_client_heartbeat_write_at_ = std::chrono::steady_clock::now(); - return true; -} - -bool ModbusTcpMotorBusRuntime::writeHeartbeatLocked_() -{ - if (!writeClientHeartbeatLocked_()) { - return false; - } - std::vector global; - if (!readRegistersLocked_(0, cmvr_plc::kGlobalRegisterCount, global)) { - return false; - } - const auto boot_id = - cmvr_plc::decodeUint32(global.data(), cmvr_plc::kPlcBootIdOffset); - if (boot_id != plc_boot_id_.load()) { - markDisconnectedLocked_("PLC boot_id changed"); - return false; - } - const auto owner_session = cmvr_plc::decodeUint32( - global.data(), cmvr_plc::kOwnerSessionIdOffset); - if (owner_session != session_id_.load() || - static_cast(global[cmvr_plc::kOwnerStateOffset]) != - cmvr_plc::OwnerState::Accepted) { - markDisconnectedLocked_("PLC session ownership was lost"); - return false; - } - const auto plc_heartbeat = cmvr_plc::decodeUint32( - global.data(), cmvr_plc::kPlcHeartbeatOffset); - const auto now = std::chrono::steady_clock::now(); - if (plc_heartbeat != plc_heartbeat_counter_) { - plc_heartbeat_counter_ = plc_heartbeat; - plc_heartbeat_changed_at_ = now; - } else if (now - plc_heartbeat_changed_at_ >= - std::chrono::milliseconds(communication_watchdog_ms_)) { - markDisconnectedLocked_("PLC heartbeat stopped advancing"); - return false; - } - return true; -} - -bool ModbusTcpMotorBusRuntime::readAxisStatusByIndex_( - const std::uint32_t axis_index, - CmvrPlcAxisStatus& status) -{ - std::vector values; - { - std::lock_guard lock(io_mutex_); - if (!connected_.load()) { - std::lock_guard state_lock(state_mutex_); - last_error_ = "Modbus TCP connection is unavailable"; - return false; - } - const auto status_base = cmvr_plc::axisBase(axis_index) + - cmvr_plc::kAxisStatusRelativeOffset; - bool stable_snapshot = false; - for (int attempt = 0; attempt < 3; ++attempt) { - std::vector before; - std::vector candidate; - std::vector after; - if (!readRegistersLocked_( - status_base + static_cast(cmvr_plc::kStateSequence), - 2, before) || - !readRegistersLocked_( - status_base, cmvr_plc::kAxisStatusRegisterCount, - candidate) || - !readRegistersLocked_( - status_base + static_cast(cmvr_plc::kStateSequence), - 2, after)) { - return false; - } - const auto before_sequence = - cmvr_plc::decodeUint32(before.data(), 0); - const auto after_sequence = - cmvr_plc::decodeUint32(after.data(), 0); - const auto block_sequence = cmvr_plc::decodeUint32( - candidate.data(), cmvr_plc::kStateSequence); - const auto block_mirror = cmvr_plc::decodeUint32( - candidate.data(), cmvr_plc::kStateSequenceMirror); - if (before_sequence != 0U && - (before_sequence & 1U) == 0U && - before_sequence == after_sequence && - before_sequence == block_sequence && - before_sequence == block_mirror) { - values = std::move(candidate); - stable_snapshot = true; - break; - } - } - if (!stable_snapshot) { - std::lock_guard state_lock(state_mutex_); - last_error_ = - "PLC axis status changed during bounded seqlock read"; - return false; - } - } - - status.ack_sequence = cmvr_plc::decodeUint32(values.data(), cmvr_plc::kAckSequence); - status.active_sequence = - cmvr_plc::decodeUint32(values.data(), cmvr_plc::kActiveSequence); - status.command_state = - static_cast(values[cmvr_plc::kCommandState]); - status.result_code = - static_cast(values[cmvr_plc::kResultCode]); - status.axis_state = values[cmvr_plc::kAxisState]; - status.current_mode = values[cmvr_plc::kCurrentMode]; - status.actual_position = - cmvr_plc::decodeInt32(values.data(), cmvr_plc::kActualPosition); - status.actual_velocity = - cmvr_plc::decodeInt32(values.data(), cmvr_plc::kActualVelocity); - status.actual_torque = - cmvr_plc::decodeInt32(values.data(), cmvr_plc::kActualTorque); - status.target_position = - cmvr_plc::decodeInt32(values.data(), cmvr_plc::kTargetPositionStatus); - status.target_velocity = - cmvr_plc::decodeInt32(values.data(), cmvr_plc::kTargetVelocityStatus); - status.status_flags = values[cmvr_plc::kStatusFlags]; - status.drive_statusword = values[cmvr_plc::kDriveStatusword]; - status.fault_code = cmvr_plc::decodeUint32(values.data(), cmvr_plc::kFaultCode); - status.zero_epoch = cmvr_plc::decodeUint32(values.data(), cmvr_plc::kZeroEpoch); - status.last_applied_cyclic_sequence = - cmvr_plc::decodeUint32(values.data(), cmvr_plc::kLastAppliedCyclicSequence); - status.state_sequence = - cmvr_plc::decodeUint32(values.data(), cmvr_plc::kStateSequence); - status.plc_monotonic_time_ms = - cmvr_plc::decodeUint32(values.data(), cmvr_plc::kPlcMonotonicTime); - status.heartbeat_age_ms = - cmvr_plc::decodeUint32(values.data(), cmvr_plc::kHeartbeatAge); - status.ack_session_id = - cmvr_plc::decodeUint32(values.data(), cmvr_plc::kAckSessionId); - if (status.heartbeat_age_ms > communication_watchdog_ms_) { - std::lock_guard state_lock(state_mutex_); - last_error_ = "PLC axis status is stale"; - return false; - } - return true; -} - -bool ModbusTcpMotorBusRuntime::waitForCommand_( - const std::uint32_t axis_index, - const std::uint32_t command_sequence, - const std::uint32_t command_session_id, - const std::uint64_t connection_epoch, - const std::uint64_t cancel_generation, - const bool cancel_on_safety_preemption, - const CmvrPlcAxisCommand& command, - const bool wait_for_terminal_state, - const CmvrPlcAxisStatus& initial_status) -{ - const auto timeout_ms = wait_for_terminal_state - ? valueOrDefault(command.command_timeout_ms, command_ack_timeout_ms_) - : command_ack_timeout_ms_; - const auto deadline = std::chrono::steady_clock::now() + - std::chrono::milliseconds(timeout_ms); - do { - if (cancel_on_safety_preemption) { - std::lock_guard state_lock(state_mutex_); - for (const auto& [motor_id, axis] : motor_axes_) { - if (axis == axis_index && - cancel_generations_[motor_id] != cancel_generation) { - last_error_ = "PLC command was preempted by a safety command"; - return false; - } - } - } - if (connection_epoch_.load() != connection_epoch) { - std::lock_guard state_lock(state_mutex_); - last_error_ = "connection session changed while awaiting PLC command"; - return false; - } - CmvrPlcAxisStatus status; - if (!readAxisStatusByIndex_(axis_index, status)) { - return false; - } - if (std::chrono::steady_clock::now() >= deadline) { - break; - } - if (status.ack_session_id == command_session_id && - status.ack_sequence == command_sequence) { - if (!validCommandState(status.command_state)) { - std::lock_guard state_lock(state_mutex_); - last_error_ = "PLC returned an unknown command state"; - return false; - } - if (cmvr_plc::isFailure(status.command_state) || - status.result_code != cmvr_plc::ResultCode::Ok) { - std::lock_guard state_lock(state_mutex_); - last_error_ = "PLC rejected or failed command, result=" + - std::to_string(static_cast(status.result_code)); - return false; - } - if (!wait_for_terminal_state) { - const bool accepted_state = - status.command_state == cmvr_plc::CommandState::Accepted || - status.command_state == cmvr_plc::CommandState::Running || - status.command_state == cmvr_plc::CommandState::TargetReached || - status.command_state == cmvr_plc::CommandState::Completed; - if (!accepted_state) { - std::lock_guard state_lock(state_mutex_); - last_error_ = "PLC ACK did not report an accepted command state"; - return false; - } - if (command.code == cmvr_plc::CommandCode::CyclicPositionSample || - command.code == cmvr_plc::CommandCode::CyclicVelocitySample) { - if (status.last_applied_cyclic_sequence == - command.cyclic_sample_sequence) { - std::lock_guard state_lock(state_mutex_); - last_error_.clear(); - return true; - } - } else if ( - command.code == cmvr_plc::CommandCode::OpenCyclicPosition || - command.code == cmvr_plc::CommandCode::OpenCyclicVelocity) { - const auto expected_mode = - command.code == - cmvr_plc::CommandCode::OpenCyclicPosition - ? msgs::RUN_MODE_CYCLIC_SYNC_POSITION - : msgs::RUN_MODE_CYCLIC_SYNC_VELOCITY; - if (status.last_applied_cyclic_sequence != 0U || - (status.status_flags & - cmvr_plc::StatusFlag::StreamActive) == 0U || - status.current_mode != - static_cast(expected_mode)) { - std::lock_guard state_lock(state_mutex_); - last_error_ = - "PLC OpenCyclic ACK did not establish a fresh active stream"; - return false; - } - std::lock_guard state_lock(state_mutex_); - last_error_.clear(); - return true; - } else { - std::lock_guard state_lock(state_mutex_); - last_error_.clear(); - return true; - } - } else { - bool completed = false; - switch (command.code) { - case cmvr_plc::CommandCode::SetZero: - completed = - status.command_state == cmvr_plc::CommandState::Completed && - (status.status_flags & cmvr_plc::StatusFlag::ZeroValid) != 0U && - status.zero_epoch != initial_status.zero_epoch; - break; - case cmvr_plc::CommandCode::Enable: - completed = - status.command_state == cmvr_plc::CommandState::Completed && - (status.status_flags & cmvr_plc::StatusFlag::Enabled) != 0U; - break; - case cmvr_plc::CommandCode::Disable: - completed = - status.command_state == cmvr_plc::CommandState::Completed && - (status.status_flags & cmvr_plc::StatusFlag::Enabled) == 0U; - break; - case cmvr_plc::CommandCode::QuickStop: - completed = - (status.command_state == cmvr_plc::CommandState::QuickStopped || - status.command_state == cmvr_plc::CommandState::Completed) && - std::abs(static_cast(status.actual_velocity)) <= 10000; - break; - default: - completed = - status.command_state == cmvr_plc::CommandState::Completed || - status.command_state == cmvr_plc::CommandState::TargetReached; - break; - } - if (completed) { - std::lock_guard state_lock(state_mutex_); - last_error_.clear(); - return true; - } - } - } - std::this_thread::sleep_for(std::chrono::milliseconds(status_poll_period_ms_)); - } while (std::chrono::steady_clock::now() < deadline && connected_.load()); - - std::lock_guard state_lock(state_mutex_); - last_error_ = wait_for_terminal_state ? "PLC command completion timeout" - : "PLC command acknowledgement timeout"; - return false; -} - -std::shared_ptr ModbusTcpMotorBusRuntime::axisMutex_( - const std::uint8_t motor_id) const -{ - const auto it = axis_mutexes_.find(motor_id); - return it == axis_mutexes_.end() ? nullptr : it->second; -} - -std::shared_ptr ModbusTcpMotorBusRuntime::safetyMutex_( - const std::uint8_t motor_id) const -{ - const auto it = safety_mutexes_.find(motor_id); - return it == safety_mutexes_.end() ? nullptr : it->second; -} - -void ModbusTcpMotorBusRuntime::workerLoop_() -{ - auto reconnect_delay = reconnect_min_ms_; - const auto interrupted = [this](const std::uint32_t wait_ms) { - std::unique_lock lock(worker_wait_mutex_); - return worker_wait_cv_.wait_for( - lock, std::chrono::milliseconds(wait_ms), - [this] { return !running_.load(); }); - }; - while (running_.load()) { - if (connected_.load()) { - if (interrupted(heartbeat_period_ms_)) { - break; - } - { - std::lock_guard lock(io_mutex_); - if (connected_.load()) { - writeHeartbeatLocked_(); - } - } - continue; - } - - if (interrupted(reconnect_delay)) { - break; - } - if (!running_.load()) { - break; - } - { - std::lock_guard lock(io_mutex_); - if (connectAndHandshakeLocked_()) { - reconnect_delay = reconnect_min_ms_; - continue; - } - } - reconnect_delay = std::min(reconnect_max_ms_, reconnect_delay * 2U); - } -} - -std::uint32_t ModbusTcpMotorBusRuntime::randomNonZeroSessionId_() -{ - std::random_device random_device; - std::mt19937 generator(random_device()); - std::uniform_int_distribution distribution( - 1U, std::numeric_limits::max()); - return distribution(generator); -} - -std::uint32_t ModbusTcpMotorBusRuntime::monotonicMilliseconds_() -{ - return static_cast( - std::chrono::duration_cast( - std::chrono::steady_clock::now().time_since_epoch()) - .count()); -} - -} // namespace cmvr::device diff --git a/cmvr-es/devices/motor/bus_runtime/modbus_tcp/tests/modbus_tcp_motor_bus_runtime_test.cpp b/cmvr-es/devices/motor/bus_runtime/modbus_tcp/tests/modbus_tcp_motor_bus_runtime_test.cpp deleted file mode 100644 index a18fb2c9..00000000 --- a/cmvr-es/devices/motor/bus_runtime/modbus_tcp/tests/modbus_tcp_motor_bus_runtime_test.cpp +++ /dev/null @@ -1,1586 +0,0 @@ -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include - -#include - -#include "devices/motor/bus_runtime/modbus_tcp/include/cmvr_plc_register_map.h" -#include "devices/motor/bus_runtime/modbus_tcp/include/modbus_tcp_motor_bus_runtime.h" -#include "devices/motor/drivers/modbus_plc_motor/include/cmvr_plc_motor_protocol.h" - -namespace cmvr::device { - -struct ModbusTcpMotorBusRuntimeTestAccess { - static std::shared_ptr axisMutex( - const ModbusTcpMotorBusRuntime& runtime, - const std::uint8_t motor_id) - { - return runtime.axisMutex_(motor_id); - } - - static std::shared_ptr safetyMutex( - const ModbusTcpMotorBusRuntime& runtime, - const std::uint8_t motor_id) - { - return runtime.safetyMutex_(motor_id); - } - - static std::uint64_t commandAdmissionCount( - const ModbusTcpMotorBusRuntime& runtime) - { - std::lock_guard lock(runtime.state_mutex_); - return runtime.command_admission_count_; - } -}; - -namespace { - -using namespace std::chrono_literals; - -std::uint16_t reserveLoopbackPort() -{ - const int socket_fd = ::socket(AF_INET, SOCK_STREAM, 0); - if (socket_fd < 0) { - return 0; - } - sockaddr_in address{}; - address.sin_family = AF_INET; - address.sin_addr.s_addr = htonl(INADDR_LOOPBACK); - address.sin_port = 0; - if (::bind(socket_fd, reinterpret_cast(&address), sizeof(address)) != 0) { - ::close(socket_fd); - return 0; - } - socklen_t length = sizeof(address); - if (::getsockname(socket_fd, reinterpret_cast(&address), &length) != 0) { - ::close(socket_fd); - return 0; - } - const auto port = ntohs(address.sin_port); - ::close(socket_fd); - return port; -} - -class FakeCmvrPlc { -public: - enum class HandshakeMutation { - None, - BootId, - ProtocolMajor, - }; - - ~FakeCmvrPlc() { stop(); } - - bool start(const std::uint16_t requested_port = 0) - { - port_ = requested_port == 0 ? reserveLoopbackPort() : requested_port; - if (port_ == 0) { - std::fprintf(stderr, "reserveLoopbackPort failed: %s\n", std::strerror(errno)); - return false; - } - context_ = modbus_new_tcp("127.0.0.1", port_); - mapping_ = modbus_mapping_new(1, 1, - cmvr_plc::kAxisFirstOffset + - cmvr_plc::kAxisRegisterStride, - 1); - if (!context_ || !mapping_) { - std::fprintf(stderr, "fake PLC allocation failed: %s\n", modbus_strerror(errno)); - stop(); - return false; - } - auto* registers = mapping_->tab_registers; - registers[cmvr_plc::kMagicCmOffset] = cmvr_plc::kMagicCm; - registers[cmvr_plc::kMagicVrOffset] = cmvr_plc::kMagicVr; - registers[cmvr_plc::kProtocolMajorOffset] = cmvr_plc::kProtocolMajor; - registers[cmvr_plc::kProtocolMinorOffset] = cmvr_plc::kProtocolMinor; - registers[cmvr_plc::kAxisCountOffset] = 1; - registers[cmvr_plc::kOwnerStateOffset] = - static_cast( - preserve_stale_rejected_owner_ - ? cmvr_plc::OwnerState::Rejected - : cmvr_plc::OwnerState::None); - if (preserve_stale_rejected_owner_) { - cmvr_plc::encodeUint32( - registers, cmvr_plc::kOwnerSessionIdOffset, 0x55667788U); - } - cmvr_plc::encodeUint32(registers, cmvr_plc::kPlcBootIdOffset, - initial_boot_id_); - auto* status = registers + cmvr_plc::axisBase(0) + - cmvr_plc::kAxisStatusRelativeOffset; - cmvr_plc::encodeUint32(status, cmvr_plc::kStateSequence, - state_sequence_); - cmvr_plc::encodeUint32(status, cmvr_plc::kStateSequenceMirror, - state_sequence_); - registers[cmvr_plc::axisBase(0) + - cmvr_plc::kAxisStatusRelativeOffset + - cmvr_plc::kCommandState] = - static_cast(cmvr_plc::CommandState::Idle); - registers[cmvr_plc::axisBase(0) + - cmvr_plc::kAxisStatusRelativeOffset + - cmvr_plc::kResultCode] = - static_cast(cmvr_plc::ResultCode::Ok); - - listen_socket_.store(modbus_tcp_listen(context_, 2)); - if (listen_socket_.load() < 0) { - std::fprintf(stderr, "modbus_tcp_listen failed on %u: %s\n", - port_, modbus_strerror(errno)); - stop(); - return false; - } - running_.store(true); - worker_ = std::thread(&FakeCmvrPlc::loop_, this); - return true; - } - - void stop() - { - running_.store(false); - const auto client = client_socket_.exchange(-1); - if (client >= 0) { - ::shutdown(client, SHUT_RDWR); - ::close(client); - } - const auto listener = listen_socket_.exchange(-1); - if (listener >= 0) { - ::shutdown(listener, SHUT_RDWR); - ::close(listener); - } - if (worker_.joinable()) { - worker_.join(); - } - if (mapping_) { - modbus_mapping_free(mapping_); - mapping_ = nullptr; - } - if (context_) { - modbus_free(context_); - context_ = nullptr; - } - } - - void disconnectClient() - { - const auto client = client_socket_.load(); - if (client >= 0) { - ::shutdown(client, SHUT_RDWR); - } - } - - std::uint16_t port() const { return port_; } - std::uint32_t commandCount() const { return command_count_.load(); } - std::uint32_t lastExpectedZeroEpoch() const - { - return last_expected_zero_epoch_.load(); - } - std::uint32_t lastProfileCommandTimeoutMs() const - { - return last_profile_command_timeout_ms_.load(); - } - std::uint32_t cyclicOpenCount() const - { - return cyclic_open_count_.load(); - } - std::uint32_t cyclicApplyCount() const - { - return cyclic_apply_count_.load(); - } - std::uint32_t currentStreamFirstSampleSequence() const - { - return current_stream_first_sample_sequence_.load(); - } - std::uint32_t clientHeartbeatWriteCount() const - { - return client_heartbeat_write_count_.load(); - } - std::uint32_t fullStatusReadCount() const - { - return full_status_read_count_.load(); - } - std::uint32_t observedProfileMailboxCount() const - { - return observed_profile_mailbox_count_.load(); - } - std::uint32_t observedMailboxCommitCount() const - { - return observed_mailbox_commit_count_.load(); - } - void setInitialBootId(const std::uint32_t boot_id) - { - initial_boot_id_ = boot_id; - } - void setHandshakeMutation(const HandshakeMutation mutation) - { - handshake_mutation_ = mutation; - } - void preserveStaleRejectedOwnerDuringHandshake(const bool preserve) - { - preserve_stale_rejected_owner_ = preserve; - } - void setHandshakeAcceptanceDelay(const std::uint32_t delay_ms) - { - handshake_acceptance_delay_ms_ = delay_ms; - } - void holdStatusSnapshotInProgress(const bool hold) - { - hold_status_snapshot_in_progress_.store(hold); - } - void publishOddEqualStatusSnapshot(const bool publish) - { - publish_odd_equal_status_snapshot_.store(publish); - } - void setQuickStopProcessingDelay(const std::uint32_t delay_ms) - { - quick_stop_processing_delay_ms_.store(delay_ms); - } - void injectMixedStablePositionOnce(const std::int32_t mixed_position, - const std::int32_t corrected_position) - { - std::lock_guard lock(mapping_mutex_); - auto* status = mapping_->tab_registers + cmvr_plc::axisBase(0) + - cmvr_plc::kAxisStatusRelativeOffset; - // Deliberately emulate a non-atomic FC3 copy whose markers still show - // the old stable version while one field comes from the next scan. - cmvr_plc::encodeInt32(status, cmvr_plc::kActualPosition, - mixed_position); - corrected_mixed_position_.store(corrected_position); - correct_mixed_snapshot_after_next_read_.store(true); - } - void publishPosition(const std::int32_t position) - { - std::lock_guard lock(mapping_mutex_); - auto* status = mapping_->tab_registers + cmvr_plc::axisBase(0) + - cmvr_plc::kAxisStatusRelativeOffset; - beginSnapshotUpdate_(status); - cmvr_plc::encodeInt32(status, cmvr_plc::kActualPosition, position); - publishSnapshot_(status); - } - void publishFatalFlags() - { - std::lock_guard lock(mapping_mutex_); - auto* status = mapping_->tab_registers + cmvr_plc::axisBase(0) + - cmvr_plc::kAxisStatusRelativeOffset; - beginSnapshotUpdate_(status); - status[cmvr_plc::kStatusFlags] |= - cmvr_plc::StatusFlag::Fault | - cmvr_plc::StatusFlag::CommunicationWatchdogExpired | - cmvr_plc::StatusFlag::CyclicWatchdogExpired; - publishSnapshot_(status); - } - void delayProfileAcks(const bool delay) { delay_profile_acks_.store(delay); } - -private: - void loop_() - { - std::array request{}; - while (running_.load()) { - int listener = listen_socket_.load(); - const int accepted = - listener < 0 ? -1 : modbus_tcp_accept(context_, &listener); - if (accepted < 0) { - if (running_.load()) { - std::this_thread::sleep_for(5ms); - } - continue; - } - client_socket_.store(accepted); - while (running_.load()) { - const auto request_length = modbus_receive(context_, request.data()); - if (request_length <= 0) { - break; - } - { - std::lock_guard mapping_lock(mapping_mutex_); - if (modbus_reply(context_, request.data(), request_length, - mapping_) < 0) { - break; - } - processMailbox_(request.data(), request_length); - } - } - const auto client = client_socket_.exchange(-1); - if (client >= 0) { - ::close(client); - } - } - } - - void processMailbox_(const std::uint8_t* request, - const int request_length) - { - auto* registers = mapping_->tab_registers; - const auto base = cmvr_plc::axisBase(0); - const auto status_base = base + cmvr_plc::kAxisStatusRelativeOffset; - int request_address = -1; - int request_count = 0; - const auto function = - request && request_length >= 8 ? request[7] : 0U; - if (request && request_length >= 12 && - (function == 0x03U || function == 0x10U)) { - request_address = - (static_cast(request[8]) << 8) | - static_cast(request[9]); - request_count = - (static_cast(request[10]) << 8) | - static_cast(request[11]); - } - if (function == 0x10U && - request_address == cmvr_plc::kCmvrSessionIdOffset && - request_count == 4) { - client_heartbeat_write_count_.fetch_add(1); - } - const bool status_read = - function == 0x03U && - request_address >= status_base && - request_address < status_base + - cmvr_plc::kAxisStatusRegisterCount; - const bool full_status_read = - function == 0x03U && - request_address == status_base && - request_count == cmvr_plc::kAxisStatusRegisterCount; - if (full_status_read) { - full_status_read_count_.fetch_add(1); - } - cmvr_plc::encodeUint32( - registers, cmvr_plc::kPlcHeartbeatOffset, ++plc_heartbeat_); - const auto session = - cmvr_plc::decodeUint32(registers, cmvr_plc::kCmvrSessionIdOffset); - const auto now = std::chrono::steady_clock::now(); - if (session != observed_session_) { - observed_session_ = session; - session_observed_at_ = now; - if (!handshake_identity_mutated_) { - if (handshake_mutation_ == HandshakeMutation::BootId) { - cmvr_plc::encodeUint32( - registers, cmvr_plc::kPlcBootIdOffset, - initial_boot_id_ + 1U); - } else if (handshake_mutation_ == - HandshakeMutation::ProtocolMajor) { - registers[cmvr_plc::kProtocolMajorOffset] = - cmvr_plc::kProtocolMajor + 1U; - } - handshake_identity_mutated_ = - handshake_mutation_ != HandshakeMutation::None; - } - if (!preserve_stale_rejected_owner_) { - registers[cmvr_plc::kOwnerStateOffset] = - static_cast( - cmvr_plc::OwnerState::Accepting); - } - last_sequence_ = 0; - last_observed_mailbox_sequence_ = 0; - last_observed_profile_sequence_ = 0; - cmvr_plc::encodeUint32(registers + base, - cmvr_plc::kPayloadSequence, 0); - cmvr_plc::encodeUint32(registers + base, - cmvr_plc::kPayloadSequenceMirror, 0); - cmvr_plc::encodeUint32(registers + base, - cmvr_plc::kCommitSequenceRelativeOffset, 0); - beginSnapshotUpdate_(registers + status_base); - cyclic_stream_open_ = false; - last_cyclic_sample_sequence_ = 0; - current_stream_first_sample_sequence_.store(0); - registers[status_base + cmvr_plc::kStatusFlags] &= - static_cast( - ~cmvr_plc::StatusFlag::StreamActive); - cmvr_plc::encodeUint32( - registers + status_base, - cmvr_plc::kLastAppliedCyclicSequence, 0); - publishSnapshot_(registers + status_base); - } - if (active_session_ != observed_session_ && - now - session_observed_at_ >= - std::chrono::milliseconds(handshake_acceptance_delay_ms_)) { - active_session_ = observed_session_; - cmvr_plc::encodeUint32( - registers, cmvr_plc::kOwnerSessionIdOffset, active_session_); - registers[cmvr_plc::kOwnerStateOffset] = - static_cast(cmvr_plc::OwnerState::Accepted); - } - if (active_session_ == 0 || active_session_ != session) { - processStatusInjection_(registers + status_base, status_read, - full_status_read); - return; - } - const auto sequence = - cmvr_plc::decodeUint32(registers + base, cmvr_plc::kPayloadSequence); - const auto mirror = - cmvr_plc::decodeUint32(registers + base, - cmvr_plc::kPayloadSequenceMirror); - const auto commit = - cmvr_plc::decodeUint32(registers + base, - cmvr_plc::kCommitSequenceRelativeOffset); - if (sequence == 0 || sequence == last_sequence_ || - sequence != mirror || sequence != commit || - cmvr_plc::decodeUint32(registers + base, - cmvr_plc::kCommandSessionId) != active_session_) { - processStatusInjection_(registers + status_base, status_read, - full_status_read); - return; - } - const auto code = static_cast( - registers[base + cmvr_plc::kCommandCode]); - if (sequence != last_observed_mailbox_sequence_) { - last_observed_mailbox_sequence_ = sequence; - observed_mailbox_commit_count_.fetch_add(1); - } - const bool profile_command = - code == cmvr_plc::CommandCode::ProfilePosition || - code == cmvr_plc::CommandCode::ProfileVelocity; - if (profile_command) { - if (sequence != last_observed_profile_sequence_) { - last_observed_profile_sequence_ = sequence; - observed_profile_mailbox_count_.fetch_add(1); - } - last_profile_command_timeout_ms_.store( - cmvr_plc::decodeUint32(registers + base, - cmvr_plc::kCommandTimeout)); - } - if (delay_profile_acks_.load() && profile_command) { - // Leave the last published status snapshot unchanged. Rewriting - // its seqlock marker on every FC3 poll would make the fake report - // a torn snapshot and release the caller's axis mutex instead of - // deterministically keeping the command pending. - return; - } - beginSnapshotUpdate_(registers + status_base); - if (code == cmvr_plc::CommandCode::QuickStop && - quick_stop_processing_delay_ms_.load() > 0U) { - std::this_thread::sleep_for(std::chrono::milliseconds( - quick_stop_processing_delay_ms_.load())); - } - const bool cyclic_sample = - code == cmvr_plc::CommandCode::CyclicPositionSample || - code == cmvr_plc::CommandCode::CyclicVelocitySample; - const auto cyclic_sample_sequence = cmvr_plc::decodeUint32( - registers + base, cmvr_plc::kCyclicSampleSequence); - if (cyclic_sample && - (!cyclic_stream_open_ || cyclic_sample_sequence == 0U || - cyclic_sample_sequence == last_cyclic_sample_sequence_)) { - last_sequence_ = sequence; - cmvr_plc::encodeUint32( - registers + status_base, cmvr_plc::kAckSequence, sequence); - cmvr_plc::encodeUint32( - registers + status_base, cmvr_plc::kAckSessionId, - active_session_); - registers[status_base + cmvr_plc::kCommandState] = - static_cast( - cmvr_plc::CommandState::Rejected); - registers[status_base + cmvr_plc::kResultCode] = - static_cast( - cmvr_plc::ResultCode::SequenceError); - publishSnapshot_(registers + status_base); - return; - } - if (code == cmvr_plc::CommandCode::SetZero) { - last_expected_zero_epoch_.store( - cmvr_plc::decodeUint32(registers + base, - cmvr_plc::kExpectedZeroEpoch)); - } - if (code == cmvr_plc::CommandCode::SetZero && - cmvr_plc::decodeUint32(registers + base, - cmvr_plc::kExpectedZeroEpoch) != - cmvr_plc::decodeUint32(registers + status_base, - cmvr_plc::kZeroEpoch)) { - last_sequence_ = sequence; - cmvr_plc::encodeUint32(registers + status_base, - cmvr_plc::kAckSequence, sequence); - cmvr_plc::encodeUint32(registers + status_base, - cmvr_plc::kAckSessionId, active_session_); - registers[status_base + cmvr_plc::kCommandState] = - static_cast(cmvr_plc::CommandState::Rejected); - registers[status_base + cmvr_plc::kResultCode] = - static_cast(cmvr_plc::ResultCode::SequenceError); - publishSnapshot_(registers + status_base); - return; - } - last_sequence_ = sequence; - command_count_.fetch_add(1); - cmvr_plc::encodeUint32(registers + status_base, cmvr_plc::kAckSequence, - sequence); - cmvr_plc::encodeUint32(registers + status_base, cmvr_plc::kActiveSequence, - sequence); - cmvr_plc::encodeUint32(registers + status_base, cmvr_plc::kAckSessionId, - active_session_); - registers[status_base + cmvr_plc::kResultCode] = - static_cast(cmvr_plc::ResultCode::Ok); - - const auto target_position = - cmvr_plc::decodeInt32(registers + base, cmvr_plc::kTargetPosition); - const auto target_velocity = - cmvr_plc::decodeInt32(registers + base, cmvr_plc::kTargetVelocity); - auto& flags = registers[status_base + cmvr_plc::kStatusFlags]; - auto state = cmvr_plc::CommandState::Completed; - switch (code) { - case cmvr_plc::CommandCode::SetZero: - cmvr_plc::encodeInt32(registers + status_base, - cmvr_plc::kActualPosition, 0); - flags |= cmvr_plc::StatusFlag::ZeroValid; - cmvr_plc::encodeUint32( - registers + status_base, cmvr_plc::kZeroEpoch, - cmvr_plc::decodeUint32(registers + status_base, - cmvr_plc::kZeroEpoch) + - 1U); - break; - case cmvr_plc::CommandCode::ProfilePosition: - registers[status_base + cmvr_plc::kCurrentMode] = - msgs::RUN_MODE_PROFILE_POSITION; - cmvr_plc::encodeInt32(registers + status_base, - cmvr_plc::kActualPosition, target_position); - cmvr_plc::encodeInt32(registers + status_base, - cmvr_plc::kTargetPositionStatus, target_position); - flags |= cmvr_plc::StatusFlag::TargetReached; - state = cmvr_plc::CommandState::TargetReached; - break; - case cmvr_plc::CommandCode::ProfileVelocity: - registers[status_base + cmvr_plc::kCurrentMode] = - msgs::RUN_MODE_PROFILE_VELOCITY; - cmvr_plc::encodeInt32(registers + status_base, - cmvr_plc::kActualVelocity, target_velocity); - cmvr_plc::encodeInt32(registers + status_base, - cmvr_plc::kTargetVelocityStatus, target_velocity); - flags |= cmvr_plc::StatusFlag::TargetReached; - state = cmvr_plc::CommandState::TargetReached; - break; - case cmvr_plc::CommandCode::OpenCyclicPosition: - registers[status_base + cmvr_plc::kCurrentMode] = - msgs::RUN_MODE_CYCLIC_SYNC_POSITION; - flags |= cmvr_plc::StatusFlag::StreamActive; - cyclic_stream_open_ = true; - last_cyclic_sample_sequence_ = 0; - current_stream_first_sample_sequence_.store(0); - cyclic_open_count_.fetch_add(1); - cmvr_plc::encodeUint32( - registers + status_base, - cmvr_plc::kLastAppliedCyclicSequence, 0); - state = cmvr_plc::CommandState::Accepted; - break; - case cmvr_plc::CommandCode::OpenCyclicVelocity: - registers[status_base + cmvr_plc::kCurrentMode] = - msgs::RUN_MODE_CYCLIC_SYNC_VELOCITY; - flags |= cmvr_plc::StatusFlag::StreamActive; - cyclic_stream_open_ = true; - last_cyclic_sample_sequence_ = 0; - current_stream_first_sample_sequence_.store(0); - cyclic_open_count_.fetch_add(1); - cmvr_plc::encodeUint32( - registers + status_base, - cmvr_plc::kLastAppliedCyclicSequence, 0); - state = cmvr_plc::CommandState::Accepted; - break; - case cmvr_plc::CommandCode::CyclicPositionSample: - cmvr_plc::encodeInt32(registers + status_base, - cmvr_plc::kActualPosition, target_position); - cmvr_plc::encodeInt32(registers + status_base, - cmvr_plc::kActualVelocity, target_velocity); - cmvr_plc::encodeUint32( - registers + status_base, - cmvr_plc::kLastAppliedCyclicSequence, - cyclic_sample_sequence); - last_cyclic_sample_sequence_ = cyclic_sample_sequence; - if (current_stream_first_sample_sequence_.load() == 0U) { - current_stream_first_sample_sequence_.store( - cyclic_sample_sequence); - } - cyclic_apply_count_.fetch_add(1); - state = cmvr_plc::CommandState::Accepted; - break; - case cmvr_plc::CommandCode::CyclicVelocitySample: - cmvr_plc::encodeInt32(registers + status_base, - cmvr_plc::kActualVelocity, target_velocity); - cmvr_plc::encodeUint32( - registers + status_base, - cmvr_plc::kLastAppliedCyclicSequence, - cyclic_sample_sequence); - last_cyclic_sample_sequence_ = cyclic_sample_sequence; - if (current_stream_first_sample_sequence_.load() == 0U) { - current_stream_first_sample_sequence_.store( - cyclic_sample_sequence); - } - cyclic_apply_count_.fetch_add(1); - state = cmvr_plc::CommandState::Accepted; - break; - case cmvr_plc::CommandCode::QuickStop: - cmvr_plc::encodeInt32(registers + status_base, - cmvr_plc::kActualVelocity, 0); - flags &= static_cast(~cmvr_plc::StatusFlag::StreamActive); - cyclic_stream_open_ = false; - state = cmvr_plc::CommandState::QuickStopped; - break; - case cmvr_plc::CommandCode::Enable: - flags |= cmvr_plc::StatusFlag::Enabled; - break; - case cmvr_plc::CommandCode::Disable: - flags &= static_cast(~cmvr_plc::StatusFlag::Enabled); - flags &= static_cast(~cmvr_plc::StatusFlag::StreamActive); - cyclic_stream_open_ = false; - break; - default: - break; - } - registers[status_base + cmvr_plc::kCommandState] = - static_cast(state); - publishSnapshot_(registers + status_base); - } - - void processStatusInjection_(std::uint16_t* status, - const bool status_read, - const bool full_status_read) - { - if (full_status_read && - correct_mixed_snapshot_after_next_read_.exchange(false)) { - beginSnapshotUpdate_(status); - cmvr_plc::encodeInt32( - status, cmvr_plc::kActualPosition, - corrected_mixed_position_.load()); - publishSnapshot_(status); - return; - } - if (status_read && - (hold_status_snapshot_in_progress_.load() || - publish_odd_equal_status_snapshot_.load())) { - beginSnapshotUpdate_(status); - publishSnapshot_(status); - } - } - - void publishSnapshot_(std::uint16_t* status) - { - cmvr_plc::encodeUint32(status, cmvr_plc::kHeartbeatAge, 0); - const auto in_progress_sequence = state_sequence_ + 1U; - if (publish_odd_equal_status_snapshot_.load()) { - cmvr_plc::encodeUint32(status, cmvr_plc::kStateSequenceMirror, - in_progress_sequence); - return; - } - if (hold_status_snapshot_in_progress_.load()) { - return; - } - auto next_stable_sequence = state_sequence_ + 2U; - if (next_stable_sequence == 0U) { - next_stable_sequence = 2U; - } - // Seqlock publication order: mirror first, sequence last. - cmvr_plc::encodeUint32(status, cmvr_plc::kStateSequenceMirror, - next_stable_sequence); - cmvr_plc::encodeUint32(status, cmvr_plc::kStateSequence, - next_stable_sequence); - state_sequence_ = next_stable_sequence; - } - - void beginSnapshotUpdate_(std::uint16_t* status) - { - // Mark the snapshot in progress before changing any status field. - // The mirror intentionally remains at the previous stable even value. - cmvr_plc::encodeUint32(status, cmvr_plc::kStateSequence, - state_sequence_ + 1U); - } - - modbus_t* context_{nullptr}; - modbus_mapping_t* mapping_{nullptr}; - std::mutex mapping_mutex_; - std::thread worker_; - std::atomic running_{false}; - std::atomic listen_socket_{-1}; - std::atomic client_socket_{-1}; - std::atomic command_count_{0}; - std::atomic last_expected_zero_epoch_{0}; - std::atomic last_profile_command_timeout_ms_{0}; - std::atomic cyclic_open_count_{0}; - std::atomic cyclic_apply_count_{0}; - std::atomic current_stream_first_sample_sequence_{0}; - std::atomic client_heartbeat_write_count_{0}; - std::atomic full_status_read_count_{0}; - std::atomic observed_profile_mailbox_count_{0}; - std::atomic observed_mailbox_commit_count_{0}; - std::atomic quick_stop_processing_delay_ms_{0}; - std::atomic corrected_mixed_position_{0}; - std::atomic correct_mixed_snapshot_after_next_read_{false}; - std::atomic delay_profile_acks_{false}; - std::atomic hold_status_snapshot_in_progress_{false}; - std::atomic publish_odd_equal_status_snapshot_{false}; - std::uint32_t active_session_{0}; - std::uint32_t observed_session_{0}; - std::uint32_t plc_heartbeat_{0}; - std::uint32_t state_sequence_{2}; - std::chrono::steady_clock::time_point session_observed_at_{}; - std::uint32_t last_sequence_{0}; - std::uint32_t last_observed_mailbox_sequence_{0}; - std::uint32_t last_observed_profile_sequence_{0}; - std::uint32_t initial_boot_id_{0x10203040U}; - HandshakeMutation handshake_mutation_{HandshakeMutation::None}; - bool handshake_identity_mutated_{false}; - bool preserve_stale_rejected_owner_{false}; - std::uint32_t handshake_acceptance_delay_ms_{20}; - bool cyclic_stream_open_{false}; - std::uint32_t last_cyclic_sample_sequence_{0}; - std::uint16_t port_{0}; -}; - -config::MotorGroupConfig makeConfig(const std::uint16_t port) -{ - config::MotorGroupConfig group; - group.set_id("test_plc"); - group.set_bus_type(config::MOTOR_BUS_MODBUS_TCP); - group.set_vendor(config::MOTOR_VENDOR_PLC_GENERIC); - group.set_protocol(config::MOTOR_PROTOCOL_CMVR_PLC_V1); - auto* modbus = group.mutable_modbus_tcp(); - modbus->set_host("127.0.0.1"); - modbus->set_port(port); - modbus->set_unit_id(1); - modbus->set_connect_timeout_ms(200); - modbus->set_io_timeout_ms(100); - modbus->set_heartbeat_period_ms(20); - modbus->set_communication_watchdog_ms(500); - modbus->set_status_poll_period_ms(2); - modbus->set_reconnect_min_ms(10); - modbus->set_reconnect_max_ms(50); - modbus->set_command_ack_timeout_ms(500); - modbus->set_cyclic_watchdog_ms(500); - auto* axis = modbus->add_axes(); - axis->set_motor_id(1); - axis->set_axis_index(0); - return group; -} - -template -bool waitUntil(Predicate&& predicate, const std::chrono::milliseconds timeout) -{ - const auto deadline = std::chrono::steady_clock::now() + timeout; - do { - if (predicate()) { - return true; - } - std::this_thread::sleep_for(5ms); - } while (std::chrono::steady_clock::now() < deadline); - return predicate(); -} - -TEST(ModbusTcpMotorBusRuntimeTest, ExecutesSupportedCmvrPlcV1Commands) -{ - FakeCmvrPlc plc; - ASSERT_TRUE(plc.start()); - auto runtime = std::make_shared(); - ASSERT_TRUE(runtime->init(makeConfig(plc.port()))); - ASSERT_TRUE(runtime->start()) << runtime->lastError(); - ASSERT_NE(runtime->sessionId(), 0U); - - CmvrPlcMotorProtocol protocol(runtime); - ASSERT_TRUE(protocol.initNode(1)); - protocol.setLimitQ(1, 2.0, -2.0); - protocol.setLimitQd(1, 2.0); - protocol.setLimitQdd(1, 5.0, -5.0); - - EXPECT_TRUE(protocol.torqueOn(1)); - EXPECT_TRUE(protocol.commandProfilePosition(1, 1.25, 0.5, 1.0)); - EXPECT_EQ(plc.lastProfileCommandTimeoutMs(), 600000U); - EXPECT_TRUE(protocol.reachedTargetQ(1)); - EXPECT_NEAR(protocol.getQ(1), 1.25, 1e-6); - EXPECT_TRUE(protocol.commandProfileVelocity(1, -0.4, 1.0)); - EXPECT_EQ(plc.lastProfileCommandTimeoutMs(), 600000U); - EXPECT_NEAR(protocol.getQd(1), -0.4, 1e-6); - EXPECT_TRUE(protocol.commandCyclicPosition(1, 0.75, 0.1)); - EXPECT_EQ(plc.cyclicOpenCount(), 1U); - EXPECT_EQ(plc.cyclicApplyCount(), 1U); - EXPECT_EQ(plc.currentStreamFirstSampleSequence(), 1U); - EXPECT_NEAR(protocol.getQ(1), 0.75, 1e-6); - EXPECT_TRUE(protocol.quickStop(1)); - EXPECT_TRUE(protocol.commandCyclicPosition(1, 0.5, 0.1)); - EXPECT_EQ(plc.cyclicOpenCount(), 2U); - EXPECT_EQ(plc.cyclicApplyCount(), 2U); - EXPECT_EQ(plc.currentStreamFirstSampleSequence(), 1U); - EXPECT_NEAR(protocol.getQ(1), 0.5, 1e-6); - EXPECT_TRUE(protocol.quickStop(1)); - EXPECT_TRUE(protocol.commandCyclicVelocity(1, -0.2)); - EXPECT_EQ(plc.cyclicOpenCount(), 3U); - EXPECT_EQ(plc.cyclicApplyCount(), 3U); - EXPECT_EQ(plc.currentStreamFirstSampleSequence(), 1U); - CmvrPlcAxisStatus cyclic_velocity_status; - ASSERT_TRUE(runtime->readAxisStatus(1, cyclic_velocity_status)); - EXPECT_EQ(cyclic_velocity_status.current_mode, - msgs::RUN_MODE_CYCLIC_SYNC_VELOCITY); - EXPECT_NE(cyclic_velocity_status.status_flags & - cmvr_plc::StatusFlag::StreamActive, - 0U); - EXPECT_EQ(cyclic_velocity_status.last_applied_cyclic_sequence, 1U); - EXPECT_EQ(cyclic_velocity_status.actual_velocity, -200000); - EXPECT_TRUE(protocol.quickStop(1)); - EXPECT_TRUE(protocol.calibrateZeroQ(1)); - EXPECT_NEAR(protocol.getQ(1), 0.0, 1e-6); - EXPECT_TRUE(protocol.calibrateZeroQ(1)); - EXPECT_EQ(plc.lastExpectedZeroEpoch(), 1U); - EXPECT_TRUE(protocol.quickStop(1)); - EXPECT_TRUE(protocol.torqueOff(1)); - EXPECT_FALSE(protocol.commandCyclicTorque(1, 0.1)); - runtime->stop(); -} - -TEST(ModbusTcpMotorBusRuntimeTest, AvoidsRedundantBaselineAndHeartbeatWrites) -{ - FakeCmvrPlc plc; - ASSERT_TRUE(plc.start()); - auto group = makeConfig(plc.port()); - group.mutable_modbus_tcp()->set_heartbeat_period_ms(1000); - group.mutable_modbus_tcp()->set_communication_watchdog_ms(3000); - auto runtime = std::make_shared(); - ASSERT_TRUE(runtime->init(group)); - ASSERT_TRUE(runtime->start()); - CmvrPlcMotorProtocol protocol(runtime); - ASSERT_TRUE(protocol.initNode(1)); - protocol.setLimitQ(1, 2.0, -2.0); - protocol.setLimitQd(1, 2.0); - protocol.setLimitQdd(1, 5.0, -5.0); - - const auto heartbeat_writes = plc.clientHeartbeatWriteCount(); - const auto reads_before_profile = plc.fullStatusReadCount(); - ASSERT_TRUE(protocol.commandProfilePosition(1, 0.5, 0.5, 1.0)); - EXPECT_EQ(plc.fullStatusReadCount() - reads_before_profile, 1U); - EXPECT_EQ(plc.clientHeartbeatWriteCount(), heartbeat_writes); - - const auto reads_before_zero = plc.fullStatusReadCount(); - ASSERT_TRUE(protocol.calibrateZeroQ(1)); - EXPECT_EQ(plc.fullStatusReadCount() - reads_before_zero, 2U); - EXPECT_EQ(plc.clientHeartbeatWriteCount(), heartbeat_writes); - runtime->stop(); -} - -TEST(ModbusTcpMotorBusRuntimeTest, ReconnectCreatesNewSessionWithoutReplayingCommands) -{ - FakeCmvrPlc plc; - ASSERT_TRUE(plc.start()); - auto runtime = std::make_shared(); - ASSERT_TRUE(runtime->init(makeConfig(plc.port()))); - ASSERT_TRUE(runtime->start()); - - CmvrPlcMotorProtocol protocol(runtime); - ASSERT_TRUE(protocol.initNode(1)); - ASSERT_TRUE(protocol.commandProfileVelocity(1, 0.2, 0.1)); - const auto old_session = runtime->sessionId(); - const auto old_epoch = runtime->connectionEpoch(); - const auto command_count = plc.commandCount(); - plc.disconnectClient(); - - ASSERT_TRUE(waitUntil( - [&] { return runtime->connected() && runtime->connectionEpoch() > old_epoch; }, - 1500ms)) << runtime->lastError(); - EXPECT_NE(runtime->sessionId(), old_session); - std::this_thread::sleep_for(100ms); - EXPECT_EQ(plc.commandCount(), command_count); - ASSERT_TRUE(protocol.commandProfileVelocity(1, -0.1, 0.1)); - EXPECT_EQ(plc.commandCount(), command_count + 1U); - const auto session_before_stop = runtime->sessionId(); - runtime->stop(); - EXPECT_EQ(runtime->sessionId(), 0U); - ASSERT_TRUE(runtime->start()) << runtime->lastError(); - EXPECT_NE(runtime->sessionId(), session_before_stop); - runtime->stop(); -} - -TEST(ModbusTcpMotorBusRuntimeTest, - ActiveCyclicGenerationRequiresExplicitRestartAfterReconnect) -{ - FakeCmvrPlc plc; - ASSERT_TRUE(plc.start()); - auto runtime = std::make_shared(); - ASSERT_TRUE(runtime->init(makeConfig(plc.port()))); - ASSERT_TRUE(runtime->start()); - - CmvrPlcMotorProtocol protocol(runtime); - ASSERT_TRUE(protocol.initNode(1)); - protocol.setMode(1, msgs::RUN_MODE_CYCLIC_SYNC_POSITION); - ASSERT_TRUE(protocol.commandCyclicPosition(1, 0.1, 0.0)); - ASSERT_EQ(plc.cyclicOpenCount(), 1U); - ASSERT_EQ(plc.cyclicApplyCount(), 1U); - - const auto old_epoch = runtime->connectionEpoch(); - plc.disconnectClient(); - ASSERT_TRUE(waitUntil( - [&] { - return runtime->connected() && - runtime->connectionEpoch() > old_epoch; - }, - 1500ms)) << runtime->lastError(); - - const auto commands_after_reconnect = plc.commandCount(); - const auto opens_after_reconnect = plc.cyclicOpenCount(); - const auto samples_after_reconnect = plc.cyclicApplyCount(); - // The first post-reconnect sample latches this upper-layer generation. - // Further calls from the same generation remain fail-closed and neither - // Open nor sample is committed into the new PLC session. - EXPECT_FALSE(protocol.commandCyclicPosition(1, 0.2, 0.0)); - EXPECT_FALSE(protocol.commandCyclicPosition(1, 0.3, 0.0)); - EXPECT_EQ(plc.commandCount(), commands_after_reconnect); - EXPECT_EQ(plc.cyclicOpenCount(), opens_after_reconnect); - EXPECT_EQ(plc.cyclicApplyCount(), samples_after_reconnect); - - // A new gRPC stream calls setMode even when its mode is unchanged. That - // explicit boundary establishes a new generation and clears the latch. - protocol.setMode(1, msgs::RUN_MODE_CYCLIC_SYNC_POSITION); - EXPECT_TRUE(protocol.commandCyclicPosition(1, 0.4, 0.0)); - EXPECT_EQ(plc.cyclicOpenCount(), opens_after_reconnect + 1U); - EXPECT_EQ(plc.cyclicApplyCount(), samples_after_reconnect + 1U); - EXPECT_EQ(plc.currentStreamFirstSampleSequence(), 1U); - runtime->stop(); -} - -TEST(ModbusTcpMotorBusRuntimeTest, - CyclicOpenQueuedAcrossReconnectCannotEnterNewSession) -{ - FakeCmvrPlc plc; - ASSERT_TRUE(plc.start()); - auto runtime = std::make_shared(); - ASSERT_TRUE(runtime->init(makeConfig(plc.port()))); - ASSERT_TRUE(runtime->start()); - - CmvrPlcMotorProtocol protocol(runtime); - ASSERT_TRUE(protocol.initNode(1)); - protocol.setMode(1, msgs::RUN_MODE_CYCLIC_SYNC_POSITION); - - auto axis_mutex = - ModbusTcpMotorBusRuntimeTestAccess::axisMutex(*runtime, 1); - ASSERT_NE(axis_mutex, nullptr); - std::unique_lock queue_hold(*axis_mutex); - const auto old_epoch = runtime->connectionEpoch(); - const auto mailbox_commits_before = plc.observedMailboxCommitCount(); - const auto opens_before = plc.cyclicOpenCount(); - const auto samples_before = plc.cyclicApplyCount(); - const auto admissions_before = - ModbusTcpMotorBusRuntimeTestAccess::commandAdmissionCount(*runtime); - - std::atomic result{true}; - std::thread queued([&] { - result.store(protocol.commandCyclicPosition(1, 0.1, 0.0)); - }); - const bool admitted = waitUntil( - [&] { - return ModbusTcpMotorBusRuntimeTestAccess::commandAdmissionCount( - *runtime) > - admissions_before; - }, - 500ms); - - // The protocol already bound OpenCyclicPosition to old_epoch, but the - // runtime invocation is still queued. Reconnect before releasing the - // queue and prove that expected_connection_epoch prevents both Open and - // its sample from entering the new PLC ownership session. - plc.disconnectClient(); - const bool reconnected = waitUntil( - [&] { - return runtime->connected() && - runtime->connectionEpoch() > old_epoch; - }, - 1500ms); - queue_hold.unlock(); - queued.join(); - - EXPECT_TRUE(admitted); - EXPECT_TRUE(reconnected) << runtime->lastError(); - EXPECT_FALSE(result.load()); - EXPECT_EQ(plc.observedMailboxCommitCount(), mailbox_commits_before); - EXPECT_EQ(plc.cyclicOpenCount(), opens_before); - EXPECT_EQ(plc.cyclicApplyCount(), samples_before); - - // Failure while crossing epochs permanently latches this generation. - EXPECT_FALSE(protocol.commandCyclicPosition(1, 0.2, 0.0)); - EXPECT_EQ(plc.observedMailboxCommitCount(), mailbox_commits_before); - EXPECT_EQ(plc.cyclicOpenCount(), opens_before); - EXPECT_EQ(plc.cyclicApplyCount(), samples_before); - - // Only the explicit boundary of a new upper-layer stream may recover. - protocol.setMode(1, msgs::RUN_MODE_CYCLIC_SYNC_POSITION); - EXPECT_TRUE(protocol.commandCyclicPosition(1, 0.3, 0.0)); - EXPECT_EQ(plc.cyclicOpenCount(), opens_before + 1U); - EXPECT_EQ(plc.cyclicApplyCount(), samples_before + 1U); - EXPECT_EQ(plc.currentStreamFirstSampleSequence(), 1U); - runtime->stop(); -} - -TEST(ModbusTcpMotorBusRuntimeTest, ProfileFeedbackIsBoundToConnectionEpoch) -{ - FakeCmvrPlc plc; - ASSERT_TRUE(plc.start()); - auto runtime = std::make_shared(); - ASSERT_TRUE(runtime->init(makeConfig(plc.port()))); - ASSERT_TRUE(runtime->start()); - - CmvrPlcMotorProtocol protocol(runtime); - ASSERT_TRUE(protocol.initNode(1)); - // Zero is deliberately chosen: the new PLC session's safe stopped - // velocity is also zero, so value equality cannot prove command identity. - ASSERT_TRUE(protocol.commandProfileVelocity(1, 0.0, 0.1)); - ASSERT_TRUE(std::isfinite(protocol.getQd(1))); - ASSERT_TRUE(protocol.reachedTargetQ(1)); - - auto old_epoch = runtime->connectionEpoch(); - plc.disconnectClient(); - ASSERT_TRUE(waitUntil( - [&] { - return runtime->connected() && - runtime->connectionEpoch() > old_epoch; - }, - 1500ms)) << runtime->lastError(); - - EXPECT_TRUE(std::isnan(protocol.getQ(1))); - EXPECT_TRUE(std::isnan(protocol.getQd(1))); - EXPECT_FALSE(protocol.reachedTargetQ(1)); - - // A successfully ACKed profile in the current session rebinds feedback. - ASSERT_TRUE(protocol.commandProfileVelocity(1, 0.0, 0.1)); - EXPECT_TRUE(std::isfinite(protocol.getQ(1))); - EXPECT_NEAR(protocol.getQd(1), 0.0, 1e-9); - - old_epoch = runtime->connectionEpoch(); - plc.disconnectClient(); - ASSERT_TRUE(waitUntil( - [&] { - return runtime->connected() && - runtime->connectionEpoch() > old_epoch; - }, - 1500ms)) << runtime->lastError(); - EXPECT_TRUE(std::isnan(protocol.getQd(1))); - - // Successful explicit safety recovery also clears the stale binding. - ASSERT_TRUE(protocol.quickStop(1)); - EXPECT_TRUE(std::isfinite(protocol.getQ(1))); - EXPECT_NEAR(protocol.getQd(1), 0.0, 1e-9); - runtime->stop(); -} - -TEST(ModbusTcpMotorBusRuntimeTest, SupervisorConnectsWhenPlcComesOnlineAfterStart) -{ - const auto port = reserveLoopbackPort(); - ASSERT_NE(port, 0U); - auto runtime = std::make_shared(); - ASSERT_TRUE(runtime->init(makeConfig(port))); - EXPECT_TRUE(runtime->start()); - EXPECT_FALSE(runtime->connected()); - CmvrPlcAxisStatus status; - EXPECT_FALSE(runtime->readAxisStatus(1, status)); - - FakeCmvrPlc plc; - ASSERT_TRUE(plc.start(port)); - ASSERT_TRUE(waitUntil([&] { return runtime->connected(); }, 1500ms)) - << runtime->lastError(); - runtime->stop(); -} - -TEST(ModbusTcpMotorBusRuntimeTest, StopDuringHandshakeLeavesRuntimeDisconnected) -{ - FakeCmvrPlc plc; - plc.setHandshakeAcceptanceDelay(500); - ASSERT_TRUE(plc.start()); - ModbusTcpMotorBusRuntime runtime; - ASSERT_TRUE(runtime.init(makeConfig(plc.port()))); - std::atomic start_result{false}; - std::thread starter([&] { start_result.store(runtime.start()); }); - std::this_thread::sleep_for(50ms); - runtime.stop(); - starter.join(); - EXPECT_TRUE(start_result.load()); - EXPECT_FALSE(runtime.connected()); - EXPECT_EQ(runtime.sessionId(), 0U); -} - -TEST(ModbusTcpMotorBusRuntimeTest, ConcurrentStartStopCannotResurrectWorker) -{ - const auto port = reserveLoopbackPort(); - ASSERT_NE(port, 0U); - ModbusTcpMotorBusRuntime runtime; - ASSERT_TRUE(runtime.init(makeConfig(port))); - for (int iteration = 0; iteration < 20; ++iteration) { - std::atomic ready{0}; - auto synchronize = [&] { - ready.fetch_add(1); - while (ready.load() < 2) { - std::this_thread::yield(); - } - }; - std::thread starter([&] { - synchronize(); - runtime.start(); - }); - std::thread stopper([&] { - synchronize(); - runtime.stop(); - }); - starter.join(); - stopper.join(); - // If start linearized after stop, this final stop owns that later - // lifecycle. It must always terminate and leave no resurrected worker. - runtime.stop(); - EXPECT_FALSE(runtime.connected()); - EXPECT_EQ(runtime.sessionId(), 0U); - } -} - -TEST(ModbusTcpMotorBusRuntimeTest, RejectsZeroPlcBootId) -{ - FakeCmvrPlc plc; - plc.setInitialBootId(0); - ASSERT_TRUE(plc.start()); - ModbusTcpMotorBusRuntime runtime; - ASSERT_TRUE(runtime.init(makeConfig(plc.port()))); - EXPECT_TRUE(runtime.start()); - EXPECT_FALSE(runtime.connected()); - EXPECT_NE(runtime.lastError().find("boot_id must be non-zero"), - std::string::npos); - runtime.stop(); -} - -TEST(ModbusTcpMotorBusRuntimeTest, RejectsBootIdChangeDuringHandshake) -{ - FakeCmvrPlc plc; - plc.setHandshakeMutation(FakeCmvrPlc::HandshakeMutation::BootId); - ASSERT_TRUE(plc.start()); - ModbusTcpMotorBusRuntime runtime; - ASSERT_TRUE(runtime.init(makeConfig(plc.port()))); - EXPECT_TRUE(runtime.start()); - EXPECT_FALSE(runtime.connected()); - EXPECT_NE(runtime.lastError().find("boot_id changed"), - std::string::npos); - runtime.stop(); -} - -TEST(ModbusTcpMotorBusRuntimeTest, RejectsProtocolVersionChangeDuringHandshake) -{ - FakeCmvrPlc plc; - plc.setHandshakeMutation( - FakeCmvrPlc::HandshakeMutation::ProtocolMajor); - ASSERT_TRUE(plc.start()); - ModbusTcpMotorBusRuntime runtime; - ASSERT_TRUE(runtime.init(makeConfig(plc.port()))); - EXPECT_TRUE(runtime.start()); - EXPECT_FALSE(runtime.connected()); - EXPECT_NE(runtime.lastError().find("identity changed"), - std::string::npos); - runtime.stop(); -} - -TEST(ModbusTcpMotorBusRuntimeTest, IgnoresRejectedDecisionForStaleSession) -{ - FakeCmvrPlc plc; - plc.preserveStaleRejectedOwnerDuringHandshake(true); - ASSERT_TRUE(plc.start()); - ModbusTcpMotorBusRuntime runtime; - ASSERT_TRUE(runtime.init(makeConfig(plc.port()))); - EXPECT_TRUE(runtime.start()) << runtime.lastError(); - EXPECT_TRUE(runtime.connected()); - runtime.stop(); -} - -TEST(ModbusTcpMotorBusRuntimeTest, RejectsInProgressAndOddStatusSnapshots) -{ - FakeCmvrPlc plc; - ASSERT_TRUE(plc.start()); - auto runtime = std::make_shared(); - ASSERT_TRUE(runtime->init(makeConfig(plc.port()))); - ASSERT_TRUE(runtime->start()); - - CmvrPlcAxisStatus status; - plc.holdStatusSnapshotInProgress(true); - EXPECT_FALSE(runtime->readAxisStatus(1, status)); - EXPECT_NE(runtime->lastError().find("bounded seqlock"), - std::string::npos); - - plc.holdStatusSnapshotInProgress(false); - plc.publishOddEqualStatusSnapshot(true); - EXPECT_FALSE(runtime->readAxisStatus(1, status)); - EXPECT_NE(runtime->lastError().find("bounded seqlock"), - std::string::npos); - runtime->stop(); -} - -TEST(ModbusTcpMotorBusRuntimeTest, RetriesTornReadAndAcceptsAdvancingStableVersions) -{ - FakeCmvrPlc plc; - ASSERT_TRUE(plc.start()); - auto runtime = std::make_shared(); - ASSERT_TRUE(runtime->init(makeConfig(plc.port()))); - ASSERT_TRUE(runtime->start()); - - CmvrPlcAxisStatus status; - for (std::int32_t position = 100; position <= 300; position += 100) { - plc.publishPosition(position); - ASSERT_TRUE(runtime->readAxisStatus(1, status)); - EXPECT_EQ(status.actual_position, position); - } - - plc.injectMixedStablePositionOnce(999999, 424242); - ASSERT_TRUE(runtime->readAxisStatus(1, status)) << runtime->lastError(); - EXPECT_EQ(status.actual_position, 424242); - runtime->stop(); -} - -TEST(ModbusTcpMotorBusRuntimeTest, QuickStopPreemptsPendingNormalCommand) -{ - FakeCmvrPlc plc; - ASSERT_TRUE(plc.start()); - auto runtime = std::make_shared(); - ASSERT_TRUE(runtime->init(makeConfig(plc.port()))); - ASSERT_TRUE(runtime->start()); - plc.delayProfileAcks(true); - - CmvrPlcAxisCommand normal; - normal.code = cmvr_plc::CommandCode::ProfileVelocity; - normal.target_velocity = 100000; - normal.command_timeout_ms = 5000; - std::atomic normal_result{true}; - std::thread pending([&] { - normal_result.store(runtime->submitAxisCommand(1, normal, false)); - }); - std::this_thread::sleep_for(50ms); - - CmvrPlcAxisCommand stop; - stop.code = cmvr_plc::CommandCode::QuickStop; - stop.command_timeout_ms = 1000; - const auto started = std::chrono::steady_clock::now(); - EXPECT_TRUE(runtime->submitAxisSafetyCommand(1, stop)); - const auto elapsed = std::chrono::duration_cast( - std::chrono::steady_clock::now() - started); - pending.join(); - EXPECT_FALSE(normal_result.load()); - EXPECT_LT(elapsed.count(), 500); - runtime->stop(); -} - -TEST(ModbusTcpMotorBusRuntimeTest, QueuedNormalCommandKeepsPreSafetyGeneration) -{ - FakeCmvrPlc plc; - ASSERT_TRUE(plc.start()); - auto runtime = std::make_shared(); - ASSERT_TRUE(runtime->init(makeConfig(plc.port()))); - ASSERT_TRUE(runtime->start()); - plc.delayProfileAcks(true); - - CmvrPlcAxisCommand normal_a; - normal_a.code = cmvr_plc::CommandCode::ProfileVelocity; - normal_a.target_velocity = 100000; - normal_a.command_timeout_ms = 5000; - std::atomic result_a{true}; - std::thread first([&] { - result_a.store(runtime->submitAxisCommand(1, normal_a, false)); - }); - EXPECT_TRUE(waitUntil( - [&] { return plc.observedProfileMailboxCount() == 1U; }, 500ms)); - - CmvrPlcAxisCommand normal_b = normal_a; - normal_b.target_velocity = 200000; - std::atomic result_b{true}; - const auto admissions_before_b = - ModbusTcpMotorBusRuntimeTestAccess::commandAdmissionCount(*runtime); - std::thread second([&] { - result_b.store(runtime->submitAxisCommand(1, normal_b, false)); - }); - // A is still holding axis_mutex. This counter advances in the exact - // critical section where B snapshots the pre-safety generation, before B - // starts waiting for the per-axis queue. - EXPECT_TRUE(waitUntil( - [&] { - return ModbusTcpMotorBusRuntimeTestAccess::commandAdmissionCount( - *runtime) > - admissions_before_b; - }, - 500ms)); - - CmvrPlcAxisCommand stop; - stop.code = cmvr_plc::CommandCode::QuickStop; - stop.command_timeout_ms = 1000; - EXPECT_TRUE(runtime->submitAxisSafetyCommand(1, stop)); - first.join(); - second.join(); - - EXPECT_FALSE(result_a.load()); - EXPECT_FALSE(result_b.load()); - EXPECT_EQ(plc.observedProfileMailboxCount(), 1U); - runtime->stop(); -} - -TEST(ModbusTcpMotorBusRuntimeTest, QueuedNormalCommandCannotCrossReconnectEpoch) -{ - FakeCmvrPlc plc; - ASSERT_TRUE(plc.start()); - auto runtime = std::make_shared(); - ASSERT_TRUE(runtime->init(makeConfig(plc.port()))); - ASSERT_TRUE(runtime->start()); - - auto axis_mutex = - ModbusTcpMotorBusRuntimeTestAccess::axisMutex(*runtime, 1); - ASSERT_NE(axis_mutex, nullptr); - std::unique_lock queue_hold(*axis_mutex); - const auto old_epoch = runtime->connectionEpoch(); - const auto mailbox_commits_before = plc.observedMailboxCommitCount(); - const auto admissions_before = - ModbusTcpMotorBusRuntimeTestAccess::commandAdmissionCount(*runtime); - - CmvrPlcAxisCommand command; - command.code = cmvr_plc::CommandCode::ProfileVelocity; - command.target_velocity = 200000; - command.command_timeout_ms = 1000; - std::atomic result{true}; - std::thread queued([&] { - result.store(runtime->submitAxisCommand(1, command, false)); - }); - const bool admitted = waitUntil( - [&] { - return ModbusTcpMotorBusRuntimeTestAccess::commandAdmissionCount( - *runtime) > - admissions_before; - }, - 500ms); - - // Keep the old invocation behind axis_mutex while the worker detects the - // broken socket and completes a new ownership handshake. - plc.disconnectClient(); - const bool reconnected = waitUntil( - [&] { - return runtime->connected() && - runtime->connectionEpoch() > old_epoch; - }, - 1500ms); - queue_hold.unlock(); - queued.join(); - - EXPECT_TRUE(admitted); - EXPECT_TRUE(reconnected) << runtime->lastError(); - EXPECT_FALSE(result.load()); - EXPECT_EQ(plc.observedMailboxCommitCount(), mailbox_commits_before); - runtime->stop(); -} - -TEST(ModbusTcpMotorBusRuntimeTest, - ProfileSubmitQueuedAcrossReconnectInvalidatesFeedback) -{ - FakeCmvrPlc plc; - ASSERT_TRUE(plc.start()); - auto runtime = std::make_shared(); - ASSERT_TRUE(runtime->init(makeConfig(plc.port()))); - ASSERT_TRUE(runtime->start()); - CmvrPlcMotorProtocol protocol(runtime); - ASSERT_TRUE(protocol.initNode(1)); - - auto axis_mutex = - ModbusTcpMotorBusRuntimeTestAccess::axisMutex(*runtime, 1); - ASSERT_NE(axis_mutex, nullptr); - std::unique_lock queue_hold(*axis_mutex); - const auto old_epoch = runtime->connectionEpoch(); - const auto mailbox_commits_before = plc.observedMailboxCommitCount(); - const auto admissions_before = - ModbusTcpMotorBusRuntimeTestAccess::commandAdmissionCount(*runtime); - - std::atomic result{true}; - std::thread queued([&] { - result.store(protocol.commandProfileVelocity(1, 0.0, 0.1)); - }); - const bool admitted = waitUntil( - [&] { - return ModbusTcpMotorBusRuntimeTestAccess::commandAdmissionCount( - *runtime) > - admissions_before; - }, - 500ms); - - plc.disconnectClient(); - const bool reconnected = waitUntil( - [&] { - return runtime->connected() && - runtime->connectionEpoch() > old_epoch; - }, - 1500ms); - queue_hold.unlock(); - queued.join(); - - EXPECT_TRUE(admitted); - EXPECT_TRUE(reconnected) << runtime->lastError(); - EXPECT_FALSE(result.load()); - EXPECT_EQ(plc.observedMailboxCommitCount(), mailbox_commits_before); - // Even though the safe S2 process image reports qdot=0, the ambiguous S1 - // invocation cannot claim that value as successful Profile feedback. - EXPECT_TRUE(std::isnan(protocol.getQd(1))); - ASSERT_TRUE(protocol.quickStop(1)); - EXPECT_NEAR(protocol.getQd(1), 0.0, 1e-9); - runtime->stop(); -} - -TEST(ModbusTcpMotorBusRuntimeTest, - RejectsCallerExpectedOldEpochWithoutMailboxCommit) -{ - FakeCmvrPlc plc; - ASSERT_TRUE(plc.start()); - auto runtime = std::make_shared(); - ASSERT_TRUE(runtime->init(makeConfig(plc.port()))); - ASSERT_TRUE(runtime->start()); - - const auto old_epoch = runtime->connectionEpoch(); - plc.disconnectClient(); - ASSERT_TRUE(waitUntil( - [&] { - return runtime->connected() && - runtime->connectionEpoch() > old_epoch; - }, - 1500ms)) << runtime->lastError(); - const auto mailbox_commits_before = plc.observedMailboxCommitCount(); - - CmvrPlcAxisCommand command; - command.code = cmvr_plc::CommandCode::ProfileVelocity; - command.target_velocity = 100000; - command.command_timeout_ms = 1000; - EXPECT_FALSE(runtime->submitAxisCommand( - 1, command, false, old_epoch)); - EXPECT_EQ(plc.observedMailboxCommitCount(), mailbox_commits_before); - runtime->stop(); -} - -TEST(ModbusTcpMotorBusRuntimeTest, QueuedSafetyCommandCannotCrossReconnectEpoch) -{ - FakeCmvrPlc plc; - ASSERT_TRUE(plc.start()); - auto runtime = std::make_shared(); - ASSERT_TRUE(runtime->init(makeConfig(plc.port()))); - ASSERT_TRUE(runtime->start()); - - auto safety_mutex = - ModbusTcpMotorBusRuntimeTestAccess::safetyMutex(*runtime, 1); - ASSERT_NE(safety_mutex, nullptr); - std::unique_lock queue_hold(*safety_mutex); - const auto old_epoch = runtime->connectionEpoch(); - const auto mailbox_commits_before = plc.observedMailboxCommitCount(); - const auto admissions_before = - ModbusTcpMotorBusRuntimeTestAccess::commandAdmissionCount(*runtime); - - CmvrPlcAxisCommand stop; - stop.code = cmvr_plc::CommandCode::QuickStop; - stop.command_timeout_ms = 1000; - std::atomic result{true}; - std::thread queued([&] { - result.store(runtime->submitAxisSafetyCommand(1, stop)); - }); - const bool admitted = waitUntil( - [&] { - return ModbusTcpMotorBusRuntimeTestAccess::commandAdmissionCount( - *runtime) > - admissions_before; - }, - 500ms); - - plc.disconnectClient(); - const bool reconnected = waitUntil( - [&] { - return runtime->connected() && - runtime->connectionEpoch() > old_epoch; - }, - 1500ms); - queue_hold.unlock(); - queued.join(); - - EXPECT_TRUE(admitted); - EXPECT_TRUE(reconnected) << runtime->lastError(); - EXPECT_FALSE(result.load()); - EXPECT_EQ(plc.observedMailboxCommitCount(), mailbox_commits_before); - runtime->stop(); -} - -TEST(ModbusTcpMotorBusRuntimeTest, ConcurrentSafetyCommandsAreSerialized) -{ - FakeCmvrPlc plc; - ASSERT_TRUE(plc.start()); - auto runtime = std::make_shared(); - ASSERT_TRUE(runtime->init(makeConfig(plc.port()))); - ASSERT_TRUE(runtime->start()); - plc.setQuickStopProcessingDelay(50); - - CmvrPlcAxisCommand stop; - stop.code = cmvr_plc::CommandCode::QuickStop; - stop.command_timeout_ms = 1000; - std::atomic first{false}; - std::atomic second{false}; - std::atomic ready{0}; - auto invoke = [&](std::atomic& result) { - ready.fetch_add(1); - while (ready.load() < 2) { - std::this_thread::yield(); - } - result.store(runtime->submitAxisSafetyCommand(1, stop)); - }; - std::thread first_thread(invoke, std::ref(first)); - std::thread second_thread(invoke, std::ref(second)); - first_thread.join(); - second_thread.join(); - EXPECT_TRUE(first.load()); - EXPECT_TRUE(second.load()); - runtime->stop(); -} - -TEST(ModbusTcpMotorBusRuntimeTest, FatalStatusFlagsInvalidateMotionFeedback) -{ - FakeCmvrPlc plc; - ASSERT_TRUE(plc.start()); - auto runtime = std::make_shared(); - ASSERT_TRUE(runtime->init(makeConfig(plc.port()))); - ASSERT_TRUE(runtime->start()); - CmvrPlcMotorProtocol protocol(runtime); - ASSERT_TRUE(protocol.initNode(1)); - - plc.publishPosition(123456); - ASSERT_TRUE(std::isfinite(protocol.getQ(1))); - plc.publishFatalFlags(); - EXPECT_TRUE(std::isnan(protocol.getQ(1))); - EXPECT_TRUE(std::isnan(protocol.getQd(1))); - EXPECT_FALSE(protocol.reachedTargetQ(1)); - runtime->stop(); -} - -TEST(ModbusTcpMotorBusRuntimeTest, RejectsOutOfRangeAxisAndReportsInvalidSamples) -{ - auto hostname_group = makeConfig(15020); - hostname_group.mutable_modbus_tcp()->set_host("localhost"); - ModbusTcpMotorBusRuntime hostname_runtime; - EXPECT_FALSE(hostname_runtime.init(hostname_group)); - - auto group = makeConfig(15020); - group.mutable_modbus_tcp()->mutable_axes(0)->set_axis_index(1000); - ModbusTcpMotorBusRuntime invalid_runtime; - EXPECT_FALSE(invalid_runtime.init(group)); - - auto runtime = std::make_shared(); - ASSERT_TRUE(runtime->init(makeConfig(15020))); - CmvrPlcMotorProtocol protocol(runtime); - ASSERT_TRUE(protocol.initNode(1)); - EXPECT_TRUE(std::isnan(protocol.getQ(1))); - EXPECT_TRUE(std::isnan(protocol.getQd(1))); - EXPECT_FALSE(protocol.commandProfilePosition( - 1, std::numeric_limits::quiet_NaN(), 0.1, 0.1)); -} - -TEST(ModbusTcpMotorBusRuntimeTest, RepositorySampleConfigParsesAndInitializesRuntime) -{ - std::ifstream input(CMVR_PLC_MOTOR_SAMPLE_CONFIG_PATH); - ASSERT_TRUE(input.is_open()) << CMVR_PLC_MOTOR_SAMPLE_CONFIG_PATH; - const std::string text((std::istreambuf_iterator(input)), - std::istreambuf_iterator()); - config::MotorRootConfig root; - ASSERT_TRUE(google::protobuf::TextFormat::ParseFromString(text, &root)); - ASSERT_TRUE(root.has_motor()); - ASSERT_EQ(root.motor().motor_groups_size(), 1); - - ModbusTcpMotorBusRuntime runtime; - EXPECT_TRUE(runtime.init(root.motor().motor_groups(0))) << runtime.lastError(); -} - -} // namespace -} // namespace cmvr::device diff --git a/cmvr-es/devices/motor/drivers/modbus_plc_motor/CMakeLists.txt b/cmvr-es/devices/motor/drivers/modbus_plc_motor/CMakeLists.txt deleted file mode 100644 index 7dbd3cdc..00000000 --- a/cmvr-es/devices/motor/drivers/modbus_plc_motor/CMakeLists.txt +++ /dev/null @@ -1,21 +0,0 @@ -add_library(modbus_plc_motor_driver SHARED - src/cmvr_plc_motor_protocol.cpp - src/modbus_plc_motor.cpp -) - -target_include_directories(modbus_plc_motor_driver - PUBLIC - ${CMAKE_CURRENT_SOURCE_DIR}/include -) - -target_link_libraries(modbus_plc_motor_driver - PUBLIC - cmvr_es::device::motor_core - cmvr_es::device::motor_bus_runtime - PRIVATE - cmvr_es::proto - glog -) - -add_library(cmvr_es::device::modbus_plc_motor_driver ALIAS modbus_plc_motor_driver) -install(TARGETS modbus_plc_motor_driver LIBRARY DESTINATION lib) diff --git a/cmvr-es/devices/motor/drivers/modbus_plc_motor/include/cmvr_plc_motor_protocol.h b/cmvr-es/devices/motor/drivers/modbus_plc_motor/include/cmvr_plc_motor_protocol.h deleted file mode 100644 index 0bf534ba..00000000 --- a/cmvr-es/devices/motor/drivers/modbus_plc_motor/include/cmvr_plc_motor_protocol.h +++ /dev/null @@ -1,89 +0,0 @@ -#ifndef CMVR_ES_CMVR_PLC_MOTOR_PROTOCOL_H -#define CMVR_ES_CMVR_PLC_MOTOR_PROTOCOL_H - -#include -#include -#include -#include -#include - -#include "devices/motor/bus_runtime/modbus_tcp/include/modbus_tcp_motor_bus_runtime.h" -#include "devices/motor/motor_protocol_interface.h" - -namespace cmvr::device { - -class CmvrPlcMotorProtocol final : public MotorProtocolInterface { -public: - explicit CmvrPlcMotorProtocol(std::shared_ptr bus_runtime); - ~CmvrPlcMotorProtocol() override = default; - - bool initNode(std::uint8_t node_id) override; - void setMode(std::uint8_t node_id, msgs::RunMode mode) override; - msgs::RunMode getMode(std::uint8_t node_id) override; - void setLimitQdd(std::uint8_t node_id, double u_qdd, double l_qdd) override; - void setLimitQd(std::uint8_t node_id, double qd) override; - void setLimitQ(std::uint8_t node_id, double ub, double lb) override; - bool calibrateZeroQ(std::uint8_t node_id) override; - bool reachedTargetQ(std::uint8_t node_id) override; - bool commandProfilePosition(std::uint8_t node_id, - double target_q, - double max_qd, - double max_qdd) override; - bool commandProfileVelocity(std::uint8_t node_id, - double target_qd, - double max_qdd) override; - bool commandCyclicPosition(std::uint8_t node_id, - double target_q, - double target_qd) override; - bool commandCyclicVelocity(std::uint8_t node_id, double target_qd) override; - bool commandCyclicTorque(std::uint8_t node_id, double target_tau) override; - void setMotorConversion(std::uint8_t node_id, - double encoder_counts_per_rev, - double gear_ratio) override; - bool torqueOn(std::uint8_t node_id) override; - bool torqueOff(std::uint8_t node_id) override; - bool brakeRelease(std::uint8_t node_id) override; - bool quickStop(std::uint8_t node_id) override; - double getQ(std::uint8_t node_id) override; - double getQd(std::uint8_t node_id) override; - -private: - struct NodeState { - msgs::RunMode requested_mode{msgs::RUN_MODE_UNSPECIFIED}; - double limit_q_lb{0.0}; - double limit_q_ub{0.0}; - double limit_qd{0.0}; - double limit_qdd{0.0}; - bool cyclic_stream_open{false}; - bool cyclic_reconnect_latched{false}; - std::uint32_t cyclic_sequence{0}; - std::uint64_t cyclic_generation{0}; - std::uint64_t cyclic_connection_epoch{0}; - bool profile_feedback_bound{false}; - std::uint64_t profile_connection_epoch{0}; - }; - - bool submitSimple_(std::uint8_t node_id, - cmvr_plc::CommandCode code, - bool wait_for_terminal, - std::uint32_t timeout_ms); - bool ensureCyclicOpen_(std::uint8_t node_id, - NodeState& state, - msgs::RunMode mode); - static std::optional toMicroUnits_(double value); - static double fromMicroUnits_(std::int32_t value); - static bool isSupportedMode_(msgs::RunMode mode); - bool validatePosition_(const NodeState& state, double position) const; - bool validateVelocity_(const NodeState& state, double velocity) const; - bool validateAcceleration_(const NodeState& state, double acceleration) const; - bool readMotionStatus_(std::uint8_t node_id, CmvrPlcAxisStatus& status); - NodeState& nodeStateLocked_(std::uint8_t node_id); - - std::shared_ptr bus_runtime_; - std::mutex nodes_mutex_; - std::unordered_map nodes_; -}; - -} // namespace cmvr::device - -#endif // CMVR_ES_CMVR_PLC_MOTOR_PROTOCOL_H diff --git a/cmvr-es/devices/motor/drivers/modbus_plc_motor/include/modbus_plc_motor.h b/cmvr-es/devices/motor/drivers/modbus_plc_motor/include/modbus_plc_motor.h deleted file mode 100644 index f0a96831..00000000 --- a/cmvr-es/devices/motor/drivers/modbus_plc_motor/include/modbus_plc_motor.h +++ /dev/null @@ -1,21 +0,0 @@ -#ifndef CMVR_ES_MODBUS_PLC_MOTOR_H -#define CMVR_ES_MODBUS_PLC_MOTOR_H - -#include "cmvr/config/motor_config/motor_config.pb.h" -#include "devices/motor/abstract_motor.h" - -namespace cmvr::device { - -class ModbusPlcMotor final : public AbstractMotor { -public: - explicit ModbusPlcMotor(const config::MotorConfigItem& config); - - std::string typeName() const override { return "ModbusPlcMotor"; } - bool init() override; - bool torqueOff() override; - bool quickStop() override; -}; - -} // namespace cmvr::device - -#endif // CMVR_ES_MODBUS_PLC_MOTOR_H diff --git a/cmvr-es/devices/motor/drivers/modbus_plc_motor/src/cmvr_plc_motor_protocol.cpp b/cmvr-es/devices/motor/drivers/modbus_plc_motor/src/cmvr_plc_motor_protocol.cpp deleted file mode 100644 index 3867303c..00000000 --- a/cmvr-es/devices/motor/drivers/modbus_plc_motor/src/cmvr_plc_motor_protocol.cpp +++ /dev/null @@ -1,582 +0,0 @@ -#include "devices/motor/drivers/modbus_plc_motor/include/cmvr_plc_motor_protocol.h" - -#include -#include -#include - -#include "common/base/logging/logger.h" - -namespace cmvr::device { -namespace { - -// MotorService accepts waits up to 10 minutes. The PLC timeout is a ceiling; -// shorter RPC timeouts still actively QuickStop from the service layer. -constexpr std::uint32_t kProfileCommandTimeoutCeilingMs = 600000; -constexpr std::uint32_t kSafetyCommandTimeoutMs = 5000; - -bool statusAllowsMotionFeedback(const CmvrPlcAxisStatus& status) -{ - constexpr std::uint16_t kFatalFlags = - cmvr_plc::StatusFlag::Fault | - cmvr_plc::StatusFlag::CommunicationWatchdogExpired | - cmvr_plc::StatusFlag::CyclicWatchdogExpired; - return (status.status_flags & kFatalFlags) == 0U && - status.result_code == cmvr_plc::ResultCode::Ok && - static_cast(status.command_state) <= - static_cast( - cmvr_plc::CommandState::CommunicationLost) && - !cmvr_plc::isFailure(status.command_state); -} - -} // namespace - -CmvrPlcMotorProtocol::CmvrPlcMotorProtocol( - std::shared_ptr bus_runtime) - : bus_runtime_(std::move(bus_runtime)) -{ - comm_proto = CommProto::CUSTOM; -} - -bool CmvrPlcMotorProtocol::initNode(const std::uint8_t node_id) -{ - if (!bus_runtime_ || !bus_runtime_->hasMotor(node_id)) { - CMVR_LOG(ERROR) << "[CmvrPlcMotorProtocol] missing PLC axis mapping for motor " - << static_cast(node_id); - return false; - } - std::lock_guard lock(nodes_mutex_); - auto& state = nodeStateLocked_(node_id); - state.cyclic_connection_epoch = bus_runtime_->connectionEpoch(); - state.cyclic_stream_open = false; - state.cyclic_reconnect_latched = false; - state.cyclic_sequence = 0; - state.cyclic_generation = 0; - state.profile_feedback_bound = false; - state.profile_connection_epoch = 0; - return true; -} - -void CmvrPlcMotorProtocol::setMode( - const std::uint8_t node_id, - const msgs::RunMode mode) -{ - if (!isSupportedMode_(mode)) { - CMVR_LOG(ERROR) << "[CmvrPlcMotorProtocol] unsupported mode " - << msgs::RunMode_Name(mode); - return; - } - std::lock_guard lock(nodes_mutex_); - auto& state = nodeStateLocked_(node_id); - const bool cyclic_mode = - mode == msgs::RUN_MODE_CYCLIC_SYNC_POSITION || - mode == msgs::RUN_MODE_CYCLIC_SYNC_VELOCITY; - if (cyclic_mode) { - // Every explicit cyclic setMode call is a new upper-layer stream - // generation, even when the mode value itself is unchanged. This is - // the only transition allowed to clear a reconnect latch. - state.requested_mode = mode; - state.cyclic_connection_epoch = - bus_runtime_ ? bus_runtime_->connectionEpoch() : 0U; - state.cyclic_stream_open = false; - state.cyclic_reconnect_latched = false; - state.cyclic_sequence = 0; - if (++state.cyclic_generation == 0U) { - ++state.cyclic_generation; - } - return; - } - if (state.requested_mode != mode) { - state.cyclic_stream_open = false; - } - state.requested_mode = mode; -} - -msgs::RunMode CmvrPlcMotorProtocol::getMode(const std::uint8_t node_id) -{ - CmvrPlcAxisStatus status; - if (bus_runtime_ && bus_runtime_->readAxisStatus(node_id, status)) { - const auto mode = static_cast(status.current_mode); - if (isSupportedMode_(mode)) { - return mode; - } - } - std::lock_guard lock(nodes_mutex_); - return nodeStateLocked_(node_id).requested_mode; -} - -void CmvrPlcMotorProtocol::setLimitQdd( - const std::uint8_t node_id, - const double u_qdd, - const double l_qdd) -{ - std::lock_guard lock(nodes_mutex_); - nodeStateLocked_(node_id).limit_qdd = std::max(std::abs(u_qdd), std::abs(l_qdd)); -} - -void CmvrPlcMotorProtocol::setLimitQd( - const std::uint8_t node_id, - const double qd) -{ - std::lock_guard lock(nodes_mutex_); - nodeStateLocked_(node_id).limit_qd = std::abs(qd); -} - -void CmvrPlcMotorProtocol::setLimitQ( - const std::uint8_t node_id, - const double ub, - const double lb) -{ - std::lock_guard lock(nodes_mutex_); - auto& state = nodeStateLocked_(node_id); - state.limit_q_ub = ub; - state.limit_q_lb = lb; -} - -bool CmvrPlcMotorProtocol::calibrateZeroQ(const std::uint8_t node_id) -{ - return submitSimple_(node_id, cmvr_plc::CommandCode::SetZero, true, - kSafetyCommandTimeoutMs); -} - -bool CmvrPlcMotorProtocol::reachedTargetQ(const std::uint8_t node_id) -{ - CmvrPlcAxisStatus status; - return readMotionStatus_(node_id, status) && - statusAllowsMotionFeedback(status) && - (status.status_flags & cmvr_plc::StatusFlag::TargetReached) != 0U; -} - -bool CmvrPlcMotorProtocol::commandProfilePosition( - const std::uint8_t node_id, - const double target_q, - const double max_qd, - const double max_qdd) -{ - NodeState state; - { - std::lock_guard lock(nodes_mutex_); - auto& stored = nodeStateLocked_(node_id); - stored.requested_mode = msgs::RUN_MODE_PROFILE_POSITION; - stored.cyclic_stream_open = false; - state = stored; - } - if (!validatePosition_(state, target_q) || - !validateVelocity_(state, max_qd) || - !validateAcceleration_(state, max_qdd)) { - CMVR_LOG(ERROR) << "[CmvrPlcMotorProtocol] invalid profile-position command"; - return false; - } - const auto position = toMicroUnits_(target_q); - const auto velocity = toMicroUnits_(std::abs(max_qd)); - const auto acceleration = toMicroUnits_(std::abs(max_qdd)); - if (!position || !velocity || !acceleration) { - return false; - } - - CmvrPlcAxisCommand command; - command.code = cmvr_plc::CommandCode::ProfilePosition; - command.target_position = *position; - command.target_velocity = *velocity; - command.acceleration = *acceleration; - command.command_timeout_ms = kProfileCommandTimeoutCeilingMs; - if (!bus_runtime_) { - return false; - } - const auto submission_epoch = bus_runtime_->connectionEpoch(); - const auto submitted = - bus_runtime_->submitAxisCommand( - node_id, command, false, submission_epoch); - const auto completed_epoch = bus_runtime_->connectionEpoch(); - if (!submitted && completed_epoch == submission_epoch) { - return false; - } - { - std::lock_guard lock(nodes_mutex_); - auto& stored = nodeStateLocked_(node_id); - // Bind on every cross-epoch outcome, including an ambiguous failed - // submit, so feedback remains invalid until safety cleanup. - stored.profile_feedback_bound = true; - stored.profile_connection_epoch = submission_epoch; - } - return submitted && completed_epoch == submission_epoch; -} - -bool CmvrPlcMotorProtocol::commandProfileVelocity( - const std::uint8_t node_id, - const double target_qd, - const double max_qdd) -{ - NodeState state; - { - std::lock_guard lock(nodes_mutex_); - auto& stored = nodeStateLocked_(node_id); - stored.requested_mode = msgs::RUN_MODE_PROFILE_VELOCITY; - stored.cyclic_stream_open = false; - state = stored; - } - if (!validateVelocity_(state, target_qd) || - !validateAcceleration_(state, max_qdd)) { - CMVR_LOG(ERROR) << "[CmvrPlcMotorProtocol] invalid profile-velocity command"; - return false; - } - const auto velocity = toMicroUnits_(target_qd); - const auto acceleration = toMicroUnits_(std::abs(max_qdd)); - if (!velocity || !acceleration) { - return false; - } - - CmvrPlcAxisCommand command; - command.code = cmvr_plc::CommandCode::ProfileVelocity; - command.target_velocity = *velocity; - command.acceleration = *acceleration; - command.command_timeout_ms = kProfileCommandTimeoutCeilingMs; - if (!bus_runtime_) { - return false; - } - const auto submission_epoch = bus_runtime_->connectionEpoch(); - const auto submitted = - bus_runtime_->submitAxisCommand( - node_id, command, false, submission_epoch); - const auto completed_epoch = bus_runtime_->connectionEpoch(); - if (!submitted && completed_epoch == submission_epoch) { - return false; - } - { - std::lock_guard lock(nodes_mutex_); - auto& stored = nodeStateLocked_(node_id); - stored.profile_feedback_bound = true; - stored.profile_connection_epoch = submission_epoch; - } - return submitted && completed_epoch == submission_epoch; -} - -bool CmvrPlcMotorProtocol::commandCyclicPosition( - const std::uint8_t node_id, - const double target_q, - const double target_qd) -{ - std::lock_guard lock(nodes_mutex_); - auto& state = nodeStateLocked_(node_id); - if (!validatePosition_(state, target_q) || - !validateVelocity_(state, target_qd) || - !ensureCyclicOpen_(node_id, state, msgs::RUN_MODE_CYCLIC_SYNC_POSITION)) { - return false; - } - const auto position = toMicroUnits_(target_q); - const auto velocity = toMicroUnits_(target_qd); - if (!position || !velocity) { - return false; - } - - CmvrPlcAxisCommand command; - command.code = cmvr_plc::CommandCode::CyclicPositionSample; - command.target_position = *position; - command.target_velocity = *velocity; - command.stream_watchdog_ms = bus_runtime_->streamWatchdogMs(); - if (++state.cyclic_sequence == 0) { - ++state.cyclic_sequence; - } - command.cyclic_sample_sequence = state.cyclic_sequence; - const auto submitted = - bus_runtime_->submitAxisCommand( - node_id, command, false, state.cyclic_connection_epoch); - if (submitted) { - state.profile_feedback_bound = false; - state.profile_connection_epoch = 0; - } else if ( - bus_runtime_->connectionEpoch() != state.cyclic_connection_epoch) { - state.cyclic_reconnect_latched = true; - state.cyclic_stream_open = false; - state.cyclic_sequence = 0; - state.cyclic_connection_epoch = bus_runtime_->connectionEpoch(); - } - return submitted; -} - -bool CmvrPlcMotorProtocol::commandCyclicVelocity( - const std::uint8_t node_id, - const double target_qd) -{ - std::lock_guard lock(nodes_mutex_); - auto& state = nodeStateLocked_(node_id); - if (!validateVelocity_(state, target_qd) || - !ensureCyclicOpen_(node_id, state, msgs::RUN_MODE_CYCLIC_SYNC_VELOCITY)) { - return false; - } - const auto velocity = toMicroUnits_(target_qd); - if (!velocity) { - return false; - } - - CmvrPlcAxisCommand command; - command.code = cmvr_plc::CommandCode::CyclicVelocitySample; - command.target_velocity = *velocity; - command.stream_watchdog_ms = bus_runtime_->streamWatchdogMs(); - if (++state.cyclic_sequence == 0) { - ++state.cyclic_sequence; - } - command.cyclic_sample_sequence = state.cyclic_sequence; - const auto submitted = - bus_runtime_->submitAxisCommand( - node_id, command, false, state.cyclic_connection_epoch); - if (submitted) { - state.profile_feedback_bound = false; - state.profile_connection_epoch = 0; - } else if ( - bus_runtime_->connectionEpoch() != state.cyclic_connection_epoch) { - state.cyclic_reconnect_latched = true; - state.cyclic_stream_open = false; - state.cyclic_sequence = 0; - state.cyclic_connection_epoch = bus_runtime_->connectionEpoch(); - } - return submitted; -} - -bool CmvrPlcMotorProtocol::commandCyclicTorque( - const std::uint8_t node_id, - const double target_tau) -{ - (void)target_tau; - CMVR_LOG(ERROR) << "[CmvrPlcMotorProtocol] cyclic torque is unsupported, motor=" - << static_cast(node_id); - return false; -} - -void CmvrPlcMotorProtocol::setMotorConversion( - const std::uint8_t node_id, - const double encoder_counts_per_rev, - const double gear_ratio) -{ - (void)node_id; - (void)encoder_counts_per_rev; - (void)gear_ratio; - // CMVR PLC v1 exchanges SI quantities in fixed-point micro-units. -} - -bool CmvrPlcMotorProtocol::torqueOn(const std::uint8_t node_id) -{ - return submitSimple_(node_id, cmvr_plc::CommandCode::Enable, true, - kSafetyCommandTimeoutMs); -} - -bool CmvrPlcMotorProtocol::torqueOff(const std::uint8_t node_id) -{ - CmvrPlcAxisCommand command; - command.code = cmvr_plc::CommandCode::Disable; - command.command_timeout_ms = kSafetyCommandTimeoutMs; - const auto result = - bus_runtime_ && bus_runtime_->submitAxisSafetyCommand(node_id, command); - std::lock_guard lock(nodes_mutex_); - auto& state = nodeStateLocked_(node_id); - state.cyclic_stream_open = false; - if (result) { - state.profile_feedback_bound = false; - state.profile_connection_epoch = 0; - } - return result; -} - -bool CmvrPlcMotorProtocol::brakeRelease(const std::uint8_t node_id) -{ - CMVR_LOG(ERROR) << "[CmvrPlcMotorProtocol] brake release is unsupported, motor=" - << static_cast(node_id); - return false; -} - -bool CmvrPlcMotorProtocol::quickStop(const std::uint8_t node_id) -{ - CmvrPlcAxisCommand command; - command.code = cmvr_plc::CommandCode::QuickStop; - command.command_timeout_ms = kSafetyCommandTimeoutMs; - const auto result = - bus_runtime_ && bus_runtime_->submitAxisSafetyCommand(node_id, command); - std::lock_guard lock(nodes_mutex_); - auto& state = nodeStateLocked_(node_id); - state.cyclic_stream_open = false; - if (result) { - state.profile_feedback_bound = false; - state.profile_connection_epoch = 0; - } - return result; -} - -double CmvrPlcMotorProtocol::getQ(const std::uint8_t node_id) -{ - CmvrPlcAxisStatus status; - if (!readMotionStatus_(node_id, status) || - !statusAllowsMotionFeedback(status)) { - return std::numeric_limits::quiet_NaN(); - } - return fromMicroUnits_(status.actual_position); -} - -double CmvrPlcMotorProtocol::getQd(const std::uint8_t node_id) -{ - CmvrPlcAxisStatus status; - if (!readMotionStatus_(node_id, status) || - !statusAllowsMotionFeedback(status)) { - return std::numeric_limits::quiet_NaN(); - } - return fromMicroUnits_(status.actual_velocity); -} - -bool CmvrPlcMotorProtocol::readMotionStatus_( - const std::uint8_t node_id, - CmvrPlcAxisStatus& status) -{ - if (!bus_runtime_) { - return false; - } - std::lock_guard lock(nodes_mutex_); - const auto& state = nodeStateLocked_(node_id); - const auto read_epoch = bus_runtime_->connectionEpoch(); - if (state.profile_feedback_bound && - state.profile_connection_epoch != read_epoch) { - return false; - } - if (!bus_runtime_->readAxisStatus(node_id, status)) { - return false; - } - // Reject a status transaction that straddled a disconnect/reconnect even - // when no profile binding was active at the first check. - if (bus_runtime_->connectionEpoch() != read_epoch) { - return false; - } - return !state.profile_feedback_bound || - state.profile_connection_epoch == read_epoch; -} - -bool CmvrPlcMotorProtocol::submitSimple_( - const std::uint8_t node_id, - const cmvr_plc::CommandCode code, - const bool wait_for_terminal, - const std::uint32_t timeout_ms) -{ - CmvrPlcAxisCommand command; - command.code = code; - command.command_timeout_ms = timeout_ms; - return bus_runtime_ && - bus_runtime_->submitAxisCommand(node_id, command, wait_for_terminal); -} - -bool CmvrPlcMotorProtocol::ensureCyclicOpen_( - const std::uint8_t node_id, - NodeState& state, - const msgs::RunMode mode) -{ - if (!bus_runtime_) { - return false; - } - const auto connection_epoch = bus_runtime_->connectionEpoch(); - if (state.cyclic_connection_epoch != connection_epoch) { - // A cyclic generation is bound to the PLC ownership epoch in which it - // was created. Never reinterpret a setpoint from that generation as - // the first setpoint of a freshly reconnected PLC stream. - if (state.cyclic_generation != 0U) { - state.cyclic_reconnect_latched = true; - } - state.cyclic_connection_epoch = connection_epoch; - state.cyclic_stream_open = false; - state.cyclic_sequence = 0; - } - if (state.cyclic_reconnect_latched) { - CMVR_LOG(WARNING) - << "[CmvrPlcMotorProtocol] cyclic stream crossed a PLC session; " - "explicit setMode from a new upper-layer stream is required, motor=" - << static_cast(node_id); - return false; - } - if (state.cyclic_stream_open && state.requested_mode == mode) { - return true; - } - - // Preserve direct protocol use that does not call setMode explicitly: - // its first successful Open still establishes a generation. Once that - // generation has observed a reconnect, only explicit setMode can recover. - if (state.cyclic_generation == 0U) { - state.cyclic_generation = 1U; - state.cyclic_connection_epoch = connection_epoch; - } - - CmvrPlcAxisCommand command; - command.code = mode == msgs::RUN_MODE_CYCLIC_SYNC_POSITION - ? cmvr_plc::CommandCode::OpenCyclicPosition - : cmvr_plc::CommandCode::OpenCyclicVelocity; - command.stream_watchdog_ms = bus_runtime_->streamWatchdogMs(); - command.command_timeout_ms = kSafetyCommandTimeoutMs; - if (!bus_runtime_->submitAxisCommand( - node_id, command, false, state.cyclic_connection_epoch)) { - if (bus_runtime_->connectionEpoch() != - state.cyclic_connection_epoch) { - state.cyclic_reconnect_latched = true; - state.cyclic_stream_open = false; - state.cyclic_sequence = 0; - state.cyclic_connection_epoch = - bus_runtime_->connectionEpoch(); - } - return false; - } - state.requested_mode = mode; - state.cyclic_stream_open = true; - state.cyclic_sequence = 0; - return true; -} - -std::optional CmvrPlcMotorProtocol::toMicroUnits_(const double value) -{ - if (!std::isfinite(value)) { - return std::nullopt; - } - const auto scaled = std::round(value * cmvr_plc::kPositionScale); - if (scaled < static_cast(std::numeric_limits::min()) || - scaled > static_cast(std::numeric_limits::max())) { - return std::nullopt; - } - return static_cast(scaled); -} - -double CmvrPlcMotorProtocol::fromMicroUnits_(const std::int32_t value) -{ - return static_cast(value) / cmvr_plc::kPositionScale; -} - -bool CmvrPlcMotorProtocol::isSupportedMode_(const msgs::RunMode mode) -{ - return mode == msgs::RUN_MODE_PROFILE_POSITION || - mode == msgs::RUN_MODE_PROFILE_VELOCITY || - mode == msgs::RUN_MODE_CYCLIC_SYNC_POSITION || - mode == msgs::RUN_MODE_CYCLIC_SYNC_VELOCITY; -} - -bool CmvrPlcMotorProtocol::validatePosition_( - const NodeState& state, - const double position) const -{ - return std::isfinite(position) && - (state.limit_q_ub <= state.limit_q_lb || - (position >= state.limit_q_lb && position <= state.limit_q_ub)); -} - -bool CmvrPlcMotorProtocol::validateVelocity_( - const NodeState& state, - const double velocity) const -{ - return std::isfinite(velocity) && - (state.limit_qd <= 0.0 || std::abs(velocity) <= state.limit_qd); -} - -bool CmvrPlcMotorProtocol::validateAcceleration_( - const NodeState& state, - const double acceleration) const -{ - return std::isfinite(acceleration) && acceleration >= 0.0 && - (state.limit_qdd <= 0.0 || acceleration <= state.limit_qdd); -} - -CmvrPlcMotorProtocol::NodeState& CmvrPlcMotorProtocol::nodeStateLocked_( - const std::uint8_t node_id) -{ - return nodes_[node_id]; -} - -} // namespace cmvr::device diff --git a/cmvr-es/devices/motor/drivers/modbus_plc_motor/src/modbus_plc_motor.cpp b/cmvr-es/devices/motor/drivers/modbus_plc_motor/src/modbus_plc_motor.cpp deleted file mode 100644 index faa9952a..00000000 --- a/cmvr-es/devices/motor/drivers/modbus_plc_motor/src/modbus_plc_motor.cpp +++ /dev/null @@ -1,55 +0,0 @@ -#include "devices/motor/drivers/modbus_plc_motor/include/modbus_plc_motor.h" - -#include "common/base/logging/logger.h" -#include "devices/motor/drivers/modbus_plc_motor/include/cmvr_plc_motor_protocol.h" -#include - -namespace cmvr::device { - -ModbusPlcMotor::ModbusPlcMotor(const config::MotorConfigItem& config) -{ - info_.id = config.id(); - info_.joint_name = config.joint_name(); - info_.limit_q_lb = config.limit_q_lb(); - info_.limit_q_ub = config.limit_q_ub(); - info_.limit_qd = config.limit_qd(); - info_.limit_qdd = config.limit_qdd(); - node_id_ = static_cast(config.id()); -} - -bool ModbusPlcMotor::init() -{ - if (!protocol_ || - !std::dynamic_pointer_cast(protocol_)) { - CMVR_LOG(ERROR) << "[ModbusPlcMotor] invalid CMVR PLC protocol for " - << info_.joint_name; - return false; - } - if (!std::isfinite(info_.limit_q_lb) || !std::isfinite(info_.limit_q_ub) || - !std::isfinite(info_.limit_qd) || !std::isfinite(info_.limit_qdd) || - info_.limit_q_ub <= info_.limit_q_lb || - info_.limit_qd <= 0.0 || info_.limit_qdd <= 0.0) { - CMVR_LOG(ERROR) << "[ModbusPlcMotor] finite position/velocity/acceleration " - "limits are required for " - << info_.joint_name; - return false; - } - setLimitQ(info_.limit_q_ub, info_.limit_q_lb); - setLimitQd(info_.limit_qd); - setLimitQdd(info_.limit_qdd, -info_.limit_qdd); - return true; -} - -bool ModbusPlcMotor::torqueOff() -{ - auto protocol = std::dynamic_pointer_cast(protocol_); - return protocol && protocol->torqueOff(node_id_); -} - -bool ModbusPlcMotor::quickStop() -{ - auto protocol = std::dynamic_pointer_cast(protocol_); - return protocol && protocol->quickStop(node_id_); -} - -} // namespace cmvr::device diff --git a/cmvr-es/devices/motor/manager/CMakeLists.txt b/cmvr-es/devices/motor/manager/CMakeLists.txt index 46f66040..8ea5f495 100644 --- a/cmvr-es/devices/motor/manager/CMakeLists.txt +++ b/cmvr-es/devices/motor/manager/CMakeLists.txt @@ -13,7 +13,6 @@ target_link_libraries(motor_manager cmvr_es::device::ti5_canopen_motor_driver cmvr_es::device::mujoco_motor_driver cmvr_es::device::ethercat_motor_driver - cmvr_es::device::modbus_plc_motor_driver cmvr_es::ik_solver glog ) diff --git a/cmvr-es/devices/motor/manager/include/motor_manager.h b/cmvr-es/devices/motor/manager/include/motor_manager.h index e67a263e..3bbd2cab 100644 --- a/cmvr-es/devices/motor/manager/include/motor_manager.h +++ b/cmvr-es/devices/motor/manager/include/motor_manager.h @@ -78,10 +78,6 @@ private: const config::MotorGroupConfig& group_cfg, const std::vector& motor_cfgs, const std::shared_ptr& bus_runtime) const; - std::vector> createModbusTcpMotors_( - const config::MotorGroupConfig& group_cfg, - const std::vector& motor_cfgs, - const std::shared_ptr& bus_runtime) const; private: config::MotorConfig cfg_; diff --git a/cmvr-es/devices/motor/manager/src/motor_manager.cpp b/cmvr-es/devices/motor/manager/src/motor_manager.cpp index ff2f37ff..5e83b043 100644 --- a/cmvr-es/devices/motor/manager/src/motor_manager.cpp +++ b/cmvr-es/devices/motor/manager/src/motor_manager.cpp @@ -15,14 +15,11 @@ #include "devices/motor/bus_runtime/can/include/can_motor_bus_runtime.h" #include "devices/motor/bus_runtime/ethercat/include/ethercat_motor_bus_runtime.h" #include "devices/motor/bus_runtime/mujoco/include/mujoco_motor_bus_runtime.h" -#include "devices/motor/bus_runtime/modbus_tcp/include/modbus_tcp_motor_bus_runtime.h" #include "devices/motor/drivers/ethercat_motor/include/cia402/cia402_protocol.h" #include "devices/motor/drivers/ethercat_motor/include/vendor/eyou/eyou_cia402_pdo_mapping.h" #include "devices/motor/drivers/ethercat_motor/include/vendor/eyou/eyou_motor.h" #include "devices/motor/drivers/ethercat_motor/include/vendor/eyou/eyou_motor_adapter.h" #include "devices/motor/drivers/mujoco/include/mujoco_motor.h" -#include "devices/motor/drivers/modbus_plc_motor/include/cmvr_plc_motor_protocol.h" -#include "devices/motor/drivers/modbus_plc_motor/include/modbus_plc_motor.h" #include "devices/motor/drivers/ti5_canopen/include/ti5_motor.h" #include "devices/motor/drivers/ti5_canopen/include/ti5_motor_canopen_protocol.h" @@ -418,16 +415,6 @@ std::shared_ptr MotorManager::createBusRuntime_( return std::make_shared(); case config::MOTOR_BUS_MUJOCO: return std::make_shared(); - case config::MOTOR_BUS_MODBUS_TCP: - if (group_cfg.vendor() == config::MOTOR_VENDOR_PLC_GENERIC && - group_cfg.protocol() == config::MOTOR_PROTOCOL_CMVR_PLC_V1) { - return std::make_shared(); - } - CMVR_LOG(ERROR) << "[MotorManager] unsupported Modbus TCP motor: vendor=" - << config::MotorVendor_Name(group_cfg.vendor()) - << ", protocol=" << config::MotorProtocol_Name(group_cfg.protocol()) - << ", group=" << group_cfg.id(); - return nullptr; case config::MOTOR_BUS_ETHERCAT: { auto runtime = std::make_shared(); if (group_cfg.vendor() == config::MOTOR_VENDOR_EYOU && @@ -467,8 +454,6 @@ std::vector> MotorManager::createMotors_( return createMujocoMotors_(group_cfg, motor_cfgs, bus_runtime); case config::MOTOR_BUS_ETHERCAT: return createEthercatMotors_(group_cfg, motor_cfgs, bus_runtime); - case config::MOTOR_BUS_MODBUS_TCP: - return createModbusTcpMotors_(group_cfg, motor_cfgs, bus_runtime); default: CMVR_LOG(ERROR) << "[MotorManager] unsupported motor bus type: " << config::MotorBusType_Name(group_cfg.bus_type()) @@ -611,50 +596,4 @@ std::vector> MotorManager::createEthercatMotors_( return motors; } -std::vector> MotorManager::createModbusTcpMotors_( - const config::MotorGroupConfig& group_cfg, - const std::vector& motor_cfgs, - const std::shared_ptr& bus_runtime) const -{ - auto modbus_runtime = - std::dynamic_pointer_cast(bus_runtime); - if (!modbus_runtime || !group_cfg.has_modbus_tcp()) { - CMVR_LOG(ERROR) << "[MotorManager] missing Modbus TCP runtime/config: " - << group_cfg.id(); - return {}; - } - if (group_cfg.vendor() != config::MOTOR_VENDOR_PLC_GENERIC || - group_cfg.protocol() != config::MOTOR_PROTOCOL_CMVR_PLC_V1) { - CMVR_LOG(ERROR) << "[MotorManager] unsupported Modbus TCP PLC motor: vendor=" - << config::MotorVendor_Name(group_cfg.vendor()) - << ", protocol=" << config::MotorProtocol_Name(group_cfg.protocol()); - return {}; - } - for (const auto& motor_cfg : motor_cfgs) { - if (motor_cfg.id() < 0 || motor_cfg.id() > 255 || - !modbus_runtime->hasMotor(static_cast(motor_cfg.id()))) { - CMVR_LOG(ERROR) << "[MotorManager] missing PLC axis mapping for motor id " - << motor_cfg.id() << " in group: " << group_cfg.id(); - return {}; - } - } - - auto protocol = std::make_shared(modbus_runtime); - std::vector> motors; - motors.reserve(motor_cfgs.size()); - for (const auto& cfg : motor_cfgs) { - auto motor = std::make_shared(cfg); - motor->setProtocol(protocol); - // initNode() and ModbusPlcMotor::init() only establish local mappings and - // limits. The shared TCP connection starts after all motors are created. - if (!motor->init()) { - CMVR_LOG(ERROR) << "[MotorManager] failed to init Modbus PLC motor: " - << cfg.joint_name(); - return {}; - } - motors.push_back(std::move(motor)); - } - return motors; -} - } // namespace cmvr::device diff --git a/cmvr-es/service/CMakeLists.txt b/cmvr-es/service/CMakeLists.txt index 9f54e4df..f4a39cf3 100644 --- a/cmvr-es/service/CMakeLists.txt +++ b/cmvr-es/service/CMakeLists.txt @@ -221,44 +221,6 @@ if(BUILD_TESTING) ENVIRONMENT "${_grpc_agv_test_environment}" ) - set(_grpc_motor_modbus_e2e_libmodbus_root - "${CMAKE_SOURCE_DIR}/dependency/${ARCH}/third_party/modbus/3.1.11") - add_executable(grpc_motor_service_modbus_e2e_test - grpc/tests/grpc_motor_service_modbus_e2e_test.cpp - ) - target_include_directories(grpc_motor_service_modbus_e2e_test - PRIVATE - ${CMAKE_SOURCE_DIR}/cmvr-es - ${CMAKE_SOURCE_DIR}/cmvr-es/manager/device_manager - ${_grpc_motor_modbus_e2e_libmodbus_root}/include - ) - target_link_directories(grpc_motor_service_modbus_e2e_test - PRIVATE - ${_grpc_motor_modbus_e2e_libmodbus_root}/lib - ) - target_link_libraries(grpc_motor_service_modbus_e2e_test - PRIVATE - service - modbus - gtest - gtest_main - pthread - ) - add_test( - NAME grpc_motor_service_modbus_e2e_test - COMMAND grpc_motor_service_modbus_e2e_test - ) - set(_grpc_motor_modbus_e2e_environment - "LD_LIBRARY_PATH=${CMVR_TEST_EXTERNAL_LIBRARY_PATH}" - ) - if(CMVR_TEST_SYSTEM_LIBSTDCXX) - list(APPEND _grpc_motor_modbus_e2e_environment - "LD_PRELOAD=${CMVR_TEST_SYSTEM_LIBSTDCXX}") - endif() - set_tests_properties(grpc_motor_service_modbus_e2e_test PROPERTIES - TIMEOUT 20 - ENVIRONMENT "${_grpc_motor_modbus_e2e_environment}" - ) endif() # -------------------------------------------------------- diff --git a/cmvr-es/service/README.md b/cmvr-es/service/README.md index 49e053cc..2369d49e 100644 --- a/cmvr-es/service/README.md +++ b/cmvr-es/service/README.md @@ -42,7 +42,7 @@ grpcurl -plaintext \ ## MotorService `MotorService` 将 gRPC 电机命令适配到已经由 `DeviceManager` 创建的 -`MotorManager` 和 `AbstractMotor`，不直接持有 PLC、现场总线或厂商驱动。 +`MotorManager` 和 `AbstractMotor`，不直接持有现场总线或厂商驱动。 关键文件： @@ -52,8 +52,6 @@ grpcurl -plaintext \ 和 [`grpc/src/grpc_motor_service.cpp`](grpc/src/grpc_motor_service.cpp) - 注册：[`../task/grpc_server_task/src/grpc_server_task.cpp`](../task/grpc_server_task/src/grpc_server_task.cpp) - 单元测试：[`grpc/tests/grpc_motor_service_test.cpp`](grpc/tests/grpc_motor_service_test.cpp) -- gRPC–Modbus 端到端测试： - [`grpc/tests/grpc_motor_service_modbus_e2e_test.cpp`](grpc/tests/grpc_motor_service_modbus_e2e_test.cpp) 服务按单电机仲裁。同步 Profile 命令、Cyclic Position/Velocity 双向流、 `setEnabled`、状态读取和软件 `emergencyStop` 共用同一控制权状态： @@ -66,11 +64,6 @@ grpcurl -plaintext \ - 只有成功执行 `setEnabled(true)` 才解除服务内软件急停锁存； - 服务层 Quick Stop 和 `emergencyStop` 都不具备功能安全等级。 -PLC 后端的连接 epoch、stream epoch、寄存器、ACK 和 TIA Portal 要求见： - -- [Modbus TCP PLC runtime](../devices/motor/bus_runtime/modbus_tcp/README.md) -- [MotorService 与 CMVR PLC v1 完整协议](../../docs/motor_service_modbus_tcp.md) - AUBO 控制柜 IO 不经过 `MotorService`，由 `ArmService/ExecuteJsonCommand` 转发到目标 `RobotArm`。厂商命令和安全约束见 [AUBO 控制柜 IO](../devices/arm/aubo_arm/README.md)。 diff --git a/cmvr-es/service/grpc/src/grpc_motor_service.cpp b/cmvr-es/service/grpc/src/grpc_motor_service.cpp index 1e2f01f4..a08e67de 100644 --- a/cmvr-es/service/grpc/src/grpc_motor_service.cpp +++ b/cmvr-es/service/grpc/src/grpc_motor_service.cpp @@ -966,7 +966,7 @@ grpc::Status gRPCMotorServiceImpl::setZeroImpl( } if (!calibrated) { const std::string error = - "zero calibration outcome unknown; inspect PLC zero_epoch/session before retry"; + "zero calibration outcome unknown; inspect device state before retry"; setLastError(resolved.control, error); lease.reset(); fillFeedback(response->mutable_header(), false, error); @@ -1984,7 +1984,7 @@ grpc::Status gRPCMotorServiceImpl::setEnabledImpl( } } - // A false acknowledgement may still mean the PLC committed the + // A false acknowledgement may still mean the backend committed the // request. A successful enable also needs rollback if cancellation or // E-stop won while torqueOn was in flight. const bool cleanup_required = diff --git a/cmvr-es/service/grpc/tests/grpc_motor_service_modbus_e2e_test.cpp b/cmvr-es/service/grpc/tests/grpc_motor_service_modbus_e2e_test.cpp deleted file mode 100644 index 142608d6..00000000 --- a/cmvr-es/service/grpc/tests/grpc_motor_service_modbus_e2e_test.cpp +++ /dev/null @@ -1,907 +0,0 @@ -#include "service/grpc/include/grpc_motor_service.h" - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include - -#include "cmvr/config/device_manager_config/device_manager_config.pb.h" -#include "cmvr/config/motor_config/motor_config.pb.h" -#include "common/config/config_files.h" -#include "devices/motor/bus_runtime/modbus_tcp/include/cmvr_plc_register_map.h" -#include "devices/motor/manager/include/motor_manager.h" -#include "manager/device_manager/include/device_manager.h" - -namespace cmvr::service { -namespace { - -using namespace std::chrono_literals; -namespace plc = device::cmvr_plc; - -constexpr char kManagerId[] = "e2e_plc_motors"; -constexpr char kMotorGroupId[] = "e2e_plc_axis_group"; -constexpr char kJointName[] = "E2E_PLC_AXIS"; - -std::uint16_t reserveLoopbackPort() -{ - const int socket_fd = ::socket(AF_INET, SOCK_STREAM, 0); - if (socket_fd < 0) { - return 0; - } - - sockaddr_in address{}; - address.sin_family = AF_INET; - address.sin_addr.s_addr = htonl(INADDR_LOOPBACK); - address.sin_port = 0; - if (::bind(socket_fd, reinterpret_cast(&address), - sizeof(address)) != 0) { - ::close(socket_fd); - return 0; - } - - socklen_t length = sizeof(address); - if (::getsockname(socket_fd, reinterpret_cast(&address), - &length) != 0) { - ::close(socket_fd); - return 0; - } - const auto port = ntohs(address.sin_port); - ::close(socket_fd); - return port; -} - -template -bool waitUntil(Predicate&& predicate, const std::chrono::milliseconds timeout) -{ - const auto deadline = std::chrono::steady_clock::now() + timeout; - do { - if (predicate()) { - return true; - } - std::this_thread::sleep_for(5ms); - } while (std::chrono::steady_clock::now() < deadline); - return predicate(); -} - -class FakeCmvrPlc final { -public: - ~FakeCmvrPlc() { stop(); } - - bool start() - { - port_ = reserveLoopbackPort(); - if (port_ == 0) { - std::fprintf(stderr, "reserveLoopbackPort failed: %s\n", - std::strerror(errno)); - return false; - } - - context_ = modbus_new_tcp("127.0.0.1", port_); - mapping_ = modbus_mapping_new( - 1, 1, plc::kAxisFirstOffset + plc::kAxisRegisterStride, 1); - if (!context_ || !mapping_) { - std::fprintf(stderr, "fake PLC allocation failed: %s\n", - modbus_strerror(errno)); - stop(); - return false; - } - - auto* registers = mapping_->tab_registers; - registers[plc::kMagicCmOffset] = plc::kMagicCm; - registers[plc::kMagicVrOffset] = plc::kMagicVr; - registers[plc::kProtocolMajorOffset] = plc::kProtocolMajor; - registers[plc::kProtocolMinorOffset] = plc::kProtocolMinor; - registers[plc::kAxisCountOffset] = 1; - registers[plc::kOwnerStateOffset] = - static_cast(plc::OwnerState::None); - plc::encodeUint32(registers, plc::kPlcBootIdOffset, 0x45453245U); - plc::encodeUint32(registers, plc::kPlcHeartbeatOffset, - plc_heartbeat_); - - const auto status_base = - plc::axisBase(0) + plc::kAxisStatusRelativeOffset; - registers[status_base + plc::kCommandState] = - static_cast(plc::CommandState::Idle); - registers[status_base + plc::kResultCode] = - static_cast(plc::ResultCode::Ok); - publishSnapshot_(registers + status_base); - - listen_socket_.store(modbus_tcp_listen(context_, 2)); - if (listen_socket_.load() < 0) { - std::fprintf(stderr, "modbus_tcp_listen failed on %u: %s\n", - port_, modbus_strerror(errno)); - stop(); - return false; - } - - running_.store(true); - worker_ = std::thread(&FakeCmvrPlc::loop_, this); - return true; - } - - void stop() - { - running_.store(false); - const auto client = client_socket_.exchange(-1); - if (client >= 0) { - ::shutdown(client, SHUT_RDWR); - ::close(client); - } - const auto listener = listen_socket_.exchange(-1); - if (listener >= 0) { - ::shutdown(listener, SHUT_RDWR); - ::close(listener); - } - if (worker_.joinable()) { - worker_.join(); - } - if (mapping_) { - modbus_mapping_free(mapping_); - mapping_ = nullptr; - } - if (context_) { - modbus_free(context_); - context_ = nullptr; - } - } - - void disconnectClient() - { - const auto client = client_socket_.load(); - if (client >= 0) { - ::shutdown(client, SHUT_RDWR); - } - } - - std::uint16_t port() const { return port_; } - std::uint32_t commandCount() const { return command_count_.load(); } - std::uint32_t acceptedSessionCount() const - { - return accepted_session_count_.load(); - } - std::uint32_t enableCount() const { return enable_count_.load(); } - std::uint32_t profilePositionCount() const - { - return profile_position_count_.load(); - } - std::uint32_t profileVelocityCount() const - { - return profile_velocity_count_.load(); - } - std::uint32_t cyclicOpenCount() const - { - return cyclic_open_count_.load(); - } - std::uint32_t cyclicSampleCount() const - { - return cyclic_sample_count_.load(); - } - std::uint32_t quickStopCount() const - { - return quick_stop_count_.load(); - } - -private: - void loop_() - { - std::array request{}; - while (running_.load()) { - int listener = listen_socket_.load(); - const int accepted = - listener < 0 ? -1 : modbus_tcp_accept(context_, &listener); - if (accepted < 0) { - if (running_.load()) { - std::this_thread::sleep_for(5ms); - } - continue; - } - - client_socket_.store(accepted); - while (running_.load()) { - const auto request_length = - modbus_receive(context_, request.data()); - if (request_length <= 0) { - break; - } - if (modbus_reply(context_, request.data(), request_length, - mapping_) < 0) { - break; - } - // Treat the mapping as the PLC's published process image. - // Read-only FC3 requests must not themselves advance the - // seqlock; otherwise the runtime's guard/full/guard snapshot - // validation could never observe one stable scan. - if (request_length > 7 && request[7] == 0x10U) { - processMailbox_(); - } - } - - const auto client = client_socket_.exchange(-1); - if (client >= 0) { - ::close(client); - } - } - } - - void processMailbox_() - { - auto* registers = mapping_->tab_registers; - const auto base = plc::axisBase(0); - const auto status_base = base + plc::kAxisStatusRelativeOffset; - beginSnapshot_(registers + status_base); - - plc::encodeUint32(registers, plc::kPlcHeartbeatOffset, - ++plc_heartbeat_); - const auto session = - plc::decodeUint32(registers, plc::kCmvrSessionIdOffset); - if (session != observed_session_) { - // A new session is not observable as Accepted until the old - // stream/mailbox state has been made safe. - registers[plc::kOwnerStateOffset] = - static_cast(plc::OwnerState::Accepting); - observed_session_ = session; - active_session_ = session; - last_sequence_ = 0; - registers[status_base + plc::kStatusFlags] &= - static_cast(~plc::StatusFlag::StreamActive); - plc::encodeUint32(registers + base, plc::kPayloadSequence, 0); - plc::encodeUint32(registers + base, - plc::kPayloadSequenceMirror, 0); - plc::encodeUint32(registers + base, - plc::kCommitSequenceRelativeOffset, 0); - plc::encodeUint32(registers, plc::kOwnerSessionIdOffset, - active_session_); - registers[plc::kOwnerStateOffset] = - static_cast( - active_session_ == 0 ? plc::OwnerState::None - : plc::OwnerState::Accepted); - if (active_session_ != 0) { - accepted_session_count_.fetch_add(1); - } - } - - if (active_session_ == 0 || active_session_ != session) { - publishSnapshot_(registers + status_base); - return; - } - - const auto sequence = - plc::decodeUint32(registers + base, plc::kPayloadSequence); - const auto sequence_mirror = - plc::decodeUint32(registers + base, - plc::kPayloadSequenceMirror); - const auto commit = - plc::decodeUint32(registers + base, - plc::kCommitSequenceRelativeOffset); - const auto command_session = - plc::decodeUint32(registers + base, plc::kCommandSessionId); - if (sequence == 0 || sequence == last_sequence_ || - sequence != sequence_mirror || sequence != commit || - command_session != active_session_) { - publishSnapshot_(registers + status_base); - return; - } - - last_sequence_ = sequence; - command_count_.fetch_add(1); - plc::encodeUint32(registers + status_base, plc::kAckSequence, - sequence); - plc::encodeUint32(registers + status_base, plc::kActiveSequence, - sequence); - plc::encodeUint32(registers + status_base, plc::kAckSessionId, - active_session_); - registers[status_base + plc::kResultCode] = - static_cast(plc::ResultCode::Ok); - - const auto code = static_cast( - registers[base + plc::kCommandCode]); - const auto target_position = - plc::decodeInt32(registers + base, plc::kTargetPosition); - const auto target_velocity = - plc::decodeInt32(registers + base, plc::kTargetVelocity); - auto& flags = registers[status_base + plc::kStatusFlags]; - auto state = plc::CommandState::Completed; - - switch (code) { - case plc::CommandCode::ProfilePosition: - profile_position_count_.fetch_add(1); - registers[status_base + plc::kCurrentMode] = - msgs::RUN_MODE_PROFILE_POSITION; - plc::encodeInt32(registers + status_base, - plc::kActualPosition, target_position); - plc::encodeInt32(registers + status_base, - plc::kActualVelocity, 0); - plc::encodeInt32(registers + status_base, - plc::kTargetPositionStatus, target_position); - flags |= plc::StatusFlag::TargetReached; - flags &= static_cast( - ~plc::StatusFlag::QuickStopActive); - state = plc::CommandState::TargetReached; - break; - case plc::CommandCode::ProfileVelocity: - profile_velocity_count_.fetch_add(1); - registers[status_base + plc::kCurrentMode] = - msgs::RUN_MODE_PROFILE_VELOCITY; - plc::encodeInt32(registers + status_base, - plc::kActualVelocity, target_velocity); - plc::encodeInt32(registers + status_base, - plc::kTargetVelocityStatus, target_velocity); - flags &= static_cast( - ~plc::StatusFlag::QuickStopActive); - state = plc::CommandState::Accepted; - break; - case plc::CommandCode::OpenCyclicPosition: - cyclic_open_count_.fetch_add(1); - registers[status_base + plc::kCurrentMode] = - msgs::RUN_MODE_CYCLIC_SYNC_POSITION; - plc::encodeUint32( - registers + status_base, - plc::kLastAppliedCyclicSequence, 0); - flags |= plc::StatusFlag::StreamActive; - flags &= static_cast( - ~plc::StatusFlag::QuickStopActive); - state = plc::CommandState::Accepted; - break; - case plc::CommandCode::CyclicPositionSample: - cyclic_sample_count_.fetch_add(1); - plc::encodeInt32(registers + status_base, - plc::kActualPosition, target_position); - plc::encodeInt32(registers + status_base, - plc::kActualVelocity, target_velocity); - plc::encodeUint32( - registers + status_base, - plc::kLastAppliedCyclicSequence, - plc::decodeUint32(registers + base, - plc::kCyclicSampleSequence)); - state = plc::CommandState::Accepted; - break; - case plc::CommandCode::QuickStop: - quick_stop_count_.fetch_add(1); - plc::encodeInt32(registers + status_base, - plc::kActualVelocity, 0); - flags |= plc::StatusFlag::QuickStopActive; - flags &= static_cast( - ~plc::StatusFlag::StreamActive); - state = plc::CommandState::QuickStopped; - break; - case plc::CommandCode::Enable: - enable_count_.fetch_add(1); - flags |= plc::StatusFlag::Enabled; - flags &= static_cast( - ~plc::StatusFlag::QuickStopActive); - break; - case plc::CommandCode::Disable: - flags &= static_cast( - ~plc::StatusFlag::Enabled); - flags &= static_cast( - ~plc::StatusFlag::StreamActive); - break; - default: - registers[status_base + plc::kResultCode] = - static_cast( - plc::ResultCode::Unsupported); - state = plc::CommandState::Rejected; - break; - } - - registers[status_base + plc::kCommandState] = - static_cast(state); - publishSnapshot_(registers + status_base); - } - - void publishSnapshot_(std::uint16_t* status) - { - if (pending_state_sequence_ == 0U) { - beginSnapshot_(status); - } - plc::encodeUint32(status, plc::kHeartbeatAge, 0); - plc::encodeUint32(status, plc::kStateSequenceMirror, - pending_state_sequence_); - plc::encodeUint32(status, plc::kStateSequence, - pending_state_sequence_); - state_sequence_ = pending_state_sequence_; - pending_state_sequence_ = 0U; - } - - void beginSnapshot_(std::uint16_t* status) - { - auto next = state_sequence_ + 2U; - if (next == 0U) { - next = 2U; - } - pending_state_sequence_ = next; - plc::encodeUint32(status, plc::kStateSequence, next - 1U); - } - - modbus_t* context_{nullptr}; - modbus_mapping_t* mapping_{nullptr}; - std::thread worker_; - std::atomic running_{false}; - std::atomic listen_socket_{-1}; - std::atomic client_socket_{-1}; - std::atomic command_count_{0}; - std::atomic accepted_session_count_{0}; - std::atomic enable_count_{0}; - std::atomic profile_position_count_{0}; - std::atomic profile_velocity_count_{0}; - std::atomic cyclic_open_count_{0}; - std::atomic cyclic_sample_count_{0}; - std::atomic quick_stop_count_{0}; - std::uint32_t observed_session_{0}; - std::uint32_t active_session_{0}; - std::uint32_t plc_heartbeat_{1}; - std::uint32_t state_sequence_{0}; - std::uint32_t pending_state_sequence_{0}; - std::uint32_t last_sequence_{0}; - std::uint16_t port_{0}; -}; - -std::string writeTemporaryMotorConfig(const std::uint16_t port) -{ - char path[] = "/tmp/cmvr_motor_service_modbus_e2e_XXXXXX.pb.txt"; - const int descriptor = ::mkstemps(path, 7); - if (descriptor < 0) { - return {}; - } - ::close(descriptor); - - std::ofstream output(path, std::ios::out | std::ios::trunc); - if (!output.is_open()) { - std::remove(path); - return {}; - } - - output << "motor {\n" - << " id: \"" << kManagerId << "\"\n" - << " motor_groups {\n" - << " id: \"" << kMotorGroupId << "\"\n" - << " bus_type: MOTOR_BUS_MODBUS_TCP\n" - << " vendor: MOTOR_VENDOR_PLC_GENERIC\n" - << " protocol: MOTOR_PROTOCOL_CMVR_PLC_V1\n" - << " modbus_tcp {\n" - << " host: \"127.0.0.1\"\n" - << " port: " << port << "\n" - << " unit_id: 1\n" - << " connect_timeout_ms: 200\n" - << " io_timeout_ms: 50\n" - << " heartbeat_period_ms: 20\n" - << " communication_watchdog_ms: 300\n" - << " status_poll_period_ms: 2\n" - << " reconnect_min_ms: 10\n" - << " reconnect_max_ms: 50\n" - << " command_ack_timeout_ms: 300\n" - << " cyclic_watchdog_ms: 300\n" - << " protocol_major: 1\n" - << " protocol_minor: 0\n" - << " axes { motor_id: 1 axis_index: 0 }\n" - << " }\n" - << " joint_limits {\n" - << " enable: true\n" - << " source: JOINT_LIMIT_SOURCE_CUSTOM\n" - << " joints {\n" - << " joint_name: \"" << kJointName << "\"\n" - << " q_lb: -2.0\n" - << " q_ub: 2.0\n" - << " qd: 2.0\n" - << " qdd: 4.0\n" - << " }\n" - << " }\n" - << " motors {\n" - << " motors { id: 1 joint_name: \"" << kJointName << "\" }\n" - << " }\n" - << " }\n" - << "}\n"; - output.close(); - if (!output) { - std::remove(path); - return {}; - } - return path; -} - -class MotorServiceModbusE2eTest : public ::testing::Test { -protected: - void SetUp() override - { - device::DeviceManager::destroyInstance(); - ASSERT_TRUE(plc_.start()); - - config_path_ = writeTemporaryMotorConfig(plc_.port()); - ASSERT_FALSE(config_path_.empty()); - - config::MotorRootConfig parsed; - ASSERT_TRUE(ConfigHelper::loadConfigFileSilent(config_path_, parsed)); - ASSERT_EQ(parsed.motor().id(), kManagerId); - ASSERT_EQ(parsed.motor().motor_groups_size(), 1); - - config::DeviceManagerConfig device_config; - device_config.set_init_all_motors_when_no_active_joints(true); - auto* entry = device_config.add_devices(); - entry->set_id(kManagerId); - entry->set_type( - config::DeviceConfigEntry::DEVICE_TYPE_MOTOR_SYSTEM); - entry->set_config_file(config_path_); - entry->set_enable(true); - - auto& device_manager = - device::DeviceManager::getInstance(device_config); - manager_ = - device_manager.getDevice(kManagerId); - ASSERT_NE(manager_, nullptr); - ASSERT_NE(manager_->getMotor(1), nullptr); - ASSERT_EQ(manager_->getMotor(1)->jointName(), kJointName); - - service_ = std::make_unique(); - ASSERT_TRUE(startGrpcServer_()); - } - - void TearDown() override - { - if (grpc_server_) { - grpc_server_->Shutdown(); - grpc_server_->Wait(); - grpc_server_.reset(); - } - stub_.reset(); - service_.reset(); - - if (manager_) { - manager_->stop(); - manager_.reset(); - } - device::DeviceManager::destroyInstance(); - plc_.stop(); - - if (!grpc_socket_path_.empty()) { - std::remove(grpc_socket_path_.c_str()); - grpc_socket_path_.clear(); - } - if (!config_path_.empty()) { - std::remove(config_path_.c_str()); - config_path_.clear(); - } - } - - static api::MotorTarget makeTarget_() - { - api::MotorTarget target; - target.mutable_header()->set_device_id(kManagerId); - target.set_motor_id(1); - return target; - } - - bool startGrpcServer_() - { - grpc_socket_path_ = - "/tmp/cmvr_motor_service_modbus_e2e_" + - std::to_string(static_cast(::getpid())) + ".sock"; - std::remove(grpc_socket_path_.c_str()); - const std::string server_address = "unix:" + grpc_socket_path_; - - grpc::ServerBuilder builder; - builder.AddListeningPort( - server_address, grpc::InsecureServerCredentials()); - builder.RegisterService(service_.get()); - grpc_server_ = builder.BuildAndStart(); - if (!grpc_server_) { - return false; - } - stub_ = api::MotorService::NewStub( - grpc::CreateChannel( - server_address, - grpc::InsecureChannelCredentials())); - return stub_ != nullptr; - } - - FakeCmvrPlc plc_; - std::shared_ptr manager_; - std::unique_ptr service_; - std::unique_ptr grpc_server_; - std::unique_ptr stub_; - std::string config_path_; - std::string grpc_socket_path_; -}; - -TEST_F(MotorServiceModbusE2eTest, - StubTraversesManagerAndModbusRuntimeWithSafetyAndReconnect) -{ - api::SetMotorEnabledRequest enable_request; - *enable_request.mutable_target() = makeTarget_(); - enable_request.set_enabled(true); - grpc::ClientContext enable_context; - enable_context.set_deadline( - std::chrono::system_clock::now() + 3s); - api::MotorCommandResponse enable_response; - const auto enable_status = - stub_->setEnabled(&enable_context, enable_request, &enable_response); - ASSERT_TRUE(enable_status.ok()) << enable_status.error_message(); - ASSERT_TRUE(enable_response.header().success()) - << enable_response.header().error_message(); - EXPECT_EQ(plc_.enableCount(), 1U); - - api::ProfilePositionRequest position_request; - *position_request.mutable_target() = makeTarget_(); - position_request.set_target_position_rad(0.75); - position_request.set_max_velocity_rad_s(0.5); - position_request.set_acceleration_rad_s2(1.0); - position_request.mutable_wait()->set_timeout_ms(1000); - position_request.mutable_wait()->set_poll_period_ms(2); - position_request.mutable_wait()->set_settle_sample_count(1); - grpc::ClientContext position_context; - position_context.set_deadline( - std::chrono::system_clock::now() + 3s); - api::MotorCommandResponse position_response; - const auto position_status = stub_->profilePosition( - &position_context, position_request, &position_response); - ASSERT_TRUE(position_status.ok()) << position_status.error_message(); - ASSERT_TRUE(position_response.header().success()) - << position_response.header().error_message(); - EXPECT_NEAR(position_response.status().position_rad(), 0.75, 1e-6); - EXPECT_TRUE(position_response.status().target_reached()); - EXPECT_EQ(plc_.profilePositionCount(), 1U); - - api::GetMotorStatusRequest get_status_request; - *get_status_request.mutable_target() = makeTarget_(); - grpc::ClientContext get_status_context; - get_status_context.set_deadline( - std::chrono::system_clock::now() + 3s); - api::GetMotorStatusResponse get_status_response; - const auto get_status = stub_->getStatus( - &get_status_context, get_status_request, &get_status_response); - ASSERT_TRUE(get_status.ok()) << get_status.error_message(); - ASSERT_TRUE(get_status_response.header().success()) - << get_status_response.header().error_message(); - EXPECT_EQ(get_status_response.status().motor_id(), 1U); - EXPECT_EQ(get_status_response.status().joint_name(), kJointName); - EXPECT_EQ(get_status_response.status().run_mode(), - msgs::RUN_MODE_PROFILE_POSITION); - EXPECT_NEAR(get_status_response.status().position_rad(), 0.75, 1e-6); - - api::ProfileVelocityRequest velocity_request; - *velocity_request.mutable_target() = makeTarget_(); - velocity_request.set_target_velocity_rad_s(-0.2); - velocity_request.set_acceleration_rad_s2(0.5); - velocity_request.mutable_wait()->set_timeout_ms(1000); - velocity_request.mutable_wait()->set_poll_period_ms(2); - velocity_request.mutable_wait()->set_settle_sample_count(1); - grpc::ClientContext velocity_context; - velocity_context.set_deadline( - std::chrono::system_clock::now() + 3s); - api::MotorCommandResponse velocity_response; - const auto velocity_status = stub_->profileVelocity( - &velocity_context, velocity_request, &velocity_response); - ASSERT_TRUE(velocity_status.ok()) << velocity_status.error_message(); - ASSERT_TRUE(velocity_response.header().success()) - << velocity_response.header().error_message(); - EXPECT_NEAR(velocity_response.status().velocity_rad_s(), -0.2, 1e-6); - EXPECT_EQ(plc_.profileVelocityCount(), 1U); - - const auto quick_stops_before_stream = plc_.quickStopCount(); - grpc::ClientContext stream_context; - stream_context.set_deadline( - std::chrono::system_clock::now() + 3s); - auto stream = stub_->streamCyclicPosition(&stream_context); - ASSERT_NE(stream, nullptr); - - api::CyclicPositionRequest open_request; - *open_request.mutable_open()->mutable_target() = makeTarget_(); - open_request.mutable_open()->set_watchdog_timeout_ms(500); - ASSERT_TRUE(stream->Write(open_request)); - - api::CyclicControlResponse stream_response; - ASSERT_TRUE(stream->Read(&stream_response)); - ASSERT_TRUE(stream_response.header().success()) - << stream_response.header().error_message(); - EXPECT_EQ(stream_response.phase(), api::CYCLIC_STREAM_OPENED); - - api::CyclicPositionRequest setpoint_request; - auto* setpoint = setpoint_request.mutable_setpoint(); - setpoint->set_sequence(1); - setpoint->set_target_position_rad(0.25); - setpoint->set_target_velocity_rad_s(0.1); - ASSERT_TRUE(stream->Write(setpoint_request)); - - stream_response.Clear(); - ASSERT_TRUE(stream->Read(&stream_response)); - ASSERT_TRUE(stream_response.header().success()) - << stream_response.header().error_message(); - EXPECT_EQ(stream_response.phase(), api::CYCLIC_STREAM_APPLIED); - EXPECT_EQ(stream_response.sequence(), 1U); - EXPECT_FALSE(stream_response.has_status()); - EXPECT_EQ(plc_.cyclicOpenCount(), 1U); - EXPECT_EQ(plc_.cyclicSampleCount(), 1U); - - ASSERT_TRUE(stream->WritesDone()); - stream_response.Clear(); - ASSERT_TRUE(stream->Read(&stream_response)); - EXPECT_TRUE(stream_response.header().success()) - << stream_response.header().error_message(); - EXPECT_EQ(stream_response.phase(), api::CYCLIC_STREAM_STOPPED); - EXPECT_FALSE(stream->Read(&stream_response)); - const auto stream_finish = stream->Finish(); - ASSERT_TRUE(stream_finish.ok()) << stream_finish.error_message(); - EXPECT_GT(plc_.quickStopCount(), quick_stops_before_stream); - - const auto quick_stops_before_emergency = plc_.quickStopCount(); - api::EmergencyStopRequest emergency_request; - *emergency_request.mutable_target() = makeTarget_(); - grpc::ClientContext emergency_context; - emergency_context.set_deadline( - std::chrono::system_clock::now() + 3s); - api::MotorCommandResponse emergency_response; - const auto emergency_status = stub_->emergencyStop( - &emergency_context, emergency_request, &emergency_response); - ASSERT_TRUE(emergency_status.ok()) << emergency_status.error_message(); - ASSERT_TRUE(emergency_response.header().success()) - << emergency_response.header().error_message(); - EXPECT_TRUE(emergency_response.status().emergency_stopped()); - EXPECT_GT(plc_.quickStopCount(), quick_stops_before_emergency); - - const auto commands_before_rejected_motion = plc_.commandCount(); - api::ProfilePositionRequest rejected_request; - *rejected_request.mutable_target() = makeTarget_(); - rejected_request.set_target_position_rad(0.5); - rejected_request.set_max_velocity_rad_s(0.5); - rejected_request.set_acceleration_rad_s2(1.0); - grpc::ClientContext rejected_context; - rejected_context.set_deadline( - std::chrono::system_clock::now() + 3s); - api::MotorCommandResponse rejected_response; - const auto rejected_status = stub_->profilePosition( - &rejected_context, rejected_request, &rejected_response); - EXPECT_EQ(rejected_status.error_code(), - grpc::StatusCode::FAILED_PRECONDITION); - EXPECT_FALSE(rejected_response.header().success()); - EXPECT_EQ(plc_.commandCount(), commands_before_rejected_motion); - - const auto sessions_before_disconnect = - plc_.acceptedSessionCount(); - const auto commands_before_disconnect = plc_.commandCount(); - plc_.disconnectClient(); - ASSERT_TRUE(waitUntil( - [&] { - return plc_.acceptedSessionCount() > - sessions_before_disconnect; - }, - 3s)); - std::this_thread::sleep_for(100ms); - EXPECT_EQ(plc_.commandCount(), commands_before_disconnect); - - api::SetMotorEnabledRequest reenable_request; - *reenable_request.mutable_target() = makeTarget_(); - reenable_request.set_enabled(true); - grpc::ClientContext reenable_context; - reenable_context.set_deadline( - std::chrono::system_clock::now() + 3s); - api::MotorCommandResponse reenable_response; - const auto reenable_status = stub_->setEnabled( - &reenable_context, reenable_request, &reenable_response); - ASSERT_TRUE(reenable_status.ok()) << reenable_status.error_message(); - EXPECT_TRUE(reenable_response.header().success()) - << reenable_response.header().error_message(); - EXPECT_EQ(plc_.commandCount(), commands_before_disconnect + 1U); - EXPECT_FALSE(reenable_response.status().emergency_stopped()); -} - -TEST_F(MotorServiceModbusE2eTest, - ActiveCyclicStreamFailsClosedAcrossReconnectAndNewStreamRecovers) -{ - grpc::ClientContext old_context; - old_context.set_deadline(std::chrono::system_clock::now() + 8s); - auto old_stream = stub_->streamCyclicPosition(&old_context); - ASSERT_NE(old_stream, nullptr); - - api::CyclicPositionRequest open_request; - *open_request.mutable_open()->mutable_target() = makeTarget_(); - open_request.mutable_open()->set_watchdog_timeout_ms(1000); - ASSERT_TRUE(old_stream->Write(open_request)); - - api::CyclicControlResponse response; - ASSERT_TRUE(old_stream->Read(&response)); - ASSERT_TRUE(response.header().success()) - << response.header().error_message(); - ASSERT_EQ(response.phase(), api::CYCLIC_STREAM_OPENED); - - api::CyclicPositionRequest first_sample; - first_sample.mutable_setpoint()->set_sequence(1); - first_sample.mutable_setpoint()->set_target_position_rad(0.1); - first_sample.mutable_setpoint()->set_target_velocity_rad_s(0.0); - ASSERT_TRUE(old_stream->Write(first_sample)); - response.Clear(); - ASSERT_TRUE(old_stream->Read(&response)); - ASSERT_TRUE(response.header().success()) - << response.header().error_message(); - ASSERT_EQ(response.phase(), api::CYCLIC_STREAM_APPLIED); - ASSERT_EQ(plc_.cyclicOpenCount(), 1U); - ASSERT_EQ(plc_.cyclicSampleCount(), 1U); - - const auto accepted_sessions_before = plc_.acceptedSessionCount(); - plc_.disconnectClient(); - ASSERT_TRUE(waitUntil( - [&] { - return plc_.acceptedSessionCount() > - accepted_sessions_before; - }, - 3s)); - - const auto opens_after_reconnect = plc_.cyclicOpenCount(); - const auto samples_after_reconnect = plc_.cyclicSampleCount(); - const auto stops_after_reconnect = plc_.quickStopCount(); - api::CyclicPositionRequest stale_sample; - stale_sample.mutable_setpoint()->set_sequence(2); - stale_sample.mutable_setpoint()->set_target_position_rad(0.2); - stale_sample.mutable_setpoint()->set_target_velocity_rad_s(0.0); - ASSERT_TRUE(old_stream->Write(stale_sample)); - - response.Clear(); - ASSERT_TRUE(old_stream->Read(&response)); - EXPECT_FALSE(response.header().success()); - EXPECT_EQ(response.phase(), api::CYCLIC_STREAM_FAILED); - EXPECT_NE(response.header().error_message().find( - "rejected cyclic position setpoint"), - std::string::npos); - EXPECT_FALSE(old_stream->Read(&response)); - const auto old_finish = old_stream->Finish(); - EXPECT_TRUE( - old_finish.error_code() == grpc::StatusCode::FAILED_PRECONDITION || - old_finish.error_code() == grpc::StatusCode::CANCELLED) - << old_finish.error_message(); - // Stream cleanup may commit a safety QuickStop, but the stale generation - // must never commit an Open or sample into the new PLC session. - EXPECT_EQ(plc_.cyclicOpenCount(), opens_after_reconnect); - EXPECT_EQ(plc_.cyclicSampleCount(), samples_after_reconnect); - EXPECT_GT(plc_.quickStopCount(), stops_after_reconnect); - - grpc::ClientContext new_context; - new_context.set_deadline(std::chrono::system_clock::now() + 5s); - auto new_stream = stub_->streamCyclicPosition(&new_context); - ASSERT_NE(new_stream, nullptr); - ASSERT_TRUE(new_stream->Write(open_request)); - - response.Clear(); - ASSERT_TRUE(new_stream->Read(&response)); - ASSERT_TRUE(response.header().success()) - << response.header().error_message(); - ASSERT_EQ(response.phase(), api::CYCLIC_STREAM_OPENED); - - api::CyclicPositionRequest fresh_sample; - fresh_sample.mutable_setpoint()->set_sequence(1); - fresh_sample.mutable_setpoint()->set_target_position_rad(0.3); - fresh_sample.mutable_setpoint()->set_target_velocity_rad_s(0.0); - ASSERT_TRUE(new_stream->Write(fresh_sample)); - response.Clear(); - ASSERT_TRUE(new_stream->Read(&response)); - ASSERT_TRUE(response.header().success()) - << response.header().error_message(); - EXPECT_EQ(response.phase(), api::CYCLIC_STREAM_APPLIED); - EXPECT_EQ(plc_.cyclicOpenCount(), opens_after_reconnect + 1U); - EXPECT_EQ(plc_.cyclicSampleCount(), samples_after_reconnect + 1U); - - ASSERT_TRUE(new_stream->WritesDone()); - response.Clear(); - ASSERT_TRUE(new_stream->Read(&response)); - EXPECT_TRUE(response.header().success()) - << response.header().error_message(); - EXPECT_EQ(response.phase(), api::CYCLIC_STREAM_STOPPED); - EXPECT_FALSE(new_stream->Read(&response)); - const auto new_finish = new_stream->Finish(); - EXPECT_TRUE(new_finish.ok()) << new_finish.error_message(); -} - -} // namespace -} // namespace cmvr::service diff --git a/cmvr-es/service/grpc/tests/grpc_motor_service_test.cpp b/cmvr-es/service/grpc/tests/grpc_motor_service_test.cpp index 79ecf8f5..df4a56fe 100644 --- a/cmvr-es/service/grpc/tests/grpc_motor_service_test.cpp +++ b/cmvr-es/service/grpc/tests/grpc_motor_service_test.cpp @@ -309,7 +309,7 @@ protected: std::string grpc_socket_path_; }; -TEST_F(MotorServiceTest, SetZeroAckLossReportsUnknownOutcome) +TEST_F(MotorServiceTest, SetZeroBackendFailureReportsUnknownOutcome) { protocol_->calibrate_success_ = false; @@ -323,7 +323,7 @@ TEST_F(MotorServiceTest, SetZeroAckLossReportsUnknownOutcome) EXPECT_FALSE(response.header().success()); EXPECT_NE(response.header().error_message().find("outcome unknown"), std::string::npos); - EXPECT_NE(response.header().error_message().find("zero_epoch"), + EXPECT_NE(response.header().error_message().find("device state"), std::string::npos); EXPECT_EQ(protocol_->quick_stop_count_.load(), 0); } diff --git a/docs/motor_service_modbus_tcp.md b/docs/motor_service_modbus_tcp.md deleted file mode 100644 index 623aa0a3..00000000 --- a/docs/motor_service_modbus_tcp.md +++ /dev/null @@ -1,1022 +0,0 @@ -# MotorService 与 Modbus TCP PLC 电机控制 - -本文说明当前 `MotorService`、CMVR PLC v1 寄存器协议和 Siemens -S7-1215C DC/DC/DC 侧的对接方法。本文只描述当前源码已经存在的接口和 -协议；寄存器表中标为“预留”或“当前未发送”的内容，不代表上层已经开放。 - -## 1. 当前范围 - -当前实现是一条刻意保持简单的单轴控制链： - -```text -gRPC MotorService - | - v -MotorManager - | - v -AbstractMotor - | - v -CmvrPlcMotorProtocol - | - v -ModbusTcpMotorBusRuntime - | - v -libmodbus -> PLC MB_SERVER -> 驱动器/电机 -``` - -各层职责如下： - -- `MotorService`：解析 `MotorManager` ID 和电机选择器，执行单电机互斥、 - 同步等待、gRPC 取消检测、流式 latest-wins 和软件急停抢占。 -- `MotorManager`：按 `motor_id` 或 `joint_name` 返回 `AbstractMotor`。 -- `AbstractMotor`：提供统一的置零、Profile、Cyclic、使能和 Quick Stop - 接口。 -- `CmvrPlcMotorProtocol`：执行关节限位检查，把 SI 单位转换为定点整数， - 并映射成 CMVR PLC v1 命令。 -- `ModbusTcpMotorBusRuntime`：管理一个 PLC 连接、握手、心跳、重连、每轴 - 命令序列、payload/commit 写入和 ACK 轮询。 -- PLC：必须原子接收命令、执行驱动器控制、发布状态，并独立执行通信和 - 周期指令 watchdog。 - -当前服务按“一个 RPC 或一个流独占一个电机”仲裁，不是多轴同步控制器。 -同一个 `MotorManager` 中不同电机可以分别被不同调用者控制。需要多轴同一扫描 -周期锁存时，应在后续版本增加 manager/runtime 级批量 commit，不应依赖客户端 -逐轴调用。 - -runtime 的 `start()` 启动连接 supervisor，而不是要求 PLC 当时已经在线。首次 -同步握手失败时 MotorManager 仍完成注册，runtime 保持 -`connected=false`，并从 `reconnect_min_ms` 起按上限退避持续重试。离线期间 -状态 RPC 返回 `UNAVAILABLE`，运动命令在写 mailbox 前安全失败；PLC 后续上线 -并完成完整身份/session 握手后无需重启 `cmvr_es`。 - -## 2. 实际 gRPC 接口 - -服务定义位于： - -- `protos/cmvr/api/motor_service.proto` -- `protos/cmvr/api/motor_command.proto` - -`MotorTarget.header.device_id` 是 `MotorManager` 的设备 ID，不是单个电机 -的 ID。`MotorTarget.selector` 必须且只能设置一个： - -```text -motor_id uint32，当前服务要求不大于 255 -joint_name 非空字符串 -``` - -### 2.1 Unary RPC - -| RPC | 当前语义 | 是否阻塞 | -| --- | --- | --- | -| `setZero` | 调用 `calibrateZeroQ()`。PLC 后端发送 `SetZero`，等待 PLC 进入成功终态后返回 | 是，PLC 后端安全命令上限当前为 5 s | -| `moveToZero` | 复用 Profile Position，把目标位置设为 `0 rad`；不是 `setZero`，也不发送寄存器图中的 `MoveToZero` opcode | 是，直到到位、取消或超时 | -| `profilePosition` | 设置 Profile Position 的目标位置、最大速度和加速度 | 是，直到到位、取消或超时 | -| `profileVelocity` | 设置 Profile Velocity 的目标速度和加速度 | 是，直到实际速度稳定进入容差 | -| `emergencyStop` | 抢占当前阻塞命令/流，设置服务内软件急停锁存并调用 `quickStop()` | 是，等待 PLC Quick Stop 成功终态 | -| `getStatus` | 读取模式、位置、速度、到位标志和 MotorService 仲裁状态 | 同步快照读取 | -| `setEnabled` | `enabled=true` 调用 `torqueOn()`，成功后解除服务内软件急停锁存；`false` 调用 `torqueOff()` | 是，等待 PLC 成功终态 | - -`moveToZero`、`profilePosition` 和 `profileVelocity` 使用 -`MotorWaitOptions`： - -| 字段 | `0` 时默认值 | 服务端范围/语义 | -| --- | ---: | --- | -| `timeout_ms` | 30000 ms | 1 ms 至 600000 ms | -| `poll_period_ms` | 10 ms | 1 ms 至 1000 ms | -| `position_tolerance_rad` | 0.001 rad | 仅正值覆盖默认值 | -| `velocity_tolerance_rad_s` | 0.01 rad/s | 仅正值覆盖默认值 | -| `settle_sample_count` | 3 | 1 至 1000 个连续样本 | - -`setZero` 只有在 PLC 返回 `Completed + ZeroValid` 且 `zero_epoch` 相比命令前 -变化时才成功。如果 RPC 返回失败、取消、断链或超时，而 commit 可能已经写出， -session/zero epoch 的最终结果是不确定的；禁止直接盲重试，应先通过 PLC/HMI -或维护诊断确认当前 session、`zero_epoch` 和实际零位状态。 - -Profile Position 只有在 PLC/驱动器报告 `target_reached`，同时实际位置和 -速度连续满足容差时才返回成功。Profile Velocity 在实际速度连续满足目标速度 -容差时返回成功；RPC 返回后速度命令仍然有效。正常停车应再发送目标速度为 -`0 rad/s` 的 `profileVelocity`，不能把 RPC 返回理解为“速度控制已经结束”。 - -Profile Position/Velocity 的 PLC payload 使用 600000 ms -`command_timeout_ms` 上限，与 MotorService 允许的最长等待一致。RPC 使用更短 -超时或被取消时，服务端仍会主动发送 Quick Stop；PLC 通信 watchdog 仍是上位机 -失联时的权威保护。该 600000 ms 是 PLC 执行上限，不会把 RPC 默认等待从 -30000 ms 延长。 - -成功 ACK 的 Profile Position/Velocity 会绑定到提交时的 -`connection_epoch`。只要 PLC 断线重连使 epoch 改变，该 Profile 的位置、速度 -和到位反馈就全部失效：`getQ/getQd` 返回 NaN，`reachedTargetQ` 返回 false。 -即使新 session 的安全停止速度恰好也是目标 `0 rad/s`，也不能把数值相等误认为 -旧命令成功。成功的新 Profile 会重新绑定当前 epoch；成功的首个 Cyclic -sample、Quick Stop 或 Disable 会清除该绑定。Enable/SetZero 本身不恢复旧 -Profile 反馈的可信性。 - -当前 `getStatus` 会依次调用模式、位置、速度和到位读取。对于 Modbus TCP 后端， -这些读取不是一次原子寄存器快照，字段可能来自相邻的 PLC 扫描周期。 -它只暴露 `AbstractMotor` 的通用状态和 MotorService 仲裁状态，不包含 -`plc_boot_id`、session、`fault_code`、`zero_epoch` 或 PLC result code。 -结果不确定或需要故障恢复时，应从 PLC/HMI 或维护诊断层读取这些原始字段， -不能只依赖本 RPC。 - -### 2.2 双向流 RPC - -当前有两个双向流： - -```proto -rpc streamCyclicPosition(stream CyclicPositionRequest) - returns (stream CyclicControlResponse); - -rpc streamCyclicVelocity(stream CyclicVelocityRequest) - returns (stream CyclicControlResponse); -``` - -首帧必须是 `open`： - -```text -open.target 选择一个 MotorManager 中的单个电机 -open.watchdog_timeout_ms gRPC 层输入 watchdog -``` - -watchdog 为 `0` 时默认 500 ms，非零值被限制到 20 ms 至 60000 ms。打开 -成功后，服务端先返回： - -```text -phase = CYCLIC_STREAM_OPENED -sequence = 0 -status = 当前电机状态 -``` - -`CYCLIC_STREAM_OPENED` 只表示 MotorService 已解析目标、取得该电机的独占 -控制权并选择了 Cyclic 模式。CMVR PLC 后端采用惰性打开：收到第一个合法 -setpoint 时才依次发送 `OpenCyclic*` 和对应的 `Cyclic*Sample`。因此客户端 -必须等到该 setpoint 的 `CYCLIC_STREAM_APPLIED`，才能确认 PLC 已 ACK 打开 -命令并锁存了首个样本。 - -每次 `OpenCyclicPosition/Velocity` 都创建新的轴级 stream epoch。PLC 必须在 -Open 的同一个原子状态事务中清零 `last_applied_cyclic_sequence`、旧样本去重 -状态和 cyclic watchdog，再设置正确的 CSP/CSV mode 与 `StreamActive=1`。 -runtime 只有确认这些 Open 后置条件才接受 ACK，Protocol 随后才从样本序列 `1` -重新开始。Quick Stop、Disable 或断链关闭旧 epoch；重开后的首个 `1` 必须 -重新应用，不能被旧流的样本 `1` 去重。 - -每次 gRPC cyclic 流首帧触发的 `setMode` 都建立一个新的上层 -`cyclic_generation`，即使模式值与上一条流相同。该 generation 绑定当时的 -`connection_epoch`。活动流遇到重连后会永久锁存失败：同一流的当前和后续 -setpoint 都返回失败，不发送新的 `OpenCyclic*` 或 `Cyclic*Sample`；服务端 -终止并 Quick Stop 该流。客户端必须创建新的 gRPC 流，由新首帧显式建立新的 -generation 后才允许重新 Open。禁止把断链前或断链期间 pending 的 latest -setpoint 自动应用到新 PLC session。 - -后续每帧只能是 `setpoint`： - -- 周期位置：严格递增且非零的 `sequence`、`target_position_rad`，以及可选 - `target_velocity_rad_s`。 -- 周期速度：严格递增且非零的 `sequence`、`target_velocity_rad_s`。 - -序列号允许跳号，但不能为 `0`、重复或倒退。服务端 reader 只保留一个尚未 -处理的最新帧；新帧覆盖旧帧时，`dropped_setpoints` 累计增加。这是低延迟 -latest-wins 语义，不适合必须逐点无损执行的离线轨迹。 - -每次 `AbstractMotor` 接受 setpoint 后返回： - -```text -phase = CYCLIC_STREAM_APPLIED -sequence = 已应用的客户端序列 -dropped_setpoints = 本流累计覆盖数 -``` - -对于 CMVR PLC 后端，`AbstractMotor` 返回成功前，runtime 已经看到相同 -command sequence 的 PLC ACK；周期样本还要求 -`last_applied_cyclic_sequence` 严格等于本次样本序列。对于其他电机后端， -`APPLIED` 只保证对应后端的同步调用返回 `true`。 -为避免每个周期样本额外触发多次现场总线状态读取，`APPLIED` 响应刻意不携带 -`status`。`OPENED` 和终止响应仍携带完整状态；需要连续遥测的客户端应以独立、 -较低频率调用 `getStatus`，不能把 `APPLIED` 当作状态采样接口。 - -客户端正常结束请求流后，服务端先同步调用 `quickStop()`；确认后返回 -`CYCLIC_STREAM_STOPPED` 并释放电机所有权。当前协议没有显式 `close` -请求帧；客户端 half-close 就是结束信号。若 Quick Stop 未被后端确认， -服务端改发 `CYCLIC_STREAM_FAILED` 并以 `INTERNAL` 结束 RPC，不会把失败 -误报成 `STOPPED`。 - -出现以下情况时，服务端会 Quick Stop： - -- gRPC context 被取消或 deadline 到期； -- 输入在 watchdog 窗口内没有新帧； -- 序列号或 payload 非法； -- PLC/后端拒绝 setpoint； -- 客户端停止读取响应； -- `emergencyStop` 增加抢占 generation。 - -服务端流使用同步 `Write()`。慢客户端可能阻塞反馈写入，reader 此时仍会 -覆盖 pending setpoint，但上层 watchdog 检查也可能被延迟。因此： - -1. 客户端必须并发、持续读取反馈； -2. gRPC watchdog 只是第一层保护； -3. PLC 侧通信 watchdog 和周期样本 watchdog 才是断网、进程卡死时的 - 权威保护。 - -### 2.3 当前没有开放的能力 - -当前 `MotorService` 没有以下 RPC： - -- Profile Torque； -- Cyclic Torque 流； -- Clear Fault； -- 普通 Stop/Halt； -- 多轴原子控制。 - -寄存器块预留了 `target_torque`，但 `CmvrPlcMotorProtocol` 当前明确拒绝 -周期力矩。不要因为寄存器存在就让 PLC 项目把该功能标记为已经可用。 - -### 2.4 仲裁和 gRPC 状态 - -同一电机已有阻塞命令或流时，新控制调用返回 -`RESOURCE_EXHAUSTED`。软件急停锁存期间，运动命令返回 -`FAILED_PRECONDITION`；只有成功执行 `setEnabled(enabled=true)` 才清除 -该服务内锁存。 - -任何取消、超时、异常或周期流结束后的 Quick Stop 如果未被后端确认成功， -MotorService 也会按 fail-closed 原则进入同一软件急停锁存。此时 RPC 的 -`INTERNAL` 表示“安全状态尚未确认”，不能继续发送运动命令；应先排查 PLC/ -驱动器状态，再通过成功的 `setEnabled(enabled=true)` 显式恢复。 -显式 `emergencyStop` 会在调用后端前先锁存；若最终 Quick Stop 未确认，它 -返回 `FAILED_PRECONDITION`，但仍保持锁存。两种返回码都不能解释为已经安全 -停机。 - -常见 gRPC 状态包括： - -- `INVALID_ARGUMENT`：选择器、数值、首帧或序列号非法； -- `NOT_FOUND`：MotorManager 或电机不存在； -- `RESOURCE_EXHAUSTED`：同一电机已有 owner； -- `FAILED_PRECONDITION`：软件急停锁存或后端拒绝命令； -- `DEADLINE_EXCEEDED`：同步等待或流 watchdog 超时； -- `ABORTED`：被 `emergencyStop` 抢占； -- `CANCELLED`：客户端取消或停止读取。 -- `UNAVAILABLE`：后端断链，或状态因故障/watchdog 返回非 finite； -- `INTERNAL`：服务异常，或命令失败后的安全清理/Quick Stop 也失败。 - -## 3. MotorManager 配置 - -仓库已提供单 PLC、两轴配置示例 -`cmvr-es/config/devices/motor/plc_motors.pb.txt`，内容如下： - -```textproto -motor { - id: "plc_motors" - - motor_groups { - id: "plc_axis_group" - bus_type: MOTOR_BUS_MODBUS_TCP - vendor: MOTOR_VENDOR_PLC_GENERIC - protocol: MOTOR_PROTOCOL_CMVR_PLC_V1 - - modbus_tcp { - host: "192.168.0.10" - port: 502 - unit_id: 1 - - connect_timeout_ms: 500 - io_timeout_ms: 100 - heartbeat_period_ms: 100 - communication_watchdog_ms: 1000 - cyclic_watchdog_ms: 500 - status_poll_period_ms: 20 - reconnect_min_ms: 100 - reconnect_max_ms: 2000 - command_ack_timeout_ms: 500 - - protocol_major: 1 - protocol_minor: 0 - - axes { motor_id: 1 axis_index: 0 } - axes { motor_id: 2 axis_index: 1 } - } - - joint_limits { - enable: true - source: JOINT_LIMIT_SOURCE_CUSTOM - joints { - joint_name: "PLC_AXIS_1" - q_lb: -3.141592653589793 - q_ub: 3.141592653589793 - qd: 1.0 - qdd: 2.0 - } - joints { - joint_name: "PLC_AXIS_2" - q_lb: -1.5707963267948966 - q_ub: 1.5707963267948966 - qd: 0.5 - qdd: 1.0 - } - } - - motors { - motors { id: 1 joint_name: "PLC_AXIS_1" } - motors { id: 2 joint_name: "PLC_AXIS_2" } - } - } -} -``` - -字段行为： - -- `host` 必须是 IPv4 字面量（例如 `192.168.0.10`），不做 DNS 解析；这样 - TCP 建连和 `stop()` 不会被无界的名称解析阻塞。 -- `port=0` 时 runtime 使用 502；`unit_id=0` 时使用 1。 -- `connect_timeout_ms=0` 时使用 500 ms；该值为 TCP 建连的独立硬超时。 -- `io_timeout_ms=0` 时使用 100 ms，并同时用于 libmodbus response timeout - 和 byte timeout。 -- 心跳、通信 watchdog、周期 watchdog、状态轮询、重连和 ACK timeout - 为 `0` 时，当前 runtime 分别使用 - 100、500、500、20、100、2000、500 ms；上面的实体样例将通信 watchdog - 显式放宽为 1000 ms。 -- `communication_watchdog_ms` 必须同时不少于 - `2 * heartbeat_period_ms` 和 - `3 * io_timeout_ms + 2 * heartbeat_period_ms`； - `command_ack_timeout_ms`、`cyclic_watchdog_ms` 必须不少于 - `3 * io_timeout_ms + status_poll_period_ms`，因为一次可靠状态读取包含 - sequence-before、完整状态块、sequence-after 三次 FC3； - `cyclic_watchdog_ms` 必须在 20 ms 至 60000 ms 内； - `reconnect_max_ms` 不得小于 `reconnect_min_ms`，否则初始化失败。 -- `protocol_major=0` 时使用当前 major `1`。握手严格检查 major； - PLC 的 minor 版本不得低于配置要求。 -- `axis_index` 是 PLC 轴块索引，必须小于 PLC 发布的 `axis_count`，且同一 - group 内不能重复。 -- `encoder_counts_per_rev` 和 `gear_ratio` 对 CMVR PLC v1 不生效，因为 PLC - 协议交换的是 SI 定点值，而不是编码器 count。 - -还需要在 DeviceManager 配置中注册该 MotorSystem： - -```textproto -device_manager { - # 只有未被 RobotArm 选择且仍希望初始化全部电机时才需要 true。 - init_all_motors_when_no_active_joints: true - - devices { - id: "plc_motors" - type: DEVICE_TYPE_MOTOR_SYSTEM - config_file: "devices/motor/plc_motors.pb.txt" - enable: true - } -} -``` - -`id` 必须与 `motor.id` 以及 gRPC -`MotorTarget.header.device_id` 一致。安装产物读取 -`output/bin/config/`；修改源码树配置后需再次执行 `cmake --install build`。 - -## 4. CMVR PLC v1 Holding Register - -### 4.1 地址和 32 位编码 - -本文和 C++ runtime 中的地址都是 **零基 Modbus PDU Holding Register -地址**。地址 `0` 是第一个 Holding Register，PLC/HMI 文档若使用 `40001` -风格显示，则通常对应本文地址 `0`。不要把 `40001` 直接传给 -`modbus_read_registers()`。 - -每个寄存器是 16 位。所有 32 位有符号或无符号值均使用： - -```text -register[offset] = bits 31..16（高 WORD） -register[offset + 1] = bits 15..0 （低 WORD） -``` - -libmodbus 负责单个 16 位寄存器在线路上的字节序；PLC 应显式按“高 WORD -在前、低 WORD 在后”组合 32 位值。TIA 中建议用移位和 OR 的辅助 FC， -不要依赖 `AT` 视图、`BLKMOV` 或 CPU 内部字节布局偶然得到相同结果。 - -### 4.2 全局区 - -全局区从地址 `0` 开始，共 32 个寄存器： - -| 地址 | 长度 | 类型 | 名称 | 方向与说明 | -| ---: | ---: | --- | --- | --- | -| 0 | 1 | `UINT16` | `magic_cm` | PLC -> CMVR，固定 `16#434D` | -| 1 | 1 | `UINT16` | `magic_vr` | PLC -> CMVR，固定 `16#5652` | -| 2 | 1 | `UINT16` | `protocol_major` | PLC -> CMVR，当前为 1 | -| 3 | 1 | `UINT16` | `protocol_minor` | PLC -> CMVR，当前为 0 | -| 4 | 1 | `UINT16` | `axis_count` | PLC -> CMVR，可用轴块数量 | -| 5 | 1 | `UINT16` | `plc_global_state` | PLC -> CMVR，全局状态 | -| 6..7 | 2 | `UINT32` | `plc_boot_id` | PLC -> CMVR，每次 PLC 程序运行实例重启必须改变 | -| 8..9 | 2 | `UINT32` | `cmvr_session_id` | CMVR -> PLC，每次成功握手/重连生成新的非零随机会话 | -| 10..11 | 2 | `UINT32` | `cmvr_heartbeat` | CMVR -> PLC，周期递增 | -| 12..13 | 2 | `UINT32` | `plc_heartbeat` | PLC -> CMVR，PLC 自己的周期计数 | -| 14..15 | 2 | `UINT32` | `communication_watchdog_ms` | CMVR -> PLC，PLC 侧通信 watchdog | -| 16 | 1 | `UINT16` | `global_error` | PLC -> CMVR，全局错误 | -| 17 | 1 | `UINT16` | `owner_state` | PLC -> CMVR，当前 owner/握手状态 | -| 18..19 | 2 | `UINT32` | `owner_session_id` | PLC -> CMVR，当前 Accepted/Rejected 决策对应的候选 session | -| 20..31 | 12 | - | reserved | 写 0，PLC 忽略 | - -当前 runtime 在握手初始快照中检查 magic、版本、`axis_count` 和非零 -`plc_boot_id`，先写通信 watchdog，最后以新 session + 首个 heartbeat 作为 -候选 owner 的发布动作。握手的 -每次轮询以及 owner 接受后的最终完整快照都必须保持相同的 magic、版本、 -`axis_count` 和 boot ID；其中任一变化或 boot ID 变为 0 都会立即断线重连。 -只有 -`owner_session_id` 等于本次 session，且 `owner_state=Accepted`，连接才 -进入可发命令状态。后续心跳周期同时检查 `plc_boot_id`、owner/session 以及 -`plc_heartbeat` 是否持续推进；PLC 心跳在通信 watchdog 窗口内不变化会主动 -断线并重连。`owner_state=Rejected` 也只有在 `owner_session_id` 回显本次 -session 时才表示本次握手被拒；旧 session 遗留的 Rejected 状态会被忽略并继续 -轮询。 - -PLC 检测到新 session 后，发布顺序必须是：先把 `owner_state` 改成 -`Accepting`（此时不能先改 `owner_session_id`），然后安全停止旧 owner、清除 -mailbox/旧 stream，再写候选 `owner_session_id`，最后发布 `Accepted`。拒绝时 -同样先处于 `Accepting`/`None`，写完对应 session 后最后发布 `Rejected`。 -否则新的 session ID 可能与残留的旧 `Accepted` 短暂组合，令 CMVR 过早发命令。 - -### 4.3 每轴布局 - -轴 `i` 的基地址： - -```text -B(i) = 100 + 128 * i -``` - -每轴占 128 个 Holding Registers： - -```text -B + 0 .. B + 63 控制区 -B + 64 .. B + 127 状态区 -``` - -#### 控制区 - -| 相对地址 | 长度 | 类型 | 名称 | 当前说明 | -| ---: | ---: | --- | --- | --- | -| 0..1 | 2 | `UINT32` | `payload_sequence` | 本次轴命令序列 | -| 2 | 1 | `UINT16` | `command_code` | 见命令码表 | -| 3 | 1 | `UINT16` | `command_flags` | 当前写 0 | -| 4..5 | 2 | `INT32` | `target_position` | micro-rad | -| 6..7 | 2 | `INT32` | `target_velocity` | micro-rad/s | -| 8..9 | 2 | `INT32` | `acceleration` | micro-rad/s^2 | -| 10..11 | 2 | `INT32` | `target_torque` | 预留，mN·m | -| 12..13 | 2 | `INT32` | `position_tolerance` | 预留，micro-rad | -| 14..15 | 2 | `INT32` | `velocity_tolerance` | 预留，micro-rad/s | -| 16..17 | 2 | `UINT32` | `command_timeout_ms` | 命令完成时限；Profile 当前写 600000 ms | -| 18..19 | 2 | `UINT32` | `stream_watchdog_ms` | PLC 侧周期样本 watchdog | -| 20..21 | 2 | `UINT32` | `cyclic_sample_sequence` | 周期样本序列 | -| 22..23 | 2 | `UINT32` | `client_monotonic_time_ms` | CMVR steady-clock 低 32 位 | -| 24..25 | 2 | `UINT32` | `expected_zero_epoch` | `SetZero` 写入命令前读到的 zero epoch | -| 26 | 1 | `UINT16` | `disconnect_action` | 预留；当前驱动写 0 | -| 27..28 | 2 | `UINT32` | `command_session_id` | 必须等于当前已接受 session | -| 29..59 | 31 | - | reserved | 写 0 | -| 60..61 | 2 | `UINT32` | `payload_sequence_mirror` | 必须等于 payload sequence | -| 62..63 | 2 | `UINT32` | `commit_sequence` | 单独、最后写入 | - -#### 状态区 - -下表地址相对于 `S = B + 64`： - -| 相对地址 | 长度 | 类型 | 名称 | 说明 | -| ---: | ---: | --- | --- | --- | -| 0..1 | 2 | `UINT32` | `ack_sequence` | PLC 已解析的 command sequence | -| 2..3 | 2 | `UINT32` | `active_sequence` | 当前执行中的 command sequence | -| 4 | 1 | `UINT16` | `command_state` | 命令状态 | -| 5 | 1 | `UINT16` | `result_code` | 结果码 | -| 6 | 1 | `UINT16` | `axis_state` | PLC/驱动器轴状态 | -| 7 | 1 | `UINT16` | `current_mode` | `cmvr.msgs.RunMode` 数值 | -| 8..9 | 2 | `INT32` | `actual_position` | micro-rad | -| 10..11 | 2 | `INT32` | `actual_velocity` | micro-rad/s | -| 12..13 | 2 | `INT32` | `actual_torque` | mN·m | -| 14..15 | 2 | `INT32` | `target_position` | PLC 当前目标，micro-rad | -| 16..17 | 2 | `INT32` | `target_velocity` | PLC 当前目标，micro-rad/s | -| 18 | 1 | bit field | `status_flags` | 见状态位表 | -| 19 | 1 | `UINT16` | `drive_statusword` | 原始驱动器状态字 | -| 20..21 | 2 | `UINT32` | `fault_code` | PLC/驱动器故障码 | -| 22..23 | 2 | `UINT32` | `zero_epoch` | 置零版本 | -| 24..25 | 2 | `UINT32` | `last_applied_cyclic_sequence` | 最后实际锁存的周期样本 | -| 26..27 | 2 | `UINT32` | `state_sequence` | 状态 seqlock；非零偶数才是稳定版本 | -| 28..29 | 2 | `UINT32` | `plc_monotonic_time_ms` | PLC 单调时间低 32 位 | -| 30..31 | 2 | `UINT32` | `heartbeat_age_ms` | PLC 计算的 CMVR 心跳年龄 | -| 32..33 | 2 | `UINT32` | `ack_session_id` | `ack_sequence` 所属 session | -| 34..61 | 28 | - | reserved | PLC 写 0 | -| 62..63 | 2 | `UINT32` | `state_sequence_mirror` | 稳定版本镜像 | - -PLC 状态区使用 seqlock 发布。稳定版本从非零偶数 `2` 开始，每次完整更新增加 -`2`；版本回绕到 `0` 时跳到 `2`。发布顺序必须是： - -1. 更新任何状态字段前，先把 `state_sequence` 写成下一奇数， - `state_sequence_mirror` 保持上一个稳定偶数； -2. 写完 ACK、状态、实际值、flags、heartbeat age 等所有字段； -3. 先把 `state_sequence_mirror` 写成新的非零偶数； -4. 最后把 `state_sequence` 写成相同偶数。 - -runtime 在同一 socket 互斥区内执行三段读取：先单独读取 -`state_sequence`，再读取完整 64-word 状态块，最后再次单独读取 -`state_sequence`。只有 before、after、块内 sequence 和 mirror 四者相等， -且为非零偶数时才接受完整块；失败最多重试 3 次。这样不要求 Siemens -`MB_SERVER` 对 64-word FC3 做原子内存快照，也允许不同的完整状态读取之间版本 -持续推进。 - -为保证活性，PLC 不得在每个高速控制扫描都无条件翻转 Holding 状态版本。建议 -驱动控制状态先写内部 shadow，再由较低频的对外发布任务在字段有意义变化时复制 -到 Holding 状态区，并让每个稳定偶数版本至少覆盖三次连续 FC3 的时间窗口； -也可以使用双缓冲后按上述 seqlock 顺序发布。否则 PLC 每次 FC3 之间都推进版本， -runtime 的 3 次有限重试会按设计失败，而不是返回可能撕裂的状态。 - -### 4.4 命令码 - -| 值 | 名称 | 当前上层使用 | -| ---: | --- | --- | -| 0 | `Nop` | 否 | -| 1 | `SetZero` | `setZero` | -| 2 | `MoveToZero` | 预留；当前 `moveToZero` 发送 `ProfilePosition(target=0)` | -| 3 | `ProfilePosition` | `moveToZero`、`profilePosition` | -| 4 | `ProfileVelocity` | `profileVelocity` | -| 5 | `OpenCyclicPosition` | 第一个周期位置样本前自动发送 | -| 6 | `CyclicPositionSample` | 周期位置样本 | -| 7 | `OpenCyclicVelocity` | 第一个周期速度样本前自动发送 | -| 8 | `CyclicVelocitySample` | 周期速度样本 | -| 9 | `CloseCyclicStream` | 预留；当前流结束使用 `QuickStop` | -| 10 | `QuickStop` | `emergencyStop`、流结束和上层超时 | -| 11 | `Enable` | `setEnabled(true)` | -| 12 | `Disable` | `setEnabled(false)` | - -PLC 必须区分： - -- `SetZero`：按已确认的项目语义设置当前位置基准，不得擅自解释为运动回原点； -- `ProfilePosition(target=0)`：运动到已经建立的零位； -- Homing/寻找原点：当前 gRPC 和寄存器协议没有独立开放。 - -### 4.5 命令状态、结果和状态位 - -`command_state` 定义： - -```text -0 Idle 1 Received 2 Validating -3 Accepted 4 Running 5 TargetReached -6 Completed 7 Rejected 8 Failed -9 TimedOut 10 QuickStopped 11 CommunicationLost -``` - -runtime 会先拒绝未知状态，再按命令检查成功后置条件： - -- `SetZero`：`Completed`、`ZeroValid=1`，且 `zero_epoch` 相比命令前变化； -- `Enable`：`Completed` 且 `Enabled=1`； -- `Disable`：`Completed` 且 `Enabled=0`； -- `QuickStop`：`QuickStopped` 或 `Completed`，且实际速度已接近 0； -- Profile：`Completed` 或 `TargetReached`； -- 非终态 ACK 只接受 `Accepted`、`Running`、`TargetReached`、`Completed`。 - -`Rejected`、`Failed`、`TimedOut`、`CommunicationLost` 始终视为失败。 - -`result_code` 定义： - -```text -0 Ok 1 InvalidCommand -2 InvalidParameter 3 AxisNotReady -4 AxisBusy 5 NotEnabled -6 PositionLimit 7 VelocityLimit -8 AccelerationLimit 9 ZeroNotValid -10 DriveFault 11 CommandTimeout -12 SequenceError 13 SessionMismatch -14 CommunicationWatchdog -15 CyclicWatchdog 16 Unsupported -17 InternalError -``` - -`status_flags`： - -| bit | 名称 | -| ---: | --- | -| 0 | Enabled | -| 1 | Moving | -| 2 | TargetReached | -| 3 | Fault | -| 4 | QuickStopActive | -| 5 | CommunicationWatchdogExpired | -| 6 | CyclicWatchdogExpired | -| 7 | ZeroValid | -| 8 | StreamActive | -| 9 | CommandBusy | - -`CmvrPlcMotorProtocol` 把 `Fault`、`CommunicationWatchdogExpired` 和 -`CyclicWatchdogExpired` 都作为致命反馈状态；失败/未知 `command_state` 或非 -`Ok result_code` 同样无效。此时 `getQ/getQd` 返回 NaN，`reachedTargetQ` -返回 false，MotorService 不得把残留的有限位置/速度误判为到位或成功状态。 - -### 4.6 缩放和范围 - -CMVR PLC v1 使用固定缩放： - -| 量 | gRPC/C++ 单位 | 寄存器值 | -| --- | --- | --- | -| 位置 | rad | `round(rad * 1,000,000)`，micro-rad | -| 速度 | rad/s | `round(rad/s * 1,000,000)`，micro-rad/s | -| 加速度 | rad/s^2 | `round(rad/s^2 * 1,000,000)`，micro-rad/s^2 | -| 力矩 | N·m | 预留为 `round(N·m * 1,000)`，mN·m | - -前三者当前由 `CmvrPlcMotorProtocol` 实际使用，并在转换前检查 finite 和 -`INT32` 范围。缩放为 1,000,000 时，理论可表示范围约为 -`[-2147.483648, 2147.483647]` 个对应 SI 单位。PLC 仍必须再次执行软件限位、 -驱动器限位和状态检查，不能只依赖上位机检查。 - -Modbus PLC 电机初始化强制要求每个轴都有有限、有效的 `q_lb`、`q_ub`、`qd` -和 `qdd`：`q_ub > q_lb`，且 `qd`、`qdd` 均大于 0。缺失、NaN、无穷或非法 -限位会令电机初始化失败，不能以“未配置限位”的方式继续带轴运行。 - -## 5. payload-first、commit-last 和 ACK - -### 5.1 CMVR 写入顺序 - -每轴的 `command_sequence` 独立递增并跳过 `0`。一次命令严格执行： - -1. 仅 `SetZero` 先可靠读取 fresh `zero_epoch`；其他命令不做冗余 baseline - 状态读取； -2. 在本地构造 62 个寄存器的完整 payload； -3. 同时写入 `payload_sequence` 和 `payload_sequence_mirror`； -4. 使用一次 Holding Register 批量写，把 `B+0 .. B+61` 写入 PLC； -5. 再使用第二次写，把同一序列写入 `B+62 .. B+63`； -6. 轮询状态区，直到 `ack_sequence` 等于本次 command sequence； -7. 检查 `command_state` 和 `result_code`； -8. 周期样本还要检查 `last_applied_cyclic_sequence`。 - -PLC 只允许在以下条件全部满足时消费 payload： - -```text -commit_sequence != last_processed_commit -payload_sequence == payload_sequence_mirror -payload_sequence == commit_sequence -command_session_id == owner_session_id -当前 cmvr_session_id 是已取得 owner 的有效 session -``` - -对于 `SetZero`，PLC 还必须要求 `expected_zero_epoch` 等于执行前的当前 -`zero_epoch`；不匹配时以 `Rejected + SequenceError` ACK，避免陈旧或重复的 -置零事务改变新的零位基准。 - -PLC 应先把完整 payload 复制到内部命令快照，再更新 -`last_processed_commit`。不要一边读取 Holding Register，一边执行驱动器动作。 - -无论接受还是拒绝，PLC 都应把 `ack_sequence` 和 `ack_session_id` 更新为 -本次命令的序列和 session，同时填写 `command_state` 和 `result_code`。 -runtime 只有在两者都精确匹配时才接受 ACK，旧连接残留的相同序列不会被误认。 -否则 CMVR 只能得到模糊的 ACK timeout。 - -如果 commit 已写成功但 ACK 读取失败，电机是否已经执行是不确定的。 -CMVR runtime 不会自动重放该运动命令。特别是 `SetZero`、非零速度和使能命令， -调用方不得在未知结果下盲目重试；应先读取 PLC 状态、boot/session、zero epoch -和实际轴状态。 - -### 5.2 boot/session 防重放 - -PLC 必须把全局 session 和每轴 command sequence 共同作为命令命名空间： - -- `cmvr_session_id` 在每次成功握手/重连时重新生成非零随机值。检测到新 - session 时，PLC 必须先把 `owner_state` 置为 `Accepting`，并且这一步必须 - 早于改写 `owner_session_id`；随后安全停止旧 owner 的轴、清除旧的 - stream-active 状态，并清空各轴旧 mailbox 的 commit/payload 接收状态。 - 清理全部完成后再写入新的 `owner_session_id`，最后一步才把 - `owner_state` 发布为 `Accepted`。这样 runtime 不会把“新 session ID + - 旧 Accepted”误认为新 owner 已就绪。拒绝候选 session 时也必须先保持 - `Accepting`、写入对应 `owner_session_id`，最后一步发布 `Rejected`。 - CMVR 只有看到该 - 握手确认后才把连接标记为可用。CMVR - 会把每轴 command sequence 从 `1` 重新开始，PLC 必须在新 session - 命名空间内接受该序列。runtime 至少保证相邻两次连接的 session 不相同， - 避免紧邻重连立即复用旧 mailbox/ACK 命名空间。 -- 每个轴应保存“当前 session 下最后处理的 commit sequence”。相同 commit - 只返回原 ACK，不能再次执行。 -- PLC 启动时必须生成新的非零 `plc_boot_id`，清除 owner 和旧 commit 接受 - 状态，并要求看到新的有效 session/heartbeat 后才接受命令。若 Holding DB - 设置为 retentive，也不能让上次启动遗留的 commit 自动执行。 -- CMVR 周期检查 `plc_boot_id`。boot ID 变化会断开连接；重连成功后 - `connection_epoch` 改变。Protocol 会锁存并拒绝旧 cyclic generation， - 不会自动重新 `OpenCyclic*`。只有客户端新建 gRPC 流并通过首帧建立新 - generation 后才能恢复。 -- Profile 和 Cyclic 命令把 Protocol 已绑定的 `connection_epoch` 作为 - `expected_connection_epoch` 传给 runtime。runtime 在调用入口、等待每轴 - 队列之后，以及持有 I/O 锁准备分配 sequence/写 commit 前都要求 - invocation/current/expected 三者严格一致。因此即使重连恰好发生在 - Protocol 读取 epoch 与 runtime 写 mailbox 之间，旧命令也只会失败，不会 - 写入新 session。Quick Stop/Disable 不绑定旧 expected epoch，它们作为新调用 - 只清理当前 session。 -- command sequence 是 32 位并会回绕。PLC 应使用 session 加序列的状态机， - 明确处理回绕；不能简单把“任何不相等的值”永远视为新命令。 - -TCP 自身有序可靠，但不能替代上述应用层规则：PLC DB 可能保留旧值，PLC 和 -CMVR 也可能独立重启。 - -## 6. S7-1215C DC/DC/DC 与 TIA Portal - -### 6.1 MB_SERVER 数据块 - -建议创建一个专用、非 retentive 的协议数据块，例如： - -```scl -HoldingRegister : ARRAY[0 .. 100 + 128 * AXIS_COUNT - 1] OF WORD; -``` - -数组下标与本文零基 PDU 地址一致。根据使用的 TIA Portal 和 CPU firmware， -`MB_HOLD_REG` 对数据块访问方式可能有要求；若编译器不允许优化 DB 的 -VARIANT/指针映射，应关闭该协议 DB 的 optimized block access。不要把命令 -状态机、驱动器实例 DB 与外部可写 Holding Register 直接重叠。 - -在 OB1 或固定周期 OB 中每个扫描周期调用一个 `MB_SERVER` 实例。典型参数 -包括： - -```text -DISCONNECT = FALSE -CONNECT_ID = 项目内唯一连接 ID -IP_PORT = 502 -MB_HOLD_REG = 协议 HoldingRegister 数组 -NDR/DR/ERROR/STATUS = 诊断输出 -``` - -不同 TIA Portal 版本的块接口和 VARIANT 写法可能略有差异，应以当前工程中 -插入的 `MB_SERVER` 指令帮助为准。一个 server 实例使用自己的 instance DB； -连接 ID 和 TCP 端口不得与其他 OUC/Modbus 实例冲突。 - -`MB_SERVER` 只负责 Modbus TCP 搬运。另建 PLC FB 完成： - -1. 初始化 magic、协议版本、axis count 和 boot ID； -2. 监视 session 与 heartbeat； -3. 对每轴执行 payload/commit 原子接收； -4. 做范围、状态、使能、零位和 command timeout 校验； -5. 调用 Technology Object、PROFINET 驱动器 telegram 或项目已有驱动器 FB； -6. 更新 ACK、命令状态、结果码、实际值和状态位； -7. 执行通信/周期 watchdog 和安全降级。 - -### 6.2 32 位辅助函数 - -PLC 侧应显式实现以下等价逻辑： - -```text -DecodeUDInt(high, low) = - SHL(WORD_TO_DWORD(high), 16) OR WORD_TO_DWORD(low) - -EncodeHigh(value) = DWORD_TO_WORD(SHR(value, 16)) -EncodeLow(value) = DWORD_TO_WORD(value AND 16#0000_FFFF) -``` - -有符号值先按 DWORD 原样组合，再解释为 DINT。负值使用二进制补码；不要分别 -对高、低 WORD 做有符号运算。 - -### 6.3 驱动器动作映射 - -映射必须由实际驱动器/Technology Object 语义决定： - -- `SetZero` 只执行已确认的“当前位置建立零位”动作，不得自动替换成会运动的 - Homing。 -- “回 0 位”当前收到的是 `ProfilePosition(target=0)`。 -- Profile Position/Velocity 的轨迹生成在 PLC/驱动器侧完成；Modbus TCP - 不是驱动器位置环或电流环。 -- Cyclic Position/Velocity 是 CMVR 到 PLC 的软实时 setpoint 更新。PLC - 在本地扫描周期锁存最新样本，再由 PLC/驱动器的确定性周期执行。 -- 稳态周期样本通常需要 5 次 Modbus 事务（payload、commit，以及 - guard/full/guard 三次 ACK 快照读取）；恰逢心跳到期时增加 1 次。首个样本 - 还需要先完成一次惰性的 Open。这个事务模型不承诺固定控制频率，也不是硬 - 实时链路。 -- Quick Stop 的减速度、抱闸时序和重力轴保持策略必须在 PLC/驱动器中配置。 -- Enable/Disable 必须检查故障、STO、抱闸和轴 ready 状态；不能只翻转一个 - 普通布尔位。 - -PLC 必须在执行前再次检查位置、速度、加速度、驱动器状态和项目级互锁，并把 -拒绝原因写入 `result_code`。 - -当前测试只在 x86 loopback fake PLC 上验证功能和协议一致性，尚未给出 -S7-1215C 实机可持续频率。投产前必须在目标 TIA Portal 程序、真实 PLC 扫描 -周期和现场交换网络下阶梯增加 CSP/CSV 发送频率，记录 ACK 延迟的 -P50/P99/最大值、`dropped_setpoints`、Modbus 异常与 watchdog 触发次数。 -`cyclic_watchdog_ms` 应依据实测最坏延迟并保留工程余量设置；在完成这项台架 -测试前，不能宣称支持某个固定 Hz。 - -CMVR 中的 `QuickStop` 和 `Disable` 走 safety-priority 通道：它们会增加该轴 -的取消 generation，令正在等待的普通命令失败，并绕过普通轴命令互斥锁。安全 -命令不先读取状态，也不在 mailbox 前插入心跳写；拿到 socket 后首先发送安全 -payload/commit。它仍与单个 Modbus socket 的一次事务互斥，不会把两条报文 -交错写入；普通命令在 commit 前会再次检查 generation，避免急停完成后补发 -旧运动命令。 - -同一轴的 safety 命令使用独立 safety mutex 串行。每个 safety 调用在等待该锁 -之前就提升普通命令的取消 generation，因此抢占不会被前一个 Quick Stop 阻塞； -但后来的 safety 调用不会取消前一个 safety 调用的 ACK 等待，多个并发 -Quick Stop/Disable 都能得到各自确定的执行结果。 - -### 6.4 心跳、watchdog 和断链 - -CMVR 默认每 100 ms 写一次 `cmvr_session_id + cmvr_heartbeat`。本仓库实体 -样例要求 PLC 在 1000 ms 通信 watchdog 内看到 heartbeat **发生变化** -（字段为 0 时 runtime 默认 500 ms）。PLC 应使用自己的 -单调时间测量“最后一次变化”的年龄，不能只检查 TCP socket 仍连接，也不能把 -重复读到同一个 counter 当作有效心跳。 - -命令提交不会为每个周期样本强制写 heartbeat；runtime 记录最后一次成功写入 -时刻，仅在 `heartbeat_period_ms` 已到期时由当前命令顺带补写。supervisor -worker 仍按周期写心跳并检查 PLC boot/session/heartbeat，因此高频 setpoint -既不会产生一倍额外 Modbus 写流量，也不会饿死通信 watchdog。 - -推荐 PLC 状态机： - -```text -无 owner - -> 收到有效非零 session 且 heartbeat 开始变化 - -> owner active - -> 接受该 session 的 commit - -owner active - -> heartbeat age 超过 communication_watchdog_ms - -> 对所有 owner 轴执行受控停止/Quick Stop - -> 设置 CommunicationWatchdogExpired - -> command_state = CommunicationLost - -> 释放 owner -``` - -周期流还需要每轴独立 watchdog。只有新的 -`Cyclic*Sample.cyclic_sample_sequence` 才刷新它；普通 CMVR 心跳不能让旧的 -非零速度无限保持。周期 watchdog 超时后应停止该轴、清除 `StreamActive`， -设置 `CyclicWatchdogExpired`，并要求重新 `OpenCyclic*`。网络恢复后不得 -自动恢复断链前的速度或 setpoint；旧 gRPC 流必须失败关闭，由客户端新建流。 - -CMVR runtime 遇到 Modbus 读写错误会关闭 socket，并按 -`reconnect_min_ms` 到 `reconnect_max_ms` 指数退避重连。它不会自动重放上一 -条运动命令。PLC 侧安全动作必须在没有 CMVR 参与的情况下独立完成。 - -## 7. 安全边界 - -标准 **S7-1215C DC/DC/DC 不是 failsafe PLC**。`MB_SERVER`、普通 OB/FB、 -普通数字输出以及本服务的 `emergencyStop` 都只是功能性控制，不能提供 -安全等级的急停、STO 或防护门联锁。 - -实际设备至少应按风险评估使用： - -- 硬接线急停回路； -- 合规的安全继电器，或 F-CPU + F-I/O； -- 驱动器 STO 双通道或经认证的安全功能； -- 接触器/抱闸反馈和必要的 EDM； -- 与机械负载、重力轴和制动距离匹配的安全设计。 - -软件 `emergencyStop` 和 Modbus Quick Stop 可以作为操作层的快速停止，但 -不能替代硬接线安全回路。标准 CPU 程序卡死、以太网交换机故障、普通输出粘连 -或软件错误时，硬件安全链仍必须独立切断危险能量。 - -## 8. 构建、依赖和测试 - -### 8.1 x86-64 - -仓库已有 x86-64 的 libmodbus 3.1.11： - -```text -dependency/x86/third_party/modbus/3.1.11/include/modbus -dependency/x86/third_party/modbus/3.1.11/lib/libmodbus.so -dependency/x86/third_party/modbus/3.1.11/lib/libmodbus.so.5 -``` - -`request.txt` 已包含 `third_party/modbus/3.1.11`。标准构建流程： - -```bash -cmake -S . -B build -DBUILD_TESTING=ON -cmake --build build -j"$(nproc)" -ctest --test-dir build --output-on-failure -cmake --install build -ldd -r output/bin/cmvr_es | grep -E 'modbus|not found' -``` - -只验证本次 MotorService/PLC 电机链路时，可执行： - -```bash -cmake --build build --target \ - modbus_tcp_motor_bus_runtime_test \ - grpc_motor_service_test \ - grpc_motor_service_modbus_e2e_test \ - -j4 -ctest --test-dir build \ - -R '^(modbus_tcp_motor_bus_runtime_test|grpc_motor_service_test|grpc_motor_service_modbus_e2e_test)$' \ - --output-on-failure -``` - -当前这 3 个 CTest 目标共包含 63 个 GoogleTest 用例：Modbus runtime 25 个、 -MotorService 36 个、gRPC–Modbus 端到端 2 个。 - -测试分别覆盖： - -- Modbus runtime 的命令、离线启动后上线、重连/stop-start session 隔离、 - stale owner 决策、握手身份漂移、三段 seqlock 撕裂重试、旧 cyclic - generation 跨 epoch 的 fail-closed 锁存、Protocol-to-runtime - epoch TOCTOU 拒绝、Profile 反馈 session 绑定、CSP/CSV 样本 ACK、冗余 - 状态/心跳事务抑制、并发 safety 串行、故障反馈拒绝、非法轴/样本，以及 - 仓库真实样例配置的解析和 runtime 初始化； -- MotorService 的同步 Profile Position/Velocity 等待、取消/超时、急停竞争、 - 异常边界、CSP/CSV 流、流背压和停止失败； -- 单进程真实链路 - gRPC stub → DeviceManager → MotorManager → `AbstractMotor` → - CMVR PLC protocol → Modbus TCP fake PLC，包括使能、Profile - Position/Velocity、CSP、half-close Quick Stop、急停锁存、活动 cyclic 流 - 跨重连失败关闭、新流恢复，以及断线重连不重放。 - -fake PLC 用例会在 loopback 地址启动本地 server，运行环境必须允许本地 TCP -bind/listen。 - -运行安装产物： - -```bash -./output/bin/cmvr_es -``` - -默认 gRPC 端口由 -`cmvr-es/config/tasks/grpc_server_task/grpc_server_task.pb.txt` 配置，当前为 -`50052`。启动前应先用禁能或脱载轴验证 PLC 寄存器和方向。 - -### 8.2 启用配置和 gRPC 调用 - -首次联调前： - -1. 把 `cmvr-es/config/devices/motor/plc_motors.pb.txt` 中的 `host`、轴映射 - 和关节限位改成现场值； -2. 完成 PLC watchdog、驱动器 Quick Stop 和硬件安全链检查； -3. 将 `cmvr-es/config/manager/device_manager.pb.txt` 中 `plc_motors` 的 - `enable` 改为 `true`； -4. 重新执行 `cmake --install build`，再启动 `./output/bin/cmvr_es`。 - -启用 reflection 后，可先确认服务和状态： - -```bash -grpcurl -plaintext 127.0.0.1:50052 list cmvr.api.MotorService - -grpcurl -plaintext \ - -d '{"target":{"header":{"deviceId":"plc_motors"},"jointName":"PLC_AXIS_1"}}' \ - 127.0.0.1:50052 cmvr.api.MotorService/getStatus -``` - -在轴已安全脱载、PLC/驱动器允许使能后，显式使能并执行一个同步位置命令： - -```bash -grpcurl -plaintext \ - -d '{"target":{"header":{"deviceId":"plc_motors"},"jointName":"PLC_AXIS_1"},"enabled":true}' \ - 127.0.0.1:50052 cmvr.api.MotorService/setEnabled - -grpcurl -plaintext \ - -d '{"target":{"header":{"deviceId":"plc_motors"},"jointName":"PLC_AXIS_1"},"targetPositionRad":0.1,"maxVelocityRadS":0.2,"accelerationRadS2":0.5,"wait":{"timeoutMs":30000}}' \ - 127.0.0.1:50052 cmvr.api.MotorService/profilePosition -``` - -软件 Quick Stop： - -```bash -grpcurl -plaintext \ - -d '{"target":{"header":{"deviceId":"plc_motors"},"jointName":"PLC_AXIS_1"}}' \ - 127.0.0.1:50052 cmvr.api.MotorService/emergencyStop -``` - -双向周期流应使用生成的 gRPC client stub，并发写 setpoint、持续读反馈；不要 -用只发送一次 JSON 的 unary 调用方式模拟。客户端必须先收到 `OPENED`，发送 -首个样本，再等待相同 sequence 的 `APPLIED`。正常退出时 half-close 写端并 -继续读取，直到收到 `STOPPED` 和最终 OK status。 - -当前服务默认监听 `0.0.0.0:50052`，使用 insecure gRPC，任何可达客户端都能 -发控制命令。现场至少应绑定可信控制网接口或回环地址并配置防火墙；不得直接 -暴露到办公网或公网。若需要跨不可信网络访问，应在进入设备前增加认证、TLS -和工业安全网关。 - -### 8.3 ARM 当前缺失 - -`dependency/arm/third_party/` 当前没有 libmodbus。现有 -`libmodbus.so.5.1.0` 是 x86-64 ELF，不能复制到 ARM 设备使用。 - -ARM 支持前需要： - -1. 为目标 ARM ABI 编译 libmodbus 3.1.11； -2. 按相同布局放入 - `dependency/arm/third_party/modbus/3.1.11/{include,lib}`； -3. 确认 ARM toolchain/顶层 CMake 选择 `dependency/arm`。当前顶层 - `CMakeLists.txt` 仍把 `ARCH` 设为 `x86`； -4. 在目标设备执行 `file`、`readelf -h` 和 `ldd -r` 验证架构、SONAME 和 - 运行时依赖； -5. 重新执行无硬件测试和 PLC 台架测试。 - -在这些步骤完成前，ARM 构建应视为不支持 Modbus PLC 电机后端。 - -### 8.4 PLC 台架检查 - -建议按以下顺序验证： - -1. PLC 上电后检查 magic、版本、axis count、boot ID。 -2. 只连接 Modbus，确认 session、CMVR heartbeat 和 PLC heartbeat。 -3. 禁能状态验证错误参数、重复 commit、旧 session 和 ACK/result。 -4. 验证 `SetZero` 的项目语义，确认没有意外运动。 -5. 低速、低加速度验证 Profile Position 和 Profile Velocity。 -6. 验证周期流正常结束、gRPC watchdog 和 PLC cyclic watchdog。 -7. 分别拔网线、停止 `cmvr_es`、重启交换机、重启 PLC，确认不会恢复旧速度。 -8. 验证 `emergencyStop` 后必须显式 enable 才能再次运动。 -9. 在硬件安全回路测试合格后，才允许带载运行。 - -## 9. 当前已知约束 - -- MotorService 是单轴 API，没有多轴同扫描周期 commit。 -- `DeviceManager`/`MotorManager` 拓扑在 gRPC 服务运行期间必须保持不变； - 当前管理器只支持启动期注册，不支持在仍有 RPC 或流持有电机时热移除、 - 热替换同 ID 的 MotorManager。需要换配置时，应先停止 gRPC 服务和设备， - 再重建运行时。 -- Profile Torque、Cyclic Torque、Clear Fault 和独立 Homing 未开放。 -- gRPC 流的同步 `Write()` 可能受慢客户端背压；PLC watchdog 必须独立。 -- `getStatus` 不是一次原子 Modbus 快照。 -- Modbus TCP 不提供认证、加密或安全完整性。控制网络应隔离，并在需要时通过 - 防火墙/VPN/工业安全网关限制访问。 -- Modbus TCP 的普通软件停止不具备功能安全等级。 diff --git a/protos/cmvr/api/motor_command.proto b/protos/cmvr/api/motor_command.proto index a02657d5..a15c1811 100644 --- a/protos/cmvr/api/motor_command.proto +++ b/protos/cmvr/api/motor_command.proto @@ -101,7 +101,7 @@ message SetMotorEnabledRequest { message CyclicStreamOpen { MotorTarget target = 1; - // The PLC/driver watchdog is authoritative. This service watchdog prevents a + // The device/driver watchdog is authoritative. This service watchdog prevents a // stalled gRPC client from retaining control indefinitely. uint32 watchdog_timeout_ms = 2; } diff --git a/protos/cmvr/config/motor_config/motor_config.proto b/protos/cmvr/config/motor_config/motor_config.proto index 6ec4ec3b..322debf2 100644 --- a/protos/cmvr/config/motor_config/motor_config.proto +++ b/protos/cmvr/config/motor_config/motor_config.proto @@ -76,35 +76,13 @@ message MujocoMotorGroupConfig { string world_id = 1; } -message ModbusTcpAxisConfig { - int32 motor_id = 1; - uint32 axis_index = 2; -} - -message ModbusTcpConfig { - string host = 1; - uint32 port = 2; - uint32 unit_id = 3; - uint32 connect_timeout_ms = 4; - uint32 io_timeout_ms = 5; - uint32 heartbeat_period_ms = 6; - uint32 communication_watchdog_ms = 7; - uint32 status_poll_period_ms = 8; - uint32 reconnect_min_ms = 9; - uint32 reconnect_max_ms = 10; - uint32 command_ack_timeout_ms = 11; - uint32 protocol_major = 12; - uint32 protocol_minor = 13; - uint32 cyclic_watchdog_ms = 14; - repeated ModbusTcpAxisConfig axes = 20; -} - enum MotorBusType { MOTOR_BUS_UNKNOWN = 0; MOTOR_BUS_CAN = 1; MOTOR_BUS_ETHERCAT = 2; MOTOR_BUS_MUJOCO = 3; - MOTOR_BUS_MODBUS_TCP = 4; + reserved 4; + reserved "MOTOR_BUS_MODBUS_TCP"; } enum MotorVendor { @@ -112,7 +90,8 @@ enum MotorVendor { MOTOR_VENDOR_TI5 = 1; MOTOR_VENDOR_MUJOCO = 2; MOTOR_VENDOR_EYOU = 3; - MOTOR_VENDOR_PLC_GENERIC = 4; + reserved 4; + reserved "MOTOR_VENDOR_PLC_GENERIC"; } enum MotorProtocol { @@ -120,10 +99,14 @@ enum MotorProtocol { MOTOR_PROTOCOL_CANOPEN = 1; MOTOR_PROTOCOL_ETHERCAT_CIA402 = 2; MOTOR_PROTOCOL_MUJOCO = 3; - MOTOR_PROTOCOL_CMVR_PLC_V1 = 4; + reserved 4; + reserved "MOTOR_PROTOCOL_CMVR_PLC_V1"; } message MotorGroupConfig { + reserved 13; + reserved "modbus_tcp"; + string id = 1; MotorBusType bus_type = 2; MotorVendor vendor = 3; @@ -133,7 +116,6 @@ message MotorGroupConfig { SocketCanConfig can = 10; EtherCATConfig ethercat = 11; MujocoMotorGroupConfig mujoco = 12; - ModbusTcpConfig modbus_tcp = 13; } JointLimitsConfig joint_limits = 30; From 08d58b2ccf5cc96f3eadcfc7f6d6fbb9d295ac20 Mon Sep 17 00:00:00 2001 From: linbo <1034003879@qq.com> Date: Tue, 4 Aug 2026 15:20:29 +0800 Subject: [PATCH 13/20] feat(quic): add robot ID to registration and heartbeat --- .../quic_edge_task/quic_edge_task.pb.txt | 1 + .../quic_edge/src/quic_edge_service.cpp | 28 +++++++++++------- .../tests/quic_edge_protocol_test.cpp | 29 ++++++++++++++++++- .../quic_edge_config/quic_edge_config.proto | 4 +++ protos/cmvr/quic_edge/v1/README.md | 18 +++++++----- protos/cmvr/quic_edge/v1/quic_edge.proto | 2 ++ 6 files changed, 63 insertions(+), 19 deletions(-) diff --git a/cmvr-es/config/tasks/quic_edge_task/quic_edge_task.pb.txt b/cmvr-es/config/tasks/quic_edge_task/quic_edge_task.pb.txt index fd4b000d..f850dc00 100644 --- a/cmvr-es/config/tasks/quic_edge_task/quic_edge_task.pb.txt +++ b/cmvr-es/config/tasks/quic_edge_task/quic_edge_task.pb.txt @@ -7,6 +7,7 @@ quic_edge { server_port: 4433 alpn: "cmvr-quic-edge/1" node_id: "cmvr-edge" + robot_id: "CN-CMVR-MBLRV1-CHAGAN-20260731-001" software_version: "0.1" # The existing cmvr-es gRPC server remains the robot-control endpoint. "auto" diff --git a/cmvr-es/service/quic_edge/src/quic_edge_service.cpp b/cmvr-es/service/quic_edge/src/quic_edge_service.cpp index bd513457..7145f3a0 100644 --- a/cmvr-es/service/quic_edge/src/quic_edge_service.cpp +++ b/cmvr-es/service/quic_edge/src/quic_edge_service.cpp @@ -217,6 +217,7 @@ void populateDescriptor(const config::QuicEdgeConfig& config, { if (!descriptor) return; descriptor->set_node_id(node_id); + descriptor->set_robot_id(config.robot_id()); descriptor->set_boot_id(boot_id); descriptor->set_software_version(software_version); const auto interfaces = @@ -442,6 +443,10 @@ bool QuicEdgeService::validateConfig(const config::QuicEdgeConfig& config, setError(error, "QUIC edge task id is empty"); return false; } + if (config.robot_id().empty()) { + setError(error, "QUIC edge robot_id is empty"); + return false; + } if (config.server_host().empty()) { setError(error, "QUIC edge server_host is empty"); return false; @@ -1097,6 +1102,7 @@ bool QuicEdgeService::sendNodeRegistration(std::string* error) << ", message_sequence=" << envelope.message_sequence() << ", payload_bytes=" << serialized.size() << ", node_id=" << node.node_id() + << ", robot_id=" << node.robot_id() << ", boot_id=" << node.boot_id() << ", grpc_endpoint=" << node.grpc_endpoint().host() << ':' << node.grpc_endpoint().port() @@ -1145,6 +1151,7 @@ bool QuicEdgeService::sendHeartbeat(const std::uint64_t sequence, envelope.set_message_sequence(control_message_sequence_++); auto* heartbeat = envelope.mutable_node_heartbeat(); heartbeat->set_node_id(node_id_); + heartbeat->set_robot_id(config_.robot_id()); heartbeat->set_boot_id(boot_id_); heartbeat->set_session_id(session_id); heartbeat->set_sequence(sequence); @@ -1163,16 +1170,17 @@ bool QuicEdgeService::sendHeartbeat(const std::uint64_t sequence, return false; } if (!sendControlEnvelope(serialized, error)) return false; - // CMVR_LOG(INFO) << "[QuicEdgeService] sent NodeHeartbeat" - // << ", message_sequence=" << envelope.message_sequence() - // << ", payload_bytes=" << serialized.size() - // << ", session=" << session_id - // << ", heartbeat_sequence=" << sequence - // << ", grpc_endpoint=" << heartbeat->grpc_endpoint().host() - // << ':' << heartbeat->grpc_endpoint().port() - // << ", interfaces=" << heartbeat->local_interfaces_size() - // << ", devices=" << heartbeat->device_manager().devices_size() - // << ", sent_at_unix_ms=" << sent_at_unix_ms; + CMVR_LOG(INFO) << "[QuicEdgeService] sent NodeHeartbeat" + << ", message_sequence=" << envelope.message_sequence() + << ", payload_bytes=" << serialized.size() + << ", robot_id=" << heartbeat->robot_id() + << ", session=" << session_id + << ", heartbeat_sequence=" << sequence + << ", grpc_endpoint=" << heartbeat->grpc_endpoint().host() + << ':' << heartbeat->grpc_endpoint().port() + << ", interfaces=" << heartbeat->local_interfaces_size() + << ", devices=" << heartbeat->device_manager().devices_size() + << ", sent_at_unix_ms=" << sent_at_unix_ms; return true; } 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 845191d8..2aa5d876 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 @@ -105,6 +105,7 @@ public: if (envelope.has_node_register_request()) { const auto& request = envelope.node_register_request(); last_registered_node_id_ = request.node().node_id(); + last_registered_robot_id_ = request.node().robot_id(); last_grpc_endpoint_port_ = request.node().grpc_endpoint().port(); last_interface_count_ = request.node().local_interfaces_size(); cmvr::quic_edge::v1::EdgeControlEnvelope response; @@ -253,6 +254,12 @@ public: return last_registered_node_id_; } + std::string lastRegisteredRobotId() const + { + std::lock_guard lock(mutex_); + return last_registered_robot_id_; + } + std::uint32_t lastGrpcEndpointPort() const { std::lock_guard lock(mutex_); @@ -335,6 +342,7 @@ private: std::uint32_t heartbeat_ack_delay_ms_{0U}; std::uint64_t server_message_sequence_{0}; std::string last_registered_node_id_; + std::string last_registered_robot_id_; std::uint32_t last_grpc_endpoint_port_{0}; int last_interface_count_{0}; std::vector> controls_; @@ -359,6 +367,7 @@ config::QuicEdgeConfig validConfig(const std::string& source_track_id) config.set_server_port(4433); config.set_alpn("cmvr-quic-edge/1"); config.set_node_id("test-node"); + config.set_robot_id("CN-CMVR-MBLRV1-CHAGAN-20260731-001"); config.set_software_version("test-version"); config.set_grpc_endpoint_host("auto"); config.set_grpc_endpoint_port(50052U); @@ -507,6 +516,8 @@ bool testServiceWithSharedHub() service.stats().heartbeats_acknowledged >= 1U; })); CHECK_TRUE(transport_view->lastRegisteredNodeId() == "test-node"); + CHECK_TRUE(transport_view->lastRegisteredRobotId() == + "CN-CMVR-MBLRV1-CHAGAN-20260731-001"); CHECK_TRUE(transport_view->lastGrpcEndpointPort() == 50052U); CHECK_TRUE(transport_view->connectCount() == 1U); @@ -664,6 +675,8 @@ bool testDeviceManagerSnapshotInHeartbeat() const auto heartbeat = transport_view->lastHeartbeat(); service.stop(); + CHECK_TRUE(heartbeat.robot_id() == + "CN-CMVR-MBLRV1-CHAGAN-20260731-001"); CHECK_TRUE(heartbeat.has_device_manager()); CHECK_TRUE(heartbeat.device_manager().manager_name() == "edge-device-manager"); @@ -1016,6 +1029,20 @@ bool testLifecycleStateGuards() return true; } +bool testRobotIdIsRequired() +{ + media::MediaSourceHub hub; + auto config = validPresenceOnlyConfig(); + config.clear_robot_id(); + auto transport = std::make_unique(); + quic_edge::QuicEdgeService service( + std::move(config), std::move(transport), hub); + std::string error; + CHECK_TRUE(!service.initialize(&error)); + CHECK_TRUE(error.find("robot_id") != std::string::npos); + return true; +} + } // namespace int main() @@ -1031,7 +1058,7 @@ int main() !testRegistrationRejectionBacksOff() || !testHeartbeatAckRequiresSessionId() || !testSlowMediaStartDoesNotBlockHeartbeat() || - !testLifecycleStateGuards()) { + !testLifecycleStateGuards() || !testRobotIdIsRequired()) { return 1; } std::cout << "quic_edge_protocol_test: PASS\n"; diff --git a/protos/cmvr/config/quic_edge_config/quic_edge_config.proto b/protos/cmvr/config/quic_edge_config/quic_edge_config.proto index 904e5604..48486a9c 100644 --- a/protos/cmvr/config/quic_edge_config/quic_edge_config.proto +++ b/protos/cmvr/config/quic_edge_config/quic_edge_config.proto @@ -83,6 +83,10 @@ message QuicEdgeConfig { // IP reporting and heartbeat continue without a camera or microphone. repeated QuicEdgeTrackConfig tracks = 20; bool include_loopback_interfaces = 21; + + // Immutable robot product identity, for example: + // CN-CMVR-MBLRV1-CHAGAN-20260731-001. + string robot_id = 22; } message QuicEdgeRootConfig { diff --git a/protos/cmvr/quic_edge/v1/README.md b/protos/cmvr/quic_edge/v1/README.md index 4361811e..be3c2178 100644 --- a/protos/cmvr/quic_edge/v1/README.md +++ b/protos/cmvr/quic_edge/v1/README.md @@ -121,8 +121,9 @@ explicit. The legal session order is: 1. The edge sends `NodeRegisterRequest` as its first application message after - every QUIC connect or reconnect. It includes `node_id`, `boot_id`, software - version, the current IPv4/IPv6 interface snapshot and advertised gRPC endpoint. + every QUIC connect or reconnect. It includes `node_id`, the immutable + `robot_id`, `boot_id`, software version, the current IPv4/IPv6 interface + snapshot and advertised gRPC endpoint. 2. The gateway replies with `NodeRegisterResponse`. Media and heartbeat must not start until `accepted=true` and a non-empty `session_id` are received. 3. The edge sends `NodeHeartbeat` at the negotiated interval. The gateway returns @@ -189,12 +190,13 @@ registered QUIC connection. The edge clamps the negotiated value to its supported safety range and returns to the local value on reconnect until a new registration response is accepted. -`device_manager = 9` is an additive protobuf field in `NodeHeartbeat`, so this -extension remains QUIC edge protocol v1. Existing gateways ignore the unknown -field. Updated gateways must continue accepting older v1 heartbeats where -`device_manager` is absent and must not treat an absent snapshot as an empty, -healthy DeviceManager. Enum values may only be appended; existing numeric -meanings must never be renumbered or reused. +`NodeDescriptor.robot_id = 6`, `NodeHeartbeat.device_manager = 9` and +`NodeHeartbeat.robot_id = 10` are additive protobuf fields, so these extensions +remain QUIC edge protocol v1. Existing gateways ignore unknown fields. Updated +gateways must continue accepting older v1 messages where these fields are absent +and must not treat an absent device snapshot as an empty, healthy DeviceManager. +Enum values may only be appended; existing numeric meanings must never be +renumbered or reused. DATAGRAM negotiation is required only when at least one media track is enabled. The reliable registration and heartbeat path remains valid for a zero-track diff --git a/protos/cmvr/quic_edge/v1/quic_edge.proto b/protos/cmvr/quic_edge/v1/quic_edge.proto index 3dca447c..48f3a321 100644 --- a/protos/cmvr/quic_edge/v1/quic_edge.proto +++ b/protos/cmvr/quic_edge/v1/quic_edge.proto @@ -128,6 +128,7 @@ message NodeDescriptor { string software_version = 3; repeated NetworkInterfaceAddress local_interfaces = 4; GrpcEndpoint grpc_endpoint = 5; + string robot_id = 6; } // This must be the first application message sent after each QUIC connection @@ -162,6 +163,7 @@ message NodeHeartbeat { repeated NetworkInterfaceAddress local_interfaces = 7; GrpcEndpoint grpc_endpoint = 8; DeviceManagerSnapshot device_manager = 9; + string robot_id = 10; } message NodeHeartbeatAck { From 459b76db1d612ca96f5d3101dd608a1e7a94da8b Mon Sep 17 00:00:00 2001 From: tankaitao <1767759995@qq.com> Date: Tue, 4 Aug 2026 16:30:42 +0800 Subject: [PATCH 14/20] translate --- cmvr-es/common/types/agv/agv_types.h | 22 +++++++ cmvr-es/config/manager/device_manager.pb.txt | 2 +- cmvr-es/config/manager/task_manager.pb.txt | 2 +- cmvr-es/devices/agv/abstract_agv.h | 15 +++++ .../seer_robokit/include/seer_robokit_agv.h | 1 + .../include/seer_robokit_protocol.h | 1 + .../src/seer_robokit_navigation.cpp | 62 +++++++++++++++++++ .../service/grpc/include/grpc_agv_service.h | 4 ++ cmvr-es/service/grpc/src/grpc_agv_service.cpp | 39 ++++++++++++ protos/cmvr/api/agv_command.proto | 17 +++++ protos/cmvr/api/agv_service.proto | 4 ++ protos/cmvr/api/agv_utils.proto | 26 ++++++++ 12 files changed, 193 insertions(+), 2 deletions(-) diff --git a/cmvr-es/common/types/agv/agv_types.h b/cmvr-es/common/types/agv/agv_types.h index 0e01a02f..d5e02b4d 100644 --- a/cmvr-es/common/types/agv/agv_types.h +++ b/cmvr-es/common/types/agv/agv_types.h @@ -91,6 +91,14 @@ enum class AgvTaskType { Custom }; +/** + * @brief 固定距离平移使用的距离参考模式。 + */ +enum class AgvTranslationMode { + Odometry = 0, + Localization +}; + /** * @brief AGV 车体坐标系下的平面速度。 * @@ -102,6 +110,20 @@ struct AgvVelocity { double wz{0.0}; }; + + +/** + * @brief AGV 车体坐标系下的固定距离平移参数。 + */ +struct AgvTranslation { + double distance{0.0}; + double vx{0.0}; + double vy{0.0}; + AgvTranslationMode mode{AgvTranslationMode::Odometry}; +}; + + + /** * @brief 导航通用运动约束和执行选项。 * diff --git a/cmvr-es/config/manager/device_manager.pb.txt b/cmvr-es/config/manager/device_manager.pb.txt index 8c8c691d..134e2a9e 100644 --- a/cmvr-es/config/manager/device_manager.pb.txt +++ b/cmvr-es/config/manager/device_manager.pb.txt @@ -132,7 +132,7 @@ device_manager { id: "src1100" type: DEVICE_TYPE_AGV config_file: "devices/agv/seer_robokit.pb.txt" - enable: false + enable: true } devices { diff --git a/cmvr-es/config/manager/task_manager.pb.txt b/cmvr-es/config/manager/task_manager.pb.txt index 4d5df7a7..223f9873 100644 --- a/cmvr-es/config/manager/task_manager.pb.txt +++ b/cmvr-es/config/manager/task_manager.pb.txt @@ -28,7 +28,7 @@ task_manager { run_mode: TASK_RUN_MODE_BLOCKING_SERVICE config_file: "tasks/quic_edge_task/quic_edge_task.pb.txt" # Host-development default: no QUIC Gateway or physical media devices. - enable: true + enable: false } tasks { id: "ume_teleop" diff --git a/cmvr-es/devices/agv/abstract_agv.h b/cmvr-es/devices/agv/abstract_agv.h index 643fa974..40ec6cb9 100644 --- a/cmvr-es/devices/agv/abstract_agv.h +++ b/cmvr-es/devices/agv/abstract_agv.h @@ -92,6 +92,21 @@ public: return AgvResult::failure(AgvErrorCode::UnsupportedCommand, "followPath not implemented"); } + + + /** + * @brief 按指定速度执行固定距离平移。 + * + * 返回成功表示控制器已经接受命令，不表示运动已经完成。 + */ + virtual AgvResult translate(const AgvTranslation& translation) + { + (void)translation; + return AgvResult::failure( + AgvErrorCode::UnsupportedCommand, + "translate not implemented"); + } + /** * @brief 发起显式站点到站点路径导航任务，并指定同步/异步选项。 * diff --git a/cmvr-es/devices/agv/seer_robokit/include/seer_robokit_agv.h b/cmvr-es/devices/agv/seer_robokit/include/seer_robokit_agv.h index 37028a0f..5a8ec3a1 100644 --- a/cmvr-es/devices/agv/seer_robokit/include/seer_robokit_agv.h +++ b/cmvr-es/devices/agv/seer_robokit/include/seer_robokit_agv.h @@ -51,6 +51,7 @@ public: AgvResult followPath( const std::vector& path, const AgvMotionOptions& options) override; + AgvResult translate(const AgvTranslation& translation) override; AgvResult pauseNavigation() override; AgvResult resumeNavigation() override; AgvResult cancelNavigation() override; 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 index 2d581cb5..89df8282 100644 --- a/cmvr-es/devices/agv/seer_robokit/include/seer_robokit_protocol.h +++ b/cmvr-es/devices/agv/seer_robokit/include/seer_robokit_protocol.h @@ -21,6 +21,7 @@ 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 kRobotTaskTranslate = 3055; constexpr std::uint16_t kRobotTaskGoTargetList = 3066; constexpr std::uint16_t kRobotTaskClearTargetList = 3067; constexpr std::uint16_t kRobotConfigLock = 4005; 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 index 39b1ebeb..edf22ead 100644 --- a/cmvr-es/devices/agv/seer_robokit/src/seer_robokit_navigation.cpp +++ b/cmvr-es/devices/agv/seer_robokit/src/seer_robokit_navigation.cpp @@ -621,6 +621,68 @@ AgvResult SeerRobokitAgv::followPath( options)); } + +AgvResult SeerRobokitAgv::translate( + const AgvTranslation& translation) +{ + if (!std::isfinite(translation.distance) + || !std::isfinite(translation.vx) + || !std::isfinite(translation.vy)) { + return AgvResult::failure( + AgvErrorCode::InvalidArgument, + "SEER Robokit translation distance, vx, and vy must be finite"); + } + if (translation.distance <= 0.0) { + return AgvResult::failure( + AgvErrorCode::InvalidArgument, + "SEER Robokit translation distance must be greater than zero"); + } + if (translation.vx == 0.0 && translation.vy == 0.0) { + return AgvResult::failure( + AgvErrorCode::InvalidArgument, + "SEER Robokit translation requires a non-zero vx or vy"); + } + + int mode = 0; + switch (translation.mode) { + case AgvTranslationMode::Odometry: + mode = 0; + break; + case AgvTranslationMode::Localization: + mode = 1; + break; + default: + return AgvResult::failure( + AgvErrorCode::InvalidArgument, + "SEER Robokit translation mode must be odometry or localization"); + } + + Json::Value payload(Json::objectValue); + jsonMember(payload, "dist") = translation.distance; + jsonMember(payload, "vx") = translation.vx; + jsonMember(payload, "vy") = translation.vy; + jsonMember(payload, "mode") = mode; + + Json::Value response; + std::uint64_t accepted_generation = 0; + auto result = sendControlledCommand_( + sock_navigation_, + kRobotTaskTranslate, + payload, + &response, + &accepted_generation); + + // API 3055 does not expose a task id. Do not publish a fake tracked task, + // but invalidate navigation state if the command may have been accepted. + if (accepted_generation != 0) { + clearPoseTask_(accepted_generation); + } + return result; +} + + + + AgvResult SeerRobokitAgv::pauseNavigation() { Json::Value response; diff --git a/cmvr-es/service/grpc/include/grpc_agv_service.h b/cmvr-es/service/grpc/include/grpc_agv_service.h index 6ec4e65d..56e6115c 100644 --- a/cmvr-es/service/grpc/include/grpc_agv_service.h +++ b/cmvr-es/service/grpc/include/grpc_agv_service.h @@ -72,6 +72,10 @@ public: grpc::Status stopMapping(grpc::ServerContext* context, const api::CommandHeader_Request* request, api::CommandHeader_Feedback* response) override; + grpc::Status translate( + grpc::ServerContext* context, + const api::AgvTranslateCommand_Request* request, + api::AgvTranslateCommand_Feedback* response) override; private: device::DeviceManager& dmgr_; diff --git a/cmvr-es/service/grpc/src/grpc_agv_service.cpp b/cmvr-es/service/grpc/src/grpc_agv_service.cpp index f289a587..4615e83c 100644 --- a/cmvr-es/service/grpc/src/grpc_agv_service.cpp +++ b/cmvr-es/service/grpc/src/grpc_agv_service.cpp @@ -121,6 +121,16 @@ device::AgvVelocity toVelocity(const msgs::AgvVelocity& src) return {src.vx(), src.vy(), src.wz()}; } +device::AgvTranslation toTranslation(const msgs::AgvTranslation& src) +{ + return { + src.distance(), + src.vx(), + src.vy(), + static_cast(src.mode()) + }; +} + device::AgvPathSegment toPathSegment(const msgs::AgvPathSegment& src) { device::AgvPathSegment dst; @@ -501,6 +511,35 @@ grpc::Status gRPCAgvServiceImpl::followPath(grpc::ServerContext* context, } } + + +grpc::Status gRPCAgvServiceImpl::translate( + grpc::ServerContext* context, + const api::AgvTranslateCommand_Request* request, + api::AgvTranslateCommand_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) { + return setDeviceNotFound(response, device_id); + } + return setResponseResult( + response, + agv->translate(toTranslation(request->translation()))); + } catch (const std::exception& e) { + fillFeedback(response->mutable_header(), false, e.what()); + return grpc::Status( + grpc::StatusCode::INTERNAL, + e.what()); + } +} + + + grpc::Status gRPCAgvServiceImpl::pauseNavigation(grpc::ServerContext*, const api::CommandHeader_Request* request, api::CommandHeader_Feedback* response) diff --git a/protos/cmvr/api/agv_command.proto b/protos/cmvr/api/agv_command.proto index 81c7676d..b434d500 100644 --- a/protos/cmvr/api/agv_command.proto +++ b/protos/cmvr/api/agv_command.proto @@ -95,6 +95,23 @@ message AgvFollowPathCommand { } } + +// 按指定速度执行固定距离平移命令。 +message AgvTranslateCommand { + message Request { + // 通用请求头。header.device_id 指定目标 AGV 设备。 + CommandHeader.Request header = 1; + // 固定距离平移参数。 + cmvr.msgs.AgvTranslation translation = 2; + } + + message Feedback { + // 仅表示控制器是否接受命令，不表示平移已经完成。 + CommandHeader.Feedback header = 1; + } +} + + // 下发底盘速度命令。 message AgvSetVelocityCommand { // 请求体。 diff --git a/protos/cmvr/api/agv_service.proto b/protos/cmvr/api/agv_service.proto index f0bdc5b2..b1b01e7e 100644 --- a/protos/cmvr/api/agv_service.proto +++ b/protos/cmvr/api/agv_service.proto @@ -69,4 +69,8 @@ service AgvService { // 停止当前建图/扫图会话。 rpc stopMapping(CommandHeader.Request) returns (CommandHeader.Feedback); + + + // 按指定速度平移固定距离。成功返回仅表示控制器已接受命令。 + rpc translate(AgvTranslateCommand.Request) returns (AgvTranslateCommand.Feedback); } diff --git a/protos/cmvr/api/agv_utils.proto b/protos/cmvr/api/agv_utils.proto index e3f697a9..c05f1a79 100644 --- a/protos/cmvr/api/agv_utils.proto +++ b/protos/cmvr/api/agv_utils.proto @@ -14,6 +14,29 @@ message AgvPose2d { double theta = 3; } +// 固定距离平移使用的距离参考模式。 +enum AgvTranslationMode { + // 根据底盘里程计算运动距离。 + AGV_TRANSLATION_MODE_ODOMETRY = 0; + // 根据定位结果计算运动距离。 + AGV_TRANSLATION_MODE_LOCALIZATION = 1; +} + + + +// AGV 车体坐标系下的固定距离平移参数。 +message AgvTranslation { + // 平移距离的绝对值，单位：米，必须大于 0。 + double distance = 1; + // 车体 X 方向速度，单位：米/秒；正为向前，负为向后。 + double vx = 2; + // 车体 Y 方向速度，单位：米/秒；正为向左，负为向右。 + double vy = 3; + // 距离参考模式；默认使用里程模式。 + AgvTranslationMode mode = 4; +} + + // AGV 车体坐标系下的平面速度。 message AgvVelocity { // 车体 X 方向线速度，单位：米/秒。 @@ -24,6 +47,9 @@ message AgvVelocity { double wz = 3; } + + + // AGV 电池状态。 message AgvBatteryState { // 电量比例，范围：[0, 1]，例如 0.8 表示 80%。 From ab4bbfac50886805b482c9687317c8050ac11136 Mon Sep 17 00:00:00 2001 From: xtkuang <87661715@qq.com> Date: Wed, 5 Aug 2026 11:11:40 +0800 Subject: [PATCH 15/20] fix(aubo): handle duplicate motion targets --- cmvr-es/devices/arm/aubo_arm/CMakeLists.txt | 13 ++++ cmvr-es/devices/arm/aubo_arm/aubo_arm.cpp | 56 ++++++++++++--- cmvr-es/devices/arm/aubo_arm/aubo_arm.h | 4 -- .../devices/arm/aubo_arm/aubo_motion_result.h | 34 +++++++++ .../tests/aubo_arm_motion_result_test.cpp | 70 +++++++++++++++++++ 5 files changed, 165 insertions(+), 12 deletions(-) create mode 100644 cmvr-es/devices/arm/aubo_arm/aubo_motion_result.h create mode 100644 cmvr-es/devices/arm/aubo_arm/tests/aubo_arm_motion_result_test.cpp diff --git a/cmvr-es/devices/arm/aubo_arm/CMakeLists.txt b/cmvr-es/devices/arm/aubo_arm/CMakeLists.txt index cd0e47ed..a3351b74 100644 --- a/cmvr-es/devices/arm/aubo_arm/CMakeLists.txt +++ b/cmvr-es/devices/arm/aubo_arm/CMakeLists.txt @@ -54,6 +54,19 @@ install(TARGETS aubo_arm LIBRARY DESTINATION lib) if(BUILD_TESTING) enable_testing() + add_executable(aubo_arm_motion_result_test + tests/aubo_arm_motion_result_test.cpp + ) + target_include_directories(aubo_arm_motion_result_test + PRIVATE + ${CMAKE_SOURCE_DIR}/cmvr-es + ) + add_test( + NAME aubo_arm_motion_result_test + COMMAND aubo_arm_motion_result_test + ) + set_tests_properties(aubo_arm_motion_result_test PROPERTIES TIMEOUT 10) + add_executable(aubo_arm_json_command_test tests/aubo_arm_json_command_test.cpp ) diff --git a/cmvr-es/devices/arm/aubo_arm/aubo_arm.cpp b/cmvr-es/devices/arm/aubo_arm/aubo_arm.cpp index 9a3e2b19..00261397 100644 --- a/cmvr-es/devices/arm/aubo_arm/aubo_arm.cpp +++ b/cmvr-es/devices/arm/aubo_arm/aubo_arm.cpp @@ -1,5 +1,7 @@ #include "devices/arm/aubo_arm/aubo_arm.h" +#include "devices/arm/aubo_arm/aubo_motion_result.h" + #include #include #include @@ -628,17 +630,36 @@ Result AuboArm::moveJ(const JointPositionCommand& target, const MotionOptions& o if (!robot_interface) { return Result::failure(ArmErrorCode::RobotNotReady, "[AuboArm] robot interface is null"); } - robot_interface->getMotionControl()->setSpeedFraction(speed_scaling_); - robot_interface->getMotionControl()->moveJoint( + auto motion_control = robot_interface->getMotionControl(); + motion_control->setSpeedFraction(speed_scaling_); + const int ret = motion_control->moveJoint( target.position, options.acceleration > 0.0 ? options.acceleration : 0.5, options.velocity > 0.0 ? options.velocity : 0.5, options.blend_radius, 0); - if (waitArrival(robot_interface) != 0) { + const auto outcome = aubo_internal::resolveMotionCommand( + ret, + arcs::common_interface::AUBO_OK, + arcs::common_interface::AUBO_REQUEST_IGNORE, + [&robot_interface]() { return waitArrival(robot_interface); }); + switch (outcome) { + case aubo_internal::MotionCommandOutcome::CompletedWithoutMotion: + CMVR_LOG(DEBUG) << "[AuboArm] moveJ completed without motion: sdk ret=" + << ret << " (" + << arcs::common_interface::returnValue2Str(ret) << ")"; + return Result::success(); + case aubo_internal::MotionCommandOutcome::CompletedAfterMotion: + return Result::success(); + case aubo_internal::MotionCommandOutcome::SubmitFailed: + return Result::failure( + ArmErrorCode::CommandFailed, + "[AuboArm] moveJ failed: sdk ret=" + std::to_string(ret) + + " (" + arcs::common_interface::returnValue2Str(ret) + ")"); + case aubo_internal::MotionCommandOutcome::CompletionFailed: return Result::failure(ArmErrorCode::CommandFailed, "[AuboArm] moveJ did not complete"); } - return Result::success(); + return Result::failure(ArmErrorCode::CommandFailed, "[AuboArm] moveJ failed: unknown outcome"); } catch (const std::exception& e) { return Result::failure(ArmErrorCode::CommandFailed, std::string("[AuboArm] moveJ failed: ") + e.what()); } @@ -728,20 +749,39 @@ Result AuboArm::moveL(const CartesianPose& target, const MotionOptions& options, if (!robot_interface) { return Result::failure(ArmErrorCode::RobotNotReady, "[AuboArm] robot interface is null"); } - robot_interface->getMotionControl()->setSpeedFraction(speed_scaling_); + auto motion_control = robot_interface->getMotionControl(); + motion_control->setSpeedFraction(speed_scaling_); std::vector tcp_offset(6, 0.0); robot_interface->getRobotConfig()->setTcpOffset(tcp_offset); std::vector pose{target.x, target.y, target.z, target.rx, target.ry, target.rz}; - robot_interface->getMotionControl()->moveLine( + const int ret = motion_control->moveLine( pose, options.acceleration > 0.0 ? options.acceleration : 0.5, options.velocity > 0.0 ? options.velocity : 0.25, options.blend_radius, 0); - if (waitArrival(robot_interface) != 0) { + const auto outcome = aubo_internal::resolveMotionCommand( + ret, + arcs::common_interface::AUBO_OK, + arcs::common_interface::AUBO_REQUEST_IGNORE, + [&robot_interface]() { return waitArrival(robot_interface); }); + switch (outcome) { + case aubo_internal::MotionCommandOutcome::CompletedWithoutMotion: + CMVR_LOG(DEBUG) << "[AuboArm] moveL completed without motion: sdk ret=" + << ret << " (" + << arcs::common_interface::returnValue2Str(ret) << ")"; + return Result::success(); + case aubo_internal::MotionCommandOutcome::CompletedAfterMotion: + return Result::success(); + case aubo_internal::MotionCommandOutcome::SubmitFailed: + return Result::failure( + ArmErrorCode::CommandFailed, + "[AuboArm] moveL failed: sdk ret=" + std::to_string(ret) + + " (" + arcs::common_interface::returnValue2Str(ret) + ")"); + case aubo_internal::MotionCommandOutcome::CompletionFailed: return Result::failure(ArmErrorCode::CommandFailed, "[AuboArm] moveL did not complete"); } - return Result::success(); + return Result::failure(ArmErrorCode::CommandFailed, "[AuboArm] moveL failed: unknown outcome"); } catch (const std::exception& e) { return Result::failure(ArmErrorCode::CommandFailed, std::string("[AuboArm] moveL failed: ") + e.what()); } diff --git a/cmvr-es/devices/arm/aubo_arm/aubo_arm.h b/cmvr-es/devices/arm/aubo_arm/aubo_arm.h index 9c8a0725..28a19663 100644 --- a/cmvr-es/devices/arm/aubo_arm/aubo_arm.h +++ b/cmvr-es/devices/arm/aubo_arm/aubo_arm.h @@ -87,9 +87,7 @@ private: bool validDof_(std::size_t size, std::string& error) const; Result ensureConnected_(const std::string& context) const; -#if defined(CMVR_HAS_AUBO_SDK) struct SdkState; -#endif private: config::RobotArmConfig cfg_; @@ -107,9 +105,7 @@ private: bool emergency_stopped_{false}; mutable std::mutex mutex_; -#if defined(CMVR_HAS_AUBO_SDK) std::unique_ptr sdk_; -#endif }; } // namespace cmvr::device diff --git a/cmvr-es/devices/arm/aubo_arm/aubo_motion_result.h b/cmvr-es/devices/arm/aubo_arm/aubo_motion_result.h new file mode 100644 index 00000000..d2cba859 --- /dev/null +++ b/cmvr-es/devices/arm/aubo_arm/aubo_motion_result.h @@ -0,0 +1,34 @@ +#ifndef CMVR_ES_AUBO_MOTION_RESULT_H +#define CMVR_ES_AUBO_MOTION_RESULT_H + +namespace cmvr::device::aubo_internal { + +enum class MotionCommandOutcome { + CompletedWithoutMotion, + CompletedAfterMotion, + SubmitFailed, + CompletionFailed, +}; + +template +MotionCommandOutcome resolveMotionCommand( + const int return_code, + const int success_code, + const int request_ignore_code, + WaitForCompletion&& wait_for_completion) +{ + if (return_code == request_ignore_code) { + return MotionCommandOutcome::CompletedWithoutMotion; + } + if (return_code != success_code) { + return MotionCommandOutcome::SubmitFailed; + } + if (wait_for_completion() != 0) { + return MotionCommandOutcome::CompletionFailed; + } + return MotionCommandOutcome::CompletedAfterMotion; +} + +} // namespace cmvr::device::aubo_internal + +#endif // CMVR_ES_AUBO_MOTION_RESULT_H diff --git a/cmvr-es/devices/arm/aubo_arm/tests/aubo_arm_motion_result_test.cpp b/cmvr-es/devices/arm/aubo_arm/tests/aubo_arm_motion_result_test.cpp new file mode 100644 index 00000000..da2c16ee --- /dev/null +++ b/cmvr-es/devices/arm/aubo_arm/tests/aubo_arm_motion_result_test.cpp @@ -0,0 +1,70 @@ +#include "devices/arm/aubo_arm/aubo_motion_result.h" + +#include + +namespace { + +#define CHECK_TRUE(condition) \ + do { \ + if (!(condition)) { \ + std::cerr << "CHECK_TRUE failed at line " << __LINE__ << ": " \ + << #condition << std::endl; \ + return 1; \ + } \ + } while (false) + +} // namespace + +int main() +{ + using cmvr::device::aubo_internal::MotionCommandOutcome; + using cmvr::device::aubo_internal::resolveMotionCommand; + + constexpr int success_code = 0; + constexpr int request_ignore_code = 13; + + int wait_calls = 0; + const auto wait_succeeded = [&wait_calls]() { + ++wait_calls; + return 0; + }; + CHECK_TRUE(resolveMotionCommand( + success_code, + success_code, + request_ignore_code, + wait_succeeded) == MotionCommandOutcome::CompletedAfterMotion); + CHECK_TRUE(wait_calls == 1); + + wait_calls = 0; + CHECK_TRUE(resolveMotionCommand( + request_ignore_code, + success_code, + request_ignore_code, + wait_succeeded) == MotionCommandOutcome::CompletedWithoutMotion); + CHECK_TRUE(wait_calls == 0); + + const int submit_failures[] = {1, 2, 3, -request_ignore_code}; + for (const int return_code : submit_failures) { + wait_calls = 0; + CHECK_TRUE(resolveMotionCommand( + return_code, + success_code, + request_ignore_code, + wait_succeeded) == MotionCommandOutcome::SubmitFailed); + CHECK_TRUE(wait_calls == 0); + } + + wait_calls = 0; + const auto wait_failed = [&wait_calls]() { + ++wait_calls; + return -1; + }; + CHECK_TRUE(resolveMotionCommand( + success_code, + success_code, + request_ignore_code, + wait_failed) == MotionCommandOutcome::CompletionFailed); + CHECK_TRUE(wait_calls == 1); + + return 0; +} From e77c2cfea83cd10e289f28d1ec878a85c8073733 Mon Sep 17 00:00:00 2001 From: xtkuang <87661715@qq.com> Date: Wed, 5 Aug 2026 15:36:49 +0800 Subject: [PATCH 16/20] fix(aubo): make stop release control safely --- cmvr-es/devices/arm/aubo_arm/CMakeLists.txt | 13 + cmvr-es/devices/arm/aubo_arm/aubo_arm.cpp | 580 ++++++++++++++---- cmvr-es/devices/arm/aubo_arm/aubo_arm.h | 9 +- .../devices/arm/aubo_arm/aubo_motion_result.h | 13 +- .../devices/arm/aubo_arm/aubo_motion_state.h | 265 ++++++++ .../tests/aubo_arm_motion_result_test.cpp | 17 +- .../aubo_arm/tests/aubo_motion_state_test.cpp | 127 ++++ .../include/control_authority_manager.h | 10 + .../src/control_authority_manager.cpp | 85 ++- .../tests/control_authority_manager_test.cpp | 32 + cmvr-es/service/grpc/src/grpc_arm_service.cpp | 73 ++- .../service/grpc/src/grpc_system_service.cpp | 65 ++ .../grpc/tests/grpc_arm_service_test.cpp | 320 +++++++++- .../grpc/tests/grpc_system_service_test.cpp | 122 ++++ 14 files changed, 1589 insertions(+), 142 deletions(-) create mode 100644 cmvr-es/devices/arm/aubo_arm/aubo_motion_state.h create mode 100644 cmvr-es/devices/arm/aubo_arm/tests/aubo_motion_state_test.cpp diff --git a/cmvr-es/devices/arm/aubo_arm/CMakeLists.txt b/cmvr-es/devices/arm/aubo_arm/CMakeLists.txt index a3351b74..aee5d72a 100644 --- a/cmvr-es/devices/arm/aubo_arm/CMakeLists.txt +++ b/cmvr-es/devices/arm/aubo_arm/CMakeLists.txt @@ -67,6 +67,19 @@ if(BUILD_TESTING) ) set_tests_properties(aubo_arm_motion_result_test PROPERTIES TIMEOUT 10) + add_executable(aubo_motion_state_test + tests/aubo_motion_state_test.cpp + ) + target_include_directories(aubo_motion_state_test + PRIVATE + ${CMAKE_SOURCE_DIR}/cmvr-es + ) + add_test( + NAME aubo_motion_state_test + COMMAND aubo_motion_state_test + ) + set_tests_properties(aubo_motion_state_test PROPERTIES TIMEOUT 10) + add_executable(aubo_arm_json_command_test tests/aubo_arm_json_command_test.cpp ) diff --git a/cmvr-es/devices/arm/aubo_arm/aubo_arm.cpp b/cmvr-es/devices/arm/aubo_arm/aubo_arm.cpp index 00261397..e2936689 100644 --- a/cmvr-es/devices/arm/aubo_arm/aubo_arm.cpp +++ b/cmvr-es/devices/arm/aubo_arm/aubo_arm.cpp @@ -1,6 +1,7 @@ #include "devices/arm/aubo_arm/aubo_arm.h" #include "devices/arm/aubo_arm/aubo_motion_result.h" +#include "devices/arm/aubo_arm/aubo_motion_state.h" #include #include @@ -18,11 +19,133 @@ namespace cmvr::device { namespace { -struct BusyGuard { - std::atomic& busy; - ~BusyGuard() { busy.store(false); } +class MotionOwnerGuard final { +public: + MotionOwnerGuard( + aubo_internal::MotionState& state, + std::atomic& busy, + const aubo_internal::MotionToken token) + : state_(state), busy_(busy), token_(token) + { + } + + ~MotionOwnerGuard() noexcept + { + try { + if (requires_settlement_ && !settled_) { + state_.failMotion(token_); + } else { + state_.finish(token_, finish_mode_); + } + busy_.store(state_.busy()); + } catch (...) { + // A failed state lock must not terminate an RPC unwind. Preserve + // the conservative externally visible state instead. + busy_.store(true); + } + } + + MotionOwnerGuard(const MotionOwnerGuard&) = delete; + MotionOwnerGuard& operator=(const MotionOwnerGuard&) = delete; + + void requireExplicitSettlement() noexcept + { + requires_settlement_ = true; + } + + void settle() noexcept { settled_ = true; } + + void clearOnFinish() noexcept + { + finish_mode_ = aubo_internal::MotionFinishMode::Clear; + } + + void retainKind() noexcept + { + finish_mode_ = aubo_internal::MotionFinishMode::Retain; + } + +private: + aubo_internal::MotionState& state_; + std::atomic& busy_; + aubo_internal::MotionToken token_; + aubo_internal::MotionFinishMode finish_mode_{ + aubo_internal::MotionFinishMode::RestorePrevious}; + bool requires_settlement_{false}; + bool settled_{false}; }; +class StopStateGuard final { +public: + StopStateGuard( + aubo_internal::MotionState& state, + std::atomic& busy) + : state_(state), busy_(busy) + { + } + + ~StopStateGuard() noexcept + { + if (!completed_) { + try { + state_.failStop(); + } catch (...) { + // Keep the facade fail-closed even if state cleanup fails. + } + busy_.store(true); + } + } + + StopStateGuard(const StopStateGuard&) = delete; + StopStateGuard& operator=(const StopStateGuard&) = delete; + + bool complete() + { + if (!state_.completeStop()) { + return false; + } + busy_.store(false); + completed_ = true; + return true; + } + +private: + aubo_internal::MotionState& state_; + std::atomic& busy_; + bool completed_{false}; +}; + +const char* motionStartFailure( + const aubo_internal::MotionStartStatus status) noexcept +{ + switch (status) { + case aubo_internal::MotionStartStatus::Invalid: + return "invalid motion type"; + case aubo_internal::MotionStartStatus::Busy: + return "another motion is active"; + case aubo_internal::MotionStartStatus::Stopping: + return "a stop operation is in progress"; + case aubo_internal::MotionStartStatus::Blocked: + return "the previous stop did not complete; retry stopMotion or reconnect"; + case aubo_internal::MotionStartStatus::Started: + break; + } + return "unknown motion state"; +} + +const char* motionKindName(const aubo_internal::MotionKind kind) noexcept +{ + switch (kind) { + case aubo_internal::MotionKind::Joint: + return "joint"; + case aubo_internal::MotionKind::Linear: + return "linear"; + case aubo_internal::MotionKind::None: + break; + } + return "unknown"; +} + std::vector defaultJointNames(const std::size_t dof) { std::vector names; @@ -182,21 +305,40 @@ bool waitForRobotMode(const RobotInterfacePtr& robot_interface, return false; } -int waitArrival(const RobotInterfacePtr& robot_interface) +template +aubo_internal::MotionWaitResult waitArrival( + const RobotInterfacePtr& robot_interface, + IsCancelled&& is_cancelled) { int retry_count = 0; + if (is_cancelled()) { + return aubo_internal::MotionWaitResult::Cancelled; + } int exec_id = robot_interface->getMotionControl()->getExecId(); while (exec_id == -1 && retry_count++ < 5) { + if (is_cancelled()) { + return aubo_internal::MotionWaitResult::Cancelled; + } std::this_thread::sleep_for(std::chrono::milliseconds(50)); + if (is_cancelled()) { + return aubo_internal::MotionWaitResult::Cancelled; + } exec_id = robot_interface->getMotionControl()->getExecId(); } if (exec_id == -1) { - return -1; + return is_cancelled() + ? aubo_internal::MotionWaitResult::Cancelled + : aubo_internal::MotionWaitResult::Failed; } while (robot_interface->getMotionControl()->getExecId() != -1) { + if (is_cancelled()) { + return aubo_internal::MotionWaitResult::Cancelled; + } std::this_thread::sleep_for(std::chrono::milliseconds(50)); } - return 0; + return is_cancelled() + ? aubo_internal::MotionWaitResult::Cancelled + : aubo_internal::MotionWaitResult::Completed; } bool waitServoModeSelect(const RobotInterfacePtr& robot_interface, const int mode) @@ -228,6 +370,8 @@ CartesianPose poseFromVector(const std::vector& values) struct AuboArm::SdkState { std::shared_ptr rpc_client; + std::shared_ptr motion_state{ + std::make_shared()}; }; AuboArm::AuboArm(const config::RobotArmConfig& cfg) @@ -423,7 +567,7 @@ ArmState AuboArm::getRobotState() const state.connected = connected_.load(); state.powered_on = state.connected; state.brake_released = state.connected; - state.moving = busy_.load(); + state.moving = busy(); state.robot_mode = getRobotMode(); state.safety_mode = getSafetyMode(); state.control_mode = getControlMode(); @@ -515,7 +659,16 @@ RobotMode AuboArm::getRobotMode() const if (emergency_stopped_) { return RobotMode::Stopped; } - return busy_.load() ? RobotMode::Running : RobotMode::Idle; + return busy() ? RobotMode::Running : RobotMode::Idle; +} + +bool AuboArm::busy() const +{ + std::lock_guard lock(mutex_); + if (sdk_) { + return sdk_->motion_state->busy(); + } + return false; } Result AuboArm::torqueOn() @@ -612,37 +765,60 @@ Result AuboArm::moveJ(const JointPositionCommand& target, const MotionOptions& o if (!validDof_(target.position.size(), error)) { return Result::failure(ArmErrorCode::InvalidDof, error); } - const auto ready = ensureConnected_("moveJ"); - if (!ready.ok()) { - return ready; - } - if (busy_.exchange(true)) { - return Result::failure(ArmErrorCode::RobotNotReady, "[AuboArm] arm is busy: " + id_); - } - BusyGuard busy_guard{busy_}; - try { - const auto robot_names = sdk_->rpc_client->getRobotNames(); + std::unique_lock submit_lock(mutex_); + const auto locked_ready = ensureConnected_("moveJ"); + if (!locked_ready.ok()) { + return locked_ready; + } + const auto rpc_client = sdk_->rpc_client; + const auto motion_state = sdk_->motion_state; + const auto motion = motion_state->begin( + aubo_internal::MotionKind::Joint); + if (!motion.started()) { + return Result::failure( + ArmErrorCode::RobotNotReady, + "[AuboArm] moveJ rejected: " + + std::string(motionStartFailure(motion.status)) + + ", id=" + id_); + } + busy_.store(true); + MotionOwnerGuard motion_owner{ + *motion_state, busy_, motion.token}; + + const auto robot_names = rpc_client->getRobotNames(); if (robot_names.empty()) { return Result::failure(ArmErrorCode::RobotNotReady, "[AuboArm] robot name list is empty"); } - auto robot_interface = sdk_->rpc_client->getRobotInterface(robot_names.front()); + auto robot_interface = rpc_client->getRobotInterface(robot_names.front()); if (!robot_interface) { return Result::failure(ArmErrorCode::RobotNotReady, "[AuboArm] robot interface is null"); } auto motion_control = robot_interface->getMotionControl(); motion_control->setSpeedFraction(speed_scaling_); + motion_owner.requireExplicitSettlement(); const int ret = motion_control->moveJoint( target.position, options.acceleration > 0.0 ? options.acceleration : 0.5, options.velocity > 0.0 ? options.velocity : 0.5, options.blend_radius, 0); + if (ret == arcs::common_interface::AUBO_OK) { + submit_lock.unlock(); + } else { + motion_owner.settle(); + } const auto outcome = aubo_internal::resolveMotionCommand( ret, arcs::common_interface::AUBO_OK, arcs::common_interface::AUBO_REQUEST_IGNORE, - [&robot_interface]() { return waitArrival(robot_interface); }); + [&robot_interface, motion_state, token = motion.token]() { + return waitArrival( + robot_interface, + [motion_state, token]() { + return motion_state->cancelled(token); + }); + }); switch (outcome) { case aubo_internal::MotionCommandOutcome::CompletedWithoutMotion: CMVR_LOG(DEBUG) << "[AuboArm] moveJ completed without motion: sdk ret=" @@ -650,7 +826,14 @@ Result AuboArm::moveJ(const JointPositionCommand& target, const MotionOptions& o << arcs::common_interface::returnValue2Str(ret) << ")"; return Result::success(); case aubo_internal::MotionCommandOutcome::CompletedAfterMotion: + motion_owner.clearOnFinish(); + motion_owner.settle(); return Result::success(); + case aubo_internal::MotionCommandOutcome::Cancelled: + motion_owner.settle(); + return Result::failure( + ArmErrorCode::CommandRejected, + "[AuboArm] moveJ stopped by stopMotion"); case aubo_internal::MotionCommandOutcome::SubmitFailed: return Result::failure( ArmErrorCode::CommandFailed, @@ -671,18 +854,30 @@ Result AuboArm::speedJ(const JointVelocityCommand& velocity, double acceleration if (!validDof_(velocity.velocity.size(), error)) { return Result::failure(ArmErrorCode::InvalidDof, error); } - const auto ready = ensureConnected_("speedJ"); - if (!ready.ok()) { - return ready; - } - if (busy_.exchange(true)) { - return Result::failure(ArmErrorCode::RobotNotReady, "[AuboArm] arm is busy: " + id_); - } - BusyGuard busy_guard{busy_}; - try { + std::unique_lock submit_lock(mutex_); + const auto locked_ready = ensureConnected_("speedJ"); + if (!locked_ready.ok()) { + return locked_ready; + } + const auto rpc_client = sdk_->rpc_client; + const auto motion_state = sdk_->motion_state; + const auto motion = motion_state->begin( + aubo_internal::MotionKind::Joint, true); + if (!motion.started()) { + return Result::failure( + ArmErrorCode::RobotNotReady, + "[AuboArm] speedJ rejected: " + + std::string(motionStartFailure(motion.status)) + + ", id=" + id_); + } + busy_.store(true); + MotionOwnerGuard motion_owner{ + *motion_state, busy_, motion.token}; + Result interface_result; - auto robot_interface = getPrimaryRobotInterface(sdk_->rpc_client, "speedJ", interface_result); + auto robot_interface = getPrimaryRobotInterface( + rpc_client, "speedJ", interface_result); if (!interface_result.ok()) { return interface_result; } @@ -690,14 +885,31 @@ Result AuboArm::speedJ(const JointVelocityCommand& velocity, double acceleration robot_interface->getMotionControl()->setSpeedFraction(speed_scaling_); const double resolved_acceleration = acceleration > 0.0 ? acceleration : 1.5; const double resolved_duration = duration > 0.0 ? duration : 100.0; + motion_owner.requireExplicitSettlement(); + submit_lock.unlock(); + if (motion_state->cancelled(motion.token)) { + motion_owner.settle(); + return Result::failure( + ArmErrorCode::CommandRejected, + "[AuboArm] speedJ stopped by stopMotion before submission"); + } const int ret = robot_interface->getMotionControl()->speedJoint( velocity.velocity, resolved_acceleration, resolved_duration); + if (motion_state->cancelled(motion.token)) { + motion_owner.settle(); + return Result::failure( + ArmErrorCode::CommandRejected, + "[AuboArm] speedJ stopped by stopMotion"); + } if (ret != 0) { + motion_owner.settle(); return Result::failure(ArmErrorCode::CommandFailed, "[AuboArm] speedJ failed: ret=" + std::to_string(ret)); } + motion_owner.retainKind(); + motion_owner.settle(); return Result::success(); } catch (const std::exception& e) { return Result::failure(ArmErrorCode::CommandFailed, std::string("[AuboArm] speedJ failed: ") + e.what()); @@ -706,46 +918,38 @@ Result AuboArm::speedJ(const JointVelocityCommand& velocity, double acceleration Result AuboArm::stopJ(double acceleration) { - if (!connected_.load() || !sdk_ || !sdk_->rpc_client) { - return Result::success(); - } - try { - Result interface_result; - auto robot_interface = getPrimaryRobotInterface(sdk_->rpc_client, "stopJ", interface_result); - if (!interface_result.ok()) { - return interface_result; - } - const double resolved_acceleration = acceleration > 0.0 ? acceleration : 31.0; - const int ret = robot_interface->getMotionControl()->stopJoint(resolved_acceleration); - busy_.store(false); - if (ret != 0) { - return Result::failure(ArmErrorCode::CommandFailed, - "[AuboArm] stopJ failed: ret=" + std::to_string(ret)); - } - return Result::success(); - } catch (const std::exception& e) { - return Result::failure(ArmErrorCode::CommandFailed, std::string("[AuboArm] stopJ failed: ") + e.what()); - } + return stopMotion_(MotionStopKind::Joint, acceleration); } Result AuboArm::moveL(const CartesianPose& target, const MotionOptions& options, FrameType frame) { (void)frame; - const auto ready = ensureConnected_("moveL"); - if (!ready.ok()) { - return ready; - } - if (busy_.exchange(true)) { - return Result::failure(ArmErrorCode::RobotNotReady, "[AuboArm] arm is busy: " + id_); - } - BusyGuard busy_guard{busy_}; - try { - const auto robot_names = sdk_->rpc_client->getRobotNames(); + std::unique_lock submit_lock(mutex_); + const auto locked_ready = ensureConnected_("moveL"); + if (!locked_ready.ok()) { + return locked_ready; + } + const auto rpc_client = sdk_->rpc_client; + const auto motion_state = sdk_->motion_state; + const auto motion = motion_state->begin( + aubo_internal::MotionKind::Linear); + if (!motion.started()) { + return Result::failure( + ArmErrorCode::RobotNotReady, + "[AuboArm] moveL rejected: " + + std::string(motionStartFailure(motion.status)) + + ", id=" + id_); + } + busy_.store(true); + MotionOwnerGuard motion_owner{ + *motion_state, busy_, motion.token}; + + const auto robot_names = rpc_client->getRobotNames(); if (robot_names.empty()) { return Result::failure(ArmErrorCode::RobotNotReady, "[AuboArm] robot name list is empty"); } - auto robot_interface = sdk_->rpc_client->getRobotInterface(robot_names.front()); + auto robot_interface = rpc_client->getRobotInterface(robot_names.front()); if (!robot_interface) { return Result::failure(ArmErrorCode::RobotNotReady, "[AuboArm] robot interface is null"); } @@ -754,17 +958,29 @@ Result AuboArm::moveL(const CartesianPose& target, const MotionOptions& options, std::vector tcp_offset(6, 0.0); robot_interface->getRobotConfig()->setTcpOffset(tcp_offset); std::vector pose{target.x, target.y, target.z, target.rx, target.ry, target.rz}; + motion_owner.requireExplicitSettlement(); const int ret = motion_control->moveLine( pose, options.acceleration > 0.0 ? options.acceleration : 0.5, options.velocity > 0.0 ? options.velocity : 0.25, options.blend_radius, 0); + if (ret == arcs::common_interface::AUBO_OK) { + submit_lock.unlock(); + } else { + motion_owner.settle(); + } const auto outcome = aubo_internal::resolveMotionCommand( ret, arcs::common_interface::AUBO_OK, arcs::common_interface::AUBO_REQUEST_IGNORE, - [&robot_interface]() { return waitArrival(robot_interface); }); + [&robot_interface, motion_state, token = motion.token]() { + return waitArrival( + robot_interface, + [motion_state, token]() { + return motion_state->cancelled(token); + }); + }); switch (outcome) { case aubo_internal::MotionCommandOutcome::CompletedWithoutMotion: CMVR_LOG(DEBUG) << "[AuboArm] moveL completed without motion: sdk ret=" @@ -772,7 +988,14 @@ Result AuboArm::moveL(const CartesianPose& target, const MotionOptions& options, << arcs::common_interface::returnValue2Str(ret) << ")"; return Result::success(); case aubo_internal::MotionCommandOutcome::CompletedAfterMotion: + motion_owner.clearOnFinish(); + motion_owner.settle(); return Result::success(); + case aubo_internal::MotionCommandOutcome::Cancelled: + motion_owner.settle(); + return Result::failure( + ArmErrorCode::CommandRejected, + "[AuboArm] moveL stopped by stopMotion"); case aubo_internal::MotionCommandOutcome::SubmitFailed: return Result::failure( ArmErrorCode::CommandFailed, @@ -789,18 +1012,30 @@ Result AuboArm::moveL(const CartesianPose& target, const MotionOptions& options, Result AuboArm::speedL(const CartesianVelocity& velocity, double acceleration, double duration, FrameType frame) { - const auto ready = ensureConnected_("speedL"); - if (!ready.ok()) { - return ready; - } - if (busy_.exchange(true)) { - return Result::failure(ArmErrorCode::RobotNotReady, "[AuboArm] arm is busy: " + id_); - } - BusyGuard busy_guard{busy_}; - try { + std::unique_lock submit_lock(mutex_); + const auto locked_ready = ensureConnected_("speedL"); + if (!locked_ready.ok()) { + return locked_ready; + } + const auto rpc_client = sdk_->rpc_client; + const auto motion_state = sdk_->motion_state; + const auto motion = motion_state->begin( + aubo_internal::MotionKind::Linear, true); + if (!motion.started()) { + return Result::failure( + ArmErrorCode::RobotNotReady, + "[AuboArm] speedL rejected: " + + std::string(motionStartFailure(motion.status)) + + ", id=" + id_); + } + busy_.store(true); + MotionOwnerGuard motion_owner{ + *motion_state, busy_, motion.token}; + Result interface_result; - auto robot_interface = getPrimaryRobotInterface(sdk_->rpc_client, "speedL", interface_result); + auto robot_interface = getPrimaryRobotInterface( + rpc_client, "speedL", interface_result); if (!interface_result.ok()) { return interface_result; } @@ -820,8 +1055,10 @@ Result AuboArm::speedL(const CartesianVelocity& velocity, double acceleration, d tool_frame[0] = 0.0; tool_frame[1] = 0.0; tool_frame[2] = 0.0; - line_speed = sdk_->rpc_client->getMath()->poseTrans(tool_frame, line_speed); - angular_speed = sdk_->rpc_client->getMath()->poseTrans(tool_frame, angular_speed); + line_speed = rpc_client->getMath()->poseTrans( + tool_frame, line_speed); + angular_speed = rpc_client->getMath()->poseTrans( + tool_frame, angular_speed); } else if (frame == FrameType::User) { return Result::failure(ArmErrorCode::UnsupportedCommand, "[AuboArm] speedL User frame requires a configured user coordinate frame"); @@ -838,14 +1075,31 @@ Result AuboArm::speedL(const CartesianVelocity& velocity, double acceleration, d const double resolved_acceleration = acceleration > 0.0 ? acceleration : 1.2; const double resolved_duration = duration > 0.0 ? duration : 100.0; + motion_owner.requireExplicitSettlement(); + submit_lock.unlock(); + if (motion_state->cancelled(motion.token)) { + motion_owner.settle(); + return Result::failure( + ArmErrorCode::CommandRejected, + "[AuboArm] speedL stopped by stopMotion before submission"); + } const int ret = robot_interface->getMotionControl()->speedLine( speed, resolved_acceleration, resolved_duration); + if (motion_state->cancelled(motion.token)) { + motion_owner.settle(); + return Result::failure( + ArmErrorCode::CommandRejected, + "[AuboArm] speedL stopped by stopMotion"); + } if (ret != 0) { + motion_owner.settle(); return Result::failure(ArmErrorCode::CommandFailed, "[AuboArm] speedL failed: ret=" + std::to_string(ret)); } + motion_owner.retainKind(); + motion_owner.settle(); return Result::success(); } catch (const std::exception& e) { return Result::failure(ArmErrorCode::CommandFailed, std::string("[AuboArm] speedL failed: ") + e.what()); @@ -854,44 +1108,165 @@ Result AuboArm::speedL(const CartesianVelocity& velocity, double acceleration, d Result AuboArm::stopL(std::optional acceleration) { - if (!connected_.load() || !sdk_ || !sdk_->rpc_client) { - return Result::success(); - } - try { - Result interface_result; - auto robot_interface = getPrimaryRobotInterface(sdk_->rpc_client, "stopL", interface_result); - if (!interface_result.ok()) { - return interface_result; - } - const double resolved_acceleration = - acceleration.has_value() && *acceleration > 0.0 ? *acceleration : 10.0; - const int ret = robot_interface->getMotionControl()->stopLine(resolved_acceleration, resolved_acceleration); - busy_.store(false); - if (ret != 0) { - return Result::failure(ArmErrorCode::CommandFailed, - "[AuboArm] stopL failed: ret=" + std::to_string(ret)); - } - return Result::success(); - } catch (const std::exception& e) { - return Result::failure(ArmErrorCode::CommandFailed, std::string("[AuboArm] stopL failed: ") + e.what()); - } + return stopMotion_( + MotionStopKind::Linear, + acceleration.has_value() ? *acceleration : 0.0); } Result AuboArm::stopMotion() { - if (!connected_.load() || !sdk_ || !sdk_->rpc_client) { + return stopMotion_(MotionStopKind::Automatic, 0.0); +} + +Result AuboArm::stopMotion_( + const MotionStopKind requested_kind, + const double acceleration) +{ + if (!connected_.load()) { return Result::success(); } + try { - const auto robot_names = sdk_->rpc_client->getRobotNames(); - if (robot_names.empty()) { + std::unique_lock submit_lock(mutex_); + if (!connected_.load() || !sdk_ || !sdk_->rpc_client) { return Result::success(); } - auto robot_interface = sdk_->rpc_client->getRobotInterface(robot_names.front()); - if (robot_interface) { - robot_interface->getMotionControl()->stopMove(true, true); + + const auto motion_state = sdk_->motion_state; + aubo_internal::MotionKind forced_kind = + aubo_internal::MotionKind::None; + if (requested_kind == MotionStopKind::Joint) { + forced_kind = aubo_internal::MotionKind::Joint; + } else if (requested_kind == MotionStopKind::Linear) { + forced_kind = aubo_internal::MotionKind::Linear; + } + + const auto stop_request = + motion_state->beginStop(forced_kind); + if (!stop_request.started()) { + return Result::failure( + ArmErrorCode::RobotNotReady, + "[AuboArm] stopMotion rejected: another stop operation is in progress"); + } + busy_.store(true); + StopStateGuard stop_state_guard{ + *motion_state, busy_}; + + Result interface_result; + auto robot_interface = getPrimaryRobotInterface( + sdk_->rpc_client, "stopMotion", interface_result); + if (!interface_result.ok()) { + return interface_result; + } + + auto motion_control = robot_interface->getMotionControl(); + auto robot_state = robot_interface->getRobotState(); + int last_exec_id = motion_control->getExecId(); + bool last_steady = robot_state->isSteady(); + const bool requires_vendor_stop = + stop_request.tracked_motion || + last_exec_id != -1 || + !last_steady; + + if (requires_vendor_stop && + stop_request.kind == aubo_internal::MotionKind::None) { + return Result::failure( + ArmErrorCode::CommandFailed, + "[AuboArm] stopMotion failed: controller is moving but the " + "active direct-motion type is unknown"); + } + + const auto issue_vendor_stop = [&]() -> Result { + int ret = 0; + if (stop_request.kind == aubo_internal::MotionKind::Joint) { + const double resolved_acceleration = + acceleration > 0.0 ? acceleration : 31.0; + ret = motion_control->stopJoint(resolved_acceleration); + } else { + const double resolved_acceleration = + acceleration > 0.0 ? acceleration : 10.0; + ret = motion_control->stopLine( + resolved_acceleration, resolved_acceleration); + } + if (ret != arcs::common_interface::AUBO_OK) { + return Result::failure( + ArmErrorCode::CommandFailed, + "[AuboArm] stopMotion failed: " + + std::string(motionKindName(stop_request.kind)) + + " stop sdk ret=" + std::to_string(ret) + + " (" + + arcs::common_interface::returnValue2Str(ret) + + ")"); + } + return Result::success(); + }; + + if (requires_vendor_stop) { + const auto stop_result = issue_vendor_stop(); + if (!stop_result.ok()) { + return stop_result; + } + } + + constexpr auto kStopTimeout = std::chrono::seconds(5); + constexpr auto kPollInterval = std::chrono::milliseconds(50); + constexpr int kStableSamples = 3; + const auto deadline = + std::chrono::steady_clock::now() + kStopTimeout; + int stable_samples = 0; + bool idle_since_stop = last_exec_id == -1 && last_steady; + bool owner_active = + motion_state->ownerActive(stop_request.active_token); + while (std::chrono::steady_clock::now() < deadline) { + last_exec_id = motion_control->getExecId(); + last_steady = robot_state->isSteady(); + owner_active = + motion_state->ownerActive(stop_request.active_token); + const bool physically_idle = + last_exec_id == -1 && last_steady; + if (!physically_idle && idle_since_stop) { + if (stop_request.kind == + aubo_internal::MotionKind::None) { + return Result::failure( + ArmErrorCode::CommandFailed, + "[AuboArm] stopMotion failed: motion started " + "after an idle observation but its type is unknown"); + } + const auto stop_result = issue_vendor_stop(); + if (!stop_result.ok()) { + return stop_result; + } + idle_since_stop = false; + } + if (physically_idle && !owner_active) { + if (++stable_samples >= kStableSamples) { + break; + } + } else { + stable_samples = 0; + } + if (physically_idle) { + idle_since_stop = true; + } + std::this_thread::sleep_for(kPollInterval); + } + if (stable_samples < kStableSamples) { + return Result::failure( + ArmErrorCode::Timeout, + "[AuboArm] stopMotion failed: timeout waiting for " + + std::string(motionKindName(stop_request.kind)) + + " motion to stop, generation=" + + std::to_string(stop_request.active_token.generation) + + ", exec_id=" + std::to_string(last_exec_id) + + ", steady=" + (last_steady ? "true" : "false") + + ", owner_active=" + + (owner_active ? "true" : "false")); + } + if (!stop_state_guard.complete()) { + return Result::failure( + ArmErrorCode::CommandFailed, + "[AuboArm] stopMotion failed: cancelled motion handler is still active"); } - busy_.store(false); return Result::success(); } catch (const std::exception& e) { return Result::failure(ArmErrorCode::CommandFailed, std::string("[AuboArm] stopMotion failed: ") + e.what()); @@ -1226,7 +1601,6 @@ Result AuboArm::stopProgram() } try { const int ret = sdk_->rpc_client->getRuntimeMachine()->abort(); - busy_.store(false); if (ret != 0) { return Result::failure(ArmErrorCode::CommandFailed, "[AuboArm] stopProgram failed: ret=" + std::to_string(ret)); diff --git a/cmvr-es/devices/arm/aubo_arm/aubo_arm.h b/cmvr-es/devices/arm/aubo_arm/aubo_arm.h index 28a19663..6f05af2d 100644 --- a/cmvr-es/devices/arm/aubo_arm/aubo_arm.h +++ b/cmvr-es/devices/arm/aubo_arm/aubo_arm.h @@ -80,12 +80,19 @@ public: CartesianPose fk(const std::string& base_link, const std::string& ee_link) override; CartesianPose fk(bool is_tcp = true) override; CartesianVelocity getSpeedLCommandTwistBase() const override { return {}; } - bool busy() const override { return busy_.load(); } + bool busy() const override; private: + enum class MotionStopKind { + Automatic, + Joint, + Linear, + }; + Result unsupported_(const std::string& name) const; bool validDof_(std::size_t size, std::string& error) const; Result ensureConnected_(const std::string& context) const; + Result stopMotion_(MotionStopKind kind, double acceleration); struct SdkState; diff --git a/cmvr-es/devices/arm/aubo_arm/aubo_motion_result.h b/cmvr-es/devices/arm/aubo_arm/aubo_motion_result.h index d2cba859..d1c92fb0 100644 --- a/cmvr-es/devices/arm/aubo_arm/aubo_motion_result.h +++ b/cmvr-es/devices/arm/aubo_arm/aubo_motion_result.h @@ -6,10 +6,17 @@ namespace cmvr::device::aubo_internal { enum class MotionCommandOutcome { CompletedWithoutMotion, CompletedAfterMotion, + Cancelled, SubmitFailed, CompletionFailed, }; +enum class MotionWaitResult { + Completed, + Cancelled, + Failed, +}; + template MotionCommandOutcome resolveMotionCommand( const int return_code, @@ -23,7 +30,11 @@ MotionCommandOutcome resolveMotionCommand( if (return_code != success_code) { return MotionCommandOutcome::SubmitFailed; } - if (wait_for_completion() != 0) { + const auto wait_result = wait_for_completion(); + if (wait_result == MotionWaitResult::Cancelled) { + return MotionCommandOutcome::Cancelled; + } + if (wait_result != MotionWaitResult::Completed) { return MotionCommandOutcome::CompletionFailed; } return MotionCommandOutcome::CompletedAfterMotion; diff --git a/cmvr-es/devices/arm/aubo_arm/aubo_motion_state.h b/cmvr-es/devices/arm/aubo_arm/aubo_motion_state.h new file mode 100644 index 00000000..76409c90 --- /dev/null +++ b/cmvr-es/devices/arm/aubo_arm/aubo_motion_state.h @@ -0,0 +1,265 @@ +#ifndef CMVR_ES_AUBO_MOTION_STATE_H +#define CMVR_ES_AUBO_MOTION_STATE_H + +#include +#include +#include +#include +#include + +namespace cmvr::device::aubo_internal { + +enum class MotionKind { + None, + Joint, + Linear, +}; + +struct MotionToken { + std::uint64_t generation{0}; + MotionKind kind{MotionKind::None}; + + bool valid() const noexcept + { + return generation != 0 && kind != MotionKind::None; + } +}; + +enum class MotionStartStatus { + Started, + Invalid, + Busy, + Stopping, + Blocked, +}; + +struct MotionStartResult { + MotionStartStatus status{MotionStartStatus::Busy}; + MotionToken token; + + bool started() const noexcept + { + return status == MotionStartStatus::Started; + } +}; + +enum class MotionFinishMode { + RestorePrevious, + Clear, + Retain, +}; + +enum class StopStartStatus { + Started, + AlreadyStopping, +}; + +struct StopRequest { + StopStartStatus status{StopStartStatus::AlreadyStopping}; + MotionKind kind{MotionKind::None}; + MotionToken active_token; + bool tracked_motion{false}; + + bool started() const noexcept + { + return status == StopStartStatus::Started; + } +}; + +// Tracks one direct AUBO motion owner. MoveJ/MoveL submissions are serialized +// through the vendor call. Speed calls release the outer mutex before their +// potentially blocking SDK call, so the generation cancellation below also +// closes the stop-vs-speed-submission race. +class MotionState final { +public: + MotionStartResult begin( + const MotionKind kind, + const bool replace_retained_same_kind = false) + { + std::lock_guard lock(mutex_); + if (kind == MotionKind::None) { + return {MotionStartStatus::Invalid, {}}; + } + if (stop_in_progress_) { + return {MotionStartStatus::Stopping, {}}; + } + if (blocked_) { + return {MotionStartStatus::Blocked, {}}; + } + if (owner_active_) { + return {MotionStartStatus::Busy, {}}; + } + if (last_kind_ != MotionKind::None && + (!replace_retained_same_kind || last_kind_ != kind)) { + return {MotionStartStatus::Busy, {}}; + } + + MotionToken token{++next_generation_, kind}; + owner_active_ = true; + active_token_ = token; + previous_kind_ = last_kind_; + return {MotionStartStatus::Started, token}; + } + + void finish( + const MotionToken& token, + const MotionFinishMode mode = MotionFinishMode::RestorePrevious) + { + std::lock_guard lock(mutex_); + if (!owner_active_ || + active_token_.generation != token.generation) { + return; + } + + owner_active_ = false; + active_token_ = {}; + if (!stop_in_progress_ && !blocked_) { + if (mode == MotionFinishMode::Retain) { + last_kind_ = token.kind; + } else if (mode == MotionFinishMode::Clear) { + last_kind_ = MotionKind::None; + } else { + last_kind_ = previous_kind_; + } + } + previous_kind_ = MotionKind::None; + owner_finished_cv_.notify_all(); + } + + void failMotion(const MotionToken& token) + { + std::lock_guard lock(mutex_); + if (!owner_active_ || + active_token_.generation != token.generation) { + return; + } + owner_active_ = false; + active_token_ = {}; + last_kind_ = token.kind; + previous_kind_ = MotionKind::None; + blocked_ = true; + owner_finished_cv_.notify_all(); + } + + StopRequest beginStop( + const MotionKind requested_kind = MotionKind::None) + { + std::lock_guard lock(mutex_); + if (stop_in_progress_) { + return {}; + } + + stop_in_progress_ = true; + const MotionToken active = owner_active_ + ? active_token_ + : MotionToken{}; + // A successful speedJoint/speedLine call may keep the controller in + // velocity mode after the SDK function returns, even when the target + // velocity is zero and the robot currently reports steady. Retain that + // motion kind until a typed stop has been acknowledged. + const bool tracked_motion = + active.valid() || last_kind_ != MotionKind::None; + if (active.valid()) { + cancelled_generation_ = std::max( + cancelled_generation_, active.generation); + } + MotionKind kind = MotionKind::None; + if (active.valid()) { + kind = active.kind; + } else if (last_kind_ != MotionKind::None) { + kind = last_kind_; + } else if (requested_kind != MotionKind::None) { + kind = requested_kind; + } else { + kind = last_kind_; + } + if (kind != MotionKind::None) { + last_kind_ = kind; + } + return { + StopStartStatus::Started, + kind, + active, + tracked_motion}; + } + + bool cancelled(const MotionToken& token) const + { + std::lock_guard lock(mutex_); + return token.valid() && + token.generation <= cancelled_generation_; + } + + bool waitForOwnerExit( + const MotionToken& token, + const std::chrono::milliseconds timeout) + { + if (!token.valid()) { + return true; + } + std::unique_lock lock(mutex_); + return owner_finished_cv_.wait_for( + lock, + timeout, + [this, &token]() { + return !owner_active_ || + active_token_.generation != token.generation; + }); + } + + bool ownerActive(const MotionToken& token) const + { + if (!token.valid()) { + return false; + } + std::lock_guard lock(mutex_); + return owner_active_ && + active_token_.generation == token.generation; + } + + bool completeStop() + { + std::lock_guard lock(mutex_); + if (owner_active_) { + return false; + } + stop_in_progress_ = false; + blocked_ = false; + active_token_ = {}; + last_kind_ = MotionKind::None; + previous_kind_ = MotionKind::None; + owner_finished_cv_.notify_all(); + return true; + } + + void failStop() + { + std::lock_guard lock(mutex_); + stop_in_progress_ = false; + blocked_ = true; + owner_finished_cv_.notify_all(); + } + + bool busy() const + { + std::lock_guard lock(mutex_); + return owner_active_ || stop_in_progress_ || blocked_ || + last_kind_ != MotionKind::None; + } + +private: + mutable std::mutex mutex_; + std::condition_variable owner_finished_cv_; + std::uint64_t next_generation_{0}; + std::uint64_t cancelled_generation_{0}; + MotionToken active_token_; + MotionKind last_kind_{MotionKind::None}; + MotionKind previous_kind_{MotionKind::None}; + bool owner_active_{false}; + bool stop_in_progress_{false}; + bool blocked_{false}; +}; + +} // namespace cmvr::device::aubo_internal + +#endif // CMVR_ES_AUBO_MOTION_STATE_H diff --git a/cmvr-es/devices/arm/aubo_arm/tests/aubo_arm_motion_result_test.cpp b/cmvr-es/devices/arm/aubo_arm/tests/aubo_arm_motion_result_test.cpp index da2c16ee..fb1656e5 100644 --- a/cmvr-es/devices/arm/aubo_arm/tests/aubo_arm_motion_result_test.cpp +++ b/cmvr-es/devices/arm/aubo_arm/tests/aubo_arm_motion_result_test.cpp @@ -18,6 +18,7 @@ namespace { int main() { using cmvr::device::aubo_internal::MotionCommandOutcome; + using cmvr::device::aubo_internal::MotionWaitResult; using cmvr::device::aubo_internal::resolveMotionCommand; constexpr int success_code = 0; @@ -26,7 +27,7 @@ int main() int wait_calls = 0; const auto wait_succeeded = [&wait_calls]() { ++wait_calls; - return 0; + return MotionWaitResult::Completed; }; CHECK_TRUE(resolveMotionCommand( success_code, @@ -57,7 +58,7 @@ int main() wait_calls = 0; const auto wait_failed = [&wait_calls]() { ++wait_calls; - return -1; + return MotionWaitResult::Failed; }; CHECK_TRUE(resolveMotionCommand( success_code, @@ -66,5 +67,17 @@ int main() wait_failed) == MotionCommandOutcome::CompletionFailed); CHECK_TRUE(wait_calls == 1); + wait_calls = 0; + const auto wait_cancelled = [&wait_calls]() { + ++wait_calls; + return MotionWaitResult::Cancelled; + }; + CHECK_TRUE(resolveMotionCommand( + success_code, + success_code, + request_ignore_code, + wait_cancelled) == MotionCommandOutcome::Cancelled); + CHECK_TRUE(wait_calls == 1); + return 0; } diff --git a/cmvr-es/devices/arm/aubo_arm/tests/aubo_motion_state_test.cpp b/cmvr-es/devices/arm/aubo_arm/tests/aubo_motion_state_test.cpp new file mode 100644 index 00000000..3bb12777 --- /dev/null +++ b/cmvr-es/devices/arm/aubo_arm/tests/aubo_motion_state_test.cpp @@ -0,0 +1,127 @@ +#include "devices/arm/aubo_arm/aubo_motion_state.h" + +#include +#include + +namespace { + +#define CHECK_TRUE(condition) \ + do { \ + if (!(condition)) { \ + std::cerr << "CHECK_TRUE failed at line " << __LINE__ << ": " \ + << #condition << std::endl; \ + return 1; \ + } \ + } while (false) + +} // namespace + +int main() +{ + using namespace cmvr::device::aubo_internal; + + MotionState state; + CHECK_TRUE(state.begin(MotionKind::None).status == + MotionStartStatus::Invalid); + const auto joint = state.begin(MotionKind::Joint); + CHECK_TRUE(joint.started()); + CHECK_TRUE(state.busy()); + CHECK_TRUE(state.begin(MotionKind::Linear).status == + MotionStartStatus::Busy); + + const auto stop_joint = state.beginStop(); + CHECK_TRUE(stop_joint.started()); + CHECK_TRUE(stop_joint.kind == MotionKind::Joint); + CHECK_TRUE(stop_joint.active_token.generation == + joint.token.generation); + CHECK_TRUE(stop_joint.tracked_motion); + CHECK_TRUE(state.cancelled(joint.token)); + CHECK_TRUE(state.beginStop().status == + StopStartStatus::AlreadyStopping); + CHECK_TRUE(state.begin(MotionKind::Linear).status == + MotionStartStatus::Stopping); + CHECK_TRUE(!state.waitForOwnerExit( + joint.token, std::chrono::milliseconds(1))); + CHECK_TRUE(!state.completeStop()); + + state.finish(joint.token); + CHECK_TRUE(state.waitForOwnerExit( + joint.token, std::chrono::milliseconds(1))); + CHECK_TRUE(state.completeStop()); + CHECK_TRUE(!state.busy()); + + const auto linear = state.begin(MotionKind::Linear); + CHECK_TRUE(linear.started()); + CHECK_TRUE(!state.cancelled(linear.token)); + // A delayed guard from the cancelled command must not release a newer one. + state.finish(joint.token); + CHECK_TRUE(state.busy()); + state.finish(linear.token, MotionFinishMode::Clear); + CHECK_TRUE(!state.busy()); + + const auto speed_joint = state.begin(MotionKind::Joint); + CHECK_TRUE(speed_joint.started()); + state.finish(speed_joint.token, MotionFinishMode::Retain); + CHECK_TRUE(state.busy()); + CHECK_TRUE(state.begin(MotionKind::Linear).status == + MotionStartStatus::Busy); + const auto rejected_speed_update = + state.begin(MotionKind::Joint, true); + CHECK_TRUE(rejected_speed_update.started()); + state.finish( + rejected_speed_update.token, + MotionFinishMode::RestorePrevious); + const auto stop_speed = state.beginStop(); + CHECK_TRUE(stop_speed.started()); + CHECK_TRUE(stop_speed.kind == MotionKind::Joint); + CHECK_TRUE(stop_speed.tracked_motion); + CHECK_TRUE(state.completeStop()); + CHECK_TRUE(!state.busy()); + + const auto idle_stop = state.beginStop(); + CHECK_TRUE(idle_stop.kind == MotionKind::None); + CHECK_TRUE(!idle_stop.tracked_motion); + state.failStop(); + const auto retry_idle_stop = state.beginStop(); + CHECK_TRUE(retry_idle_stop.kind == MotionKind::None); + CHECK_TRUE(!retry_idle_stop.tracked_motion); + CHECK_TRUE(state.completeStop()); + + const auto mismatched_stop_motion = state.begin(MotionKind::Linear); + CHECK_TRUE(mismatched_stop_motion.started()); + const auto mismatched_stop = state.beginStop(MotionKind::Joint); + CHECK_TRUE(mismatched_stop.kind == MotionKind::Linear); + state.finish(mismatched_stop_motion.token); + CHECK_TRUE(state.completeStop()); + + const auto uncertain_motion = state.begin(MotionKind::Joint); + CHECK_TRUE(uncertain_motion.started()); + state.failMotion(uncertain_motion.token); + CHECK_TRUE(state.busy()); + CHECK_TRUE(state.begin(MotionKind::Linear).status == + MotionStartStatus::Blocked); + const auto stop_uncertain = state.beginStop(); + CHECK_TRUE(stop_uncertain.kind == MotionKind::Joint); + CHECK_TRUE(stop_uncertain.tracked_motion); + CHECK_TRUE(state.completeStop()); + + const auto failed_stop_motion = state.begin(MotionKind::Linear); + CHECK_TRUE(failed_stop_motion.started()); + const auto failed_stop = state.beginStop(); + CHECK_TRUE(failed_stop.kind == MotionKind::Linear); + state.failStop(); + CHECK_TRUE(state.busy()); + CHECK_TRUE(state.begin(MotionKind::Joint).status == + MotionStartStatus::Blocked); + state.finish(failed_stop_motion.token); + + const auto retry = state.beginStop(); + CHECK_TRUE(retry.started()); + CHECK_TRUE(retry.kind == MotionKind::Linear); + CHECK_TRUE(state.completeStop()); + const auto recovered = state.begin(MotionKind::Joint); + CHECK_TRUE(recovered.started()); + state.finish(recovered.token, MotionFinishMode::Clear); + + return 0; +} diff --git a/cmvr-es/manager/control_authority/include/control_authority_manager.h b/cmvr-es/manager/control_authority/include/control_authority_manager.h index 4e29f24f..b3547c2f 100644 --- a/cmvr-es/manager/control_authority/include/control_authority_manager.h +++ b/cmvr-es/manager/control_authority/include/control_authority_manager.h @@ -41,6 +41,14 @@ public: const std::string& resource_id, const std::string& owner_id, Duration ttl); + + // Atomically invalidates a normal control lease and joins a safety + // barrier. Each safety caller receives an independent token; normal + // control remains blocked until the last safety token is released. + ControlAcquireResult preemptAcquire( + const std::string& resource_id, + const std::string& owner_id, + Duration ttl); bool renew(const ControlLeaseToken& token, Duration ttl); bool validate(const ControlLeaseToken& token); void release(const ControlLeaseToken& token) noexcept; @@ -60,6 +68,8 @@ private: std::string owner_id; std::uint64_t generation{0}; std::chrono::steady_clock::time_point deadline; + bool preemptible{true}; + std::unordered_map safety_holders; }; bool expired_(const Entry& entry) const noexcept; diff --git a/cmvr-es/manager/control_authority/src/control_authority_manager.cpp b/cmvr-es/manager/control_authority/src/control_authority_manager.cpp index e52d3353..53facdd8 100644 --- a/cmvr-es/manager/control_authority/src/control_authority_manager.cpp +++ b/cmvr-es/manager/control_authority/src/control_authority_manager.cpp @@ -42,7 +42,50 @@ ControlAcquireResult ControlAuthorityManager::tryAcquire( Entry{ owner_id, token.generation, - std::chrono::steady_clock::now() + ttl}); + std::chrono::steady_clock::now() + ttl, + true, + {}}); + return {true, std::move(token), {}}; +} + +ControlAcquireResult ControlAuthorityManager::preemptAcquire( + const std::string& resource_id, + const std::string& owner_id, + const Duration ttl) +{ + if (resource_id.empty() || owner_id.empty() || + ttl <= Duration::zero()) { + return {false, {}, "invalid control barrier request"}; + } + + std::lock_guard lock(mutex_); + const auto existing = entries_.find(resource_id); + if (existing != entries_.end()) { + if (!expired_(existing->second) && + !existing->second.preemptible) { + ControlLeaseToken token; + token.resource_id = resource_id; + token.owner_id = owner_id; + token.generation = ++next_generation_; + existing->second.safety_holders.emplace( + token.generation, token.owner_id); + return {true, std::move(token), {}}; + } + entries_.erase(existing); + } + + ControlLeaseToken token; + token.resource_id = resource_id; + token.owner_id = owner_id; + token.generation = ++next_generation_; + entries_.emplace( + resource_id, + Entry{ + owner_id, + token.generation, + std::chrono::steady_clock::time_point::max(), + false, + {{token.generation, owner_id}}}); return {true, std::move(token), {}}; } @@ -55,15 +98,22 @@ bool ControlAuthorityManager::renew( } std::lock_guard lock(mutex_); const auto found = entries_.find(token.resource_id); - if (found == entries_.end() || - expired_(found->second) || - found->second.owner_id != token.owner_id || - found->second.generation != token.generation) { + if (found == entries_.end() || expired_(found->second)) { if (found != entries_.end() && expired_(found->second)) { entries_.erase(found); } return false; } + if (!found->second.preemptible) { + const auto holder = + found->second.safety_holders.find(token.generation); + return holder != found->second.safety_holders.end() && + holder->second == token.owner_id; + } + if (found->second.owner_id != token.owner_id || + found->second.generation != token.generation) { + return false; + } found->second.deadline = std::chrono::steady_clock::now() + ttl; return true; @@ -84,6 +134,12 @@ bool ControlAuthorityManager::validate( entries_.erase(found); return false; } + if (!found->second.preemptible) { + const auto holder = + found->second.safety_holders.find(token.generation); + return holder != found->second.safety_holders.end() && + holder->second == token.owner_id; + } return found->second.owner_id == token.owner_id && found->second.generation == token.generation; } @@ -97,9 +153,22 @@ void ControlAuthorityManager::release( try { std::lock_guard lock(mutex_); const auto found = entries_.find(token.resource_id); - if (found != entries_.end() && - found->second.owner_id == token.owner_id && - found->second.generation == token.generation) { + if (found == entries_.end()) { + return; + } + if (!found->second.preemptible) { + const auto holder = + found->second.safety_holders.find(token.generation); + if (holder == found->second.safety_holders.end() || + holder->second != token.owner_id) { + return; + } + found->second.safety_holders.erase(holder); + if (found->second.safety_holders.empty()) { + entries_.erase(found); + } + } else if (found->second.owner_id == token.owner_id && + found->second.generation == token.generation) { entries_.erase(found); } } catch (...) { diff --git a/cmvr-es/manager/control_authority/tests/control_authority_manager_test.cpp b/cmvr-es/manager/control_authority/tests/control_authority_manager_test.cpp index b6b2b965..855d4853 100644 --- a/cmvr-es/manager/control_authority/tests/control_authority_manager_test.cpp +++ b/cmvr-es/manager/control_authority/tests/control_authority_manager_test.cpp @@ -61,6 +61,38 @@ TEST_F(ControlAuthorityManagerTest, StaleGenerationCannotReleaseNewLease) EXPECT_TRUE(manager.validate(current.token)); } +TEST_F(ControlAuthorityManagerTest, + SafetyBarrierAtomicallyPreemptsControlAndRejectsOtherOwners) +{ + auto& manager = ControlAuthorityManager::instance(); + const auto control = + manager.tryAcquire("right_arm", "move-session", 100ms); + ASSERT_TRUE(control.acquired); + + const auto barrier = manager.preemptAcquire( + "right_arm", "stop-operation", 100ms); + ASSERT_TRUE(barrier.acquired) << barrier.detail; + EXPECT_FALSE(manager.validate(control.token)); + EXPECT_TRUE(manager.validate(barrier.token)); + + const auto move_during_stop = + manager.tryAcquire("right_arm", "new-move", 100ms); + EXPECT_FALSE(move_during_stop.acquired); + const auto second_stop = manager.preemptAcquire( + "right_arm", "second-stop", 100ms); + ASSERT_TRUE(second_stop.acquired) << second_stop.detail; + + manager.release(control.token); + EXPECT_TRUE(manager.validate(barrier.token)); + manager.release(barrier.token); + EXPECT_TRUE(manager.validate(second_stop.token)); + EXPECT_FALSE( + manager.tryAcquire("right_arm", "new-move", 100ms) + .acquired); + manager.release(second_stop.token); + EXPECT_FALSE(manager.isLeased("right_arm")); +} + TEST_F(ControlAuthorityManagerTest, ExpiryAndRenewUseMonotonicLocalTime) { auto& manager = ControlAuthorityManager::instance(); diff --git a/cmvr-es/service/grpc/src/grpc_arm_service.cpp b/cmvr-es/service/grpc/src/grpc_arm_service.cpp index b1567f00..a8fb5622 100644 --- a/cmvr-es/service/grpc/src/grpc_arm_service.cpp +++ b/cmvr-es/service/grpc/src/grpc_arm_service.cpp @@ -126,11 +126,16 @@ grpc::Status setDeviceNotFound(Response* response, const std::string& device_id) grpc::Status setControlLeaseConflict( api::CommandHeader_Feedback* response, - const std::string& device_id) + const std::string& device_id, + const std::string& detail) { - const std::string message = + std::string message = "RobotArm control is leased by another active control operation: " + device_id; + if (!detail.empty()) { + CMVR_LOG(WARNING) << "[gRPCArmServiceImpl] control lease conflict, id=" + << device_id << ", detail=" << detail; + } fillFeedback(response, false, message); return grpc::Status( grpc::StatusCode::FAILED_PRECONDITION, message); @@ -139,17 +144,19 @@ grpc::Status setControlLeaseConflict( template grpc::Status setControlLeaseConflict( Response* response, - const std::string& device_id) + const std::string& device_id, + const std::string& detail) { return setControlLeaseConflict( - response->mutable_header(), device_id); + response->mutable_header(), device_id, detail); } class ScopedUnaryControlLease final { public: ScopedUnaryControlLease( const std::string& device_id, - const char* operation) + const char* operation, + const bool preemptive = false) : manager_(control::ControlAuthorityManager::instance()) { static std::atomic sequence{0}; @@ -159,14 +166,15 @@ public: sequence.fetch_add( 1U, std::memory_order_relaxed) + 1U); - auto acquired = manager_.tryAcquire( - device_id, - owner, - std::chrono::duration_cast< - control::ControlAuthorityManager::Duration>( - std::chrono::hours(24))); + const auto ttl = std::chrono::duration_cast< + control::ControlAuthorityManager::Duration>( + std::chrono::hours(24)); + auto acquired = preemptive + ? manager_.preemptAcquire(device_id, owner, ttl) + : manager_.tryAcquire(device_id, owner, ttl); acquired_ = acquired.acquired; token_ = std::move(acquired.token); + detail_ = std::move(acquired.detail); } ~ScopedUnaryControlLease() @@ -175,10 +183,12 @@ public: } bool acquired() const noexcept { return acquired_; } + const std::string& detail() const noexcept { return detail_; } private: control::ControlAuthorityManager& manager_; control::ControlLeaseToken token_; + std::string detail_; bool acquired_{false}; }; @@ -199,10 +209,12 @@ grpc::Status gRPCArmServiceImpl::torqueOff(grpc::ServerContext*, if (!arm) { return setDeviceNotFound(response, device_id); } - // A safety-disable command preempts any network teleoperation lease. - // The teleoperation executor must fail its next renew before it can - // dispatch another setpoint. - control::ControlAuthorityManager::instance().revoke(device_id); + ScopedUnaryControlLease control_barrier( + device_id, "torqueOff", true); + if (!control_barrier.acquired()) { + return setControlLeaseConflict( + response, device_id, control_barrier.detail()); + } const auto result = arm->torqueOff(); fillFeedback(response, result.ok(), result.ok() ? "" : result.message); if (result.ok()) { @@ -228,7 +240,8 @@ grpc::Status gRPCArmServiceImpl::torqueOn(grpc::ServerContext*, ScopedUnaryControlLease control_lease( device_id, "torqueOn"); if (!control_lease.acquired()) { - return setControlLeaseConflict(response, device_id); + return setControlLeaseConflict( + response, device_id, control_lease.detail()); } const auto result = arm->torqueOn(); fillFeedback(response, result.ok(), result.ok() ? "" : result.message); @@ -255,7 +268,8 @@ grpc::Status gRPCArmServiceImpl::moveJ(grpc::ServerContext*, ScopedUnaryControlLease control_lease( device_id, "moveJ"); if (!control_lease.acquired()) { - return setControlLeaseConflict(response, device_id); + return setControlLeaseConflict( + response, device_id, control_lease.detail()); } const auto result = arm->moveJ(toJointPositionCommand(request->target()), toMotionOptions(request->options())); @@ -283,7 +297,8 @@ grpc::Status gRPCArmServiceImpl::moveL(grpc::ServerContext*, ScopedUnaryControlLease control_lease( device_id, "moveL"); if (!control_lease.acquired()) { - return setControlLeaseConflict(response, device_id); + return setControlLeaseConflict( + response, device_id, control_lease.detail()); } const auto result = arm->moveL(toCartesianPose(request->target()), toMotionOptions(request->options()), @@ -312,7 +327,8 @@ grpc::Status gRPCArmServiceImpl::speedJ(grpc::ServerContext*, ScopedUnaryControlLease control_lease( device_id, "speedJ"); if (!control_lease.acquired()) { - return setControlLeaseConflict(response, device_id); + return setControlLeaseConflict( + response, device_id, control_lease.detail()); } const auto result = arm->speedJ(toJointVelocityCommand(request->velocity()), request->acceleration(), @@ -343,7 +359,8 @@ grpc::Status gRPCArmServiceImpl::speedL(grpc::ServerContext*, ScopedUnaryControlLease control_lease( device_id, "speedL"); if (!control_lease.acquired()) { - return setControlLeaseConflict(response, device_id); + return setControlLeaseConflict( + response, device_id, control_lease.detail()); } const auto result = arm->speedL(toCartesianVelocity(request->velocity()), request->acceleration(), @@ -375,7 +392,8 @@ grpc::Status gRPCArmServiceImpl::servoJ(grpc::ServerContext*, ScopedUnaryControlLease control_lease( device_id, "servoJ"); if (!control_lease.acquired()) { - return setControlLeaseConflict(response, device_id); + return setControlLeaseConflict( + response, device_id, control_lease.detail()); } const auto result = arm->servoJ(toJointPositionCommand(request->target())); if (result.ok()) { @@ -399,7 +417,12 @@ grpc::Status gRPCArmServiceImpl::stopMotion(grpc::ServerContext*, if (!arm) { return setDeviceNotFound(response, device_id); } - control::ControlAuthorityManager::instance().revoke(device_id); + ScopedUnaryControlLease control_barrier( + device_id, "stopMotion", true); + if (!control_barrier.acquired()) { + return setControlLeaseConflict( + response, device_id, control_barrier.detail()); + } const auto result = arm->stopMotion(); fillFeedback(response, result.ok(), result.ok() ? "" : result.message); if (result.ok()) { @@ -480,7 +503,8 @@ grpc::Status gRPCArmServiceImpl::calibrateZeroQ(grpc::ServerContext*, ScopedUnaryControlLease control_lease( device_id, "calibrateZeroQ"); if (!control_lease.acquired()) { - return setControlLeaseConflict(response, device_id); + return setControlLeaseConflict( + response, device_id, control_lease.detail()); } const auto result = arm->calibrateZeroQ(request->joint_name()); if (result.ok()) { @@ -557,7 +581,8 @@ grpc::Status gRPCArmServiceImpl::clearFault(grpc::ServerContext *context, ScopedUnaryControlLease control_lease( device_id, "clearFault"); if (!control_lease.acquired()) { - return setControlLeaseConflict(response, device_id); + return setControlLeaseConflict( + response, device_id, control_lease.detail()); } const auto result = arm->clearFault(); fillFeedback(response, result.ok(), result.ok() ? "" : result.message); diff --git a/cmvr-es/service/grpc/src/grpc_system_service.cpp b/cmvr-es/service/grpc/src/grpc_system_service.cpp index 908f4c91..87c592cc 100644 --- a/cmvr-es/service/grpc/src/grpc_system_service.cpp +++ b/cmvr-es/service/grpc/src/grpc_system_service.cpp @@ -4,16 +4,65 @@ #include "../include/grpc_system_service.h" +#include #include #include +#include +#include +#include #include "common/base/logging/logger.h" +#include "manager/control_authority/include/control_authority_manager.h" using namespace cmvr::device; using namespace cmvr::service; namespace { +class ScopedControlBarrierSet final { +public: + ~ScopedControlBarrierSet() + { + auto& authority = + cmvr::control::ControlAuthorityManager::instance(); + for (const auto& token : tokens_) { + authority.release(token); + } + } + + bool acquire(const std::string& device_id, std::string& detail) + { + static std::atomic sequence{0}; + const std::string owner = + "grpc-system:stop-all:" + + std::to_string( + sequence.fetch_add(1U, std::memory_order_relaxed) + 1U); + const auto result = + cmvr::control::ControlAuthorityManager::instance() + .preemptAcquire( + device_id, + owner, + std::chrono::duration_cast< + cmvr::control::ControlAuthorityManager::Duration>( + std::chrono::hours(24))); + if (!result.acquired) { + detail = result.detail; + return false; + } + try { + tokens_.push_back(result.token); + } catch (...) { + cmvr::control::ControlAuthorityManager::instance().release( + result.token); + throw; + } + return true; + } + +private: + std::vector tokens_; +}; + std::uint64_t unixTimeMs() noexcept { const auto elapsed = std::chrono::duration_cast( @@ -239,6 +288,22 @@ grpc::Status gRPCSystemServiceImpl::StopAll(grpc::ServerContext* context, const cmvr::api::StopAllCommand_Request* request, cmvr::api::StopAllCommand_Feedback* response) { try { + const auto snapshot = dmgr_.snapshot(); + ScopedControlBarrierSet control_barriers; + for (const auto& device : snapshot.devices) { + if (device.kind == cmvr::device::DeviceKind::Arm) { + std::string detail; + if (!control_barriers.acquire(device.id, detail)) { + CMVR_LOG(WARNING) + << "[gRPCSystemServiceImpl] (StopAll): failed to " + "acquire arm safety barrier, id=" + << device.id << ", detail=" << detail; + throw std::runtime_error( + "StopAll could not acquire the RobotArm safety " + "barrier: " + device.id); + } + } + } dmgr_.stop(); response->mutable_header()->set_success(true); setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); diff --git a/cmvr-es/service/grpc/tests/grpc_arm_service_test.cpp b/cmvr-es/service/grpc/tests/grpc_arm_service_test.cpp index 57c9d060..f5b2c46f 100644 --- a/cmvr-es/service/grpc/tests/grpc_arm_service_test.cpp +++ b/cmvr-es/service/grpc/tests/grpc_arm_service_test.cpp @@ -1,6 +1,10 @@ #include "service/grpc/include/grpc_arm_service.h" +#include +#include +#include #include +#include #include #include #include @@ -11,6 +15,7 @@ #include #include "cmvr/config/device_manager_config/device_manager_config.pb.h" +#include "manager/control_authority/include/control_authority_manager.h" #include "manager/device_manager/include/device_manager.h" namespace cmvr::service { @@ -57,7 +62,12 @@ public: } device::Result torqueOn() override { return device::Result::success(); } - device::Result torqueOff() override { return device::Result::success(); } + device::Result torqueOff() override + { + std::lock_guard lock(motion_mutex_); + ++torque_off_calls_; + return device::Result::success(); + } device::Result calibrateZeroQ(const std::string&) override { return device::Result::success(); @@ -82,7 +92,7 @@ public: device::Result moveJ(const device::JointPositionCommand&, const device::MotionOptions&) override { - return device::Result::success(); + return enterMotion("moveJ", move_j_calls_); } device::Result speedJ(const device::JointVelocityCommand&, double, @@ -99,7 +109,7 @@ public: const device::MotionOptions&, device::FrameType = device::FrameType::Base) override { - return device::Result::success(); + return enterMotion("moveL", move_l_calls_); } device::Result speedL( const device::CartesianVelocity&, @@ -115,9 +125,101 @@ public: } device::Result stopMotion() override { + std::unique_lock lock(motion_mutex_); + ++stop_motion_calls_; + if (block_next_stop_) { + block_next_stop_ = false; + blocking_stop_started_ = true; + stop_started_cv_.notify_all(); + stop_release_cv_.wait( + lock, + [this]() { return release_blocking_stop_; }); + } return device::Result::success(); } + void blockNextMotion() + { + std::lock_guard lock(motion_mutex_); + block_next_motion_ = true; + blocking_motion_started_ = false; + release_blocking_motion_ = false; + blocking_motion_name_.clear(); + } + + bool waitForBlockingMotion( + const std::string& operation, + const std::chrono::milliseconds timeout) + { + std::unique_lock lock(motion_mutex_); + return motion_started_cv_.wait_for( + lock, + timeout, + [this, &operation]() { + return blocking_motion_started_ && + blocking_motion_name_ == operation; + }); + } + + void releaseBlockingMotion() + { + { + std::lock_guard lock(motion_mutex_); + release_blocking_motion_ = true; + } + motion_release_cv_.notify_all(); + } + + void blockNextStopMotion() + { + std::lock_guard lock(motion_mutex_); + block_next_stop_ = true; + blocking_stop_started_ = false; + release_blocking_stop_ = false; + } + + bool waitForBlockingStop(const std::chrono::milliseconds timeout) + { + std::unique_lock lock(motion_mutex_); + return stop_started_cv_.wait_for( + lock, + timeout, + [this]() { return blocking_stop_started_; }); + } + + void releaseBlockingStop() + { + { + std::lock_guard lock(motion_mutex_); + release_blocking_stop_ = true; + } + stop_release_cv_.notify_all(); + } + + int moveJCalls() const + { + std::lock_guard lock(motion_mutex_); + return move_j_calls_; + } + + int moveLCalls() const + { + std::lock_guard lock(motion_mutex_); + return move_l_calls_; + } + + int stopMotionCalls() const + { + std::lock_guard lock(motion_mutex_); + return stop_motion_calls_; + } + + int torqueOffCalls() const + { + std::lock_guard lock(motion_mutex_); + return torque_off_calls_; + } + device::Result startServoMode(const device::ServoOptions&) override { return device::Result::success(); @@ -217,6 +319,42 @@ public: bool next_success{true}; std::string next_response_json; std::string last_request_json; + +private: + device::Result enterMotion(const char* operation, int& call_count) + { + std::unique_lock lock(motion_mutex_); + ++call_count; + if (!block_next_motion_) { + return device::Result::success(); + } + + block_next_motion_ = false; + blocking_motion_started_ = true; + blocking_motion_name_ = operation; + motion_started_cv_.notify_all(); + motion_release_cv_.wait( + lock, + [this]() { return release_blocking_motion_; }); + return device::Result::success(); + } + + mutable std::mutex motion_mutex_; + std::condition_variable motion_started_cv_; + std::condition_variable motion_release_cv_; + std::condition_variable stop_started_cv_; + std::condition_variable stop_release_cv_; + bool block_next_motion_{false}; + bool blocking_motion_started_{false}; + bool release_blocking_motion_{false}; + bool block_next_stop_{false}; + bool blocking_stop_started_{false}; + bool release_blocking_stop_{false}; + std::string blocking_motion_name_; + int move_j_calls_{0}; + int move_l_calls_{0}; + int stop_motion_calls_{0}; + int torque_off_calls_{0}; }; class JsonCommandNonArmDevice final : public device::AbstractDevice { @@ -243,6 +381,7 @@ class GrpcArmServiceTest : public ::testing::Test { protected: void SetUp() override { + control::ControlAuthorityManager::instance().clear(); device::DeviceManager::destroyInstance(); config::DeviceManagerConfig config; auto& manager = device::DeviceManager::getInstance(config); @@ -263,6 +402,7 @@ protected: aubo_arm_.reset(); left_arm_.reset(); device::DeviceManager::destroyInstance(); + control::ControlAuthorityManager::instance().clear(); } grpc::Status execute(const std::string& device_id, @@ -276,6 +416,60 @@ protected: return service_->ExecuteJsonCommand(&context, &request, &response); } + struct MoveOutcome { + grpc::Status status; + bool response_success{false}; + std::string response_error; + }; + + MoveOutcome moveJ(const std::string& device_id) + { + api::MoveJ_Request request; + request.mutable_header()->set_device_id(device_id); + request.mutable_target()->add_position(0.1); + api::MoveJ_Response response; + grpc::ServerContext context; + auto status = service_->moveJ(&context, &request, &response); + return { + std::move(status), + response.header().success(), + response.header().error_message()}; + } + + MoveOutcome moveL(const std::string& device_id) + { + api::MoveL_Request request; + request.mutable_header()->set_device_id(device_id); + request.mutable_target()->set_x(0.1); + api::MoveL_Response response; + grpc::ServerContext context; + auto status = service_->moveL(&context, &request, &response); + return { + std::move(status), + response.header().success(), + response.header().error_message()}; + } + + grpc::Status stopMotion( + const std::string& device_id, + api::CommandHeader_Feedback& response) + { + api::CommandHeader_Request request; + request.set_device_id(device_id); + grpc::ServerContext context; + return service_->stopMotion(&context, &request, &response); + } + + grpc::Status torqueOff( + const std::string& device_id, + api::CommandHeader_Feedback& response) + { + api::CommandHeader_Request request; + request.set_device_id(device_id); + grpc::ServerContext context; + return service_->torqueOff(&context, &request, &response); + } + std::shared_ptr left_arm_; std::shared_ptr aubo_arm_; std::shared_ptr non_arm_; @@ -374,5 +568,125 @@ TEST_F(GrpcArmServiceTest, EXPECT_EQ(left_arm_->execute_calls, 0); } +TEST_F(GrpcArmServiceTest, + StopMotionRevokesBlockedMoveJLeaseBeforeMoveLReturns) +{ + aubo_arm_->blockNextMotion(); + auto blocked_move = std::async( + std::launch::async, + [this]() { return moveJ("aubo_arm"); }); + + const bool move_started = aubo_arm_->waitForBlockingMotion( + "moveJ", std::chrono::seconds(2)); + + MoveOutcome conflict; + MoveOutcome during_stop; + api::CommandHeader_Feedback torque_off_response; + grpc::Status torque_off_status; + api::CommandHeader_Feedback stop_response; + grpc::Status stop_status; + MoveOutcome resumed_move; + bool stop_started = false; + std::future blocked_stop; + if (move_started) { + conflict = moveL("aubo_arm"); + aubo_arm_->blockNextStopMotion(); + blocked_stop = std::async( + std::launch::async, + [this, &stop_response]() { + return stopMotion("aubo_arm", stop_response); + }); + stop_started = aubo_arm_->waitForBlockingStop( + std::chrono::seconds(2)); + if (stop_started) { + torque_off_status = torqueOff( + "aubo_arm", torque_off_response); + during_stop = moveL("aubo_arm"); + } + aubo_arm_->releaseBlockingStop(); + stop_status = blocked_stop.get(); + resumed_move = moveL("aubo_arm"); + } + + // Keep the original RPC active until after the replacement MoveL has + // attempted to acquire control. This models a driver whose stopped motion + // takes time to unwind and guards the lease hand-off itself. + aubo_arm_->releaseBlockingMotion(); + const auto original_move = blocked_move.get(); + + ASSERT_TRUE(move_started); + ASSERT_TRUE(stop_started); + EXPECT_EQ(conflict.status.error_code(), + grpc::StatusCode::FAILED_PRECONDITION); + EXPECT_EQ(conflict.response_error, conflict.status.error_message()); + EXPECT_EQ(during_stop.status.error_code(), + grpc::StatusCode::FAILED_PRECONDITION); + EXPECT_EQ(during_stop.response_error, + during_stop.status.error_message()); + EXPECT_TRUE(torque_off_status.ok()) + << torque_off_status.error_message(); + EXPECT_TRUE(torque_off_response.success()) + << torque_off_response.error_message(); + EXPECT_TRUE(stop_status.ok()) << stop_status.error_message(); + EXPECT_TRUE(stop_response.success()) + << stop_response.error_message(); + EXPECT_TRUE(resumed_move.status.ok()) + << resumed_move.status.error_message(); + EXPECT_TRUE(resumed_move.response_success) + << resumed_move.response_error; + EXPECT_TRUE(original_move.status.ok()) + << original_move.status.error_message(); + EXPECT_TRUE(original_move.response_success) + << original_move.response_error; + EXPECT_EQ(aubo_arm_->moveJCalls(), 1); + EXPECT_EQ(aubo_arm_->moveLCalls(), 1); + EXPECT_EQ(aubo_arm_->stopMotionCalls(), 1); + EXPECT_EQ(aubo_arm_->torqueOffCalls(), 1); +} + +TEST_F(GrpcArmServiceTest, + StopMotionRevokesBlockedMoveLLeaseBeforeMoveJReturns) +{ + aubo_arm_->blockNextMotion(); + auto blocked_move = std::async( + std::launch::async, + [this]() { return moveL("aubo_arm"); }); + + const bool move_started = aubo_arm_->waitForBlockingMotion( + "moveL", std::chrono::seconds(2)); + + MoveOutcome conflict; + api::CommandHeader_Feedback stop_response; + grpc::Status stop_status; + MoveOutcome resumed_move; + if (move_started) { + conflict = moveJ("aubo_arm"); + stop_status = stopMotion("aubo_arm", stop_response); + resumed_move = moveJ("aubo_arm"); + } + + aubo_arm_->releaseBlockingMotion(); + const auto original_move = blocked_move.get(); + + ASSERT_TRUE(move_started); + EXPECT_EQ(conflict.status.error_code(), + grpc::StatusCode::FAILED_PRECONDITION); + EXPECT_EQ(conflict.response_error, conflict.status.error_message()); + EXPECT_TRUE(stop_status.ok()) << stop_status.error_message(); + EXPECT_TRUE(stop_response.success()) + << stop_response.error_message(); + EXPECT_TRUE(resumed_move.status.ok()) + << resumed_move.status.error_message(); + EXPECT_TRUE(resumed_move.response_success) + << resumed_move.response_error; + EXPECT_TRUE(original_move.status.ok()) + << original_move.status.error_message(); + EXPECT_TRUE(original_move.response_success) + << original_move.response_error; + EXPECT_EQ(aubo_arm_->moveJCalls(), 1); + EXPECT_EQ(aubo_arm_->moveLCalls(), 1); + EXPECT_EQ(aubo_arm_->stopMotionCalls(), 1); +} + } // namespace } // namespace cmvr::service diff --git a/cmvr-es/service/grpc/tests/grpc_system_service_test.cpp b/cmvr-es/service/grpc/tests/grpc_system_service_test.cpp index 8642a5b4..22eac5e4 100644 --- a/cmvr-es/service/grpc/tests/grpc_system_service_test.cpp +++ b/cmvr-es/service/grpc/tests/grpc_system_service_test.cpp @@ -2,8 +2,11 @@ #include #include +#include #include +#include #include +#include #include #include #include @@ -12,6 +15,7 @@ #include #include "cmvr/config/device_manager_config/device_manager_config.pb.h" +#include "manager/control_authority/include/control_authority_manager.h" #include "manager/device_manager/include/device_manager.h" namespace cmvr::service { @@ -36,11 +40,58 @@ public: { return health_; } + bool stop() override + { + std::unique_lock lock(stop_mutex_); + ++stop_calls_; + if (block_next_stop_) { + block_next_stop_ = false; + stop_started_ = true; + stop_started_cv_.notify_all(); + stop_release_cv_.wait( + lock, + [this]() { return release_stop_; }); + } + return true; + } + int stopCalls() const + { + std::lock_guard lock(stop_mutex_); + return stop_calls_; + } + void blockNextStop() + { + std::lock_guard lock(stop_mutex_); + block_next_stop_ = true; + stop_started_ = false; + release_stop_ = false; + } + bool waitForStop(const std::chrono::milliseconds timeout) + { + std::unique_lock lock(stop_mutex_); + return stop_started_cv_.wait_for( + lock, timeout, [this]() { return stop_started_; }); + } + void releaseStop() + { + { + std::lock_guard lock(stop_mutex_); + release_stop_ = true; + } + stop_release_cv_.notify_all(); + } private: device::DeviceKind kind_; std::string type_name_; device::DeviceHealthSnapshot health_; + mutable std::mutex stop_mutex_; + std::condition_variable stop_started_cv_; + std::condition_variable stop_release_cv_; + int stop_calls_{0}; + bool block_next_stop_{false}; + bool stop_started_{false}; + bool release_stop_{false}; }; std::uint64_t currentUnixTimeMs() @@ -66,6 +117,7 @@ class GrpcSystemServiceTest : public ::testing::Test { protected: void SetUp() override { + control::ControlAuthorityManager::instance().clear(); device::DeviceManager::destroyInstance(); } @@ -74,6 +126,7 @@ protected: service_.reset(); owned_devices_.clear(); device::DeviceManager::destroyInstance(); + control::ControlAuthorityManager::instance().clear(); } api::GetDeviceListCommand_Feedback getDeviceList() @@ -287,5 +340,74 @@ TEST_F(GrpcSystemServiceTest, MapsEveryKnownDeviceKind) } } +TEST_F(GrpcSystemServiceTest, + StopAllStopsRegisteredDevicesAndRevokesOnlyArmLease) +{ + config::DeviceManagerConfig config; + auto& manager = device::DeviceManager::getInstance(config); + auto arm = std::make_shared( + "leased_arm", device::DeviceKind::Arm, "TestArm"); + auto camera = std::make_shared( + "leased_camera", device::DeviceKind::Camera, "TestCamera"); + auto already_stopping_arm = std::make_shared( + "already_stopping_arm", device::DeviceKind::Arm, "TestArm"); + registerDevice(manager, arm); + registerDevice(manager, camera); + registerDevice(manager, already_stopping_arm); + + auto& authority = control::ControlAuthorityManager::instance(); + const auto arm_lease = authority.tryAcquire( + arm->id(), "arm-session", std::chrono::seconds(30)); + const auto camera_lease = authority.tryAcquire( + camera->id(), "camera-session", std::chrono::seconds(30)); + const auto existing_stop_barrier = authority.preemptAcquire( + already_stopping_arm->id(), + "existing-stop", + std::chrono::seconds(30)); + ASSERT_TRUE(arm_lease.acquired) << arm_lease.detail; + ASSERT_TRUE(camera_lease.acquired) << camera_lease.detail; + ASSERT_TRUE(existing_stop_barrier.acquired) + << existing_stop_barrier.detail; + ASSERT_TRUE(authority.validate(arm_lease.token)); + ASSERT_TRUE(authority.validate(camera_lease.token)); + + service_ = std::make_unique(); + arm->blockNextStop(); + api::StopAllCommand_Request request; + api::StopAllCommand_Feedback response; + auto stop_all = std::async( + std::launch::async, + [this, &request, &response]() { + grpc::ServerContext context; + return service_->StopAll(&context, &request, &response); + }); + const bool stop_started = arm->waitForStop( + std::chrono::seconds(2)); + const auto move_during_stop = authority.tryAcquire( + arm->id(), "move-during-stop", std::chrono::seconds(30)); + arm->releaseStop(); + const auto status = stop_all.get(); + + ASSERT_TRUE(stop_started); + EXPECT_FALSE(move_during_stop.acquired); + ASSERT_TRUE(status.ok()) << status.error_message(); + ASSERT_TRUE(response.header().success()) + << response.header().error_message(); + EXPECT_EQ(arm->stopCalls(), 1); + EXPECT_EQ(camera->stopCalls(), 1); + EXPECT_EQ(already_stopping_arm->stopCalls(), 1); + EXPECT_FALSE(authority.validate(arm_lease.token)); + EXPECT_FALSE(authority.isLeased(arm->id())); + EXPECT_TRUE(authority.validate(camera_lease.token)); + EXPECT_TRUE(authority.isLeased(camera->id())); + EXPECT_TRUE(authority.validate(existing_stop_barrier.token)); + EXPECT_TRUE(authority.isLeased(already_stopping_arm->id())); + const auto move_after_stop = authority.tryAcquire( + arm->id(), "move-after-stop", std::chrono::seconds(30)); + EXPECT_TRUE(move_after_stop.acquired) << move_after_stop.detail; + authority.release(move_after_stop.token); + authority.release(existing_stop_barrier.token); +} + } // namespace } // namespace cmvr::service From e3726e5a987b1aad251dcea6332836c2b035feb0 Mon Sep 17 00:00:00 2001 From: linbo <1034003879@qq.com> Date: Thu, 6 Aug 2026 15:21:01 +0800 Subject: [PATCH 17/20] Add QSV hardware encoding support to FFmpeg and upgrade to v6.1 --- cmvr-es/config/manager/device_manager.pb.txt | 2 +- cmvr-es/config/manager/task_manager.pb.txt | 2 +- .../quic_edge_task/quic_edge_task.pb.txt | 10 ++--- .../agv/seer_robokit/src/seer_robokit_map.cpp | 39 +++++++++++++++++-- 4 files changed, 41 insertions(+), 12 deletions(-) diff --git a/cmvr-es/config/manager/device_manager.pb.txt b/cmvr-es/config/manager/device_manager.pb.txt index 134e2a9e..dcb2a5cc 100644 --- a/cmvr-es/config/manager/device_manager.pb.txt +++ b/cmvr-es/config/manager/device_manager.pb.txt @@ -93,7 +93,7 @@ device_manager { id: "aubo_arm" type: DEVICE_TYPE_ROBOT_ARM config_file: "devices/arm/aubo_arm.pb.txt" - enable: false + enable: true } devices { diff --git a/cmvr-es/config/manager/task_manager.pb.txt b/cmvr-es/config/manager/task_manager.pb.txt index 223f9873..4d5df7a7 100644 --- a/cmvr-es/config/manager/task_manager.pb.txt +++ b/cmvr-es/config/manager/task_manager.pb.txt @@ -28,7 +28,7 @@ task_manager { run_mode: TASK_RUN_MODE_BLOCKING_SERVICE config_file: "tasks/quic_edge_task/quic_edge_task.pb.txt" # Host-development default: no QUIC Gateway or physical media devices. - enable: false + enable: true } tasks { id: "ume_teleop" diff --git a/cmvr-es/config/tasks/quic_edge_task/quic_edge_task.pb.txt b/cmvr-es/config/tasks/quic_edge_task/quic_edge_task.pb.txt index f850dc00..d502fa57 100644 --- a/cmvr-es/config/tasks/quic_edge_task/quic_edge_task.pb.txt +++ b/cmvr-es/config/tasks/quic_edge_task/quic_edge_task.pb.txt @@ -3,11 +3,11 @@ quic_edge { # Task enablement is controlled by manager/task_manager.pb.txt. Configure a # reachable QUIC Gateway and TLS policy before enabling the task there. - server_host: "quic-gateway.example.com" + server_host: "192.168.0.222" server_port: 4433 alpn: "cmvr-quic-edge/1" node_id: "cmvr-edge" - robot_id: "CN-CMVR-MBLRV1-CHAGAN-20260731-001" + robot_id: "CN-CMVR-MBLRV1-AIMA-20260806-001" software_version: "0.1" # The existing cmvr-es gRPC server remains the robot-control endpoint. "auto" @@ -24,10 +24,8 @@ quic_edge { control_response_timeout_ms: 1000 tls { - ca_file: "certs/quic_gateway_ca.pem" - certificate_file: "certs/cmvr_edge_cert.pem" - private_key_file: "certs/cmvr_edge_key.pem" - server_name: "quic-gateway.example.com" + ca_file: "certs/cmvr-quic-ca.crt" + server_name: "192.168.0.222" allow_insecure: false } 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 index d520d462..d4b5ce2f 100644 --- a/cmvr-es/devices/agv/seer_robokit/src/seer_robokit_map.cpp +++ b/cmvr-es/devices/agv/seer_robokit/src/seer_robokit_map.cpp @@ -256,17 +256,48 @@ AgvResult SeerRobokitAgv::uploadMap(const std::string& map_name, const std::stri return result.ok() ? resultFromResponse_(response) : result; } -AgvResult SeerRobokitAgv::downloadMap(const std::string& map_name, std::string& content) const +// 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::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); + 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); + + // 4011 失败响应包含 ret_code；成功响应本身就是完整地图 JSON。 + if (hasNumericControllerRetCode(response)) { + return resultFromResponse_(response); + } + + content = toJsonString_(response); + if (content.empty()) { + return AgvResult::failure( + AgvErrorCode::CommandFailed, + "SEER Robokit downloaded map content is empty"); + } + + return AgvResult::success(); } + AgvResult SeerRobokitAgv::startMapping(const AgvMappingOptions& options) { auto result = ensureOtherSocket_(); From 3b87f681cf1e909a5c839ce3c0e8ae0e7101cd68 Mon Sep 17 00:00:00 2001 From: xtkuang <87661715@qq.com> Date: Fri, 7 Aug 2026 13:35:30 +0800 Subject: [PATCH 18/20] fix(aubo): prevent resume after hardware e-stop --- cmvr-es/devices/arm/aubo_arm/CMakeLists.txt | 16 + cmvr-es/devices/arm/aubo_arm/README.md | 20 +- cmvr-es/devices/arm/aubo_arm/aubo_arm.cpp | 1743 ++++++++++++++++- cmvr-es/devices/arm/aubo_arm/aubo_arm.h | 25 +- .../devices/arm/aubo_arm/aubo_motion_state.h | 31 + .../devices/arm/aubo_arm/aubo_safety_state.h | 184 ++ .../aubo_arm/tests/aubo_motion_state_test.cpp | 29 + .../aubo_arm/tests/aubo_safety_state_test.cpp | 88 + 8 files changed, 2075 insertions(+), 61 deletions(-) create mode 100644 cmvr-es/devices/arm/aubo_arm/aubo_safety_state.h create mode 100644 cmvr-es/devices/arm/aubo_arm/tests/aubo_safety_state_test.cpp diff --git a/cmvr-es/devices/arm/aubo_arm/CMakeLists.txt b/cmvr-es/devices/arm/aubo_arm/CMakeLists.txt index aee5d72a..d52da874 100644 --- a/cmvr-es/devices/arm/aubo_arm/CMakeLists.txt +++ b/cmvr-es/devices/arm/aubo_arm/CMakeLists.txt @@ -2,6 +2,8 @@ add_library(aubo_arm SHARED aubo_arm.cpp ) +find_package(Threads REQUIRED) + target_include_directories(aubo_arm PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) set(AUBO_SDK_ROOT ${CMAKE_SOURCE_DIR}/dependency/${ARCH}/third_party/aubo_sdk/v0.27.1) @@ -47,6 +49,7 @@ target_link_libraries(aubo_arm PRIVATE glog jsoncpp + Threads::Threads ) add_library(cmvr_es::device::aubo_arm ALIAS aubo_arm) @@ -80,6 +83,19 @@ if(BUILD_TESTING) ) set_tests_properties(aubo_motion_state_test PROPERTIES TIMEOUT 10) + add_executable(aubo_safety_state_test + tests/aubo_safety_state_test.cpp + ) + target_include_directories(aubo_safety_state_test + PRIVATE + ${CMAKE_SOURCE_DIR}/cmvr-es + ) + add_test( + NAME aubo_safety_state_test + COMMAND aubo_safety_state_test + ) + set_tests_properties(aubo_safety_state_test PROPERTIES TIMEOUT 10) + add_executable(aubo_arm_json_command_test tests/aubo_arm_json_command_test.cpp ) diff --git a/cmvr-es/devices/arm/aubo_arm/README.md b/cmvr-es/devices/arm/aubo_arm/README.md index 0045108b..05e6d903 100644 --- a/cmvr-es/devices/arm/aubo_arm/README.md +++ b/cmvr-es/devices/arm/aubo_arm/README.md @@ -103,6 +103,19 @@ cmake --install build ## 安全与语义边界 +- 后端使用独立 SDK RPC 会话持续读取控制器的 `SafetyModeType`、 + `RobotModeType` 和硬件急停来源；首次有效样本前、监控断线或样本过期时， + 所有 Move、Speed、Servo 和程序启动请求均按不安全状态拒绝； +- 硬件急停、防护停机、Safety Fault/Violation 会锁存安全事件，并使当前运动 + generation 失效。控制器重新报告 `Normal`/`ReducedMode` 不会自动解除锁存； +- 锁存后会终止直接运动与程序、关闭 servo 模式并清理控制器轨迹。只有确认 + `ExecId == -1`、普通队列和轨迹队列均为空、运行时已停止且机械臂稳定后， + 显式 `torqueOn`/`clearFault`/`unlockProtectiveStop` 才可能恢复运动权限； +- 恢复流程不会调用 `resume`、`arbitraryResume`、`startMove`，也不会重新提交 + 急停前的目标、速度、servo 指令或程序； +- AUBO SDK 未在本地文档中保证急停期间 `clearPath` 的可用性，也未说明释放 + 急停开关后的控制器恢复时序。因此本实现保持 fail-closed 并在释放后再次清队列， + 但“释放开关后零位移”的最终保证仍需真机验证及控制器侧安全配置配合； - 只访问控制柜 Standard 数字 IO，不访问工具端 IO、可配置 IO 或安全 IO； - `set_do` 不修改输出 runstate； - 只有 `StandardOutputRunState::None` 的通道允许写入，否则返回 @@ -116,9 +129,12 @@ cmake --install build ## 测试 ```bash -cmake --build build --target aubo_arm_json_command_test -j4 +cmake --build build --target \ + aubo_safety_state_test \ + aubo_motion_state_test \ + aubo_arm_json_command_test -j4 ctest --test-dir build \ - -R '^aubo_arm_json_command_test$' \ + -R 'aubo_(safety_state|motion_state|arm_json_command)_test' \ --output-on-failure ``` diff --git a/cmvr-es/devices/arm/aubo_arm/aubo_arm.cpp b/cmvr-es/devices/arm/aubo_arm/aubo_arm.cpp index e2936689..ed446d4f 100644 --- a/cmvr-es/devices/arm/aubo_arm/aubo_arm.cpp +++ b/cmvr-es/devices/arm/aubo_arm/aubo_arm.cpp @@ -2,14 +2,18 @@ #include "devices/arm/aubo_arm/aubo_motion_result.h" #include "devices/arm/aubo_arm/aubo_motion_state.h" +#include "devices/arm/aubo_arm/aubo_safety_state.h" #include #include #include +#include #include #include +#include #include #include +#include #include "common/base/logging/logger.h" #include "json/json.h" @@ -115,6 +119,51 @@ private: bool completed_{false}; }; +class SafetyRecoveryGuard final { +public: + SafetyRecoveryGuard( + std::shared_ptr state, + const aubo_internal::RecoveryToken token) + : state_(std::move(state)), token_(token) + { + } + + ~SafetyRecoveryGuard() noexcept + { + if (!completed_ && state_) { + try { + state_->failRecovery(token_); + } catch (...) { + // The safety latch itself remains set on every failure path. + } + } + } + + SafetyRecoveryGuard(const SafetyRecoveryGuard&) = delete; + SafetyRecoveryGuard& operator=(const SafetyRecoveryGuard&) = delete; + + bool complete( + const bool robot_running, + const bool controller_idle, + const bool cancellation_confirmed) + { + if (!state_->completeRecovery( + token_, + robot_running, + controller_idle, + cancellation_confirmed)) { + return false; + } + completed_ = true; + return true; + } + +private: + std::shared_ptr state_; + aubo_internal::RecoveryToken token_; + bool completed_{false}; +}; + const char* motionStartFailure( const aubo_internal::MotionStartStatus status) noexcept { @@ -168,9 +217,664 @@ std::string vendorBrandName(const config::VendorRobotArmBrand brand) } using arcs::common_interface::RobotModeType; +using arcs::common_interface::RuntimeState; +using arcs::common_interface::SafetyModeType; using arcs::aubo_sdk::RobotInterfacePtr; +RobotInterfacePtr getPrimaryRobotInterface( + const std::shared_ptr& rpc_client, + const std::string& context, + Result& result); + constexpr int kAuboServoMode = 3; +constexpr auto kSafetyPollInterval = std::chrono::milliseconds(50); +constexpr auto kSafetyReconnectInterval = std::chrono::milliseconds(250); +constexpr auto kSafetySampleMaxAge = std::chrono::milliseconds(500); + +std::int64_t monotonicNowNs() noexcept +{ + return std::chrono::duration_cast( + std::chrono::steady_clock::now().time_since_epoch()) + .count(); +} + +aubo_internal::SafetyCondition safetyConditionFromSdk( + const SafetyModeType mode) noexcept +{ + using Condition = aubo_internal::SafetyCondition; + switch (mode) { + case SafetyModeType::Normal: + return Condition::Normal; + case SafetyModeType::ReducedMode: + return Condition::Reduced; + case SafetyModeType::Recovery: + return Condition::Recovery; + case SafetyModeType::Violation: + return Condition::Violation; + case SafetyModeType::ProtectiveStop: + return Condition::ProtectiveStop; + case SafetyModeType::SafeguardStop: + return Condition::SafeguardStop; + case SafetyModeType::SystemEmergencyStop: + return Condition::SystemEmergencyStop; + case SafetyModeType::RobotEmergencyStop: + return Condition::RobotEmergencyStop; + case SafetyModeType::Fault: + return Condition::Fault; + case SafetyModeType::Undefined: + break; + } + return Condition::Unknown; +} + +const char* safetyConditionName( + const aubo_internal::SafetyCondition condition) noexcept +{ + using Condition = aubo_internal::SafetyCondition; + switch (condition) { + case Condition::Normal: + return "Normal"; + case Condition::Reduced: + return "Reduced"; + case Condition::Recovery: + return "Recovery"; + case Condition::Violation: + return "Violation"; + case Condition::ProtectiveStop: + return "ProtectiveStop"; + case Condition::SafeguardStop: + return "SafeguardStop"; + case Condition::SystemEmergencyStop: + return "SystemEmergencyStop"; + case Condition::RobotEmergencyStop: + return "RobotEmergencyStop"; + case Condition::Fault: + return "Fault"; + case Condition::Unknown: + break; + } + return "Unknown"; +} + +SafetyMode publicSafetyMode( + const aubo_internal::SafetyCondition condition) noexcept +{ + using Condition = aubo_internal::SafetyCondition; + switch (condition) { + case Condition::Normal: + return SafetyMode::Normal; + case Condition::Reduced: + return SafetyMode::Reduced; + case Condition::ProtectiveStop: + return SafetyMode::ProtectiveStop; + case Condition::SafeguardStop: + return SafetyMode::SafeguardStop; + case Condition::SystemEmergencyStop: + return SafetyMode::SystemEmergencyStop; + case Condition::RobotEmergencyStop: + return SafetyMode::EmergencyStop; + case Condition::Violation: + case Condition::Fault: + return SafetyMode::Fault; + case Condition::Recovery: + case Condition::Unknown: + break; + } + return SafetyMode::Unknown; +} + +RobotMode publicRobotMode(const RobotModeType mode) noexcept +{ + switch (mode) { + case RobotModeType::NoController: + case RobotModeType::Disconnected: + return RobotMode::Disconnected; + case RobotModeType::PowerOff: + case RobotModeType::PowerOffing: + return RobotMode::PowerOff; + case RobotModeType::Running: + return RobotMode::Running; + case RobotModeType::Error: + return RobotMode::Fault; + case RobotModeType::PowerOn: + case RobotModeType::Idle: + case RobotModeType::BrakeReleasing: + case RobotModeType::BackDrive: + return RobotMode::Idle; + case RobotModeType::ConfirmSafety: + case RobotModeType::Booting: + case RobotModeType::Maintaince: + break; + } + return RobotMode::Unknown; +} + +struct AuboSafetyMonitor final { + std::shared_ptr safety_state{ + std::make_shared()}; + std::shared_ptr motion_state; + std::atomic safety_mode{ + static_cast(SafetyModeType::Undefined)}; + std::atomic robot_mode{ + static_cast(RobotModeType::Disconnected)}; + std::atomic runtime_state{ + static_cast(RuntimeState::Stopped)}; + std::atomic emergency_stop_source{-1}; + std::atomic servo_mode_select{0}; + std::atomic last_sample_ns{0}; + std::atomic cancellation_confirmed{true}; + std::atomic runtime_abort_required{false}; + std::atomic servo_disable_required{false}; + std::atomic path_clear_required{false}; + std::atomic stop_requested{false}; + std::mutex wait_mutex; + std::condition_variable wait_cv; + std::mutex termination_mutex; + std::recursive_mutex command_rpc_mutex; + std::string arm_id; +}; + +std::shared_ptr makeRpcClient() +{ + return std::shared_ptr( + ::createRpcClient(), + [](arcs::aubo_sdk::RpcClient* client) { + if (client) { + ::destroyRpcClient(client); + } + }); +} + +bool monitorWait( + const std::shared_ptr& monitor, + const std::chrono::milliseconds duration) +{ + std::unique_lock lock(monitor->wait_mutex); + return monitor->wait_cv.wait_for( + lock, + duration, + [&monitor]() { return monitor->stop_requested.load(); }); +} + +void cancelForSafetyTransition( + const std::shared_ptr& monitor) +{ + monitor->cancellation_confirmed.store(false); + if (monitor->runtime_state.load() != + static_cast(RuntimeState::Stopped)) { + monitor->runtime_abort_required.store(true); + } + if (monitor->servo_mode_select.load() != 0) { + monitor->servo_disable_required.store(true); + } + monitor->motion_state->cancelActiveForSafety(); +} + +void publishSafetySample( + const std::shared_ptr& monitor, + const SafetyModeType safety_mode, + const RobotModeType robot_mode, + const RuntimeState runtime_state, + const int emergency_stop_source, + const int servo_mode_select) +{ + const auto previous = monitor->safety_state->snapshot(); + const int previous_runtime_state = monitor->runtime_state.load(); + const int previous_servo_mode = monitor->servo_mode_select.load(); + const auto condition = aubo_internal::effectiveSafetyCondition( + safetyConditionFromSdk(safety_mode), emergency_stop_source); + monitor->safety_state->observe(condition); + const auto current = monitor->safety_state->snapshot(); + + monitor->safety_mode.store(static_cast(safety_mode)); + monitor->robot_mode.store(static_cast(robot_mode)); + monitor->runtime_state.store(static_cast(runtime_state)); + monitor->emergency_stop_source.store(emergency_stop_source); + monitor->servo_mode_select.store(servo_mode_select); + monitor->last_sample_ns.store(monotonicNowNs()); + + if (previous.observed != condition || + (!previous.latched && current.latched)) { + if (current.latched) { + if (previous_runtime_state != + static_cast(RuntimeState::Stopped) || + runtime_state != RuntimeState::Stopped) { + monitor->runtime_abort_required.store(true); + } + if (previous_servo_mode != 0 || servo_mode_select != 0) { + monitor->servo_disable_required.store(true); + } + cancelForSafetyTransition(monitor); + } + if (current.latched) { + CMVR_LOG(WARNING) + << "[AuboArm] safety state changed, id=" << monitor->arm_id + << ", state=" << safetyConditionName(condition) + << ", latched=true"; + } else { + CMVR_LOG(INFO) + << "[AuboArm] safety state changed, id=" << monitor->arm_id + << ", state=" << safetyConditionName(condition) + << ", latched=false"; + } + } +} + +void refreshSafetySample( + const std::shared_ptr& rpc_client, + const std::shared_ptr& monitor, + const RobotInterfacePtr& robot_interface) +{ + auto robot_state = robot_interface->getRobotState(); + publishSafetySample( + monitor, + robot_state->getSafetyModeType(), + robot_state->getRobotModeType(), + rpc_client->getRuntimeMachine()->getRuntimeState(), + robot_interface->getRobotConfig() + ->getRobotEmergencyStopSource(), + robot_interface->getMotionControl()->getServoModeSelect()); +} + +void publishSafetyUnavailable( + const std::shared_ptr& monitor, + const std::string& reason) +{ + const auto previous = monitor->safety_state->snapshot(); + monitor->safety_state->observe( + aubo_internal::SafetyCondition::Unknown); + monitor->safety_mode.store( + static_cast(SafetyModeType::Undefined)); + monitor->robot_mode.store( + static_cast(RobotModeType::Disconnected)); + monitor->emergency_stop_source.store(-1); + monitor->last_sample_ns.store(0); + if (previous.observed != aubo_internal::SafetyCondition::Unknown || + !previous.latched) { + cancelForSafetyTransition(monitor); + CMVR_LOG(WARNING) + << "[AuboArm] safety monitor unavailable, id=" + << monitor->arm_id << ", reason=" << reason; + } +} + +bool safetySampleFresh( + const std::shared_ptr& monitor) noexcept +{ + const auto sample_ns = monitor->last_sample_ns.load(); + if (sample_ns <= 0) { + return false; + } + const auto age_ns = monotonicNowNs() - sample_ns; + return age_ns >= 0 && + age_ns <= std::chrono::duration_cast( + kSafetySampleMaxAge) + .count(); +} + +bool validateSafetyPermit( + const std::shared_ptr& monitor, + const aubo_internal::SafetyPermit permit) +{ + if (!safetySampleFresh(monitor)) { + publishSafetyUnavailable(monitor, "sample is stale"); + return false; + } + return monitor->emergency_stop_source.load() == 0 && + monitor->robot_mode.load() == + static_cast(RobotModeType::Running) && + monitor->safety_state->validate(permit); +} + +bool enforceControllerTermination( + const std::shared_ptr& rpc_client, + const std::shared_ptr& monitor) +{ + std::unique_lock termination_lock(monitor->termination_mutex); + monitor->motion_state->cancelActiveForSafety(); + const auto stop_request = monitor->motion_state->beginStop(); + if (!stop_request.started()) { + return monitor->cancellation_confirmed.load(); + } + + const auto fail = [&monitor]() { + monitor->motion_state->failStop(); + monitor->cancellation_confirmed.store(false); + return false; + }; + + try { + Result interface_result; + auto robot_interface = getPrimaryRobotInterface( + rpc_client, "safety termination", interface_result); + if (!interface_result.ok()) { + return fail(); + } + auto motion_control = robot_interface->getMotionControl(); + auto robot_state = robot_interface->getRobotState(); + auto runtime = rpc_client->getRuntimeMachine(); + + constexpr auto kTerminationTimeout = std::chrono::seconds(2); + constexpr int kStableSamples = 3; + const auto deadline = + std::chrono::steady_clock::now() + kTerminationTimeout; + int stable_samples = 0; + int iteration = 0; + bool typed_stop_acknowledged = + !stop_request.tracked_motion; + while (!monitor->stop_requested.load() && + std::chrono::steady_clock::now() < deadline) { + const int exec_id = motion_control->getExecId(); + const bool steady = robot_state->isSteady(); + int queue_size = motion_control->getQueueSize(); + int trajectory_queue_size = + motion_control->getTrajectoryQueueSize(); + int servo_mode = motion_control->getServoModeSelect(); + auto runtime_state = runtime->getRuntimeState(); + + if (runtime_state != RuntimeState::Stopped) { + monitor->runtime_abort_required.store(true); + } + if (servo_mode != 0) { + monitor->servo_disable_required.store(true); + } + if (queue_size != 0 || trajectory_queue_size != 0) { + monitor->path_clear_required.store(true); + } + + if (monitor->runtime_abort_required.load() && + iteration % 4 == 0 && + runtime->abort() == arcs::common_interface::AUBO_OK) { + monitor->runtime_abort_required.store(false); + } + if (monitor->servo_disable_required.load() && + iteration % 4 == 0 && + motion_control->setServoModeSelect(0) == + arcs::common_interface::AUBO_OK) { + monitor->servo_disable_required.store(false); + } + + const bool controller_moving = exec_id != -1 || !steady; + if ((stop_request.tracked_motion || controller_moving) && + iteration % 4 == 0) { + int stop_ret = arcs::common_interface::AUBO_OK; + if (stop_request.kind == aubo_internal::MotionKind::Joint) { + stop_ret = motion_control->stopJoint(31.0); + } else if (stop_request.kind == + aubo_internal::MotionKind::Linear) { + stop_ret = motion_control->stopLine(10.0, 10.0); + } else { + // RuntimeMachine::abort() is the only typed-independent + // SDK primitive documented to stop arbitrary operation. + monitor->runtime_abort_required.store(true); + stop_ret = runtime->abort(); + if (stop_ret == arcs::common_interface::AUBO_OK) { + monitor->runtime_abort_required.store(false); + } + } + if (stop_request.kind != + aubo_internal::MotionKind::None && + stop_ret == arcs::common_interface::AUBO_OK) { + typed_stop_acknowledged = true; + } + } + + if (monitor->path_clear_required.load() && + iteration % 4 == 0 && + motion_control->clearPath() == + arcs::common_interface::AUBO_OK) { + monitor->path_clear_required.store(false); + } + + queue_size = motion_control->getQueueSize(); + trajectory_queue_size = + motion_control->getTrajectoryQueueSize(); + servo_mode = motion_control->getServoModeSelect(); + runtime_state = runtime->getRuntimeState(); + const bool owner_active = monitor->motion_state->ownerActive( + stop_request.active_token); + const bool idle = + motion_control->getExecId() == -1 && + robot_state->isSteady() && + queue_size == 0 && + trajectory_queue_size == 0 && servo_mode == 0 && + runtime_state == RuntimeState::Stopped && !owner_active && + typed_stop_acknowledged && + !monitor->runtime_abort_required.load() && + !monitor->servo_disable_required.load() && + !monitor->path_clear_required.load() && + aubo_internal::isMotionSafe( + monitor->safety_state->snapshot().observed) && + monitor->emergency_stop_source.load() == 0; + + if (idle) { + if (++stable_samples >= kStableSamples) { + if (!monitor->motion_state->completeStop()) { + return fail(); + } + monitor->servo_mode_select.store(0); + monitor->runtime_state.store( + static_cast(RuntimeState::Stopped)); + monitor->cancellation_confirmed.store(true); + CMVR_LOG(INFO) + << "[AuboArm] safety termination confirmed, id=" + << monitor->arm_id; + return true; + } + } else { + stable_samples = 0; + } + + ++iteration; + if (monitorWait(monitor, kSafetyPollInterval)) { + break; + } + } + } catch (const std::exception& e) { + CMVR_LOG(WARNING) + << "[AuboArm] safety termination attempt failed, id=" + << monitor->arm_id << ", error=" << e.what(); + } + return fail(); +} + +// Powering on to Idle keeps the brakes engaged. This pre-startup phase clears +// the controller queues without completing MotionState, so the retained +// Joint/Linear kind survives until a typed stop is acknowledged in Running. +bool prepareControllerForStartup( + const std::shared_ptr& rpc_client, + const std::shared_ptr& monitor) +{ + std::unique_lock termination_lock(monitor->termination_mutex); + const auto cancellation = + monitor->motion_state->cancelActiveForSafety(); + + try { + Result interface_result; + auto robot_interface = getPrimaryRobotInterface( + rpc_client, "pre-startup safety cleanup", interface_result); + if (!interface_result.ok()) { + return false; + } + auto motion_control = robot_interface->getMotionControl(); + auto runtime = rpc_client->getRuntimeMachine(); + + constexpr auto kCleanupTimeout = std::chrono::seconds(2); + constexpr int kStableSamples = 3; + const auto deadline = + std::chrono::steady_clock::now() + kCleanupTimeout; + int stable_samples = 0; + int iteration = 0; + while (!monitor->stop_requested.load() && + std::chrono::steady_clock::now() < deadline) { + int queue_size = motion_control->getQueueSize(); + int trajectory_queue_size = + motion_control->getTrajectoryQueueSize(); + int servo_mode = motion_control->getServoModeSelect(); + auto runtime_state = runtime->getRuntimeState(); + + if (runtime_state != RuntimeState::Stopped) { + monitor->runtime_abort_required.store(true); + } + if (servo_mode != 0) { + monitor->servo_disable_required.store(true); + } + if (queue_size != 0 || trajectory_queue_size != 0) { + monitor->path_clear_required.store(true); + } + + if (iteration % 4 == 0) { + if (monitor->runtime_abort_required.load() && + runtime->abort() == arcs::common_interface::AUBO_OK) { + monitor->runtime_abort_required.store(false); + } + if (monitor->servo_disable_required.load() && + motion_control->setServoModeSelect(0) == + arcs::common_interface::AUBO_OK) { + monitor->servo_disable_required.store(false); + } + if (monitor->path_clear_required.load() && + motion_control->clearPath() == + arcs::common_interface::AUBO_OK) { + monitor->path_clear_required.store(false); + } + } + + queue_size = motion_control->getQueueSize(); + trajectory_queue_size = + motion_control->getTrajectoryQueueSize(); + servo_mode = motion_control->getServoModeSelect(); + runtime_state = runtime->getRuntimeState(); + const bool owner_active = + monitor->motion_state->ownerActive( + cancellation.active_token); + const bool queues_cleared = + motion_control->getExecId() == -1 && + queue_size == 0 && trajectory_queue_size == 0 && + servo_mode == 0 && + runtime_state == RuntimeState::Stopped && + !owner_active && + !monitor->runtime_abort_required.load() && + !monitor->servo_disable_required.load() && + !monitor->path_clear_required.load(); + if (queues_cleared) { + if (++stable_samples >= kStableSamples) { + CMVR_LOG(INFO) + << "[AuboArm] pre-startup safety cleanup confirmed, id=" + << monitor->arm_id; + return true; + } + } else { + stable_samples = 0; + } + + ++iteration; + if (monitorWait(monitor, kSafetyPollInterval)) { + break; + } + } + } catch (const std::exception& e) { + CMVR_LOG(WARNING) + << "[AuboArm] pre-startup safety cleanup failed, id=" + << monitor->arm_id << ", error=" << e.what(); + } + return false; +} + +bool controllerStillQuiescent( + const std::shared_ptr& rpc_client, + const RobotInterfacePtr& robot_interface) +{ + auto motion_control = robot_interface->getMotionControl(); + return motion_control->getExecId() == -1 && + motion_control->getQueueSize() == 0 && + motion_control->getTrajectoryQueueSize() == 0 && + robot_interface->getRobotState()->isSteady() && + motion_control->getServoModeSelect() == 0 && + rpc_client->getRuntimeMachine()->getRuntimeState() == + RuntimeState::Stopped; +} + +void runSafetyMonitor( + const std::shared_ptr& monitor, + const std::string& ip, + const int port, + const std::string& username, + const std::string& password) +{ + while (!monitor->stop_requested.load()) { + auto rpc_client = makeRpcClient(); + try { + if (!rpc_client) { + publishSafetyUnavailable(monitor, "create RPC client failed"); + } else { + rpc_client->setRequestTimeout(250); + const int connect_ret = rpc_client->connect(ip, port); + const int login_ret = connect_ret == 0 + ? rpc_client->login(username, password) + : connect_ret; + if (connect_ret != 0 || login_ret != 0) { + publishSafetyUnavailable( + monitor, + "monitor RPC connect/login failed"); + } else { + Result interface_result; + auto robot_interface = getPrimaryRobotInterface( + rpc_client, "safety monitor", interface_result); + if (!interface_result.ok()) { + publishSafetyUnavailable( + monitor, interface_result.message); + } else { + while (!monitor->stop_requested.load()) { + refreshSafetySample( + rpc_client, monitor, robot_interface); + + if (monitor->safety_state->snapshot().latched) { + if (monitor->cancellation_confirmed.load() && + !controllerStillQuiescent( + rpc_client, robot_interface)) { + CMVR_LOG(WARNING) + << "[AuboArm] controller activity reappeared while safety was latched, id=" + << monitor->arm_id; + cancelForSafetyTransition(monitor); + } + if (!monitor->cancellation_confirmed.load()) { + (void)enforceControllerTermination( + rpc_client, monitor); + } + } + if (monitorWait( + monitor, kSafetyPollInterval)) { + break; + } + } + } + } + } + } catch (const std::exception& e) { + publishSafetyUnavailable(monitor, e.what()); + } + + if (rpc_client) { + try { + if (rpc_client->hasLogined()) { + rpc_client->logout(); + } + if (rpc_client->hasConnected()) { + rpc_client->disconnect(); + } + } catch (const std::exception& e) { + CMVR_LOG(WARNING) + << "[AuboArm] safety monitor cleanup failed, id=" + << monitor->arm_id << ", error=" << e.what(); + } + } + if (!monitor->stop_requested.load()) { + publishSafetyUnavailable(monitor, "monitor RPC disconnected"); + (void)monitorWait(monitor, kSafetyReconnectInterval); + } + } +} enum class CabinetIoOperation { GetDigitalInput, @@ -372,6 +1076,19 @@ struct AuboArm::SdkState { std::shared_ptr rpc_client; std::shared_ptr motion_state{ std::make_shared()}; + std::shared_ptr safety_monitor; + std::thread safety_monitor_thread; + + ~SdkState() + { + if (safety_monitor) { + safety_monitor->stop_requested.store(true); + safety_monitor->wait_cv.notify_all(); + } + if (safety_monitor_thread.joinable()) { + safety_monitor_thread.join(); + } + } }; AuboArm::AuboArm(const config::RobotArmConfig& cfg) @@ -565,13 +1282,31 @@ ArmState AuboArm::getRobotState() const { ArmState state; state.connected = connected_.load(); - state.powered_on = state.connected; - state.brake_released = state.connected; state.moving = busy(); state.robot_mode = getRobotMode(); state.safety_mode = getSafetyMode(); state.control_mode = getControlMode(); - state.emergency_stopped = emergency_stopped_; + state.powered_on = state.connected && + state.robot_mode != RobotMode::Disconnected && + state.robot_mode != RobotMode::PowerOff && + state.robot_mode != RobotMode::Unknown; + state.brake_released = + state.robot_mode == RobotMode::Running && + state.safety_mode != SafetyMode::EmergencyStop && + state.safety_mode != SafetyMode::SystemEmergencyStop && + state.safety_mode != SafetyMode::SafeguardStop; + state.program_running = false; + { + std::lock_guard lock(mutex_); + if (sdk_ && sdk_->safety_monitor) { + state.program_running = + sdk_->safety_monitor->runtime_state.load() == + static_cast(RuntimeState::Running); + } + } + state.protective_stopped = isProtectiveStopped(); + state.emergency_stopped = isEmergencyStopped(); + state.fault = isFault(); state.speed_scaling = speed_scaling_; state.actual_joint_state = getJointState(); state.target_joint_state = state.actual_joint_state; @@ -656,10 +1391,75 @@ RobotMode AuboArm::getRobotMode() const if (!connected_.load()) { return RobotMode::Disconnected; } - if (emergency_stopped_) { + const auto safety_mode = getSafetyMode(); + if (safety_mode == SafetyMode::Fault) { + return RobotMode::Fault; + } + if (safety_mode == SafetyMode::ProtectiveStop || + safety_mode == SafetyMode::SafeguardStop || + safety_mode == SafetyMode::EmergencyStop || + safety_mode == SafetyMode::SystemEmergencyStop) { return RobotMode::Stopped; } - return busy() ? RobotMode::Running : RobotMode::Idle; + + std::lock_guard lock(mutex_); + if (!sdk_ || !sdk_->safety_monitor) { + return RobotMode::Unknown; + } + return publicRobotMode(static_cast( + sdk_->safety_monitor->robot_mode.load())); +} + +SafetyMode AuboArm::getSafetyMode() const +{ + if (!connected_.load()) { + return SafetyMode::Unknown; + } + std::lock_guard lock(mutex_); + if (!sdk_ || !sdk_->safety_monitor) { + return SafetyMode::Unknown; + } + const auto monitor = sdk_->safety_monitor; + if (!safetySampleFresh(monitor)) { + publishSafetyUnavailable(monitor, "sample is stale"); + } + const auto snapshot = monitor->safety_state->snapshot(); + return publicSafetyMode( + snapshot.latched ? snapshot.latched_reason : snapshot.observed); +} + +ControlMode AuboArm::getControlMode() const +{ + if (!connected_.load()) { + return ControlMode::None; + } + std::lock_guard lock(mutex_); + if (sdk_ && sdk_->safety_monitor && + sdk_->safety_monitor->servo_mode_select.load() != 0) { + return ControlMode::Servo; + } + return ControlMode::Position; +} + +bool AuboArm::isProtectiveStopped() const +{ + const auto mode = getSafetyMode(); + return mode == SafetyMode::ProtectiveStop || + mode == SafetyMode::SafeguardStop; +} + +bool AuboArm::isEmergencyStopped() const +{ + const auto mode = getSafetyMode(); + return emergency_stopped_.load() || + mode == SafetyMode::EmergencyStop || + mode == SafetyMode::SystemEmergencyStop; +} + +bool AuboArm::isFault() const +{ + return getSafetyMode() == SafetyMode::Fault || + getRobotMode() == RobotMode::Fault; } bool AuboArm::busy() const @@ -673,19 +1473,123 @@ bool AuboArm::busy() const Result AuboArm::torqueOn() { - const auto ready = ensureConnected_("torqueOn"); - if (!ready.ok()) { - return ready; + std::shared_ptr rpc_client; + std::shared_ptr monitor; + { + std::lock_guard lock(mutex_); + const auto ready = ensureConnected_("torqueOn"); + if (!ready.ok()) { + return ready; + } + rpc_client = sdk_->rpc_client; + monitor = sdk_->safety_monitor; } try { - const auto robot_names = sdk_->rpc_client->getRobotNames(); - if (robot_names.empty()) { - return Result::failure(ArmErrorCode::RobotNotReady, "[AuboArm] robot name list is empty"); + if (!monitor) { + return Result::failure( + ArmErrorCode::RobotNotReady, + "[AuboArm] torqueOn failed: hardware safety monitor is unavailable"); } - auto robot_interface = sdk_->rpc_client->getRobotInterface(robot_names.front()); - if (!robot_interface) { - return Result::failure(ArmErrorCode::RobotNotReady, "[AuboArm] robot interface is null"); + std::unique_lock command_rpc_lock( + monitor->command_rpc_mutex); + Result interface_result; + auto robot_interface = getPrimaryRobotInterface( + rpc_client, "torqueOn", interface_result); + if (!interface_result.ok()) { + return interface_result; + } + + refreshSafetySample(rpc_client, monitor, robot_interface); + auto safety_snapshot = monitor->safety_state->snapshot(); + const std::uint64_t entry_safety_epoch = safety_snapshot.epoch; + if (monitor->emergency_stop_source.load() != 0) { + return Result::failure( + ArmErrorCode::RobotInEmergencyStop, + "[AuboArm] torqueOn rejected: hardware emergency-stop input is still active"); + } + + aubo_internal::RecoveryToken recovery_token; + std::unique_ptr recovery_guard; + bool recovering = safety_snapshot.latched; + if (recovering && + !aubo_internal::isMotionSafe(safety_snapshot.observed)) { + const auto condition = safety_snapshot.observed; + if (aubo_internal::needsProtectiveUnlock(condition)) { + return Result::failure( + ArmErrorCode::RobotInProtectiveStop, + "[AuboArm] torqueOn rejected: ProtectiveStop/Violation must be cleared with unlockProtectiveStop first"); + } + if (condition == + aubo_internal::SafetyCondition::SafeguardStop) { + return Result::failure( + ArmErrorCode::RobotInProtectiveStop, + "[AuboArm] torqueOn rejected: SafeguardStop requires the external safety IO to be cleared"); + } + if (condition == + aubo_internal::SafetyCondition::Recovery) { + return Result::failure( + ArmErrorCode::RobotNotReady, + "[AuboArm] torqueOn rejected: Recovery mode requires manually moving the arm inside its safety limits"); + } + if (!aubo_internal::needsInterfaceBoardRestart(condition)) { + return Result::failure( + ArmErrorCode::RobotNotReady, + "[AuboArm] torqueOn rejected: safety state is " + + std::string(safetyConditionName(condition))); + } + + const int restart_ret = + robot_interface->getRobotManage()->restartInterfaceBoard(); + if (restart_ret != arcs::common_interface::AUBO_OK) { + return Result::failure( + ArmErrorCode::CommandFailed, + "[AuboArm] torqueOn recovery failed: restartInterfaceBoard ret=" + + std::to_string(restart_ret)); + } + + const auto safety_deadline = + std::chrono::steady_clock::now() + + std::chrono::seconds(10); + do { + std::this_thread::sleep_for( + std::chrono::milliseconds(100)); + refreshSafetySample( + rpc_client, monitor, robot_interface); + safety_snapshot = monitor->safety_state->snapshot(); + if (aubo_internal::isMotionSafe( + safety_snapshot.observed)) { + break; + } + } while (std::chrono::steady_clock::now() < + safety_deadline); + } + + safety_snapshot = monitor->safety_state->snapshot(); + if (safety_snapshot.epoch != entry_safety_epoch) { + return Result::failure( + ArmErrorCode::CommandRejected, + "[AuboArm] torqueOn recovery rejected: a new safety event occurred while resetting the controller; retry recovery explicitly"); + } + if (!aubo_internal::isMotionSafe(safety_snapshot.observed)) { + return Result::failure( + ArmErrorCode::RobotNotReady, + "[AuboArm] torqueOn rejected: safety state is " + + std::string(safetyConditionName( + safety_snapshot.observed))); + } + + if (recovering) { + const auto token = monitor->safety_state->beginRecovery( + entry_safety_epoch); + if (!token.has_value()) { + return Result::failure( + ArmErrorCode::RobotNotReady, + "[AuboArm] torqueOn recovery rejected: safety latch changed"); + } + recovery_token = *token; + recovery_guard = std::make_unique( + monitor->safety_state, recovery_token); } double mass = 0.0; @@ -694,18 +1598,106 @@ Result AuboArm::torqueOn() std::vector inertia(6, 0.0); robot_interface->getRobotConfig()->setPayload(mass, cog, aom, inertia); - const auto current_mode = robot_interface->getRobotState()->getRobotModeType(); - if (current_mode != arcs::common_interface::RobotModeType::Running) { - robot_interface->getRobotManage()->poweron(); + auto current_mode = + robot_interface->getRobotState()->getRobotModeType(); + if (current_mode != RobotModeType::Running && + current_mode != RobotModeType::Idle) { + const int poweron_ret = + robot_interface->getRobotManage()->poweron(); + if (poweron_ret != arcs::common_interface::AUBO_OK) { + return Result::failure( + ArmErrorCode::CommandFailed, + "[AuboArm] torqueOn failed: poweron ret=" + + std::to_string(poweron_ret)); + } if (!waitForRobotMode(robot_interface, arcs::common_interface::RobotModeType::Idle)) { return Result::failure(ArmErrorCode::CommandFailed, "[AuboArm] torqueOn failed: timeout waiting for Idle"); } - robot_interface->getRobotManage()->startup(); + current_mode = RobotModeType::Idle; + } + + refreshSafetySample(rpc_client, monitor, robot_interface); + auto before_brake_release = + monitor->safety_state->snapshot(); + const std::uint64_t expected_epoch = recovering + ? recovery_token.epoch + : entry_safety_epoch; + const bool recovery_token_current = !recovering || + (before_brake_release.recovery_in_progress && + before_brake_release.epoch == recovery_token.epoch); + if (before_brake_release.epoch != expected_epoch || + !recovery_token_current || before_brake_release.latched != recovering || + !aubo_internal::isMotionSafe( + before_brake_release.observed) || + monitor->emergency_stop_source.load() != 0) { + return Result::failure( + ArmErrorCode::CommandRejected, + "[AuboArm] torqueOn rejected: safety state changed before brake release; the new event remains latched"); + } + + if (recovering) { + cancelForSafetyTransition(monitor); + const bool cleanup_ok = current_mode == RobotModeType::Running + ? enforceControllerTermination(rpc_client, monitor) + : prepareControllerForStartup(rpc_client, monitor); + if (!cleanup_ok) { + return Result::failure( + ArmErrorCode::CommandFailed, + "[AuboArm] torqueOn recovery failed: old controller queue could not be acknowledged and cleared before brake release"); + } + } + + if (current_mode != RobotModeType::Running) { + const int startup_ret = + robot_interface->getRobotManage()->startup(); + if (startup_ret != arcs::common_interface::AUBO_OK) { + return Result::failure( + ArmErrorCode::CommandFailed, + "[AuboArm] torqueOn failed: startup ret=" + + std::to_string(startup_ret)); + } if (!waitForRobotMode(robot_interface, arcs::common_interface::RobotModeType::Running)) { return Result::failure(ArmErrorCode::CommandFailed, "[AuboArm] torqueOn failed: timeout waiting for Running"); } } - emergency_stopped_ = false; + + refreshSafetySample(rpc_client, monitor, robot_interface); + const auto after_startup = monitor->safety_state->snapshot(); + const bool post_recovery_token_current = !recovering || + (after_startup.recovery_in_progress && + after_startup.epoch == recovery_token.epoch); + if (after_startup.epoch != expected_epoch || + !post_recovery_token_current || after_startup.latched != recovering || + !aubo_internal::isMotionSafe(after_startup.observed) || + monitor->emergency_stop_source.load() != 0 || + monitor->robot_mode.load() != + static_cast(RobotModeType::Running)) { + return Result::failure( + ArmErrorCode::CommandRejected, + "[AuboArm] torqueOn rejected: safety state changed during startup; the new event remains latched"); + } + if (recovering) { + cancelForSafetyTransition(monitor); + if (!enforceControllerTermination(rpc_client, monitor)) { + return Result::failure( + ArmErrorCode::CommandFailed, + "[AuboArm] torqueOn recovery failed: controller did not reach an empty, steady state after startup"); + } + refreshSafetySample(rpc_client, monitor, robot_interface); + const bool robot_running = + monitor->robot_mode.load() == + static_cast(RobotModeType::Running); + if (!recovery_guard->complete( + robot_running, + true, + monitor->cancellation_confirmed.load())) { + return Result::failure( + ArmErrorCode::CommandRejected, + "[AuboArm] torqueOn recovery failed: safety state changed during recovery"); + } + } + emergency_stopped_.store(false); + servo_mode_.store(false); return Result::success(); } catch (const std::exception& e) { return Result::failure(ArmErrorCode::CommandFailed, std::string("[AuboArm] torqueOn failed: ") + e.what()); @@ -746,7 +1738,28 @@ Result AuboArm::calibrateZeroQ(const std::string& joint_name) Result AuboArm::emergencyStop() { - emergency_stopped_ = true; + emergency_stopped_.store(true); + { + std::lock_guard lock(mutex_); + if (sdk_ && sdk_->safety_monitor) { + sdk_->safety_monitor->safety_state->observe( + aubo_internal::SafetyCondition::RobotEmergencyStop); + cancelForSafetyTransition(sdk_->safety_monitor); + } + } + return stopMotion(); +} + +Result AuboArm::protectiveStop() +{ + { + std::lock_guard lock(mutex_); + if (sdk_ && sdk_->safety_monitor) { + sdk_->safety_monitor->safety_state->observe( + aubo_internal::SafetyCondition::ProtectiveStop); + cancelForSafetyTransition(sdk_->safety_monitor); + } + } return stopMotion(); } @@ -771,8 +1784,16 @@ Result AuboArm::moveJ(const JointPositionCommand& target, const MotionOptions& o if (!locked_ready.ok()) { return locked_ready; } + std::uint64_t safety_epoch = 0; + const auto safety_ready = ensureMotionReady_( + "moveJ", safety_epoch); + if (!safety_ready.ok()) { + return safety_ready; + } const auto rpc_client = sdk_->rpc_client; const auto motion_state = sdk_->motion_state; + const auto safety_monitor = sdk_->safety_monitor; + const aubo_internal::SafetyPermit safety_permit{safety_epoch}; const auto motion = motion_state->begin( aubo_internal::MotionKind::Joint); if (!motion.started()) { @@ -797,6 +1818,12 @@ Result AuboArm::moveJ(const JointPositionCommand& target, const MotionOptions& o auto motion_control = robot_interface->getMotionControl(); motion_control->setSpeedFraction(speed_scaling_); motion_owner.requireExplicitSettlement(); + if (!validateSafetyPermit(safety_monitor, safety_permit)) { + motion_owner.settle(); + return Result::failure( + ArmErrorCode::CommandRejected, + "[AuboArm] moveJ cancelled by hardware safety before submission"); + } const int ret = motion_control->moveJoint( target.position, options.acceleration > 0.0 ? options.acceleration : 0.5, @@ -812,13 +1839,28 @@ Result AuboArm::moveJ(const JointPositionCommand& target, const MotionOptions& o ret, arcs::common_interface::AUBO_OK, arcs::common_interface::AUBO_REQUEST_IGNORE, - [&robot_interface, motion_state, token = motion.token]() { + [&robot_interface, + motion_state, + safety_monitor, + safety_permit, + token = motion.token]() { return waitArrival( robot_interface, - [motion_state, token]() { - return motion_state->cancelled(token); + [motion_state, + safety_monitor, + safety_permit, + token]() { + return motion_state->cancelled(token) || + !validateSafetyPermit( + safety_monitor, safety_permit); }); }); + if (!validateSafetyPermit(safety_monitor, safety_permit)) { + motion_owner.settle(); + return Result::failure( + ArmErrorCode::CommandRejected, + "[AuboArm] moveJ cancelled by hardware safety event"); + } switch (outcome) { case aubo_internal::MotionCommandOutcome::CompletedWithoutMotion: CMVR_LOG(DEBUG) << "[AuboArm] moveJ completed without motion: sdk ret=" @@ -833,7 +1875,7 @@ Result AuboArm::moveJ(const JointPositionCommand& target, const MotionOptions& o motion_owner.settle(); return Result::failure( ArmErrorCode::CommandRejected, - "[AuboArm] moveJ stopped by stopMotion"); + "[AuboArm] moveJ cancelled by stopMotion or hardware safety event"); case aubo_internal::MotionCommandOutcome::SubmitFailed: return Result::failure( ArmErrorCode::CommandFailed, @@ -860,8 +1902,16 @@ Result AuboArm::speedJ(const JointVelocityCommand& velocity, double acceleration if (!locked_ready.ok()) { return locked_ready; } + std::uint64_t safety_epoch = 0; + const auto safety_ready = ensureMotionReady_( + "speedJ", safety_epoch); + if (!safety_ready.ok()) { + return safety_ready; + } const auto rpc_client = sdk_->rpc_client; const auto motion_state = sdk_->motion_state; + const auto safety_monitor = sdk_->safety_monitor; + const aubo_internal::SafetyPermit safety_permit{safety_epoch}; const auto motion = motion_state->begin( aubo_internal::MotionKind::Joint, true); if (!motion.started()) { @@ -887,21 +1937,23 @@ Result AuboArm::speedJ(const JointVelocityCommand& velocity, double acceleration const double resolved_duration = duration > 0.0 ? duration : 100.0; motion_owner.requireExplicitSettlement(); submit_lock.unlock(); - if (motion_state->cancelled(motion.token)) { + if (motion_state->cancelled(motion.token) || + !validateSafetyPermit(safety_monitor, safety_permit)) { motion_owner.settle(); return Result::failure( ArmErrorCode::CommandRejected, - "[AuboArm] speedJ stopped by stopMotion before submission"); + "[AuboArm] speedJ cancelled before submission"); } const int ret = robot_interface->getMotionControl()->speedJoint( velocity.velocity, resolved_acceleration, resolved_duration); - if (motion_state->cancelled(motion.token)) { + if (motion_state->cancelled(motion.token) || + !validateSafetyPermit(safety_monitor, safety_permit)) { motion_owner.settle(); return Result::failure( ArmErrorCode::CommandRejected, - "[AuboArm] speedJ stopped by stopMotion"); + "[AuboArm] speedJ cancelled by stopMotion or hardware safety event"); } if (ret != 0) { motion_owner.settle(); @@ -930,8 +1982,16 @@ Result AuboArm::moveL(const CartesianPose& target, const MotionOptions& options, if (!locked_ready.ok()) { return locked_ready; } + std::uint64_t safety_epoch = 0; + const auto safety_ready = ensureMotionReady_( + "moveL", safety_epoch); + if (!safety_ready.ok()) { + return safety_ready; + } const auto rpc_client = sdk_->rpc_client; const auto motion_state = sdk_->motion_state; + const auto safety_monitor = sdk_->safety_monitor; + const aubo_internal::SafetyPermit safety_permit{safety_epoch}; const auto motion = motion_state->begin( aubo_internal::MotionKind::Linear); if (!motion.started()) { @@ -959,6 +2019,12 @@ Result AuboArm::moveL(const CartesianPose& target, const MotionOptions& options, robot_interface->getRobotConfig()->setTcpOffset(tcp_offset); std::vector pose{target.x, target.y, target.z, target.rx, target.ry, target.rz}; motion_owner.requireExplicitSettlement(); + if (!validateSafetyPermit(safety_monitor, safety_permit)) { + motion_owner.settle(); + return Result::failure( + ArmErrorCode::CommandRejected, + "[AuboArm] moveL cancelled by hardware safety before submission"); + } const int ret = motion_control->moveLine( pose, options.acceleration > 0.0 ? options.acceleration : 0.5, @@ -974,13 +2040,28 @@ Result AuboArm::moveL(const CartesianPose& target, const MotionOptions& options, ret, arcs::common_interface::AUBO_OK, arcs::common_interface::AUBO_REQUEST_IGNORE, - [&robot_interface, motion_state, token = motion.token]() { + [&robot_interface, + motion_state, + safety_monitor, + safety_permit, + token = motion.token]() { return waitArrival( robot_interface, - [motion_state, token]() { - return motion_state->cancelled(token); + [motion_state, + safety_monitor, + safety_permit, + token]() { + return motion_state->cancelled(token) || + !validateSafetyPermit( + safety_monitor, safety_permit); }); }); + if (!validateSafetyPermit(safety_monitor, safety_permit)) { + motion_owner.settle(); + return Result::failure( + ArmErrorCode::CommandRejected, + "[AuboArm] moveL cancelled by hardware safety event"); + } switch (outcome) { case aubo_internal::MotionCommandOutcome::CompletedWithoutMotion: CMVR_LOG(DEBUG) << "[AuboArm] moveL completed without motion: sdk ret=" @@ -995,7 +2076,7 @@ Result AuboArm::moveL(const CartesianPose& target, const MotionOptions& options, motion_owner.settle(); return Result::failure( ArmErrorCode::CommandRejected, - "[AuboArm] moveL stopped by stopMotion"); + "[AuboArm] moveL cancelled by stopMotion or hardware safety event"); case aubo_internal::MotionCommandOutcome::SubmitFailed: return Result::failure( ArmErrorCode::CommandFailed, @@ -1018,8 +2099,16 @@ Result AuboArm::speedL(const CartesianVelocity& velocity, double acceleration, d if (!locked_ready.ok()) { return locked_ready; } + std::uint64_t safety_epoch = 0; + const auto safety_ready = ensureMotionReady_( + "speedL", safety_epoch); + if (!safety_ready.ok()) { + return safety_ready; + } const auto rpc_client = sdk_->rpc_client; const auto motion_state = sdk_->motion_state; + const auto safety_monitor = sdk_->safety_monitor; + const aubo_internal::SafetyPermit safety_permit{safety_epoch}; const auto motion = motion_state->begin( aubo_internal::MotionKind::Linear, true); if (!motion.started()) { @@ -1077,21 +2166,23 @@ Result AuboArm::speedL(const CartesianVelocity& velocity, double acceleration, d const double resolved_duration = duration > 0.0 ? duration : 100.0; motion_owner.requireExplicitSettlement(); submit_lock.unlock(); - if (motion_state->cancelled(motion.token)) { + if (motion_state->cancelled(motion.token) || + !validateSafetyPermit(safety_monitor, safety_permit)) { motion_owner.settle(); return Result::failure( ArmErrorCode::CommandRejected, - "[AuboArm] speedL stopped by stopMotion before submission"); + "[AuboArm] speedL cancelled before submission"); } const int ret = robot_interface->getMotionControl()->speedLine( speed, resolved_acceleration, resolved_duration); - if (motion_state->cancelled(motion.token)) { + if (motion_state->cancelled(motion.token) || + !validateSafetyPermit(safety_monitor, safety_permit)) { motion_owner.settle(); return Result::failure( ArmErrorCode::CommandRejected, - "[AuboArm] speedL stopped by stopMotion"); + "[AuboArm] speedL cancelled by stopMotion or hardware safety event"); } if (ret != 0) { motion_owner.settle(); @@ -1279,6 +2370,14 @@ Result AuboArm::startServoMode(const ServoOptions& options) if (!ready.ok()) { return ready; } + std::uint64_t safety_epoch = 0; + const auto safety_ready = ensureMotionReady_( + "startServoMode", safety_epoch); + if (!safety_ready.ok()) { + return safety_ready; + } + const auto safety_monitor = sdk_->safety_monitor; + const aubo_internal::SafetyPermit safety_permit{safety_epoch}; try { Result interface_result; @@ -1286,6 +2385,11 @@ Result AuboArm::startServoMode(const ServoOptions& options) if (!interface_result.ok()) { return interface_result; } + if (!validateSafetyPermit(safety_monitor, safety_permit)) { + return Result::failure( + ArmErrorCode::CommandRejected, + "[AuboArm] startServoMode cancelled by hardware safety"); + } const int ret = robot_interface->getMotionControl()->setServoModeSelect(kAuboServoMode); if (ret != 0) { return Result::failure(ArmErrorCode::CommandFailed, @@ -1295,8 +2399,15 @@ Result AuboArm::startServoMode(const ServoOptions& options) return Result::failure(ArmErrorCode::Timeout, "[AuboArm] startServoMode failed: timeout waiting for servo mode"); } + if (!validateSafetyPermit(safety_monitor, safety_permit)) { + (void)robot_interface->getMotionControl()->setServoModeSelect(0); + return Result::failure( + ArmErrorCode::CommandRejected, + "[AuboArm] startServoMode cancelled by hardware safety event"); + } servo_options_ = options; servo_mode_.store(true); + safety_monitor->servo_mode_select.store(kAuboServoMode); return Result::success(); } catch (const std::exception& e) { return Result::failure(ArmErrorCode::CommandFailed, @@ -1314,6 +2425,13 @@ Result AuboArm::servoJ(const JointPositionCommand& target) if (!ready.ok()) { return ready; } + std::uint64_t safety_epoch = 0; + const auto safety_ready = ensureMotionReady_("servoJ", safety_epoch); + if (!safety_ready.ok()) { + return safety_ready; + } + const auto safety_monitor = sdk_->safety_monitor; + const aubo_internal::SafetyPermit safety_permit{safety_epoch}; try { Result interface_result; @@ -1328,6 +2446,11 @@ Result AuboArm::servoJ(const JointPositionCommand& target) } } const double period = servo_options_.period > 0.0 ? servo_options_.period : 0.008; + if (!validateSafetyPermit(safety_monitor, safety_permit)) { + return Result::failure( + ArmErrorCode::CommandRejected, + "[AuboArm] servoJ cancelled by hardware safety before submission"); + } const int ret = robot_interface->getMotionControl()->servoJoint( target.position, 0.0, @@ -1339,6 +2462,11 @@ Result AuboArm::servoJ(const JointPositionCommand& target) return Result::failure(ArmErrorCode::CommandFailed, "[AuboArm] servoJ failed: ret=" + std::to_string(ret)); } + if (!validateSafetyPermit(safety_monitor, safety_permit)) { + return Result::failure( + ArmErrorCode::CommandRejected, + "[AuboArm] servoJ cancelled by hardware safety event"); + } return Result::success(); } catch (const std::exception& e) { return Result::failure(ArmErrorCode::CommandFailed, std::string("[AuboArm] servoJ failed: ") + e.what()); @@ -1351,6 +2479,13 @@ Result AuboArm::servoL(const CartesianPose& target, FrameType frame) if (!ready.ok()) { return ready; } + std::uint64_t safety_epoch = 0; + const auto safety_ready = ensureMotionReady_("servoL", safety_epoch); + if (!safety_ready.ok()) { + return safety_ready; + } + const auto safety_monitor = sdk_->safety_monitor; + const aubo_internal::SafetyPermit safety_permit{safety_epoch}; try { Result interface_result; @@ -1379,6 +2514,11 @@ Result AuboArm::servoL(const CartesianPose& target, FrameType frame) } const double period = servo_options_.period > 0.0 ? servo_options_.period : 0.008; + if (!validateSafetyPermit(safety_monitor, safety_permit)) { + return Result::failure( + ArmErrorCode::CommandRejected, + "[AuboArm] servoL cancelled by hardware safety before submission"); + } const int ret = robot_interface->getMotionControl()->servoCartesian( pose, 0.0, @@ -1390,6 +2530,11 @@ Result AuboArm::servoL(const CartesianPose& target, FrameType frame) return Result::failure(ArmErrorCode::CommandFailed, "[AuboArm] servoL failed: ret=" + std::to_string(ret)); } + if (!validateSafetyPermit(safety_monitor, safety_permit)) { + return Result::failure( + ArmErrorCode::CommandRejected, + "[AuboArm] servoL cancelled by hardware safety event"); + } return Result::success(); } catch (const std::exception& e) { return Result::failure(ArmErrorCode::CommandFailed, std::string("[AuboArm] servoL failed: ") + e.what()); @@ -1422,6 +2567,9 @@ Result AuboArm::stopServoMode() } const int ret = robot_interface->getMotionControl()->setServoModeSelect(0); servo_mode_.store(false); + if (sdk_->safety_monitor) { + sdk_->safety_monitor->servo_mode_select.store(0); + } if (ret != 0) { return Result::failure(ArmErrorCode::CommandFailed, "[AuboArm] stopServoMode failed: ret=" + std::to_string(ret)); @@ -1450,13 +2598,7 @@ Result AuboArm::connect(const std::string& ip, const int port) try { const int resolved_port = port > 0 ? port : 30004; auto sdk_state = std::make_unique(); - sdk_state->rpc_client = std::shared_ptr( - ::createRpcClient(), - [](arcs::aubo_sdk::RpcClient* client) { - if (client) { - ::destroyRpcClient(client); - } - }); + sdk_state->rpc_client = makeRpcClient(); if (!sdk_state->rpc_client) { return Result::failure(ArmErrorCode::ConnectionFailed, "[AuboArm] connect failed: create RPC client failed"); @@ -1492,8 +2634,38 @@ Result AuboArm::connect(const std::string& ip, const int port) "[AuboArm] connect failed: robot name list is empty"); } + auto robot_interface = sdk_state->rpc_client->getRobotInterface( + robot_names.front()); + if (!robot_interface) { + sdk_state->rpc_client->logout(); + sdk_state->rpc_client->disconnect(); + return Result::failure(ArmErrorCode::ConnectionFailed, + "[AuboArm] connect failed: robot interface is null"); + } + + sdk_state->safety_monitor = + std::make_shared(); + sdk_state->safety_monitor->motion_state = + sdk_state->motion_state; + sdk_state->safety_monitor->arm_id = id_; + publishSafetySample( + sdk_state->safety_monitor, + robot_interface->getRobotState()->getSafetyModeType(), + robot_interface->getRobotState()->getRobotModeType(), + sdk_state->rpc_client->getRuntimeMachine()->getRuntimeState(), + robot_interface->getRobotConfig() + ->getRobotEmergencyStopSource(), + robot_interface->getMotionControl()->getServoModeSelect()); + ip_ = ip; port_ = resolved_port; + sdk_state->safety_monitor_thread = std::thread( + runSafetyMonitor, + sdk_state->safety_monitor, + ip_, + port_, + username_, + password_); sdk_ = std::move(sdk_state); connected_.store(true); return Result::success(); @@ -1509,21 +2681,38 @@ Result AuboArm::disconnect() { std::lock_guard lock(mutex_); try { - if (sdk_ && sdk_->rpc_client) { - if (sdk_->rpc_client->hasLogined()) { - sdk_->rpc_client->logout(); - } - if (sdk_->rpc_client->hasConnected()) { - sdk_->rpc_client->disconnect(); + connected_.store(false); + if (sdk_ && sdk_->safety_monitor) { + sdk_->safety_monitor->stop_requested.store(true); + sdk_->safety_monitor->wait_cv.notify_all(); + } + if (sdk_ && sdk_->safety_monitor_thread.joinable()) { + sdk_->safety_monitor_thread.join(); + } + const auto close_command_rpc = [this]() { + if (sdk_ && sdk_->rpc_client) { + if (sdk_->rpc_client->hasLogined()) { + sdk_->rpc_client->logout(); + } + if (sdk_->rpc_client->hasConnected()) { + sdk_->rpc_client->disconnect(); + } } + }; + if (sdk_ && sdk_->safety_monitor) { + std::unique_lock command_rpc_lock( + sdk_->safety_monitor->command_rpc_mutex); + close_command_rpc(); + } else { + close_command_rpc(); } } catch (const std::exception& e) { CMVR_LOG(ERROR) << "[AuboArm] disconnect failed: " << e.what(); } sdk_.reset(); - connected_.store(false); busy_.store(false); servo_mode_.store(false); + emergency_stopped_.store(false); return Result::success(); } @@ -1533,6 +2722,234 @@ Result AuboArm::shutdown() return disconnect(); } +Result AuboArm::clearFault() +{ + std::shared_ptr rpc_client; + std::shared_ptr monitor; + { + std::lock_guard lock(mutex_); + const auto ready = ensureConnected_("clearFault"); + if (!ready.ok()) { + return ready; + } + rpc_client = sdk_->rpc_client; + monitor = sdk_->safety_monitor; + } + + try { + if (!monitor) { + return Result::failure( + ArmErrorCode::RobotNotReady, + "[AuboArm] clearFault failed: hardware safety monitor is unavailable"); + } + std::unique_lock command_rpc_lock( + monitor->command_rpc_mutex); + Result interface_result; + auto robot_interface = getPrimaryRobotInterface( + rpc_client, "clearFault", interface_result); + if (!interface_result.ok()) { + return interface_result; + } + refreshSafetySample(rpc_client, monitor, robot_interface); + auto snapshot = monitor->safety_state->snapshot(); + const std::uint64_t expected_safety_epoch = snapshot.epoch; + if (!snapshot.latched && + aubo_internal::isMotionSafe(snapshot.observed) && + monitor->robot_mode.load() != + static_cast(RobotModeType::Error)) { + return Result::success(); + } + if (!snapshot.latched && + monitor->robot_mode.load() == + static_cast(RobotModeType::Error)) { + return Result::failure( + ArmErrorCode::RobotInFault, + "[AuboArm] clearFault rejected: RobotMode is Error even though the safety mode is Normal/Reduced"); + } + if (monitor->emergency_stop_source.load() != 0) { + return Result::failure( + ArmErrorCode::RobotInEmergencyStop, + "[AuboArm] clearFault rejected: hardware emergency-stop input is still active"); + } + + if (aubo_internal::needsProtectiveUnlock(snapshot.observed) || + aubo_internal::needsProtectiveUnlock( + snapshot.latched_reason)) { + command_rpc_lock.unlock(); + return unlockProtectiveStop_(expected_safety_epoch); + } + + if (aubo_internal::isMotionSafe(snapshot.observed)) { + if (monitor->robot_mode.load() == + static_cast(RobotModeType::Running)) { + command_rpc_lock.unlock(); + return completeSafetyRecovery_( + "clearFault", expected_safety_epoch); + } + return Result::failure( + ArmErrorCode::RobotNotPowered, + "[AuboArm] safety condition is clear, but torqueOn is required to verify the old queue and complete recovery"); + } + + if (!aubo_internal::needsInterfaceBoardRestart( + snapshot.observed)) { + const std::string guidance = + snapshot.observed == + aubo_internal::SafetyCondition::SafeguardStop + ? "clear the external safety IO" + : (snapshot.observed == + aubo_internal::SafetyCondition::Recovery + ? "manually move the arm inside its safety limits" + : "restore a valid controller safety state"); + return Result::failure( + ArmErrorCode::RobotNotReady, + "[AuboArm] clearFault rejected for " + + std::string(safetyConditionName(snapshot.observed)) + + ": " + guidance); + } + + const int ret = + robot_interface->getRobotManage()->restartInterfaceBoard(); + if (ret != arcs::common_interface::AUBO_OK) { + return Result::failure( + ArmErrorCode::CommandFailed, + "[AuboArm] clearFault failed: restartInterfaceBoard ret=" + + std::to_string(ret)); + } + + const auto deadline = + std::chrono::steady_clock::now() + std::chrono::seconds(10); + while (std::chrono::steady_clock::now() < deadline) { + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + refreshSafetySample(rpc_client, monitor, robot_interface); + snapshot = monitor->safety_state->snapshot(); + if (aubo_internal::isMotionSafe(snapshot.observed)) { + break; + } + } + if (!aubo_internal::isMotionSafe(snapshot.observed)) { + return Result::failure( + ArmErrorCode::Timeout, + "[AuboArm] clearFault failed: timeout waiting for a safe controller state"); + } + if (monitor->robot_mode.load() == + static_cast(RobotModeType::Running)) { + command_rpc_lock.unlock(); + return completeSafetyRecovery_( + "clearFault", expected_safety_epoch); + } + return Result::failure( + ArmErrorCode::RobotNotPowered, + "[AuboArm] controller fault was reset, but the safety latch remains until torqueOn verifies an empty queue in Running mode"); + } catch (const std::exception& e) { + return Result::failure( + ArmErrorCode::CommandFailed, + std::string("[AuboArm] clearFault failed: ") + e.what()); + } +} + +Result AuboArm::unlockProtectiveStop() +{ + return unlockProtectiveStop_(std::nullopt); +} + +Result AuboArm::unlockProtectiveStop_( + const std::optional expected_safety_epoch) +{ + std::shared_ptr rpc_client; + std::shared_ptr monitor; + { + std::lock_guard lock(mutex_); + const auto ready = ensureConnected_("unlockProtectiveStop"); + if (!ready.ok()) { + return ready; + } + rpc_client = sdk_->rpc_client; + monitor = sdk_->safety_monitor; + } + + try { + if (!monitor) { + return Result::failure( + ArmErrorCode::RobotNotReady, + "[AuboArm] unlockProtectiveStop failed: hardware safety monitor is unavailable"); + } + std::unique_lock command_rpc_lock( + monitor->command_rpc_mutex); + Result interface_result; + auto robot_interface = getPrimaryRobotInterface( + rpc_client, "unlockProtectiveStop", interface_result); + if (!interface_result.ok()) { + return interface_result; + } + refreshSafetySample(rpc_client, monitor, robot_interface); + auto snapshot = monitor->safety_state->snapshot(); + const std::uint64_t recovery_epoch = + expected_safety_epoch.value_or(snapshot.epoch); + if (snapshot.epoch != recovery_epoch) { + return Result::failure( + ArmErrorCode::CommandRejected, + "[AuboArm] unlockProtectiveStop rejected: a newer safety event superseded this recovery request"); + } + if (!snapshot.latched && + aubo_internal::isMotionSafe(snapshot.observed)) { + return Result::success(); + } + if (monitor->emergency_stop_source.load() != 0) { + return Result::failure( + ArmErrorCode::RobotInEmergencyStop, + "[AuboArm] unlockProtectiveStop rejected: hardware emergency-stop input is active"); + } + + if (aubo_internal::needsProtectiveUnlock( + snapshot.observed)) { + const int ret = robot_interface->getRobotManage() + ->setUnlockProtectiveStop(); + if (ret != arcs::common_interface::AUBO_OK) { + return Result::failure( + ArmErrorCode::CommandFailed, + "[AuboArm] unlockProtectiveStop failed: sdk ret=" + + std::to_string(ret)); + } + } else if (!aubo_internal::isMotionSafe(snapshot.observed)) { + return Result::failure( + ArmErrorCode::RobotInProtectiveStop, + "[AuboArm] unlockProtectiveStop rejected: current safety state is " + + std::string(safetyConditionName(snapshot.observed))); + } + + const auto deadline = + std::chrono::steady_clock::now() + std::chrono::seconds(5); + while (std::chrono::steady_clock::now() < deadline) { + refreshSafetySample(rpc_client, monitor, robot_interface); + snapshot = monitor->safety_state->snapshot(); + if (aubo_internal::isMotionSafe(snapshot.observed)) { + break; + } + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + } + if (!aubo_internal::isMotionSafe(snapshot.observed)) { + return Result::failure( + ArmErrorCode::Timeout, + "[AuboArm] unlockProtectiveStop failed: safety mode did not return to Normal/Reduced"); + } + if (monitor->robot_mode.load() != + static_cast(RobotModeType::Running)) { + return Result::failure( + ArmErrorCode::RobotNotPowered, + "[AuboArm] protective stop was unlocked, but torqueOn is required to complete safety recovery"); + } + command_rpc_lock.unlock(); + return completeSafetyRecovery_( + "unlockProtectiveStop", recovery_epoch); + } catch (const std::exception& e) { + return Result::failure( + ArmErrorCode::CommandFailed, + std::string("[AuboArm] unlockProtectiveStop failed: ") + + e.what()); + } +} + Result AuboArm::loadProgram(const std::string& program_name) { if (program_name.empty()) { @@ -1561,12 +2978,33 @@ Result AuboArm::playProgram() if (!ready.ok()) { return ready; } + std::uint64_t safety_epoch = 0; + const auto safety_ready = ensureMotionReady_( + "playProgram", safety_epoch); + if (!safety_ready.ok()) { + return safety_ready; + } + const auto safety_monitor = sdk_->safety_monitor; + const aubo_internal::SafetyPermit safety_permit{safety_epoch}; try { + if (!validateSafetyPermit(safety_monitor, safety_permit)) { + return Result::failure( + ArmErrorCode::CommandRejected, + "[AuboArm] playProgram cancelled by hardware safety before submission"); + } const int ret = sdk_->rpc_client->getRuntimeMachine()->runProgram(); if (ret != 0) { return Result::failure(ArmErrorCode::CommandFailed, "[AuboArm] playProgram failed: ret=" + std::to_string(ret)); } + safety_monitor->runtime_state.store( + static_cast(RuntimeState::Running)); + if (!validateSafetyPermit(safety_monitor, safety_permit)) { + (void)sdk_->rpc_client->getRuntimeMachine()->abort(); + return Result::failure( + ArmErrorCode::CommandRejected, + "[AuboArm] playProgram cancelled by hardware safety event"); + } return Result::success(); } catch (const std::exception& e) { return Result::failure(ArmErrorCode::CommandFailed, @@ -1586,6 +3024,10 @@ Result AuboArm::pauseProgram() return Result::failure(ArmErrorCode::CommandFailed, "[AuboArm] pauseProgram failed: ret=" + std::to_string(ret)); } + if (sdk_->safety_monitor) { + sdk_->safety_monitor->runtime_state.store( + static_cast(RuntimeState::Paused)); + } return Result::success(); } catch (const std::exception& e) { return Result::failure(ArmErrorCode::CommandFailed, @@ -1605,6 +3047,11 @@ Result AuboArm::stopProgram() return Result::failure(ArmErrorCode::CommandFailed, "[AuboArm] stopProgram failed: ret=" + std::to_string(ret)); } + if (sdk_->safety_monitor) { + sdk_->safety_monitor->runtime_state.store( + static_cast(RuntimeState::Stopped)); + sdk_->safety_monitor->runtime_abort_required.store(false); + } return Result::success(); } catch (const std::exception& e) { return Result::failure(ArmErrorCode::CommandFailed, @@ -1701,6 +3148,119 @@ bool AuboArm::validDof_(const std::size_t size, std::string& error) const return true; } +Result AuboArm::completeSafetyRecovery_( + const std::string& context, + const std::uint64_t expected_safety_epoch) +{ + std::shared_ptr rpc_client; + std::shared_ptr monitor; + { + std::lock_guard lock(mutex_); + const auto ready = ensureConnected_(context); + if (!ready.ok()) { + return ready; + } + rpc_client = sdk_->rpc_client; + monitor = sdk_->safety_monitor; + } + + try { + if (!monitor) { + return Result::failure( + ArmErrorCode::RobotNotReady, + "[AuboArm] " + context + + " failed: hardware safety monitor is unavailable"); + } + std::unique_lock command_rpc_lock( + monitor->command_rpc_mutex); + Result interface_result; + auto robot_interface = getPrimaryRobotInterface( + rpc_client, context, interface_result); + if (!interface_result.ok()) { + return interface_result; + } + refreshSafetySample(rpc_client, monitor, robot_interface); + if (monitor->emergency_stop_source.load() != 0) { + return Result::failure( + ArmErrorCode::RobotInEmergencyStop, + "[AuboArm] " + context + + " rejected: hardware emergency-stop input is active"); + } + + const auto snapshot = monitor->safety_state->snapshot(); + if (snapshot.epoch != expected_safety_epoch) { + return Result::failure( + ArmErrorCode::CommandRejected, + "[AuboArm] " + context + + " rejected: a newer safety event superseded this recovery request"); + } + if (!snapshot.latched) { + return aubo_internal::isMotionSafe(snapshot.observed) + ? Result::success() + : Result::failure( + ArmErrorCode::RobotNotReady, + "[AuboArm] " + context + + " failed: safety state is " + + safetyConditionName(snapshot.observed)); + } + if (!aubo_internal::isMotionSafe(snapshot.observed)) { + return Result::failure( + ArmErrorCode::RobotNotReady, + "[AuboArm] " + context + + " rejected: hardware safety state is " + + safetyConditionName(snapshot.observed)); + } + if (monitor->robot_mode.load() != + static_cast(RobotModeType::Running)) { + return Result::failure( + ArmErrorCode::RobotNotReady, + "[AuboArm] " + context + + " rejected: robot must be Running before the safety latch can be cleared"); + } + + const auto recovery_token = + monitor->safety_state->beginRecovery( + expected_safety_epoch); + if (!recovery_token.has_value()) { + return Result::failure( + ArmErrorCode::CommandRejected, + "[AuboArm] " + context + + " rejected: another recovery is active or the safety state changed"); + } + SafetyRecoveryGuard recovery_guard{ + monitor->safety_state, *recovery_token}; + cancelForSafetyTransition(monitor); + if (!enforceControllerTermination(rpc_client, monitor)) { + return Result::failure( + ArmErrorCode::CommandFailed, + "[AuboArm] " + context + + " failed: old motion/program could not be terminated and verified"); + } + + refreshSafetySample(rpc_client, monitor, robot_interface); + const bool robot_running = + monitor->robot_mode.load() == + static_cast(RobotModeType::Running); + if (!recovery_guard.complete( + robot_running, + true, + monitor->cancellation_confirmed.load())) { + return Result::failure( + ArmErrorCode::CommandRejected, + "[AuboArm] " + context + + " failed: safety state changed while recovery was being verified"); + } + emergency_stopped_.store(false); + servo_mode_.store(false); + return Result::success(); + } catch (const std::exception& e) { + return Result::failure( + ArmErrorCode::CommandFailed, + "[AuboArm] " + context + + " failed during safety recovery: " + e.what()); + } +} + Result AuboArm::ensureConnected_(const std::string& context) const { if (!connected_.load()) { @@ -1714,4 +3274,87 @@ Result AuboArm::ensureConnected_(const std::string& context) const return Result::success(); } +Result AuboArm::ensureMotionReady_( + const std::string& context, + std::uint64_t& safety_epoch) const +{ + const auto connected = ensureConnected_(context); + if (!connected.ok()) { + return connected; + } + if (!sdk_->safety_monitor) { + return Result::failure( + ArmErrorCode::RobotNotReady, + "[AuboArm] " + context + + " rejected: hardware safety monitor is unavailable"); + } + + const auto monitor = sdk_->safety_monitor; + if (!safetySampleFresh(monitor)) { + publishSafetyUnavailable(monitor, "sample is stale"); + return Result::failure( + ArmErrorCode::RobotNotReady, + "[AuboArm] " + context + + " rejected: hardware safety state is unavailable or stale"); + } + + if (monitor->emergency_stop_source.load() != 0) { + const auto previous = monitor->safety_state->snapshot(); + monitor->safety_state->observe( + aubo_internal::SafetyCondition::RobotEmergencyStop); + if (!previous.latched || + previous.observed != + aubo_internal::SafetyCondition::RobotEmergencyStop) { + cancelForSafetyTransition(monitor); + } + } + + const auto snapshot = monitor->safety_state->snapshot(); + const auto condition = snapshot.latched + ? snapshot.latched_reason + : snapshot.observed; + if (snapshot.latched || + !aubo_internal::isMotionSafe(snapshot.observed)) { + ArmErrorCode code = ArmErrorCode::RobotNotReady; + if (condition == + aubo_internal::SafetyCondition::RobotEmergencyStop || + condition == + aubo_internal::SafetyCondition::SystemEmergencyStop) { + code = ArmErrorCode::RobotInEmergencyStop; + } else if ( + condition == aubo_internal::SafetyCondition::ProtectiveStop || + condition == aubo_internal::SafetyCondition::SafeguardStop) { + code = ArmErrorCode::RobotInProtectiveStop; + } else if ( + condition == aubo_internal::SafetyCondition::Fault || + condition == aubo_internal::SafetyCondition::Violation) { + code = ArmErrorCode::RobotInFault; + } + return Result::failure( + code, + "[AuboArm] " + context + + " rejected: hardware safety latch is " + + safetyConditionName(condition) + + "; clear the hardware condition and perform explicit recovery"); + } + + if (monitor->robot_mode.load() != + static_cast(RobotModeType::Running)) { + return Result::failure( + ArmErrorCode::RobotNotReady, + "[AuboArm] " + context + + " rejected: robot is not in Running mode"); + } + + const auto permit = monitor->safety_state->tryPermit(); + if (!permit.has_value()) { + return Result::failure( + ArmErrorCode::RobotNotReady, + "[AuboArm] " + context + + " rejected: no valid hardware safety permit"); + } + safety_epoch = permit->epoch; + return Result::success(); +} + } // namespace cmvr::device diff --git a/cmvr-es/devices/arm/aubo_arm/aubo_arm.h b/cmvr-es/devices/arm/aubo_arm/aubo_arm.h index 6f05af2d..e708c748 100644 --- a/cmvr-es/devices/arm/aubo_arm/aubo_arm.h +++ b/cmvr-es/devices/arm/aubo_arm/aubo_arm.h @@ -2,6 +2,7 @@ #define CMVR_ES_AUBO_ARM_H #include +#include #include #include #include @@ -30,19 +31,19 @@ public: JointGroupState getJointState() const override; CartesianPose getTcpPose(FrameType frame = FrameType::Base) const override; RobotMode getRobotMode() const override; - SafetyMode getSafetyMode() const override { return SafetyMode::Normal; } - ControlMode getControlMode() const override { return servo_mode_.load() ? ControlMode::Servo : ControlMode::Position; } + SafetyMode getSafetyMode() const override; + ControlMode getControlMode() const override; Result torqueOn() override; Result torqueOff() override; Result calibrateZeroQ(const std::string& joint_name) override; Result emergencyStop() override; - Result protectiveStop() override { return emergencyStop(); } + Result protectiveStop() override; Result setSpeedScaling(double scaling) override; double getSpeedScaling() const override { return speed_scaling_; } - bool isProtectiveStopped() const override { return false; } - bool isEmergencyStopped() const override { return emergency_stopped_; } - bool isFault() const override { return false; } + bool isProtectiveStopped() const override; + bool isEmergencyStopped() const override; + bool isFault() const override; Result moveJ(const JointPositionCommand& target, const MotionOptions& options) override; Result speedJ(const JointVelocityCommand& velocity, double acceleration, double duration) override; @@ -66,8 +67,8 @@ public: Result powerOff() override { return torqueOff(); } Result brakeRelease() override { return torqueOn(); } Result shutdown() override; - Result clearFault() override { return Result::success(); } - Result unlockProtectiveStop() override { return Result::success(); } + Result clearFault() override; + Result unlockProtectiveStop() override; Result loadProgram(const std::string& program_name) override; Result playProgram() override; Result pauseProgram() override; @@ -92,6 +93,12 @@ private: Result unsupported_(const std::string& name) const; bool validDof_(std::size_t size, std::string& error) const; Result ensureConnected_(const std::string& context) const; + Result ensureMotionReady_(const std::string& context, + std::uint64_t& safety_epoch) const; + Result completeSafetyRecovery_(const std::string& context, + std::uint64_t expected_safety_epoch); + Result unlockProtectiveStop_( + std::optional expected_safety_epoch); Result stopMotion_(MotionStopKind kind, double acceleration); struct SdkState; @@ -109,7 +116,7 @@ private: std::atomic connected_{false}; std::atomic busy_{false}; std::atomic servo_mode_{false}; - bool emergency_stopped_{false}; + std::atomic emergency_stopped_{false}; mutable std::mutex mutex_; std::unique_ptr sdk_; diff --git a/cmvr-es/devices/arm/aubo_arm/aubo_motion_state.h b/cmvr-es/devices/arm/aubo_arm/aubo_motion_state.h index 76409c90..f3012c89 100644 --- a/cmvr-es/devices/arm/aubo_arm/aubo_motion_state.h +++ b/cmvr-es/devices/arm/aubo_arm/aubo_motion_state.h @@ -66,6 +66,12 @@ struct StopRequest { } }; +struct SafetyCancelResult { + MotionKind kind{MotionKind::None}; + MotionToken active_token; + bool tracked_motion{false}; +}; + // Tracks one direct AUBO motion owner. MoveJ/MoveL submissions are serialized // through the vendor call. Speed calls release the outer mutex before their // potentially blocking SDK call, so the generation cancellation below also @@ -183,6 +189,31 @@ public: tracked_motion}; } + SafetyCancelResult cancelActiveForSafety() + { + std::lock_guard lock(mutex_); + const MotionToken active = owner_active_ + ? active_token_ + : MotionToken{}; + if (active.valid()) { + cancelled_generation_ = std::max( + cancelled_generation_, active.generation); + } + + const MotionKind kind = active.valid() + ? active.kind + : last_kind_; + if (kind != MotionKind::None) { + last_kind_ = kind; + } + // This block is intentionally independent of stop_in_progress_. The + // monitor may observe the safety event while a software Stop owns the + // stop transaction; either way no new motion may enter. + blocked_ = true; + owner_finished_cv_.notify_all(); + return {kind, active, active.valid() || kind != MotionKind::None}; + } + bool cancelled(const MotionToken& token) const { std::lock_guard lock(mutex_); diff --git a/cmvr-es/devices/arm/aubo_arm/aubo_safety_state.h b/cmvr-es/devices/arm/aubo_arm/aubo_safety_state.h new file mode 100644 index 00000000..8c13b4d8 --- /dev/null +++ b/cmvr-es/devices/arm/aubo_arm/aubo_safety_state.h @@ -0,0 +1,184 @@ +#ifndef CMVR_ES_AUBO_SAFETY_STATE_H +#define CMVR_ES_AUBO_SAFETY_STATE_H + +#include +#include +#include + +namespace cmvr::device::aubo_internal { + +// This is deliberately richer than RobotArm::SafetyMode. Recovery and +// Violation have no lossless public mapping, but both must remain fail-closed. +enum class SafetyCondition { + Unknown, + Normal, + Reduced, + Recovery, + Violation, + ProtectiveStop, + SafeguardStop, + SystemEmergencyStop, + RobotEmergencyStop, + Fault, +}; + +inline bool isMotionSafe(const SafetyCondition condition) noexcept +{ + return condition == SafetyCondition::Normal || + condition == SafetyCondition::Reduced; +} + +inline SafetyCondition effectiveSafetyCondition( + const SafetyCondition reported_condition, + const int robot_emergency_stop_source) noexcept +{ + if (robot_emergency_stop_source < 0) { + return SafetyCondition::Unknown; + } + if (robot_emergency_stop_source != 0) { + return SafetyCondition::RobotEmergencyStop; + } + return reported_condition; +} + +inline bool needsProtectiveUnlock( + const SafetyCondition condition) noexcept +{ + return condition == SafetyCondition::ProtectiveStop || + condition == SafetyCondition::Violation; +} + +inline bool needsInterfaceBoardRestart( + const SafetyCondition condition) noexcept +{ + return condition == SafetyCondition::SystemEmergencyStop || + condition == SafetyCondition::RobotEmergencyStop || + condition == SafetyCondition::Fault; +} + +struct SafetyPermit { + std::uint64_t epoch{0}; + + bool valid() const noexcept { return epoch != 0; } +}; + +struct RecoveryToken { + std::uint64_t epoch{0}; + + bool valid() const noexcept { return epoch != 0; } +}; + +struct SafetySnapshot { + SafetyCondition observed{SafetyCondition::Unknown}; + SafetyCondition latched_reason{SafetyCondition::Unknown}; + std::uint64_t epoch{0}; + bool latched{false}; + bool recovery_in_progress{false}; +}; + +// Hardware safety is an event, not a level. Once an unsafe state has been +// observed, returning to Normal only changes the observed level. A separate, +// explicit recovery must prove that the old controller operation has been +// cancelled before new motion permits can be issued. +class SafetyState final { +public: + SafetyState() = default; + + void observe(const SafetyCondition condition) + { + std::lock_guard lock(mutex_); + const bool changed = observed_ != condition; + observed_ = condition; + if (isMotionSafe(condition)) { + return; + } + + if (!latched_ || recovery_in_progress_ || changed) { + ++epoch_; + } + latched_ = true; + recovery_in_progress_ = false; + latched_reason_ = condition; + } + + std::optional tryPermit() const + { + std::lock_guard lock(mutex_); + if (latched_ || !isMotionSafe(observed_)) { + return std::nullopt; + } + return SafetyPermit{epoch_}; + } + + bool validate(const SafetyPermit permit) const + { + std::lock_guard lock(mutex_); + return permit.valid() && permit.epoch == epoch_ && !latched_ && + isMotionSafe(observed_); + } + + std::optional beginRecovery( + const std::uint64_t expected_epoch) + { + std::lock_guard lock(mutex_); + if (expected_epoch == 0 || expected_epoch != epoch_ || !latched_ || + recovery_in_progress_ || + !isMotionSafe(observed_)) { + return std::nullopt; + } + recovery_in_progress_ = true; + return RecoveryToken{epoch_}; + } + + bool completeRecovery( + const RecoveryToken token, + const bool robot_running, + const bool controller_idle, + const bool cancellation_confirmed) + { + std::lock_guard lock(mutex_); + if (!token.valid() || token.epoch != epoch_ || !latched_ || + !recovery_in_progress_ || !isMotionSafe(observed_) || + !robot_running || !controller_idle || + !cancellation_confirmed) { + return false; + } + + latched_ = false; + recovery_in_progress_ = false; + latched_reason_ = SafetyCondition::Unknown; + ++epoch_; + return true; + } + + void failRecovery(const RecoveryToken token) + { + std::lock_guard lock(mutex_); + if (token.valid() && token.epoch == epoch_) { + recovery_in_progress_ = false; + } + } + + SafetySnapshot snapshot() const + { + std::lock_guard lock(mutex_); + return { + observed_, + latched_reason_, + epoch_, + latched_, + recovery_in_progress_}; + } + +private: + mutable std::mutex mutex_; + SafetyCondition observed_{SafetyCondition::Unknown}; + SafetyCondition latched_reason_{SafetyCondition::Unknown}; + std::uint64_t epoch_{1}; + bool latched_{false}; + bool recovery_in_progress_{false}; +}; + +} // namespace cmvr::device::aubo_internal + +#endif // CMVR_ES_AUBO_SAFETY_STATE_H diff --git a/cmvr-es/devices/arm/aubo_arm/tests/aubo_motion_state_test.cpp b/cmvr-es/devices/arm/aubo_arm/tests/aubo_motion_state_test.cpp index 3bb12777..bbeb9169 100644 --- a/cmvr-es/devices/arm/aubo_arm/tests/aubo_motion_state_test.cpp +++ b/cmvr-es/devices/arm/aubo_arm/tests/aubo_motion_state_test.cpp @@ -123,5 +123,34 @@ int main() CHECK_TRUE(recovered.started()); state.finish(recovered.token, MotionFinishMode::Clear); + const auto safety_motion = state.begin(MotionKind::Linear); + CHECK_TRUE(safety_motion.started()); + const auto safety_cancel = state.cancelActiveForSafety(); + CHECK_TRUE(safety_cancel.kind == MotionKind::Linear); + CHECK_TRUE(safety_cancel.tracked_motion); + CHECK_TRUE(state.cancelled(safety_motion.token)); + CHECK_TRUE(state.begin(MotionKind::Joint).status == + MotionStartStatus::Blocked); + state.finish(safety_motion.token); + const auto safety_stop = state.beginStop(); + CHECK_TRUE(safety_stop.started()); + CHECK_TRUE(safety_stop.kind == MotionKind::Linear); + CHECK_TRUE(state.completeStop()); + + const auto retained_speed = state.begin(MotionKind::Joint); + CHECK_TRUE(retained_speed.started()); + state.finish(retained_speed.token, MotionFinishMode::Retain); + const auto retained_cancel = state.cancelActiveForSafety(); + CHECK_TRUE(retained_cancel.kind == MotionKind::Joint); + CHECK_TRUE(retained_cancel.tracked_motion); + CHECK_TRUE(!retained_cancel.active_token.valid()); + CHECK_TRUE(state.begin(MotionKind::Linear).status == + MotionStartStatus::Blocked); + const auto retained_stop = state.beginStop(); + CHECK_TRUE(retained_stop.started()); + CHECK_TRUE(retained_stop.kind == MotionKind::Joint); + CHECK_TRUE(retained_stop.tracked_motion); + CHECK_TRUE(state.completeStop()); + return 0; } diff --git a/cmvr-es/devices/arm/aubo_arm/tests/aubo_safety_state_test.cpp b/cmvr-es/devices/arm/aubo_arm/tests/aubo_safety_state_test.cpp new file mode 100644 index 00000000..b38fc6c2 --- /dev/null +++ b/cmvr-es/devices/arm/aubo_arm/tests/aubo_safety_state_test.cpp @@ -0,0 +1,88 @@ +#include "devices/arm/aubo_arm/aubo_safety_state.h" + +#include + +namespace { + +#define CHECK_TRUE(condition) \ + do { \ + if (!(condition)) { \ + std::cerr << "CHECK_TRUE failed at line " << __LINE__ << ": " \ + << #condition << std::endl; \ + return 1; \ + } \ + } while (false) + +} // namespace + +int main() +{ + using namespace cmvr::device::aubo_internal; + + SafetyState state; + CHECK_TRUE(!state.tryPermit().has_value()); + + state.observe(SafetyCondition::Normal); + const auto initial_permit = state.tryPermit(); + CHECK_TRUE(initial_permit.has_value()); + CHECK_TRUE(state.validate(*initial_permit)); + + CHECK_TRUE(effectiveSafetyCondition(SafetyCondition::Normal, 1) == + SafetyCondition::RobotEmergencyStop); + CHECK_TRUE(effectiveSafetyCondition(SafetyCondition::Normal, -1) == + SafetyCondition::Unknown); + CHECK_TRUE(effectiveSafetyCondition(SafetyCondition::Reduced, 0) == + SafetyCondition::Reduced); + CHECK_TRUE(needsProtectiveUnlock(SafetyCondition::ProtectiveStop)); + CHECK_TRUE(needsProtectiveUnlock(SafetyCondition::Violation)); + CHECK_TRUE(!needsProtectiveUnlock(SafetyCondition::SafeguardStop)); + CHECK_TRUE(needsInterfaceBoardRestart( + SafetyCondition::RobotEmergencyStop)); + CHECK_TRUE(needsInterfaceBoardRestart( + SafetyCondition::SystemEmergencyStop)); + CHECK_TRUE(needsInterfaceBoardRestart(SafetyCondition::Fault)); + CHECK_TRUE(!needsInterfaceBoardRestart(SafetyCondition::Recovery)); + + state.observe(SafetyCondition::RobotEmergencyStop); + CHECK_TRUE(!state.validate(*initial_permit)); + CHECK_TRUE(state.snapshot().latched); + CHECK_TRUE(!state.beginRecovery(state.snapshot().epoch).has_value()); + + // Releasing the hardware switch must not unlock motion by itself. + state.observe(SafetyCondition::Normal); + CHECK_TRUE(state.snapshot().latched); + CHECK_TRUE(!state.tryPermit().has_value()); + + const auto recovery = state.beginRecovery(state.snapshot().epoch); + CHECK_TRUE(recovery.has_value()); + CHECK_TRUE(!state.completeRecovery(*recovery, true, true, false)); + state.failRecovery(*recovery); + + const auto retry = state.beginRecovery(state.snapshot().epoch); + CHECK_TRUE(retry.has_value()); + CHECK_TRUE(state.completeRecovery(*retry, true, true, true)); + const auto recovered_permit = state.tryPermit(); + CHECK_TRUE(recovered_permit.has_value()); + CHECK_TRUE(state.validate(*recovered_permit)); + + // A new safety event invalidates an in-flight recovery token. + state.observe(SafetyCondition::ProtectiveStop); + state.observe(SafetyCondition::Reduced); + const auto stale_recovery = state.beginRecovery( + state.snapshot().epoch); + CHECK_TRUE(stale_recovery.has_value()); + state.observe(SafetyCondition::SafeguardStop); + state.observe(SafetyCondition::Normal); + CHECK_TRUE(!state.completeRecovery( + *stale_recovery, true, true, true)); + CHECK_TRUE(state.snapshot().latched); + + // An old API call must not begin recovery for a newer safety event. + const auto stale_epoch = state.snapshot().epoch; + state.observe(SafetyCondition::RobotEmergencyStop); + state.observe(SafetyCondition::Normal); + CHECK_TRUE(!state.beginRecovery(stale_epoch).has_value()); + CHECK_TRUE(!state.snapshot().recovery_in_progress); + + return 0; +} From 9095fbf68cf3c3d9cca533dc8811baf52313bd5b Mon Sep 17 00:00:00 2001 From: xtkuang <87661715@qq.com> Date: Fri, 7 Aug 2026 15:11:26 +0800 Subject: [PATCH 19/20] fix(huayan): harden motion and safety lifecycle --- cmvr-es/devices/arm/huayan_arm/CMakeLists.txt | 51 +- cmvr-es/devices/arm/huayan_arm/huayan_arm.cpp | 2500 ++++++++++++++--- cmvr-es/devices/arm/huayan_arm/huayan_arm.h | 76 +- .../arm/huayan_arm/huayan_lifecycle_state.h | 504 ++++ .../huayan_arm/tests/huayan_arm_sdk_test.cpp | 834 ++++++ .../tests/huayan_lifecycle_state_test.cpp | 232 ++ 6 files changed, 3840 insertions(+), 357 deletions(-) create mode 100644 cmvr-es/devices/arm/huayan_arm/huayan_lifecycle_state.h create mode 100644 cmvr-es/devices/arm/huayan_arm/tests/huayan_arm_sdk_test.cpp create mode 100644 cmvr-es/devices/arm/huayan_arm/tests/huayan_lifecycle_state_test.cpp diff --git a/cmvr-es/devices/arm/huayan_arm/CMakeLists.txt b/cmvr-es/devices/arm/huayan_arm/CMakeLists.txt index 617fab7d..6001a5d7 100644 --- a/cmvr-es/devices/arm/huayan_arm/CMakeLists.txt +++ b/cmvr-es/devices/arm/huayan_arm/CMakeLists.txt @@ -1,5 +1,7 @@ add_library(huayan_arm SHARED huayan_arm.cpp) +find_package(Threads REQUIRED) + set(HUAYAN_ARM_SDK_DIR ${CMAKE_SOURCE_DIR}/dependency/${ARCH}/third_party/huayan_arm/v1.0) target_include_directories(huayan_arm @@ -20,9 +22,56 @@ target_link_libraries(huayan_arm PRIVATE HR_Pro glog + Threads::Threads ) add_library(cmvr_es::device::huayan_arm ALIAS huayan_arm) install(TARGETS huayan_arm LIBRARY DESTINATION lib) -install(FILES ${HUAYAN_ARM_SDK_DIR}/lib/libHR_Pro.so DESTINATION lib) \ No newline at end of file +install(FILES ${HUAYAN_ARM_SDK_DIR}/lib/libHR_Pro.so DESTINATION lib) + +if(BUILD_TESTING) + add_executable(huayan_lifecycle_state_test + tests/huayan_lifecycle_state_test.cpp + ) + target_include_directories(huayan_lifecycle_state_test + PRIVATE + ${CMAKE_SOURCE_DIR}/cmvr-es + ) + target_link_libraries(huayan_lifecycle_state_test + PRIVATE + Threads::Threads + ) + add_test( + NAME huayan_lifecycle_state_test + COMMAND huayan_lifecycle_state_test + ) + set_tests_properties(huayan_lifecycle_state_test PROPERTIES TIMEOUT 10) + + if(UNIX AND NOT APPLE) + add_executable(huayan_arm_sdk_test + tests/huayan_arm_sdk_test.cpp + ) + target_include_directories(huayan_arm_sdk_test + PRIVATE + ${CMAKE_SOURCE_DIR}/cmvr-es + ${HUAYAN_ARM_SDK_DIR}/include + ) + target_link_libraries(huayan_arm_sdk_test + PRIVATE + cmvr_es::device::huayan_arm + Threads::Threads + ) + # Export the fake HRIF_* definitions so libhuayan_arm resolves its SDK + # calls to the deterministic test controller instead of real hardware. + target_link_options(huayan_arm_sdk_test PRIVATE -Wl,--export-dynamic) + add_test( + NAME huayan_arm_sdk_test + COMMAND huayan_arm_sdk_test + ) + set_tests_properties(huayan_arm_sdk_test PROPERTIES + TIMEOUT 20 + ENVIRONMENT "LD_LIBRARY_PATH=${CMVR_TEST_EXTERNAL_LIBRARY_PATH}" + ) + endif() +endif() diff --git a/cmvr-es/devices/arm/huayan_arm/huayan_arm.cpp b/cmvr-es/devices/arm/huayan_arm/huayan_arm.cpp index 74ef039f..dbc2093a 100644 --- a/cmvr-es/devices/arm/huayan_arm/huayan_arm.cpp +++ b/cmvr-es/devices/arm/huayan_arm/huayan_arm.cpp @@ -2,20 +2,37 @@ #include #include +#include #include +#include +#include #include +#include -#include "huayan_arm/v1.0/include/HR_Pro.h" #include "common/base/logging/logger.h" +#include "huayan_arm/v1.0/include/HR_Pro.h" namespace cmvr::device { namespace { +using huayan_internal::MotionFinishMode; +using huayan_internal::MotionKind; +using huayan_internal::MotionStartStatus; +using huayan_internal::SafetyCondition; + constexpr double kPi = 3.14159265358979323846; constexpr double kDefaultMoveJVelocityDeg = 30.0; constexpr double kDefaultMoveJAccelerationDeg = 60.0; constexpr double kDefaultMoveLVelocityMm = 100.0; constexpr double kDefaultMoveLAccelerationMm = 200.0; +constexpr double kJointTargetToleranceRad = 0.002; +constexpr double kTcpPositionToleranceM = 0.0005; +constexpr double kTcpRotationToleranceRad = 0.003; +constexpr double kIdleVelocityToleranceRad = 0.01; +constexpr auto kSafetyPollPeriod = std::chrono::milliseconds(50); +constexpr auto kControllerStopTimeout = std::chrono::milliseconds(3000); +constexpr auto kOwnerExitTimeout = std::chrono::milliseconds(3000); +constexpr auto kCompletionCorrelationGrace = std::chrono::milliseconds(250); double radToDeg(const double value) { @@ -37,6 +54,18 @@ double mmToMeters(const double value) return value / 1000.0; } +double angularDistance(const double lhs, const double rhs) +{ + return std::abs(std::remainder(lhs - rhs, 2.0 * kPi)); +} + +std::int64_t monotonicNowNs() +{ + return std::chrono::duration_cast( + std::chrono::steady_clock::now().time_since_epoch()) + .count(); +} + std::vector defaultJointNames(const std::size_t dof) { std::vector names; @@ -47,11 +76,13 @@ std::vector defaultJointNames(const std::size_t dof) return names; } -std::array toSix(const std::vector& values, const double fill = 0.0) +std::array toSix( + const std::vector& values, + const double fill = 0.0) { std::array out{fill, fill, fill, fill, fill, fill}; - const auto n = std::min(out.size(), values.size()); - for (std::size_t i = 0; i < n; ++i) { + const auto count = std::min(out.size(), values.size()); + for (std::size_t i = 0; i < count; ++i) { out[i] = values[i]; } return out; @@ -59,12 +90,13 @@ std::array toSix(const std::vector& values, const double fill std::vector poseToHrCoord(const CartesianPose& pose) { - return {metersToMm(pose.x), - metersToMm(pose.y), - metersToMm(pose.z), - radToDeg(pose.rx), - radToDeg(pose.ry), - radToDeg(pose.rz)}; + return { + metersToMm(pose.x), + metersToMm(pose.y), + metersToMm(pose.z), + radToDeg(pose.rx), + radToDeg(pose.ry), + radToDeg(pose.rz)}; } std::vector zeroHrFrame() @@ -72,8 +104,58 @@ std::vector zeroHrFrame() return {0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; } +SafetyMode safetyModeFromCondition(const SafetyCondition condition) +{ + switch (condition) { + case SafetyCondition::Normal: + return SafetyMode::Normal; + case SafetyCondition::EmergencyStop: + case SafetyCondition::SoftwareEmergencyStop: + return SafetyMode::EmergencyStop; + case SafetyCondition::SafeguardStop: + return SafetyMode::SafeguardStop; + case SafetyCondition::SoftwareProtectiveStop: + return SafetyMode::ProtectiveStop; + case SafetyCondition::RobotFault: + case SafetyCondition::EmergencySignalFault: + case SafetyCondition::SafeguardSignalFault: + return SafetyMode::Fault; + case SafetyCondition::Unknown: + return SafetyMode::Unknown; + } + return SafetyMode::Unknown; +} + +bool isEmergencyCondition(const SafetyCondition condition) +{ + return condition == SafetyCondition::EmergencyStop || + condition == SafetyCondition::SoftwareEmergencyStop || + condition == SafetyCondition::EmergencySignalFault; +} + +bool isProtectiveCondition(const SafetyCondition condition) +{ + return condition == SafetyCondition::SafeguardStop || + condition == SafetyCondition::SoftwareProtectiveStop || + condition == SafetyCondition::SafeguardSignalFault; +} + } // namespace +struct HuayanRobot::RuntimeState { + huayan_internal::MotionState motion; + huayan_internal::SafetyState safety; + std::atomic monitor_running{true}; + std::atomic program_active{false}; + std::atomic termination_confirmed{false}; + std::atomic last_valid_sample_ns{0}; + std::atomic speed_completion_not_before_ns{0}; + mutable std::recursive_mutex termination_mutex; + mutable std::recursive_mutex submission_mutex; + mutable std::mutex wait_mutex; + std::condition_variable wait_cv; +}; + HuayanRobot::HuayanRobot(const config::RobotArmConfig& cfg) : cfg_(cfg) { @@ -84,14 +166,24 @@ HuayanRobot::HuayanRobot(const config::RobotArmConfig& cfg) ip_ = vendor_cfg_.ip(); port_ = vendor_cfg_.port() > 0 ? vendor_cfg_.port() : 10003; - tcp_name_ = vendor_cfg_.tool_frame().empty() ? "TCP" : vendor_cfg_.tool_frame(); - ucs_name_ = vendor_cfg_.base_frame().empty() ? "Base" : vendor_cfg_.base_frame(); + tcp_name_ = vendor_cfg_.tool_frame().empty() + ? "TCP" + : vendor_cfg_.tool_frame(); + ucs_name_ = vendor_cfg_.base_frame().empty() + ? "Base" + : vendor_cfg_.base_frame(); - const auto dof = vendor_cfg_.dof() > 0 ? static_cast(vendor_cfg_.dof()) : 6U; - model_.name = vendor_cfg_.model().empty() ? "HuayanRobot" : vendor_cfg_.model(); + const auto dof = vendor_cfg_.dof() > 0 + ? static_cast(vendor_cfg_.dof()) + : 6U; + model_.name = vendor_cfg_.model().empty() + ? "HuayanRobot" + : vendor_cfg_.model(); model_.manufacturer = "Huayan"; model_.dof = dof; - model_.joint_names.assign(vendor_cfg_.joint_names().begin(), vendor_cfg_.joint_names().end()); + model_.joint_names.assign( + vendor_cfg_.joint_names().begin(), + vendor_cfg_.joint_names().end()); if (model_.joint_names.empty()) { model_.joint_names = defaultJointNames(dof); } @@ -103,7 +195,23 @@ HuayanRobot::HuayanRobot(const config::RobotArmConfig& cfg) HuayanRobot::~HuayanRobot() { - (void)disconnect(); + const auto result = disconnect(); + if (result.ok()) { + return; + } + CMVR_LOG(ERROR) << "[HuayanRobot] destructor forced a best-effort disconnect " + << "after safe disconnect failed: " << result.message; + const auto runtime = runtimeSnapshot_(); + if (runtime) { + runtime->monitor_running.store(false); + runtime->wait_cv.notify_all(); + } + if (safety_monitor_thread_.joinable()) { + safety_monitor_thread_.join(); + } + std::lock_guard sdk_lock(sdk_mutex_); + (void)HRIF_DisConnect(box_id_); + connected_.store(false); } bool HuayanRobot::init() @@ -117,7 +225,12 @@ bool HuayanRobot::init() CMVR_LOG(ERROR) << "[HuayanRobot] init failed: " << result.message; return false; } - setSpeedScaling(1); + const auto scaling_result = setSpeedScaling(1.0); + if (!scaling_result.ok()) { + CMVR_LOG(ERROR) << "[HuayanRobot] set speed scaling failed: " + << scaling_result.message; + return false; + } return true; } @@ -129,20 +242,48 @@ bool HuayanRobot::stop() ArmState HuayanRobot::getRobotState() const { const auto hr_state = readHrState_(); + const auto runtime = runtimeSnapshot_(); + const auto safety = runtime + ? runtime->safety.snapshot() + : huayan_internal::SafetySnapshot{}; + const auto motion = runtime + ? runtime->motion.snapshot() + : huayan_internal::MotionSnapshot{}; ArmState state; state.connected = isConnected(); - state.powered_on = hr_state.valid ? hr_state.electrified != 0 : state.connected; - state.brake_released = hr_state.valid ? hr_state.brake != 0 : state.connected; - state.moving = hr_state.valid ? hr_state.moving != 0 : busy_.load(); - state.program_running = state.moving; - state.protective_stopped = hr_state.valid ? hr_state.safeguard != 0 : false; - state.emergency_stopped = hr_state.valid ? hr_state.emergency_stop != 0 : false; - state.fault = hr_state.valid ? hr_state.error != 0 : false; - state.robot_mode = getRobotMode(); - state.safety_mode = getSafetyMode(); + state.powered_on = hr_state.valid && hr_state.electrified != 0; + state.brake_released = hr_state.valid && hr_state.brake != 0; + state.moving = hr_state.valid && hr_state.moving != 0; + state.program_running = runtime && runtime->program_active.load(); + state.protective_stopped = safety.latched && + isProtectiveCondition(safety.latched_reason); + state.emergency_stopped = safety.latched && + isEmergencyCondition(safety.latched_reason); + state.fault = !hr_state.valid || hr_state.error != 0 || + (safety.latched && safetyModeFromCondition(safety.latched_reason) == SafetyMode::Fault); + if (!state.connected) { + state.robot_mode = RobotMode::Disconnected; + } else if (!hr_state.valid) { + state.robot_mode = RobotMode::Unknown; + } else if (state.fault) { + state.robot_mode = RobotMode::Fault; + } else if (safety.latched) { + state.robot_mode = RobotMode::Stopped; + } else if (hr_state.paused != 0) { + state.robot_mode = RobotMode::Paused; + } else if (hr_state.electrified == 0) { + state.robot_mode = RobotMode::PowerOff; + } else if (hr_state.moving != 0 || state.program_running || motion.owner_active) { + state.robot_mode = RobotMode::Running; + } else { + state.robot_mode = RobotMode::Idle; + } + state.safety_mode = safety.latched + ? safetyModeFromCondition(safety.latched_reason) + : safetyModeFromCondition(safety.observed); state.control_mode = getControlMode(); - state.speed_scaling = speed_scaling_; + state.speed_scaling = speed_scaling_.load(); state.actual_joint_state = getJointState(); state.target_joint_state = state.actual_joint_state; state.actual_tcp_pose = readTcpPose_(); @@ -153,15 +294,20 @@ ArmState HuayanRobot::getRobotState() const JointGroupState HuayanRobot::getJointState() const { JointGroupState state; - state.position = readJointPositionRad_(); - state.velocity = readJointVelocityRad_(); + state.position_valid = readJointPositionSample_(state.position); + state.velocity_valid = readJointVelocitySample_(state.velocity); state.effort.assign(model_.dof, 0.0); + state.effort_valid = false; + state.sample_monotonic_ns = monotonicNowNs(); return state; } CartesianPose HuayanRobot::getTcpPose(const FrameType frame) const { - (void)frame; + if (frame != FrameType::Base) { + CMVR_LOG(ERROR) << "[HuayanRobot] getTcpPose supports Base frame only"; + return {}; + } return readTcpPose_(); } @@ -170,42 +316,69 @@ RobotMode HuayanRobot::getRobotMode() const if (!isConnected()) { return RobotMode::Disconnected; } - const auto state = readHrState_(); if (!state.valid) { - return busy_.load() ? RobotMode::Running : RobotMode::Idle; + return RobotMode::Unknown; + } + const auto runtime = runtimeSnapshot_(); + if (runtime) { + const auto safety = runtime->safety.snapshot(); + if (safety.latched) { + const auto mode = safetyModeFromCondition(safety.latched_reason); + return mode == SafetyMode::Fault ? RobotMode::Fault : RobotMode::Stopped; + } } if (state.error != 0) { return RobotMode::Fault; } - if (state.emergency_stop != 0) { - return RobotMode::Stopped; - } if (state.paused != 0) { return RobotMode::Paused; } if (state.electrified == 0) { return RobotMode::PowerOff; } - return state.moving != 0 ? RobotMode::Running : RobotMode::Idle; + if (state.moving != 0 || (runtime && runtime->program_active.load())) { + return RobotMode::Running; + } + return RobotMode::Idle; } SafetyMode HuayanRobot::getSafetyMode() const { - const auto state = readHrState_(); - if (!state.valid) { + const auto runtime = runtimeSnapshot_(); + if (!runtime) { return SafetyMode::Unknown; } - if (state.error != 0) { - return SafetyMode::Fault; + (void)readHrState_(); + const auto safety = runtime->safety.snapshot(); + return safetyModeFromCondition( + safety.latched ? safety.latched_reason : safety.observed); +} + +ControlMode HuayanRobot::getControlMode() const +{ + const auto runtime = runtimeSnapshot_(); + if (!runtime) { + return ControlMode::None; } - if (state.emergency_stop != 0) { - return SafetyMode::EmergencyStop; + const auto motion = runtime->motion.snapshot(); + const auto kind = motion.owner_active + ? motion.active_kind + : motion.retained_kind; + switch (kind) { + case MotionKind::Joint: + case MotionKind::Linear: + return ControlMode::Position; + case MotionKind::SpeedJoint: + case MotionKind::SpeedLinear: + return ControlMode::Velocity; + case MotionKind::Servo: + return ControlMode::Servo; + case MotionKind::None: + case MotionKind::Program: + return ControlMode::None; } - if (state.safeguard != 0) { - return SafetyMode::SafeguardStop; - } - return SafetyMode::Normal; + return ControlMode::None; } Result HuayanRobot::torqueOn() @@ -214,20 +387,127 @@ Result HuayanRobot::torqueOn() if (!ready.ok()) { return ready; } - std::lock_guard lock(mutex_); - return hrResult_(HRIF_GrpEnable(box_id_, robot_id_), "GrpEnable"); + const auto runtime = runtimeSnapshot_(); + if (!runtime) { + return Result::failure( + ArmErrorCode::RobotNotReady, + "[HuayanRobot] torqueOn failed: runtime is unavailable"); + } + + const auto state = sampleHrState_(runtime); + publishHrState_(runtime, state); + const auto safety = runtime->safety.snapshot(); + if (safety.latched) { + if (!isEmergencyCondition(safety.latched_reason)) { + return Result::failure( + ArmErrorCode::CommandRejected, + "[HuayanRobot] torqueOn cannot clear this safety latch; use the typed recovery API"); + } + return completeSafetyRecovery_( + "torqueOn", + runtime, + safety.epoch, + true, + software_protective_stopped_.load()); + } + if (!state.valid) { + return Result::failure( + ArmErrorCode::RobotNotReady, + "[HuayanRobot] torqueOn failed: safety state is unavailable"); + } + + int ret = 0; + { + std::lock_guard sdk_lock(sdk_mutex_); + ret = HRIF_GrpEnable(box_id_, robot_id_); + } + const auto result = hrResult_(ret, "GrpEnable"); + if (!result.ok()) { + return result; + } + const auto after = sampleHrState_(runtime); + publishHrState_(runtime, after); + if (!after.valid || after.enabled == 0 || after.electrified == 0 || + runtime->safety.snapshot().latched) { + return Result::failure( + ArmErrorCode::RobotNotReady, + "[HuayanRobot] torqueOn failed: enabled state was not confirmed"); + } + return Result::success(); } Result HuayanRobot::torqueOff() { - const auto ready = ensureConnected_("torqueOff"); if (!ready.ok()) { return ready; } + const auto runtime = runtimeSnapshot_(); + if (!runtime) { + return Result::failure( + ArmErrorCode::RobotNotReady, + "[HuayanRobot] torqueOff failed: runtime is unavailable"); + } + std::lock_guard termination_lock( + runtime->termination_mutex); + const auto stop_result = stopMotion(); + if (!stop_result.ok()) { + return stop_result; + } - std::lock_guard lock(mutex_); - return hrResult_(HRIF_GrpDisable(box_id_, robot_id_), "GrpDisable"); + huayan_internal::StopRequest poweroff_barrier; + int ret = 0; + { + // Keep a Stop generation active through GrpDisable. A command that + // raced the first Stop is cancelled here before it can submit. + std::lock_guard submission_lock( + runtime->submission_mutex); + poweroff_barrier = runtime->motion.beginStop(); + if (!poweroff_barrier.started()) { + return Result::failure( + ArmErrorCode::CommandRejected, + "[HuayanRobot] torqueOff failed: could not establish the power-off barrier"); + } + runtime->wait_cv.notify_all(); + if (!terminateController_( + runtime, + kControllerStopTimeout, + runtime->program_active.load() || + poweroff_barrier.kind == MotionKind::Program)) { + runtime->motion.failStop(); + return Result::failure( + ArmErrorCode::CommandFailed, + "[HuayanRobot] torqueOff failed: final controller Stop was not confirmed"); + } + std::lock_guard sdk_lock(sdk_mutex_); + ret = HRIF_GrpDisable(box_id_, robot_id_); + } + if (ret != 0) { + runtime->motion.failStop(); + return hrResult_(ret, "GrpDisable"); + } + const bool owner_exited = runtime->motion.waitForOwnerExit( + poweroff_barrier.active_token, kOwnerExitTimeout); + bool disabled = false; + const auto deadline = std::chrono::steady_clock::now() + + kControllerStopTimeout; + while (owner_exited && std::chrono::steady_clock::now() < deadline) { + const auto state = sampleHrState_(runtime); + publishHrState_(runtime, state); + if (state.valid && state.enabled == 0 && state.electrified == 0) { + disabled = true; + break; + } + std::unique_lock wait_lock(runtime->wait_mutex); + runtime->wait_cv.wait_for(wait_lock, kSafetyPollPeriod); + } + if (!owner_exited || !disabled || !runtime->motion.completeStop()) { + runtime->motion.failStop(); + return Result::failure( + ArmErrorCode::CommandFailed, + "[HuayanRobot] torqueOff failed: disabled state was not confirmed"); + } + return Result::success(); } Result HuayanRobot::calibrateZeroQ(const std::string& joint_name) @@ -238,105 +518,287 @@ Result HuayanRobot::calibrateZeroQ(const std::string& joint_name) Result HuayanRobot::emergencyStop() { + const auto ready = ensureConnected_("emergencyStop"); + if (!ready.ok()) { + return ready; + } + const auto runtime = runtimeSnapshot_(); + if (!runtime) { + return Result::failure( + ArmErrorCode::RobotNotReady, + "[HuayanRobot] emergencyStop failed: runtime is unavailable"); + } + software_emergency_stopped_.store(true); + { + std::lock_guard submission_lock( + runtime->submission_mutex); + runtime->safety.observe(SafetyCondition::SoftwareEmergencyStop); + runtime->termination_confirmed.store(false); + (void)runtime->motion.cancelActiveForSafety(); + } + runtime->wait_cv.notify_all(); + return stopMotion(); +} + +Result HuayanRobot::protectiveStop() +{ + const auto ready = ensureConnected_("protectiveStop"); + if (!ready.ok()) { + return ready; + } + const auto runtime = runtimeSnapshot_(); + if (!runtime) { + return Result::failure( + ArmErrorCode::RobotNotReady, + "[HuayanRobot] protectiveStop failed: runtime is unavailable"); + } + int ret = 0; + { + std::lock_guard submission_lock( + runtime->submission_mutex); + // Publish/cancel the local safety event before the vendor call while + // excluding every motion submission. No previously permitted command + // can slip in after EnterSafetyGuard and escape local cancellation. + software_protective_stopped_.store(true); + runtime->safety.observe(SafetyCondition::SoftwareProtectiveStop); + runtime->termination_confirmed.store(false); + (void)runtime->motion.cancelActiveForSafety(); + std::lock_guard sdk_lock(sdk_mutex_); + ret = HRIF_EnterSafetyGuard(box_id_, robot_id_, 1); + } + const auto result = hrResult_(ret, "EnterSafetyGuard"); + if (!result.ok()) { + // The event remains latched because the physical outcome is uncertain. + runtime->wait_cv.notify_all(); + return result; + } + runtime->wait_cv.notify_all(); return stopMotion(); } Result HuayanRobot::setSpeedScaling(const double scaling) { if (scaling < 0.0 || scaling > 1.0) { - return Result::failure(ArmErrorCode::InvalidArgument, "speed scaling must be in [0, 1]"); + return Result::failure( + ArmErrorCode::InvalidArgument, + "speed scaling must be in [0, 1]"); } - speed_scaling_ = scaling; - if (isConnected()) { - return hrResult_(HRIF_SetOverride(box_id_, robot_id_, scaling), "SetOverride"); + if (!isConnected()) { + speed_scaling_.store(scaling); + return Result::success(); } - return Result::success(); + int ret = 0; + { + std::lock_guard sdk_lock(sdk_mutex_); + ret = HRIF_SetOverride(box_id_, robot_id_, scaling); + } + const auto result = hrResult_(ret, "SetOverride"); + if (result.ok()) { + speed_scaling_.store(scaling); + } + return result; } bool HuayanRobot::isProtectiveStopped() const { - const auto state = readHrState_(); - return state.valid && state.safeguard != 0; + const auto runtime = runtimeSnapshot_(); + if (!runtime) { + return false; + } + (void)readHrState_(); + const auto safety = runtime->safety.snapshot(); + return safety.latched && isProtectiveCondition(safety.latched_reason); } bool HuayanRobot::isEmergencyStopped() const { - const auto state = readHrState_(); - return state.valid && state.emergency_stop != 0; + const auto runtime = runtimeSnapshot_(); + if (!runtime) { + return false; + } + (void)readHrState_(); + const auto safety = runtime->safety.snapshot(); + return safety.latched && isEmergencyCondition(safety.latched_reason); } bool HuayanRobot::isFault() const { + const auto runtime = runtimeSnapshot_(); + if (!runtime) { + return false; + } const auto state = readHrState_(); - return state.valid && state.error != 0; + const auto safety = runtime->safety.snapshot(); + return !state.valid || state.error != 0 || + (safety.latched && + safetyModeFromCondition(safety.latched_reason) == SafetyMode::Fault); } -Result HuayanRobot::moveJ(const JointPositionCommand& target, const MotionOptions& options) +Result HuayanRobot::moveJ( + const JointPositionCommand& target, + const MotionOptions& options) { std::string error; if (!validDof_(target.position.size(), error)) { return Result::failure(ArmErrorCode::InvalidDof, error); } - const auto ready = ensureConnected_("moveJ"); + const auto runtime = runtimeSnapshot_(); + if (!runtime) { + return Result::failure( + ArmErrorCode::RobotNotReady, + "[HuayanRobot] moveJ failed: runtime is unavailable"); + } + std::unique_lock admission_lock( + runtime->submission_mutex); + huayan_internal::SafetyPermit permit; + const auto ready = ensureMotionReady_("moveJ", runtime, permit); if (!ready.ok()) { return ready; } - if (busy_.exchange(true)) { - return Result::failure(ArmErrorCode::RobotNotReady, "[HuayanRobot] arm is busy: " + id_); + const auto start = runtime->motion.begin(MotionKind::Joint); + if (!start.started()) { + return motionStartFailure_("moveJ", start.status); + } + + if (targetReached_(&target.position, nullptr) && + controllerIdleStable_(runtime, std::chrono::milliseconds(300))) { + if (!runtime->safety.validate(permit)) { + runtime->motion.finish(start.token, MotionFinishMode::Clear); + return Result::failure( + ArmErrorCode::CommandRejected, + "[HuayanRobot] moveJ cancelled by a safety transition"); + } + runtime->motion.finish(start.token, MotionFinishMode::Clear); + return Result::success(); } std::array q_deg{}; - for (std::size_t i = 0; i < std::min(target.position.size(), q_deg.size()); ++i) { + for (std::size_t i = 0; + i < std::min(target.position.size(), q_deg.size()); + ++i) { q_deg[i] = radToDeg(target.position[i]); } - - const double velocity = options.velocity > 0.0 ? radToDeg(options.velocity) : kDefaultMoveJVelocityDeg; - const double acceleration = options.acceleration > 0.0 ? radToDeg(options.acceleration) : kDefaultMoveJAccelerationDeg; + const double velocity = options.velocity > 0.0 + ? radToDeg(options.velocity) + : kDefaultMoveJVelocityDeg; + const double acceleration = options.acceleration > 0.0 + ? radToDeg(options.acceleration) + : kDefaultMoveJAccelerationDeg; const double blend = metersToMm(options.blend_radius); - const std::string command_id = nextCommandId_(); + const auto command_id = nextCommandId_(); - const int ret = HRIF_MoveJ(box_id_, robot_id_, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - q_deg[0], q_deg[1], q_deg[2], q_deg[3], q_deg[4], q_deg[5], - tcp_name_, ucs_name_, velocity * speed_scaling_, acceleration, blend, - 1, 0, 0, 0, command_id); + if (!runtime->safety.validate(permit)) { + runtime->motion.finish(start.token, MotionFinishMode::Clear); + return Result::failure( + ArmErrorCode::CommandRejected, + "[HuayanRobot] moveJ cancelled before submission by a safety transition"); + } + int ret = 0; + { + std::lock_guard submission_lock( + runtime->submission_mutex); + std::lock_guard sdk_lock(sdk_mutex_); + if (runtime->motion.cancelled(start.token) || + !runtime->safety.validate(permit)) { + runtime->motion.finish(start.token, MotionFinishMode::Clear); + return Result::failure( + ArmErrorCode::CommandRejected, + "[HuayanRobot] moveJ cancelled before submission"); + } + ret = HRIF_MoveJ( + box_id_, robot_id_, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + q_deg[0], q_deg[1], q_deg[2], q_deg[3], q_deg[4], q_deg[5], + tcp_name_, ucs_name_, velocity * speed_scaling_.load(), acceleration, + blend, 1, 0, 0, 0, command_id); + } if (ret != 0) { - busy_.store(false); + runtime->motion.finish(start.token, MotionFinishMode::Clear); return hrResult_(ret, "moveJ"); } - const auto wait_result = waitMotionDone_("moveJ", 60000); - busy_.store(false); - return wait_result; + admission_lock.unlock(); + return waitMotionDone_( + "moveJ", runtime, start.token, permit, command_id, + &target.position, nullptr, 60000); } -Result HuayanRobot::speedJ(const JointVelocityCommand& velocity, const double acceleration, const double duration) +Result HuayanRobot::speedJ( + const JointVelocityCommand& velocity, + const double acceleration, + const double duration) { std::string error; if (!validDof_(velocity.velocity.size(), error)) { return Result::failure(ArmErrorCode::InvalidDof, error); } - const auto ready = ensureConnected_("speedJ"); + const auto runtime = runtimeSnapshot_(); + if (!runtime) { + return Result::failure( + ArmErrorCode::RobotNotReady, + "[HuayanRobot] speedJ failed: runtime is unavailable"); + } + std::unique_lock admission_lock( + runtime->submission_mutex); + huayan_internal::SafetyPermit permit; + const auto ready = ensureMotionReady_("speedJ", runtime, permit); if (!ready.ok()) { return ready; } + const auto start = runtime->motion.begin(MotionKind::SpeedJoint); + if (!start.started()) { + return motionStartFailure_("speedJ", start.status); + } + + const bool zero_command = std::all_of( + velocity.velocity.begin(), velocity.velocity.end(), + [](const double value) { return std::abs(value) < 1e-12; }); + if (zero_command) { + runtime->motion.finish(start.token, MotionFinishMode::Clear); + return Result::success(); + } std::array qd_deg{}; - for (std::size_t i = 0; i < std::min(velocity.velocity.size(), qd_deg.size()); ++i) { + for (std::size_t i = 0; + i < std::min(velocity.velocity.size(), qd_deg.size()); + ++i) { qd_deg[i] = radToDeg(velocity.velocity[i]); } - const double acc_deg = acceleration > 0.0 ? radToDeg(acceleration) : kDefaultMoveJAccelerationDeg; - const double runtime = duration > 0.0 ? duration : 0.1; - - const int ret = HRIF_SpeedJ(box_id_, robot_id_, - qd_deg[0], qd_deg[1], qd_deg[2], qd_deg[3], qd_deg[4], qd_deg[5], - acc_deg, runtime); + const double acceleration_deg = acceleration > 0.0 + ? radToDeg(acceleration) + : kDefaultMoveJAccelerationDeg; + const double run_time = duration > 0.0 ? duration : 0.1; + int ret = 0; + { + std::lock_guard submission_lock( + runtime->submission_mutex); + std::lock_guard sdk_lock(sdk_mutex_); + if (runtime->motion.cancelled(start.token) || + !runtime->safety.validate(permit)) { + runtime->motion.finish(start.token, MotionFinishMode::Clear); + return Result::failure( + ArmErrorCode::CommandRejected, + "[HuayanRobot] speedJ cancelled before submission by a safety transition"); + } + ret = HRIF_SpeedJ( + box_id_, robot_id_, + qd_deg[0], qd_deg[1], qd_deg[2], + qd_deg[3], qd_deg[4], qd_deg[5], + acceleration_deg, run_time); + if (ret == 0) { + runtime->speed_completion_not_before_ns.store( + monotonicNowNs() + + static_cast(run_time * 1e9)); + } + } if (ret != 0) { - busy_.store(false); + runtime->speed_completion_not_before_ns.store(0); + runtime->motion.finish(start.token, MotionFinishMode::Clear); return hrResult_(ret, "SpeedJ"); } - const auto wait_result = waitMotionDone_("SpeedJ", 60000); - busy_.store(false); - return wait_result; + admission_lock.unlock(); + return waitMotionDone_( + "SpeedJ", runtime, start.token, permit, {}, nullptr, nullptr, + std::max(3000, static_cast(run_time * 1000.0) + 3000)); } Result HuayanRobot::stopJ(const double acceleration) @@ -345,76 +807,191 @@ Result HuayanRobot::stopJ(const double acceleration) return stopMotion(); } -Result HuayanRobot::moveL(const CartesianPose& target, const MotionOptions& options, const FrameType frame) +Result HuayanRobot::moveL( + const CartesianPose& target, + const MotionOptions& options, + const FrameType frame) { - (void)frame; - const auto ready = ensureConnected_("moveL"); + if (frame != FrameType::Base) { + return Result::failure( + ArmErrorCode::UnsupportedCommand, + "[HuayanRobot] moveL supports Base frame only"); + } + const auto runtime = runtimeSnapshot_(); + if (!runtime) { + return Result::failure( + ArmErrorCode::RobotNotReady, + "[HuayanRobot] moveL failed: runtime is unavailable"); + } + std::unique_lock admission_lock( + runtime->submission_mutex); + huayan_internal::SafetyPermit permit; + const auto ready = ensureMotionReady_("moveL", runtime, permit); if (!ready.ok()) { return ready; } - if (busy_.exchange(true)) { - return Result::failure(ArmErrorCode::RobotNotReady, "[HuayanRobot] arm is busy: " + id_); + const auto start = runtime->motion.begin(MotionKind::Linear); + if (!start.started()) { + return motionStartFailure_("moveL", start.status); } - const auto pose = poseToHrCoord(target); - const auto q_deg = toSix(currentJointPositionDeg_()); - const double velocity = options.velocity > 0.0 ? metersToMm(options.velocity) : kDefaultMoveLVelocityMm; - const double acceleration = options.acceleration > 0.0 ? metersToMm(options.acceleration) : kDefaultMoveLAccelerationMm; - const double blend = metersToMm(options.blend_radius); - const std::string command_id = nextCommandId_(); + if (targetReached_(nullptr, &target) && + controllerIdleStable_(runtime, std::chrono::milliseconds(300))) { + if (!runtime->safety.validate(permit)) { + runtime->motion.finish(start.token, MotionFinishMode::Clear); + return Result::failure( + ArmErrorCode::CommandRejected, + "[HuayanRobot] moveL cancelled by a safety transition"); + } + runtime->motion.finish(start.token, MotionFinishMode::Clear); + return Result::success(); + } - const int ret = HRIF_MoveL(box_id_, robot_id_, - pose[0], pose[1], pose[2], pose[3], pose[4], pose[5], - q_deg[0], q_deg[1], q_deg[2], q_deg[3], q_deg[4], q_deg[5], - tcp_name_, ucs_name_, velocity * speed_scaling_, acceleration, blend, - 0, 0, 0, command_id); + std::vector q_rad; + if (!readJointPositionSample_(q_rad)) { + runtime->motion.finish(start.token, MotionFinishMode::Clear); + return Result::failure( + ArmErrorCode::CommandFailed, + "[HuayanRobot] moveL failed: unable to read reference joints"); + } + for (auto& value : q_rad) { + value = radToDeg(value); + } + const auto q_deg = toSix(q_rad); + const auto pose = poseToHrCoord(target); + const double velocity = options.velocity > 0.0 + ? metersToMm(options.velocity) + : kDefaultMoveLVelocityMm; + const double acceleration = options.acceleration > 0.0 + ? metersToMm(options.acceleration) + : kDefaultMoveLAccelerationMm; + const double blend = metersToMm(options.blend_radius); + const auto command_id = nextCommandId_(); + + if (!runtime->safety.validate(permit)) { + runtime->motion.finish(start.token, MotionFinishMode::Clear); + return Result::failure( + ArmErrorCode::CommandRejected, + "[HuayanRobot] moveL cancelled before submission by a safety transition"); + } + int ret = 0; + { + std::lock_guard submission_lock( + runtime->submission_mutex); + std::lock_guard sdk_lock(sdk_mutex_); + if (runtime->motion.cancelled(start.token) || + !runtime->safety.validate(permit)) { + runtime->motion.finish(start.token, MotionFinishMode::Clear); + return Result::failure( + ArmErrorCode::CommandRejected, + "[HuayanRobot] moveL cancelled before submission"); + } + ret = HRIF_MoveL( + box_id_, robot_id_, + pose[0], pose[1], pose[2], pose[3], pose[4], pose[5], + q_deg[0], q_deg[1], q_deg[2], q_deg[3], q_deg[4], q_deg[5], + tcp_name_, ucs_name_, velocity * speed_scaling_.load(), acceleration, + blend, 0, 0, 0, command_id); + } if (ret != 0) { - busy_.store(false); + runtime->motion.finish(start.token, MotionFinishMode::Clear); return hrResult_(ret, "moveL"); } - const auto wait_result = waitMotionDone_("moveL", 60000); - busy_.store(false); - return wait_result; + admission_lock.unlock(); + return waitMotionDone_( + "moveL", runtime, start.token, permit, command_id, + nullptr, &target, 60000); } -Result HuayanRobot::speedL(const CartesianVelocity& velocity, - const double acceleration, - const double duration, - const FrameType frame) +Result HuayanRobot::speedL( + const CartesianVelocity& velocity, + const double acceleration, + const double duration, + const FrameType frame) { - const auto ready = ensureConnected_("speedL"); + if (frame != FrameType::Base) { + return Result::failure( + ArmErrorCode::UnsupportedCommand, + "[HuayanRobot] speedL supports Base frame only"); + } + const auto runtime = runtimeSnapshot_(); + if (!runtime) { + return Result::failure( + ArmErrorCode::RobotNotReady, + "[HuayanRobot] speedL failed: runtime is unavailable"); + } + std::unique_lock admission_lock( + runtime->submission_mutex); + huayan_internal::SafetyPermit permit; + const auto ready = ensureMotionReady_("speedL", runtime, permit); if (!ready.ok()) { return ready; } - const double vx_mm = metersToMm(velocity.vx); - const double vy_mm = metersToMm(velocity.vy); - const double vz_mm = metersToMm(velocity.vz); - const double wx_deg = radToDeg(velocity.wx); - const double wy_deg = radToDeg(velocity.wy); - const double wz_deg = radToDeg(velocity.wz); + const auto start = runtime->motion.begin(MotionKind::SpeedLinear); + if (!start.started()) { + return motionStartFailure_("speedL", start.status); + } - const double linear_acc_mm = - acceleration > 0.0 ? metersToMm(acceleration) : kDefaultMoveLAccelerationMm; + const bool zero_command = + std::abs(velocity.vx) < 1e-12 && + std::abs(velocity.vy) < 1e-12 && + std::abs(velocity.vz) < 1e-12 && + std::abs(velocity.wx) < 1e-12 && + std::abs(velocity.wy) < 1e-12 && + std::abs(velocity.wz) < 1e-12; + if (zero_command) { + runtime->motion.finish(start.token, MotionFinishMode::Clear); + return Result::success(); + } - const double angular_acc_deg = - acceleration > 0.0 ? radToDeg(acceleration) : kDefaultMoveJAccelerationDeg; - - const double runtime = duration > 0.0 ? duration : 0.5; - - std::lock_guard lock(mutex_); - servo_mode_.store(false); - const int ret = HRIF_SpeedL(box_id_, robot_id_, vx_mm, vy_mm, vz_mm, - wx_deg, wy_deg, wz_deg, linear_acc_mm, angular_acc_deg, runtime); + const double linear_acceleration = acceleration > 0.0 + ? metersToMm(acceleration) + : kDefaultMoveLAccelerationMm; + const double angular_acceleration = acceleration > 0.0 + ? radToDeg(acceleration) + : kDefaultMoveJAccelerationDeg; + const double run_time = duration > 0.0 ? duration : 0.5; + int ret = 0; + { + std::lock_guard submission_lock( + runtime->submission_mutex); + std::lock_guard sdk_lock(sdk_mutex_); + if (runtime->motion.cancelled(start.token) || + !runtime->safety.validate(permit)) { + runtime->motion.finish(start.token, MotionFinishMode::Clear); + return Result::failure( + ArmErrorCode::CommandRejected, + "[HuayanRobot] speedL cancelled before submission by a safety transition"); + } + ret = HRIF_SpeedL( + box_id_, robot_id_, + metersToMm(velocity.vx), + metersToMm(velocity.vy), + metersToMm(velocity.vz), + radToDeg(velocity.wx), + radToDeg(velocity.wy), + radToDeg(velocity.wz), + linear_acceleration, + angular_acceleration, + run_time); + if (ret == 0) { + runtime->speed_completion_not_before_ns.store( + monotonicNowNs() + + static_cast(run_time * 1e9)); + } + } if (ret != 0) { - busy_.store(false); + runtime->speed_completion_not_before_ns.store(0); + runtime->motion.finish(start.token, MotionFinishMode::Clear); return hrResult_(ret, "SpeedL"); } - const auto wait_result = waitMotionDone_("SpeedL", 60000); - busy_.store(false); - return wait_result; + admission_lock.unlock(); + return waitMotionDone_( + "SpeedL", runtime, start.token, permit, {}, nullptr, nullptr, + std::max(3000, static_cast(run_time * 1000.0) + 3000)); } -Result HuayanRobot::stopL(std::optional acceleration = std::nullopt) +Result HuayanRobot::stopL(const std::optional acceleration) { (void)acceleration; return stopMotion(); @@ -422,31 +999,113 @@ Result HuayanRobot::stopL(std::optional acceleration = std::nullopt) Result HuayanRobot::stopMotion() { - if (!isConnected()) { - busy_.store(false); + if (!connected_.load()) { servo_mode_.store(false); return Result::success(); } + const auto runtime = runtimeSnapshot_(); + if (!runtime) { + return Result::failure( + ArmErrorCode::RobotNotReady, + "[HuayanRobot] stopMotion failed: runtime is unavailable"); + } - const auto result = hrResult_(HRIF_GrpStop(box_id_, robot_id_), "GrpStop"); - busy_.store(false); + std::lock_guard termination_lock( + runtime->termination_mutex); + huayan_internal::StopRequest request; + bool stopped = false; + { + // Linearize cancellation and the vendor Stop with command submission, + // then release this lock before waiting for the old owner. The owner + // may need publishHrState_ (and therefore submission_mutex) in order to + // observe cancellation and exit. + std::lock_guard submission_lock( + runtime->submission_mutex); + request = runtime->motion.beginStop(); + if (!request.started()) { + return Result::failure( + ArmErrorCode::CommandRejected, + "[HuayanRobot] stopMotion rejected: another Stop is in progress"); + } + runtime->wait_cv.notify_all(); + stopped = terminateController_( + runtime, + kControllerStopTimeout, + runtime->program_active.load() || request.kind == MotionKind::Program); + } + const bool owner_exited = runtime->motion.waitForOwnerExit( + request.active_token, + kOwnerExitTimeout); + if (!stopped || !owner_exited || !runtime->motion.completeStop()) { + runtime->motion.failStop(); + return Result::failure( + ArmErrorCode::CommandFailed, + "[HuayanRobot] stopMotion failed: controller idle was not confirmed"); + } servo_mode_.store(false); - return result; + return Result::success(); } Result HuayanRobot::startServoMode(const ServoOptions& options) { - const auto ready = ensureConnected_("startServoMode"); + const auto runtime = runtimeSnapshot_(); + if (!runtime) { + return Result::failure( + ArmErrorCode::RobotNotReady, + "[HuayanRobot] startServoMode failed: runtime is unavailable"); + } + std::unique_lock admission_lock( + runtime->submission_mutex); + huayan_internal::SafetyPermit permit; + const auto ready = ensureMotionReady_("startServoMode", runtime, permit); if (!ready.ok()) { return ready; } - const double period = options.period > 0.0 ? options.period : 0.008; - const double lookahead = options.lookahead_time > 0.0 ? options.lookahead_time : 0.1; - const auto result = hrResult_(HRIF_StartServo(box_id_, robot_id_, period, lookahead), "StartServo"); - if (result.ok()) { - servo_mode_.store(true); + if (servo_mode_.load()) { + return Result::success(); } - return result; + const auto start = runtime->motion.begin(MotionKind::Servo); + if (!start.started()) { + return motionStartFailure_("startServoMode", start.status); + } + const double period = options.period > 0.0 ? options.period : 0.008; + const double lookahead = options.lookahead_time > 0.0 + ? options.lookahead_time + : 0.1; + int ret = 0; + { + std::lock_guard submission_lock( + runtime->submission_mutex); + std::lock_guard sdk_lock(sdk_mutex_); + if (runtime->motion.cancelled(start.token) || + !runtime->safety.validate(permit)) { + runtime->motion.finish(start.token, MotionFinishMode::Clear); + return Result::failure( + ArmErrorCode::CommandRejected, + "[HuayanRobot] startServoMode cancelled by a safety transition"); + } + ret = HRIF_StartServo(box_id_, robot_id_, period, lookahead); + } + if (ret != 0) { + runtime->motion.finish(start.token, MotionFinishMode::Clear); + return hrResult_(ret, "StartServo"); + } + { + std::lock_guard submission_lock( + runtime->submission_mutex); + if (runtime->motion.cancelled(start.token) || + !runtime->safety.validate(permit)) { + (void)runtime->motion.cancelActiveForSafety(); + runtime->motion.finish(start.token, MotionFinishMode::Clear); + runtime->wait_cv.notify_all(); + return Result::failure( + ArmErrorCode::CommandRejected, + "[HuayanRobot] startServoMode cancelled by a safety transition"); + } + servo_mode_.store(true); + runtime->motion.finish(start.token, MotionFinishMode::Retain); + } + return Result::success(); } Result HuayanRobot::servoJ(const JointPositionCommand& target) @@ -455,32 +1114,137 @@ Result HuayanRobot::servoJ(const JointPositionCommand& target) if (!validDof_(target.position.size(), error)) { return Result::failure(ArmErrorCode::InvalidDof, error); } - const auto ready = ensureConnected_("servoJ"); + const auto runtime = runtimeSnapshot_(); + if (!runtime) { + return Result::failure( + ArmErrorCode::RobotNotReady, + "[HuayanRobot] servoJ failed: runtime is unavailable"); + } + std::unique_lock admission_lock( + runtime->submission_mutex); + if (!servo_mode_.load()) { + return Result::failure( + ArmErrorCode::RobotNotReady, + "[HuayanRobot] servoJ failed: servo mode is not active"); + } + huayan_internal::SafetyPermit permit; + const auto ready = ensureMotionReady_("servoJ", runtime, permit); if (!ready.ok()) { return ready; } - + const auto start = runtime->motion.begin(MotionKind::Servo, true); + if (!start.started()) { + return motionStartFailure_("servoJ", start.status); + } std::array q_deg{}; - for (std::size_t i = 0; i < std::min(target.position.size(), q_deg.size()); ++i) { + for (std::size_t i = 0; + i < std::min(target.position.size(), q_deg.size()); + ++i) { q_deg[i] = radToDeg(target.position[i]); } - return hrResult_(HRIF_PushServoJ(box_id_, robot_id_, - q_deg[0], q_deg[1], q_deg[2], q_deg[3], q_deg[4], q_deg[5]), - "servoJ"); + int ret = 0; + { + std::lock_guard submission_lock( + runtime->submission_mutex); + std::lock_guard sdk_lock(sdk_mutex_); + if (runtime->motion.cancelled(start.token) || + !runtime->safety.validate(permit)) { + runtime->motion.finish(start.token, MotionFinishMode::RestorePrevious); + return Result::failure( + ArmErrorCode::CommandRejected, + "[HuayanRobot] servoJ cancelled by a safety transition"); + } + ret = HRIF_PushServoJ( + box_id_, robot_id_, + q_deg[0], q_deg[1], q_deg[2], + q_deg[3], q_deg[4], q_deg[5]); + } + if (ret != 0) { + runtime->motion.finish(start.token, MotionFinishMode::RestorePrevious); + return hrResult_(ret, "servoJ"); + } + { + std::lock_guard submission_lock( + runtime->submission_mutex); + if (runtime->motion.cancelled(start.token) || + !runtime->safety.validate(permit)) { + runtime->motion.finish(start.token, MotionFinishMode::RestorePrevious); + runtime->wait_cv.notify_all(); + return Result::failure( + ArmErrorCode::CommandRejected, + "[HuayanRobot] servoJ cancelled by a safety transition"); + } + runtime->motion.finish(start.token, MotionFinishMode::RestorePrevious); + } + return Result::success(); } -Result HuayanRobot::servoL(const CartesianPose& target, const FrameType frame) +Result HuayanRobot::servoL( + const CartesianPose& target, + const FrameType frame) { - (void)frame; - const auto ready = ensureConnected_("servoL"); + if (frame != FrameType::Base) { + return Result::failure( + ArmErrorCode::UnsupportedCommand, + "[HuayanRobot] servoL supports Base frame only"); + } + const auto runtime = runtimeSnapshot_(); + if (!runtime) { + return Result::failure( + ArmErrorCode::RobotNotReady, + "[HuayanRobot] servoL failed: runtime is unavailable"); + } + std::unique_lock admission_lock( + runtime->submission_mutex); + if (!servo_mode_.load()) { + return Result::failure( + ArmErrorCode::RobotNotReady, + "[HuayanRobot] servoL failed: servo mode is not active"); + } + huayan_internal::SafetyPermit permit; + const auto ready = ensureMotionReady_("servoL", runtime, permit); if (!ready.ok()) { return ready; } - + const auto start = runtime->motion.begin(MotionKind::Servo, true); + if (!start.started()) { + return motionStartFailure_("servoL", start.status); + } auto coord = poseToHrCoord(target); auto ucs = zeroHrFrame(); auto tcp = zeroHrFrame(); - return hrResult_(HRIF_PushServoP(box_id_, robot_id_, coord, ucs, tcp), "servoL"); + int ret = 0; + { + std::lock_guard submission_lock( + runtime->submission_mutex); + std::lock_guard sdk_lock(sdk_mutex_); + if (runtime->motion.cancelled(start.token) || + !runtime->safety.validate(permit)) { + runtime->motion.finish(start.token, MotionFinishMode::RestorePrevious); + return Result::failure( + ArmErrorCode::CommandRejected, + "[HuayanRobot] servoL cancelled by a safety transition"); + } + ret = HRIF_PushServoP(box_id_, robot_id_, coord, ucs, tcp); + } + if (ret != 0) { + runtime->motion.finish(start.token, MotionFinishMode::RestorePrevious); + return hrResult_(ret, "servoL"); + } + { + std::lock_guard submission_lock( + runtime->submission_mutex); + if (runtime->motion.cancelled(start.token) || + !runtime->safety.validate(permit)) { + runtime->motion.finish(start.token, MotionFinishMode::RestorePrevious); + runtime->wait_cv.notify_all(); + return Result::failure( + ArmErrorCode::CommandRejected, + "[HuayanRobot] servoL cancelled by a safety transition"); + } + runtime->motion.finish(start.token, MotionFinishMode::RestorePrevious); + } + return Result::success(); } Result HuayanRobot::servoSpeedJ(const JointVelocityCommand& velocity) @@ -489,7 +1253,9 @@ Result HuayanRobot::servoSpeedJ(const JointVelocityCommand& velocity) return unsupported_("servoSpeedJ"); } -Result HuayanRobot::servoSpeedL(const CartesianVelocity& velocity, const FrameType frame) +Result HuayanRobot::servoSpeedL( + const CartesianVelocity& velocity, + const FrameType frame) { (void)velocity; (void)frame; @@ -498,52 +1264,221 @@ Result HuayanRobot::servoSpeedL(const CartesianVelocity& velocity, const FrameTy Result HuayanRobot::stopServoMode() { - servo_mode_.store(false); return stopMotion(); } Result HuayanRobot::connect(const std::string& ip, const int port) { - if (connected_.load()) { + if (isConnected()) { return Result::success(); } if (ip.empty()) { - return Result::failure(ArmErrorCode::InvalidArgument, "[HuayanRobot] ip is empty"); + return Result::failure( + ArmErrorCode::InvalidArgument, + "[HuayanRobot] ip is empty"); } - std::lock_guard lock(mutex_); - const int use_port = port > 0 ? port : 10003; - const auto result = hrResult_(HRIF_Connect(box_id_, ip.c_str(), static_cast(use_port)), - "Connect"); - if (!result.ok()) { - connected_.store(false); - return result; + // A dropped transport can leave the local flag, monitor and an old waiter + // alive. Cancel and join that generation before a new SDK session can be + // created; otherwise the old waiter could issue HRIF reads against it. + const auto stale_runtime = runtimeSnapshot_(); + if (stale_runtime) { + huayan_internal::SafetyCancelResult cancelled; + { + std::unique_lock termination_lock( + stale_runtime->termination_mutex); + std::lock_guard submission_lock( + stale_runtime->submission_mutex); + cancelled = stale_runtime->motion.cancelActiveForSafety(); + connected_.store(false); + stale_runtime->monitor_running.store(false); + stale_runtime->wait_cv.notify_all(); + } + const bool owner_exited = stale_runtime->motion.waitForOwnerExit( + cancelled.active_token, kOwnerExitTimeout); + if (safety_monitor_thread_.joinable()) { + safety_monitor_thread_.join(); + } + if (!owner_exited) { + stale_runtime->motion.failStop(); + return Result::failure( + ArmErrorCode::CommandFailed, + "[HuayanRobot] reconnect failed: the old command owner did not exit"); + } + { + std::lock_guard sdk_lock(sdk_mutex_); + (void)HRIF_DisConnect(box_id_); + } + { + std::lock_guard state_lock(mutex_); + connected_.store(false); + servo_mode_.store(false); + runtime_.reset(); + } } - ip_ = ip; - port_ = use_port; - connected_.store(true); + + const int use_port = port > 0 ? port : 10003; + int ret = 0; + { + std::lock_guard state_lock(mutex_); + std::lock_guard sdk_lock(sdk_mutex_); + ret = HRIF_Connect( + box_id_, ip.c_str(), static_cast(use_port)); + if (ret == 0) { + ip_ = ip; + port_ = use_port; + runtime_ = std::make_shared(); + connected_.store(true); + servo_mode_.store(false); + software_emergency_stopped_.store(false); + software_protective_stopped_.store(false); + } + } + if (ret != 0) { + connected_.store(false); + return hrResult_(ret, "Connect"); + } + + const auto runtime = runtimeSnapshot_(); + const auto initial_state = sampleHrState_(runtime); + publishHrState_(runtime, initial_state); + if (!initial_state.valid) { + { + std::lock_guard sdk_lock(sdk_mutex_); + (void)HRIF_DisConnect(box_id_); + } + std::lock_guard state_lock(mutex_); + connected_.store(false); + runtime_.reset(); + return Result::failure( + ArmErrorCode::ConnectionFailed, + "[HuayanRobot] Connect failed: initial safety state is unavailable"); + } + + // A reconnect must not replace the software generation state while an old + // controller waypoint or box-wide script can still resume. Stop both and + // require three stable idle samples before the first motion permit exists. + if (!terminateController_(runtime, kControllerStopTimeout, true)) { + { + std::lock_guard sdk_lock(sdk_mutex_); + (void)HRIF_DisConnect(box_id_); + } + std::lock_guard state_lock(mutex_); + connected_.store(false); + runtime_.reset(); + return Result::failure( + ArmErrorCode::ConnectionFailed, + "[HuayanRobot] Connect failed: stale controller work could not be cleared"); + } + + safety_monitor_thread_ = std::thread( + &HuayanRobot::safetyMonitorLoop_, this, runtime); return Result::success(); } Result HuayanRobot::disconnect() { - if (connected_.load() || HRIF_IsConnected(box_id_)) { - const auto result = hrResult_(HRIF_DisConnect(box_id_), "DisConnect"); - connected_.store(false); - busy_.store(false); - servo_mode_.store(false); - return result; + const auto runtime = runtimeSnapshot_(); + if (!runtime && !connected_.load()) { + return Result::success(); } - connected_.store(false); - busy_.store(false); - servo_mode_.store(false); - return Result::success(); + const bool transport_connected = isConnected(); + huayan_internal::StopRequest disconnect_barrier; + bool owner_exited = true; + bool stop_confirmed = !connected_.load(); + int disconnect_ret = 0; + std::unique_lock termination_lock; + if (runtime) { + termination_lock = std::unique_lock( + runtime->termination_mutex); + } + + if (connected_.load() && runtime && transport_connected) { + const auto stop_result = stopMotion(); + if (!stop_result.ok()) { + // Keep the monitor, safety latch and runtime ownership alive. A + // failed Stop must not be hidden by throwing away local state. + return stop_result; + } + stop_confirmed = true; + } + + if (runtime) { + { + std::lock_guard submission_lock( + runtime->submission_mutex); + disconnect_barrier = runtime->motion.beginStop(); + if (!disconnect_barrier.started()) { + return Result::failure( + ArmErrorCode::CommandRejected, + "[HuayanRobot] disconnect failed: could not establish the disconnect barrier"); + } + runtime->wait_cv.notify_all(); + if (transport_connected) { + stop_confirmed = terminateController_( + runtime, + kControllerStopTimeout, + runtime->program_active.load() || + disconnect_barrier.kind == MotionKind::Program); + if (!stop_confirmed) { + runtime->motion.failStop(); + return Result::failure( + ArmErrorCode::CommandFailed, + "[HuayanRobot] disconnect failed: final controller Stop was not confirmed"); + } + } + std::lock_guard sdk_lock(sdk_mutex_); + if (connected_.load() || HRIF_IsConnected(box_id_)) { + disconnect_ret = HRIF_DisConnect(box_id_); + } + if (disconnect_ret != 0 && transport_connected) { + runtime->motion.failStop(); + return hrResult_(disconnect_ret, "DisConnect"); + } + connected_.store(false); + runtime->monitor_running.store(false); + } + runtime->wait_cv.notify_all(); + owner_exited = runtime->motion.waitForOwnerExit( + disconnect_barrier.active_token, kOwnerExitTimeout); + if (stop_confirmed && owner_exited) { + (void)runtime->motion.completeStop(); + } else { + runtime->motion.failStop(); + } + } else { + connected_.store(false); + } + + if (termination_lock.owns_lock()) { + termination_lock.unlock(); + } + if (safety_monitor_thread_.joinable()) { + safety_monitor_thread_.join(); + } + { + std::lock_guard state_lock(mutex_); + servo_mode_.store(false); + software_emergency_stopped_.store(false); + software_protective_stopped_.store(false); + runtime_.reset(); + } + if (!stop_confirmed || !owner_exited) { + return Result::failure( + ArmErrorCode::CommandFailed, + "[HuayanRobot] disconnect completed locally, but controller Stop was not confirmed before transport loss"); + } + return hrResult_(disconnect_ret, "DisConnect"); } bool HuayanRobot::isConnected() const { - return connected_.load() && HRIF_IsConnected(box_id_); + if (!connected_.load()) { + return false; + } + std::lock_guard sdk_lock(sdk_mutex_); + return HRIF_IsConnected(box_id_); } Result HuayanRobot::shutdown() @@ -551,11 +1486,81 @@ Result HuayanRobot::shutdown() if (!isConnected()) { return Result::success(); } - auto result = hrResult_(HRIF_ShutdownRobot(box_id_), "ShutdownRobot"); - if (!result.ok()) { - return result; + const auto runtime = runtimeSnapshot_(); + if (!runtime) { + return Result::failure( + ArmErrorCode::RobotNotReady, + "[HuayanRobot] shutdown failed: runtime is unavailable"); } - return disconnect(); + const auto poweroff_result = torqueOff(); + if (!poweroff_result.ok()) { + return poweroff_result; + } + + std::unique_lock termination_lock( + runtime->termination_mutex); + huayan_internal::StopRequest shutdown_barrier; + int shutdown_ret = 0; + { + std::lock_guard submission_lock( + runtime->submission_mutex); + shutdown_barrier = runtime->motion.beginStop(); + if (!shutdown_barrier.started()) { + return Result::failure( + ArmErrorCode::CommandRejected, + "[HuayanRobot] shutdown failed: could not establish the shutdown barrier"); + } + runtime->wait_cv.notify_all(); + if (!terminateController_( + runtime, + kControllerStopTimeout, + runtime->program_active.load() || + shutdown_barrier.kind == MotionKind::Program)) { + runtime->motion.failStop(); + return Result::failure( + ArmErrorCode::CommandFailed, + "[HuayanRobot] shutdown failed: final controller Stop was not confirmed"); + } + std::lock_guard sdk_lock(sdk_mutex_); + shutdown_ret = HRIF_ShutdownRobot(box_id_); + if (shutdown_ret == 0) { + connected_.store(false); + runtime->monitor_running.store(false); + } + } + if (shutdown_ret != 0) { + runtime->motion.failStop(); + return hrResult_(shutdown_ret, "ShutdownRobot"); + } + runtime->wait_cv.notify_all(); + const bool owner_exited = runtime->motion.waitForOwnerExit( + shutdown_barrier.active_token, kOwnerExitTimeout); + if (owner_exited) { + (void)runtime->motion.completeStop(); + } else { + runtime->motion.failStop(); + } + termination_lock.unlock(); + if (safety_monitor_thread_.joinable()) { + safety_monitor_thread_.join(); + } + { + std::lock_guard sdk_lock(sdk_mutex_); + (void)HRIF_DisConnect(box_id_); + } + { + std::lock_guard state_lock(mutex_); + servo_mode_.store(false); + software_emergency_stopped_.store(false); + software_protective_stopped_.store(false); + runtime_.reset(); + } + if (!owner_exited) { + return Result::failure( + ArmErrorCode::CommandFailed, + "[HuayanRobot] shutdown succeeded, but a raced command owner did not exit cleanly"); + } + return Result::success(); } Result HuayanRobot::clearFault() @@ -564,22 +1569,169 @@ Result HuayanRobot::clearFault() if (!ready.ok()) { return ready; } - return hrResult_(HRIF_GrpReset(box_id_, robot_id_), "GrpReset"); + const auto runtime = runtimeSnapshot_(); + if (!runtime) { + return Result::failure( + ArmErrorCode::RobotNotReady, + "[HuayanRobot] clearFault failed: runtime is unavailable"); + } + const auto state = sampleHrState_(runtime); + publishHrState_(runtime, state); + const auto safety = runtime->safety.snapshot(); + if (safety.latched) { + if (safety.latched_reason == SafetyCondition::RobotFault || + safety.latched_reason == SafetyCondition::Unknown) { + return completeSafetyRecovery_( + "clearFault", + runtime, + safety.epoch, + false, + false); + } + // Resetting a controller fault is useful after a physical E-stop, but + // this API deliberately does not clear that differently typed latch. + int reset_ret = 0; + { + std::lock_guard sdk_lock(sdk_mutex_); + reset_ret = HRIF_GrpReset(box_id_, robot_id_); + } + return hrResult_(reset_ret, "GrpReset"); + } + int ret = 0; + { + std::lock_guard sdk_lock(sdk_mutex_); + ret = HRIF_GrpReset(box_id_, robot_id_); + } + return hrResult_(ret, "GrpReset"); +} + +Result HuayanRobot::unlockProtectiveStop() +{ + const auto ready = ensureConnected_("unlockProtectiveStop"); + if (!ready.ok()) { + return ready; + } + const auto runtime = runtimeSnapshot_(); + if (!runtime) { + return Result::failure( + ArmErrorCode::RobotNotReady, + "[HuayanRobot] unlockProtectiveStop failed: runtime is unavailable"); + } + const auto state = sampleHrState_(runtime); + publishHrState_(runtime, state); + const auto safety = runtime->safety.snapshot(); + if (!safety.latched) { + return Result::success(); + } + if (!isProtectiveCondition(safety.latched_reason) && + !software_protective_stopped_.load()) { + return Result::failure( + ArmErrorCode::RobotInEmergencyStop, + "[HuayanRobot] unlockProtectiveStop rejected: the latched stop is not protective"); + } + return completeSafetyRecovery_( + "unlockProtectiveStop", + runtime, + safety.epoch, + false, + software_protective_stopped_.load()); } Result HuayanRobot::loadProgram(const std::string& program_name) { - (void)program_name; - return Result::success(); + if (program_name.empty()) { + return Result::failure( + ArmErrorCode::InvalidArgument, + "[HuayanRobot] loadProgram failed: program name is empty"); + } + const auto runtime = runtimeSnapshot_(); + if (!runtime) { + return Result::failure( + ArmErrorCode::RobotNotReady, + "[HuayanRobot] loadProgram failed: runtime is unavailable"); + } + std::unique_lock admission_lock( + runtime->submission_mutex); + huayan_internal::SafetyPermit permit; + const auto ready = ensureMotionReady_("loadProgram", runtime, permit); + if (!ready.ok()) { + return ready; + } + const auto start = runtime->motion.begin(MotionKind::Program); + if (!start.started()) { + return motionStartFailure_("loadProgram", start.status); + } + int ret = 0; + { + std::lock_guard submission_lock( + runtime->submission_mutex); + std::lock_guard sdk_lock(sdk_mutex_); + if (runtime->motion.cancelled(start.token) || + !runtime->safety.validate(permit)) { + runtime->motion.finish(start.token, MotionFinishMode::Clear); + return Result::failure( + ArmErrorCode::CommandRejected, + "[HuayanRobot] loadProgram cancelled by a safety transition"); + } + ret = HRIF_SwitchScript(box_id_, robot_id_, program_name); + runtime->motion.finish(start.token, MotionFinishMode::Clear); + } + return hrResult_(ret, "SwitchScript"); } Result HuayanRobot::playProgram() { - const auto ready = ensureConnected_("playProgram"); + const auto runtime = runtimeSnapshot_(); + if (!runtime) { + return Result::failure( + ArmErrorCode::RobotNotReady, + "[HuayanRobot] playProgram failed: runtime is unavailable"); + } + std::unique_lock admission_lock( + runtime->submission_mutex); + huayan_internal::SafetyPermit permit; + const auto ready = ensureMotionReady_("playProgram", runtime, permit); if (!ready.ok()) { return ready; } - return hrResult_(HRIF_StartScript(box_id_), "StartScript"); + const auto start = runtime->motion.begin(MotionKind::Program); + if (!start.started()) { + return motionStartFailure_("playProgram", start.status); + } + int ret = 0; + { + std::lock_guard submission_lock( + runtime->submission_mutex); + std::lock_guard sdk_lock(sdk_mutex_); + if (runtime->motion.cancelled(start.token) || + !runtime->safety.validate(permit)) { + runtime->motion.finish(start.token, MotionFinishMode::Clear); + return Result::failure( + ArmErrorCode::CommandRejected, + "[HuayanRobot] playProgram cancelled by a safety transition"); + } + ret = HRIF_StartScript(box_id_); + } + if (ret != 0) { + runtime->motion.finish(start.token, MotionFinishMode::Clear); + return hrResult_(ret, "StartScript"); + } + { + std::lock_guard submission_lock( + runtime->submission_mutex); + if (runtime->motion.cancelled(start.token) || + !runtime->safety.validate(permit)) { + (void)runtime->motion.cancelActiveForSafety(); + runtime->motion.finish(start.token, MotionFinishMode::Clear); + runtime->wait_cv.notify_all(); + return Result::failure( + ArmErrorCode::CommandRejected, + "[HuayanRobot] playProgram cancelled by a safety transition"); + } + runtime->program_active.store(true); + runtime->motion.finish(start.token, MotionFinishMode::Retain); + } + return Result::success(); } Result HuayanRobot::pauseProgram() @@ -588,7 +1740,38 @@ Result HuayanRobot::pauseProgram() if (!ready.ok()) { return ready; } - return hrResult_(HRIF_PauseScript(box_id_), "PauseScript"); + const auto runtime = runtimeSnapshot_(); + if (!runtime) { + return Result::failure( + ArmErrorCode::RobotNotReady, + "[HuayanRobot] pauseProgram failed: runtime is unavailable"); + } + std::unique_lock admission_lock( + runtime->submission_mutex); + if (!runtime || !runtime->program_active.load()) { + return Result::failure( + ArmErrorCode::RobotNotReady, + "[HuayanRobot] pauseProgram failed: no tracked program is active"); + } + const auto safety = runtime->safety.tryPermit(); + if (!safety) { + return Result::failure( + ArmErrorCode::CommandRejected, + "[HuayanRobot] pauseProgram rejected by the safety latch"); + } + int ret = 0; + { + std::lock_guard submission_lock( + runtime->submission_mutex); + std::lock_guard sdk_lock(sdk_mutex_); + if (!runtime->safety.validate(*safety)) { + return Result::failure( + ArmErrorCode::CommandRejected, + "[HuayanRobot] pauseProgram cancelled by a safety transition"); + } + ret = HRIF_PauseScript(box_id_); + } + return hrResult_(ret, "PauseScript"); } Result HuayanRobot::stopProgram() @@ -597,12 +1780,45 @@ Result HuayanRobot::stopProgram() if (!ready.ok()) { return ready; } - return hrResult_(HRIF_StopScript(box_id_), "StopScript"); + const auto runtime = runtimeSnapshot_(); + if (!runtime) { + return Result::failure( + ArmErrorCode::RobotNotReady, + "[HuayanRobot] stopProgram failed: runtime is unavailable"); + } + std::lock_guard termination_lock( + runtime->termination_mutex); + huayan_internal::StopRequest request; + bool stopped = false; + { + std::lock_guard submission_lock( + runtime->submission_mutex); + request = runtime->motion.beginStop(MotionKind::Program); + if (!request.started()) { + return Result::failure( + ArmErrorCode::CommandRejected, + "[HuayanRobot] stopProgram rejected: another Stop is in progress"); + } + runtime->wait_cv.notify_all(); + stopped = terminateController_( + runtime, kControllerStopTimeout, true); + } + const bool owner_exited = runtime->motion.waitForOwnerExit( + request.active_token, kOwnerExitTimeout); + if (!stopped || !owner_exited || !runtime->motion.completeStop()) { + runtime->motion.failStop(); + return Result::failure( + ArmErrorCode::CommandFailed, + "[HuayanRobot] stopProgram failed: controller idle was not confirmed"); + } + servo_mode_.store(false); + return Result::success(); } -std::vector HuayanRobot::ik(const std::string& base_link, - const std::string& ee_link, - const CartesianPose& pose) +std::vector HuayanRobot::ik( + const std::string& base_link, + const std::string& ee_link, + const CartesianPose& pose) { (void)base_link; (void)ee_link; @@ -611,7 +1827,9 @@ std::vector HuayanRobot::ik(const std::string& base_link, return {}; } -CartesianPose HuayanRobot::fk(const std::string& base_link, const std::string& ee_link) +CartesianPose HuayanRobot::fk( + const std::string& base_link, + const std::string& ee_link) { (void)base_link; (void)ee_link; @@ -629,50 +1847,163 @@ CartesianVelocity HuayanRobot::getSpeedLCommandTwistBase() const return readTcpVelocity_(); } +bool HuayanRobot::busy() const +{ + const auto runtime = runtimeSnapshot_(); + return runtime && runtime->motion.busy(); +} + Result HuayanRobot::ensureConnected_(const std::string& context) const { if (!isConnected()) { - return Result::failure(ArmErrorCode::NotConnected, - "[HuayanRobot] " + context + " failed: arm is not connected"); + return Result::failure( + ArmErrorCode::NotConnected, + "[HuayanRobot] " + context + " failed: arm is not connected"); } return Result::success(); } +Result HuayanRobot::ensureMotionReady_( + const std::string& context, + const std::shared_ptr& runtime, + huayan_internal::SafetyPermit& permit) const +{ + const auto connected = ensureConnected_(context); + if (!connected.ok()) { + return connected; + } + if (!runtime) { + return Result::failure( + ArmErrorCode::RobotNotReady, + "[HuayanRobot] " + context + " failed: runtime is unavailable"); + } + if (runtimeSnapshot_().get() != runtime.get()) { + return Result::failure( + ArmErrorCode::CommandRejected, + "[HuayanRobot] " + context + + " rejected: the controller session changed before admission"); + } + const auto state = sampleHrState_(runtime); + publishHrState_(runtime, state); + if (!state.valid) { + return Result::failure( + ArmErrorCode::RobotNotReady, + "[HuayanRobot] " + context + + " failed: current safety state is unavailable"); + } + + const auto safety = runtime->safety.snapshot(); + if (safety.latched || + !huayan_internal::isMotionSafe(safety.observed)) { + const auto reason = safety.latched + ? safety.latched_reason + : safety.observed; + ArmErrorCode code = ArmErrorCode::CommandRejected; + if (isEmergencyCondition(reason)) { + code = ArmErrorCode::RobotInEmergencyStop; + } else if (isProtectiveCondition(reason)) { + code = ArmErrorCode::RobotInProtectiveStop; + } else if (safetyModeFromCondition(reason) == SafetyMode::Fault) { + code = ArmErrorCode::RobotInFault; + } + return Result::failure( + code, + "[HuayanRobot] " + context + + " rejected: safety event is latched; explicit recovery is required"); + } + if (state.error != 0) { + return Result::failure( + ArmErrorCode::RobotInFault, + "[HuayanRobot] " + context + " failed: robot error, code=" + + std::to_string(state.error_code)); + } + if (state.electrified == 0 || state.enabled == 0) { + return Result::failure( + ArmErrorCode::RobotNotPowered, + "[HuayanRobot] " + context + " failed: robot is not enabled"); + } + const auto maybe_permit = runtime->safety.tryPermit(); + if (!maybe_permit) { + return Result::failure( + ArmErrorCode::CommandRejected, + "[HuayanRobot] " + context + " rejected by the safety state"); + } + permit = *maybe_permit; + return Result::success(); +} + Result HuayanRobot::unsupported_(const std::string& name) const { - const std::string message = "[HuayanRobot] " + name + " is not implemented"; + const std::string message = + "[HuayanRobot] " + name + " is not implemented"; CMVR_LOG(ERROR) << message; return Result::failure(ArmErrorCode::UnsupportedCommand, message); } -Result HuayanRobot::hrResult_(const int code, const std::string& context) const +Result HuayanRobot::hrResult_( + const int code, + const std::string& context) const { if (code == 0) { return Result::success(); } std::string sdk_message; - (void)HRIF_GetErrorCodeStr(box_id_, code, sdk_message); - std::ostringstream oss; - oss << "[HuayanRobot] " << context << " failed, code=" << code; - if (!sdk_message.empty()) { - oss << ", message=" << sdk_message; + { + std::lock_guard sdk_lock(sdk_mutex_); + (void)HRIF_GetErrorCodeStr(box_id_, code, sdk_message); } - const auto message = oss.str(); - CMVR_LOG(ERROR) << message; - return Result::failure(ArmErrorCode::CommandFailed, message); + std::ostringstream message; + message << "[HuayanRobot] " << context << " failed, code=" << code; + if (!sdk_message.empty()) { + message << ", message=" << sdk_message; + } + CMVR_LOG(ERROR) << message.str(); + return Result::failure(ArmErrorCode::CommandFailed, message.str()); } -bool HuayanRobot::validDof_(const std::size_t size, std::string& error) const +Result HuayanRobot::motionStartFailure_( + const std::string& context, + const MotionStartStatus status) const +{ + ArmErrorCode code = ArmErrorCode::RobotNotReady; + std::string reason = "arm is busy"; + switch (status) { + case MotionStartStatus::Stopping: + reason = "a Stop operation is in progress"; + break; + case MotionStartStatus::Blocked: + code = ArmErrorCode::CommandRejected; + reason = "controller ownership is blocked until explicit recovery"; + break; + case MotionStartStatus::Invalid: + code = ArmErrorCode::InvalidArgument; + reason = "invalid motion kind"; + break; + case MotionStartStatus::Busy: + break; + case MotionStartStatus::Started: + return Result::success(); + } + return Result::failure( + code, + "[HuayanRobot] " + context + " rejected: " + reason); +} + +bool HuayanRobot::validDof_( + const std::size_t size, + std::string& error) const { if (size != model_.dof) { - error = "[HuayanRobot] command dof mismatch, expected=" + std::to_string(model_.dof) + - ", actual=" + std::to_string(size); + error = "[HuayanRobot] command dof mismatch, expected=" + + std::to_string(model_.dof) + ", actual=" + + std::to_string(size); CMVR_LOG(ERROR) << error; return false; } if (model_.dof > 6) { - error = "[HuayanRobot] command dof exceeds SDK limit: " + std::to_string(model_.dof); + error = "[HuayanRobot] command dof exceeds SDK limit: " + + std::to_string(model_.dof); CMVR_LOG(ERROR) << error; return false; } @@ -680,132 +2011,142 @@ bool HuayanRobot::validDof_(const std::size_t size, std::string& error) const } HuayanRobot::HrState HuayanRobot::readHrState_() const +{ + const auto runtime = runtimeSnapshot_(); + if (!runtime) { + return {}; + } + const auto state = sampleHrState_(runtime); + publishHrState_(runtime, state); + return state; +} + +HuayanRobot::HrState HuayanRobot::sampleHrState_( + const std::shared_ptr& runtime) const { HrState state; - if (!isConnected()) { + if (!runtime || !connected_.load()) { return state; } - const int ret = HRIF_ReadRobotState(box_id_, robot_id_, - state.moving, - state.enabled, - state.error, - state.error_code, - state.error_axis, - state.brake, - state.paused, - state.emergency_stop, - state.safeguard, - state.electrified, - state.connected_to_box, - state.blending_done, - state.in_pos); - state.valid = ret == 0; - if (ret != 0) { - CMVR_LOG(ERROR) << "[HuayanRobot] read robot state failed, code=" << ret; + int state_ret = -1; + int safety_ret = -1; + { + std::lock_guard sdk_lock(sdk_mutex_); + if (!HRIF_IsConnected(box_id_)) { + return state; + } + state_ret = HRIF_ReadRobotState( + box_id_, robot_id_, + state.moving, + state.enabled, + state.error, + state.error_code, + state.error_axis, + state.brake, + state.paused, + state.emergency_stop, + state.safeguard, + state.electrified, + state.connected_to_box, + state.blending_done, + state.in_pos); + safety_ret = HRIF_ReadEmergencyInfo( + box_id_, robot_id_, + state.emergency_signal_fault, + state.emergency_input, + state.safeguard_signal_fault, + state.safeguard_input); } + state.valid = state_ret == 0 && safety_ret == 0 && + state.connected_to_box != 0; return state; } +void HuayanRobot::publishHrState_( + const std::shared_ptr& runtime, + const HrState& state) const +{ + if (!runtime) { + return; + } + std::lock_guard submission_lock( + runtime->submission_mutex); + const auto before = runtime->safety.snapshot(); + huayan_internal::RawSafetyState raw; + raw.valid = state.valid; + raw.emergency_signal_fault = state.emergency_signal_fault; + raw.emergency_stop = state.emergency_stop != 0 || + state.emergency_input != 0; + raw.safeguard_signal_fault = state.safeguard_signal_fault; + raw.safeguard_stop = state.safeguard != 0 || + state.safeguard_input != 0; + raw.robot_fault = state.error; + raw.software_emergency_stop = software_emergency_stopped_.load(); + raw.software_protective_stop = software_protective_stopped_.load(); + runtime->safety.observe(raw); + const auto after = runtime->safety.snapshot(); + if (state.valid) { + runtime->last_valid_sample_ns.store(monotonicNowNs()); + } + if (after.latched && + (!before.latched || after.epoch != before.epoch || + after.observed != before.observed)) { + runtime->termination_confirmed.store(false); + (void)runtime->motion.cancelActiveForSafety(); + runtime->wait_cv.notify_all(); + } +} + std::vector HuayanRobot::readJointPositionRad_() const { - std::vector q(model_.dof, 0.0); - if (!isConnected()) { - return q; + std::vector values; + if (!readJointPositionSample_(values)) { + values.assign(model_.dof, 0.0); } - - double j1 = 0.0; - double j2 = 0.0; - double j3 = 0.0; - double j4 = 0.0; - double j5 = 0.0; - double j6 = 0.0; - const int ret = HRIF_ReadActJointPos(box_id_, robot_id_, j1, j2, j3, j4, j5, j6); - if (ret != 0) { - CMVR_LOG(ERROR) << "[HuayanRobot] read joint position failed, code=" << ret; - return q; - } - - const std::array values{j1, j2, j3, j4, j5, j6}; - for (std::size_t i = 0; i < std::min(q.size(), values.size()); ++i) { - q[i] = degToRad(values[i]); - } - return q; + return values; } std::vector HuayanRobot::readJointVelocityRad_() const { - std::vector qd(model_.dof, 0.0); - if (!isConnected()) { - return qd; + std::vector values; + if (!readJointVelocitySample_(values)) { + values.assign(model_.dof, 0.0); } - - double j1 = 0.0; - double j2 = 0.0; - double j3 = 0.0; - double j4 = 0.0; - double j5 = 0.0; - double j6 = 0.0; - const int ret = HRIF_ReadActJointVel(box_id_, robot_id_, j1, j2, j3, j4, j5, j6); - if (ret != 0) { - CMVR_LOG(ERROR) << "[HuayanRobot] read joint velocity failed, code=" << ret; - return qd; - } - - const std::array values{j1, j2, j3, j4, j5, j6}; - for (std::size_t i = 0; i < std::min(qd.size(), values.size()); ++i) { - qd[i] = degToRad(values[i]); - } - return qd; + return values; } CartesianPose HuayanRobot::readTcpPose_() const { CartesianPose pose; - if (!isConnected()) { - return pose; - } - - double x = 0.0; - double y = 0.0; - double z = 0.0; - double rx = 0.0; - double ry = 0.0; - double rz = 0.0; - const int ret = HRIF_ReadActTcpPos(box_id_, robot_id_, x, y, z, rx, ry, rz); - if (ret != 0) { - CMVR_LOG(ERROR) << "[HuayanRobot] read tcp pose failed, code=" << ret; - return pose; - } - - pose.x = mmToMeters(x); - pose.y = mmToMeters(y); - pose.z = mmToMeters(z); - pose.rx = degToRad(rx); - pose.ry = degToRad(ry); - pose.rz = degToRad(rz); + (void)readTcpPoseSample_(pose); return pose; } CartesianVelocity HuayanRobot::readTcpVelocity_() const { CartesianVelocity velocity; - if (!isConnected()) { + if (!connected_.load()) { return velocity; } - double x = 0.0; double y = 0.0; double z = 0.0; double rx = 0.0; double ry = 0.0; double rz = 0.0; - const int ret = HRIF_ReadActTcpVel(box_id_, robot_id_, x, y, z, rx, ry, rz); + int ret = 0; + { + std::lock_guard sdk_lock(sdk_mutex_); + if (!HRIF_IsConnected(box_id_)) { + return velocity; + } + ret = HRIF_ReadActTcpVel( + box_id_, robot_id_, x, y, z, rx, ry, rz); + } if (ret != 0) { - CMVR_LOG(ERROR) << "[HuayanRobot] read tcp velocity failed, code=" << ret; return velocity; } - velocity.vx = mmToMeters(x); velocity.vy = mmToMeters(y); velocity.vz = mmToMeters(z); @@ -815,14 +2156,114 @@ CartesianVelocity HuayanRobot::readTcpVelocity_() const return velocity; } +bool HuayanRobot::readJointPositionSample_( + std::vector& values) const +{ + values.assign(model_.dof, 0.0); + if (!connected_.load()) { + return false; + } + double j1 = 0.0; + double j2 = 0.0; + double j3 = 0.0; + double j4 = 0.0; + double j5 = 0.0; + double j6 = 0.0; + int ret = 0; + { + std::lock_guard sdk_lock(sdk_mutex_); + if (!HRIF_IsConnected(box_id_)) { + return false; + } + ret = HRIF_ReadActJointPos( + box_id_, robot_id_, j1, j2, j3, j4, j5, j6); + } + if (ret != 0) { + return false; + } + const std::array sample{j1, j2, j3, j4, j5, j6}; + for (std::size_t i = 0; + i < std::min(values.size(), sample.size()); + ++i) { + values[i] = degToRad(sample[i]); + } + return true; +} + +bool HuayanRobot::readJointVelocitySample_( + std::vector& values) const +{ + values.assign(model_.dof, 0.0); + if (!connected_.load()) { + return false; + } + double j1 = 0.0; + double j2 = 0.0; + double j3 = 0.0; + double j4 = 0.0; + double j5 = 0.0; + double j6 = 0.0; + int ret = 0; + { + std::lock_guard sdk_lock(sdk_mutex_); + if (!HRIF_IsConnected(box_id_)) { + return false; + } + ret = HRIF_ReadActJointVel( + box_id_, robot_id_, j1, j2, j3, j4, j5, j6); + } + if (ret != 0) { + return false; + } + const std::array sample{j1, j2, j3, j4, j5, j6}; + for (std::size_t i = 0; + i < std::min(values.size(), sample.size()); + ++i) { + values[i] = degToRad(sample[i]); + } + return true; +} + +bool HuayanRobot::readTcpPoseSample_(CartesianPose& pose) const +{ + pose = {}; + if (!connected_.load()) { + return false; + } + double x = 0.0; + double y = 0.0; + double z = 0.0; + double rx = 0.0; + double ry = 0.0; + double rz = 0.0; + int ret = 0; + { + std::lock_guard sdk_lock(sdk_mutex_); + if (!HRIF_IsConnected(box_id_)) { + return false; + } + ret = HRIF_ReadActTcpPos( + box_id_, robot_id_, x, y, z, rx, ry, rz); + } + if (ret != 0) { + return false; + } + pose.x = mmToMeters(x); + pose.y = mmToMeters(y); + pose.z = mmToMeters(z); + pose.rx = degToRad(rx); + pose.ry = degToRad(ry); + pose.rz = degToRad(rz); + return true; +} + std::vector HuayanRobot::currentJointPositionDeg_() const { - const auto q_rad = readJointPositionRad_(); - std::vector q_deg(q_rad.size(), 0.0); - for (std::size_t i = 0; i < q_rad.size(); ++i) { - q_deg[i] = radToDeg(q_rad[i]); + auto values = readJointPositionRad_(); + for (auto& value : values) { + value = radToDeg(value); } - return q_deg; + return values; } std::string HuayanRobot::nextCommandId_() const @@ -830,56 +2271,423 @@ std::string HuayanRobot::nextCommandId_() const return id_ + "_" + std::to_string(++command_seq_); } -Result HuayanRobot::waitMotionDone_(const std::string& context, const int timeout_ms) const +Result HuayanRobot::waitMotionDone_( + const std::string& context, + const std::shared_ptr& runtime, + const huayan_internal::MotionToken motion_token, + const huayan_internal::SafetyPermit safety_permit, + const std::string& command_id, + const std::vector* joint_target, + const CartesianPose* tcp_target, + const int timeout_ms) const { - const auto start = std::chrono::steady_clock::now(); - while (true) { + const auto started_at = std::chrono::steady_clock::now(); + bool saw_motion = false; + bool saw_command_id = false; + int stable_completion_samples = 0; + + while (runtime->monitor_running.load()) { + if (runtime->motion.cancelled(motion_token) || + !runtime->safety.validate(safety_permit)) { + runtime->motion.finish(motion_token, MotionFinishMode::Clear); + return Result::failure( + ArmErrorCode::CommandRejected, + "[HuayanRobot] " + context + + " cancelled by Stop or a safety transition"); + } + bool done = false; - const int ret = HRIF_IsMotionDone(box_id_, robot_id_, done); - if (ret != 0) { - return hrResult_(ret, "IsMotionDone(" + context + ")"); - } - const auto state = readHrState_(); - if (state.valid) { - if (state.error != 0) { - return Result::failure( - ArmErrorCode::CommandFailed, - "[HuayanRobot] " + context + " failed: robot error, code=" + - std::to_string(state.error_code)); - } - - if (state.emergency_stop != 0) { - return Result::failure( - ArmErrorCode::CommandFailed, - "[HuayanRobot] " + context + " failed: emergency stop"); - } - - if (state.safeguard != 0) { - return Result::failure( - ArmErrorCode::CommandFailed, - "[HuayanRobot] " + context + " failed: safeguard stop"); + std::string current_command_id; + int done_ret = 0; + int id_ret = 0; + { + std::lock_guard sdk_lock(sdk_mutex_); + done_ret = HRIF_IsMotionDone(box_id_, robot_id_, done); + if (!command_id.empty()) { + id_ret = HRIF_ReadCurWaypointID( + box_id_, robot_id_, current_command_id); } } + if (done_ret != 0 || id_ret != 0) { + const bool stopped = terminateController_( + runtime, kControllerStopTimeout, false); + if (stopped) { + runtime->motion.finish(motion_token, MotionFinishMode::Clear); + } else { + runtime->motion.failMotion(motion_token); + } + return hrResult_( + done_ret != 0 ? done_ret : id_ret, + done_ret != 0 + ? "IsMotionDone(" + context + ")" + : "ReadCurWaypointID(" + context + ")"); + } - if (done) { - return Result::success(); + const auto state = sampleHrState_(runtime); + publishHrState_(runtime, state); + if (!state.valid) { + continue; + } + saw_motion = saw_motion || state.moving != 0 || !done; + saw_command_id = saw_command_id || + (!command_id.empty() && current_command_id == command_id); + + if (state.error != 0 || state.emergency_stop != 0 || + state.safeguard != 0 || state.emergency_input != 0 || + state.safeguard_input != 0) { + continue; } const auto elapsed = std::chrono::duration_cast( - std::chrono::steady_clock::now() - start).count(); - - if (elapsed > timeout_ms) { - { - std::lock_guard lock(mutex_); - (void)HRIF_GrpStop(box_id_, robot_id_); + std::chrono::steady_clock::now() - started_at); + const bool correlated = command_id.empty() + ? (saw_motion || + monotonicNowNs() >= + runtime->speed_completion_not_before_ns.load()) + : (saw_motion || saw_command_id || + elapsed >= kCompletionCorrelationGrace); + const bool at_target = targetReached_(joint_target, tcp_target); + if (done && state.moving == 0 && correlated && at_target) { + ++stable_completion_samples; + if (stable_completion_samples >= 2) { + runtime->motion.finish(motion_token, MotionFinishMode::Clear); + return Result::success(); } + } else { + stable_completion_samples = 0; + } + if (elapsed.count() > timeout_ms) { + const bool stopped = terminateController_( + runtime, kControllerStopTimeout, false); + if (stopped) { + runtime->motion.finish(motion_token, MotionFinishMode::Clear); + } else { + runtime->motion.failMotion(motion_token); + } return Result::failure( - ArmErrorCode::CommandFailed, + ArmErrorCode::Timeout, "[HuayanRobot] " + context + " timeout"); } - std::this_thread::sleep_for(std::chrono::milliseconds(500)); + std::unique_lock wait_lock(runtime->wait_mutex); + runtime->wait_cv.wait_for(wait_lock, kSafetyPollPeriod); + } + + runtime->motion.failMotion(motion_token); + return Result::failure( + ArmErrorCode::NotConnected, + "[HuayanRobot] " + context + " cancelled by disconnect"); +} + +bool HuayanRobot::targetReached_( + const std::vector* joint_target, + const CartesianPose* tcp_target) const +{ + if (joint_target) { + std::vector current; + if (!readJointPositionSample_(current) || + current.size() != joint_target->size()) { + return false; + } + for (std::size_t i = 0; i < current.size(); ++i) { + if (angularDistance(current[i], (*joint_target)[i]) > + kJointTargetToleranceRad) { + return false; + } + } + } + if (tcp_target) { + CartesianPose current; + if (!readTcpPoseSample_(current)) { + return false; + } + if (std::abs(current.x - tcp_target->x) > kTcpPositionToleranceM || + std::abs(current.y - tcp_target->y) > kTcpPositionToleranceM || + std::abs(current.z - tcp_target->z) > kTcpPositionToleranceM || + angularDistance(current.rx, tcp_target->rx) > kTcpRotationToleranceRad || + angularDistance(current.ry, tcp_target->ry) > kTcpRotationToleranceRad || + angularDistance(current.rz, tcp_target->rz) > kTcpRotationToleranceRad) { + return false; + } + } + return true; +} + +bool HuayanRobot::controllerIdleStable_( + const std::shared_ptr& runtime, + const std::chrono::milliseconds timeout) const +{ + const auto deadline = std::chrono::steady_clock::now() + timeout; + int stable_samples = 0; + while (std::chrono::steady_clock::now() < deadline) { + const auto state = sampleHrState_(runtime); + publishHrState_(runtime, state); + bool done = false; + int done_ret = 0; + { + std::lock_guard sdk_lock(sdk_mutex_); + done_ret = HRIF_IsMotionDone(box_id_, robot_id_, done); + } + std::vector velocity; + const bool velocity_valid = readJointVelocitySample_(velocity); + const bool velocity_zero = velocity_valid && std::all_of( + velocity.begin(), velocity.end(), + [](const double value) { + return std::abs(value) <= kIdleVelocityToleranceRad; + }); + if (state.valid && done_ret == 0 && done && + state.moving == 0 && velocity_zero) { + ++stable_samples; + if (stable_samples >= 3) { + return true; + } + } else { + stable_samples = 0; + } + std::unique_lock wait_lock(runtime->wait_mutex); + runtime->wait_cv.wait_for(wait_lock, kSafetyPollPeriod); + } + return false; +} + +bool HuayanRobot::terminateController_( + const std::shared_ptr& runtime, + const std::chrono::milliseconds timeout, + const bool stop_program) const +{ + std::lock_guard termination_lock( + runtime->termination_mutex); + std::lock_guard submission_lock( + runtime->submission_mutex); + runtime->termination_confirmed.store(false); + int stop_ret = 0; + int script_ret = 0; + { + std::lock_guard sdk_lock(sdk_mutex_); + stop_ret = HRIF_GrpStop(box_id_, robot_id_); + if (stop_program) { + script_ret = HRIF_StopScript(box_id_); + } + } + if (stop_ret != 0 || script_ret != 0) { + (void)hrResult_(stop_ret != 0 ? stop_ret : script_ret, + stop_ret != 0 ? "GrpStop" : "StopScript"); + return false; + } + if (stop_program) { + runtime->program_active.store(false); + } + servo_mode_.store(false); + const bool idle = controllerIdleStable_(runtime, timeout); + runtime->termination_confirmed.store(idle); + return idle; +} + +Result HuayanRobot::completeSafetyRecovery_( + const std::string& context, + const std::shared_ptr& runtime, + const std::uint64_t expected_epoch, + const bool enable_robot, + const bool release_software_guard) +{ + if (!runtime || expected_epoch == 0 || + runtime->safety.snapshot().epoch != expected_epoch) { + return Result::failure( + ArmErrorCode::CommandRejected, + "[HuayanRobot] " + context + + " rejected: safety event changed before recovery"); + } + std::lock_guard termination_lock( + runtime->termination_mutex); + + if (release_software_guard) { + int ret = 0; + { + std::lock_guard sdk_lock(sdk_mutex_); + ret = HRIF_EnterSafetyGuard(box_id_, robot_id_, 0); + } + const auto result = hrResult_(ret, "ExitSafetyGuard"); + if (!result.ok()) { + return result; + } + } + software_protective_stopped_.store(false); + software_emergency_stopped_.store(false); + + auto state = sampleHrState_(runtime); + publishHrState_(runtime, state); + if (!state.valid || state.emergency_signal_fault != 0 || + state.emergency_input != 0 || state.emergency_stop != 0 || + state.safeguard_signal_fault != 0 || state.safeguard_input != 0 || + state.safeguard != 0) { + return Result::failure( + ArmErrorCode::CommandRejected, + "[HuayanRobot] " + context + + " rejected: hardware safety input is still active or unreadable"); + } + if (runtime->safety.snapshot().epoch != expected_epoch) { + return Result::failure( + ArmErrorCode::CommandRejected, + "[HuayanRobot] " + context + + " rejected: a newer safety event was observed"); + } + + huayan_internal::StopRequest stop_request; + bool stopped = false; + { + std::lock_guard submission_lock( + runtime->submission_mutex); + stop_request = runtime->motion.beginStop(); + if (!stop_request.started()) { + return Result::failure( + ArmErrorCode::CommandRejected, + "[HuayanRobot] " + context + + " rejected: another Stop operation is in progress"); + } + runtime->wait_cv.notify_all(); + stopped = terminateController_( + runtime, + kControllerStopTimeout, + runtime->program_active.load() || + stop_request.kind == MotionKind::Program); + } + const bool owner_exited = runtime->motion.waitForOwnerExit( + stop_request.active_token, + kOwnerExitTimeout); + if (!stopped || !owner_exited) { + runtime->motion.failStop(); + return Result::failure( + ArmErrorCode::CommandFailed, + "[HuayanRobot] " + context + + " failed: controller termination was not confirmed"); + } + + int reset_ret = 0; + int enable_ret = 0; + { + std::lock_guard sdk_lock(sdk_mutex_); + reset_ret = HRIF_GrpReset(box_id_, robot_id_); + if (reset_ret == 0 && enable_robot) { + enable_ret = HRIF_GrpEnable(box_id_, robot_id_); + } + } + if (reset_ret != 0 || enable_ret != 0) { + runtime->motion.failStop(); + return hrResult_( + reset_ret != 0 ? reset_ret : enable_ret, + reset_ret != 0 ? "GrpReset" : "GrpEnable"); + } + + const auto recovery_deadline = + std::chrono::steady_clock::now() + kControllerStopTimeout; + bool robot_ready = false; + while (std::chrono::steady_clock::now() < recovery_deadline) { + state = sampleHrState_(runtime); + publishHrState_(runtime, state); + const auto safety = runtime->safety.snapshot(); + if (safety.epoch != expected_epoch) { + runtime->motion.failStop(); + return Result::failure( + ArmErrorCode::CommandRejected, + "[HuayanRobot] " + context + + " cancelled by a newer safety event"); + } + robot_ready = state.valid && state.error == 0 && + state.emergency_stop == 0 && state.emergency_input == 0 && + state.safeguard == 0 && state.safeguard_input == 0 && + (!enable_robot || + (state.enabled != 0 && state.electrified != 0)); + if (robot_ready && + huayan_internal::isMotionSafe(safety.observed)) { + break; + } + std::unique_lock wait_lock(runtime->wait_mutex); + runtime->wait_cv.wait_for(wait_lock, kSafetyPollPeriod); + } + if (!robot_ready) { + runtime->motion.failStop(); + return Result::failure( + ArmErrorCode::RobotNotReady, + "[HuayanRobot] " + context + + " failed: safe robot state was not confirmed after reset"); + } + + const auto recovery = runtime->safety.beginRecovery(expected_epoch); + if (!recovery) { + runtime->motion.failStop(); + return Result::failure( + ArmErrorCode::CommandRejected, + "[HuayanRobot] " + context + + " rejected: safety recovery epoch is no longer valid"); + } + const bool idle = controllerIdleStable_(runtime, kControllerStopTimeout); + if (!idle || !runtime->motion.completeStop()) { + runtime->safety.failRecovery(*recovery); + runtime->motion.failStop(); + return Result::failure( + ArmErrorCode::CommandFailed, + "[HuayanRobot] " + context + + " failed: stable controller idle was not confirmed"); + } + if (!runtime->safety.completeRecovery( + *recovery, + robot_ready, + idle, + runtime->termination_confirmed.load())) { + (void)runtime->motion.cancelActiveForSafety(); + return Result::failure( + ArmErrorCode::CommandRejected, + "[HuayanRobot] " + context + + " cancelled by a safety transition during recovery"); + } + return Result::success(); +} + +std::shared_ptr +HuayanRobot::runtimeSnapshot_() const +{ + std::lock_guard state_lock(mutex_); + return runtime_; +} + +void HuayanRobot::safetyMonitorLoop_( + const std::shared_ptr& runtime) +{ + while (runtime->monitor_running.load()) { + const auto state = sampleHrState_(runtime); + publishHrState_(runtime, state); + const auto safety = runtime->safety.snapshot(); + if (safety.latched && + (!runtime->termination_confirmed.load() || + !state.valid || state.moving != 0 || + runtime->program_active.load())) { + std::lock_guard termination_lock( + runtime->termination_mutex); + huayan_internal::SafetyCancelResult cancelled; + bool stopped = false; + { + std::lock_guard submission_lock( + runtime->submission_mutex); + cancelled = runtime->motion.cancelActiveForSafety(); + stopped = terminateController_( + runtime, + std::chrono::milliseconds(1000), + runtime->program_active.load() || + cancelled.kind == MotionKind::Program); + } + const bool owner_exited = runtime->motion.waitForOwnerExit( + cancelled.active_token, + std::chrono::milliseconds(1000)); + runtime->termination_confirmed.store(stopped && owner_exited); + servo_mode_.store(false); + } + + std::unique_lock wait_lock(runtime->wait_mutex); + runtime->wait_cv.wait_for( + wait_lock, + kSafetyPollPeriod, + [runtime]() { return !runtime->monitor_running.load(); }); } } diff --git a/cmvr-es/devices/arm/huayan_arm/huayan_arm.h b/cmvr-es/devices/arm/huayan_arm/huayan_arm.h index 92a3d1a3..010e0339 100644 --- a/cmvr-es/devices/arm/huayan_arm/huayan_arm.h +++ b/cmvr-es/devices/arm/huayan_arm/huayan_arm.h @@ -9,12 +9,17 @@ #define CMVR_ES_HUAYAN_ROBOT_H #include +#include +#include #include #include +#include #include +#include #include #include "cmvr/config/arm_config/arm_config.pb.h" +#include "devices/arm/huayan_arm/huayan_lifecycle_state.h" #include "devices/arm/robot_arm.h" namespace cmvr::device { @@ -35,15 +40,15 @@ public: CartesianPose getTcpPose(FrameType frame = FrameType::Base) const override; RobotMode getRobotMode() const override; SafetyMode getSafetyMode() const override; - ControlMode getControlMode() const override { return servo_mode_.load() ? ControlMode::Servo : ControlMode::Position; } + ControlMode getControlMode() const override; Result torqueOn() override; Result torqueOff() override; Result calibrateZeroQ(const std::string& joint_name) override; Result emergencyStop() override; - Result protectiveStop() override { return emergencyStop(); } + Result protectiveStop() override; Result setSpeedScaling(double scaling) override; - double getSpeedScaling() const override { return speed_scaling_; } + double getSpeedScaling() const override { return speed_scaling_.load(); } bool isProtectiveStopped() const override; bool isEmergencyStopped() const override; bool isFault() const override; @@ -71,7 +76,7 @@ public: Result brakeRelease() override { return torqueOn(); } Result shutdown() override; Result clearFault() override; - Result unlockProtectiveStop() override { return clearFault(); } + Result unlockProtectiveStop() override; Result loadProgram(const std::string& program_name) override; Result playProgram() override; Result pauseProgram() override; @@ -84,7 +89,7 @@ public: CartesianPose fk(const std::string& base_link, const std::string& ee_link) override; CartesianPose fk(bool is_tcp = true) override; CartesianVelocity getSpeedLCommandTwistBase() const override; - bool busy() const override { return busy_.load(); } + bool busy() const override; private: struct HrState { @@ -101,21 +106,68 @@ private: int connected_to_box{0}; int blending_done{0}; int in_pos{0}; + int emergency_signal_fault{0}; + int emergency_input{0}; + int safeguard_signal_fault{0}; + int safeguard_input{0}; bool valid{false}; }; + struct RuntimeState; + Result ensureConnected_(const std::string& context) const; + Result ensureMotionReady_( + const std::string& context, + const std::shared_ptr& runtime, + huayan_internal::SafetyPermit& permit) const; Result unsupported_(const std::string& name) const; Result hrResult_(int code, const std::string& context) const; + Result motionStartFailure_( + const std::string& context, + huayan_internal::MotionStartStatus status) const; bool validDof_(std::size_t size, std::string& error) const; HrState readHrState_() const; + HrState sampleHrState_( + const std::shared_ptr& runtime) const; + void publishHrState_( + const std::shared_ptr& runtime, + const HrState& state) const; std::vector readJointPositionRad_() const; std::vector readJointVelocityRad_() const; CartesianPose readTcpPose_() const; CartesianVelocity readTcpVelocity_() const; + bool readJointPositionSample_(std::vector& values) const; + bool readJointVelocitySample_(std::vector& values) const; + bool readTcpPoseSample_(CartesianPose& pose) const; std::vector currentJointPositionDeg_() const; std::string nextCommandId_() const; - Result waitMotionDone_(const std::string& context, int timeout_ms) const; + Result waitMotionDone_( + const std::string& context, + const std::shared_ptr& runtime, + huayan_internal::MotionToken motion_token, + huayan_internal::SafetyPermit safety_permit, + const std::string& command_id, + const std::vector* joint_target, + const CartesianPose* tcp_target, + int timeout_ms) const; + bool targetReached_( + const std::vector* joint_target, + const CartesianPose* tcp_target) const; + bool controllerIdleStable_( + const std::shared_ptr& runtime, + std::chrono::milliseconds timeout) const; + bool terminateController_( + const std::shared_ptr& runtime, + std::chrono::milliseconds timeout, + bool stop_program) const; + Result completeSafetyRecovery_( + const std::string& context, + const std::shared_ptr& runtime, + std::uint64_t expected_epoch, + bool enable_robot, + bool release_software_guard); + std::shared_ptr runtimeSnapshot_() const; + void safetyMonitorLoop_(const std::shared_ptr& runtime); private: config::RobotArmConfig cfg_; @@ -127,12 +179,16 @@ private: unsigned int robot_id_{0}; std::string tcp_name_{"TCP"}; std::string ucs_name_{"Base"}; - double speed_scaling_{1.0}; + std::atomic speed_scaling_{1.0}; std::atomic connected_{false}; - std::atomic busy_{false}; - std::atomic servo_mode_{false}; + mutable std::atomic servo_mode_{false}; + std::atomic software_emergency_stopped_{false}; + std::atomic software_protective_stopped_{false}; mutable std::mutex mutex_; + mutable std::recursive_mutex sdk_mutex_; mutable std::atomic command_seq_{0}; + std::shared_ptr runtime_; + std::thread safety_monitor_thread_; }; } // namespace cmvr::device @@ -140,4 +196,4 @@ private: #endif // CMVR_ES_HUAYAN_ROBOT_H -#endif //CMVR_ES_HUAYAN_ARM_H \ No newline at end of file +#endif //CMVR_ES_HUAYAN_ARM_H diff --git a/cmvr-es/devices/arm/huayan_arm/huayan_lifecycle_state.h b/cmvr-es/devices/arm/huayan_arm/huayan_lifecycle_state.h new file mode 100644 index 00000000..6475abd4 --- /dev/null +++ b/cmvr-es/devices/arm/huayan_arm/huayan_lifecycle_state.h @@ -0,0 +1,504 @@ +#ifndef CMVR_ES_HUAYAN_LIFECYCLE_STATE_H +#define CMVR_ES_HUAYAN_LIFECYCLE_STATE_H + +#include +#include +#include +#include +#include +#include + +namespace cmvr::device::huayan_internal { + +enum class MotionKind { + None, + Joint, + Linear, + SpeedJoint, + SpeedLinear, + Servo, + Program, +}; + +struct MotionToken { + std::uint64_t generation{0}; + MotionKind kind{MotionKind::None}; + + bool valid() const noexcept + { + return generation != 0 && kind != MotionKind::None; + } +}; + +enum class MotionStartStatus { + Started, + Invalid, + Busy, + Stopping, + Blocked, +}; + +struct MotionStartResult { + MotionStartStatus status{MotionStartStatus::Busy}; + MotionToken token; + + bool started() const noexcept + { + return status == MotionStartStatus::Started; + } +}; + +enum class MotionFinishMode { + RestorePrevious, + Clear, + Retain, +}; + +enum class StopStartStatus { + Started, + AlreadyStopping, +}; + +struct StopRequest { + StopStartStatus status{StopStartStatus::AlreadyStopping}; + MotionKind kind{MotionKind::None}; + MotionToken active_token; + bool tracked_motion{false}; + + bool started() const noexcept + { + return status == StopStartStatus::Started; + } +}; + +struct SafetyCancelResult { + MotionKind kind{MotionKind::None}; + MotionToken active_token; + bool tracked_motion{false}; +}; + +struct MotionSnapshot { + MotionKind active_kind{MotionKind::None}; + MotionKind retained_kind{MotionKind::None}; + std::uint64_t active_generation{0}; + bool owner_active{false}; + bool stop_in_progress{false}; + bool blocked{false}; +}; + +// Tracks a single Huayan controller operation owner. Generation tokens make +// completion from an older RPC harmless after Stop or a safety event has +// cancelled it. Servo and program operations may retain their kind after the +// submitting RPC returns; begin(kind, true) supports same-kind updates while +// that retained controller mode remains active. +class MotionState final { +public: + MotionStartResult begin( + const MotionKind kind, + const bool replace_retained_same_kind = false) + { + std::lock_guard lock(mutex_); + if (kind == MotionKind::None) { + return {MotionStartStatus::Invalid, {}}; + } + if (stop_in_progress_) { + return {MotionStartStatus::Stopping, {}}; + } + if (blocked_) { + return {MotionStartStatus::Blocked, {}}; + } + if (owner_active_) { + return {MotionStartStatus::Busy, {}}; + } + if (retained_kind_ != MotionKind::None && + (!replace_retained_same_kind || retained_kind_ != kind)) { + return {MotionStartStatus::Busy, {}}; + } + + const MotionToken token{++next_generation_, kind}; + owner_active_ = true; + active_token_ = token; + previous_kind_ = retained_kind_; + return {MotionStartStatus::Started, token}; + } + + void finish( + const MotionToken token, + const MotionFinishMode mode = MotionFinishMode::RestorePrevious) + { + std::lock_guard lock(mutex_); + if (!owner_active_ || + active_token_.generation != token.generation) { + return; + } + + owner_active_ = false; + active_token_ = {}; + if (!stop_in_progress_ && !blocked_) { + switch (mode) { + case MotionFinishMode::RestorePrevious: + retained_kind_ = previous_kind_; + break; + case MotionFinishMode::Clear: + retained_kind_ = MotionKind::None; + break; + case MotionFinishMode::Retain: + retained_kind_ = token.kind; + break; + } + } + previous_kind_ = MotionKind::None; + owner_finished_cv_.notify_all(); + } + + void failMotion(const MotionToken token) + { + std::lock_guard lock(mutex_); + if (!owner_active_ || + active_token_.generation != token.generation) { + return; + } + + owner_active_ = false; + active_token_ = {}; + retained_kind_ = token.kind; + previous_kind_ = MotionKind::None; + blocked_ = true; + owner_finished_cv_.notify_all(); + } + + StopRequest beginStop( + const MotionKind requested_kind = MotionKind::None) + { + std::lock_guard lock(mutex_); + if (stop_in_progress_) { + return {}; + } + + stop_in_progress_ = true; + const MotionToken active = owner_active_ + ? active_token_ + : MotionToken{}; + if (active.valid()) { + cancelled_generation_ = std::max( + cancelled_generation_, active.generation); + } + + MotionKind kind = MotionKind::None; + if (active.valid()) { + kind = active.kind; + } else if (retained_kind_ != MotionKind::None) { + kind = retained_kind_; + } else { + kind = requested_kind; + } + if (kind != MotionKind::None) { + retained_kind_ = kind; + } + + return { + StopStartStatus::Started, + kind, + active, + active.valid() || kind != MotionKind::None}; + } + + SafetyCancelResult cancelActiveForSafety() + { + std::lock_guard lock(mutex_); + const MotionToken active = owner_active_ + ? active_token_ + : MotionToken{}; + if (active.valid()) { + cancelled_generation_ = std::max( + cancelled_generation_, active.generation); + } + + const MotionKind kind = active.valid() + ? active.kind + : retained_kind_; + if (kind != MotionKind::None) { + retained_kind_ = kind; + } + // A hardware safety transition is independent of a concurrent + // software Stop. New controller operations remain rejected until + // termination is positively confirmed. + blocked_ = true; + owner_finished_cv_.notify_all(); + return {kind, active, active.valid() || kind != MotionKind::None}; + } + + bool cancelled(const MotionToken token) const + { + std::lock_guard lock(mutex_); + return token.valid() && + token.generation <= cancelled_generation_; + } + + bool waitForOwnerExit( + const MotionToken token, + const std::chrono::milliseconds timeout) + { + if (!token.valid()) { + return true; + } + std::unique_lock lock(mutex_); + return owner_finished_cv_.wait_for( + lock, + timeout, + [this, token]() { + return !owner_active_ || + active_token_.generation != token.generation; + }); + } + + bool ownerActive(const MotionToken token) const + { + if (!token.valid()) { + return false; + } + std::lock_guard lock(mutex_); + return owner_active_ && + active_token_.generation == token.generation; + } + + bool completeStop() + { + std::lock_guard lock(mutex_); + if (owner_active_) { + return false; + } + stop_in_progress_ = false; + blocked_ = false; + active_token_ = {}; + retained_kind_ = MotionKind::None; + previous_kind_ = MotionKind::None; + owner_finished_cv_.notify_all(); + return true; + } + + void failStop() + { + std::lock_guard lock(mutex_); + stop_in_progress_ = false; + blocked_ = true; + owner_finished_cv_.notify_all(); + } + + MotionSnapshot snapshot() const + { + std::lock_guard lock(mutex_); + return { + owner_active_ ? active_token_.kind : MotionKind::None, + retained_kind_, + owner_active_ ? active_token_.generation : 0, + owner_active_, + stop_in_progress_, + blocked_}; + } + + bool busy() const + { + const auto state = snapshot(); + return state.owner_active || state.stop_in_progress || + state.blocked || + state.retained_kind != MotionKind::None; + } + +private: + mutable std::mutex mutex_; + std::condition_variable owner_finished_cv_; + std::uint64_t next_generation_{0}; + std::uint64_t cancelled_generation_{0}; + MotionToken active_token_; + MotionKind retained_kind_{MotionKind::None}; + MotionKind previous_kind_{MotionKind::None}; + bool owner_active_{false}; + bool stop_in_progress_{false}; + bool blocked_{false}; +}; + +enum class SafetyCondition { + Unknown, + Normal, + EmergencyStop, + SafeguardStop, + RobotFault, + EmergencySignalFault, + SafeguardSignalFault, + SoftwareEmergencyStop, + SoftwareProtectiveStop, +}; + +struct RawSafetyState { + bool valid{false}; + int emergency_signal_fault{0}; + int emergency_stop{0}; + int safeguard_signal_fault{0}; + int safeguard_stop{0}; + int robot_fault{0}; + bool software_emergency_stop{false}; + bool software_protective_stop{false}; +}; + +inline SafetyCondition classifySafetyCondition( + const RawSafetyState& state) noexcept +{ + if (!state.valid) { + return SafetyCondition::Unknown; + } + if (state.emergency_signal_fault != 0) { + return SafetyCondition::EmergencySignalFault; + } + if (state.safeguard_signal_fault != 0) { + return SafetyCondition::SafeguardSignalFault; + } + if (state.emergency_stop != 0) { + return SafetyCondition::EmergencyStop; + } + if (state.safeguard_stop != 0) { + return SafetyCondition::SafeguardStop; + } + if (state.robot_fault != 0) { + return SafetyCondition::RobotFault; + } + if (state.software_emergency_stop) { + return SafetyCondition::SoftwareEmergencyStop; + } + if (state.software_protective_stop) { + return SafetyCondition::SoftwareProtectiveStop; + } + return SafetyCondition::Normal; +} + +inline bool isMotionSafe(const SafetyCondition condition) noexcept +{ + return condition == SafetyCondition::Normal; +} + +struct SafetyPermit { + std::uint64_t epoch{0}; + + bool valid() const noexcept { return epoch != 0; } +}; + +struct RecoveryToken { + std::uint64_t epoch{0}; + + bool valid() const noexcept { return epoch != 0; } +}; + +struct SafetySnapshot { + SafetyCondition observed{SafetyCondition::Unknown}; + SafetyCondition latched_reason{SafetyCondition::Unknown}; + std::uint64_t epoch{0}; + bool latched{false}; + bool recovery_in_progress{false}; +}; + +// Safety inputs are events, not merely levels. Returning to Normal never +// clears a prior unsafe event. Explicit recovery is tied atomically to the +// event epoch, so a second event invalidates an older in-flight recovery. +class SafetyState final { +public: + void observe(const RawSafetyState& raw_state) + { + observe(classifySafetyCondition(raw_state)); + } + + void observe(const SafetyCondition condition) + { + std::lock_guard lock(mutex_); + const bool changed = observed_ != condition; + observed_ = condition; + if (isMotionSafe(condition)) { + return; + } + + if (!latched_ || recovery_in_progress_ || changed) { + ++epoch_; + } + latched_ = true; + recovery_in_progress_ = false; + latched_reason_ = condition; + } + + std::optional tryPermit() const + { + std::lock_guard lock(mutex_); + if (latched_ || !isMotionSafe(observed_)) { + return std::nullopt; + } + return SafetyPermit{epoch_}; + } + + bool validate(const SafetyPermit permit) const + { + std::lock_guard lock(mutex_); + return permit.valid() && permit.epoch == epoch_ && !latched_ && + isMotionSafe(observed_); + } + + std::optional beginRecovery( + const std::uint64_t expected_epoch) + { + std::lock_guard lock(mutex_); + if (expected_epoch == 0 || expected_epoch != epoch_ || !latched_ || + recovery_in_progress_ || !isMotionSafe(observed_)) { + return std::nullopt; + } + recovery_in_progress_ = true; + return RecoveryToken{epoch_}; + } + + bool completeRecovery( + const RecoveryToken token, + const bool robot_ready, + const bool controller_idle, + const bool cancellation_confirmed) + { + std::lock_guard lock(mutex_); + if (!token.valid() || token.epoch != epoch_ || !latched_ || + !recovery_in_progress_ || !isMotionSafe(observed_) || + !robot_ready || !controller_idle || !cancellation_confirmed) { + return false; + } + + latched_ = false; + recovery_in_progress_ = false; + latched_reason_ = SafetyCondition::Unknown; + ++epoch_; + return true; + } + + void failRecovery(const RecoveryToken token) + { + std::lock_guard lock(mutex_); + if (token.valid() && token.epoch == epoch_) { + recovery_in_progress_ = false; + } + } + + SafetySnapshot snapshot() const + { + std::lock_guard lock(mutex_); + return { + observed_, + latched_reason_, + epoch_, + latched_, + recovery_in_progress_}; + } + +private: + mutable std::mutex mutex_; + SafetyCondition observed_{SafetyCondition::Unknown}; + SafetyCondition latched_reason_{SafetyCondition::Unknown}; + std::uint64_t epoch_{1}; + bool latched_{false}; + bool recovery_in_progress_{false}; +}; + +} // namespace cmvr::device::huayan_internal + +#endif // CMVR_ES_HUAYAN_LIFECYCLE_STATE_H diff --git a/cmvr-es/devices/arm/huayan_arm/tests/huayan_arm_sdk_test.cpp b/cmvr-es/devices/arm/huayan_arm/tests/huayan_arm_sdk_test.cpp new file mode 100644 index 00000000..b2569995 --- /dev/null +++ b/cmvr-es/devices/arm/huayan_arm/tests/huayan_arm_sdk_test.cpp @@ -0,0 +1,834 @@ +#include "devices/arm/huayan_arm/huayan_arm.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "HR_Pro.h" + +namespace { + +using Clock = std::chrono::steady_clock; +using namespace std::chrono_literals; + +constexpr double kPi = 3.14159265358979323846; + +double radToDeg(const double value) +{ + return value * 180.0 / kPi; +} + +struct FakeSdkState final { + std::mutex mutex; + bool connected{false}; + bool enabled{true}; + bool electrified{true}; + bool robot_error{false}; + bool paused{false}; + bool emergency_input{false}; + bool emergency_signal_fault{false}; + bool safeguard_input{false}; + bool safeguard_signal_fault{false}; + bool software_safeguard{false}; + + bool motion_active{false}; + bool motion_is_joint{true}; + bool stop_pending{false}; + bool hold_next_motion{false}; + bool stale_done_once{false}; + Clock::time_point completion_at{}; + Clock::time_point stop_complete_at{}; + std::array joint_position_deg{}; + std::array joint_target_deg{}; + std::array tcp_position_hr{}; + std::array tcp_target_hr{}; + std::string waypoint_id; + + bool servo_started{false}; + bool program_running{false}; + std::string selected_program; + + int move_j_calls{0}; + int move_l_calls{0}; + int speed_j_calls{0}; + int speed_l_calls{0}; + int group_stop_calls{0}; + int group_reset_calls{0}; + int start_servo_calls{0}; + int stop_script_calls{0}; + int idle_velocity_reads_after_stop{0}; + bool count_idle_reads{false}; + + void refreshLocked() + { + const auto now = Clock::now(); + const bool safety_active = emergency_input || safeguard_input || + software_safeguard; + + if (stop_pending && now >= stop_complete_at) { + stop_pending = false; + motion_active = false; + stale_done_once = false; + count_idle_reads = true; + } + + if (motion_active && !stop_pending && !safety_active && + completion_at != Clock::time_point{} && now >= completion_at) { + motion_active = false; + stale_done_once = false; + if (motion_is_joint) { + joint_position_deg = joint_target_deg; + } else { + tcp_position_hr = tcp_target_hr; + } + } + } + + bool movingLocked() + { + refreshLocked(); + return motion_active && !emergency_input && !safeguard_input && + !software_safeguard; + } + + bool doneLocked() + { + refreshLocked(); + return !motion_active; + } + + void startMotionLocked(const bool joint) + { + motion_active = true; + motion_is_joint = joint; + stop_pending = false; + count_idle_reads = false; + stale_done_once = true; + if (hold_next_motion) { + // The fallback deadline keeps a failed test from leaving a worker + // blocked for the production 60 second timeout. + completion_at = Clock::now() + 3s; + hold_next_motion = false; + } else { + completion_at = Clock::now() + 120ms; + } + } +}; + +FakeSdkState g_sdk; + +void resetFakeSdk() +{ + std::lock_guard lock(g_sdk.mutex); + g_sdk.connected = false; + g_sdk.enabled = true; + g_sdk.electrified = true; + g_sdk.robot_error = false; + g_sdk.paused = false; + g_sdk.emergency_input = false; + g_sdk.emergency_signal_fault = false; + g_sdk.safeguard_input = false; + g_sdk.safeguard_signal_fault = false; + g_sdk.software_safeguard = false; + g_sdk.motion_active = false; + g_sdk.motion_is_joint = true; + g_sdk.stop_pending = false; + g_sdk.hold_next_motion = false; + g_sdk.stale_done_once = false; + g_sdk.completion_at = {}; + g_sdk.stop_complete_at = {}; + g_sdk.joint_position_deg = {}; + g_sdk.joint_target_deg = {}; + g_sdk.tcp_position_hr = {}; + g_sdk.tcp_target_hr = {}; + g_sdk.waypoint_id.clear(); + g_sdk.servo_started = false; + g_sdk.program_running = false; + g_sdk.selected_program.clear(); + g_sdk.move_j_calls = 0; + g_sdk.move_l_calls = 0; + g_sdk.speed_j_calls = 0; + g_sdk.speed_l_calls = 0; + g_sdk.group_stop_calls = 0; + g_sdk.group_reset_calls = 0; + g_sdk.start_servo_calls = 0; + g_sdk.stop_script_calls = 0; + g_sdk.idle_velocity_reads_after_stop = 0; + g_sdk.count_idle_reads = false; +} + +void holdNextMotion() +{ + std::lock_guard lock(g_sdk.mutex); + g_sdk.hold_next_motion = true; +} + +void setHardwareEmergencyStop(const bool active) +{ + std::lock_guard lock(g_sdk.mutex); + g_sdk.emergency_input = active; + // If the wrapper never sends a real group Stop, releasing the switch makes + // the pending fake waypoint move again. This models the field failure. +} + +void setEmergencySignalFault(const bool active) +{ + std::lock_guard lock(g_sdk.mutex); + g_sdk.emergency_signal_fault = active; +} + +void dropFakeTransport() +{ + std::lock_guard lock(g_sdk.mutex); + g_sdk.connected = false; +} + +template +bool waitUntil(Predicate&& predicate, + const std::chrono::milliseconds timeout = 2s) +{ + const auto deadline = Clock::now() + timeout; + while (Clock::now() < deadline) { + if (predicate()) { + return true; + } + std::this_thread::sleep_for(10ms); + } + return predicate(); +} + +cmvr::config::RobotArmConfig makeConfig() +{ + cmvr::config::RobotArmConfig cfg; + cfg.set_id("huayan_fake_sdk"); + auto* vendor = cfg.mutable_vendor(); + vendor->set_brand(cmvr::config::VENDOR_ROBOT_ARM_BRAND_HUAYAN_ARM); + vendor->set_ip("127.0.0.1"); + vendor->set_port(10003); + vendor->set_model("HuayanFake"); + vendor->set_dof(6); + vendor->set_base_frame("Base"); + vendor->set_tool_frame("TCP"); + for (int i = 1; i <= 6; ++i) { + vendor->add_joint_names("joint_" + std::to_string(i)); + } + return cfg; +} + +int failures = 0; + +#define CHECK_TRUE(condition) \ + do { \ + if (!(condition)) { \ + std::cerr << "CHECK_TRUE failed at line " << __LINE__ << ": " \ + << #condition << std::endl; \ + ++failures; \ + } \ + } while (false) + +} // namespace + +// The test executable exports these strong symbols. On ELF platforms they +// interpose the real SDK definitions used by libhuayan_arm, giving the test a +// deterministic controller without opening a network connection. +extern "C" { + +int HRIF_Connect(unsigned int, const char*, unsigned short) +{ + std::lock_guard lock(g_sdk.mutex); + g_sdk.connected = true; + return 0; +} + +int HRIF_DisConnect(unsigned int) +{ + std::lock_guard lock(g_sdk.mutex); + g_sdk.connected = false; + g_sdk.motion_active = false; + return 0; +} + +bool HRIF_IsConnected(unsigned int) +{ + std::lock_guard lock(g_sdk.mutex); + return g_sdk.connected; +} + +int HRIF_GetErrorCodeStr(unsigned int, int error_code, std::string& message) +{ + message = "fake SDK error " + std::to_string(error_code); + return 0; +} + +int HRIF_GrpEnable(unsigned int, unsigned int) +{ + std::lock_guard lock(g_sdk.mutex); + if (g_sdk.emergency_input || g_sdk.safeguard_input || + g_sdk.software_safeguard) { + return 101; + } + g_sdk.enabled = true; + g_sdk.electrified = true; + return 0; +} + +int HRIF_GrpDisable(unsigned int, unsigned int) +{ + std::lock_guard lock(g_sdk.mutex); + g_sdk.enabled = false; + g_sdk.electrified = false; + return 0; +} + +int HRIF_GrpReset(unsigned int, unsigned int) +{ + std::lock_guard lock(g_sdk.mutex); + ++g_sdk.group_reset_calls; + if (g_sdk.emergency_input || g_sdk.safeguard_input || + g_sdk.software_safeguard) { + return 102; + } + g_sdk.robot_error = false; + return 0; +} + +int HRIF_GrpStop(unsigned int, unsigned int) +{ + std::lock_guard lock(g_sdk.mutex); + ++g_sdk.group_stop_calls; + g_sdk.idle_velocity_reads_after_stop = 0; + g_sdk.count_idle_reads = false; + if (g_sdk.motion_active) { + g_sdk.stop_pending = true; + g_sdk.stop_complete_at = Clock::now() + 120ms; + g_sdk.completion_at = {}; + } else { + g_sdk.stop_pending = false; + g_sdk.count_idle_reads = true; + } + g_sdk.servo_started = false; + return 0; +} + +int HRIF_SetOverride(unsigned int, unsigned int, double) +{ + return 0; +} + +int HRIF_ReadRobotState(unsigned int, unsigned int, + int& moving, int& enabled, int& error, + int& error_code, int& error_axis, int& brake, + int& paused, int& emergency_stop, int& safeguard, + int& electrified, int& connected_to_box, + int& blending_done, int& in_position) +{ + std::lock_guard lock(g_sdk.mutex); + if (!g_sdk.connected) { + return 201; + } + moving = g_sdk.movingLocked() ? 1 : 0; + enabled = g_sdk.enabled ? 1 : 0; + error = g_sdk.robot_error ? 1 : 0; + error_code = g_sdk.robot_error ? 9001 : 0; + error_axis = 0; + brake = g_sdk.enabled ? 1 : 0; + paused = g_sdk.paused ? 1 : 0; + emergency_stop = g_sdk.emergency_input ? 1 : 0; + safeguard = (g_sdk.safeguard_input || g_sdk.software_safeguard) ? 1 : 0; + electrified = g_sdk.electrified ? 1 : 0; + connected_to_box = 1; + blending_done = moving == 0 ? 1 : 0; + in_position = g_sdk.doneLocked() ? 1 : 0; + return 0; +} + +int HRIF_ReadEmergencyInfo(unsigned int, unsigned int, + int& emergency_signal_fault, + int& emergency_input, + int& safeguard_signal_fault, + int& safeguard_input) +{ + std::lock_guard lock(g_sdk.mutex); + if (!g_sdk.connected) { + return 202; + } + emergency_signal_fault = g_sdk.emergency_signal_fault ? 1 : 0; + emergency_input = g_sdk.emergency_input ? 1 : 0; + safeguard_signal_fault = g_sdk.safeguard_signal_fault ? 1 : 0; + safeguard_input = + (g_sdk.safeguard_input || g_sdk.software_safeguard) ? 1 : 0; + return 0; +} + +int HRIF_ReadCurWaypointID(unsigned int, unsigned int, std::string& waypoint) +{ + std::lock_guard lock(g_sdk.mutex); + waypoint = g_sdk.waypoint_id; + return 0; +} + +int HRIF_IsMotionDone(unsigned int, unsigned int, bool& done) +{ + std::lock_guard lock(g_sdk.mutex); + if (g_sdk.stale_done_once) { + g_sdk.stale_done_once = false; + done = true; + } else { + done = g_sdk.doneLocked(); + } + return 0; +} + +int HRIF_ReadActJointPos(unsigned int, unsigned int, + double& j1, double& j2, double& j3, + double& j4, double& j5, double& j6) +{ + std::lock_guard lock(g_sdk.mutex); + g_sdk.refreshLocked(); + j1 = g_sdk.joint_position_deg[0]; + j2 = g_sdk.joint_position_deg[1]; + j3 = g_sdk.joint_position_deg[2]; + j4 = g_sdk.joint_position_deg[3]; + j5 = g_sdk.joint_position_deg[4]; + j6 = g_sdk.joint_position_deg[5]; + return 0; +} + +int HRIF_ReadActJointVel(unsigned int, unsigned int, + double& j1, double& j2, double& j3, + double& j4, double& j5, double& j6) +{ + std::lock_guard lock(g_sdk.mutex); + const double velocity = g_sdk.movingLocked() ? 5.0 : 0.0; + j1 = j2 = j3 = j4 = j5 = j6 = velocity; + if (velocity == 0.0 && g_sdk.count_idle_reads) { + ++g_sdk.idle_velocity_reads_after_stop; + } + return 0; +} + +int HRIF_ReadActTcpPos(unsigned int, unsigned int, + double& x, double& y, double& z, + double& rx, double& ry, double& rz) +{ + std::lock_guard lock(g_sdk.mutex); + g_sdk.refreshLocked(); + x = g_sdk.tcp_position_hr[0]; + y = g_sdk.tcp_position_hr[1]; + z = g_sdk.tcp_position_hr[2]; + rx = g_sdk.tcp_position_hr[3]; + ry = g_sdk.tcp_position_hr[4]; + rz = g_sdk.tcp_position_hr[5]; + return 0; +} + +int HRIF_ReadActTcpVel(unsigned int, unsigned int, + double& x, double& y, double& z, + double& rx, double& ry, double& rz) +{ + std::lock_guard lock(g_sdk.mutex); + const double velocity = g_sdk.movingLocked() ? 5.0 : 0.0; + x = y = z = rx = ry = rz = velocity; + return 0; +} + +int HRIF_MoveJ(unsigned int, unsigned int, + double, double, double, double, double, double, + double j1, double j2, double j3, + double j4, double j5, double j6, + std::string, std::string, double, double, double, + int, int, int, int, std::string command_id) +{ + std::lock_guard lock(g_sdk.mutex); + ++g_sdk.move_j_calls; + g_sdk.joint_target_deg = {j1, j2, j3, j4, j5, j6}; + g_sdk.waypoint_id = std::move(command_id); + g_sdk.startMotionLocked(true); + return 0; +} + +int HRIF_MoveL(unsigned int, unsigned int, + double x, double y, double z, + double rx, double ry, double rz, + double, double, double, double, double, double, + std::string, std::string, double, double, double, + int, int, int, std::string command_id) +{ + std::lock_guard lock(g_sdk.mutex); + ++g_sdk.move_l_calls; + g_sdk.tcp_target_hr = {x, y, z, rx, ry, rz}; + g_sdk.waypoint_id = std::move(command_id); + g_sdk.startMotionLocked(false); + return 0; +} + +int HRIF_SpeedJ(unsigned int, unsigned int, + double, double, double, double, double, double, + double, double) +{ + std::lock_guard lock(g_sdk.mutex); + ++g_sdk.speed_j_calls; + g_sdk.startMotionLocked(true); + return 0; +} + +int HRIF_SpeedL(unsigned int, unsigned int, + double, double, double, double, double, double, + double, double, double) +{ + std::lock_guard lock(g_sdk.mutex); + ++g_sdk.speed_l_calls; + g_sdk.startMotionLocked(false); + return 0; +} + +int HRIF_StartServo(unsigned int, unsigned int, double, double) +{ + std::lock_guard lock(g_sdk.mutex); + ++g_sdk.start_servo_calls; + g_sdk.servo_started = true; + return 0; +} + +int HRIF_PushServoJ(unsigned int, unsigned int, + double j1, double j2, double j3, + double j4, double j5, double j6) +{ + std::lock_guard lock(g_sdk.mutex); + if (!g_sdk.servo_started) { + return 301; + } + g_sdk.joint_position_deg = {j1, j2, j3, j4, j5, j6}; + return 0; +} + +int HRIF_PushServoP(unsigned int, unsigned int, + std::vector& coord, + std::vector&, + std::vector&) +{ + std::lock_guard lock(g_sdk.mutex); + if (!g_sdk.servo_started || coord.size() < 6) { + return 302; + } + std::copy_n(coord.begin(), 6, g_sdk.tcp_position_hr.begin()); + return 0; +} + +int HRIF_SwitchScript(unsigned int, unsigned int, std::string script_name) +{ + std::lock_guard lock(g_sdk.mutex); + if (script_name.empty()) { + return 401; + } + g_sdk.selected_program = std::move(script_name); + return 0; +} + +int HRIF_StartScript(unsigned int) +{ + std::lock_guard lock(g_sdk.mutex); + if (g_sdk.selected_program.empty()) { + return 402; + } + g_sdk.program_running = true; + g_sdk.paused = false; + return 0; +} + +int HRIF_PauseScript(unsigned int) +{ + std::lock_guard lock(g_sdk.mutex); + if (!g_sdk.program_running) { + return 403; + } + g_sdk.paused = true; + return 0; +} + +int HRIF_StopScript(unsigned int) +{ + std::lock_guard lock(g_sdk.mutex); + ++g_sdk.stop_script_calls; + g_sdk.program_running = false; + g_sdk.paused = false; + return 0; +} + +int HRIF_EnterSafetyGuard(unsigned int, unsigned int, int flag) +{ + std::lock_guard lock(g_sdk.mutex); + g_sdk.software_safeguard = flag != 0; + return 0; +} + +int HRIF_ShutdownRobot(unsigned int) +{ + std::lock_guard lock(g_sdk.mutex); + g_sdk.connected = false; + g_sdk.enabled = false; + g_sdk.electrified = false; + return 0; +} + +} // extern "C" + +int main() +{ + using namespace cmvr::device; + + resetFakeSdk(); + HuayanRobot arm(makeConfig()); + CHECK_TRUE(arm.connect("127.0.0.1", 10003).ok()); + + MotionOptions options; + options.velocity = 0.4; + options.acceleration = 0.8; + + // The first IsMotionDone read intentionally reports the preceding idle + // state. Completion must be correlated with the command/target. Once the + // target is reached, an identical command is an idempotent no-op. + JointPositionCommand joint_a{{0.10, -0.05, 0.08, 0.0, 0.02, -0.03}}; + CHECK_TRUE(arm.moveJ(joint_a, options).ok()); + int move_j_after_first = 0; + { + std::lock_guard lock(g_sdk.mutex); + move_j_after_first = g_sdk.move_j_calls; + } + CHECK_TRUE(arm.moveJ(joint_a, options).ok()); + { + std::lock_guard lock(g_sdk.mutex); + CHECK_TRUE(g_sdk.move_j_calls == move_j_after_first); + } + + CartesianPose pose_a; + pose_a.x = 0.31; + pose_a.y = -0.12; + pose_a.z = 0.42; + pose_a.rx = 0.08; + pose_a.ry = -0.04; + pose_a.rz = 0.12; + CHECK_TRUE(arm.moveL(pose_a, options).ok()); + int move_l_after_first = 0; + { + std::lock_guard lock(g_sdk.mutex); + move_l_after_first = g_sdk.move_l_calls; + } + CHECK_TRUE(arm.moveL(pose_a, options).ok()); + { + std::lock_guard lock(g_sdk.mutex); + CHECK_TRUE(g_sdk.move_l_calls == move_l_after_first); + } + + // Stop must cancel the old owner and wait until the controller reports + // stable idle; clearing the owner immediately after GrpStop would fail the + // elapsed-time and consecutive-idle checks below. + JointPositionCommand joint_b{{0.22, -0.08, 0.14, 0.03, 0.04, -0.01}}; + holdNextMotion(); + const int before_held_move = move_j_after_first; + auto held_move = std::async(std::launch::async, [&]() { + return arm.moveJ(joint_b, options); + }); + CHECK_TRUE(waitUntil([&]() { + std::lock_guard lock(g_sdk.mutex); + return g_sdk.move_j_calls > before_held_move; + })); + const auto stop_started = Clock::now(); + CHECK_TRUE(arm.stopMotion().ok()); + const auto stop_elapsed = Clock::now() - stop_started; + CHECK_TRUE(stop_elapsed >= 100ms); + CHECK_TRUE(held_move.wait_for(1s) == std::future_status::ready); + if (held_move.wait_for(0ms) == std::future_status::ready) { + CHECK_TRUE(!held_move.get().ok()); + } + CHECK_TRUE(!arm.busy()); + { + std::lock_guard lock(g_sdk.mutex); + CHECK_TRUE(g_sdk.idle_velocity_reads_after_stop >= 3); + } + CHECK_TRUE(arm.moveJ(joint_b, options).ok()); + + // A hardware E-stop cancels and terminates the active waypoint. Releasing + // the switch does not clear the software latch or grant a new permit. + JointPositionCommand joint_c{{0.34, -0.02, 0.09, 0.05, -0.02, 0.07}}; + holdNextMotion(); + int before_estop_move = 0; + int before_estop_stop = 0; + { + std::lock_guard lock(g_sdk.mutex); + before_estop_move = g_sdk.move_j_calls; + before_estop_stop = g_sdk.group_stop_calls; + } + auto estop_move = std::async(std::launch::async, [&]() { + return arm.moveJ(joint_c, options); + }); + CHECK_TRUE(waitUntil([&]() { + std::lock_guard lock(g_sdk.mutex); + return g_sdk.move_j_calls > before_estop_move; + })); + setHardwareEmergencyStop(true); + CHECK_TRUE(waitUntil([&]() { + std::lock_guard lock(g_sdk.mutex); + return g_sdk.group_stop_calls > before_estop_stop; + })); + CHECK_TRUE(estop_move.wait_for(2s) == std::future_status::ready); + if (estop_move.wait_for(0ms) == std::future_status::ready) { + CHECK_TRUE(!estop_move.get().ok()); + } + setHardwareEmergencyStop(false); + std::this_thread::sleep_for(150ms); + + int move_count_while_latched = 0; + { + std::lock_guard lock(g_sdk.mutex); + move_count_while_latched = g_sdk.move_j_calls; + } + const auto rejected_while_latched = arm.moveJ(joint_a, options); + CHECK_TRUE(!rejected_while_latched.ok()); + { + std::lock_guard lock(g_sdk.mutex); + CHECK_TRUE(g_sdk.move_j_calls == move_count_while_latched); + } + CHECK_TRUE(arm.clearFault().ok()); + CHECK_TRUE(arm.torqueOn().ok()); + CHECK_TRUE(arm.moveJ(joint_a, options).ok()); + + // Speed commands own the controller while waiting. A different motion is + // rejected, and Stop releases ownership only after termination. + holdNextMotion(); + JointVelocityCommand speed{{0.1, 0.0, 0.0, 0.0, 0.0, 0.0}}; + int speed_calls_before = 0; + { + std::lock_guard lock(g_sdk.mutex); + speed_calls_before = g_sdk.speed_j_calls; + } + auto speed_motion = std::async(std::launch::async, [&]() { + return arm.speedJ(speed, 0.5, 2.0); + }); + CHECK_TRUE(waitUntil([&]() { + std::lock_guard lock(g_sdk.mutex); + return g_sdk.speed_j_calls > speed_calls_before; + })); + CHECK_TRUE(!arm.moveL(pose_a, options).ok()); + CHECK_TRUE(arm.stopMotion().ok()); + CHECK_TRUE(speed_motion.wait_for(1s) == std::future_status::ready); + if (speed_motion.wait_for(0ms) == std::future_status::ready) { + CHECK_TRUE(!speed_motion.get().ok()); + } + CHECK_TRUE(!arm.busy()); + + // SpeedL used to hold the SDK mutex while waiting, which deadlocked its + // own timeout/Stop path. A concurrent Stop must cancel it, settle the + // controller, and allow a genuinely new Move command afterwards. + holdNextMotion(); + CartesianVelocity line_speed; + line_speed.vx = 0.05; + int speed_l_calls_before = 0; + { + std::lock_guard lock(g_sdk.mutex); + speed_l_calls_before = g_sdk.speed_l_calls; + } + auto line_speed_motion = std::async(std::launch::async, [&]() { + return arm.speedL(line_speed, 0.5, 2.0, FrameType::Base); + }); + CHECK_TRUE(waitUntil([&]() { + std::lock_guard lock(g_sdk.mutex); + return g_sdk.speed_l_calls > speed_l_calls_before; + })); + CHECK_TRUE(arm.stopMotion().ok()); + CHECK_TRUE(line_speed_motion.wait_for(1s) == std::future_status::ready); + if (line_speed_motion.wait_for(0ms) == std::future_status::ready) { + CHECK_TRUE(!line_speed_motion.get().ok()); + } + CHECK_TRUE(!arm.busy()); + CHECK_TRUE(arm.moveJ(joint_b, options).ok()); + + // A dual-channel emergency input mismatch is a typed emergency latch. It + // remains blocked after the wiring level is healthy and is recovered only + // through the emergency recovery path. + int stops_before_signal_fault = 0; + { + std::lock_guard lock(g_sdk.mutex); + stops_before_signal_fault = g_sdk.group_stop_calls; + } + setEmergencySignalFault(true); + CHECK_TRUE(waitUntil([&]() { + std::lock_guard lock(g_sdk.mutex); + return g_sdk.group_stop_calls > stops_before_signal_fault; + })); + setEmergencySignalFault(false); + std::this_thread::sleep_for(100ms); + CHECK_TRUE(!arm.moveJ(joint_c, options).ok()); + CHECK_TRUE(arm.torqueOn().ok()); + CHECK_TRUE(arm.moveJ(joint_c, options).ok()); + + // Power-off holds a terminal barrier through GrpDisable. Motion remains + // denied until an explicit enable confirms the powered state again. + CHECK_TRUE(arm.torqueOff().ok()); + CHECK_TRUE(!arm.moveJ(joint_a, options).ok()); + CHECK_TRUE(arm.torqueOn().ok()); + CHECK_TRUE(arm.moveJ(joint_a, options).ok()); + + // Servo and program modes retain ownership beyond the start call. Stop of + // a retained program must use StopScript as well as the group stop path. + ServoOptions servo_options; + CHECK_TRUE(arm.startServoMode(servo_options).ok()); + CHECK_TRUE(!arm.moveJ(joint_b, options).ok()); + CHECK_TRUE(arm.servoJ(joint_b).ok()); + CHECK_TRUE(arm.stopServoMode().ok()); + CHECK_TRUE(!arm.busy()); + + CHECK_TRUE(arm.loadProgram("fake_program.script").ok()); + CHECK_TRUE(arm.playProgram().ok()); + CHECK_TRUE(!arm.moveJ(joint_c, options).ok()); + int stop_script_calls_before = 0; + { + std::lock_guard lock(g_sdk.mutex); + stop_script_calls_before = g_sdk.stop_script_calls; + } + CHECK_TRUE(arm.stopMotion().ok()); + { + std::lock_guard lock(g_sdk.mutex); + CHECK_TRUE(g_sdk.stop_script_calls > stop_script_calls_before); + } + CHECK_TRUE(!arm.busy()); + + // Retire an in-flight stale generation after transport loss before + // reconnecting; no old waiter may issue SDK reads into the new session. + holdNextMotion(); + int moves_before_transport_loss = 0; + { + std::lock_guard lock(g_sdk.mutex); + moves_before_transport_loss = g_sdk.move_j_calls; + } + auto transport_lost_move = std::async(std::launch::async, [&]() { + return arm.moveJ(joint_c, options); + }); + CHECK_TRUE(waitUntil([&]() { + std::lock_guard lock(g_sdk.mutex); + return g_sdk.move_j_calls > moves_before_transport_loss; + })); + dropFakeTransport(); + CHECK_TRUE(arm.connect("127.0.0.1", 10003).ok()); + CHECK_TRUE(transport_lost_move.wait_for(1s) == std::future_status::ready); + if (transport_lost_move.wait_for(0ms) == std::future_status::ready) { + CHECK_TRUE(!transport_lost_move.get().ok()); + } + CHECK_TRUE(arm.moveJ(joint_b, options).ok()); + CHECK_TRUE(arm.disconnect().ok()); + + resetFakeSdk(); + HuayanRobot shutdown_arm(makeConfig()); + CHECK_TRUE(shutdown_arm.connect("127.0.0.1", 10003).ok()); + CHECK_TRUE(shutdown_arm.shutdown().ok()); + CHECK_TRUE(!shutdown_arm.isConnected()); + return failures == 0 ? 0 : 1; +} diff --git a/cmvr-es/devices/arm/huayan_arm/tests/huayan_lifecycle_state_test.cpp b/cmvr-es/devices/arm/huayan_arm/tests/huayan_lifecycle_state_test.cpp new file mode 100644 index 00000000..8bb93ff7 --- /dev/null +++ b/cmvr-es/devices/arm/huayan_arm/tests/huayan_lifecycle_state_test.cpp @@ -0,0 +1,232 @@ +#include "devices/arm/huayan_arm/huayan_lifecycle_state.h" + +#include +#include + +namespace { + +#define CHECK_TRUE(condition) \ + do { \ + if (!(condition)) { \ + std::cerr << "CHECK_TRUE failed at line " << __LINE__ << ": " \ + << #condition << std::endl; \ + return 1; \ + } \ + } while (false) + +} // namespace + +int main() +{ + using namespace cmvr::device::huayan_internal; + + MotionState motion; + CHECK_TRUE(motion.begin(MotionKind::None).status == + MotionStartStatus::Invalid); + + // A completed target does not poison an identical subsequent command, + // while an actually concurrent command is rejected. + const auto first_joint = motion.begin(MotionKind::Joint); + CHECK_TRUE(first_joint.started()); + CHECK_TRUE(motion.begin(MotionKind::Joint).status == + MotionStartStatus::Busy); + CHECK_TRUE(motion.begin(MotionKind::Linear).status == + MotionStartStatus::Busy); + motion.finish(first_joint.token); + const auto repeated_joint = motion.begin(MotionKind::Joint); + CHECK_TRUE(repeated_joint.started()); + CHECK_TRUE(repeated_joint.token.generation > + first_joint.token.generation); + motion.finish(first_joint.token); + CHECK_TRUE(motion.ownerActive(repeated_joint.token)); + motion.finish(repeated_joint.token); + CHECK_TRUE(!motion.busy()); + + // Stop cancels the current generation and cannot complete before its + // owner exits. + const auto linear = motion.begin(MotionKind::Linear); + CHECK_TRUE(linear.started()); + const auto stop_linear = motion.beginStop(); + CHECK_TRUE(stop_linear.started()); + CHECK_TRUE(stop_linear.kind == MotionKind::Linear); + CHECK_TRUE(stop_linear.active_token.generation == + linear.token.generation); + CHECK_TRUE(stop_linear.tracked_motion); + CHECK_TRUE(motion.cancelled(linear.token)); + CHECK_TRUE(motion.beginStop().status == + StopStartStatus::AlreadyStopping); + CHECK_TRUE(motion.begin(MotionKind::Joint).status == + MotionStartStatus::Stopping); + CHECK_TRUE(!motion.waitForOwnerExit( + linear.token, std::chrono::milliseconds(1))); + CHECK_TRUE(!motion.completeStop()); + motion.finish(linear.token); + CHECK_TRUE(motion.waitForOwnerExit( + linear.token, std::chrono::milliseconds(1))); + CHECK_TRUE(motion.completeStop()); + CHECK_TRUE(!motion.busy()); + + // An uncertain submission/completion remains fail-closed until a + // positively acknowledged Stop clears it. + const auto failed_speed = motion.begin(MotionKind::SpeedLinear); + CHECK_TRUE(failed_speed.started()); + motion.failMotion(failed_speed.token); + CHECK_TRUE(motion.snapshot().blocked); + CHECK_TRUE(motion.begin(MotionKind::Joint).status == + MotionStartStatus::Blocked); + const auto stop_failed_speed = motion.beginStop(); + CHECK_TRUE(stop_failed_speed.kind == MotionKind::SpeedLinear); + CHECK_TRUE(stop_failed_speed.tracked_motion); + CHECK_TRUE(motion.completeStop()); + + const auto failed_stop_motion = motion.begin(MotionKind::Joint); + CHECK_TRUE(failed_stop_motion.started()); + const auto failed_stop = motion.beginStop(); + CHECK_TRUE(failed_stop.kind == MotionKind::Joint); + motion.failStop(); + CHECK_TRUE(motion.snapshot().blocked); + CHECK_TRUE(motion.begin(MotionKind::Linear).status == + MotionStartStatus::Blocked); + motion.finish(failed_stop_motion.token); + const auto retry_failed_stop = motion.beginStop(); + CHECK_TRUE(retry_failed_stop.kind == MotionKind::Joint); + CHECK_TRUE(motion.completeStop()); + + // Servo and program modes remain owned after their start RPC returns. + const auto servo = motion.begin(MotionKind::Servo); + CHECK_TRUE(servo.started()); + motion.finish(servo.token, MotionFinishMode::Retain); + CHECK_TRUE(motion.snapshot().retained_kind == MotionKind::Servo); + CHECK_TRUE(motion.begin(MotionKind::Program).status == + MotionStartStatus::Busy); + const auto servo_update = motion.begin(MotionKind::Servo, true); + CHECK_TRUE(servo_update.started()); + motion.finish(servo_update.token); + CHECK_TRUE(motion.snapshot().retained_kind == MotionKind::Servo); + const auto stop_servo = motion.beginStop(); + CHECK_TRUE(stop_servo.kind == MotionKind::Servo); + CHECK_TRUE(stop_servo.tracked_motion); + CHECK_TRUE(motion.completeStop()); + + const auto program = motion.begin(MotionKind::Program); + CHECK_TRUE(program.started()); + motion.finish(program.token, MotionFinishMode::Retain); + const auto cancelled_program = motion.cancelActiveForSafety(); + CHECK_TRUE(cancelled_program.kind == MotionKind::Program); + CHECK_TRUE(cancelled_program.tracked_motion); + CHECK_TRUE(!cancelled_program.active_token.valid()); + CHECK_TRUE(motion.begin(MotionKind::Joint).status == + MotionStartStatus::Blocked); + const auto stop_program = motion.beginStop(); + CHECK_TRUE(stop_program.kind == MotionKind::Program); + CHECK_TRUE(motion.completeStop()); + + const auto safety_move = motion.begin(MotionKind::SpeedJoint); + CHECK_TRUE(safety_move.started()); + const auto cancelled_move = motion.cancelActiveForSafety(); + CHECK_TRUE(cancelled_move.kind == MotionKind::SpeedJoint); + CHECK_TRUE(cancelled_move.active_token.generation == + safety_move.token.generation); + CHECK_TRUE(motion.cancelled(safety_move.token)); + motion.finish(safety_move.token); + const auto stop_safety_move = motion.beginStop(); + CHECK_TRUE(stop_safety_move.kind == MotionKind::SpeedJoint); + CHECK_TRUE(motion.completeStop()); + + RawSafetyState raw; + CHECK_TRUE(classifySafetyCondition(raw) == SafetyCondition::Unknown); + raw.valid = true; + CHECK_TRUE(classifySafetyCondition(raw) == SafetyCondition::Normal); + raw.software_protective_stop = true; + CHECK_TRUE(classifySafetyCondition(raw) == + SafetyCondition::SoftwareProtectiveStop); + raw.software_emergency_stop = true; + CHECK_TRUE(classifySafetyCondition(raw) == + SafetyCondition::SoftwareEmergencyStop); + raw.robot_fault = 1; + CHECK_TRUE(classifySafetyCondition(raw) == + SafetyCondition::RobotFault); + raw.safeguard_stop = 1; + CHECK_TRUE(classifySafetyCondition(raw) == + SafetyCondition::SafeguardStop); + raw.emergency_stop = 1; + CHECK_TRUE(classifySafetyCondition(raw) == + SafetyCondition::EmergencyStop); + raw.safeguard_signal_fault = 1; + CHECK_TRUE(classifySafetyCondition(raw) == + SafetyCondition::SafeguardSignalFault); + raw.emergency_signal_fault = 1; + CHECK_TRUE(classifySafetyCondition(raw) == + SafetyCondition::EmergencySignalFault); + + SafetyState safety; + CHECK_TRUE(!safety.tryPermit().has_value()); + safety.observe(SafetyCondition::Normal); + const auto initial_permit = safety.tryPermit(); + CHECK_TRUE(initial_permit.has_value()); + CHECK_TRUE(safety.validate(*initial_permit)); + + safety.observe(SafetyCondition::EmergencyStop); + CHECK_TRUE(safety.snapshot().latched); + CHECK_TRUE(!safety.validate(*initial_permit)); + CHECK_TRUE(!safety.beginRecovery(safety.snapshot().epoch).has_value()); + const auto first_emergency_epoch = safety.snapshot().epoch; + safety.observe(SafetyCondition::EmergencyStop); + safety.observe(SafetyCondition::EmergencyStop); + CHECK_TRUE(safety.snapshot().epoch == first_emergency_epoch); + + // Releasing the hardware switch only changes the observed level; it does + // not clear the event latch or issue a new motion permit. + safety.observe(SafetyCondition::Normal); + CHECK_TRUE(safety.snapshot().latched); + CHECK_TRUE(!safety.tryPermit().has_value()); + + const auto not_ready = safety.beginRecovery(safety.snapshot().epoch); + CHECK_TRUE(not_ready.has_value()); + CHECK_TRUE(!safety.completeRecovery(*not_ready, false, true, true)); + CHECK_TRUE(safety.snapshot().latched); + safety.failRecovery(*not_ready); + + const auto not_idle = safety.beginRecovery(safety.snapshot().epoch); + CHECK_TRUE(not_idle.has_value()); + CHECK_TRUE(!safety.completeRecovery(*not_idle, true, false, true)); + CHECK_TRUE(safety.snapshot().latched); + safety.failRecovery(*not_idle); + + const auto not_cancelled = + safety.beginRecovery(safety.snapshot().epoch); + CHECK_TRUE(not_cancelled.has_value()); + CHECK_TRUE(!safety.completeRecovery(*not_cancelled, true, true, false)); + CHECK_TRUE(safety.snapshot().latched); + safety.failRecovery(*not_cancelled); + + const auto recovery_retry = + safety.beginRecovery(safety.snapshot().epoch); + CHECK_TRUE(recovery_retry.has_value()); + CHECK_TRUE(safety.completeRecovery( + *recovery_retry, true, true, true)); + const auto recovered_permit = safety.tryPermit(); + CHECK_TRUE(recovered_permit.has_value()); + CHECK_TRUE(safety.validate(*recovered_permit)); + + // A second safety event, including the same physical E-stop being pressed + // again, invalidates an older recovery token atomically. + safety.observe(SafetyCondition::EmergencyStop); + safety.observe(SafetyCondition::Normal); + const auto stale_recovery = + safety.beginRecovery(safety.snapshot().epoch); + CHECK_TRUE(stale_recovery.has_value()); + safety.observe(SafetyCondition::EmergencyStop); + safety.observe(SafetyCondition::Normal); + CHECK_TRUE(!safety.completeRecovery( + *stale_recovery, true, true, true)); + CHECK_TRUE(safety.snapshot().latched); + CHECK_TRUE(!safety.snapshot().recovery_in_progress); + + const auto stale_epoch = safety.snapshot().epoch; + safety.observe(SafetyCondition::SafeguardStop); + safety.observe(SafetyCondition::Normal); + CHECK_TRUE(!safety.beginRecovery(stale_epoch).has_value()); + + return 0; +} From 912d8689f7f7564d75a57da86093e57069f9d9ba Mon Sep 17 00:00:00 2001 From: xtkuang <87661715@qq.com> Date: Wed, 12 Aug 2026 11:15:46 +0800 Subject: [PATCH 20/20] feat(system): add serial arm and AGV action queue --- cmvr-es/common/types/arm/arm_types.h | 6 + cmvr-es/devices/agv/abstract_agv.h | 29 + .../seer_robokit/include/seer_robokit_agv.h | 15 +- .../src/seer_robokit_navigation_wait.cpp | 83 +- .../seer_robokit_control_authority_test.cpp | 138 ++ cmvr-es/devices/arm/aubo_arm/aubo_arm.cpp | 70 +- cmvr-es/devices/arm/aubo_arm/aubo_arm.h | 1 + cmvr-es/devices/arm/huayan_arm/huayan_arm.cpp | 63 +- cmvr-es/devices/arm/huayan_arm/huayan_arm.h | 5 +- .../huayan_arm/tests/huayan_arm_sdk_test.cpp | 42 +- cmvr-es/devices/arm/robot_arm.h | 6 + .../include/control_authority_manager.h | 16 + .../src/control_authority_manager.cpp | 126 +- .../tests/control_authority_manager_test.cpp | 133 + cmvr-es/service/CMakeLists.txt | 1 + cmvr-es/service/README.md | 51 + .../action/include/action_queue_executor.h | 65 + .../action/src/action_queue_executor.cpp | 2204 +++++++++++++++++ .../grpc/include/grpc_system_service.h | 12 +- cmvr-es/service/grpc/src/grpc_agv_service.cpp | 251 +- cmvr-es/service/grpc/src/grpc_arm_service.cpp | 13 +- .../service/grpc/src/grpc_system_service.cpp | 223 +- .../grpc/tests/grpc_agv_service_test.cpp | 234 ++ .../grpc/tests/grpc_arm_service_test.cpp | 71 + .../grpc/tests/grpc_system_service_test.cpp | 1437 +++++++++++ .../grpc_server_task/src/grpc_server_task.cpp | 5 + protos/cmvr/api/system_command.proto | 100 + protos/cmvr/api/system_service.proto | 2 + 28 files changed, 5323 insertions(+), 79 deletions(-) create mode 100644 cmvr-es/service/action/include/action_queue_executor.h create mode 100644 cmvr-es/service/action/src/action_queue_executor.cpp diff --git a/cmvr-es/common/types/arm/arm_types.h b/cmvr-es/common/types/arm/arm_types.h index 9d8c819b..60e602f3 100644 --- a/cmvr-es/common/types/arm/arm_types.h +++ b/cmvr-es/common/types/arm/arm_types.h @@ -2,6 +2,7 @@ #define CMVR_ES_ARM_TYPES_H #include +#include #include #include @@ -162,6 +163,11 @@ struct MotionOptions { double jerk{5.0}; std::vector joint_velocity_limits; bool asynchronous{false}; + // Framework-independent cancellation check used by queued synchronous + // motion. Cancellation after device acceptance retains a typed motion + // barrier; the owner must call stopMotion() to confirm physical idle. + // Drivers must not retain this callback after moveJ/moveL returns. + std::function cancellation_requested; }; struct ServoOptions { diff --git a/cmvr-es/devices/agv/abstract_agv.h b/cmvr-es/devices/agv/abstract_agv.h index 40ec6cb9..8f436a21 100644 --- a/cmvr-es/devices/agv/abstract_agv.h +++ b/cmvr-es/devices/agv/abstract_agv.h @@ -15,6 +15,12 @@ namespace cmvr::device { +enum class AgvActionKind { + NavigateToPose, + NavigateToStation, + FollowPath, +}; + /** * @brief AGV/移动底盘设备抽象基类。 * @@ -28,6 +34,16 @@ public: DeviceKind kind() const noexcept override { return DeviceKind::AGV; } + /** + * @brief Whether this backend provides terminal-state and stopped-motion + * confirmation plus bounded cancellation suitable for synchronous + * Action execution. + */ + virtual bool supportsSynchronousAction(AgvActionKind) const noexcept + { + return false; + } + /** * @brief 获取 AGV 运行状态快照。 */ @@ -167,6 +183,19 @@ public: return setVelocity(AgvVelocity{}); } + /** + * @brief 确认 AGV 已进入可安全释放控制权的停止状态。 + * + * 该接口只在导航任务已终止且底盘速度经过连续采样确认为零后返回 + * 成功；仅收到取消、停止或零速度命令的应答不构成成功。 + */ + virtual AgvResult confirmMotionStopped() + { + return AgvResult::failure( + AgvErrorCode::UnsupportedCommand, + "confirmMotionStopped not implemented"); + } + /** * @brief 查询 AGV 可用地图名称列表。 */ diff --git a/cmvr-es/devices/agv/seer_robokit/include/seer_robokit_agv.h b/cmvr-es/devices/agv/seer_robokit/include/seer_robokit_agv.h index 5a8ec3a1..f06510f2 100644 --- a/cmvr-es/devices/agv/seer_robokit/include/seer_robokit_agv.h +++ b/cmvr-es/devices/agv/seer_robokit/include/seer_robokit_agv.h @@ -27,6 +27,17 @@ public: std::string typeName() const override { return "SeerRobokitAgv"; } + bool supportsSynchronousAction(AgvActionKind kind) const noexcept override + { + switch (kind) { + case AgvActionKind::NavigateToPose: + case AgvActionKind::NavigateToStation: + case AgvActionKind::FollowPath: + return true; + } + return false; + } + bool init() override; bool start() override; bool stop() override; @@ -57,6 +68,7 @@ public: AgvResult cancelNavigation() override; AgvResult setVelocity(const AgvVelocity& velocity) override; + AgvResult confirmMotionStopped() override; AgvResult listMaps(std::vector& maps) const override; AgvResult listStations(std::vector& stations) const override; @@ -166,7 +178,8 @@ private: AgvResult waitForCanceledTaskToStop_( const TrackedNavigationContext& context, const AgvMotionOptions& options, - const std::string& reason); + const std::string& reason, + bool require_global_stopped = false); AgvResult failAndCancelTrackedNavigation_( const TrackedNavigationContext& context, const AgvMotionOptions& options, 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 index fc9cf13f..62aaf35c 100644 --- 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 @@ -473,10 +473,71 @@ AgvResult SeerRobokitAgv::cancelTrackedNavigation_( return result; } +AgvResult SeerRobokitAgv::confirmMotionStopped() +{ + TrackedNavigationContext tracked_navigation; + if (currentTrackedNavigation_(tracked_navigation)) { + return waitForCanceledTaskToStop_( + tracked_navigation, + AgvMotionOptions{}, + "explicit motion-stop confirmation", + true); + } + + const auto confirmation_window = + kNavigationCancelConfirmationTimeout; + const auto deadline = + std::chrono::steady_clock::now() + confirmation_window; + int stopped_samples = 0; + std::string last_detail = "no 1101 status received"; + while (std::chrono::steady_clock::now() < deadline) { + NavigationSnapshot snapshot; + const auto snapshot_result = queryNavigationSnapshot_(snapshot); + if (!snapshot_result.ok()) { + return AgvResult::failure( + snapshot_result.code, + "SEER Robokit stopped state could not be confirmed with 1101: " + + snapshot_result.message); + } + + last_detail = snapshot.detail; + const bool no_active_navigation = + snapshot.task_status_present && + globalTaskStateIsKnownTerminal(snapshot.task_status); + if (no_active_navigation && navigationStopped(snapshot)) { + ++stopped_samples; + if (stopped_samples >= kRequiredCompletedStopSamples) { + return { + AgvErrorCode::OK, + "no active navigation and two zero-velocity samples were " + "confirmed with 1101"}; + } + } 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 did not confirm an inactive navigation task and two " + "zero-velocity samples within " + + std::to_string(confirmation_window.count()) + + " ms; last_status=" + last_detail); +} + AgvResult SeerRobokitAgv::waitForCanceledTaskToStop_( const TrackedNavigationContext& context, const AgvMotionOptions& options, - const std::string& reason) + const std::string& reason, + const bool require_global_stopped) { if (context.task_ids.empty()) { return AgvResult::failure( @@ -525,7 +586,8 @@ AgvResult SeerRobokitAgv::waitForCanceledTaskToStop_( const bool another_local_navigation_started = currentTrackedNavigation_(active_context) && active_context.token != context.token; - if (all_exact_tasks_terminal && another_local_navigation_started) { + if (all_exact_tasks_terminal && another_local_navigation_started && + !require_global_stopped) { // 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. @@ -567,14 +629,17 @@ AgvResult SeerRobokitAgv::waitForCanceledTaskToStop_( 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); + const bool global_terminal = snapshot.task_status_present && + (snapshot.task_status == 0 + || (exactTaskStateIsKnownTerminal(snapshot.task_status) + && snapshot.task_type == expected_global_type + && global_target_matches)); + const bool task_termination_confirmed = require_global_stopped + ? (!any_exact_task_active && global_terminal) + : (all_exact_tasks_terminal + || (!any_exact_task_active && global_terminal)); - if ((all_exact_tasks_terminal - || (!any_exact_task_active && global_terminal)) - && navigationStopped(snapshot)) { + if (task_termination_confirmed && navigationStopped(snapshot)) { ++stopped_samples; if (stopped_samples >= kRequiredCompletedStopSamples) { for (const auto& task_id : context.task_ids) { 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 index 905b9105..b1109f12 100644 --- 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 @@ -605,6 +605,144 @@ protected: std::unique_ptr agv_; }; +TEST_F(SeerRobokitControlAuthorityTest, DeclaresSynchronousActionSupport) +{ + EXPECT_TRUE(agv_->supportsSynchronousAction( + AgvActionKind::NavigateToPose)); + EXPECT_TRUE(agv_->supportsSynchronousAction( + AgvActionKind::NavigateToStation)); + EXPECT_TRUE(agv_->supportsSynchronousAction( + AgvActionKind::FollowPath)); +} + +TEST_F( + SeerRobokitControlAuthorityTest, + ConfirmMotionStoppedWithoutTrackedTaskUsesGlobalStatusOnly) +{ + controller_.clearRecords(); + + const auto result = agv_->confirmMotionStopped(); + + 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 == kRobotStatusAll2; + }), + 2); + EXPECT_EQ( + std::count_if( + records.begin(), + records.end(), + [](const CommandRecord& record) { + return record.command == kRobotStatusTaskPackage; + }), + 0); +} + +TEST_F( + SeerRobokitControlAuthorityTest, + ConfirmMotionStoppedForTrackedStationChecksExactTaskAndGlobalVelocity) +{ + const auto navigate_result = agv_->navigateToStation( + "station-confirm-stop", + asynchronousMotionOptions()); + ASSERT_TRUE(navigate_result.ok()) << navigate_result.message; + 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-confirm-stop","blocked":false,"vx":0.0,"vy":0.0,"w":0.0,"fatals":[],"errors":[]})"); + controller_.clearRecords(); + + const auto result = agv_->confirmMotionStopped(); + + 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; + }), + 2); + EXPECT_GE( + std::count_if( + records.begin(), + records.end(), + [](const CommandRecord& record) { + return record.command == kRobotStatusAll2; + }), + 2); +} + +TEST_F( + SeerRobokitControlAuthorityTest, + ConfirmMotionStoppedTimesOutWhenTrackedTaskIsTerminalButVelocityIsNonzero) +{ + const auto navigate_result = agv_->navigateToStation( + "station-still-moving", + asynchronousMotionOptions()); + ASSERT_TRUE(navigate_result.ok()) << navigate_result.message; + 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-still-moving","blocked":false,"vx":0.1,"vy":0.0,"w":0.0,"fatals":[],"errors":[]})"); + controller_.clearRecords(); + + const auto result = agv_->confirmMotionStopped(); + + EXPECT_EQ(result.code, AgvErrorCode::Timeout); + EXPECT_NE( + result.message.find("stopped velocity were not confirmed"), + std::string::npos); +} + +TEST_F( + SeerRobokitControlAuthorityTest, + ConfirmMotionStoppedWaitsForGlobalTaskToBecomeTerminal) +{ + const auto navigate_result = agv_->navigateToStation( + "station-global-active", + asynchronousMotionOptions()); + ASSERT_TRUE(navigate_result.ok()) << navigate_result.message; + 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":2,"task_type":2,"target_id":"station-global-active","blocked":false,"vx":0.0,"vy":0.0,"w":0.0,"fatals":[],"errors":[]})"); + controller_.clearRecords(); + std::atomic finished{false}; + AgvResult result; + + std::thread confirmation([this, &finished, &result]() { + result = agv_->confirmMotionStopped(); + finished.store(true, std::memory_order_release); + }); + const bool zero_samples_observed = waitForCommandCount( + kRobotStatusAll2, 2, 1000); + std::this_thread::sleep_for(std::chrono::milliseconds(50)); + const bool returned_while_global_active = + finished.load(std::memory_order_acquire); + + controller_.setResponsePayload( + kRobotStatusAll2, + R"({"ret_code":0,"task_status":4,"task_type":2,"target_id":"station-global-active","blocked":false,"vx":0.0,"vy":0.0,"w":0.0,"fatals":[],"errors":[]})"); + confirmation.join(); + + EXPECT_TRUE(zero_samples_observed); + EXPECT_FALSE(returned_while_global_active); + EXPECT_TRUE(result.ok()) << result.message; +} + TEST_F(SeerRobokitControlAuthorityTest, EveryImplementedMutatingOperationAcquiresAuthorityFirst) { controller_.clearRecords(); diff --git a/cmvr-es/devices/arm/aubo_arm/aubo_arm.cpp b/cmvr-es/devices/arm/aubo_arm/aubo_arm.cpp index ed446d4f..2842c9af 100644 --- a/cmvr-es/devices/arm/aubo_arm/aubo_arm.cpp +++ b/cmvr-es/devices/arm/aubo_arm/aubo_arm.cpp @@ -23,6 +23,21 @@ namespace cmvr::device { namespace { +bool cancellationRequested( + const std::function& cancellation_requested) noexcept +{ + if (!cancellation_requested) { + return false; + } + try { + return cancellation_requested(); + } catch (...) { + // A broken cancellation source must never permit a queued motion to be + // submitted after its ownership can no longer be established. + return true; + } +} + class MotionOwnerGuard final { public: MotionOwnerGuard( @@ -1818,11 +1833,12 @@ Result AuboArm::moveJ(const JointPositionCommand& target, const MotionOptions& o auto motion_control = robot_interface->getMotionControl(); motion_control->setSpeedFraction(speed_scaling_); motion_owner.requireExplicitSettlement(); - if (!validateSafetyPermit(safety_monitor, safety_permit)) { + if (cancellationRequested(options.cancellation_requested) || + !validateSafetyPermit(safety_monitor, safety_permit)) { motion_owner.settle(); return Result::failure( ArmErrorCode::CommandRejected, - "[AuboArm] moveJ cancelled by hardware safety before submission"); + "[AuboArm] moveJ cancelled before submission"); } const int ret = motion_control->moveJoint( target.position, @@ -1843,18 +1859,39 @@ Result AuboArm::moveJ(const JointPositionCommand& target, const MotionOptions& o motion_state, safety_monitor, safety_permit, + cancellation_requested = options.cancellation_requested, token = motion.token]() { return waitArrival( robot_interface, [motion_state, safety_monitor, safety_permit, + cancellation_requested, token]() { - return motion_state->cancelled(token) || + return cancellationRequested( + cancellation_requested) || + motion_state->cancelled(token) || !validateSafetyPermit( safety_monitor, safety_permit); }); }); + if (cancellationRequested(options.cancellation_requested)) { + if (ret == arcs::common_interface::AUBO_OK) { + if (outcome == + aubo_internal::MotionCommandOutcome::CompletedAfterMotion) { + motion_owner.clearOnFinish(); + } else { + // The request was accepted but its completion is no longer + // owned by this caller. Preserve the typed motion state so + // the cancellation owner can issue stopJoint/stopLine. + motion_owner.retainKind(); + } + } + motion_owner.settle(); + return Result::failure( + ArmErrorCode::CommandRejected, + "[AuboArm] moveJ cancelled by its caller"); + } if (!validateSafetyPermit(safety_monitor, safety_permit)) { motion_owner.settle(); return Result::failure( @@ -2019,11 +2056,12 @@ Result AuboArm::moveL(const CartesianPose& target, const MotionOptions& options, robot_interface->getRobotConfig()->setTcpOffset(tcp_offset); std::vector pose{target.x, target.y, target.z, target.rx, target.ry, target.rz}; motion_owner.requireExplicitSettlement(); - if (!validateSafetyPermit(safety_monitor, safety_permit)) { + if (cancellationRequested(options.cancellation_requested) || + !validateSafetyPermit(safety_monitor, safety_permit)) { motion_owner.settle(); return Result::failure( ArmErrorCode::CommandRejected, - "[AuboArm] moveL cancelled by hardware safety before submission"); + "[AuboArm] moveL cancelled before submission"); } const int ret = motion_control->moveLine( pose, @@ -2044,18 +2082,38 @@ Result AuboArm::moveL(const CartesianPose& target, const MotionOptions& options, motion_state, safety_monitor, safety_permit, + cancellation_requested = options.cancellation_requested, token = motion.token]() { return waitArrival( robot_interface, [motion_state, safety_monitor, safety_permit, + cancellation_requested, token]() { - return motion_state->cancelled(token) || + return cancellationRequested( + cancellation_requested) || + motion_state->cancelled(token) || !validateSafetyPermit( safety_monitor, safety_permit); }); }); + if (cancellationRequested(options.cancellation_requested)) { + if (ret == arcs::common_interface::AUBO_OK) { + if (outcome == + aubo_internal::MotionCommandOutcome::CompletedAfterMotion) { + motion_owner.clearOnFinish(); + } else { + // Keep the accepted linear kind until a typed Stop confirms + // that the controller is idle. + motion_owner.retainKind(); + } + } + motion_owner.settle(); + return Result::failure( + ArmErrorCode::CommandRejected, + "[AuboArm] moveL cancelled by its caller"); + } if (!validateSafetyPermit(safety_monitor, safety_permit)) { motion_owner.settle(); return Result::failure( diff --git a/cmvr-es/devices/arm/aubo_arm/aubo_arm.h b/cmvr-es/devices/arm/aubo_arm/aubo_arm.h index e708c748..4f22ddfe 100644 --- a/cmvr-es/devices/arm/aubo_arm/aubo_arm.h +++ b/cmvr-es/devices/arm/aubo_arm/aubo_arm.h @@ -33,6 +33,7 @@ public: RobotMode getRobotMode() const override; SafetyMode getSafetyMode() const override; ControlMode getControlMode() const override; + bool supportsActionQueueMotion() const noexcept override { return true; } Result torqueOn() override; Result torqueOff() override; diff --git a/cmvr-es/devices/arm/huayan_arm/huayan_arm.cpp b/cmvr-es/devices/arm/huayan_arm/huayan_arm.cpp index dbc2093a..90c0e627 100644 --- a/cmvr-es/devices/arm/huayan_arm/huayan_arm.cpp +++ b/cmvr-es/devices/arm/huayan_arm/huayan_arm.cpp @@ -34,6 +34,21 @@ constexpr auto kControllerStopTimeout = std::chrono::milliseconds(3000); constexpr auto kOwnerExitTimeout = std::chrono::milliseconds(3000); constexpr auto kCompletionCorrelationGrace = std::chrono::milliseconds(250); +bool cancellationRequested( + const std::function& cancellation_requested) noexcept +{ + if (!cancellation_requested) { + return false; + } + try { + return cancellation_requested(); + } catch (...) { + // Cancellation sources are part of the motion-admission safety gate. + // Treat an exception as cancellation instead of admitting new motion. + return true; + } +} + double radToDeg(const double value) { return value * 180.0 / kPi; @@ -662,11 +677,12 @@ Result HuayanRobot::moveJ( if (targetReached_(&target.position, nullptr) && controllerIdleStable_(runtime, std::chrono::milliseconds(300))) { - if (!runtime->safety.validate(permit)) { + if (cancellationRequested(options.cancellation_requested) || + !runtime->safety.validate(permit)) { runtime->motion.finish(start.token, MotionFinishMode::Clear); return Result::failure( ArmErrorCode::CommandRejected, - "[HuayanRobot] moveJ cancelled by a safety transition"); + "[HuayanRobot] moveJ cancelled by its caller or a safety transition"); } runtime->motion.finish(start.token, MotionFinishMode::Clear); return Result::success(); @@ -687,18 +703,20 @@ Result HuayanRobot::moveJ( const double blend = metersToMm(options.blend_radius); const auto command_id = nextCommandId_(); - if (!runtime->safety.validate(permit)) { + if (cancellationRequested(options.cancellation_requested) || + !runtime->safety.validate(permit)) { runtime->motion.finish(start.token, MotionFinishMode::Clear); return Result::failure( ArmErrorCode::CommandRejected, - "[HuayanRobot] moveJ cancelled before submission by a safety transition"); + "[HuayanRobot] moveJ cancelled before submission"); } int ret = 0; { std::lock_guard submission_lock( runtime->submission_mutex); std::lock_guard sdk_lock(sdk_mutex_); - if (runtime->motion.cancelled(start.token) || + if (cancellationRequested(options.cancellation_requested) || + runtime->motion.cancelled(start.token) || !runtime->safety.validate(permit)) { runtime->motion.finish(start.token, MotionFinishMode::Clear); return Result::failure( @@ -719,7 +737,8 @@ Result HuayanRobot::moveJ( admission_lock.unlock(); return waitMotionDone_( "moveJ", runtime, start.token, permit, command_id, - &target.position, nullptr, 60000); + &target.position, nullptr, 60000, + options.cancellation_requested); } Result HuayanRobot::speedJ( @@ -837,11 +856,12 @@ Result HuayanRobot::moveL( if (targetReached_(nullptr, &target) && controllerIdleStable_(runtime, std::chrono::milliseconds(300))) { - if (!runtime->safety.validate(permit)) { + if (cancellationRequested(options.cancellation_requested) || + !runtime->safety.validate(permit)) { runtime->motion.finish(start.token, MotionFinishMode::Clear); return Result::failure( ArmErrorCode::CommandRejected, - "[HuayanRobot] moveL cancelled by a safety transition"); + "[HuayanRobot] moveL cancelled by its caller or a safety transition"); } runtime->motion.finish(start.token, MotionFinishMode::Clear); return Result::success(); @@ -868,18 +888,20 @@ Result HuayanRobot::moveL( const double blend = metersToMm(options.blend_radius); const auto command_id = nextCommandId_(); - if (!runtime->safety.validate(permit)) { + if (cancellationRequested(options.cancellation_requested) || + !runtime->safety.validate(permit)) { runtime->motion.finish(start.token, MotionFinishMode::Clear); return Result::failure( ArmErrorCode::CommandRejected, - "[HuayanRobot] moveL cancelled before submission by a safety transition"); + "[HuayanRobot] moveL cancelled before submission"); } int ret = 0; { std::lock_guard submission_lock( runtime->submission_mutex); std::lock_guard sdk_lock(sdk_mutex_); - if (runtime->motion.cancelled(start.token) || + if (cancellationRequested(options.cancellation_requested) || + runtime->motion.cancelled(start.token) || !runtime->safety.validate(permit)) { runtime->motion.finish(start.token, MotionFinishMode::Clear); return Result::failure( @@ -900,7 +922,8 @@ Result HuayanRobot::moveL( admission_lock.unlock(); return waitMotionDone_( "moveL", runtime, start.token, permit, command_id, - nullptr, &target, 60000); + nullptr, &target, 60000, + options.cancellation_requested); } Result HuayanRobot::speedL( @@ -2279,7 +2302,8 @@ Result HuayanRobot::waitMotionDone_( const std::string& command_id, const std::vector* joint_target, const CartesianPose* tcp_target, - const int timeout_ms) const + const int timeout_ms, + const std::function& cancellation_requested) const { const auto started_at = std::chrono::steady_clock::now(); bool saw_motion = false; @@ -2287,13 +2311,20 @@ Result HuayanRobot::waitMotionDone_( int stable_completion_samples = 0; while (runtime->monitor_running.load()) { - if (runtime->motion.cancelled(motion_token) || + const bool caller_cancelled = + cancellationRequested(cancellation_requested); + if (caller_cancelled || + runtime->motion.cancelled(motion_token) || !runtime->safety.validate(safety_permit)) { - runtime->motion.finish(motion_token, MotionFinishMode::Clear); + runtime->motion.finish( + motion_token, + caller_cancelled + ? MotionFinishMode::Retain + : MotionFinishMode::Clear); return Result::failure( ArmErrorCode::CommandRejected, "[HuayanRobot] " + context + - " cancelled by Stop or a safety transition"); + " cancelled by its caller, Stop, or a safety transition"); } bool done = false; diff --git a/cmvr-es/devices/arm/huayan_arm/huayan_arm.h b/cmvr-es/devices/arm/huayan_arm/huayan_arm.h index 010e0339..9e5a684f 100644 --- a/cmvr-es/devices/arm/huayan_arm/huayan_arm.h +++ b/cmvr-es/devices/arm/huayan_arm/huayan_arm.h @@ -11,6 +11,7 @@ #include #include #include +#include #include #include #include @@ -41,6 +42,7 @@ public: RobotMode getRobotMode() const override; SafetyMode getSafetyMode() const override; ControlMode getControlMode() const override; + bool supportsActionQueueMotion() const noexcept override { return true; } Result torqueOn() override; Result torqueOff() override; @@ -149,7 +151,8 @@ private: const std::string& command_id, const std::vector* joint_target, const CartesianPose* tcp_target, - int timeout_ms) const; + int timeout_ms, + const std::function& cancellation_requested = {}) const; bool targetReached_( const std::vector* joint_target, const CartesianPose* tcp_target) const; diff --git a/cmvr-es/devices/arm/huayan_arm/tests/huayan_arm_sdk_test.cpp b/cmvr-es/devices/arm/huayan_arm/tests/huayan_arm_sdk_test.cpp index b2569995..00cb8e27 100644 --- a/cmvr-es/devices/arm/huayan_arm/tests/huayan_arm_sdk_test.cpp +++ b/cmvr-es/devices/arm/huayan_arm/tests/huayan_arm_sdk_test.cpp @@ -592,11 +592,51 @@ int main() MotionOptions options; options.velocity = 0.4; options.acceleration = 0.8; + CHECK_TRUE(arm.supportsActionQueueMotion()); + + JointPositionCommand joint_a{{0.10, -0.05, 0.08, 0.0, 0.02, -0.03}}; + MotionOptions cancelled_options = options; + cancelled_options.cancellation_requested = []() { return true; }; + CHECK_TRUE(!arm.moveJ(joint_a, cancelled_options).ok()); + CartesianPose cancelled_pose; + cancelled_pose.x = 0.20; + cancelled_pose.z = 0.30; + CHECK_TRUE(!arm.moveL(cancelled_pose, cancelled_options).ok()); + { + std::lock_guard lock(g_sdk.mutex); + CHECK_TRUE(g_sdk.move_j_calls == 0); + CHECK_TRUE(g_sdk.move_l_calls == 0); + } + + // Once a command has been accepted, caller cancellation returns promptly + // but retains the typed motion barrier until Stop confirms controller idle. + std::atomic cancel_during_wait{false}; + MotionOptions cancellable_options = options; + cancellable_options.cancellation_requested = [&cancel_during_wait]() { + return cancel_during_wait.load(); + }; + JointPositionCommand cancelled_in_wait{ + {0.05, -0.02, 0.04, 0.01, 0.0, -0.01}}; + holdNextMotion(); + auto cancelled_motion = std::async(std::launch::async, [&]() { + return arm.moveJ(cancelled_in_wait, cancellable_options); + }); + CHECK_TRUE(waitUntil([&]() { + std::lock_guard lock(g_sdk.mutex); + return g_sdk.move_j_calls > 0; + })); + cancel_during_wait.store(true); + CHECK_TRUE(cancelled_motion.wait_for(1s) == std::future_status::ready); + if (cancelled_motion.wait_for(0ms) == std::future_status::ready) { + CHECK_TRUE(!cancelled_motion.get().ok()); + } + CHECK_TRUE(arm.busy()); + CHECK_TRUE(arm.stopMotion().ok()); + CHECK_TRUE(!arm.busy()); // The first IsMotionDone read intentionally reports the preceding idle // state. Completion must be correlated with the command/target. Once the // target is reached, an identical command is an idempotent no-op. - JointPositionCommand joint_a{{0.10, -0.05, 0.08, 0.0, 0.02, -0.03}}; CHECK_TRUE(arm.moveJ(joint_a, options).ok()); int move_j_after_first = 0; { diff --git a/cmvr-es/devices/arm/robot_arm.h b/cmvr-es/devices/arm/robot_arm.h index 071f3ce9..4f63cf4d 100644 --- a/cmvr-es/devices/arm/robot_arm.h +++ b/cmvr-es/devices/arm/robot_arm.h @@ -28,6 +28,12 @@ public: virtual SafetyMode getSafetyMode() const = 0; virtual ControlMode getControlMode() const = 0; + // Queued actions require synchronous completion, cooperative cancellation + // at the final device-submission boundary, and a bounded typed Stop which + // returns success only after controller idle is confirmed. Backends must + // opt in only after all of these semantics have been validated. + virtual bool supportsActionQueueMotion() const noexcept { return false; } + // ArmTeleop requires an explicitly reviewed group-servo implementation. // Existing and vendor arms remain unavailable until their implementations // override this capability after timing and partial-write validation. diff --git a/cmvr-es/manager/control_authority/include/control_authority_manager.h b/cmvr-es/manager/control_authority/include/control_authority_manager.h index b3547c2f..bda89ad5 100644 --- a/cmvr-es/manager/control_authority/include/control_authority_manager.h +++ b/cmvr-es/manager/control_authority/include/control_authority_manager.h @@ -49,6 +49,20 @@ public: const std::string& resource_id, const std::string& owner_id, Duration ttl); + + // Converts the expected normal lease into a safety barrier only while it + // is still the current lease. A stale token never preempts a successor or + // joins an existing safety barrier. + ControlAcquireResult preemptAcquireIfCurrent( + const ControlLeaseToken& expected_token, + const std::string& owner_id, + Duration ttl); + + // Permanently blocks the resource only if the expected normal lease is + // still current. Quarantine does not allocate and can only be removed by + // an explicit revoke/clear. + bool quarantineIfCurrent( + const ControlLeaseToken& expected_token) noexcept; bool renew(const ControlLeaseToken& token, Duration ttl); bool validate(const ControlLeaseToken& token); void release(const ControlLeaseToken& token) noexcept; @@ -69,9 +83,11 @@ private: std::uint64_t generation{0}; std::chrono::steady_clock::time_point deadline; bool preemptible{true}; + bool quarantined{false}; std::unordered_map safety_holders; }; + static void quarantine_(Entry& entry) noexcept; bool expired_(const Entry& entry) const noexcept; std::mutex mutex_; diff --git a/cmvr-es/manager/control_authority/src/control_authority_manager.cpp b/cmvr-es/manager/control_authority/src/control_authority_manager.cpp index 53facdd8..00c89730 100644 --- a/cmvr-es/manager/control_authority/src/control_authority_manager.cpp +++ b/cmvr-es/manager/control_authority/src/control_authority_manager.cpp @@ -1,5 +1,6 @@ #include "manager/control_authority/include/control_authority_manager.h" +#include #include namespace cmvr::control { @@ -44,6 +45,7 @@ ControlAcquireResult ControlAuthorityManager::tryAcquire( token.generation, std::chrono::steady_clock::now() + ttl, true, + false, {}}); return {true, std::move(token), {}}; } @@ -71,22 +73,112 @@ ControlAcquireResult ControlAuthorityManager::preemptAcquire( token.generation, token.owner_id); return {true, std::move(token), {}}; } - entries_.erase(existing); } - ControlLeaseToken token; - token.resource_id = resource_id; - token.owner_id = owner_id; - token.generation = ++next_generation_; - entries_.emplace( - resource_id, - Entry{ + const bool has_existing = existing != entries_.end(); + const bool replacing_normal = + has_existing && existing->second.preemptible; + try { + ControlLeaseToken token; + token.resource_id = resource_id; + token.owner_id = owner_id; + token.generation = ++next_generation_; + Entry replacement{ owner_id, token.generation, std::chrono::steady_clock::time_point::max(), false, - {{token.generation, owner_id}}}); - return {true, std::move(token), {}}; + false, + {{token.generation, owner_id}}}; + + if (has_existing) { + static_assert( + std::is_nothrow_move_assignable_v, + "safety barrier replacement must not throw"); + existing->second = std::move(replacement); + } else { + entries_.emplace(resource_id, std::move(replacement)); + } + return {true, std::move(token), {}}; + } catch (...) { + if (replacing_normal && existing->second.preemptible) { + quarantine_(existing->second); + } + throw; + } +} + +ControlAcquireResult ControlAuthorityManager::preemptAcquireIfCurrent( + const ControlLeaseToken& expected_token, + const std::string& owner_id, + const Duration ttl) +{ + if (!expected_token.valid() || owner_id.empty() || + ttl <= Duration::zero()) { + return {false, {}, "invalid conditional control barrier request"}; + } + + std::lock_guard lock(mutex_); + const auto existing = entries_.find(expected_token.resource_id); + if (existing == entries_.end() || + expired_(existing->second) || + !existing->second.preemptible || + existing->second.owner_id != expected_token.owner_id || + existing->second.generation != expected_token.generation) { + return { + false, + {}, + "expected control lease is no longer current"}; + } + + try { + ControlLeaseToken token; + token.resource_id = expected_token.resource_id; + token.owner_id = owner_id; + token.generation = ++next_generation_; + Entry replacement{ + owner_id, + token.generation, + std::chrono::steady_clock::time_point::max(), + false, + false, + {{token.generation, owner_id}}}; + + static_assert( + std::is_nothrow_move_assignable_v, + "safety barrier replacement must not throw"); + existing->second = std::move(replacement); + return {true, std::move(token), {}}; + } catch (...) { + if (existing->second.preemptible) { + quarantine_(existing->second); + } + throw; + } +} + +bool ControlAuthorityManager::quarantineIfCurrent( + const ControlLeaseToken& expected_token) noexcept +{ + if (!expected_token.valid()) { + return false; + } + try { + std::lock_guard lock(mutex_); + const auto existing = + entries_.find(expected_token.resource_id); + if (existing == entries_.end() || + expired_(existing->second) || + !existing->second.preemptible || + existing->second.owner_id != expected_token.owner_id || + existing->second.generation != expected_token.generation) { + return false; + } + quarantine_(existing->second); + return true; + } catch (...) { + return false; + } } bool ControlAuthorityManager::renew( @@ -164,7 +256,8 @@ void ControlAuthorityManager::release( return; } found->second.safety_holders.erase(holder); - if (found->second.safety_holders.empty()) { + if (found->second.safety_holders.empty() && + !found->second.quarantined) { entries_.erase(found); } } else if (found->second.owner_id == token.owner_id && @@ -215,7 +308,16 @@ void ControlAuthorityManager::clear() noexcept bool ControlAuthorityManager::expired_( const Entry& entry) const noexcept { - return std::chrono::steady_clock::now() >= entry.deadline; + return !entry.quarantined && + std::chrono::steady_clock::now() >= entry.deadline; +} + +void ControlAuthorityManager::quarantine_(Entry& entry) noexcept +{ + entry.deadline = + std::chrono::steady_clock::time_point::max(); + entry.preemptible = false; + entry.quarantined = true; } } // namespace cmvr::control diff --git a/cmvr-es/manager/control_authority/tests/control_authority_manager_test.cpp b/cmvr-es/manager/control_authority/tests/control_authority_manager_test.cpp index 855d4853..c95cef21 100644 --- a/cmvr-es/manager/control_authority/tests/control_authority_manager_test.cpp +++ b/cmvr-es/manager/control_authority/tests/control_authority_manager_test.cpp @@ -93,6 +93,139 @@ TEST_F(ControlAuthorityManagerTest, EXPECT_FALSE(manager.isLeased("right_arm")); } +TEST_F(ControlAuthorityManagerTest, + ConditionalSafetyBarrierPreemptsMatchingCurrentLease) +{ + auto& manager = ControlAuthorityManager::instance(); + const auto control = + manager.tryAcquire("right_arm", "move-session", 100ms); + ASSERT_TRUE(control.acquired); + + const auto barrier = manager.preemptAcquireIfCurrent( + control.token, "timed-out-action", 100ms); + ASSERT_TRUE(barrier.acquired) << barrier.detail; + EXPECT_FALSE(manager.validate(control.token)); + EXPECT_TRUE(manager.validate(barrier.token)); + manager.release(control.token); + EXPECT_TRUE(manager.validate(barrier.token)); + EXPECT_FALSE( + manager.tryAcquire("right_arm", "new-move", 100ms) + .acquired); + + manager.release(barrier.token); + EXPECT_FALSE(manager.isLeased("right_arm")); +} + +TEST_F(ControlAuthorityManagerTest, + ConditionalSafetyBarrierDoesNotPreemptSuccessorForStaleToken) +{ + auto& manager = ControlAuthorityManager::instance(); + const auto old = + manager.tryAcquire("right_arm", "move-session", 100ms); + ASSERT_TRUE(old.acquired); + const auto direct_stop = manager.preemptAcquire( + "right_arm", "direct-stop", 100ms); + ASSERT_TRUE(direct_stop.acquired) << direct_stop.detail; + manager.release(direct_stop.token); + const auto successor = + manager.tryAcquire("right_arm", "move-session", 100ms); + ASSERT_TRUE(successor.acquired); + ASSERT_NE(old.token.generation, successor.token.generation); + + const auto barrier = manager.preemptAcquireIfCurrent( + old.token, "delayed-stop", 100ms); + EXPECT_FALSE(barrier.acquired); + EXPECT_FALSE(barrier.token.valid()); + EXPECT_TRUE(manager.validate(successor.token)); + EXPECT_FALSE( + manager.tryAcquire("right_arm", "competing-move", 100ms) + .acquired); + + manager.release(successor.token); + EXPECT_FALSE(manager.isLeased("right_arm")); +} + +TEST_F(ControlAuthorityManagerTest, + ConditionalSafetyBarrierDoesNotJoinExistingSafetyBarrier) +{ + auto& manager = ControlAuthorityManager::instance(); + const auto control = + manager.tryAcquire("right_arm", "move-session", 100ms); + ASSERT_TRUE(control.acquired); + const auto existing_barrier = manager.preemptAcquire( + "right_arm", "direct-stop", 100ms); + ASSERT_TRUE(existing_barrier.acquired) << existing_barrier.detail; + + const auto delayed_barrier = manager.preemptAcquireIfCurrent( + control.token, "delayed-action-stop", 100ms); + EXPECT_FALSE(delayed_barrier.acquired); + EXPECT_FALSE(delayed_barrier.token.valid()); + EXPECT_TRUE(manager.validate(existing_barrier.token)); + + manager.release(existing_barrier.token); + EXPECT_FALSE(manager.isLeased("right_arm")); + EXPECT_TRUE( + manager.tryAcquire("right_arm", "new-move", 100ms) + .acquired); +} + +TEST_F(ControlAuthorityManagerTest, + QuarantineSurvivesNormalAndTemporarySafetyTokenRelease) +{ + auto& manager = ControlAuthorityManager::instance(); + const auto control = + manager.tryAcquire("right_arm", "move-session", 20ms); + ASSERT_TRUE(control.acquired); + + ASSERT_TRUE(manager.quarantineIfCurrent(control.token)); + EXPECT_FALSE(manager.validate(control.token)); + EXPECT_TRUE(manager.isLeased("right_arm")); + + manager.release(control.token); + std::this_thread::sleep_for(30ms); + EXPECT_TRUE(manager.isLeased("right_arm")); + EXPECT_FALSE( + manager.tryAcquire("right_arm", "new-move", 100ms) + .acquired); + + const auto temporary_stop = manager.preemptAcquire( + "right_arm", "temporary-stop", 100ms); + ASSERT_TRUE(temporary_stop.acquired) << temporary_stop.detail; + EXPECT_TRUE(manager.validate(temporary_stop.token)); + manager.release(temporary_stop.token); + + EXPECT_TRUE(manager.isLeased("right_arm")); + EXPECT_FALSE( + manager.tryAcquire("right_arm", "new-move", 100ms) + .acquired); + + manager.revoke("right_arm"); + EXPECT_FALSE(manager.isLeased("right_arm")); +} + +TEST_F(ControlAuthorityManagerTest, + QuarantineWithStaleTokenDoesNotAffectSuccessor) +{ + auto& manager = ControlAuthorityManager::instance(); + const auto old = + manager.tryAcquire("right_arm", "move-session", 100ms); + ASSERT_TRUE(old.acquired); + manager.release(old.token); + const auto successor = + manager.tryAcquire("right_arm", "move-session", 100ms); + ASSERT_TRUE(successor.acquired); + ASSERT_NE(old.token.generation, successor.token.generation); + + EXPECT_FALSE(manager.quarantineIfCurrent(old.token)); + EXPECT_TRUE(manager.validate(successor.token)); + EXPECT_FALSE( + manager.tryAcquire("right_arm", "competing-move", 100ms) + .acquired); + + manager.release(successor.token); + EXPECT_FALSE(manager.isLeased("right_arm")); +} + TEST_F(ControlAuthorityManagerTest, ExpiryAndRenewUseMonotonicLocalTime) { auto& manager = ControlAuthorityManager::instance(); diff --git a/cmvr-es/service/CMakeLists.txt b/cmvr-es/service/CMakeLists.txt index f4a39cf3..920a0433 100644 --- a/cmvr-es/service/CMakeLists.txt +++ b/cmvr-es/service/CMakeLists.txt @@ -1,5 +1,6 @@ add_library(service + action/src/action_queue_executor.cpp grpc/src/grpc_camera_service.cpp grpc/src/grpc_system_service.cpp grpc/src/grpc_speaker_service.cpp diff --git a/cmvr-es/service/README.md b/cmvr-es/service/README.md index 2369d49e..9a78d3e6 100644 --- a/cmvr-es/service/README.md +++ b/cmvr-es/service/README.md @@ -8,6 +8,7 @@ | 目录 | 职责 | | --- | --- | +| `action/` | SystemService ActionQueue 的校验、幂等账本和边缘端 FIFO 执行器 | | `grpc/` | 入站设备控制、状态查询和兼容流式接口 | | `quic_edge/` | 边缘端主动连接平台的 QUIC client、控制状态机和媒体 packetizer | | `quic_edge/tests/` | 已登记到 CTest 的 QUIC 协议测试 | @@ -39,6 +40,56 @@ grpcurl -plaintext \ cmvr.api.SystemService/GetDeviceList ``` +## SystemService ActionQueue + +`SystemService/ExecuteActionQueue` 接收一个完整的有限动作序列，在边缘端排队并 +逐步串行执行，所有步骤结束后返回最终结果。平台只需要提交一次请求，因此连续机械臂 +动作不会再受到每个单独 gRPC 往返和 Wi-Fi 抖动的影响。 + +当前 v1 仅允许以下 `ActionStep.command`： + +- 机械臂同步 `MoveJ`、`MoveL`； +- AGV 同步 `navigateToPose`、`navigateToStation`、`followPath`； +- 边缘端本地 `delay`。 + +机械臂和 AGV 请求复用各自已有的类型化 Request，目标设备仍由每一步的 +`header.device_id` 指定。所有运动步骤必须设置 `asynchronous=false`；`MoveL` v1 仅接受 +Base frame；AGV 后端还必须明确支持同步导航终态确认。`speedJ`、`speedL`、`servoJ`、 +AGV `translate`、速度控制、查询和流式 RPC 都不属于 ActionQueue v1。 + +ActionQueue 遵循以下执行语义： + +- 平台先调用 `GetSystemInfo` 读取 `action_service_instance_id`，并在每次提交和重试中填入 + `expected_service_instance_id`。ActionQueue 账本随服务实例重建；若断线期间边缘服务重启， + 旧实例 ID 会被拒绝，平台必须先对账，不能用新 ID 自动重放不确定的动作； +- `action_id` 是必填的全局唯一幂等键；同一服务实例内，相同内容的已受理请求不会重复下发 + 设备命令，相同 ID 但内容不同的请求必须拒绝。服务端缓存最近 4096 个完整结果，更早的 + 已执行 ID 由精确 retired-ID 账本 fail-closed 拒绝、不会重跑；单实例最多记录 + 262144 个已受理 ID，达到容量后仅拒绝新 ID，已有 ID 仍可查询； +- 入队前校验全部步骤、设备、参数和同步能力，校验失败时不会执行任何步骤； +- v1 每个请求最多 256 步、序列化大小最多 512 KiB、排队或执行中的 Action 最多 64 个、 + 同时提交或等待结果的 RPC 最多 256 个；Action 与单步超时上限均为 24 小时， + `total_timeout_ms=0` 使用 30 分钟默认值，AGV 路径最多 4096 段； +- `total_timeout_ms` 包含排队与执行时间，单步 `timeout_ms=0` 时继承 Action 剩余时间 + 或服务端默认值；所有超时值均由服务端施加上限； +- 任一步失败、取消或超时后立即停止序列，不再执行后续步骤；`completed_steps` 表示此前 + 成功完成的步骤数，`failed_step_index` 仅在存在对应失败步骤时出现； +- Action 一旦受理，不因平台连接中断而自动取消；断线只结束该 RPC waiter，边缘动作继续。 + 平台可用相同 `action_id` 重试并取得仍在缓存中的同一次执行结果； +- `StopAll`、机械臂 `stopMotion`、AGV `cancelNavigation` 和软件急停不进入 FIFO，必须 + 作为高优先级安全/抢占路径执行。它们仍不具备功能安全等级。 +- 机械臂步骤超时会立即走 typed `stopMotion` 并等待停车确认；若无法确认停车，设备控制权 + 保持隔离，不会继续后续步骤或接受新的普通控制命令；需先按设备安全流程确认状态，再 + 重启边缘服务恢复控制。 +- AGV 的取消 ACK、零速度 ACK 均不等于停稳；ActionQueue 和安全停止 RPC 只有在导航任务 + 终态且底盘连续零速度采样确认后才释放控制权，否则同样保留隔离。 + +已知的执行完成、业务失败、取消、超时和预校验拒绝由 `ActionResultCode` 与 +`CommandHeader.Feedback` 表达。`ActionDeduplicationStatus` 结构化区分新受理、合并等待、 +缓存结果、已淘汰结果、账本耗尽、ID 冲突和服务实例不匹配；平台不得通过解析错误字符串 +判断动作是否执行过。 +`ACTION_RESULT_CODE_UNSPECIFIED` 不得作为服务端最终结果。 + ## MotorService `MotorService` 将 gRPC 电机命令适配到已经由 `DeviceManager` 创建的 diff --git a/cmvr-es/service/action/include/action_queue_executor.h b/cmvr-es/service/action/include/action_queue_executor.h new file mode 100644 index 00000000..3dbcac59 --- /dev/null +++ b/cmvr-es/service/action/include/action_queue_executor.h @@ -0,0 +1,65 @@ +#ifndef CMVR_ES_ACTION_QUEUE_EXECUTOR_H +#define CMVR_ES_ACTION_QUEUE_EXECUTOR_H + +#include +#include +#include +#include +#include + +#include "cmvr/api/system_command.pb.h" + +namespace cmvr::device { +class DeviceManager; +} + +namespace cmvr::service { + +// Owns the process-local FIFO used by SystemService ActionQueue requests. +// The executor intentionally has no grpc::ServerContext dependency: once a +// request is accepted, loss of the platform connection must not cancel device +// motion on the edge. +class ActionQueueExecutor final { +public: + static constexpr std::size_t kDefaultMaxAcceptedActionIds = + 256U * 1024U; + + enum class WaitResult { + Terminal, + CanceledBeforeAdmission, + CanceledAfterAdmission, + }; + + explicit ActionQueueExecutor( + device::DeviceManager& device_manager, + std::size_t max_accepted_action_ids = + kDefaultMaxAcceptedActionIds); + ~ActionQueueExecutor(); + + ActionQueueExecutor(const ActionQueueExecutor&) = delete; + ActionQueueExecutor& operator=(const ActionQueueExecutor&) = delete; + + // Validates, idempotently enqueues, and waits for the terminal result. + // Protocol and execution outcomes are represented in Feedback. + WaitResult submitAndWait( + const api::ActionQueueCommand_Request& request, + api::ActionQueueCommand_Feedback& feedback, + const std::function& waiter_canceled = {}); + + // StopAll uses this fail-closed transition. It rejects future submissions, + // cancels pending actions, and requests a typed stop for the active action. + // Returns true when every active Action device reported a confirmed stop. + // False means at least one resource remains fail-closed quarantined. + bool cancelAllAndDisable(); + + bool waitForIdle(std::chrono::milliseconds timeout); + const std::string& instanceId() const noexcept; + +private: + struct Impl; + std::unique_ptr impl_; +}; + +} // namespace cmvr::service + +#endif // CMVR_ES_ACTION_QUEUE_EXECUTOR_H diff --git a/cmvr-es/service/action/src/action_queue_executor.cpp b/cmvr-es/service/action/src/action_queue_executor.cpp new file mode 100644 index 00000000..e8641dd4 --- /dev/null +++ b/cmvr-es/service/action/src/action_queue_executor.cpp @@ -0,0 +1,2204 @@ +#include "service/action/include/action_queue_executor.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#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 "devices/agv/abstract_agv.h" +#include "devices/arm/robot_arm.h" +#include "manager/control_authority/include/control_authority_manager.h" +#include "manager/device_manager/include/device_manager.h" + +namespace cmvr::service { +namespace { + +using Clock = std::chrono::steady_clock; +using Milliseconds = std::chrono::milliseconds; + +constexpr std::size_t kMaxSteps = 256; +constexpr std::size_t kMaxQueuedActions = 64; +constexpr std::size_t kMaxTerminalResults = 4096; +constexpr std::size_t kMaxRequestBytes = 512U * 1024U; +constexpr std::size_t kMaxIdentifierBytes = 128; +constexpr std::size_t kMaxPathSegments = 4096; +constexpr std::size_t kMaxConcurrentSubmitters = 256; +constexpr Milliseconds kDefaultTotalTimeout = std::chrono::minutes(30); +constexpr Milliseconds kMaximumTimeout = std::chrono::hours(24); +constexpr Milliseconds kLeaseRetryPeriod{10}; +constexpr Milliseconds kWaiterCancellationPollPeriod{20}; +constexpr Milliseconds kArmWatchdogPollPeriod{5}; +constexpr Milliseconds kArmIdleConfirmationPollPeriod{10}; +constexpr auto kConcurrentArmStopConfirmationTimeout = + std::chrono::seconds(7); +constexpr auto kControlLeaseTtl = std::chrono::hours(25); + +enum class PreparedStepKind { + ArmMoveJ, + ArmMoveL, + AgvNavigateToPose, + AgvNavigateToStation, + AgvFollowPath, + Delay, +}; + +struct PreparedStep { + PreparedStepKind kind{PreparedStepKind::Delay}; + std::string device_id; + std::shared_ptr arm; + std::shared_ptr agv; +}; + +struct PreparedAction { + std::vector steps; + std::vector resource_ids; +}; + +struct ValidationResult { + bool valid{false}; + std::string error; + int failed_step_index{-1}; + PreparedAction prepared; +}; + +struct RequestFingerprint { + std::array words{}; + std::uint64_t serialized_size{0}; + + bool operator==(const RequestFingerprint& other) const noexcept + { + return serialized_size == other.serialized_size && + words == other.words; + } + + bool operator!=(const RequestFingerprint& other) const noexcept + { + return !(*this == other); + } +}; + +std::uint64_t stableHash( + const std::string& value, + const std::uint64_t seed) noexcept +{ + std::uint64_t hash = 1469598103934665603ULL ^ seed; + for (const unsigned char byte : value) { + hash ^= static_cast(byte); + hash *= 1099511628211ULL; + } + hash ^= hash >> 33U; + hash *= 0xff51afd7ed558ccdULL; + hash ^= hash >> 33U; + hash *= 0xc4ceb9fe1a85ec53ULL; + hash ^= hash >> 33U; + return hash; +} + +std::string generateServiceInstanceId() +{ + std::array bytes{}; + std::size_t offset = 0; + while (offset < bytes.size()) { + const auto received = ::getrandom( + bytes.data() + offset, + bytes.size() - offset, + 0); + if (received < 0) { + if (errno == EINTR) { + continue; + } + throw std::system_error( + errno, + std::generic_category(), + "could not generate the ActionQueue service instance id"); + } + if (received == 0) { + throw std::runtime_error( + "could not generate the ActionQueue service instance id"); + } + offset += static_cast(received); + } + + // RFC 4122 variant and version bits make the epoch recognizable as a + // random UUID without reducing its collision resistance materially. + bytes[6] = static_cast((bytes[6] & 0x0fU) | 0x40U); + bytes[8] = static_cast((bytes[8] & 0x3fU) | 0x80U); + std::ostringstream stream; + stream << std::hex << std::setfill('0'); + for (std::size_t index = 0; index < bytes.size(); ++index) { + if (index == 4U || index == 6U || index == 8U || index == 10U) { + stream << '-'; + } + stream << std::setw(2) << static_cast(bytes[index]); + } + return stream.str(); +} + +std::size_t validatedAcceptedActionLimit(const std::size_t limit) +{ + if (limit == 0U) { + throw std::invalid_argument( + "ActionQueue accepted-action limit must be positive"); + } + return limit; +} + +void fillTerminalFeedback( + api::ActionQueueCommand_Feedback& feedback, + const std::string& action_id, + const api::ActionResultCode result, + const std::uint32_t completed_steps, + const std::string& message, + const int failed_step_index = -1) +{ + feedback.Clear(); + feedback.set_action_id(action_id); + feedback.set_result(result); + feedback.set_completed_steps(completed_steps); + if (failed_step_index >= 0) { + feedback.set_failed_step_index( + static_cast(failed_step_index)); + } + auto* header = feedback.mutable_header(); + header->set_success(result == api::ACTION_RESULT_CODE_COMPLETED); + header->set_error_message(message); + *header->mutable_timestamp() = + google::protobuf::util::TimeUtil::GetCurrentTime(); +} + +bool finite(const double value) noexcept +{ + return std::isfinite(value); +} + +bool finiteNonNegative(const double value) noexcept +{ + return finite(value) && value >= 0.0; +} + +bool validArmOptions( + const api::MotionOptions& options, + std::string& error) +{ + if (options.asynchronous()) { + error = "ActionQueue requires synchronous RobotArm motion"; + return false; + } + if (!finiteNonNegative(options.velocity()) || + !finiteNonNegative(options.acceleration()) || + !finiteNonNegative(options.blend_radius()) || + !finiteNonNegative(options.jerk())) { + error = "RobotArm motion options must be finite and non-negative"; + return false; + } + for (const double limit : options.joint_velocity_limits()) { + if (!finiteNonNegative(limit)) { + error = "RobotArm joint velocity limits must be finite and non-negative"; + return false; + } + } + return true; +} + +bool validAgvOptions( + const msgs::AgvMotionOptions& options, + std::string& error) +{ + if (options.asynchronous()) { + error = "ActionQueue requires synchronous AGV navigation"; + return false; + } + if (!finiteNonNegative(options.max_speed()) || + !finiteNonNegative(options.max_angular_speed()) || + !finiteNonNegative(options.max_acceleration()) || + !finiteNonNegative(options.max_angular_acceleration()) || + !finiteNonNegative(options.reach_distance()) || + !finiteNonNegative(options.reach_angle()) || + !finiteNonNegative(options.speed_ratio())) { + error = "AGV motion options must be finite and non-negative"; + return false; + } + if (options.speed_ratio() > 1.0) { + error = "AGV speed_ratio must be in [0, 1]"; + return false; + } + if (options.wait_timeout_ms() < 0 || + options.poll_interval_ms() < 0) { + error = "AGV timeout and poll interval must be non-negative"; + return false; + } + if (options.poll_interval_ms() > 5000) { + error = "AGV poll_interval_ms must not exceed 5000"; + return false; + } + if (options.wait_timeout_ms() > 0 && + options.poll_interval_ms() > options.wait_timeout_ms()) { + error = "AGV poll_interval_ms must not exceed wait_timeout_ms"; + return false; + } + return true; +} + +device::FrameType toFrameType(const api::ArmFrameType frame) +{ + switch (frame) { + case api::ARM_FRAME_BASE: + return device::FrameType::Base; + case api::ARM_FRAME_TOOL: + return device::FrameType::Tool; + case api::ARM_FRAME_WORLD: + return device::FrameType::World; + case api::ARM_FRAME_USER: + return device::FrameType::User; + } + return device::FrameType::Base; +} + +device::MotionOptions toArmMotionOptions( + const api::MotionOptions& source) +{ + device::MotionOptions destination; + destination.velocity = source.velocity(); + destination.acceleration = source.acceleration(); + destination.blend_radius = source.blend_radius(); + destination.jerk = source.jerk() > 0.0 ? source.jerk() : 5.0; + destination.joint_velocity_limits.assign( + source.joint_velocity_limits().begin(), + source.joint_velocity_limits().end()); + destination.asynchronous = source.asynchronous(); + return destination; +} + +device::AgvAdapterParams toAgvAdapterParams( + const msgs::AgvAdapterParams& source) +{ + device::AgvAdapterParams destination; + for (const auto& item : source.values()) { + destination.values.emplace(item.first, item.second); + } + return destination; +} + +device::AgvMotionOptions toAgvMotionOptions( + const msgs::AgvMotionOptions& source) +{ + device::AgvMotionOptions destination; + destination.max_speed = source.max_speed(); + destination.max_angular_speed = source.max_angular_speed(); + destination.max_acceleration = source.max_acceleration(); + destination.max_angular_acceleration = + source.max_angular_acceleration(); + destination.reach_distance = source.reach_distance(); + destination.reach_angle = source.reach_angle(); + destination.speed_ratio = + source.speed_ratio() > 0.0 ? source.speed_ratio() : 1.0; + destination.asynchronous = source.asynchronous(); + destination.wait_timeout_ms = source.wait_timeout_ms(); + destination.poll_interval_ms = source.poll_interval_ms(); + return destination; +} + +std::string stepPrefix( + const int index, + const api::ActionStep& step) +{ + std::string prefix = "step " + std::to_string(index); + if (!step.step_id().empty()) { + prefix += " (" + step.step_id() + ")"; + } + return prefix + ": "; +} + +std::string canonicalRequest( + const api::ActionQueueCommand_Request& request) +{ + api::ActionQueueCommand_Request normalized(request); + normalized.DiscardUnknownFields(); + for (auto& step : *normalized.mutable_steps()) { + switch (step.command_case()) { + case api::ActionStep::kArmMoveJ: + step.mutable_arm_move_j() + ->mutable_header()->clear_timestamp(); + break; + case api::ActionStep::kArmMoveL: + step.mutable_arm_move_l() + ->mutable_header()->clear_timestamp(); + break; + case api::ActionStep::kAgvNavigateToPose: + step.mutable_agv_navigate_to_pose() + ->mutable_header()->clear_timestamp(); + break; + case api::ActionStep::kAgvNavigateToStation: + step.mutable_agv_navigate_to_station() + ->mutable_header()->clear_timestamp(); + break; + case api::ActionStep::kAgvFollowPath: + step.mutable_agv_follow_path() + ->mutable_header()->clear_timestamp(); + break; + case api::ActionStep::kDelay: + case api::ActionStep::COMMAND_NOT_SET: + break; + } + } + + std::string serialized; + google::protobuf::io::StringOutputStream stream(&serialized); + google::protobuf::io::CodedOutputStream coded_stream(&stream); + coded_stream.SetSerializationDeterministic(true); + if (!normalized.SerializeToCodedStream(&coded_stream)) { + return {}; + } + coded_stream.Trim(); + return serialized; +} + +std::optional fingerprintRequest( + const api::ActionQueueCommand_Request& request) +{ + const std::string serialized = canonicalRequest(request); + if (serialized.empty()) { + return std::nullopt; + } + RequestFingerprint fingerprint; + fingerprint.serialized_size = serialized.size(); + constexpr std::array seeds{ + 0xa4093822299f31d0ULL, + 0x082efa98ec4e6c89ULL, + 0x452821e638d01377ULL, + 0xbe5466cf34e90c6cULL}; + for (std::size_t index = 0; index < seeds.size(); ++index) { + fingerprint.words[index] = stableHash(serialized, seeds[index]); + } + return fingerprint; +} + +std::string validateRequestEnvelope( + const api::ActionQueueCommand_Request& request) +{ + if (request.action_id().empty()) { + return "action_id is required"; + } + if (request.action_id().size() > kMaxIdentifierBytes) { + return "action_id is too long"; + } + if (request.expected_service_instance_id().empty()) { + return "expected_service_instance_id is required; obtain it from GetSystemInfo"; + } + if (request.expected_service_instance_id().size() > + kMaxIdentifierBytes) { + return "expected_service_instance_id is too long"; + } + if (request.steps().empty()) { + return "ActionQueue requires at least one step"; + } + if (static_cast(request.steps_size()) > kMaxSteps) { + return "ActionQueue exceeds the maximum step count"; + } + if (request.ByteSizeLong() > kMaxRequestBytes) { + return "ActionQueue request exceeds the 512 KiB application limit"; + } + if (request.total_timeout_ms() > + static_cast(kMaximumTimeout.count())) { + return "ActionQueue total timeout exceeds the server limit"; + } + return {}; +} + +ValidationResult validateRequest( + device::DeviceManager& device_manager, + const api::ActionQueueCommand_Request& request) +{ + ValidationResult result; + result.error = validateRequestEnvelope(request); + if (!result.error.empty()) { + return result; + } + + std::unordered_set step_ids; + std::set resources; + result.prepared.steps.reserve( + static_cast(request.steps_size())); + + for (int index = 0; index < request.steps_size(); ++index) { + result.failed_step_index = index; + const auto& step = request.steps(index); + const std::string prefix = stepPrefix(index, step); + if (step.step_id().empty()) { + result.error = prefix + "step_id is required"; + return result; + } + if (step.step_id().size() > kMaxIdentifierBytes) { + result.error = prefix + "step_id is too long"; + return result; + } + if (!step_ids.emplace(step.step_id()).second) { + result.error = prefix + "step_id must be unique"; + return result; + } + if (step.timeout_ms() > + static_cast(kMaximumTimeout.count())) { + result.error = prefix + "timeout exceeds the server limit"; + return result; + } + + PreparedStep prepared; + std::string options_error; + switch (step.command_case()) { + case api::ActionStep::kArmMoveJ: { + prepared.kind = PreparedStepKind::ArmMoveJ; + const auto& command = step.arm_move_j(); + prepared.device_id = command.header().device_id(); + if (prepared.device_id.empty()) { + result.error = prefix + "RobotArm device_id is required"; + return result; + } + prepared.arm = device_manager.getDevice( + prepared.device_id); + if (!prepared.arm) { + result.error = prefix + "RobotArm device not found: " + + prepared.device_id; + return result; + } + if (!prepared.arm->supportsActionQueueMotion()) { + result.error = prefix + + "RobotArm backend does not support safe ActionQueue motion: " + + prepared.device_id; + return result; + } + if (!validArmOptions(command.options(), options_error)) { + result.error = prefix + options_error; + return result; + } + const auto dof = prepared.arm->getDof(); + if (dof == 0U || command.target().position_size() != + static_cast(dof)) { + result.error = prefix + + "MoveJ target size does not match RobotArm DOF"; + return result; + } + if (!command.options().joint_velocity_limits().empty() && + command.options().joint_velocity_limits_size() != + static_cast(dof)) { + result.error = prefix + + "MoveJ joint velocity limit size does not match RobotArm DOF"; + return result; + } + for (const double position : command.target().position()) { + if (!finite(position)) { + result.error = prefix + + "MoveJ target must contain finite values"; + return result; + } + } + resources.emplace(prepared.device_id); + break; + } + case api::ActionStep::kArmMoveL: { + prepared.kind = PreparedStepKind::ArmMoveL; + const auto& command = step.arm_move_l(); + prepared.device_id = command.header().device_id(); + if (prepared.device_id.empty()) { + result.error = prefix + "RobotArm device_id is required"; + return result; + } + prepared.arm = device_manager.getDevice( + prepared.device_id); + if (!prepared.arm) { + result.error = prefix + "RobotArm device not found: " + + prepared.device_id; + return result; + } + if (!prepared.arm->supportsActionQueueMotion()) { + result.error = prefix + + "RobotArm backend does not support safe ActionQueue motion: " + + prepared.device_id; + return result; + } + if (!validArmOptions(command.options(), options_error)) { + result.error = prefix + options_error; + return result; + } + if (!api::ArmFrameType_IsValid(command.frame())) { + result.error = prefix + "MoveL frame is invalid"; + return result; + } + if (command.frame() != api::ARM_FRAME_BASE) { + result.error = prefix + + "ActionQueue MoveL currently supports the Base frame only"; + return result; + } + const auto dof = prepared.arm->getDof(); + if (!command.options().joint_velocity_limits().empty() && + command.options().joint_velocity_limits_size() != + static_cast(dof)) { + result.error = prefix + + "MoveL joint velocity limit size does not match RobotArm DOF"; + return result; + } + const auto& target = command.target(); + if (!finite(target.x()) || !finite(target.y()) || + !finite(target.z()) || !finite(target.rx()) || + !finite(target.ry()) || !finite(target.rz())) { + result.error = prefix + + "MoveL target must contain finite values"; + return result; + } + resources.emplace(prepared.device_id); + break; + } + case api::ActionStep::kAgvNavigateToPose: { + prepared.kind = PreparedStepKind::AgvNavigateToPose; + const auto& command = step.agv_navigate_to_pose(); + prepared.device_id = command.header().device_id(); + if (prepared.device_id.empty()) { + result.error = prefix + "AGV device_id is required"; + return result; + } + prepared.agv = device_manager.getDevice( + prepared.device_id); + if (!prepared.agv) { + result.error = prefix + "AGV device not found: " + + prepared.device_id; + return result; + } + if (!prepared.agv->supportsSynchronousAction( + device::AgvActionKind::NavigateToPose)) { + result.error = prefix + + "AGV backend cannot confirm synchronous pose navigation: " + + prepared.device_id; + return result; + } + if (!validAgvOptions(command.options(), options_error)) { + result.error = prefix + options_error; + return result; + } + if (!finite(command.pose().x()) || + !finite(command.pose().y()) || + !finite(command.pose().theta())) { + result.error = prefix + + "AGV pose must contain finite values"; + return result; + } + resources.emplace(prepared.device_id); + break; + } + case api::ActionStep::kAgvNavigateToStation: { + prepared.kind = PreparedStepKind::AgvNavigateToStation; + const auto& command = step.agv_navigate_to_station(); + prepared.device_id = command.header().device_id(); + if (prepared.device_id.empty()) { + result.error = prefix + "AGV device_id is required"; + return result; + } + if (command.station_id().empty()) { + result.error = prefix + "AGV station_id is required"; + return result; + } + prepared.agv = device_manager.getDevice( + prepared.device_id); + if (!prepared.agv) { + result.error = prefix + "AGV device not found: " + + prepared.device_id; + return result; + } + if (!prepared.agv->supportsSynchronousAction( + device::AgvActionKind::NavigateToStation)) { + result.error = prefix + + "AGV backend cannot confirm synchronous station navigation: " + + prepared.device_id; + return result; + } + if (!validAgvOptions(command.options(), options_error)) { + result.error = prefix + options_error; + return result; + } + resources.emplace(prepared.device_id); + break; + } + case api::ActionStep::kAgvFollowPath: { + prepared.kind = PreparedStepKind::AgvFollowPath; + const auto& command = step.agv_follow_path(); + prepared.device_id = command.header().device_id(); + if (prepared.device_id.empty()) { + result.error = prefix + "AGV device_id is required"; + return result; + } + if (command.path().empty()) { + result.error = prefix + "AGV path must not be empty"; + return result; + } + if (static_cast(command.path_size()) > + kMaxPathSegments) { + result.error = prefix + "AGV path is too large"; + return result; + } + for (const auto& segment : command.path()) { + if (segment.source_station().empty() || + segment.target_station().empty()) { + result.error = prefix + + "AGV path station ids must not be empty"; + return result; + } + } + prepared.agv = device_manager.getDevice( + prepared.device_id); + if (!prepared.agv) { + result.error = prefix + "AGV device not found: " + + prepared.device_id; + return result; + } + if (!prepared.agv->supportsSynchronousAction( + device::AgvActionKind::FollowPath)) { + result.error = prefix + + "AGV backend cannot confirm synchronous path navigation: " + + prepared.device_id; + return result; + } + if (!validAgvOptions(command.options(), options_error)) { + result.error = prefix + options_error; + return result; + } + resources.emplace(prepared.device_id); + break; + } + case api::ActionStep::kDelay: + prepared.kind = PreparedStepKind::Delay; + if (step.delay().duration_ms() > + static_cast(kMaximumTimeout.count())) { + result.error = prefix + + "delay exceeds the server limit"; + return result; + } + break; + case api::ActionStep::COMMAND_NOT_SET: + result.error = prefix + "command is not set"; + return result; + } + result.prepared.steps.push_back(std::move(prepared)); + } + + result.prepared.resource_ids.assign( + resources.begin(), resources.end()); + result.failed_step_index = -1; + result.valid = true; + return result; +} + +device::AgvActionKind toAgvActionKind( + const PreparedStepKind kind) +{ + switch (kind) { + case PreparedStepKind::AgvNavigateToPose: + return device::AgvActionKind::NavigateToPose; + case PreparedStepKind::AgvNavigateToStation: + return device::AgvActionKind::NavigateToStation; + case PreparedStepKind::AgvFollowPath: + return device::AgvActionKind::FollowPath; + default: + return device::AgvActionKind::NavigateToPose; + } +} + +} // namespace + +struct ActionQueueExecutor::Impl { + struct Record { + api::ActionQueueCommand_Request request; + RequestFingerprint fingerprint; + PreparedAction prepared; + Clock::time_point deadline; + std::atomic cancel_requested{false}; + std::atomic timed_out{false}; + // -1 means that no device step is currently inside a backend call. + // StopAll reads this without taking Record::mutex so it can preempt + // the physically active device before stopping the remaining action + // resources. + std::atomic active_step_index{-1}; + // Exact normal leases acquired for this execution. Typed-stop paths + // may convert only these generations into safety barriers, so a stale + // Action can never preempt a later command on the same device. + std::unordered_map + control_tokens; + // Fallback for an invariant or manager failure which prevents an exact + // lease from being converted into a permanent quarantine. + std::atomic retain_control_leases{false}; + std::mutex mutex; + std::condition_variable condition; + bool done{false}; + std::string stop_error; + api::ActionQueueCommand_Feedback feedback; + }; + + struct TerminalResult { + RequestFingerprint fingerprint; + api::ActionQueueCommand_Feedback feedback; + }; + + struct LeaseSet { + explicit LeaseSet(std::shared_ptr owner_record) + : record(std::move(owner_record)) + { + } + + ~LeaseSet() + { + if (record && record->retain_control_leases.load( + std::memory_order_acquire)) { + return; + } + auto& manager = + control::ControlAuthorityManager::instance(); + for (const auto& token : tokens) { + manager.release(token); + } + } + + const control::ControlLeaseToken* find( + const std::string& resource_id) const + { + const auto found = std::find_if( + tokens.begin(), tokens.end(), + [&resource_id](const auto& token) { + return token.resource_id == resource_id; + }); + return found == tokens.end() ? nullptr : &*found; + } + + std::vector tokens; + std::shared_ptr record; + }; + + struct SubmitterGuard { + explicit SubmitterGuard(std::atomic& value) + : count(value) + { + } + + ~SubmitterGuard() + { + count.fetch_sub(1U, std::memory_order_acq_rel); + } + + std::atomic& count; + }; + + struct DeduplicationFeedbackGuard { + ~DeduplicationFeedbackGuard() + { + feedback.set_deduplication_status(status); + } + + api::ActionQueueCommand_Feedback& feedback; + api::ActionDeduplicationStatus& status; + }; + + explicit Impl( + device::DeviceManager& manager, + const std::size_t accepted_action_limit) + : device_manager(manager), + instance_id(generateServiceInstanceId()), + max_accepted_action_ids( + validatedAcceptedActionLimit(accepted_action_limit)), + worker([this]() { workerLoop(); }) + { + } + + ~Impl() + { + shutdown(); + } + + void shutdown() + { + std::shared_ptr active_record; + { + std::lock_guard lock(mutex); + if (joined) { + return; + } + accepting = false; + stopping = true; + for (const auto& record : queue) { + record->cancel_requested.store( + true, std::memory_order_release); + record->condition.notify_all(); + } + active_record = active; + if (active_record) { + active_record->cancel_requested.store( + true, std::memory_order_release); + active_record->condition.notify_all(); + } + } + (void)requestTypedStop(active_record); + queue_condition.notify_all(); + if (worker.joinable()) { + worker.join(); + } + joined = true; + } + + bool cancelAllAndDisable() + { + std::shared_ptr active_record; + { + std::lock_guard lock(mutex); + accepting = false; + for (const auto& record : queue) { + record->cancel_requested.store( + true, std::memory_order_release); + record->condition.notify_all(); + } + active_record = active; + if (active_record) { + active_record->cancel_requested.store( + true, std::memory_order_release); + active_record->condition.notify_all(); + } + } + const bool stopped = requestTypedStop(active_record); + queue_condition.notify_all(); + return stopped; + } + + static bool waitForArmIdle( + const std::shared_ptr& arm, + const Clock::duration timeout) + { + const auto deadline = Clock::now() + timeout; + while (Clock::now() < deadline) { + try { + if (!arm->busy()) { + return true; + } + } catch (...) { + return false; + } + std::this_thread::sleep_for( + kArmIdleConfirmationPollPeriod); + } + try { + return !arm->busy(); + } catch (...) { + return false; + } + } + + bool requestTypedStop(const std::shared_ptr& record) + { + if (!record) { + return true; + } + bool all_stopped = true; + std::unordered_set stopped_arms; + std::unordered_set stopped_agvs; + std::vector stop_order; + stop_order.reserve(record->prepared.steps.size()); + const int active_step_index = + record->active_step_index.load(std::memory_order_acquire); + if (active_step_index >= 0 && + static_cast(active_step_index) < + record->prepared.steps.size()) { + stop_order.push_back( + static_cast(active_step_index)); + } + for (std::size_t index = 0; + index < record->prepared.steps.size(); ++index) { + if (active_step_index >= 0 && + index == static_cast(active_step_index)) { + continue; + } + stop_order.push_back(index); + } + for (const std::size_t index : stop_order) { + const auto& step = record->prepared.steps[index]; + const bool is_arm = step.arm && + stopped_arms.emplace(step.device_id).second; + const bool is_agv = step.agv && + stopped_agvs.emplace(step.device_id).second; + if (!is_arm && !is_agv) { + continue; + } + auto& authority = + control::ControlAuthorityManager::instance(); + const std::string owner = + "grpc-system:action-cancel:" + + record->request.action_id() + ":" + + std::to_string( + sequence.fetch_add( + 1U, std::memory_order_relaxed) + 1U); + const auto ttl = std::chrono::duration_cast< + control::ControlAuthorityManager::Duration>( + std::chrono::minutes(1)); + std::optional expected_token; + { + std::lock_guard lock(record->mutex); + const auto found = + record->control_tokens.find(step.device_id); + if (found != record->control_tokens.end()) { + expected_token = found->second; + } + } + if (!expected_token) { + // Cancellation may race an Action which is still waiting to + // acquire its device set. No Action-owned motion has been + // submitted in that state, so there is nothing to stop. + if (active_step_index >= 0) { + all_stopped = false; + record->retain_control_leases.store( + true, std::memory_order_release); + CMVR_LOG(ERROR) + << "[ActionQueueExecutor] active device has no exact " + "control token; retaining Action leases, id=" + << step.device_id; + } + continue; + } + + control::ControlAcquireResult barrier; + try { + barrier = authority.preemptAcquireIfCurrent( + *expected_token, owner, ttl); + } catch (const std::exception& error) { + all_stopped = false; + (void)authority.quarantineIfCurrent(*expected_token); + record->retain_control_leases.store( + true, std::memory_order_release); + CMVR_LOG(ERROR) + << "[ActionQueueExecutor] could not establish typed stop " + "barrier; control remains quarantined, id=" + << step.device_id << ", error=" << error.what(); + continue; + } catch (...) { + all_stopped = false; + (void)authority.quarantineIfCurrent(*expected_token); + record->retain_control_leases.store( + true, std::memory_order_release); + CMVR_LOG(ERROR) + << "[ActionQueueExecutor] could not establish typed stop " + "barrier; control remains quarantined, id=" + << step.device_id; + continue; + } + if (!barrier.acquired) { + if (authority.quarantineIfCurrent(*expected_token)) { + all_stopped = false; + record->retain_control_leases.store( + true, std::memory_order_release); + CMVR_LOG(ERROR) + << "[ActionQueueExecutor] exact typed stop barrier was " + "not established while the Action lease remained " + "current; control remains quarantined, id=" + << step.device_id + << ", detail=" << barrier.detail; + } + continue; + } + + bool stop_confirmed = false; + try { + if (is_arm) { + const auto result = step.arm->stopMotion(); + stop_confirmed = result.ok(); + if (!stop_confirmed) { + CMVR_LOG(WARNING) + << "[ActionQueueExecutor] typed RobotArm stop failed, id=" + << step.device_id << ", error=" << result.message; + stop_confirmed = waitForArmIdle( + step.arm, + kConcurrentArmStopConfirmationTimeout); + } + } else { + const auto cancel = step.agv->cancelNavigation(); + if (!cancel.ok()) { + CMVR_LOG(WARNING) + << "[ActionQueueExecutor] typed AGV cancel failed, id=" + << step.device_id << ", error=" << cancel.message; + } + const auto velocity_stop = + step.agv->stopVelocityControl(); + if (!velocity_stop.ok() && + velocity_stop.code != + device::AgvErrorCode::UnsupportedCommand) { + CMVR_LOG(WARNING) + << "[ActionQueueExecutor] typed AGV velocity stop failed, id=" + << step.device_id + << ", error=" << velocity_stop.message; + } + const auto stopped = + step.agv->confirmMotionStopped(); + stop_confirmed = stopped.ok(); + if (!stop_confirmed) { + CMVR_LOG(ERROR) + << "[ActionQueueExecutor] AGV stopped state was " + "not confirmed, id=" + << step.device_id + << ", error=" << stopped.message; + } + } + } catch (const std::exception& error) { + CMVR_LOG(ERROR) + << "[ActionQueueExecutor] typed stop threw, id=" + << step.device_id << ", error=" << error.what(); + } catch (...) { + CMVR_LOG(ERROR) + << "[ActionQueueExecutor] typed stop threw, id=" + << step.device_id; + } + if (stop_confirmed) { + authority.release(barrier.token); + } else { + all_stopped = false; + CMVR_LOG(ERROR) + << "[ActionQueueExecutor] typed stop was not confirmed; " + "control remains quarantined, id=" + << step.device_id; + // Retain this safety holder fail-closed. A normal lease must + // not be admitted while the physical outcome is unknown. + } + } + return all_stopped; + } + + static bool waiterCanceled( + const std::function& waiter_canceled) noexcept + { + if (!waiter_canceled) { + return false; + } + try { + return waiter_canceled(); + } catch (...) { + // A broken waiter must not cancel an admitted edge action. End only + // this caller's wait and leave the worker-owned Record untouched. + return true; + } + } + + ActionQueueExecutor::WaitResult submitAndWait( + const api::ActionQueueCommand_Request& request, + api::ActionQueueCommand_Feedback& feedback, + const std::function& waiter_canceled) + { + api::ActionDeduplicationStatus deduplication_status = + api::ACTION_DEDUPLICATION_STATUS_UNSPECIFIED; + DeduplicationFeedbackGuard deduplication_feedback{ + feedback, deduplication_status}; + const auto previous_submitters = concurrent_submitters.fetch_add( + 1U, std::memory_order_acq_rel); + if (previous_submitters >= kMaxConcurrentSubmitters) { + concurrent_submitters.fetch_sub(1U, std::memory_order_acq_rel); + fillTerminalFeedback( + feedback, request.action_id(), + api::ACTION_RESULT_CODE_REJECTED, 0, + "ActionQueue has too many concurrent submitters"); + return ActionQueueExecutor::WaitResult::Terminal; + } + SubmitterGuard submitter_guard(concurrent_submitters); + + const std::string envelope_error = + validateRequestEnvelope(request); + if (!envelope_error.empty()) { + fillTerminalFeedback( + feedback, request.action_id(), + api::ACTION_RESULT_CODE_REJECTED, 0, + envelope_error); + return ActionQueueExecutor::WaitResult::Terminal; + } + if (request.expected_service_instance_id() != instance_id) { + deduplication_status = + api::ACTION_DEDUPLICATION_STATUS_SERVICE_INSTANCE_MISMATCH; + fillTerminalFeedback( + feedback, request.action_id(), + api::ACTION_RESULT_CODE_REJECTED, 0, + "expected_service_instance_id does not match the active ActionQueue service instance; reconcile the prior action before submitting a new id"); + return ActionQueueExecutor::WaitResult::Terminal; + } + const auto fingerprint = fingerprintRequest(request); + if (!fingerprint) { + fillTerminalFeedback( + feedback, request.action_id(), + api::ACTION_RESULT_CODE_REJECTED, 0, + "ActionQueue request could not be serialized"); + return ActionQueueExecutor::WaitResult::Terminal; + } + std::shared_ptr record; + const auto lookup_existing_locked = [&]() { + const auto found = records.find(request.action_id()); + if (found != records.end()) { + if (found->second->fingerprint != *fingerprint) { + deduplication_status = + api::ACTION_DEDUPLICATION_STATUS_ACTION_ID_CONFLICT; + fillTerminalFeedback( + feedback, request.action_id(), + api::ACTION_RESULT_CODE_REJECTED, 0, + "action_id is already associated with a different request"); + return true; + } + record = found->second; + { + std::lock_guard record_lock(record->mutex); + deduplication_status = record->done + ? api::ACTION_DEDUPLICATION_STATUS_CACHED_RESULT + : api::ACTION_DEDUPLICATION_STATUS_JOINED_IN_FLIGHT; + } + return false; + } + + const auto terminal = terminal_results.find(request.action_id()); + if (terminal != terminal_results.end()) { + if (terminal->second.fingerprint != *fingerprint) { + deduplication_status = + api::ACTION_DEDUPLICATION_STATUS_ACTION_ID_CONFLICT; + fillTerminalFeedback( + feedback, request.action_id(), + api::ACTION_RESULT_CODE_REJECTED, 0, + "action_id is already associated with a different request"); + } else { + feedback = terminal->second.feedback; + deduplication_status = + api::ACTION_DEDUPLICATION_STATUS_CACHED_RESULT; + } + return true; + } + + if (retired_action_ids.find(request.action_id()) != + retired_action_ids.end()) { + fillTerminalFeedback( + feedback, request.action_id(), + api::ACTION_RESULT_CODE_REJECTED, 0, + "action_id was already completed but its result is no longer cached; it will not be re-executed"); + deduplication_status = + api::ACTION_DEDUPLICATION_STATUS_RESULT_EVICTED; + return true; + } + return false; + }; + + bool handled = false; + const bool initially_canceled = waiterCanceled(waiter_canceled); + { + std::lock_guard lock(mutex); + handled = lookup_existing_locked(); + } + if (handled) { + return ActionQueueExecutor::WaitResult::Terminal; + } + if (record) { + return waitForRecord(record, feedback, waiter_canceled); + } + if (initially_canceled) { + return ActionQueueExecutor::WaitResult::CanceledBeforeAdmission; + } + + const auto validation = validateRequest(device_manager, request); + if (!validation.valid) { + fillTerminalFeedback( + feedback, request.action_id(), + api::ACTION_RESULT_CODE_REJECTED, 0, + validation.error, + validation.failed_step_index); + return ActionQueueExecutor::WaitResult::Terminal; + } + const auto total_timeout = request.total_timeout_ms() == 0U + ? kDefaultTotalTimeout + : Milliseconds(request.total_timeout_ms()); + auto candidate = std::make_shared(); + candidate->request = request; + candidate->fingerprint = *fingerprint; + candidate->prepared = validation.prepared; + candidate->deadline = Clock::now() + total_timeout; + + const bool canceled_before_admission = + waiterCanceled(waiter_canceled); + bool canceled_without_record = false; + { + std::lock_guard lock(mutex); + handled = lookup_existing_locked(); + if (!handled && !record && canceled_before_admission) { + canceled_without_record = true; + } else if (!handled && !record && (!accepting || stopping)) { + fillTerminalFeedback( + feedback, request.action_id(), + api::ACTION_RESULT_CODE_REJECTED, 0, + "ActionQueue is disabled by a system stop"); + handled = true; + } else if (!handled && !record && + queue.size() + (active ? 1U : 0U) >= + kMaxQueuedActions) { + fillTerminalFeedback( + feedback, request.action_id(), + api::ACTION_RESULT_CODE_REJECTED, 0, + "ActionQueue is full"); + handled = true; + } else if (!handled && !record && + records.size() + terminal_results.size() + + retired_action_ids.size() >= + max_accepted_action_ids) { + fillTerminalFeedback( + feedback, request.action_id(), + api::ACTION_RESULT_CODE_REJECTED, 0, + "ActionQueue idempotency ledger capacity is exhausted; restart with a new service instance only after reconciling prior actions"); + deduplication_status = + api::ACTION_DEDUPLICATION_STATUS_LEDGER_EXHAUSTED; + handled = true; + } else if (!handled && !record) { + record = std::move(candidate); + const auto inserted = + records.emplace(request.action_id(), record); + if (!inserted.second) { + throw std::logic_error( + "ActionQueue admission record already exists"); + } + try { + queue.push_back(record); + } catch (...) { + // No other thread can observe the record while the queue + // mutex is held. Roll it back so a failed deque allocation + // cannot leave an ID which waits forever without work. + records.erase(inserted.first); + record.reset(); + throw; + } + deduplication_status = + api::ACTION_DEDUPLICATION_STATUS_ACCEPTED_NEW; + queue_condition.notify_one(); + } + } + if (handled) { + return ActionQueueExecutor::WaitResult::Terminal; + } + if (record) { + return waitForRecord(record, feedback, waiter_canceled); + } + if (canceled_without_record) { + return ActionQueueExecutor::WaitResult::CanceledBeforeAdmission; + } + return waitForRecord(record, feedback, waiter_canceled); + } + + static ActionQueueExecutor::WaitResult waitForRecord( + const std::shared_ptr& record, + api::ActionQueueCommand_Feedback& feedback, + const std::function& waiter_canceled) + { + std::unique_lock lock(record->mutex); + if (!waiter_canceled) { + record->condition.wait(lock, [&record]() { + return record->done; + }); + feedback = record->feedback; + return ActionQueueExecutor::WaitResult::Terminal; + } + + for (;;) { + if (record->done) { + feedback = record->feedback; + return ActionQueueExecutor::WaitResult::Terminal; + } + lock.unlock(); + if (waiterCanceled(waiter_canceled)) { + return ActionQueueExecutor::WaitResult::CanceledAfterAdmission; + } + lock.lock(); + record->condition.wait_for( + lock, + kWaiterCancellationPollPeriod, + [&record]() { return record->done; }); + } + } + + bool waitForIdle(const Milliseconds timeout) + { + std::unique_lock lock(mutex); + return idle_condition.wait_for(lock, timeout, [this]() { + return queue.empty() && !active; + }); + } + + void workerLoop() + { + for (;;) { + std::shared_ptr record; + { + std::unique_lock lock(mutex); + queue_condition.wait(lock, [this]() { + return stopping || !queue.empty(); + }); + if (stopping && queue.empty()) { + return; + } + record = queue.front(); + queue.pop_front(); + active = record; + } + + try { + execute(record); + } catch (const std::exception& error) { + complete( + record, api::ACTION_RESULT_CODE_FAILED, 0, + std::string("ActionQueue worker failed: ") + + error.what()); + } catch (...) { + complete( + record, api::ACTION_RESULT_CODE_FAILED, 0, + "ActionQueue worker failed with an unknown exception"); + } + { + std::lock_guard lock(mutex); + if (active == record) { + active.reset(); + } + try { + TerminalResult terminal; + terminal.fingerprint = record->fingerprint; + { + std::lock_guard record_lock(record->mutex); + terminal.feedback = record->feedback; + } + const std::string action_id = + record->request.action_id(); + const auto live = records.find(action_id); + terminal_result_order.push_back(action_id); + try { + const auto inserted = terminal_results.emplace( + action_id, std::move(terminal)); + if (!inserted.second) { + throw std::logic_error( + "ActionQueue terminal result already exists"); + } + } catch (...) { + // Keep the full live record as the source of truth if + // the compact cache cannot be committed atomically. + terminal_result_order.pop_back(); + throw; + } + if (live != records.end()) { + records.erase(live); + } + while (terminal_result_order.size() > + kMaxTerminalResults) { + const std::string retired = + terminal_result_order.front(); + // Insert into the exact ledger before dropping the + // cached result. Allocation failure must retain the + // old result rather than create a replay window. + retired_action_ids.emplace(retired); + terminal_results.erase(retired); + terminal_result_order.pop_front(); + } + } catch (const std::exception& error) { + CMVR_LOG(ERROR) + << "[ActionQueueExecutor] could not compact terminal " + "idempotency state; retaining existing state: " + << error.what(); + } catch (...) { + CMVR_LOG(ERROR) + << "[ActionQueueExecutor] could not compact terminal " + "idempotency state; retaining existing state"; + } + idle_condition.notify_all(); + } + } + } + + void complete( + const std::shared_ptr& record, + const api::ActionResultCode result, + const std::uint32_t completed_steps, + const std::string& message, + const int failed_step_index = -1) + { + { + std::lock_guard lock(record->mutex); + fillTerminalFeedback( + record->feedback, + record->request.action_id(), + result, + completed_steps, + message, + failed_step_index); + record->done = true; + } + record->condition.notify_all(); + } + + bool acquireLeases( + const std::shared_ptr& record, + LeaseSet& leases, + std::string& error) + { + auto& authority = + control::ControlAuthorityManager::instance(); + const std::string owner = + "grpc-system:action:" + record->request.action_id() + ":" + + std::to_string( + sequence.fetch_add(1U, std::memory_order_relaxed) + 1U); + const auto ttl = std::chrono::duration_cast< + control::ControlAuthorityManager::Duration>(kControlLeaseTtl); + + while (Clock::now() < record->deadline) { + if (record->cancel_requested.load(std::memory_order_acquire)) { + error = "ActionQueue was canceled before device reservation"; + return false; + } + bool all_acquired = true; + std::string conflict; + for (const auto& resource_id : record->prepared.resource_ids) { + auto acquired = authority.tryAcquire( + resource_id, owner, ttl); + if (!acquired.acquired) { + all_acquired = false; + conflict = std::move(acquired.detail); + break; + } + leases.tokens.push_back(std::move(acquired.token)); + } + if (all_acquired) { + std::lock_guard record_lock(record->mutex); + record->control_tokens.clear(); + for (const auto& token : leases.tokens) { + record->control_tokens.emplace( + token.resource_id, token); + } + return true; + } + for (const auto& token : leases.tokens) { + authority.release(token); + } + leases.tokens.clear(); + error = conflict.empty() + ? "device control is unavailable" + : std::move(conflict); + + std::unique_lock record_lock(record->mutex); + const auto wake_at = std::min( + record->deadline, + Clock::now() + kLeaseRetryPeriod); + record->condition.wait_until( + record_lock, wake_at, [&record]() { + return record->cancel_requested.load( + std::memory_order_acquire); + }); + } + record->timed_out.store(true, std::memory_order_release); + error = "ActionQueue timed out while waiting for device control"; + return false; + } + + bool validateLeases(const LeaseSet& leases) const + { + auto& authority = + control::ControlAuthorityManager::instance(); + return std::all_of( + leases.tokens.begin(), leases.tokens.end(), + [&authority](const auto& token) { + return authority.validate(token); + }); + } + + bool validateDevicesIdle( + const std::shared_ptr& record, + std::string& error) const + { + std::unordered_set inspected_arms; + std::unordered_set inspected_agvs; + for (const auto& step : record->prepared.steps) { + if (step.arm && + inspected_arms.emplace(step.device_id).second && + step.arm->busy()) { + error = "RobotArm already has active motion before ActionQueue execution: " + + step.device_id; + return false; + } + if (!step.agv || + !inspected_agvs.emplace(step.device_id).second) { + continue; + } + const auto runtime = step.agv->runtimeState(); + const auto navigation = step.agv->navigationStatus(); + if (runtime.emergency_stopped || runtime.fault) { + error = "AGV is faulted or emergency-stopped before ActionQueue execution: " + + step.device_id; + return false; + } + if (runtime.moving || + navigation.state == device::AgvTaskState::Waiting || + navigation.state == device::AgvTaskState::Running || + navigation.state == device::AgvTaskState::Paused) { + error = "AGV already has active motion before ActionQueue execution: " + + step.device_id; + return false; + } + } + return true; + } + + Clock::time_point stepDeadline( + const std::shared_ptr& record, + const api::ActionStep& step) const + { + if (step.timeout_ms() == 0U) { + return record->deadline; + } + return std::min( + record->deadline, + Clock::now() + Milliseconds(step.timeout_ms())); + } + + void armTimeoutWatchdog( + const std::shared_ptr& record, + const PreparedStep& step, + const Clock::time_point deadline, + const std::shared_ptr>& disarmed, + const std::shared_ptr>& stop_started) + { + while (!disarmed->load(std::memory_order_acquire)) { + const auto now = Clock::now(); + if (now >= deadline) { + record->timed_out.store(true, std::memory_order_release); + requestTimedOutArmStop(record, step, stop_started); + record->condition.notify_all(); + return; + } + std::this_thread::sleep_for(std::min( + kArmWatchdogPollPeriod, + std::chrono::duration_cast(deadline - now))); + } + } + + void requestTimedOutArmStop( + const std::shared_ptr& record, + const PreparedStep& step, + const std::shared_ptr>& stop_started) + { + bool expected = false; + if (!stop_started->compare_exchange_strong( + expected, true, + std::memory_order_acq_rel, + std::memory_order_acquire)) { + return; + } + auto& authority = + control::ControlAuthorityManager::instance(); + const std::string owner = + "grpc-system:action-timeout:" + + record->request.action_id(); + const auto ttl = std::chrono::duration_cast< + control::ControlAuthorityManager::Duration>( + std::chrono::minutes(1)); + std::optional expected_token; + { + std::lock_guard lock(record->mutex); + const auto found = record->control_tokens.find(step.device_id); + if (found != record->control_tokens.end()) { + expected_token = found->second; + } + } + if (!step.arm || !expected_token) { + record->retain_control_leases.store( + true, std::memory_order_release); + const std::string detail = + "could not establish timed-out RobotArm stop barrier: " + "the active Action lease is unavailable"; + { + std::lock_guard lock(record->mutex); + record->stop_error = detail; + } + CMVR_LOG(ERROR) << "[ActionQueueExecutor] " << detail + << ", id=" << step.device_id; + return; + } + + control::ControlAcquireResult barrier; + try { + barrier = authority.preemptAcquireIfCurrent( + *expected_token, owner, ttl); + } catch (...) { + (void)authority.quarantineIfCurrent(*expected_token); + record->retain_control_leases.store( + true, std::memory_order_release); + throw; + } + if (!barrier.acquired) { + // A direct Stop or another Action stop may already have converted + // our lease. Do not join that barrier and, critically, do not + // preempt a successor which acquired control after it completed. + if (authority.quarantineIfCurrent(*expected_token)) { + record->retain_control_leases.store( + true, std::memory_order_release); + const std::string detail = + "could not establish timed-out RobotArm stop barrier " + "while the Action lease remained current: " + + barrier.detail; + { + std::lock_guard lock(record->mutex); + record->stop_error = detail; + } + CMVR_LOG(ERROR) << "[ActionQueueExecutor] " << detail + << ", id=" << step.device_id; + } + return; + } + bool stop_confirmed = false; + std::string stop_error; + try { + const auto stop = step.arm->stopMotion(); + stop_confirmed = stop.ok(); + if (!stop_confirmed) { + stop_error = stop.message.empty() + ? "RobotArm stop did not confirm idle" + : stop.message; + CMVR_LOG(ERROR) + << "[ActionQueueExecutor] timed-out RobotArm stop failed, id=" + << step.device_id << ", error=" << stop.message; + } + } catch (const std::exception& error) { + stop_error = error.what(); + CMVR_LOG(ERROR) + << "[ActionQueueExecutor] timed-out RobotArm stop threw, id=" + << step.device_id << ", error=" << error.what(); + } catch (...) { + stop_error = "RobotArm stop threw an unknown exception"; + CMVR_LOG(ERROR) + << "[ActionQueueExecutor] timed-out RobotArm stop threw, id=" + << step.device_id; + } + if (!stop_confirmed) { + // A user Stop/StopAll may already own the driver termination state. + // In that case this second stop call is expected to be rejected. + // Keep our safety holder while waiting for the first owner to + // publish the driver's confirmed-idle state; only quarantine if + // that bounded confirmation also fails. + stop_confirmed = waitForArmIdle( + step.arm, + kConcurrentArmStopConfirmationTimeout); + } + if (stop_confirmed) { + authority.release(barrier.token); + } else { + // Deliberately retain the safety barrier when idle was not + // confirmed. Releasing it would allow a new command to overlap an + // unknown physical outcome. Recovery requires an explicit device + // safety procedure or process restart. + std::lock_guard lock(record->mutex); + record->stop_error = + "RobotArm stop was not confirmed; control remains quarantined: " + + stop_error; + } + } + + enum class StepOutcome { + Completed, + Failed, + Canceled, + TimedOut, + }; + + struct StepResult { + StepOutcome outcome{StepOutcome::Failed}; + std::string message; + }; + + StepResult executeArmStep( + const std::shared_ptr& record, + const PreparedStep& prepared, + const api::ActionStep& source, + const control::ControlLeaseToken& token, + const Clock::time_point deadline) + { + auto& authority = + control::ControlAuthorityManager::instance(); + auto cancellation_requested = + [record, token, deadline, &authority]() { + return record->cancel_requested.load( + std::memory_order_acquire) || + Clock::now() >= deadline || + !authority.validate(token); + }; + + auto disarmed = std::make_shared>(false); + auto timeout_stop_started = + std::make_shared>(false); + std::thread watchdog( + [this, record, prepared, deadline, disarmed, + timeout_stop_started]() { + try { + armTimeoutWatchdog( + record, prepared, deadline, disarmed, + timeout_stop_started); + } catch (const std::exception& error) { + try { + std::lock_guard lock(record->mutex); + record->stop_error = + "RobotArm timeout watchdog failed: " + + std::string(error.what()); + } catch (...) { + } + try { + CMVR_LOG(ERROR) + << "[ActionQueueExecutor] RobotArm timeout " + "watchdog failed, id=" + << prepared.device_id + << ", error=" << error.what(); + } catch (...) { + } + } catch (...) { + try { + std::lock_guard lock(record->mutex); + record->stop_error = + "RobotArm timeout watchdog failed with an unknown exception"; + } catch (...) { + } + try { + CMVR_LOG(ERROR) + << "[ActionQueueExecutor] RobotArm timeout " + "watchdog failed, id=" + << prepared.device_id; + } catch (...) { + } + } + }); + + device::Result motion_result; + try { + if (prepared.kind == PreparedStepKind::ArmMoveJ) { + const auto& command = source.arm_move_j(); + device::JointPositionCommand target; + target.position.assign( + command.target().position().begin(), + command.target().position().end()); + auto options = toArmMotionOptions(command.options()); + options.cancellation_requested = cancellation_requested; + motion_result = prepared.arm->moveJ(target, options); + } else { + const auto& command = source.arm_move_l(); + const device::CartesianPose target{ + command.target().x(), command.target().y(), + command.target().z(), command.target().rx(), + command.target().ry(), command.target().rz()}; + auto options = toArmMotionOptions(command.options()); + options.cancellation_requested = cancellation_requested; + motion_result = prepared.arm->moveL( + target, options, toFrameType(command.frame())); + } + } catch (const std::exception& error) { + motion_result = device::Result::failure( + device::ArmErrorCode::CommandFailed, error.what()); + } catch (...) { + motion_result = device::Result::failure( + device::ArmErrorCode::CommandFailed, + "RobotArm command threw an unknown exception"); + } + + const bool expired = + record->timed_out.load(std::memory_order_acquire) || + Clock::now() >= deadline; + if (expired) { + // The device callback can observe the deadline and return before + // the watchdog gets scheduled. Preserve the invariant that every + // timed-out synchronous arm command goes through a typed Stop. + record->timed_out.store(true, std::memory_order_release); + requestTimedOutArmStop( + record, prepared, timeout_stop_started); + } + disarmed->store(true, std::memory_order_release); + if (watchdog.joinable()) { + watchdog.join(); + } + if (expired || + record->timed_out.load(std::memory_order_acquire)) { + std::string message = "RobotArm ActionQueue step timed out"; + { + std::lock_guard lock(record->mutex); + if (!record->stop_error.empty()) { + message += "; " + record->stop_error; + } + } + return {StepOutcome::TimedOut, + std::move(message)}; + } + if (record->cancel_requested.load(std::memory_order_acquire) || + !authority.validate(token)) { + std::string message = + "RobotArm ActionQueue step was canceled or preempted"; + if (!requestTypedStop(record)) { + message += + "; one or more Action devices did not confirm a stopped " + "state and remain quarantined"; + } + return {StepOutcome::Canceled, std::move(message)}; + } + if (!motion_result.ok()) { + std::string message = motion_result.message; + if (!requestTypedStop(record)) { + message += + "; one or more Action devices did not confirm a stopped " + "state and remain quarantined"; + } + return {StepOutcome::Failed, std::move(message)}; + } + return {StepOutcome::Completed, {}}; + } + + StepResult executeAgvStep( + const std::shared_ptr& record, + const PreparedStep& prepared, + const api::ActionStep& source, + const control::ControlLeaseToken& token, + const Clock::time_point deadline) + { + auto& authority = + control::ControlAuthorityManager::instance(); + auto cancellation_requested = + [record, token, deadline, &authority]() { + return record->cancel_requested.load( + std::memory_order_acquire) || + Clock::now() >= deadline || + !authority.validate(token); + }; + + device::AgvResult navigation_result; + try { + if (prepared.kind == PreparedStepKind::AgvNavigateToPose) { + const auto& command = source.agv_navigate_to_pose(); + auto options = toAgvMotionOptions(command.options()); + applyAgvDeadline(options, deadline); + options.cancellation_requested = cancellation_requested; + navigation_result = prepared.agv->navigateToPose( + {command.pose().x(), command.pose().y(), + command.pose().theta()}, + options, + toAgvAdapterParams(command.adapter_params())); + } else if (prepared.kind == + PreparedStepKind::AgvNavigateToStation) { + const auto& command = source.agv_navigate_to_station(); + auto options = toAgvMotionOptions(command.options()); + applyAgvDeadline(options, deadline); + options.cancellation_requested = cancellation_requested; + navigation_result = prepared.agv->navigateToStation( + command.station_id(), + options, + toAgvAdapterParams(command.adapter_params())); + } else { + const auto& command = source.agv_follow_path(); + std::vector path; + path.reserve(static_cast(command.path_size())); + for (const auto& segment : command.path()) { + path.push_back({ + segment.source_station(), + segment.target_station()}); + } + auto options = toAgvMotionOptions(command.options()); + applyAgvDeadline(options, deadline); + options.cancellation_requested = cancellation_requested; + navigation_result = prepared.agv->followPath(path, options); + } + } catch (const std::exception& error) { + navigation_result = device::AgvResult::failure( + device::AgvErrorCode::CommandFailed, error.what()); + } catch (...) { + navigation_result = device::AgvResult::failure( + device::AgvErrorCode::CommandFailed, + "AGV command threw an unknown exception"); + } + + const auto stopOrQuarantine = + [this, &record](std::string message) { + if (!requestTypedStop(record)) { + message += + "; one or more Action devices did not confirm a " + "stopped state and remain quarantined"; + } + return message; + }; + + if (Clock::now() >= deadline || + navigation_result.code == device::AgvErrorCode::Timeout) { + record->timed_out.store(true, std::memory_order_release); + return {StepOutcome::TimedOut, + stopOrQuarantine( + navigation_result.message.empty() + ? "AGV ActionQueue step timed out" + : navigation_result.message)}; + } + if (record->cancel_requested.load(std::memory_order_acquire) || + !authority.validate(token) || + navigation_result.code == device::AgvErrorCode::TaskCanceled) { + return {StepOutcome::Canceled, + stopOrQuarantine( + navigation_result.message.empty() + ? "AGV ActionQueue step was canceled or preempted" + : navigation_result.message)}; + } + if (!navigation_result.ok()) { + return {StepOutcome::Failed, + stopOrQuarantine(navigation_result.message)}; + } + return {StepOutcome::Completed, {}}; + } + + static void applyAgvDeadline( + device::AgvMotionOptions& options, + const Clock::time_point deadline) + { + const auto remaining = std::max( + 1, + std::chrono::duration_cast( + deadline - Clock::now()).count()); + const int bounded = static_cast(std::min( + remaining, + std::numeric_limits::max())); + if (options.wait_timeout_ms <= 0 || + options.wait_timeout_ms > bounded) { + options.wait_timeout_ms = bounded; + } + if (options.poll_interval_ms > options.wait_timeout_ms) { + options.poll_interval_ms = options.wait_timeout_ms; + } + } + + StepResult executeDelay( + const std::shared_ptr& record, + const api::ActionStep& step, + const Clock::time_point deadline) + { + const auto delay_deadline = std::min( + deadline, + Clock::now() + Milliseconds(step.delay().duration_ms())); + std::unique_lock lock(record->mutex); + const bool canceled = record->condition.wait_until( + lock, delay_deadline, [&record]() { + return record->cancel_requested.load( + std::memory_order_acquire); + }); + if (canceled) { + return {StepOutcome::Canceled, + "ActionQueue delay was canceled"}; + } + if (Clock::now() >= deadline) { + record->timed_out.store(true, std::memory_order_release); + return {StepOutcome::TimedOut, + "ActionQueue delay timed out"}; + } + return {StepOutcome::Completed, {}}; + } + + void execute(const std::shared_ptr& record) + { + if (record->cancel_requested.load(std::memory_order_acquire)) { + complete(record, api::ACTION_RESULT_CODE_CANCELED, 0, + "ActionQueue was canceled before execution"); + return; + } + if (Clock::now() >= record->deadline) { + complete(record, api::ACTION_RESULT_CODE_TIMED_OUT, 0, + "ActionQueue expired while waiting in the queue"); + return; + } + + LeaseSet leases(record); + std::string lease_error; + if (!acquireLeases(record, leases, lease_error)) { + if (record->timed_out.load(std::memory_order_acquire)) { + complete(record, api::ACTION_RESULT_CODE_TIMED_OUT, 0, + lease_error); + } else { + complete(record, api::ACTION_RESULT_CODE_CANCELED, 0, + lease_error); + } + return; + } + if (!validateLeases(leases)) { + complete(record, api::ACTION_RESULT_CODE_CANCELED, 0, + "ActionQueue device control was preempted before execution"); + return; + } + + std::string dynamic_error; + if (!validateDevicesIdle(record, dynamic_error)) { + complete(record, api::ACTION_RESULT_CODE_REJECTED, 0, + dynamic_error); + return; + } + + std::uint32_t completed_steps = 0; + for (int index = 0; index < record->request.steps_size(); ++index) { + if (record->cancel_requested.load(std::memory_order_acquire) || + !validateLeases(leases)) { + complete( + record, api::ACTION_RESULT_CODE_CANCELED, + completed_steps, + "ActionQueue was canceled or device control was preempted"); + return; + } + if (Clock::now() >= record->deadline) { + complete( + record, api::ACTION_RESULT_CODE_TIMED_OUT, + completed_steps, + "ActionQueue total timeout elapsed"); + return; + } + + const auto& source = record->request.steps(index); + const auto& prepared = record->prepared.steps[ + static_cast(index)]; + const auto deadline = stepDeadline(record, source); + StepResult step_result; + if (prepared.kind == PreparedStepKind::Delay) { + record->active_step_index.store( + -1, std::memory_order_release); + step_result = executeDelay(record, source, deadline); + } else { + const auto* token = leases.find(prepared.device_id); + if (!token) { + complete( + record, api::ACTION_RESULT_CODE_FAILED, + completed_steps, + "ActionQueue internal device reservation is missing", + index); + return; + } + if (prepared.arm) { + record->active_step_index.store( + index, std::memory_order_release); + step_result = executeArmStep( + record, prepared, source, *token, deadline); + } else { + if (!prepared.agv->supportsSynchronousAction( + toAgvActionKind(prepared.kind))) { + complete( + record, api::ACTION_RESULT_CODE_REJECTED, + completed_steps, + "AGV ActionQueue capability changed before execution", + index); + return; + } + record->active_step_index.store( + index, std::memory_order_release); + step_result = executeAgvStep( + record, prepared, source, *token, deadline); + } + record->active_step_index.store( + -1, std::memory_order_release); + } + + switch (step_result.outcome) { + case StepOutcome::Completed: + ++completed_steps; + break; + case StepOutcome::Failed: + complete( + record, api::ACTION_RESULT_CODE_FAILED, + completed_steps, step_result.message, index); + return; + case StepOutcome::Canceled: + complete( + record, api::ACTION_RESULT_CODE_CANCELED, + completed_steps, step_result.message, index); + return; + case StepOutcome::TimedOut: + complete( + record, api::ACTION_RESULT_CODE_TIMED_OUT, + completed_steps, step_result.message, index); + return; + } + } + + complete(record, api::ACTION_RESULT_CODE_COMPLETED, + completed_steps, {}); + CMVR_LOG(DEBUG) + << "[ActionQueueExecutor] action completed, id=" + << record->request.action_id() + << ", steps=" << completed_steps; + } + + device::DeviceManager& device_manager; + const std::string instance_id; + const std::size_t max_accepted_action_ids; + std::mutex mutex; + std::condition_variable queue_condition; + std::condition_variable idle_condition; + std::deque> queue; + std::unordered_map> records; + std::unordered_map terminal_results; + std::deque terminal_result_order; + std::unordered_set retired_action_ids; + std::shared_ptr active; + bool accepting{true}; + bool stopping{false}; + bool joined{false}; + std::atomic sequence{0}; + std::atomic concurrent_submitters{0}; + std::thread worker; +}; + +ActionQueueExecutor::ActionQueueExecutor( + device::DeviceManager& device_manager, + const std::size_t max_accepted_action_ids) + : impl_(std::make_unique( + device_manager, max_accepted_action_ids)) +{ +} + +ActionQueueExecutor::~ActionQueueExecutor() = default; + +ActionQueueExecutor::WaitResult ActionQueueExecutor::submitAndWait( + const api::ActionQueueCommand_Request& request, + api::ActionQueueCommand_Feedback& feedback, + const std::function& waiter_canceled) +{ + const auto result = + impl_->submitAndWait(request, feedback, waiter_canceled); + feedback.set_service_instance_id(impl_->instance_id); + return result; +} + +bool ActionQueueExecutor::cancelAllAndDisable() +{ + return impl_->cancelAllAndDisable(); +} + +bool ActionQueueExecutor::waitForIdle( + const std::chrono::milliseconds timeout) +{ + return impl_->waitForIdle(timeout); +} + +const std::string& ActionQueueExecutor::instanceId() const noexcept +{ + return impl_->instance_id; +} + +} // namespace cmvr::service diff --git a/cmvr-es/service/grpc/include/grpc_system_service.h b/cmvr-es/service/grpc/include/grpc_system_service.h index 978ca2f4..f579aba6 100644 --- a/cmvr-es/service/grpc/include/grpc_system_service.h +++ b/cmvr-es/service/grpc/include/grpc_system_service.h @@ -5,23 +5,33 @@ #ifndef GRPC_SYSTEM_SERVICE_H #define GRPC_SYSTEM_SERVICE_H +#include + #include "cmvr/api/system_service.grpc.pb.h" #include "common/base/grpc_utils.h" #include "manager/device_manager/include/device_manager.h" namespace cmvr::service { + class ActionQueueExecutor; + class gRPCSystemServiceImpl: public api::SystemService::Service { public: gRPCSystemServiceImpl(); - ~gRPCSystemServiceImpl() override = default; + ~gRPCSystemServiceImpl() override; + // Called by GrpcServerTask before grpc::Server::Shutdown so accepted + // ActionQueue handlers can reach a terminal result and do not hold the + // synchronous server shutdown open indefinitely. + void prepareForShutdown(); grpc::Status GetSystemInfo(grpc::ServerContext* context, const api::GetSystemInfoCommand_Request* request, api::GetSystemInfoCommand_Feedback* response) override; grpc::Status GetSystemStatus(grpc::ServerContext* context, const api::GetSystemStatusCommand_Request* request, api::GetSystemStatusCommand_Feedback* response) override; grpc::Status GetDeviceList(grpc::ServerContext* context, const api::GetDeviceListCommand_Request* request, api::GetDeviceListCommand_Feedback* response) override; grpc::Status UpdateParams(grpc::ServerContext* context, const cmvr::api::UpdateParamsCommand_Request* request, cmvr::api::UpdateParamsCommand_Feedback* response) override; grpc::Status StopAll(grpc::ServerContext* context, const cmvr::api::StopAllCommand_Request* request, cmvr::api::StopAllCommand_Feedback* response) override; + grpc::Status ExecuteActionQueue(grpc::ServerContext* context, const cmvr::api::ActionQueueCommand_Request* request, cmvr::api::ActionQueueCommand_Feedback* response) override; private: device::DeviceManager& dmgr_; + std::unique_ptr action_queue_; }; } diff --git a/cmvr-es/service/grpc/src/grpc_agv_service.cpp b/cmvr-es/service/grpc/src/grpc_agv_service.cpp index 4615e83c..b905593f 100644 --- a/cmvr-es/service/grpc/src/grpc_agv_service.cpp +++ b/cmvr-es/service/grpc/src/grpc_agv_service.cpp @@ -1,12 +1,18 @@ #include "service/grpc/include/grpc_agv_service.h" +#include +#include #include #include #include +#include #include #include +#include "common/base/logging/logger.h" +#include "manager/control_authority/include/control_authority_manager.h" + using google::protobuf::util::TimeUtil; namespace cmvr::service { @@ -75,6 +81,114 @@ grpc::Status setDeviceNotFound(api::CommandHeader_Feedback* response, const std: return grpc::Status(grpc::StatusCode::NOT_FOUND, message); } +grpc::Status setControlLeaseConflict( + api::CommandHeader_Feedback* response, + const std::string& device_id, + const std::string& detail) +{ + const std::string message = + "AGV control is leased by another active control operation: " + + device_id; + if (!detail.empty()) { + CMVR_LOG(WARNING) << "[gRPCAgvServiceImpl] control lease conflict, id=" + << device_id << ", detail=" << detail; + } + fillFeedback(response, false, message); + return grpc::Status( + grpc::StatusCode::FAILED_PRECONDITION, message); +} + +template +grpc::Status setControlLeaseConflict( + Response* response, + const std::string& device_id, + const std::string& detail) +{ + return setControlLeaseConflict( + response->mutable_header(), device_id, detail); +} + +class ScopedUnaryAgvControlLease final { +public: + ScopedUnaryAgvControlLease( + const std::string& device_id, + const char* operation, + const bool preemptive = false) + : manager_(control::ControlAuthorityManager::instance()) + { + static std::atomic sequence{0}; + const std::string owner = + std::string(preemptive + ? "grpc-agv-safety:" + : "grpc-agv-unary:") + + operation + ":" + + std::to_string( + sequence.fetch_add( + 1U, std::memory_order_relaxed) + + 1U); + const auto ttl = std::chrono::duration_cast< + control::ControlAuthorityManager::Duration>( + std::chrono::hours(24)); + auto acquired = preemptive + ? manager_.preemptAcquire(device_id, owner, ttl) + : manager_.tryAcquire(device_id, owner, ttl); + acquired_ = acquired.acquired; + token_ = std::move(acquired.token); + detail_ = std::move(acquired.detail); + } + + ~ScopedUnaryAgvControlLease() + { + if (release_on_destroy_) { + manager_.release(token_); + } + } + + bool acquired() const noexcept { return acquired_; } + const std::string& detail() const noexcept { return detail_; } + + // Unknown physical outcomes stay fail-closed until an explicit device + // safety procedure or process restart clears the retained holder. + void quarantine() noexcept { release_on_destroy_ = false; } + +private: + control::ControlAuthorityManager& manager_; + control::ControlLeaseToken token_; + std::string detail_; + bool acquired_{false}; + bool release_on_destroy_{true}; +}; + +template +grpc::Status executeConfirmedAgvStop( + Response* response, + const std::shared_ptr& agv, + ScopedUnaryAgvControlLease& control_barrier, + const char* operation_name, + Operation&& operation) +{ + try { + const auto command_result = operation(); + const auto stopped = agv->confirmMotionStopped(); + if (!stopped.ok()) { + control_barrier.quarantine(); + std::string message = std::string(operation_name) + + " did not reach a confirmed stopped state: " + + stopped.message; + if (!command_result.ok()) { + message += "; command_result=" + command_result.message; + } + return setResponseResult( + response, + device::AgvResult::failure(stopped.code, message)); + } + return setResponseResult(response, command_result); + } catch (...) { + control_barrier.quarantine(); + throw; + } +} + template grpc::Status setNavigationRequestCanceled(Response* response) { @@ -409,11 +523,20 @@ grpc::Status gRPCAgvServiceImpl::emergencyStop(grpc::ServerContext*, api::CommandHeader_Feedback* response) { try { - auto agv = dmgr_.getDevice(request->device_id()); + const std::string device_id = request->device_id(); + auto agv = dmgr_.getDevice(device_id); if (!agv) { - return setDeviceNotFound(response, request->device_id()); + return setDeviceNotFound(response, device_id); } - return setResponseResult(response, agv->emergencyStop()); + ScopedUnaryAgvControlLease control_barrier( + device_id, "emergencyStop", true); + if (!control_barrier.acquired()) { + return setControlLeaseConflict( + response, device_id, control_barrier.detail()); + } + return executeConfirmedAgvStop( + response, agv, control_barrier, "emergencyStop", + [&agv]() { return agv->emergencyStop(); }); } catch (const std::exception& e) { fillFeedback(response, false, e.what()); return grpc::Status(grpc::StatusCode::INTERNAL, e.what()); @@ -425,9 +548,16 @@ grpc::Status gRPCAgvServiceImpl::clearFault(grpc::ServerContext*, api::CommandHeader_Feedback* response) { try { - auto agv = dmgr_.getDevice(request->device_id()); + const std::string device_id = request->device_id(); + auto agv = dmgr_.getDevice(device_id); if (!agv) { - return setDeviceNotFound(response, request->device_id()); + return setDeviceNotFound(response, device_id); + } + ScopedUnaryAgvControlLease control_lease( + device_id, "clearFault"); + if (!control_lease.acquired()) { + return setControlLeaseConflict( + response, device_id, control_lease.detail()); } return setResponseResult(response, agv->clearFault()); } catch (const std::exception& e) { @@ -449,6 +579,12 @@ grpc::Status gRPCAgvServiceImpl::navigateToPose(grpc::ServerContext* context, if (!agv) { return setDeviceNotFound(response, device_id); } + ScopedUnaryAgvControlLease control_lease( + device_id, "navigateToPose"); + if (!control_lease.acquired()) { + return setControlLeaseConflict( + response, device_id, control_lease.detail()); + } return setResponseResult(response, agv->navigateToPose( toPose2d(request->pose()), toMotionOptions(request->options(), context), @@ -472,6 +608,12 @@ grpc::Status gRPCAgvServiceImpl::navigateToStation(grpc::ServerContext* context, if (!agv) { return setDeviceNotFound(response, device_id); } + ScopedUnaryAgvControlLease control_lease( + device_id, "navigateToStation"); + if (!control_lease.acquired()) { + return setControlLeaseConflict( + response, device_id, control_lease.detail()); + } return setResponseResult(response, agv->navigateToStation( request->station_id(), toMotionOptions(request->options(), context), @@ -495,6 +637,12 @@ grpc::Status gRPCAgvServiceImpl::followPath(grpc::ServerContext* context, if (!agv) { return setDeviceNotFound(response, device_id); } + ScopedUnaryAgvControlLease control_lease( + device_id, "followPath"); + if (!control_lease.acquired()) { + return setControlLeaseConflict( + response, device_id, control_lease.detail()); + } std::vector path; path.reserve(static_cast(request->path_size())); for (const auto& segment : request->path()) { @@ -527,6 +675,12 @@ grpc::Status gRPCAgvServiceImpl::translate( if (!agv) { return setDeviceNotFound(response, device_id); } + ScopedUnaryAgvControlLease control_lease( + device_id, "translate"); + if (!control_lease.acquired()) { + return setControlLeaseConflict( + response, device_id, control_lease.detail()); + } return setResponseResult( response, agv->translate(toTranslation(request->translation()))); @@ -545,9 +699,16 @@ grpc::Status gRPCAgvServiceImpl::pauseNavigation(grpc::ServerContext*, api::CommandHeader_Feedback* response) { try { - auto agv = dmgr_.getDevice(request->device_id()); + const std::string device_id = request->device_id(); + auto agv = dmgr_.getDevice(device_id); if (!agv) { - return setDeviceNotFound(response, request->device_id()); + return setDeviceNotFound(response, device_id); + } + ScopedUnaryAgvControlLease control_lease( + device_id, "pauseNavigation"); + if (!control_lease.acquired()) { + return setControlLeaseConflict( + response, device_id, control_lease.detail()); } return setResponseResult(response, agv->pauseNavigation()); } catch (const std::exception& e) { @@ -561,9 +722,16 @@ grpc::Status gRPCAgvServiceImpl::resumeNavigation(grpc::ServerContext*, api::CommandHeader_Feedback* response) { try { - auto agv = dmgr_.getDevice(request->device_id()); + const std::string device_id = request->device_id(); + auto agv = dmgr_.getDevice(device_id); if (!agv) { - return setDeviceNotFound(response, request->device_id()); + return setDeviceNotFound(response, device_id); + } + ScopedUnaryAgvControlLease control_lease( + device_id, "resumeNavigation"); + if (!control_lease.acquired()) { + return setControlLeaseConflict( + response, device_id, control_lease.detail()); } return setResponseResult(response, agv->resumeNavigation()); } catch (const std::exception& e) { @@ -577,11 +745,20 @@ grpc::Status gRPCAgvServiceImpl::cancelNavigation(grpc::ServerContext*, api::CommandHeader_Feedback* response) { try { - auto agv = dmgr_.getDevice(request->device_id()); + const std::string device_id = request->device_id(); + auto agv = dmgr_.getDevice(device_id); if (!agv) { - return setDeviceNotFound(response, request->device_id()); + return setDeviceNotFound(response, device_id); } - return setResponseResult(response, agv->cancelNavigation()); + ScopedUnaryAgvControlLease control_barrier( + device_id, "cancelNavigation", true); + if (!control_barrier.acquired()) { + return setControlLeaseConflict( + response, device_id, control_barrier.detail()); + } + return executeConfirmedAgvStop( + response, agv, control_barrier, "cancelNavigation", + [&agv]() { return agv->cancelNavigation(); }); } catch (const std::exception& e) { fillFeedback(response, false, e.what()); return grpc::Status(grpc::StatusCode::INTERNAL, e.what()); @@ -598,6 +775,12 @@ grpc::Status gRPCAgvServiceImpl::setVelocity(grpc::ServerContext*, if (!agv) { return setDeviceNotFound(response, device_id); } + ScopedUnaryAgvControlLease control_lease( + device_id, "setVelocity"); + if (!control_lease.acquired()) { + return setControlLeaseConflict( + response, device_id, control_lease.detail()); + } return setResponseResult(response, agv->setVelocity(toVelocity(request->velocity()))); } catch (const std::exception& e) { fillFeedback(response->mutable_header(), false, e.what()); @@ -610,11 +793,20 @@ grpc::Status gRPCAgvServiceImpl::stopVelocityControl(grpc::ServerContext*, api::CommandHeader_Feedback* response) { try { - auto agv = dmgr_.getDevice(request->device_id()); + const std::string device_id = request->device_id(); + auto agv = dmgr_.getDevice(device_id); if (!agv) { - return setDeviceNotFound(response, request->device_id()); + return setDeviceNotFound(response, device_id); } - return setResponseResult(response, agv->stopVelocityControl()); + ScopedUnaryAgvControlLease control_barrier( + device_id, "stopVelocityControl", true); + if (!control_barrier.acquired()) { + return setControlLeaseConflict( + response, device_id, control_barrier.detail()); + } + return executeConfirmedAgvStop( + response, agv, control_barrier, "stopVelocityControl", + [&agv]() { return agv->stopVelocityControl(); }); } catch (const std::exception& e) { fillFeedback(response, false, e.what()); return grpc::Status(grpc::StatusCode::INTERNAL, e.what()); @@ -679,6 +871,12 @@ grpc::Status gRPCAgvServiceImpl::switchMap(grpc::ServerContext*, if (!agv) { return setDeviceNotFound(response, device_id); } + ScopedUnaryAgvControlLease control_lease( + device_id, "switchMap"); + if (!control_lease.acquired()) { + return setControlLeaseConflict( + response, device_id, control_lease.detail()); + } return setResponseResult(response, agv->switchMap(request->map_name())); } catch (const std::exception& e) { fillFeedback(response->mutable_header(), false, e.what()); @@ -696,6 +894,12 @@ grpc::Status gRPCAgvServiceImpl::uploadMap(grpc::ServerContext*, if (!agv) { return setDeviceNotFound(response, device_id); } + ScopedUnaryAgvControlLease control_lease( + device_id, "uploadMap"); + if (!control_lease.acquired()) { + return setControlLeaseConflict( + response, device_id, control_lease.detail()); + } return setResponseResult(response, agv->uploadMap(request->map_name(), request->content())); } catch (const std::exception& e) { fillFeedback(response->mutable_header(), false, e.what()); @@ -735,6 +939,12 @@ grpc::Status gRPCAgvServiceImpl::startMapping(grpc::ServerContext*, if (!agv) { return setDeviceNotFound(response, device_id); } + ScopedUnaryAgvControlLease control_lease( + device_id, "startMapping"); + if (!control_lease.acquired()) { + return setControlLeaseConflict( + response, device_id, control_lease.detail()); + } device::AgvMappingOptions options; options.dimension = toMapDimension(request->dimension()); options.map_name = request->map_name(); @@ -825,9 +1035,16 @@ grpc::Status gRPCAgvServiceImpl::stopMapping(grpc::ServerContext*, api::CommandHeader_Feedback* response) { try { - auto agv = dmgr_.getDevice(request->device_id()); + const std::string device_id = request->device_id(); + auto agv = dmgr_.getDevice(device_id); if (!agv) { - return setDeviceNotFound(response, request->device_id()); + return setDeviceNotFound(response, device_id); + } + ScopedUnaryAgvControlLease control_lease( + device_id, "stopMapping"); + if (!control_lease.acquired()) { + return setControlLeaseConflict( + response, device_id, control_lease.detail()); } return setResponseResult(response, agv->stopMapping()); } catch (const std::exception& e) { diff --git a/cmvr-es/service/grpc/src/grpc_arm_service.cpp b/cmvr-es/service/grpc/src/grpc_arm_service.cpp index a8fb5622..ed635ce3 100644 --- a/cmvr-es/service/grpc/src/grpc_arm_service.cpp +++ b/cmvr-es/service/grpc/src/grpc_arm_service.cpp @@ -175,21 +175,26 @@ public: acquired_ = acquired.acquired; token_ = std::move(acquired.token); detail_ = std::move(acquired.detail); + release_on_destroy_ = !preemptive; } ~ScopedUnaryControlLease() { - manager_.release(token_); + if (release_on_destroy_) { + manager_.release(token_); + } } bool acquired() const noexcept { return acquired_; } const std::string& detail() const noexcept { return detail_; } + void confirmSafeToRelease() noexcept { release_on_destroy_ = true; } private: control::ControlAuthorityManager& manager_; control::ControlLeaseToken token_; std::string detail_; bool acquired_{false}; + bool release_on_destroy_{true}; }; } // namespace @@ -216,6 +221,9 @@ grpc::Status gRPCArmServiceImpl::torqueOff(grpc::ServerContext*, response, device_id, control_barrier.detail()); } const auto result = arm->torqueOff(); + if (result.ok()) { + control_barrier.confirmSafeToRelease(); + } fillFeedback(response, result.ok(), result.ok() ? "" : result.message); if (result.ok()) { logRpcSuccess("torqueOff", device_id); @@ -424,6 +432,9 @@ grpc::Status gRPCArmServiceImpl::stopMotion(grpc::ServerContext*, response, device_id, control_barrier.detail()); } const auto result = arm->stopMotion(); + if (result.ok()) { + control_barrier.confirmSafeToRelease(); + } fillFeedback(response, result.ok(), result.ok() ? "" : result.message); if (result.ok()) { logRpcSuccess("stopMotion", device_id); diff --git a/cmvr-es/service/grpc/src/grpc_system_service.cpp b/cmvr-es/service/grpc/src/grpc_system_service.cpp index 87c592cc..f67a72c9 100644 --- a/cmvr-es/service/grpc/src/grpc_system_service.cpp +++ b/cmvr-es/service/grpc/src/grpc_system_service.cpp @@ -12,7 +12,10 @@ #include #include "common/base/logging/logger.h" +#include "devices/agv/abstract_agv.h" +#include "devices/arm/robot_arm.h" #include "manager/control_authority/include/control_authority_manager.h" +#include "service/action/include/action_queue_executor.h" using namespace cmvr::device; using namespace cmvr::service; @@ -25,8 +28,10 @@ public: { auto& authority = cmvr::control::ControlAuthorityManager::instance(); - for (const auto& token : tokens_) { - authority.release(token); + for (const auto& barrier : barriers_) { + if (barrier.release_on_destroy) { + authority.release(barrier.token); + } } } @@ -49,18 +54,44 @@ public: detail = result.detail; return false; } - try { - tokens_.push_back(result.token); - } catch (...) { - cmvr::control::ControlAuthorityManager::instance().release( - result.token); - throw; - } + // StopAll barriers default to fail-closed. If retaining the token in + // this local vector throws, deliberately leave the manager-side safety + // holder installed: releasing it would reopen control after StopAll + // already preempted an in-flight command. + barriers_.push_back({result.token, false}); return true; } + void quarantine(const std::string& device_id) + { + for (auto& barrier : barriers_) { + if (barrier.token.resource_id == device_id) { + barrier.release_on_destroy = false; + } + } + } + + void quarantineAll() + { + for (auto& barrier : barriers_) { + barrier.release_on_destroy = false; + } + } + + void confirmSafeToReleaseAll() + { + for (auto& barrier : barriers_) { + barrier.release_on_destroy = true; + } + } + private: - std::vector tokens_; + struct Barrier { + cmvr::control::ControlLeaseToken token; + bool release_on_destroy{false}; + }; + + std::vector barriers_; }; std::uint64_t unixTimeMs() noexcept @@ -154,7 +185,18 @@ cmvr::api::SystemDeviceHealth toApiDeviceHealth( } // namespace -gRPCSystemServiceImpl::gRPCSystemServiceImpl(): dmgr_(DeviceManager::getInstance()) {} +gRPCSystemServiceImpl::gRPCSystemServiceImpl() + : dmgr_(DeviceManager::getInstance()), + action_queue_(std::make_unique(dmgr_)) +{ +} + +gRPCSystemServiceImpl::~gRPCSystemServiceImpl() = default; + +void gRPCSystemServiceImpl::prepareForShutdown() +{ + (void)action_queue_->cancelAllAndDisable(); +} grpc::Status gRPCSystemServiceImpl::GetSystemInfo(grpc::ServerContext* context, const api::GetSystemInfoCommand_Request* request, api::GetSystemInfoCommand_Feedback* response) @@ -162,6 +204,8 @@ grpc::Status gRPCSystemServiceImpl::GetSystemInfo(grpc::ServerContext* context, try { response->set_version(dmgr_.version()); response->set_system_name(dmgr_.name()); + response->set_action_service_instance_id( + action_queue_->instanceId()); response->mutable_header()->set_success(true); setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); CMVR_LOG(DEBUG) << "[gRPCSystemServiceImpl] (GetSystemInfo): success, name=" @@ -287,27 +331,132 @@ grpc::Status gRPCSystemServiceImpl::UpdateParams(grpc::ServerContext* context, c grpc::Status gRPCSystemServiceImpl::StopAll(grpc::ServerContext* context, const cmvr::api::StopAllCommand_Request* request, cmvr::api::StopAllCommand_Feedback* response) { + (void)context; + (void)request; try { const auto snapshot = dmgr_.snapshot(); ScopedControlBarrierSet control_barriers; for (const auto& device : snapshot.devices) { - if (device.kind == cmvr::device::DeviceKind::Arm) { + if (device.kind == cmvr::device::DeviceKind::Arm || + device.kind == cmvr::device::DeviceKind::AGV) { std::string detail; if (!control_barriers.acquire(device.id, detail)) { CMVR_LOG(WARNING) << "[gRPCSystemServiceImpl] (StopAll): failed to " - "acquire arm safety barrier, id=" + "acquire device safety barrier, id=" << device.id << ", detail=" << detail; throw std::runtime_error( - "StopAll could not acquire the RobotArm safety " + "StopAll could not acquire the device safety " "barrier: " + device.id); } } } + const bool action_stop_confirmed = + action_queue_->cancelAllAndDisable(); + if (!action_stop_confirmed) { + control_barriers.quarantineAll(); + } + std::vector unconfirmed_devices; + for (const auto& device : snapshot.devices) { + if (device.kind != cmvr::device::DeviceKind::Arm) { + continue; + } + auto arm = dmgr_.getDevice(device.id); + if (!arm) { + continue; + } + try { + const auto stopped = arm->stopMotion(); + if (!stopped.ok()) { + control_barriers.quarantine(device.id); + unconfirmed_devices.push_back( + device.id + ": " + stopped.message); + } + } catch (const std::exception& error) { + control_barriers.quarantine(device.id); + unconfirmed_devices.push_back( + device.id + ": stop threw: " + error.what()); + } catch (...) { + control_barriers.quarantine(device.id); + unconfirmed_devices.push_back( + device.id + ": stop threw an unknown exception"); + } + } + + // AbstractAGV::stop() is a lifecycle hook and some backends do not + // map it to a motion stop. Use the typed non-E-stop controls here; + // StopAll must not be silently upgraded to emergencyStop semantics. + for (const auto& device : snapshot.devices) { + if (device.kind != cmvr::device::DeviceKind::AGV) { + continue; + } + auto agv = dmgr_.getDevice( + device.id); + if (!agv) { + continue; + } + try { + const auto cancel_result = agv->cancelNavigation(); + if (!cancel_result.ok() && + cancel_result.code != + cmvr::device::AgvErrorCode::UnsupportedCommand) { + CMVR_LOG(WARNING) + << "[gRPCSystemServiceImpl] (StopAll): AGV navigation " + "cancel failed, id=" + << device.id << ", detail=" << cancel_result.message; + } + const auto velocity_stop = agv->stopVelocityControl(); + if (!velocity_stop.ok() && + velocity_stop.code != + cmvr::device::AgvErrorCode::UnsupportedCommand) { + CMVR_LOG(WARNING) + << "[gRPCSystemServiceImpl] (StopAll): AGV velocity " + "stop failed, id=" + << device.id << ", detail=" + << velocity_stop.message; + } + const auto stopped = agv->confirmMotionStopped(); + if (!stopped.ok()) { + control_barriers.quarantine(device.id); + unconfirmed_devices.push_back( + device.id + ": " + stopped.message); + } + } catch (const std::exception& error) { + control_barriers.quarantine(device.id); + unconfirmed_devices.push_back( + device.id + ": stop confirmation threw: " + + error.what()); + } catch (...) { + control_barriers.quarantine(device.id); + unconfirmed_devices.push_back( + device.id + + ": stop confirmation threw an unknown exception"); + } + } + if (!action_queue_->waitForIdle(std::chrono::seconds(15))) { + control_barriers.quarantineAll(); + throw std::runtime_error( + "StopAll timed out waiting for ActionQueue to become idle"); + } + if (!action_stop_confirmed) { + throw std::runtime_error( + "StopAll could not confirm that every active ActionQueue " + "device stopped; affected control resources remain " + "quarantined"); + } + if (!unconfirmed_devices.empty()) { + throw std::runtime_error( + "StopAll could not confirm that every device stopped; " + "affected control resources remain quarantined: " + + unconfirmed_devices.front()); + } + // Do not close device transports while the Action worker may still be + // unwinding a synchronous driver call. dmgr_.stop(); response->mutable_header()->set_success(true); setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); CMVR_LOG(DEBUG) << "[gRPCSystemServiceImpl] (StopAll): success"; + control_barriers.confirmSafeToReleaseAll(); return grpc::Status::OK; } catch (std::exception& e) { @@ -317,3 +466,49 @@ grpc::Status gRPCSystemServiceImpl::StopAll(grpc::ServerContext* context, return grpc::Status::OK; } } + +grpc::Status gRPCSystemServiceImpl::ExecuteActionQueue( + grpc::ServerContext* context, + const cmvr::api::ActionQueueCommand_Request* request, + cmvr::api::ActionQueueCommand_Feedback* response) +{ + if (!request || !response) { + return grpc::Status( + grpc::StatusCode::INVALID_ARGUMENT, + "ActionQueue request and response are required"); + } + try { + // The callback is consumed only on this synchronous handler stack. It + // is never retained by the worker-owned action Record, so returning the + // RPC cannot leave a dangling ServerContext reference. + const auto wait_result = action_queue_->submitAndWait( + *request, + *response, + [context]() { + return context && context->IsCancelled(); + }); + if (wait_result == + ActionQueueExecutor::WaitResult::CanceledBeforeAdmission) { + return grpc::Status( + grpc::StatusCode::CANCELLED, + "ActionQueue RPC was canceled before admission"); + } + if (wait_result == + ActionQueueExecutor::WaitResult::CanceledAfterAdmission) { + return grpc::Status( + grpc::StatusCode::CANCELLED, + "ActionQueue RPC waiter was canceled after admission; the edge action continues and its result can be retrieved with the same action_id"); + } + return grpc::Status::OK; + } catch (const std::exception& error) { + response->Clear(); + response->set_action_id(request->action_id()); + response->set_service_instance_id(action_queue_->instanceId()); + response->set_result(api::ACTION_RESULT_CODE_FAILED); + response->mutable_header()->set_success(false); + response->mutable_header()->set_error_message(error.what()); + setCurrentTimestamp( + response->mutable_header()->mutable_timestamp()); + return grpc::Status::OK; + } +} 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 ff144764..e6c60806 100644 --- a/cmvr-es/service/grpc/tests/grpc_agv_service_test.cpp +++ b/cmvr-es/service/grpc/tests/grpc_agv_service_test.cpp @@ -1,5 +1,6 @@ #include "service/grpc/include/grpc_agv_service.h" +#include #include #include #include @@ -8,6 +9,7 @@ #include #include "cmvr/config/device_manager_config/device_manager_config.pb.h" +#include "manager/control_authority/include/control_authority_manager.h" #include "manager/device_manager/include/device_manager.h" namespace cmvr::service { @@ -27,6 +29,14 @@ public: std::string typeName() const override { return "FakeAgv"; } + device::AgvResult emergencyStop() override + { + ++emergency_stop_calls_; + emergency_stop_barrier_observed_ = normalLeaseIsBlocked_( + "test-probe:emergencyStop"); + return device::AgvResult::success(); + } + device::AgvResult navigateToPose( const math::Pose2d& pose, const device::AgvMotionOptions& options, @@ -79,6 +89,41 @@ public: kNativeErrorMessage); } + device::AgvResult cancelNavigation() override + { + ++cancel_navigation_calls_; + cancel_navigation_barrier_observed_ = normalLeaseIsBlocked_( + "test-probe:cancelNavigation"); + return device::AgvResult::success(); + } + + device::AgvResult stopVelocityControl() override + { + ++stop_velocity_calls_; + stop_velocity_barrier_observed_ = normalLeaseIsBlocked_( + "test-probe:stopVelocityControl"); + return device::AgvResult::success(); + } + + device::AgvResult confirmMotionStopped() override + { + ++confirm_stopped_calls_; + confirm_stopped_barrier_observed_ = normalLeaseIsBlocked_( + "test-probe:confirmMotionStopped"); + return confirm_stopped_result_; + } + + bool normalLeaseIsBlocked_(const std::string& owner) + { + auto& authority = control::ControlAuthorityManager::instance(); + const auto probe = authority.tryAcquire( + id_, owner, std::chrono::hours(1)); + if (probe.acquired) { + authority.release(probe.token); + } + return !probe.acquired; + } + math::Pose2d pose_; device::AgvMotionOptions pose_options_; device::AgvResult pose_result_{device::AgvResult::success()}; @@ -93,6 +138,16 @@ public: bool station_cancellation_requested_during_call_{false}; bool path_cancellation_bound_{false}; bool path_cancellation_requested_during_call_{false}; + int emergency_stop_calls_{0}; + int cancel_navigation_calls_{0}; + int stop_velocity_calls_{0}; + int confirm_stopped_calls_{0}; + bool emergency_stop_barrier_observed_{false}; + bool cancel_navigation_barrier_observed_{false}; + bool stop_velocity_barrier_observed_{false}; + bool confirm_stopped_barrier_observed_{false}; + device::AgvResult confirm_stopped_result_{ + device::AgvResult::success()}; }; class LegacyFollowPathAgv final : public device::AbstractAGV { @@ -113,6 +168,7 @@ class GrpcAgvServiceTest : public ::testing::Test { protected: void SetUp() override { + control::ControlAuthorityManager::instance().clear(); config::DeviceManagerConfig config; auto& manager = device::DeviceManager::getInstance(config); agv_ = std::make_shared(); @@ -125,6 +181,7 @@ protected: service_.reset(); agv_.reset(); device::DeviceManager::destroyInstance(); + control::ControlAuthorityManager::instance().clear(); } std::shared_ptr agv_; @@ -293,6 +350,171 @@ TEST_F(GrpcAgvServiceTest, ExplicitAsynchronousNavigationIsForwarded) EXPECT_FALSE(agv_->station_cancellation_requested_during_call_); } +TEST_F(GrpcAgvServiceTest, ActionLeaseBlocksOrdinaryMutatingRpcs) +{ + auto& authority = control::ControlAuthorityManager::instance(); + const auto action_lease = authority.tryAcquire( + "test-agv", + "action-sequence:test-action", + std::chrono::hours(1)); + ASSERT_TRUE(action_lease.acquired) << action_lease.detail; + + api::AgvNavigateToPoseCommand_Request navigation_request; + navigation_request.mutable_header()->set_device_id("test-agv"); + api::AgvNavigateToPoseCommand_Feedback navigation_response; + grpc::ServerContext navigation_context; + const auto navigation_status = service_->navigateToPose( + &navigation_context, + &navigation_request, + &navigation_response); + EXPECT_EQ( + navigation_status.error_code(), + grpc::StatusCode::FAILED_PRECONDITION); + EXPECT_FALSE(navigation_response.header().success()); + + api::CommandHeader_Request clear_fault_request; + clear_fault_request.set_device_id("test-agv"); + api::CommandHeader_Feedback clear_fault_response; + grpc::ServerContext clear_fault_context; + const auto clear_fault_status = service_->clearFault( + &clear_fault_context, + &clear_fault_request, + &clear_fault_response); + EXPECT_EQ( + clear_fault_status.error_code(), + grpc::StatusCode::FAILED_PRECONDITION); + EXPECT_FALSE(clear_fault_response.success()); + + api::AgvMapCommand_Request switch_map_request; + switch_map_request.mutable_header()->set_device_id("test-agv"); + switch_map_request.set_map_name("map-1"); + api::AgvMapCommand_Feedback switch_map_response; + grpc::ServerContext switch_map_context; + const auto switch_map_status = service_->switchMap( + &switch_map_context, + &switch_map_request, + &switch_map_response); + EXPECT_EQ( + switch_map_status.error_code(), + grpc::StatusCode::FAILED_PRECONDITION); + EXPECT_FALSE(switch_map_response.header().success()); + + authority.release(action_lease.token); +} + +TEST_F(GrpcAgvServiceTest, QueriesBypassAndSafetyStopsPreemptActionLease) +{ + auto& authority = control::ControlAuthorityManager::instance(); + const auto action_lease = authority.tryAcquire( + "test-agv", + "action-sequence:test-action", + std::chrono::hours(1)); + ASSERT_TRUE(action_lease.acquired) << action_lease.detail; + + api::AgvRuntimeStateCommand_Request state_request; + state_request.mutable_header()->set_device_id("test-agv"); + api::AgvRuntimeStateCommand_Feedback state_response; + grpc::ServerContext state_context; + const auto state_status = service_->getRuntimeState( + &state_context, + &state_request, + &state_response); + EXPECT_TRUE(state_status.ok()) << state_status.error_message(); + EXPECT_TRUE(state_response.header().success()); + EXPECT_TRUE(authority.validate(action_lease.token)); + + api::CommandHeader_Request stop_request; + stop_request.set_device_id("test-agv"); + + api::CommandHeader_Feedback emergency_response; + grpc::ServerContext emergency_context; + const auto emergency_status = service_->emergencyStop( + &emergency_context, + &stop_request, + &emergency_response); + EXPECT_TRUE(emergency_status.ok()) << emergency_status.error_message(); + EXPECT_FALSE(authority.validate(action_lease.token)); + EXPECT_TRUE(agv_->emergency_stop_barrier_observed_); + + const auto lease_after_emergency = authority.tryAcquire( + "test-agv", + "action-sequence:after-emergency", + std::chrono::hours(1)); + ASSERT_TRUE(lease_after_emergency.acquired) + << lease_after_emergency.detail; + + api::CommandHeader_Feedback cancel_response; + grpc::ServerContext cancel_context; + const auto cancel_status = service_->cancelNavigation( + &cancel_context, + &stop_request, + &cancel_response); + EXPECT_TRUE(cancel_status.ok()) << cancel_status.error_message(); + EXPECT_FALSE(authority.validate(lease_after_emergency.token)); + EXPECT_TRUE(agv_->cancel_navigation_barrier_observed_); + + const auto lease_after_cancel = authority.tryAcquire( + "test-agv", + "action-sequence:after-cancel", + std::chrono::hours(1)); + ASSERT_TRUE(lease_after_cancel.acquired) + << lease_after_cancel.detail; + + api::CommandHeader_Feedback velocity_response; + grpc::ServerContext velocity_context; + const auto velocity_status = service_->stopVelocityControl( + &velocity_context, + &stop_request, + &velocity_response); + EXPECT_TRUE(velocity_status.ok()) << velocity_status.error_message(); + EXPECT_FALSE(authority.validate(lease_after_cancel.token)); + EXPECT_TRUE(agv_->stop_velocity_barrier_observed_); + + EXPECT_EQ(agv_->emergency_stop_calls_, 1); + EXPECT_EQ(agv_->cancel_navigation_calls_, 1); + EXPECT_EQ(agv_->stop_velocity_calls_, 1); + EXPECT_EQ(agv_->confirm_stopped_calls_, 3); + EXPECT_TRUE(agv_->confirm_stopped_barrier_observed_); + + const auto lease_after_stops = authority.tryAcquire( + "test-agv", + "action-sequence:after-stops", + std::chrono::hours(1)); + ASSERT_TRUE(lease_after_stops.acquired) + << lease_after_stops.detail; + authority.release(lease_after_stops.token); +} + +TEST_F(GrpcAgvServiceTest, + SafetyStopQuarantinesControlWhenStoppedStateIsUnconfirmed) +{ + agv_->confirm_stopped_result_ = device::AgvResult::failure( + device::AgvErrorCode::Timeout, + "two zero-velocity samples were not observed"); + + api::CommandHeader_Request request; + request.set_device_id("test-agv"); + api::CommandHeader_Feedback response; + grpc::ServerContext context; + const auto status = service_->cancelNavigation( + &context, &request, &response); + + EXPECT_EQ(status.error_code(), grpc::StatusCode::DEADLINE_EXCEEDED); + EXPECT_FALSE(response.success()); + EXPECT_NE( + response.error_message().find("confirmed stopped state"), + std::string::npos); + EXPECT_TRUE(agv_->cancel_navigation_barrier_observed_); + EXPECT_TRUE(agv_->confirm_stopped_barrier_observed_); + + const auto lease = + control::ControlAuthorityManager::instance().tryAcquire( + "test-agv", + "normal-control-after-unconfirmed-stop", + std::chrono::hours(1)); + EXPECT_FALSE(lease.acquired); +} + TEST_F(GrpcAgvServiceTest, NavigateToStationForwardsPgvAdapterParams) { api::AgvNavigateToStationCommand_Request request; @@ -384,5 +606,17 @@ TEST(AbstractAgvCompatibilityTest, FollowPathOptionsDelegateToLegacyOverride) EXPECT_EQ(legacy.path_[0].target_station, "station-2"); } +TEST(AbstractAgvCompatibilityTest, SynchronousActionSupportDefaultsToFalse) +{ + LegacyFollowPathAgv legacy; + + EXPECT_FALSE(legacy.supportsSynchronousAction( + device::AgvActionKind::NavigateToPose)); + EXPECT_FALSE(legacy.supportsSynchronousAction( + device::AgvActionKind::NavigateToStation)); + EXPECT_FALSE(legacy.supportsSynchronousAction( + device::AgvActionKind::FollowPath)); +} + } // namespace } // namespace cmvr::service diff --git a/cmvr-es/service/grpc/tests/grpc_arm_service_test.cpp b/cmvr-es/service/grpc/tests/grpc_arm_service_test.cpp index f5b2c46f..e44671d3 100644 --- a/cmvr-es/service/grpc/tests/grpc_arm_service_test.cpp +++ b/cmvr-es/service/grpc/tests/grpc_arm_service_test.cpp @@ -6,6 +6,7 @@ #include #include #include +#include #include #include #include @@ -135,6 +136,16 @@ public: lock, [this]() { return release_blocking_stop_; }); } + if (throw_next_stop_) { + throw_next_stop_ = false; + throw std::runtime_error("simulated stopMotion exception"); + } + if (fail_next_stop_) { + fail_next_stop_ = false; + return device::Result::failure( + device::ArmErrorCode::CommandFailed, + "simulated stopMotion failure"); + } return device::Result::success(); } @@ -178,6 +189,18 @@ public: release_blocking_stop_ = false; } + void failNextStopMotion() + { + std::lock_guard lock(motion_mutex_); + fail_next_stop_ = true; + } + + void throwNextStopMotion() + { + std::lock_guard lock(motion_mutex_); + throw_next_stop_ = true; + } + bool waitForBlockingStop(const std::chrono::milliseconds timeout) { std::unique_lock lock(motion_mutex_); @@ -350,6 +373,8 @@ private: bool block_next_stop_{false}; bool blocking_stop_started_{false}; bool release_blocking_stop_{false}; + bool fail_next_stop_{false}; + bool throw_next_stop_{false}; std::string blocking_motion_name_; int move_j_calls_{0}; int move_l_calls_{0}; @@ -688,5 +713,51 @@ TEST_F(GrpcArmServiceTest, EXPECT_EQ(aubo_arm_->stopMotionCalls(), 1); } +TEST_F(GrpcArmServiceTest, StopMotionFailureRetainsSafetyBarrier) +{ + auto& authority = control::ControlAuthorityManager::instance(); + const auto action_lease = authority.tryAcquire( + "aubo_arm", "action-queue:test", std::chrono::hours(1)); + ASSERT_TRUE(action_lease.acquired) << action_lease.detail; + aubo_arm_->failNextStopMotion(); + + api::CommandHeader_Feedback stop_response; + const auto stop_status = stopMotion("aubo_arm", stop_response); + const auto rejected_move = moveJ("aubo_arm"); + + EXPECT_EQ(stop_status.error_code(), grpc::StatusCode::INTERNAL); + EXPECT_FALSE(stop_response.success()); + EXPECT_FALSE(authority.validate(action_lease.token)); + EXPECT_TRUE(authority.isLeased("aubo_arm")); + EXPECT_EQ( + rejected_move.status.error_code(), + grpc::StatusCode::FAILED_PRECONDITION); + EXPECT_EQ(aubo_arm_->moveJCalls(), 0); + EXPECT_EQ(aubo_arm_->stopMotionCalls(), 1); +} + +TEST_F(GrpcArmServiceTest, StopMotionExceptionRetainsSafetyBarrier) +{ + auto& authority = control::ControlAuthorityManager::instance(); + const auto action_lease = authority.tryAcquire( + "aubo_arm", "action-queue:test", std::chrono::hours(1)); + ASSERT_TRUE(action_lease.acquired) << action_lease.detail; + aubo_arm_->throwNextStopMotion(); + + api::CommandHeader_Feedback stop_response; + const auto stop_status = stopMotion("aubo_arm", stop_response); + const auto rejected_move = moveL("aubo_arm"); + + EXPECT_EQ(stop_status.error_code(), grpc::StatusCode::INTERNAL); + EXPECT_FALSE(stop_response.success()); + EXPECT_FALSE(authority.validate(action_lease.token)); + EXPECT_TRUE(authority.isLeased("aubo_arm")); + EXPECT_EQ( + rejected_move.status.error_code(), + grpc::StatusCode::FAILED_PRECONDITION); + EXPECT_EQ(aubo_arm_->moveLCalls(), 0); + EXPECT_EQ(aubo_arm_->stopMotionCalls(), 1); +} + } // namespace } // namespace cmvr::service diff --git a/cmvr-es/service/grpc/tests/grpc_system_service_test.cpp b/cmvr-es/service/grpc/tests/grpc_system_service_test.cpp index 22eac5e4..70e991a6 100644 --- a/cmvr-es/service/grpc/tests/grpc_system_service_test.cpp +++ b/cmvr-es/service/grpc/tests/grpc_system_service_test.cpp @@ -1,13 +1,17 @@ #include "service/grpc/include/grpc_system_service.h" +#include #include +#include #include #include #include #include #include #include +#include #include +#include #include #include @@ -15,8 +19,11 @@ #include #include "cmvr/config/device_manager_config/device_manager_config.pb.h" +#include "devices/agv/abstract_agv.h" +#include "devices/arm/robot_arm.h" #include "manager/control_authority/include/control_authority_manager.h" #include "manager/device_manager/include/device_manager.h" +#include "service/action/include/action_queue_executor.h" namespace cmvr::service { namespace { @@ -94,6 +101,613 @@ private: bool release_stop_{false}; }; +class ActionTrace final { +public: + using TimePoint = std::chrono::steady_clock::time_point; + + struct Entry { + std::string name; + TimePoint time; + }; + + void add(std::string name) + { + std::lock_guard lock(mutex_); + entries_.push_back({std::move(name), std::chrono::steady_clock::now()}); + } + + std::vector names() const + { + std::lock_guard lock(mutex_); + std::vector result; + result.reserve(entries_.size()); + for (const auto& entry : entries_) { + result.push_back(entry.name); + } + return result; + } + + std::optional firstTime(const std::string& name) const + { + std::lock_guard lock(mutex_); + const auto found = std::find_if( + entries_.begin(), entries_.end(), + [&name](const Entry& entry) { return entry.name == name; }); + return found == entries_.end() + ? std::nullopt + : std::optional(found->time); + } + +private: + mutable std::mutex mutex_; + std::vector entries_; +}; + +class ActionTestArm final : public device::RobotArm { +public: + ActionTestArm(std::string id, std::shared_ptr trace) + : trace_(std::move(trace)) + { + id_ = std::move(id); + } + + std::string typeName() const override { return "ActionTestArm"; } + bool supportsActionQueueMotion() const noexcept override { return true; } + device::RobotModel getRobotModel() const override + { + device::RobotModel model; + model.name = "ActionTestArm"; + model.dof = kDof; + model.joint_names.assign(kDof, "joint"); + return model; + } + std::size_t getDof() const override { return kDof; } + device::ArmState getRobotState() const override { return {}; } + device::JointGroupState getJointState() const override { return {}; } + device::CartesianPose getTcpPose( + device::FrameType = device::FrameType::Base) const override + { + return {}; + } + device::RobotMode getRobotMode() const override + { + return device::RobotMode::Idle; + } + device::SafetyMode getSafetyMode() const override + { + return device::SafetyMode::Normal; + } + device::ControlMode getControlMode() const override + { + return device::ControlMode::Position; + } + + device::Result torqueOn() override { return device::Result::success(); } + device::Result torqueOff() override { return device::Result::success(); } + device::Result calibrateZeroQ(const std::string&) override + { + return device::Result::success(); + } + device::Result emergencyStop() override + { + return device::Result::success(); + } + device::Result protectiveStop() override + { + return device::Result::success(); + } + device::Result setSpeedScaling(double) override + { + return device::Result::success(); + } + double getSpeedScaling() const override { return 1.0; } + bool isProtectiveStopped() const override { return false; } + bool isEmergencyStopped() const override { return false; } + bool isFault() const override { return false; } + + device::Result moveJ(const device::JointPositionCommand&, + const device::MotionOptions& options) override + { + return performMotion("arm:J", options); + } + device::Result speedJ(const device::JointVelocityCommand&, + double, + double) override + { + return device::Result::success(); + } + device::Result stopJ(double) override { return stopMotion(); } + device::Result moveL( + const device::CartesianPose& target, + const device::MotionOptions& options, + device::FrameType = device::FrameType::Base) override + { + return performMotion( + "arm:L:" + std::to_string(static_cast(target.x)), + options); + } + device::Result speedL( + const device::CartesianVelocity&, + double, + double, + device::FrameType = device::FrameType::Base) override + { + return device::Result::success(); + } + device::Result stopL(std::optional = std::nullopt) override + { + return stopMotion(); + } + device::Result stopMotion() override + { + { + std::lock_guard lock(mutex_); + ++stop_motion_calls_; + stop_requested_ = true; + } + trace_->add("arm:stop:" + id_); + motion_condition_.notify_all(); + return device::Result::success(); + } + + device::Result startServoMode(const device::ServoOptions&) override + { + return device::Result::success(); + } + device::Result servoJ(const device::JointPositionCommand&) override + { + return device::Result::success(); + } + device::Result servoL( + const device::CartesianPose&, + device::FrameType = device::FrameType::Base) override + { + return device::Result::success(); + } + device::Result servoSpeedJ( + const device::JointVelocityCommand&) override + { + return device::Result::success(); + } + device::Result servoSpeedL( + const device::CartesianVelocity&, + device::FrameType = device::FrameType::Base) override + { + return device::Result::success(); + } + device::Result stopServoMode() override + { + return device::Result::success(); + } + + device::Result connect(const std::string&, int) override + { + return device::Result::success(); + } + device::Result disconnect() override { return device::Result::success(); } + bool isConnected() const override { return true; } + device::Result powerOn() override { return device::Result::success(); } + device::Result powerOff() override { return device::Result::success(); } + device::Result brakeRelease() override { return device::Result::success(); } + device::Result shutdown() override { return device::Result::success(); } + device::Result clearFault() override { return device::Result::success(); } + device::Result unlockProtectiveStop() override + { + return device::Result::success(); + } + device::Result loadProgram(const std::string&) override + { + return device::Result::success(); + } + device::Result playProgram() override { return device::Result::success(); } + device::Result pauseProgram() override { return device::Result::success(); } + device::Result stopProgram() override { return device::Result::success(); } + std::vector ik(const std::string&, + const std::string&, + const device::CartesianPose&) override + { + return {}; + } + std::shared_ptr kinematicsSolver() const override + { + return nullptr; + } + device::CartesianPose fk(const std::string&, + const std::string&) override + { + return {}; + } + device::CartesianPose fk(bool = true) override { return {}; } + device::CartesianVelocity getSpeedLCommandTwistBase() const override + { + return {}; + } + bool busy() const override + { + std::lock_guard lock(mutex_); + return active_motions_ != 0; + } + + void blockNextMotion() + { + std::lock_guard lock(mutex_); + block_next_motion_ = true; + release_blocked_motion_ = false; + stop_requested_ = false; + } + + void blockCanceledMotionReturn() + { + std::lock_guard lock(mutex_); + block_canceled_motion_return_ = true; + canceled_motion_return_blocked_ = false; + release_canceled_motion_return_ = false; + } + + bool waitForCanceledMotionReturn( + const std::chrono::milliseconds timeout) + { + std::unique_lock lock(mutex_); + return canceled_motion_return_condition_.wait_for( + lock, + timeout, + [this]() { return canceled_motion_return_blocked_; }); + } + + void releaseCanceledMotionReturn() + { + { + std::lock_guard lock(mutex_); + release_canceled_motion_return_ = true; + } + motion_condition_.notify_all(); + } + + void releaseBlockedMotion() + { + { + std::lock_guard lock(mutex_); + release_blocked_motion_ = true; + } + motion_condition_.notify_all(); + } + + void failOnMotionCall(const int call_index) + { + std::lock_guard lock(mutex_); + fail_on_motion_call_ = call_index; + } + + bool waitForMotionCalls( + const int expected, + const std::chrono::milliseconds timeout) + { + std::unique_lock lock(mutex_); + return motion_started_condition_.wait_for( + lock, timeout, + [this, expected]() { return motion_calls_ >= expected; }); + } + + int motionCalls() const + { + std::lock_guard lock(mutex_); + return motion_calls_; + } + + int stopMotionCalls() const + { + std::lock_guard lock(mutex_); + return stop_motion_calls_; + } + + int maxActiveMotions() const + { + std::lock_guard lock(mutex_); + return max_active_motions_; + } + +private: + device::Result performMotion( + const std::string& event, + const device::MotionOptions& options) + { + bool block = false; + bool fail = false; + { + std::lock_guard lock(mutex_); + ++motion_calls_; + ++active_motions_; + max_active_motions_ = std::max( + max_active_motions_, active_motions_); + block = block_next_motion_; + block_next_motion_ = false; + fail = motion_calls_ == fail_on_motion_call_; + } + trace_->add(event); + motion_started_condition_.notify_all(); + + bool canceled = false; + bool safety_timeout = false; + if (block) { + const auto safety_deadline = + std::chrono::steady_clock::now() + std::chrono::seconds(1); + std::unique_lock lock(mutex_); + while (!release_blocked_motion_ && !stop_requested_ && + !(options.cancellation_requested && + options.cancellation_requested())) { + if (std::chrono::steady_clock::now() >= safety_deadline) { + safety_timeout = true; + break; + } + motion_condition_.wait_for(lock, std::chrono::milliseconds(1)); + } + canceled = stop_requested_ || + (options.cancellation_requested && + options.cancellation_requested()); + } + + { + std::unique_lock lock(mutex_); + --active_motions_; + if (canceled && block_canceled_motion_return_) { + block_canceled_motion_return_ = false; + canceled_motion_return_blocked_ = true; + canceled_motion_return_condition_.notify_all(); + motion_condition_.wait( + lock, + [this]() { return release_canceled_motion_return_; }); + } + } + if (safety_timeout) { + return device::Result::failure( + device::ArmErrorCode::Timeout, + "test arm safety wait expired"); + } + if (canceled) { + return device::Result::failure( + device::ArmErrorCode::CommandRejected, + "test arm motion canceled"); + } + if (fail) { + return device::Result::failure( + device::ArmErrorCode::CommandFailed, + "injected RobotArm failure"); + } + return device::Result::success(); + } + + static constexpr std::size_t kDof = 6U; + std::shared_ptr trace_; + mutable std::mutex mutex_; + std::condition_variable motion_condition_; + std::condition_variable motion_started_condition_; + std::condition_variable canceled_motion_return_condition_; + int motion_calls_{0}; + int stop_motion_calls_{0}; + int active_motions_{0}; + int max_active_motions_{0}; + int fail_on_motion_call_{-1}; + bool block_next_motion_{false}; + bool release_blocked_motion_{false}; + bool stop_requested_{false}; + bool block_canceled_motion_return_{false}; + bool canceled_motion_return_blocked_{false}; + bool release_canceled_motion_return_{false}; +}; + +class ActionTestAgv final : public device::AbstractAGV { +public: + ActionTestAgv(std::string id, std::shared_ptr trace) + : trace_(std::move(trace)) + { + id_ = std::move(id); + } + + std::string typeName() const override { return "ActionTestAgv"; } + bool supportsSynchronousAction(device::AgvActionKind) const noexcept override + { + return supports_synchronous_.load(std::memory_order_acquire); + } + device::AgvRuntimeState runtimeState() const override { return {}; } + device::AgvNavigationStatus navigationStatus() const override { return {}; } + device::AgvResult navigateToPose( + const math::Pose2d& pose, + const device::AgvMotionOptions&, + const device::AgvAdapterParams&) override + { + { + std::lock_guard lock(parameters_mutex_); + last_pose_ = pose; + } + return record("agv:pose"); + } + device::AgvResult navigateToStation( + const std::string& station_id, + const device::AgvMotionOptions&, + const device::AgvAdapterParams&) override + { + return record("agv:station:" + station_id); + } + device::AgvResult followPath( + const std::vector& path, + const device::AgvMotionOptions&) override + { + { + std::lock_guard lock(parameters_mutex_); + last_path_ = path; + } + return record("agv:path"); + } + device::AgvResult cancelNavigation() override + { + cancel_calls_.fetch_add(1, std::memory_order_relaxed); + trace_->add("agv:cancel"); + return device::AgvResult::success(); + } + device::AgvResult confirmMotionStopped() override + { + return stopped_confirmed_.load(std::memory_order_acquire) + ? device::AgvResult::success() + : device::AgvResult::failure( + device::AgvErrorCode::Timeout, + "test AGV stopped state is unconfirmed"); + } + void setSupportsSynchronous(const bool value) + { + supports_synchronous_.store(value, std::memory_order_release); + } + + void setNavigationFailure(const bool value) + { + fail_navigation_.store(value, std::memory_order_release); + } + + void setStoppedConfirmed(const bool value) + { + stopped_confirmed_.store(value, std::memory_order_release); + } + + int navigationCalls() const + { + return navigation_calls_.load(std::memory_order_relaxed); + } + + math::Pose2d lastPose() const + { + std::lock_guard lock(parameters_mutex_); + return last_pose_; + } + + std::vector lastPath() const + { + std::lock_guard lock(parameters_mutex_); + return last_path_; + } + +private: + device::AgvResult record(std::string event) + { + navigation_calls_.fetch_add(1, std::memory_order_relaxed); + trace_->add(std::move(event)); + if (fail_navigation_.load(std::memory_order_acquire)) { + return device::AgvResult::failure( + device::AgvErrorCode::TaskFailed, + "injected AGV navigation failure"); + } + return device::AgvResult::success(); + } + + std::shared_ptr trace_; + mutable std::mutex parameters_mutex_; + math::Pose2d last_pose_{}; + std::vector last_path_; + std::atomic supports_synchronous_{true}; + std::atomic fail_navigation_{false}; + std::atomic stopped_confirmed_{true}; + std::atomic navigation_calls_{0}; + std::atomic cancel_calls_{0}; +}; + +api::ActionStep* addMoveLStep( + api::ActionQueueCommand_Request& request, + const std::string& step_id, + const std::string& device_id, + const double marker, + const std::uint32_t timeout_ms = 0U, + const bool asynchronous = false) +{ + auto* step = request.add_steps(); + step->set_step_id(step_id); + step->set_timeout_ms(timeout_ms); + auto* command = step->mutable_arm_move_l(); + command->mutable_header()->set_device_id(device_id); + command->mutable_target()->set_x(marker); + command->set_frame(api::ARM_FRAME_BASE); + command->mutable_options()->set_asynchronous(asynchronous); + return step; +} + +api::ActionStep* addMoveJStep( + api::ActionQueueCommand_Request& request, + const std::string& step_id, + const std::string& device_id, + const bool asynchronous = false) +{ + auto* step = request.add_steps(); + step->set_step_id(step_id); + auto* command = step->mutable_arm_move_j(); + command->mutable_header()->set_device_id(device_id); + for (std::size_t index = 0; index < 6U; ++index) { + command->mutable_target()->add_position( + static_cast(index) * 0.1); + } + command->mutable_options()->set_asynchronous(asynchronous); + return step; +} + +api::ActionStep* addAgvStationStep( + api::ActionQueueCommand_Request& request, + const std::string& step_id, + const std::string& device_id, + const std::string& station_id, + const bool asynchronous = false) +{ + auto* step = request.add_steps(); + step->set_step_id(step_id); + auto* command = step->mutable_agv_navigate_to_station(); + command->mutable_header()->set_device_id(device_id); + command->set_station_id(station_id); + command->mutable_options()->set_asynchronous(asynchronous); + return step; +} + +api::ActionStep* addAgvPoseStep( + api::ActionQueueCommand_Request& request, + const std::string& step_id, + const std::string& device_id, + const double x, + const double y, + const double theta) +{ + auto* step = request.add_steps(); + step->set_step_id(step_id); + auto* command = step->mutable_agv_navigate_to_pose(); + command->mutable_header()->set_device_id(device_id); + command->mutable_pose()->set_x(x); + command->mutable_pose()->set_y(y); + command->mutable_pose()->set_theta(theta); + return step; +} + +api::ActionStep* addAgvPathStep( + api::ActionQueueCommand_Request& request, + const std::string& step_id, + const std::string& device_id) +{ + auto* step = request.add_steps(); + step->set_step_id(step_id); + auto* command = step->mutable_agv_follow_path(); + command->mutable_header()->set_device_id(device_id); + auto* first = command->add_path(); + first->set_source_station("start"); + first->set_target_station("middle"); + auto* second = command->add_path(); + second->set_source_station("middle"); + second->set_target_station("finish"); + return step; +} + +api::ActionStep* addDelayStep( + api::ActionQueueCommand_Request& request, + const std::string& step_id, + const std::uint32_t duration_ms) +{ + auto* step = request.add_steps(); + step->set_step_id(step_id); + step->mutable_delay()->set_duration_ms(duration_ms); + return step; +} + std::uint64_t currentUnixTimeMs() { const auto elapsed = std::chrono::duration_cast( @@ -124,6 +738,9 @@ protected: void TearDown() override { service_.reset(); + action_agv_.reset(); + action_arm_.reset(); + action_trace_.reset(); owned_devices_.clear(); device::DeviceManager::destroyInstance(); control::ControlAuthorityManager::instance().clear(); @@ -148,8 +765,50 @@ protected: owned_devices_.push_back(std::move(device)); } + void initializeActionDevices() + { + config::DeviceManagerConfig config; + auto& manager = device::DeviceManager::getInstance(config); + action_trace_ = std::make_shared(); + action_arm_ = std::make_shared( + "action-arm", action_trace_); + action_agv_ = std::make_shared( + "action-agv", action_trace_); + manager.registerDevice(action_arm_); + manager.registerDevice(action_agv_); + service_ = std::make_unique(); + api::GetSystemInfoCommand_Request info_request; + api::GetSystemInfoCommand_Feedback info_response; + grpc::ServerContext info_context; + const auto info_status = service_->GetSystemInfo( + &info_context, &info_request, &info_response); + ASSERT_TRUE(info_status.ok()) << info_status.error_message(); + service_instance_id_ = info_response.action_service_instance_id(); + ASSERT_FALSE(service_instance_id_.empty()); + } + + api::ActionQueueCommand_Feedback executeAction( + const api::ActionQueueCommand_Request& request) + { + auto bound_request = request; + if (bound_request.expected_service_instance_id().empty()) { + bound_request.set_expected_service_instance_id( + service_instance_id_); + } + api::ActionQueueCommand_Feedback response; + grpc::ServerContext context; + const auto status = service_->ExecuteActionQueue( + &context, &bound_request, &response); + EXPECT_TRUE(status.ok()) << status.error_message(); + return response; + } + std::unique_ptr service_; std::vector> owned_devices_; + std::shared_ptr action_trace_; + std::shared_ptr action_arm_; + std::shared_ptr action_agv_; + std::string service_instance_id_; }; TEST_F(GrpcSystemServiceTest, @@ -340,6 +999,784 @@ TEST_F(GrpcSystemServiceTest, MapsEveryKnownDeviceKind) } } +TEST_F(GrpcSystemServiceTest, ActionQueueExecutesFourMoveLStepsSerially) +{ + initializeActionDevices(); + api::ActionQueueCommand_Request request; + request.set_action_id("four-movel"); + for (int marker = 1; marker <= 4; ++marker) { + addMoveLStep( + request, + "move-" + std::to_string(marker), + action_arm_->id(), + static_cast(marker)); + } + + const auto response = executeAction(request); + + EXPECT_TRUE(response.header().success()) + << response.header().error_message(); + EXPECT_EQ(response.result(), api::ACTION_RESULT_CODE_COMPLETED); + EXPECT_EQ(response.completed_steps(), 4U); + EXPECT_FALSE(response.has_failed_step_index()); + EXPECT_EQ(action_arm_->motionCalls(), 4); + EXPECT_EQ(action_arm_->maxActiveMotions(), 1); + EXPECT_EQ( + action_trace_->names(), + (std::vector{ + "arm:L:1", "arm:L:2", "arm:L:3", "arm:L:4"})); +} + +TEST_F(GrpcSystemServiceTest, ActionQueuePreservesArmDelayAgvOrder) +{ + initializeActionDevices(); + api::ActionQueueCommand_Request request; + request.set_action_id("arm-delay-agv"); + addMoveJStep(request, "arm", action_arm_->id()); + addDelayStep(request, "settle", 25U); + addAgvStationStep( + request, "agv", action_agv_->id(), "dock"); + + const auto response = executeAction(request); + const auto arm_time = action_trace_->firstTime("arm:J"); + const auto agv_time = action_trace_->firstTime("agv:station:dock"); + + ASSERT_TRUE(response.header().success()) + << response.header().error_message(); + EXPECT_EQ(response.result(), api::ACTION_RESULT_CODE_COMPLETED); + EXPECT_EQ(response.completed_steps(), 3U); + EXPECT_EQ( + action_trace_->names(), + (std::vector{"arm:J", "agv:station:dock"})); + ASSERT_TRUE(arm_time.has_value()); + ASSERT_TRUE(agv_time.has_value()); + EXPECT_GE( + std::chrono::duration_cast( + *agv_time - *arm_time), + std::chrono::milliseconds(15)); +} + +TEST_F(GrpcSystemServiceTest, ActionQueueExecutesAgvPoseAndPathSteps) +{ + initializeActionDevices(); + api::ActionQueueCommand_Request request; + request.set_action_id("agv-pose-and-path"); + addAgvPoseStep( + request, "pose", action_agv_->id(), 1.0, 2.0, 0.5); + addAgvPathStep(request, "path", action_agv_->id()); + + const auto response = executeAction(request); + + ASSERT_TRUE(response.header().success()) + << response.header().error_message(); + EXPECT_EQ(response.result(), api::ACTION_RESULT_CODE_COMPLETED); + EXPECT_EQ(response.completed_steps(), 2U); + EXPECT_EQ(action_agv_->navigationCalls(), 2); + const auto pose = action_agv_->lastPose(); + EXPECT_DOUBLE_EQ(pose.x, 1.0); + EXPECT_DOUBLE_EQ(pose.y, 2.0); + EXPECT_DOUBLE_EQ(pose.theta, 0.5); + const auto path = action_agv_->lastPath(); + ASSERT_EQ(path.size(), 2U); + EXPECT_EQ(path[0].source_station, "start"); + EXPECT_EQ(path[0].target_station, "middle"); + EXPECT_EQ(path[1].source_station, "middle"); + EXPECT_EQ(path[1].target_station, "finish"); + EXPECT_EQ( + action_trace_->names(), + (std::vector{"agv:pose", "agv:path"})); +} + +TEST_F(GrpcSystemServiceTest, + ActionQueuePrevalidatesAllStepsBeforeAnyDispatch) +{ + initializeActionDevices(); + api::ActionQueueCommand_Request request; + request.set_action_id("prevalidate-all"); + addMoveLStep(request, "valid-first", action_arm_->id(), 1.0); + addAgvStationStep( + request, "missing-second", "missing-agv", "dock"); + + const auto response = executeAction(request); + + EXPECT_FALSE(response.header().success()); + EXPECT_EQ(response.result(), api::ACTION_RESULT_CODE_REJECTED); + EXPECT_EQ(response.completed_steps(), 0U); + ASSERT_TRUE(response.has_failed_step_index()); + EXPECT_EQ(response.failed_step_index(), 1U); + EXPECT_EQ(action_arm_->motionCalls(), 0); + EXPECT_EQ(action_agv_->navigationCalls(), 0); + EXPECT_TRUE(action_trace_->names().empty()); +} + +TEST_F(GrpcSystemServiceTest, ActionQueueRejectsAsynchronousMotion) +{ + initializeActionDevices(); + api::ActionQueueCommand_Request arm_request; + arm_request.set_action_id("async-arm"); + addMoveLStep( + arm_request, "arm", action_arm_->id(), 1.0, 0U, true); + const auto arm_response = executeAction(arm_request); + + api::ActionQueueCommand_Request agv_request; + agv_request.set_action_id("async-agv"); + addAgvStationStep( + agv_request, "agv", action_agv_->id(), "dock", true); + const auto agv_response = executeAction(agv_request); + + EXPECT_EQ(arm_response.result(), api::ACTION_RESULT_CODE_REJECTED); + EXPECT_EQ(agv_response.result(), api::ACTION_RESULT_CODE_REJECTED); + EXPECT_EQ(action_arm_->motionCalls(), 0); + EXPECT_EQ(action_agv_->navigationCalls(), 0); + EXPECT_TRUE(action_trace_->names().empty()); +} + +TEST_F(GrpcSystemServiceTest, + ActionQueueStopsAfterFailureAndReportsFailedStep) +{ + initializeActionDevices(); + action_arm_->failOnMotionCall(2); + api::ActionQueueCommand_Request request; + request.set_action_id("fail-fast"); + addMoveLStep(request, "first", action_arm_->id(), 1.0); + addMoveLStep(request, "fails", action_arm_->id(), 2.0); + addMoveLStep(request, "must-not-run", action_arm_->id(), 3.0); + + const auto response = executeAction(request); + + EXPECT_FALSE(response.header().success()); + EXPECT_EQ(response.result(), api::ACTION_RESULT_CODE_FAILED); + EXPECT_EQ(response.completed_steps(), 1U); + ASSERT_TRUE(response.has_failed_step_index()); + EXPECT_EQ(response.failed_step_index(), 1U); + EXPECT_EQ(action_arm_->motionCalls(), 2); + const auto trace = action_trace_->names(); + ASSERT_GE(trace.size(), 2U); + EXPECT_EQ(trace[0], "arm:L:1"); + EXPECT_EQ(trace[1], "arm:L:2"); + EXPECT_GE(action_arm_->stopMotionCalls(), 1); +} + +TEST_F(GrpcSystemServiceTest, + ActionQueueQuarantinesAgvAfterUnconfirmedFailureStop) +{ + initializeActionDevices(); + action_agv_->setNavigationFailure(true); + action_agv_->setStoppedConfirmed(false); + api::ActionQueueCommand_Request request; + request.set_action_id("agv-unconfirmed-stop"); + addAgvStationStep( + request, "fails", action_agv_->id(), "dock"); + + const auto response = executeAction(request); + + EXPECT_EQ(response.result(), api::ACTION_RESULT_CODE_FAILED); + ASSERT_TRUE(response.has_failed_step_index()); + EXPECT_EQ(response.failed_step_index(), 0U); + EXPECT_NE( + response.header().error_message().find("remain quarantined"), + std::string::npos); + const auto lease = + control::ControlAuthorityManager::instance().tryAcquire( + action_agv_->id(), + "normal-control-after-unconfirmed-action-stop", + std::chrono::hours(1)); + EXPECT_FALSE(lease.acquired); +} + +TEST_F(GrpcSystemServiceTest, + ActionQueueIsIdempotentAndRejectsConflictingPayload) +{ + initializeActionDevices(); + api::ActionQueueCommand_Request request; + request.set_action_id("idempotent-action"); + addMoveLStep(request, "only-step", action_arm_->id(), 1.0); + + const auto first = executeAction(request); + const auto retry = executeAction(request); + auto conflicting = request; + conflicting.mutable_steps(0) + ->mutable_arm_move_l()->mutable_target()->set_x(2.0); + const auto conflict = executeAction(conflicting); + + EXPECT_EQ(first.result(), api::ACTION_RESULT_CODE_COMPLETED); + EXPECT_EQ( + first.deduplication_status(), + api::ACTION_DEDUPLICATION_STATUS_ACCEPTED_NEW); + EXPECT_EQ(retry.result(), api::ACTION_RESULT_CODE_COMPLETED); + EXPECT_EQ( + retry.deduplication_status(), + api::ACTION_DEDUPLICATION_STATUS_CACHED_RESULT); + EXPECT_EQ(retry.completed_steps(), 1U); + EXPECT_EQ(conflict.result(), api::ACTION_RESULT_CODE_REJECTED); + EXPECT_EQ( + conflict.deduplication_status(), + api::ACTION_DEDUPLICATION_STATUS_ACTION_ID_CONFLICT); + EXPECT_EQ(action_arm_->motionCalls(), 1); + EXPECT_EQ( + action_trace_->names(), + (std::vector{"arm:L:1"})); +} + +TEST_F(GrpcSystemServiceTest, + ActionQueueRetryAfterTerminalCacheChurnDoesNotRedispatch) +{ + initializeActionDevices(); + api::ActionQueueCommand_Request original; + original.set_action_id("idempotent-after-cache-churn"); + addMoveLStep(original, "only-step", action_arm_->id(), 1.0); + + const auto first = executeAction(original); + ASSERT_EQ(first.result(), api::ACTION_RESULT_CODE_COMPLETED); + ASSERT_EQ(action_arm_->motionCalls(), 1); + + constexpr int kActionsBeyondTerminalCacheCapacity = 257; + for (int index = 0; index < kActionsBeyondTerminalCacheCapacity; ++index) { + SCOPED_TRACE(index); + api::ActionQueueCommand_Request filler; + filler.set_action_id("terminal-cache-filler-" + std::to_string(index)); + addDelayStep(filler, "delay", 0U); + + const auto response = executeAction(filler); + ASSERT_EQ(response.result(), api::ACTION_RESULT_CODE_COMPLETED) + << response.header().error_message(); + } + + const auto retry = executeAction(original); + + EXPECT_EQ(retry.result(), api::ACTION_RESULT_CODE_COMPLETED); + EXPECT_EQ(retry.completed_steps(), 1U); + EXPECT_EQ(action_arm_->motionCalls(), 1); + + // submitAndWait() may wake before the worker completes terminal-cache + // rotation for the final filler. Advance one additional terminal action so + // the original ID is deterministically present in the retired-ID filter. + constexpr int kTotalActionsToRetireOriginal = 4353; + for (int index = kActionsBeyondTerminalCacheCapacity; + index < kTotalActionsToRetireOriginal; ++index) { + SCOPED_TRACE(index); + api::ActionQueueCommand_Request filler; + filler.set_action_id("terminal-cache-filler-" + std::to_string(index)); + addDelayStep(filler, "delay", 0U); + + const auto response = executeAction(filler); + ASSERT_EQ(response.result(), api::ACTION_RESULT_CODE_COMPLETED) + << response.header().error_message(); + } + + const auto retired_retry = executeAction(original); + + EXPECT_EQ(retired_retry.result(), api::ACTION_RESULT_CODE_REJECTED); + EXPECT_EQ( + retired_retry.deduplication_status(), + api::ACTION_DEDUPLICATION_STATUS_RESULT_EVICTED); + EXPECT_NE( + retired_retry.header().error_message().find("no longer cached"), + std::string::npos); + EXPECT_EQ(action_arm_->motionCalls(), 1); +} + +TEST_F(GrpcSystemServiceTest, + ActionQueueRejectsMissingOrStaleServiceInstance) +{ + initializeActionDevices(); + api::ActionQueueCommand_Request request; + request.set_action_id("service-instance-check"); + addMoveLStep(request, "move", action_arm_->id(), 1.0); + + api::ActionQueueCommand_Feedback missing_response; + grpc::ServerContext missing_context; + const auto missing_status = service_->ExecuteActionQueue( + &missing_context, &request, &missing_response); + + request.set_expected_service_instance_id("stale-instance"); + api::ActionQueueCommand_Feedback stale_response; + grpc::ServerContext stale_context; + const auto stale_status = service_->ExecuteActionQueue( + &stale_context, &request, &stale_response); + + EXPECT_TRUE(missing_status.ok()) << missing_status.error_message(); + EXPECT_EQ( + missing_response.result(), api::ACTION_RESULT_CODE_REJECTED); + EXPECT_EQ(missing_response.service_instance_id(), service_instance_id_); + EXPECT_TRUE(stale_status.ok()) << stale_status.error_message(); + EXPECT_EQ(stale_response.result(), api::ACTION_RESULT_CODE_REJECTED); + EXPECT_EQ( + stale_response.deduplication_status(), + api::ACTION_DEDUPLICATION_STATUS_SERVICE_INSTANCE_MISMATCH); + EXPECT_EQ(stale_response.service_instance_id(), service_instance_id_); + EXPECT_EQ(action_arm_->motionCalls(), 0); +} + +TEST_F(GrpcSystemServiceTest, + ActionQueueServiceRestartRejectsRetryBoundToPriorInstance) +{ + initializeActionDevices(); + api::ActionQueueCommand_Request request; + request.set_action_id("prior-instance-retry"); + request.set_expected_service_instance_id(service_instance_id_); + addMoveLStep(request, "move", action_arm_->id(), 1.0); + const std::string prior_instance = service_instance_id_; + + const auto first = executeAction(request); + ASSERT_EQ(first.result(), api::ACTION_RESULT_CODE_COMPLETED) + << first.header().error_message(); + ASSERT_EQ(action_arm_->motionCalls(), 1); + + service_.reset(); + service_ = std::make_unique(); + api::GetSystemInfoCommand_Request info_request; + api::GetSystemInfoCommand_Feedback info_response; + grpc::ServerContext info_context; + ASSERT_TRUE(service_->GetSystemInfo( + &info_context, &info_request, &info_response).ok()); + service_instance_id_ = info_response.action_service_instance_id(); + ASSERT_FALSE(service_instance_id_.empty()); + ASSERT_NE(service_instance_id_, prior_instance); + + api::ActionQueueCommand_Feedback response; + grpc::ServerContext context; + const auto status = service_->ExecuteActionQueue( + &context, &request, &response); + + EXPECT_TRUE(status.ok()) << status.error_message(); + EXPECT_EQ(response.result(), api::ACTION_RESULT_CODE_REJECTED); + EXPECT_EQ( + response.deduplication_status(), + api::ACTION_DEDUPLICATION_STATUS_SERVICE_INSTANCE_MISMATCH); + EXPECT_EQ(response.service_instance_id(), service_instance_id_); + EXPECT_EQ(action_arm_->motionCalls(), 1); +} + +TEST_F(GrpcSystemServiceTest, + ActionQueueLedgerCapacityRejectsOnlyNewIds) +{ + initializeActionDevices(); + ActionQueueExecutor executor( + device::DeviceManager::getInstance(), 2U); + const auto make_request = [&executor](const std::string& action_id) { + api::ActionQueueCommand_Request request; + request.set_action_id(action_id); + request.set_expected_service_instance_id(executor.instanceId()); + addDelayStep(request, "delay", 0U); + return request; + }; + const auto first_request = make_request("ledger-first"); + const auto second_request = make_request("ledger-second"); + const auto rejected_request = make_request("ledger-third"); + api::ActionQueueCommand_Feedback first; + api::ActionQueueCommand_Feedback second; + api::ActionQueueCommand_Feedback rejected; + api::ActionQueueCommand_Feedback retry; + + EXPECT_EQ( + executor.submitAndWait(first_request, first), + ActionQueueExecutor::WaitResult::Terminal); + EXPECT_EQ( + executor.submitAndWait(second_request, second), + ActionQueueExecutor::WaitResult::Terminal); + EXPECT_EQ( + executor.submitAndWait(rejected_request, rejected), + ActionQueueExecutor::WaitResult::Terminal); + EXPECT_EQ( + executor.submitAndWait(first_request, retry), + ActionQueueExecutor::WaitResult::Terminal); + + EXPECT_EQ(first.result(), api::ACTION_RESULT_CODE_COMPLETED); + EXPECT_EQ(second.result(), api::ACTION_RESULT_CODE_COMPLETED); + EXPECT_EQ(rejected.result(), api::ACTION_RESULT_CODE_REJECTED); + EXPECT_EQ( + rejected.deduplication_status(), + api::ACTION_DEDUPLICATION_STATUS_LEDGER_EXHAUSTED); + EXPECT_EQ(retry.result(), api::ACTION_RESULT_CODE_COMPLETED); + EXPECT_EQ( + retry.deduplication_status(), + api::ACTION_DEDUPLICATION_STATUS_CACHED_RESULT); +} + +TEST_F(GrpcSystemServiceTest, + ActionQueueCanceledWaiterLeavesAcceptedActionRunning) +{ + initializeActionDevices(); + ActionQueueExecutor executor(device::DeviceManager::getInstance()); + action_arm_->blockNextMotion(); + + api::ActionQueueCommand_Request request; + request.set_action_id("canceled-waiter-action-continues"); + request.set_expected_service_instance_id(executor.instanceId()); + request.set_total_timeout_ms(1000U); + addMoveLStep(request, "move", action_arm_->id(), 1.0); + + std::atomic waiter_canceled{false}; + api::ActionQueueCommand_Feedback abandoned_feedback; + auto waiter = std::async( + std::launch::async, + [&executor, &request, &abandoned_feedback, &waiter_canceled]() { + return executor.submitAndWait( + request, + abandoned_feedback, + [&waiter_canceled]() { + return waiter_canceled.load(std::memory_order_acquire); + }); + }); + ASSERT_TRUE(action_arm_->waitForMotionCalls( + 1, std::chrono::milliseconds(500))); + + waiter_canceled.store(true, std::memory_order_release); + const auto waiter_status = waiter.wait_for(std::chrono::milliseconds(500)); + if (waiter_status != std::future_status::ready) { + action_arm_->releaseBlockedMotion(); + } + ASSERT_EQ(waiter_status, std::future_status::ready); + EXPECT_EQ( + waiter.get(), + ActionQueueExecutor::WaitResult::CanceledAfterAdmission); + EXPECT_EQ(action_arm_->motionCalls(), 1); + + action_arm_->releaseBlockedMotion(); + ASSERT_TRUE(executor.waitForIdle(std::chrono::milliseconds(500))); + + api::ActionQueueCommand_Feedback retry_feedback; + const auto retry_result = executor.submitAndWait(request, retry_feedback); + + EXPECT_EQ(retry_result, ActionQueueExecutor::WaitResult::Terminal); + EXPECT_TRUE(retry_feedback.header().success()) + << retry_feedback.header().error_message(); + EXPECT_EQ( + retry_feedback.result(), + api::ACTION_RESULT_CODE_COMPLETED); + EXPECT_EQ(retry_feedback.completed_steps(), 1U); + EXPECT_EQ(action_arm_->motionCalls(), 1); +} + +TEST_F(GrpcSystemServiceTest, + ConcurrentIdenticalActionIdJoinsInFlightExecution) +{ + initializeActionDevices(); + ActionQueueExecutor executor(device::DeviceManager::getInstance()); + action_arm_->blockNextMotion(); + + api::ActionQueueCommand_Request request; + request.set_action_id("join-identical-in-flight-action"); + request.set_expected_service_instance_id(executor.instanceId()); + request.set_total_timeout_ms(1000U); + addMoveLStep(request, "move", action_arm_->id(), 1.0); + + api::ActionQueueCommand_Feedback first_feedback; + api::ActionQueueCommand_Feedback joined_feedback; + auto first = std::async( + std::launch::async, + [&executor, &request, &first_feedback]() { + return executor.submitAndWait(request, first_feedback); + }); + const bool motion_started = action_arm_->waitForMotionCalls( + 1, std::chrono::milliseconds(500)); + + std::atomic joined_wait_polls{0}; + auto joined = std::async( + std::launch::async, + [&executor, &request, &joined_feedback, &joined_wait_polls]() { + return executor.submitAndWait( + request, + joined_feedback, + [&joined_wait_polls]() { + joined_wait_polls.fetch_add( + 1, std::memory_order_acq_rel); + return false; + }); + }); + const auto poll_deadline = + std::chrono::steady_clock::now() + std::chrono::milliseconds(500); + while (joined_wait_polls.load(std::memory_order_acquire) < 2 && + std::chrono::steady_clock::now() < poll_deadline) { + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + } + const bool joined_is_waiting = + joined_wait_polls.load(std::memory_order_acquire) >= 2; + action_arm_->releaseBlockedMotion(); + + const auto first_result = first.get(); + const auto joined_result = joined.get(); + + ASSERT_TRUE(motion_started); + ASSERT_TRUE(joined_is_waiting); + EXPECT_EQ(first_result, ActionQueueExecutor::WaitResult::Terminal); + EXPECT_EQ(joined_result, ActionQueueExecutor::WaitResult::Terminal); + EXPECT_EQ(first_feedback.result(), api::ACTION_RESULT_CODE_COMPLETED); + EXPECT_EQ(joined_feedback.result(), api::ACTION_RESULT_CODE_COMPLETED); + EXPECT_EQ( + first_feedback.deduplication_status(), + api::ACTION_DEDUPLICATION_STATUS_ACCEPTED_NEW); + EXPECT_EQ( + joined_feedback.deduplication_status(), + api::ACTION_DEDUPLICATION_STATUS_JOINED_IN_FLIGHT); + EXPECT_EQ(action_arm_->motionCalls(), 1); +} + +TEST_F(GrpcSystemServiceTest, + ConcurrentActionQueuesRemainFifoAndHoldTheControlLease) +{ + initializeActionDevices(); + action_arm_->blockNextMotion(); + + api::ActionQueueCommand_Request first_request; + first_request.set_action_id("fifo-first"); + addMoveLStep(first_request, "first-1", action_arm_->id(), 10.0); + addMoveLStep(first_request, "first-2", action_arm_->id(), 11.0); + api::ActionQueueCommand_Request second_request; + second_request.set_action_id("fifo-second"); + addMoveLStep(second_request, "second-1", action_arm_->id(), 20.0); + + auto first = std::async( + std::launch::async, + [this, first_request]() { return executeAction(first_request); }); + const bool started = action_arm_->waitForMotionCalls( + 1, std::chrono::milliseconds(500)); + auto second = std::async( + std::launch::async, + [this, second_request]() { return executeAction(second_request); }); + std::this_thread::sleep_for(std::chrono::milliseconds(5)); + + const auto competing = + control::ControlAuthorityManager::instance().tryAcquire( + action_arm_->id(), "ordinary-control", + std::chrono::seconds(1)); + if (competing.acquired) { + // Keep a failed assertion from stranding the worker behind this test + // lease and turning the diagnostic into a long timeout. + control::ControlAuthorityManager::instance().release( + competing.token); + } + action_arm_->releaseBlockedMotion(); + const auto first_response = first.get(); + const auto second_response = second.get(); + + EXPECT_TRUE(started); + EXPECT_FALSE(competing.acquired); + EXPECT_EQ(first_response.result(), api::ACTION_RESULT_CODE_COMPLETED); + EXPECT_EQ(second_response.result(), api::ACTION_RESULT_CODE_COMPLETED); + EXPECT_EQ(action_arm_->maxActiveMotions(), 1); + EXPECT_EQ( + action_trace_->names(), + (std::vector{ + "arm:L:10", "arm:L:11", "arm:L:20"})); +} + +TEST_F(GrpcSystemServiceTest, + ActionQueueTotalAndStepTimeoutsIssueTypedArmStop) +{ + initializeActionDevices(); + + action_arm_->blockNextMotion(); + api::ActionQueueCommand_Request total_timeout; + total_timeout.set_action_id("total-timeout"); + total_timeout.set_total_timeout_ms(40U); + addMoveLStep( + total_timeout, "total", action_arm_->id(), 1.0); + const auto total_response = executeAction(total_timeout); + const int stops_after_total = action_arm_->stopMotionCalls(); + + action_arm_->blockNextMotion(); + api::ActionQueueCommand_Request step_timeout; + step_timeout.set_action_id("step-timeout"); + step_timeout.set_total_timeout_ms(500U); + addMoveLStep( + step_timeout, "step", action_arm_->id(), 2.0, 30U); + const auto step_response = executeAction(step_timeout); + + EXPECT_EQ(total_response.result(), api::ACTION_RESULT_CODE_TIMED_OUT); + EXPECT_EQ(total_response.completed_steps(), 0U); + ASSERT_TRUE(total_response.has_failed_step_index()); + EXPECT_EQ(total_response.failed_step_index(), 0U); + EXPECT_GE(stops_after_total, 1); + EXPECT_EQ(step_response.result(), api::ACTION_RESULT_CODE_TIMED_OUT); + EXPECT_EQ(step_response.completed_steps(), 0U); + ASSERT_TRUE(step_response.has_failed_step_index()); + EXPECT_EQ(step_response.failed_step_index(), 0U); + EXPECT_GT(action_arm_->stopMotionCalls(), stops_after_total); +} + +TEST_F(GrpcSystemServiceTest, + DelayedActionCancellationCannotStopSuccessorControlLease) +{ + initializeActionDevices(); + action_arm_->blockNextMotion(); + action_arm_->blockCanceledMotionReturn(); + + api::ActionQueueCommand_Request request; + request.set_action_id("stale-action-stop-must-not-preempt-successor"); + request.set_total_timeout_ms(2000U); + addMoveLStep(request, "active", action_arm_->id(), 1.0); + + auto action = std::async( + std::launch::async, + [this, request]() { return executeAction(request); }); + const bool motion_started = action_arm_->waitForMotionCalls( + 1, std::chrono::milliseconds(500)); + + auto& authority = control::ControlAuthorityManager::instance(); + control::ControlAcquireResult direct_stop; + if (motion_started) { + direct_stop = authority.preemptAcquire( + action_arm_->id(), + "direct-stop-before-successor", + std::chrono::seconds(30)); + } + if (direct_stop.acquired) { + (void)action_arm_->stopMotion(); + } else { + action_arm_->releaseBlockedMotion(); + } + const bool old_driver_ready_to_return = + action_arm_->waitForCanceledMotionReturn( + std::chrono::milliseconds(500)); + if (direct_stop.acquired) { + authority.release(direct_stop.token); + } + + control::ControlAcquireResult successor; + if (old_driver_ready_to_return) { + successor = authority.tryAcquire( + action_arm_->id(), + "successor-move", + std::chrono::seconds(30)); + } + action_arm_->releaseCanceledMotionReturn(); + + const auto action_status = action.wait_for(std::chrono::seconds(1)); + if (action_status != std::future_status::ready) { + service_->prepareForShutdown(); + } + ASSERT_EQ(action_status, std::future_status::ready); + const auto response = action.get(); + const bool successor_still_current = + successor.acquired && authority.validate(successor.token); + if (successor.acquired) { + authority.release(successor.token); + } + + ASSERT_TRUE(motion_started); + ASSERT_TRUE(direct_stop.acquired) << direct_stop.detail; + ASSERT_TRUE(old_driver_ready_to_return); + ASSERT_TRUE(successor.acquired) << successor.detail; + EXPECT_EQ(response.result(), api::ACTION_RESULT_CODE_CANCELED); + EXPECT_TRUE(successor_still_current); + EXPECT_EQ(action_arm_->stopMotionCalls(), 1); +} + +TEST_F(GrpcSystemServiceTest, + StopAllCancelsActiveActionAndSkipsRemainingSteps) +{ + initializeActionDevices(); + action_arm_->blockNextMotion(); + api::ActionQueueCommand_Request action_request; + action_request.set_action_id("stop-all-action"); + action_request.set_total_timeout_ms(500U); + addMoveLStep(action_request, "active", action_arm_->id(), 1.0); + addMoveLStep(action_request, "must-not-run", action_arm_->id(), 2.0); + + auto action = std::async( + std::launch::async, + [this, action_request]() { return executeAction(action_request); }); + const bool started = action_arm_->waitForMotionCalls( + 1, std::chrono::milliseconds(500)); + + api::StopAllCommand_Request stop_request; + api::StopAllCommand_Feedback stop_response; + grpc::ServerContext stop_context; + const auto stop_status = service_->StopAll( + &stop_context, &stop_request, &stop_response); + const auto action_response = action.get(); + + EXPECT_TRUE(started); + ASSERT_TRUE(stop_status.ok()) << stop_status.error_message(); + EXPECT_TRUE(stop_response.header().success()) + << stop_response.header().error_message(); + EXPECT_EQ(action_response.result(), api::ACTION_RESULT_CODE_CANCELED); + EXPECT_EQ(action_response.completed_steps(), 0U); + ASSERT_TRUE(action_response.has_failed_step_index()); + EXPECT_EQ(action_response.failed_step_index(), 0U); + EXPECT_EQ(action_arm_->motionCalls(), 1); + EXPECT_GE(action_arm_->stopMotionCalls(), 1); +} + +TEST_F(GrpcSystemServiceTest, + StopAllFailsClosedWhenAgvStoppedStateIsUnconfirmed) +{ + initializeActionDevices(); + action_agv_->setStoppedConfirmed(false); + + api::StopAllCommand_Request request; + api::StopAllCommand_Feedback response; + grpc::ServerContext context; + const auto status = service_->StopAll( + &context, &request, &response); + + ASSERT_TRUE(status.ok()) << status.error_message(); + EXPECT_FALSE(response.header().success()); + EXPECT_NE( + response.header().error_message().find("remain quarantined"), + std::string::npos); + const auto lease = + control::ControlAuthorityManager::instance().tryAcquire( + action_agv_->id(), + "normal-control-after-unconfirmed-stop-all", + std::chrono::hours(1)); + EXPECT_FALSE(lease.acquired); +} + +TEST_F(GrpcSystemServiceTest, + ActionQueueCancellationStopsActiveLaterArmBeforeEarlierArm) +{ + config::DeviceManagerConfig config; + auto& manager = device::DeviceManager::getInstance(config); + action_trace_ = std::make_shared(); + auto earlier_arm = std::make_shared( + "earlier-arm", action_trace_); + auto active_arm = std::make_shared( + "active-arm", action_trace_); + manager.registerDevice(earlier_arm); + manager.registerDevice(active_arm); + service_ = std::make_unique(); + api::GetSystemInfoCommand_Request info_request; + api::GetSystemInfoCommand_Feedback info_response; + grpc::ServerContext info_context; + ASSERT_TRUE(service_->GetSystemInfo( + &info_context, &info_request, &info_response).ok()); + service_instance_id_ = info_response.action_service_instance_id(); + ASSERT_FALSE(service_instance_id_.empty()); + + active_arm->blockNextMotion(); + api::ActionQueueCommand_Request request; + request.set_action_id("cancel-active-later-arm-first"); + request.set_total_timeout_ms(1000U); + addMoveLStep(request, "earlier-completes", earlier_arm->id(), 1.0); + addMoveLStep(request, "active-blocks", active_arm->id(), 2.0); + + auto action = std::async( + std::launch::async, + [this, request]() { return executeAction(request); }); + const bool active_started = active_arm->waitForMotionCalls( + 1, std::chrono::milliseconds(500)); + + service_->prepareForShutdown(); + const auto response = action.get(); + const auto trace = action_trace_->names(); + const auto active_stop = std::find( + trace.begin(), trace.end(), "arm:stop:active-arm"); + const auto earlier_stop = std::find( + trace.begin(), trace.end(), "arm:stop:earlier-arm"); + + ASSERT_TRUE(active_started); + EXPECT_EQ(response.result(), api::ACTION_RESULT_CODE_CANCELED); + EXPECT_EQ(response.completed_steps(), 1U); + ASSERT_TRUE(response.has_failed_step_index()); + EXPECT_EQ(response.failed_step_index(), 1U); + ASSERT_NE(active_stop, trace.end()); + ASSERT_NE(earlier_stop, trace.end()); + EXPECT_LT(active_stop, earlier_stop); +} + TEST_F(GrpcSystemServiceTest, StopAllStopsRegisteredDevicesAndRevokesOnlyArmLease) { diff --git a/cmvr-es/task/grpc_server_task/src/grpc_server_task.cpp b/cmvr-es/task/grpc_server_task/src/grpc_server_task.cpp index 46a0bf61..74815d2a 100644 --- a/cmvr-es/task/grpc_server_task/src/grpc_server_task.cpp +++ b/cmvr-es/task/grpc_server_task/src/grpc_server_task.cpp @@ -223,6 +223,11 @@ void GrpcServerTask::stop() { { std::lock_guard lock(mutex_); + if (auto* system_service = + dynamic_cast( + system_service_.get())) { + system_service->prepareForShutdown(); + } if (server_) { server_->Shutdown(); } diff --git a/protos/cmvr/api/system_command.proto b/protos/cmvr/api/system_command.proto index c61fa36d..b572d230 100644 --- a/protos/cmvr/api/system_command.proto +++ b/protos/cmvr/api/system_command.proto @@ -1,5 +1,7 @@ syntax = "proto3"; +import "cmvr/api/agv_command.proto"; +import "cmvr/api/arm_command.proto"; import "cmvr/api/common.proto"; package cmvr.api; @@ -103,6 +105,10 @@ message GetSystemInfoCommand { string os = 5; string kernel_version = 6; string architecture = 7; + + // Changes whenever the in-process ActionQueue idempotency ledger is + // recreated. Clients bind submissions and retries to this value. + string action_service_instance_id = 8; } } @@ -141,3 +147,97 @@ message StopAllCommand { CommandHeader.Feedback header = 1; } } + +// Final outcome of one ActionQueue execution. +enum ActionResultCode { + ACTION_RESULT_CODE_UNSPECIFIED = 0; + ACTION_RESULT_CODE_COMPLETED = 1; + ACTION_RESULT_CODE_FAILED = 2; + ACTION_RESULT_CODE_CANCELED = 3; + ACTION_RESULT_CODE_TIMED_OUT = 4; + ACTION_RESULT_CODE_REJECTED = 5; +} + +// Describes whether this RPC admitted a new action or observed an existing +// idempotency record. Clients must not infer this from an error string. +enum ActionDeduplicationStatus { + ACTION_DEDUPLICATION_STATUS_UNSPECIFIED = 0; + ACTION_DEDUPLICATION_STATUS_ACCEPTED_NEW = 1; + ACTION_DEDUPLICATION_STATUS_JOINED_IN_FLIGHT = 2; + ACTION_DEDUPLICATION_STATUS_CACHED_RESULT = 3; + ACTION_DEDUPLICATION_STATUS_RESULT_EVICTED = 4; + ACTION_DEDUPLICATION_STATUS_LEDGER_EXHAUSTED = 5; + ACTION_DEDUPLICATION_STATUS_ACTION_ID_CONFLICT = 6; + ACTION_DEDUPLICATION_STATUS_SERVICE_INSTANCE_MISMATCH = 7; +} + +// Edge-local delay between two device commands. +message DelayAction { + // Delay duration in milliseconds. The server applies a bounded maximum. + uint32 duration_ms = 1; +} + +// One finite, synchronous command in an ActionQueue request. +message ActionStep { + // Client-provided identifier used for diagnostics. It must be unique within + // one ActionQueue request. + string step_id = 1; + + // Per-step timeout in milliseconds. Zero inherits the remaining action + // timeout or the server default. + uint32 timeout_ms = 2; + + // Tags are grouped by domain so compatible commands can be added without + // renumbering existing alternatives: Arm 10-19, AGV 20-29, built-ins 90+. + oneof command { + MoveJ.Request arm_move_j = 10; + MoveL.Request arm_move_l = 11; + + AgvNavigateToPoseCommand.Request agv_navigate_to_pose = 20; + AgvNavigateToStationCommand.Request agv_navigate_to_station = 21; + AgvFollowPathCommand.Request agv_follow_path = 22; + + DelayAction delay = 90; + } +} + +// Atomically submits a complete command sequence for edge-local serial +// execution. Device motion alternatives must use synchronous execution. +message ActionQueueCommand { + message Request { + // Client-generated globally unique idempotency key. During one Action + // service instance, retrying an identical accepted request with the + // same action_id does not dispatch its steps a second time. Recent + // terminal results can be returned; older accepted IDs are rejected + // fail-closed after their result is evicted. Deduplication is not + // persisted across an edge-service restart; the required instance + // epoch below prevents an old retry from being replayed after restart. + string action_id = 1; + repeated ActionStep steps = 2; + + // Total queue-wait plus execution timeout in milliseconds. Zero uses a + // bounded server default. + uint32 total_timeout_ms = 3; + + // Required instance epoch obtained from GetSystemInfo. A mismatch + // means the process-local deduplication ledger was recreated, so the + // server rejects the request instead of risking a replay. + string expected_service_instance_id = 4; + } + + message Feedback { + CommandHeader.Feedback header = 1; + string action_id = 2; + + // Number of steps which completed successfully before the final result. + uint32 completed_steps = 3; + + // Present only when a particular step caused failure, cancellation, + // timeout, or rejection. Presence distinguishes index zero from no + // failed step. + optional uint32 failed_step_index = 4; + ActionResultCode result = 5; + string service_instance_id = 6; + ActionDeduplicationStatus deduplication_status = 7; + } +} diff --git a/protos/cmvr/api/system_service.proto b/protos/cmvr/api/system_service.proto index 83f19afd..bb6dc4b8 100644 --- a/protos/cmvr/api/system_service.proto +++ b/protos/cmvr/api/system_service.proto @@ -13,4 +13,6 @@ service SystemService { rpc UpdateParams(UpdateParamsCommand.Request) returns (UpdateParamsCommand.Feedback) {} rpc StopAll(StopAllCommand.Request) returns (StopAllCommand.Feedback) {} + + rpc ExecuteActionQueue(ActionQueueCommand.Request) returns (ActionQueueCommand.Feedback) {} }