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] 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;