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;